Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
WMBroadcaster.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2020-2022 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
16
17#include <functional>
18#include <mutex>
20#include <carma_wm/Geometry.hpp>
22#include <autoware_lanelet2_ros2_interface/utility/message_conversion.hpp>
23#include <lanelet2_extension/projection/local_frame_projector.h>
24#include <lanelet2_core/primitives/Lanelet.h>
25#include <lanelet2_core/geometry/Lanelet.h>
26#include <lanelet2_core/geometry/BoundingBox.h>
27#include <lanelet2_core/primitives/BoundingBox.h>
28#include <type_traits>
29#include <carma_perception_msgs/msg/check_active_geofence.hpp>
30#include <lanelet2_core/primitives/Polygon.h>
31#include <proj.h>
32#include <lanelet2_io/Projection.h>
33#include <lanelet2_core/utility/Units.h>
34#include <lanelet2_core/Forward.h>
35#include <autoware_lanelet2_ros2_interface/utility/utilities.hpp>
36#include <algorithm>
37#include <limits>
38#include <carma_wm/Geometry.hpp>
39#include <math.h>
40#include <boost/date_time/date_defs.hpp>
42
43namespace carma_wm_ctrl
44{
45using std::placeholders::_1;
46
47WMBroadcaster::WMBroadcaster(const PublishMapCallback& map_pub, const PublishMapUpdateCallback& map_update_pub, const PublishCtrlRequestCallback& control_msg_pub,
48const PublishActiveGeofCallback& active_pub, std::shared_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory, const PublishMobilityOperationCallback& tcm_ack_pub)
49 : map_pub_(map_pub), map_update_pub_(map_update_pub), control_msg_pub_(control_msg_pub), active_pub_(active_pub), scheduler_(timer_factory), tcm_ack_pub_(tcm_ack_pub)
50{
53 std::bind(&WMBroadcaster::routeCallbackMessage, this, _1);
54};
55
56void WMBroadcaster::baseMapCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg)
57{
58 std::lock_guard<std::mutex> guard(map_mutex_);
59
60 static bool firstCall = true;
61 // This function should generally only ever be called one time so log a warning if it occurs multiple times
62 if (firstCall)
63 {
64 firstCall = false;
65 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called for first time with new map message");
66 }
67 else
68 {
69 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called multiple times in the same node");
70 }
71
72 lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap);
73 lanelet::LaneletMapPtr new_map_to_change(new lanelet::LaneletMap);
74
76 lanelet::utils::conversion::fromBinMsg(*map_msg, new_map_to_change);
77
78 base_map_ = new_map; // Store map
79 current_map_ = new_map_to_change; // broadcaster makes changes to this
80
81 lanelet::MapConformer::ensureCompliance(base_map_, config_limit); // Update map to ensure it complies with expectations
83
84 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Building routing graph for base map");
85
86 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
87 lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_);
88 current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
89
90 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done building routing graph for base map");
91
92 // Publish map
93 current_map_version_ += 1; // Increment the map version. It should always start from 1 for the first map
94
95 autoware_lanelet2_msgs::msg::MapBin compliant_map_msg;
96
97 // Populate the routing graph message
98 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message.");
99
100 auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
101
102 compliant_map_msg.routing_graph = readable_graph->routingGraphToMsg(participant_);
103 compliant_map_msg.has_routing_graph = true;
104
105 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message.");
106
108 compliant_map_msg.map_version = current_map_version_;
109 map_pub_(compliant_map_msg);
110};
111
117void WMBroadcaster::addScheduleFromMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01)
118{
119 // Handle schedule processing
120 carma_v2x_msgs::msg::TrafficControlSchedule msg_schedule = msg_v01.params.schedule;
121 double ns_to_sec = 1.0e9;
122 auto clock_type = scheduler_.getClockType();
123
124 rclcpp::Time end_time = {msg_schedule.end, clock_type};
125 if (!msg_schedule.end_exists) {
126 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "No end time for geofence, using rclcpp::Time::max()");
127 end_time = {rclcpp::Time::max(), clock_type}; // If there is no end time use the max time
128 }
129
130 // If the days of the week are specified then convert them to the boost format.
131 GeofenceSchedule::DayOfTheWeekSet week_day_set = { 0, 1, 2, 3, 4, 5, 6 }; // Default to all days 0==Sun to 6==Sat
132 if (msg_schedule.dow_exists) {
133 // sun (6), mon (5), tue (4), wed (3), thu (2), fri (1), sat (0)
134 week_day_set.clear();
135 for (size_t i = 0; i < msg_schedule.dow.dow.size(); i++) {
136 if (msg_schedule.dow.dow[i] == 1) {
137 switch(i) {
138 case 6: // sun
139 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Sunday);
140 break;
141 case 5: // mon
142 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Monday);
143 break;
144 case 4: // tue
145 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Tuesday);
146 break;
147 case 3: // wed
148 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Wednesday);
149 break;
150 case 2: // thur
151 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Thursday);
152 break;
153 case 1: // fri
154 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Friday);
155 break;
156 case 0: // sat
157 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Saturday);
158 break;
159 default:
160 throw std::invalid_argument("Unrecognized weekday value: " + std::to_string(i));
161 }
162 }
163 }
164 }
165
166 if (msg_schedule.between_exists) {
167
168 for (auto daily_schedule : msg_schedule.between)
169 {
170 if (msg_schedule.repeat_exists) {
171 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
172 end_time,
173 rclcpp::Duration(daily_schedule.begin),
174 rclcpp::Duration(daily_schedule.duration),
175 rclcpp::Duration(msg_schedule.repeat.offset),
176 rclcpp::Duration(msg_schedule.repeat.span),
177 rclcpp::Duration(msg_schedule.repeat.period),
178 week_day_set));
179 } else {
180 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
181 end_time,
182 rclcpp::Duration(daily_schedule.begin),
183 rclcpp::Duration(daily_schedule.duration),
184 rclcpp::Duration(0.0), // No offset
185 rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // Compute schedule portion end time
186 rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // No repetition so same as portion end time
187 week_day_set));
188 }
189
190 }
191 }
192 else {
193 if (msg_schedule.repeat_exists) {
194 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
195 end_time,
196 rclcpp::Duration(0.0),
197 rclcpp::Duration(86400.0e9), // 24 hr daily application
198 rclcpp::Duration(msg_schedule.repeat.offset),
199 rclcpp::Duration(msg_schedule.repeat.span),
200 rclcpp::Duration(msg_schedule.repeat.period),
201 week_day_set));
202 } else {
203 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
204 end_time,
205 rclcpp::Duration(0.0),
206 rclcpp::Duration(86400.0e9), // 24 hr daily application
207 rclcpp::Duration(0.0), // No offset
208 rclcpp::Duration(86400.0e9), // Applied for full lenth of 24 hrs
209 rclcpp::Duration(86400.0e9), // No repetition
210 week_day_set));
211 }
212
213 }
214}
215
216
217std::vector<std::shared_ptr<Geofence>> WMBroadcaster::geofenceFromMapMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::MapData& map_msg)
218{
219 std::vector<std::shared_ptr<Geofence>> updates_to_send;
220 std::vector<std::shared_ptr<lanelet::SignalizedIntersection>> intersections;
221 std::vector<std::shared_ptr<lanelet::CarmaTrafficSignal>> traffic_signals;
222
223 auto sim_copy = std::make_shared<carma_wm::SignalizedIntersectionManager>(*sim_);
224
225 sim_->createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, current_map_, current_routing_graph_);
226
227 if (*sim_ == *sim_copy) // if no change
228 {
229 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), ">>> Detected no change from previous, ignoring duplicate message! with gf id: " << gf_ptr->id_);
230 return {};
231 }
232
233 for (auto intersection : intersections)
234 {
235 auto update = std::make_shared<Geofence>();
236 update->id_ = boost::uuids::random_generator()();
238 update->regulatory_element_ = intersection;
239 for (auto llt : intersection->getEntryLanelets())
240 {
241 update->affected_parts_.push_back(llt);
242 }
243 updates_to_send.push_back(update);
244 }
245
246 for (auto signal : traffic_signals)
247 {
248 auto update = std::make_shared<Geofence>();
249 update->id_ = boost::uuids::random_generator()();
250 update->label_ = carma_wm_ctrl::MAP_MSG_TF_SIGNAL;
251 update->regulatory_element_ = signal;
252 for (auto llt : signal->getControlStartLanelets())
253 {
254 update->affected_parts_.push_back(llt);
255 }
256 updates_to_send.push_back(update);
257 }
258
259 return updates_to_send;
260}
261
262void WMBroadcaster::geofenceFromMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01)
263{
264 bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find("SIG_WZ") != std::string::npos;
265 carma_v2x_msgs::msg::TrafficControlDetail msg_detail = msg_v01.params.detail;
266
267 // Get ID
268 std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin());
269
270 gf_ptr->gf_pts = getPointsInLocalFrame(msg_v01);
271
272 gf_ptr->affected_parts_ = getAffectedLaneletOrAreas(gf_ptr->gf_pts);
273
274 if (gf_ptr->affected_parts_.size() == 0) {
275 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "There is no applicable component in map for the new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
276 return; // Return empty geofence list
277 }
278
279 std::vector<lanelet::Lanelet> affected_llts;
280 std::vector<lanelet::Area> affected_areas;
281
282 // used for assigning them to the regem as parameters
283 for (auto llt_or_area : gf_ptr->affected_parts_)
284 {
285
286 if (llt_or_area.isLanelet()) affected_llts.push_back(current_map_->laneletLayer.get(llt_or_area.lanelet()->id()));
287 if (llt_or_area.isArea()) affected_areas.push_back(current_map_->areaLayer.get(llt_or_area.area()->id()));
288 }
289
290 // TODO: logic to determine what type of geofence goes here
291 // currently only converting portion of control message that is relevant to:
292 // - digital speed limit, passing control line, digital minimum gap, region access rule, and series of workzone related messages
293 lanelet::Velocity sL;
294
295
296 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
297 {
298 //Acquire speed limit information from TafficControlDetail msg
299 sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPS());
300 std::string reason = "";
301 if (msg_v01.package.label_exists)
302 reason = msg_v01.package.label;
303
304 if(config_limit > 0_mph && config_limit < 80_mph && config_limit < sL)//Accounting for the configured speed limit, input zero when not in use
305 sL = config_limit;
306 //Ensure Geofences do not provide invalid speed limit data (exceed predetermined maximum value)
307 // @SONAR_STOP@
308 if(sL > 80_mph )
309 {
310 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message
311 sL = 80_mph; //Cap the speed limit to the predetermined maximum value
312
313 }
314 if(sL < 0_mph)
315 {
316 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph.");
317 sL = 0_mph;
318 }// @SONAR_START@
319
320 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
321 sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason));
322 }
323 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE)
324 {
325 //Acquire speed limit information from TafficControlDetail msg
326 sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPS());
327 if(config_limit > 0_mph && config_limit < 80_mph)//Accounting for the configured speed limit, input zero when not in use
328 sL = config_limit;
329
330 std::string reason = "";
331 if (msg_v01.package.label_exists)
332 reason = msg_v01.package.label;
333
334 //Ensure Geofences do not provide invalid speed limit data
335 // @SONAR_STOP@
336 if(sL > 80_mph )
337 {
338 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value capped at max speed limit.");
339 sL = 80_mph;
340 }
341 if(sL < 0_mph)
342 {
343 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph.");
344 sL = 0_mph;
345 }// @SONAR_START@
346 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
347 sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason));
348 }
349 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE)
350 {
351 addPassingControlLineFromMsg(gf_ptr, msg_v01, affected_llts);
352 }
353 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE)
354 {
355
356 double min_gap = (double)msg_detail.minhdwy;
357
358 if(min_gap < 0)
359 {
360 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital min gap is invalid. Value set to 0 meter.");
361 min_gap = 0;
362 }
363 addRegionMinimumGap(gf_ptr,msg_v01, min_gap, affected_llts, affected_areas);
364 }
365
366 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later
367 {
368 gf_ptr->label_ = msg_v01.package.label; // to extract intersection, and signal group id
369 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
370 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
371 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
372 {
373 work_zone_geofence_cache_[msg_detail.closed] = gf_ptr;
374 }
375 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
376 {
378 }
379 return;
380 }
381 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==carma_v2x_msgs::msg::TrafficControlDetail::CLOSED) // if stand-alone closed signal apart from Workzone
382 {
383 addRegionAccessRule(gf_ptr,msg_v01,affected_llts);
384 }
385
386 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::RESTRICTED_CHOICE) {
387 addRegionAccessRule(gf_ptr, msg_v01, affected_llts);
388 }
389
390 return;
391}
392
393std::shared_ptr<Geofence> WMBroadcaster::createWorkzoneGeofence(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache)
394{
395 std::shared_ptr<std::vector<lanelet::Lanelet>> parallel_llts = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
396 std::shared_ptr<std::vector<lanelet::Lanelet>> middle_opposite_lanelets = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
397
398 // Split existing lanelets and filter into parallel_llts and middle_opposite_lanelets
399 preprocessWorkzoneGeometry(work_zone_geofence_cache, parallel_llts, middle_opposite_lanelets);
400
401 // Create geofence and rest of the required lanelets along with traffic light for completing workzone area
402 auto gf_ptr = createWorkzoneGeometry(work_zone_geofence_cache, parallel_llts->front(), parallel_llts->back(), middle_opposite_lanelets );
403
404 // copy static info from the existing workzone
405 gf_ptr->id_ = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->id_; //using taperright's id as the whole geofence's id
406
407 // schedule
408 gf_ptr->schedules = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->schedules; //using taperright's schedule as the whole geofence's schedule
409
410 // erase cache now that it is processed
411 for (auto pair : work_zone_geofence_cache)
412 {
413 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone geofence finished processing. Therefore following geofence id is being dropped from cache as it is processed as part of it: " << pair.second->id_);
414 }
415 work_zone_geofence_cache.clear();
416
417 return gf_ptr;
418}
419
420std::shared_ptr<Geofence> WMBroadcaster::createWorkzoneGeometry(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache, lanelet::Lanelet parallel_llt_front, lanelet::Lanelet parallel_llt_back,
421 std::shared_ptr<std::vector<lanelet::Lanelet>> middle_opposite_lanelets)
422{
423 auto gf_ptr = std::make_shared<Geofence>();
424
426 //FRONT DIAGONAL LANELET
428
429 lanelet::Lanelet front_llt_diag = createLinearInterpolatingLanelet(parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back(),
430 middle_opposite_lanelets->back().rightBound3d().back(), middle_opposite_lanelets->back().leftBound3d().back());
431 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created diag front_llt_diag id:" << front_llt_diag.id());
432 for (auto regem : middle_opposite_lanelets->back().regulatoryElements()) //copy existing regem into the new llts
433 {
434 front_llt_diag.addRegulatoryElement(regem);
435 }
436
438 //BACK DIAGONAL LANELET
440
441 lanelet::Lanelet back_llt_diag = createLinearInterpolatingLanelet(middle_opposite_lanelets->front().rightBound3d().front(), middle_opposite_lanelets->front().leftBound3d().front(),
442 parallel_llt_back.leftBound3d().front(), parallel_llt_back.rightBound3d().front());
443 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created back_llt_diag diag id:" << back_llt_diag.id());
444 for (auto regem : parallel_llt_back.regulatoryElements()) //copy existing regem into the new llts
445 {
446 back_llt_diag.addRegulatoryElement(regem);
447 }
448
450 //MIDDLE LANELETS TO MATCH PARALLEL DIRECTION
452
453 std::vector<lanelet::Lanelet> middle_llts;
454 for (int i = middle_opposite_lanelets->size() - 1; i >= 0; i--) //do no use size_t as it might overflow
455 {
456 lanelet::Lanelet middle_llt (lanelet::utils::getId(), (*(middle_opposite_lanelets.get()))[i].rightBound3d().invert(), (*(middle_opposite_lanelets.get()))[i].leftBound3d().invert());
457 for (auto regem : (*(middle_opposite_lanelets.get()))[i].regulatoryElements())
458 {
459 middle_llt.addRegulatoryElement(regem); //copy existing regem into the new llts
460 }
461 middle_llts.push_back(middle_llt);
462 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created matching direction of middle_llt id:" << middle_llt.id());
463 }
464
466 //ADD TF_LIGHT TO PARALLEL LANELET
468 lanelet::LineString3d parallel_stop_ls(lanelet::utils::getId(), {parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back()});
469 // controlled lanelet:
470 std::vector<lanelet::Lanelet> controlled_taper_right;
471
472 controlled_taper_right.push_back(parallel_llt_front); //which has the light
473
474 controlled_taper_right.push_back(front_llt_diag);
475
476 controlled_taper_right.insert(controlled_taper_right.end(), middle_llts.begin(), middle_llts.end());
477
478 controlled_taper_right.push_back(back_llt_diag);
479
480 lanelet::CarmaTrafficSignalPtr tfl_parallel = std::make_shared<lanelet::CarmaTrafficSignal>(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {parallel_stop_ls}, {controlled_taper_right.front()}, {controlled_taper_right.back()}));
481
482 gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->label_),tfl_parallel->id()});
483
484 parallel_llt_front.addRegulatoryElement(tfl_parallel);
485 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created TF_LIGHT of Id: " << tfl_parallel->id() << ", to parallel_llt_front id:" << parallel_llt_front.id());
486
488 //ADD TF_LIGHT TO OPPOSITE LANELET
490 std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts_with_stop_line = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
491
492 auto old_opposite_llts = splitOppositeLaneletWithPoint(opposite_llts_with_stop_line, work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d(),
493 current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id()), error_distance_);
494
495 lanelet::LineString3d opposite_stop_ls(lanelet::utils::getId(), {opposite_llts_with_stop_line->front().leftBound3d().back(), opposite_llts_with_stop_line->front().rightBound3d().back()});
496
497 std::vector<lanelet::Lanelet> controlled_open_right;
498
499 controlled_open_right.insert(controlled_open_right.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());; //split lanelet one of which has light
500
501 for (auto llt : work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_)
502 {
503 controlled_open_right.push_back(current_map_->laneletLayer.get(llt.lanelet().get().id()));
504 }
505
506 lanelet::CarmaTrafficSignalPtr tfl_opposite = std::make_shared<lanelet::CarmaTrafficSignal>(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {opposite_stop_ls}, {controlled_open_right.front()}, {controlled_open_right.back()}));
507
508 gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->label_), tfl_opposite->id()});
509
510 opposite_llts_with_stop_line->front().addRegulatoryElement(tfl_opposite);
511 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created TF_LIGHT of Id: " << tfl_opposite->id() << ", to opposite_llts_with_stop_line->front() id:" << opposite_llts_with_stop_line->front().id());
512
514 //ADD ALL NEWLY CREATED LANELETS INTO GEOFENCE
515 //OBJECTS TO BE PROCESSED LATER BY SCHEDULER
517 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_front id:" << parallel_llt_front.id());
518 gf_ptr->lanelet_additions_.push_back(parallel_llt_front);
519
520 gf_ptr->lanelet_additions_.push_back(front_llt_diag);
521
522 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), middle_llts.begin(), middle_llts.end());
523
524 gf_ptr->lanelet_additions_.push_back(back_llt_diag);
525 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_back id:" << parallel_llt_back.id());
526 gf_ptr->lanelet_additions_.push_back(parallel_llt_back);
527
528 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());;
529
531 // ADD REGION_ACCESS_RULE REGEM TO THE OUTDATED LANELETS
532 // AS WELL AS LANELETS BEING BLOCKED BY WORKZONE GEOFENCE
534
535 // fill information for paricipants and reason for blocking
536 carma_v2x_msgs::msg::TrafficControlMessageV01 participants_and_reason_only;
537
538 j2735_v2x_msgs::msg::TrafficControlVehClass participant; // sending all possible VEHICLE will be processed as they are not accessuble by regionAccessRule
539
540 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE;
541
542 participants_and_reason_only.params.vclasses.push_back(participant);
543
544 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BUS;
545
546 participants_and_reason_only.params.vclasses.push_back(participant);
547
548 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR;
549
550 participants_and_reason_only.params.vclasses.push_back(participant);
551
552 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK;
553
554 participants_and_reason_only.params.vclasses.push_back(participant);
555
556 participants_and_reason_only.package.label = "SIG_WZ";
557
558 std::vector<lanelet::Lanelet> old_or_blocked_llts; // this is needed to addRegionAccessRule input signatures
559
560 // from all affected parts, remove duplicate entries
561 for (auto llt : work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_)
562 {
563 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
564 {
565 gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
566 old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
567 }
568 }
569 for (auto llt : work_zone_geofence_cache[WorkZoneSection::CLOSED]->affected_parts_)
570 {
571 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
572 {
573 gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
574 old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
575 }
576 }
577 for (auto llt : work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_)
578 {
579 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
580 {
581 gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
582 old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
583 }
584 }
585
586 gf_ptr->affected_parts_.push_back(old_opposite_llts[0]); // block old lanelet in the opposing lanelet that will be replaced with split lanelets that have trafficlight
587 old_or_blocked_llts.push_back(old_opposite_llts[0]);
588
589 // actual regulatory element adder
590 addRegionAccessRule(gf_ptr, participants_and_reason_only, old_or_blocked_llts);
591
592 return gf_ptr;
593}
594
595void WMBroadcaster::setErrorDistance(double error_distance)
596{
597 error_distance_ = error_distance;
598}
599
600void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache, std::shared_ptr<std::vector<lanelet::Lanelet>> parallel_llts, std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts)
601{
602 if (!current_map_ || current_map_->laneletLayer.size() == 0)
603 {
604 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
605 }
609 std::vector <lanelet::Lanelet> new_taper_right_llts;
610 auto taper_right_first_pt = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->gf_pts.front().basicPoint2d();
611 auto taper_right_first_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id());
612
613 new_taper_right_llts = splitLaneletWithPoint({taper_right_first_pt}, taper_right_first_llt, error_distance_);
614 double check_dist_tpr = lanelet::geometry::distance2d(taper_right_first_llt.centerline2d().front().basicPoint2d(), taper_right_first_pt);
615
616 // if no splitting happened and taperright's first point is close to starting boundary, we need to create duplicate of previous lanelet of TAPERRIGHT's first lanelet
617 // to match the output expected of this function as if split happened
618 if (new_taper_right_llts.size() == 1 && check_dist_tpr <= error_distance_)
619 {
620 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating duplicate lanelet of 'previous lanelet' due to TAPERRIGHT using entire lanelet...");
621 auto previous_lanelets = current_routing_graph_->previous(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get());
622 if (previous_lanelets.empty()) //error if bad match
623 {
624 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone area starts from lanelet with no previous lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id()
625 << ". This case is rare and not supported at the moment.");
626 return;
627 }
628
629 // get previous lanelet of affected part of TAPERRIGHT (doesn't matter which previous, as the new lanelet will only be duplicate anyways)
630 auto prev_lanelet_to_copy = current_map_->laneletLayer.get(previous_lanelets.front().id());
631
632 // parallel_llts will have a copy of `prev_lanelet_to_copy` with new id to be used as part of workzone area
633 new_taper_right_llts = splitLaneletWithPoint({prev_lanelet_to_copy.centerline2d().back()}, prev_lanelet_to_copy, error_distance_);
634 }
635 parallel_llts->insert(parallel_llts->end(), new_taper_right_llts.begin(), new_taper_right_llts.end());
636 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished TAPERRIGHT processing of size: " << new_taper_right_llts.size());
637
641 std::vector <lanelet::Lanelet> new_open_right_llts;
642 auto open_right_last_pt = work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d();
643 auto open_right_last_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id());
644
645 new_open_right_llts = splitLaneletWithPoint({open_right_last_pt}, open_right_last_llt, error_distance_);
646 double check_dist_opr = lanelet::geometry::distance2d(open_right_last_llt.centerline2d().back().basicPoint2d(), open_right_last_pt);
647
648 // if no splitting happened, we need to create duplicate of next lanelet of OPENRIGHT's last lanelet
649 // to match the output expected of this function as if split happened
650 if (new_open_right_llts.size() == 1 && check_dist_opr <= error_distance_)
651 {
652 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating duplicate lanelet of 'next lanelet' due to OPENRIGHT using entire lanelet...");
653 auto next_lanelets = current_routing_graph_->following(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get());
654 if (next_lanelets.empty()) //error if bad match
655 {
656 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone area ends at lanelet with no following lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id()
657 << ". This case is rare and not supported at the moment.");
658 return;
659 }
660
661 // get next lanelet of affected part of OPENRIGHT (doesn't matter which next, as the new lanelet will only be duplicate anyways)
662 auto next_lanelet_to_copy = current_map_->laneletLayer.get(next_lanelets.front().id());
663
664 // parallel_llts will have a copy of `next_lanelet_to_copy` with new id to be used as part of workzone area
665 new_open_right_llts = splitLaneletWithPoint({next_lanelet_to_copy.centerline2d().back()}, next_lanelet_to_copy, error_distance_);
666 }
667 parallel_llts->insert(parallel_llts->end(), new_open_right_llts.begin(), new_open_right_llts.end());
668 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished OPENRIGHT processing of size: " << new_open_right_llts.size());
669
673
674 auto reverse_back_llts = carma_wm::query::getLaneletsFromPoint(current_map_, work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d());
675 auto reverse_back_llt = reverse_back_llts[0];
676 auto reverse_front_llts = carma_wm::query::getLaneletsFromPoint(current_map_, work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d());
677 auto reverse_front_llt = reverse_front_llts[0];
678
679 if (reverse_back_llt.id() == reverse_front_llt.id()) //means there is only 1 middle lanelet, which needs to be split into 3 lanelets
680 {
681 std::vector<lanelet::Lanelet> temp_llts;
682 temp_llts = splitLaneletWithPoint({work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(),
683 work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d()},
684 current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id()), error_distance_);
685
686 if (temp_llts.size() < 2) // if there is only 1 lanelet
687 {
688 // we found what we want, so return
689 opposite_llts->insert(opposite_llts->end(), temp_llts.begin(), temp_llts.end());
690 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size());
691 return;
692 }
693 else if (temp_llts.size() == 2) // determine which
694 {
695 // back gap is bigger than front's, we should lose front lanelet from the two
696 if (lanelet::geometry::distance2d(work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d(), reverse_front_llt.centerline2d().back().basicPoint2d()) >
697 lanelet::geometry::distance2d(work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(), reverse_front_llt.centerline2d().front().basicPoint2d()))
698 {
699 opposite_llts->push_back(temp_llts.back());
700 }
701 else
702 {
703 opposite_llts->push_back(temp_llts.front());
704 }
705 }
706 else if (temp_llts.size() == 3) // leave only middle lanelets from 3
707 {
708 opposite_llts->insert(opposite_llts->end(), temp_llts.begin() + 1, temp_llts.end()- 1);
709 }
710 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished REVERSE processing of size: " << opposite_llts->size() << " from original of 1 REVERSE lanelet size");
711 }
712 else //if there are two or more lanelets
713 {
715 auto reverse_first_pt = work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d();
716 auto reverse_first_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id());
717 std::vector<lanelet::Lanelet> temp_opposite_front_llts;
718 temp_opposite_front_llts = splitLaneletWithPoint( {reverse_first_pt}, reverse_first_llt, error_distance_);
719 if (temp_opposite_front_llts.size() > 1)
720 {
721 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin() + 1, temp_opposite_front_llts.end());
722 }
723 else
724 {
725 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin(), temp_opposite_front_llts.end());
726 }
728 if (work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.size() > 2)
729 {
730 for (int i = 1; i < work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.size() - 1; i ++)
731 {
732 opposite_llts->push_back(current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_[i].id()));
733 }
734 }
736
737 auto reverse_last_pt = work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d();
738 auto reverse_last_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.back().lanelet().get().id());
739 std::vector<lanelet::Lanelet> temp_opposite_back_llts;
740 temp_opposite_back_llts = splitLaneletWithPoint({reverse_last_pt}, reverse_last_llt, error_distance_);
741 if (temp_opposite_back_llts.size() > 1)
742 {
743 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end()- 1);
744 }
745 else
746 {
747 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end());
748 }
749 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished REVERSE processing of size: " << opposite_llts->size() << " from original of more than one REVERSE lanelet size");
750 }
751
752 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size());
753}
754
755std::vector<lanelet::Lanelet> WMBroadcaster::splitLaneletWithPoint(const std::vector<lanelet::BasicPoint2d>& input_pts, const lanelet::Lanelet& input_llt, double error_distance)
756{
757 // get ratio of this point and split
758 std::vector<lanelet::Lanelet> parallel_llts;
759 double llt_downtrack = carma_wm::geometry::trackPos(input_llt, input_llt.centerline().back().basicPoint2d()).downtrack;
760 std::vector<double> ratios;
761
762 for (auto pt : input_pts)
763 {
764 double point_downtrack = carma_wm::geometry::trackPos(input_llt, pt).downtrack;
765 double point_downtrack_ratio = point_downtrack / llt_downtrack;
766 ratios.push_back(point_downtrack_ratio);
767 }
768
769 auto new_parallel_llts = splitLaneletWithRatio(ratios, input_llt, error_distance);
770
771 parallel_llts.insert(parallel_llts.end(),new_parallel_llts.begin(), new_parallel_llts.end());
772 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithPoint returning lanelets size: " << parallel_llts.size());
773 return parallel_llts;
774}
775
776lanelet::Lanelets WMBroadcaster::splitOppositeLaneletWithPoint(std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts, const lanelet::BasicPoint2d& input_pt, const lanelet::Lanelet& input_llt, double error_distance)
777{
778 // get ratio of this point and split
779 auto point_downtrack = carma_wm::geometry::trackPos(input_llt, input_pt).downtrack;
780 auto point_downtrack_ratio = point_downtrack / carma_wm::geometry::trackPos(input_llt, input_llt.centerline().back().basicPoint2d()).downtrack;
781
782 // get opposing lanelets and split
783 auto opposing_llts = carma_wm::query::nonConnectedAdjacentLeft(current_map_, input_pt);
784
785 if (opposing_llts.empty())
786 {
787 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster was not able to find opposing lane for given point in geofence related to Work Zone! Returning");
788 return {};
789 }
790
791 auto new_llts_opposite = splitLaneletWithRatio({1 - point_downtrack_ratio}, opposing_llts[0], error_distance);
792 opposite_llts->insert(opposite_llts->begin(),new_llts_opposite.begin(), new_llts_opposite.end());
793 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitOppositeLaneletWithPoint returning lanelets size: " << opposite_llts->size());
794 return opposing_llts;
795}
796
797std::vector<lanelet::Lanelet> WMBroadcaster::splitLaneletWithRatio(std::vector<double> ratios, lanelet::Lanelet input_lanelet, double error_distance) const
798{
799 if (!current_map_ || current_map_->laneletLayer.size() == 0)
800 {
801 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
802 }
803 if (ratios.empty())
804 {
805 throw lanelet::InvalidInputError(std::string("Ratios list is empty! Cannot split"));
806 }
807
808 std::vector<lanelet::Lanelet> created_llts;
809
810 std::sort(ratios.begin(), ratios.end());
811 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio evaluating input ratios of size: " << ratios.size());
812
813 ratios.push_back(1.0); //needed to complete the loop
814
815 int left_ls_size = input_lanelet.leftBound2d().size();
816 int right_ls_size = input_lanelet.rightBound2d().size();
817
818 int left_next_pt_idx = 0;
819 int left_prev_pt_idx = 0;
820 int right_next_pt_idx = 0;
821 int right_prev_pt_idx = 0;
822 for (int i = 0 ; i < ratios.size(); i ++)
823 {
824 left_next_pt_idx = std::round(ratios[i] * (left_ls_size - 1));
825 right_next_pt_idx = std::round(ratios[i] * (right_ls_size - 1));
826 // check if edge ratios are too close to any boundaries. if so, skip
827 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
828 {
829 // assuming both linestrings have roughly the same number of points and
830 // assuming distance between 0th and index-th points are small enough we can approximate the curve between them as a line:
831 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ratio: " << ratios[i] << ", is too close to the lanelet's front boundary! Therefore, ignoring... Allowed error_distance: " << error_distance << ", Distance: "
832 << lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
833 continue;
834 }
835 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
836 {
837 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ratio: " << ratios[i] << ", is too close to the lanelet's back boundary! Therefore, ignoring... Allowed error_distance: " << error_distance << ", Distance: "
838 << lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
839
840 left_next_pt_idx = left_ls_size - 1;
841 right_next_pt_idx = right_ls_size - 1;
842 }
843 // create lanelet
844 std::vector<lanelet::Point3d> left_pts;
845 left_pts.insert(left_pts.end(), current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_prev_pt_idx, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_next_pt_idx + 1);
846
847 lanelet::LineString3d left_ls(lanelet::utils::getId(), left_pts, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().attributes());
848
849 std::vector<lanelet::Point3d> right_pts;
850 right_pts.insert(right_pts.end(), current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_prev_pt_idx, current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_next_pt_idx + 1);
851
852 lanelet::LineString3d right_ls(lanelet::utils::getId(), right_pts);
853
854 lanelet::Lanelet new_llt (lanelet::utils::getId(), left_ls, right_ls, current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().attributes());
855
856 for (auto regem : current_map_->laneletLayer.get(input_lanelet.id()).regulatoryElements()) //copy existing regem into new llts
857 {
858 new_llt.addRegulatoryElement(current_map_->regulatoryElementLayer.get(regem->id()));
859 }
860
861 left_prev_pt_idx = left_next_pt_idx;
862 right_prev_pt_idx = right_next_pt_idx;
863
864 created_llts.push_back(new_llt);
865
866 // Detected the end already. Exiting now
867 if (left_prev_pt_idx == left_ls_size - 1 || right_prev_pt_idx == right_ls_size - 1)
868 {
869 break;
870 }
871
872 }
873
874 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio returning lanelets size: " << created_llts.size());
875
876 return created_llts;
877}
878
879
880void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector<lanelet::Lanelet>& affected_llts) const
881{
882 carma_v2x_msgs::msg::TrafficControlDetail msg_detail;
883 msg_detail = msg_v01.params.detail;
884 // Get affected bounds
885 lanelet::LineStrings3d pcl_bounds;
886 if (msg_detail.lataffinity == carma_v2x_msgs::msg::TrafficControlDetail::LEFT)
887 {
888 for (auto llt : affected_llts) pcl_bounds.push_back(llt.leftBound());
889 gf_ptr->pcl_affects_left_ = true;
890 }
891 else //right
892 {
893 for (auto llt : affected_llts) pcl_bounds.push_back(llt.rightBound());
894 gf_ptr->pcl_affects_right_ = true;
895 }
896
897 // Get specified participants
898 std::vector<std::string> left_participants;
899 std::vector<std::string> right_participants;
900 std::vector<std::string> participants=participantsChecker(msg_v01);
901
902 // Create the pcl depending on the allowed passing control direction, left, right, or both
903 if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
904 msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
905 {
906 left_participants = participants;
907 }
908 else if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
909 {
910 left_participants.push_back(lanelet::Participants::VehicleEmergency);
911 }
912 if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
913 msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
914 {
915 right_participants = participants;
916 }
917 else if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
918 {
919 right_participants.push_back(lanelet::Participants::VehicleEmergency);
920 }
921
922 gf_ptr->regulatory_element_ = std::make_shared<lanelet::PassingControlLine>(lanelet::PassingControlLine::buildData(
923 lanelet::utils::getId(), pcl_bounds, left_participants, right_participants));
924}
925
926void WMBroadcaster::addRegionAccessRule(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector<lanelet::Lanelet>& affected_llts) const
927{
928 const std::string& reason = msg_v01.package.label;
929 gf_ptr->label_ = msg_v01.package.label;
930 auto regulatory_element = std::make_shared<lanelet::RegionAccessRule>(lanelet::RegionAccessRule::buildData(lanelet::utils::getId(),affected_llts,{},invertParticipants(participantsChecker(msg_v01)), reason));
931
932 if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck ))
933 {
934 gf_ptr->invalidate_route_=true;
935 }
936 else
937 {
938 gf_ptr->invalidate_route_=false;
939 }
940 gf_ptr->regulatory_element_ = regulatory_element;
941}
942
943void WMBroadcaster::addRegionMinimumGap(std::shared_ptr<Geofence> gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector<lanelet::Lanelet>& affected_llts, const std::vector<lanelet::Area>& affected_areas) const
944{
945 auto regulatory_element = std::make_shared<lanelet::DigitalMinimumGap>(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(),
946 min_gap, affected_llts, affected_areas,participantsChecker(msg_v01) ));
947
948 gf_ptr->regulatory_element_ = regulatory_element;
949}
950
951std::vector<std::string> WMBroadcaster::participantsChecker(const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01) const
952{
953 std::vector<std::string> participants;
954 for (j2735_v2x_msgs::msg::TrafficControlVehClass participant : msg_v01.params.vclasses)
955 {
956 // Currently j2735_v2x_msgs::msg::TrafficControlVehClass::RAIL is not supported
957 if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::ANY)
958 {
959 participants = {lanelet::Participants::Vehicle, lanelet::Participants::Pedestrian, lanelet::Participants::Bicycle};
960 break;
961 }
962 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN)
963 {
964 participants.push_back(lanelet::Participants::Pedestrian);
965 }
966 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BICYCLE)
967 {
968 participants.push_back(lanelet::Participants::Bicycle);
969 }
970 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE ||
971 participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MOTORCYCLE)
972 {
973 participants.push_back(lanelet::Participants::VehicleMotorcycle);
974 }
975 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BUS)
976 {
977 participants.push_back(lanelet::Participants::VehicleBus);
978 }
979 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::LIGHT_TRUCK_VAN ||
980 participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR)
981 {
982 participants.push_back(lanelet::Participants::VehicleCar);
983 }
984 else if (8<= participant.vehicle_class && participant.vehicle_class <= 16) // Truck enum definition range from 8-16 currently
985 {
986 participants.push_back(lanelet::Participants::VehicleTruck);
987 }
988 }
989 // combine to single vehicle type if possible, otherwise pass through
990 return combineParticipantsToVehicle(participants);
991}
992
993std::vector<std::string> WMBroadcaster::invertParticipants(const std::vector<std::string>& input_participants) const
994{
995 std::vector<std::string> participants;
996
997 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Pedestrian ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Pedestrian);
998 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Bicycle ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Bicycle);
999 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Vehicle ) == input_participants.end())
1000 {
1001 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleMotorcycle);
1002 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleBus);
1003 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleCar);
1004 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleTruck);
1005 }
1006 return participants;
1007}
1008
1009std::vector<std::string> WMBroadcaster::combineParticipantsToVehicle(const std::vector<std::string>& input_participants) const
1010{
1011 std::vector<std::string> participants;
1012
1013 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)!= input_participants.end() &&
1014 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus) != input_participants.end() &&
1015 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar) != input_participants.end() &&
1016 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck) != input_participants.end())
1017 {
1018 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Detected participants to cover all possible vehicle types");
1019 participants.emplace_back(lanelet::Participants::Vehicle);
1020 }
1021 else
1022 {
1023 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Not making any changes to the participants list");
1024 participants = input_participants;
1025 }
1026
1027 return participants;
1028}
1029
1030void WMBroadcaster::externalMapMsgCallback(carma_v2x_msgs::msg::MapData::UniquePtr map_msg)
1031{
1032 auto gf_ptr = std::make_shared<Geofence>();
1033
1034 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1035 {
1036 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Map is not available yet. Skipping MAP msg");
1037 return;
1038 }
1039
1040 // check if we have seen this message already
1041 bool up_to_date = false;
1042 if (sim_->intersection_id_to_regem_id_.size() == map_msg->intersections.size())
1043 {
1044 up_to_date = true;
1045 // check id of the intersection only
1046 for (auto intersection : map_msg->intersections)
1047 {
1048 if (sim_->intersection_id_to_regem_id_.find(intersection.id.id) == sim_->intersection_id_to_regem_id_.end())
1049 {
1050 up_to_date = false;
1051 break;
1052 }
1053 }
1054 }
1055
1056 if(up_to_date)
1057 {
1058 return;
1059 }
1060
1061 gf_ptr->map_msg_ = *map_msg;
1062 gf_ptr->msg_.package.label_exists = true;
1063 gf_ptr->msg_.package.label = "MAP_MSG";
1064 gf_ptr->id_ = boost::uuids::random_generator()();
1065
1066 // create dummy traffic Control message to add instant activation schedule
1067 carma_v2x_msgs::msg::TrafficControlMessageV01 traffic_control_msg;
1068
1069 // process schedule from message
1070 addScheduleFromMsg(gf_ptr, traffic_control_msg);
1071
1072 scheduleGeofence(gf_ptr);
1073
1074}
1075
1076// currently only supports geofence message version 1: TrafficControlMessageV01
1077void WMBroadcaster::geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr geofence_msg)
1078{
1079
1080 std::lock_guard<std::mutex> guard(map_mutex_);
1081 std::stringstream reason_ss;
1082 // quickly check if the id has been added
1083 if (geofence_msg->choice != carma_v2x_msgs::msg::TrafficControlMessage::TCMV01) {
1084 reason_ss << "Dropping received geofence for unsupported TrafficControl version: " << geofence_msg->choice;
1085 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str());
1086 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::REJECTED), reason_ss.str());
1087 return;
1088 }
1089
1090 boost::uuids::uuid id;
1091 std::copy(geofence_msg->tcm_v01.id.id.begin(), geofence_msg->tcm_v01.id.id.end(), id.begin());
1093 reason_ss.str("");
1094 reason_ss << "Dropping received TrafficControl message with already handled id: " << boost::uuids::to_string(id);
1095 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str());
1096 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str());
1097 return;
1098 }
1099
1100 // convert reqid to string check if it has been seen before
1101 boost::array<uint8_t, 16UL> req_id;
1102 for (auto i = 0; i < 8; i ++) req_id[i] = geofence_msg->tcm_v01.reqid.id[i];
1103 boost::uuids::uuid uuid_id;
1104 std::copy(req_id.begin(),req_id.end(), uuid_id.begin());
1105 std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
1106 // drop if the req has never been sent
1107 if (generated_geofence_reqids_.find(reqid) == generated_geofence_reqids_.end() && reqid.compare("00000000") != 0)
1108 {
1109 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid);
1110 return;
1111 }
1112
1114
1115 auto gf_ptr = std::make_shared<Geofence>();
1116
1117 gf_ptr->msg_ = geofence_msg->tcm_v01;
1118
1119 try
1120 {
1121 // process schedule from message
1122 addScheduleFromMsg(gf_ptr, geofence_msg->tcm_v01);
1123 scheduleGeofence(gf_ptr);
1124 reason_ss.str("");
1125 reason_ss << "Successfully processed TCM.";
1126 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str());
1127 }
1128 catch(std::exception& ex)
1129 {
1130 reason_ss.str("");
1131 reason_ss << "Failed to process TCM. " << ex.what();
1132 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::REJECTED), reason_ss.str());
1133 throw; //rethrows the exception object
1134 }
1135};
1136
1137void WMBroadcaster::scheduleGeofence(std::shared_ptr<carma_wm_ctrl::Geofence> gf_ptr)
1138{
1139 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
1140
1141 bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos;
1142
1143 carma_v2x_msgs::msg::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail;
1144
1145 // create workzone specific extra speed geofence
1146 if (detected_workzone_signal && msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1147 {
1148 // duplicate the messages with inverted points to support new lanelets created from workzone
1149 // as carma-cloud currently does not support geofence points with direction opposite to that of the road
1150
1151 auto gf_ptr_speed = std::make_shared<Geofence>();
1152 gf_ptr_speed->schedules = gf_ptr->schedules;
1153
1154 carma_v2x_msgs::msg::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_;
1155
1156 std::reverse(duplicate_msg.geometry.nodes.begin() + 1, duplicate_msg.geometry.nodes.end());
1157 double first_x = 0;
1158 double first_y = 0;
1159
1160 // this fancy logic is needed as each node is expressed as an offset from the last one
1161 for (auto& pt: duplicate_msg.geometry.nodes)
1162 {
1163 first_x+= pt.x;
1164 first_y+= pt.y;
1165 pt.x = -1* pt.x;
1166 pt.y = -1* pt.y;
1167 }
1168 duplicate_msg.geometry.nodes[0].x = first_x;
1169 duplicate_msg.geometry.nodes[0].y = first_y;
1170
1171 gf_ptr_speed->msg_ = duplicate_msg;
1172 scheduler_.addGeofence(gf_ptr_speed);
1173 }
1174 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later
1175 {
1176 gf_ptr->label_ = gf_ptr->msg_.package.label; // to extract intersection, and signal group id
1177 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
1178 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
1179 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
1180 {
1181 work_zone_geofence_cache_[msg_detail.closed] = gf_ptr;
1182 }
1183 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
1184 {
1186 }
1188 {
1189 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now...");
1190 return;
1191 }
1192 }
1193
1194 scheduler_.addGeofence(gf_ptr); // Add the geofence to the scheduler
1195
1196}
1197
1198void WMBroadcaster::geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_ref)
1199{
1200 std::lock_guard<std::mutex> guard(map_mutex_);
1201 sim_->setTargetFrame(geo_ref->data);
1202 base_map_georef_ = geo_ref->data;
1203}
1204
1205void WMBroadcaster::setMaxLaneWidth(double max_lane_width)
1206{
1207 sim_ = std::make_shared<carma_wm::SignalizedIntersectionManager>();
1208
1209 max_lane_width_ = max_lane_width;
1210 sim_->setMaxLaneWidth(max_lane_width_);
1211}
1212
1213void WMBroadcaster::setIntersectionCoordCorrection(const std::vector<int64_t>& intersection_ids_for_correction, const std::vector<double>& intersection_correction)
1214{
1215 if (intersection_correction.size() % 2 != 0 || intersection_ids_for_correction.size() != intersection_correction.size() / 2)
1216 {
1217 throw std::invalid_argument("Some of intersection coordinate correction parameters are not fully set!");
1218 }
1219
1220 for (auto i = 0; i < intersection_correction.size(); i = i + 2)
1221 {
1222 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].first = intersection_correction[i]; //x
1223 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].second = intersection_correction[i + 1]; //y
1224 }
1225
1226}
1227
1229{
1230 /*Logic to change config_lim to Velocity value config_limit*/
1231 config_limit = lanelet::Velocity(cL * lanelet::units::MPH());
1232}
1233
1234void WMBroadcaster::setConfigVehicleId(const std::string& vehicle_id){
1235 vehicle_id_ = vehicle_id;
1236}
1237
1239 ack_pub_times_ = ack_pub_times;
1240}
1241
1243{
1244 participant_ = participant;
1245}
1246
1248{
1249 return participant_;
1250}
1251
1252uint32_t WMBroadcaster::generate32BitId(const std::string& label)
1253{
1254 auto pos1 = label.find("INT_ID:") + 7;
1255 auto pos2 = label.find("SG_ID:") + 6;
1256
1257 uint16_t intersection_id = std::stoi(label.substr(pos1 , 4));
1258 uint8_t signal_id = std::stoi(label.substr(pos2 , 3));
1259
1260 return carma_wm::utils::get32BitId(intersection_id, signal_id);
1261}
1262
1263// currently only supports geofence message version 1: TrafficControlMessageV01
1264lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01& tcm_v01)
1265{
1266 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Getting affected lanelets");
1267 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1268 {
1269 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
1270 }
1271 if (base_map_georef_ == "")
1272 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map has empty proj string loaded as georeference. Therefore, WMBroadcaster failed to\n ") +
1273 std::string("get transformation between the geofence and the map"));
1274
1275 // This next section handles the geofence projection conversion
1276 // The datum field is used to identify the frame for the provided referance lat/lon.
1277 // This reference is then converted to the provided projection as a reference origin point
1278 // From the reference the message projection to map projection transformation is used to convert the nodes in the TrafficControlMessage
1279 std::string projection = tcm_v01.geometry.proj;
1280 std::string datum = tcm_v01.geometry.datum;
1281 if (datum.empty()) {
1282 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field not populated. Attempting to use WGS84");
1283 datum = "WGS84";
1284 }
1285
1286 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Projection field: " << projection);
1287 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field: " << datum);
1288
1289 std::string universal_frame = datum; //lat/long included in TCM is in this datum
1290
1291
1292 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Traffic Control heading provided: " << tcm_v01.geometry.heading << " System understanding is that this value will not affect the projection and is only provided for supporting derivative calculations.");
1293
1294 // Create the resulting projection transformation
1295 PJ* universal_to_target = proj_create_crs_to_crs(PJ_DEFAULT_CTX, universal_frame.c_str(), projection.c_str(), nullptr);
1296 if (universal_to_target == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
1297
1298 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1299 << " universal_frame: " << universal_frame << " projection: " << projection);
1300
1301 return {}; // Ignore geofence if it could not be projected from universal to TCM frame
1302 }
1303
1304 PJ* target_to_map = proj_create_crs_to_crs(PJ_DEFAULT_CTX, projection.c_str(), base_map_georef_.c_str(), nullptr);
1305
1306 if (target_to_map == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
1307
1308 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1309 << " target_to_map: " << target_to_map << " base_map_georef_: " << base_map_georef_);
1310
1311 return {}; // Ignore geofence if it could not be projected into the map frame
1312
1313 }
1314
1315 // convert all geofence points into our map's frame
1316 std::vector<lanelet::Point3d> gf_pts;
1317 carma_v2x_msgs::msg::PathNode prev_pt;
1318 PJ_COORD c_init_latlong{{tcm_v01.geometry.reflat, tcm_v01.geometry.reflon, tcm_v01.geometry.refelv}};
1319 PJ_COORD c_init = proj_trans(universal_to_target, PJ_FWD, c_init_latlong);
1320
1321 prev_pt.x = c_init.xyz.x;
1322 prev_pt.y = c_init.xyz.y;
1323
1324 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "In TCM's frame, initial Point X "<< prev_pt.x<<" Before conversion: Point Y "<< prev_pt.y );
1325 for (auto pt : tcm_v01.geometry.nodes)
1326 {
1327 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion in TCM frame: Point X "<< pt.x <<" Before conversion: Point Y "<< pt.y);
1328
1329 PJ_COORD c {{prev_pt.x + pt.x, prev_pt.y + pt.y, 0, 0}}; // z is not currently used
1330 PJ_COORD c_out;
1331 c_out = proj_trans(target_to_map, PJ_FWD, c);
1332
1333 gf_pts.push_back(lanelet::Point3d{current_map_->pointLayer.uniqueId(), c_out.xyz.x, c_out.xyz.y});
1334 prev_pt.x += pt.x;
1335 prev_pt.y += pt.y;
1336
1337 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion in Map frame: Point X "<< gf_pts.back().x() <<" After conversion: Point Y "<< gf_pts.back().y());
1338 }
1339
1340 // save the points converted to local map frame
1341 return gf_pts;
1342}
1343
1344lanelet::ConstLaneletOrAreas WMBroadcaster::getAffectedLaneletOrAreas(const lanelet::Points3d& gf_pts)
1345{
1347}
1348
1357bool WMBroadcaster::shouldChangeControlLine(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr<Geofence> gf_ptr) const
1358{
1359 // should change if if the regem is not a passing control line or area, which is not supported by this logic
1360 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) != 0 || !el.isLanelet())
1361 {
1362 return true;
1363 }
1364
1365 lanelet::PassingControlLinePtr pcl = std::dynamic_pointer_cast<lanelet::PassingControlLine>(current_map_->regulatoryElementLayer.get(regem->id()));
1366 // if this geofence's pcl doesn't match with the lanelets current bound side, return false as we shouldn't change
1367 bool should_change_pcl = false;
1368 for (auto control_line : pcl->controlLine())
1369 {
1370 if ((control_line.id() == el.lanelet()->leftBound2d().id() && gf_ptr->pcl_affects_left_) ||
1371 (control_line.id() == el.lanelet()->rightBound2d().id() && gf_ptr->pcl_affects_right_))
1372 {
1373 should_change_pcl = true;
1374 break;
1375 }
1376 }
1377 return should_change_pcl;
1378}
1379
1388bool WMBroadcaster::shouldChangeTrafficSignal(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr<carma_wm::SignalizedIntersectionManager> sim) const
1389{
1390 // should change if the regem is not a CarmaTrafficSignal, which is not supported by this logic
1391 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::CarmaTrafficSignal::RuleName) != 0 || !el.isLanelet() || !sim_)
1392 {
1393 return true;
1394 }
1395
1396 lanelet::CarmaTrafficSignalPtr traffic_signal = std::dynamic_pointer_cast<lanelet::CarmaTrafficSignal>(current_map_->regulatoryElementLayer.get(regem->id()));
1397 uint8_t signal_id = 0;
1398
1399 for (auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it)
1400 {
1401 if (regem->id() == it->second)
1402 {
1403 signal_id = it->first;
1404 }
1405 }
1406
1407 if (signal_id == 0) // doesn't exist in the record
1408 return true;
1409
1410 if (sim->signal_group_to_entry_lanelet_ids_[signal_id].find(el.id()) != sim->signal_group_to_entry_lanelet_ids_[signal_id].end())
1411 return false; // signal group's entry lane is still part of the intersection, so don't change
1412
1413 return true;
1414}
1415
1416void WMBroadcaster::addRegulatoryComponent(std::shared_ptr<Geofence> gf_ptr) const
1417{
1418 // First loop is to save the relation between element and regulatory element
1419 // so that we can add back the old one after geofence deactivates
1420 for (auto el: gf_ptr->affected_parts_)
1421 {
1422 for (auto regem : el.regulatoryElements())
1423 {
1424 if (!shouldChangeControlLine(el, regem, gf_ptr) ||
1425 !shouldChangeTrafficSignal(el, regem, sim_))
1426 continue;
1427
1428 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1429 {
1430 lanelet::RegulatoryElementPtr nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
1431 gf_ptr->prev_regems_.push_back(std::make_pair(el.id(), nonconst_regem));
1432 gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
1433 current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
1434 }
1435 }
1436 }
1437
1438
1439 // this loop is also kept separately because previously we assumed
1440 // there was existing regem, but this handles changes to all of the elements
1441 for (auto el: gf_ptr->affected_parts_)
1442 {
1443 // update it with new regem
1444 if (gf_ptr->regulatory_element_->id() != lanelet::InvalId)
1445 {
1446 current_map_->update(current_map_->laneletLayer.get(el.id()), gf_ptr->regulatory_element_);
1447 gf_ptr->update_list_.push_back(std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>(el.id(), gf_ptr->regulatory_element_));
1448 } else {
1449 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Regulatory element with invalid id in geofence cannot be added to the map");
1450 }
1451 }
1452
1453
1454
1455}
1456
1457void WMBroadcaster::addBackRegulatoryComponent(std::shared_ptr<Geofence> gf_ptr) const
1458{
1459 // First loop is to remove the relation between element and regulatory element that this geofence added initially
1460
1461 for (auto el: gf_ptr->affected_parts_)
1462 {
1463 for (auto regem : el.regulatoryElements())
1464 {
1465 if (!shouldChangeControlLine(el, regem, gf_ptr)) continue;
1466
1467 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1468 {
1469 auto nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
1470 gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
1471 current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
1472 }
1473 }
1474 }
1475
1476 // As this gf received is the first gf that was sent in through addGeofence,
1477 // we have prev speed limit information inside it to put them back
1478 for (auto pair : gf_ptr->prev_regems_)
1479 {
1480 if (pair.second->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1481 {
1482 current_map_->update(current_map_->laneletLayer.get(pair.first), pair.second);
1483 gf_ptr->update_list_.push_back(pair);
1484 }
1485 }
1486}
1487
1488void WMBroadcaster::addGeofence(std::shared_ptr<Geofence> gf_ptr)
1489{
1490
1491 std::lock_guard<std::mutex> guard(map_mutex_);
1492 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Adding active geofence to the map with geofence id: " << gf_ptr->id_);
1493
1494 // if applying workzone geometry geofence, utilize workzone chache to create one
1495 // also multiple map updates can be sent from one geofence object
1496 std::vector<std::shared_ptr<Geofence>> updates_to_send;
1497
1498 bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos;
1499 bool detected_map_msg_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("MAP_MSG") != std::string::npos;
1500 if (detected_workzone_signal && gf_ptr->msg_.params.detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1501 {
1502 for (auto gf_cache_ptr : work_zone_geofence_cache_)
1503 {
1504 geofenceFromMsg(gf_cache_ptr.second, gf_cache_ptr.second->msg_);
1505 }
1506 updates_to_send.push_back(createWorkzoneGeofence(work_zone_geofence_cache_));
1507 }
1508 else if (detected_map_msg_signal)
1509 {
1510 updates_to_send = geofenceFromMapMsg(gf_ptr, gf_ptr->map_msg_);
1511 }
1512 else
1513 {
1514 geofenceFromMsg(gf_ptr, gf_ptr->msg_);
1515 updates_to_send.push_back(gf_ptr);
1516 }
1517
1518 for (auto update : updates_to_send)
1519 {
1520 // add marker to rviz
1521 tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(update->gf_pts)); // create visualizer in rviz
1522
1523 if (update->affected_parts_.empty())
1524 continue;
1525
1526 // Process the geofence object to populate update remove lists
1527 addGeofenceHelper(update);
1528
1529 if (!detected_map_msg_signal)
1530 {
1531 for (auto pair : update->update_list_) active_geofence_llt_ids_.insert(pair.first);
1532 }
1533
1534 autoware_lanelet2_msgs::msg::MapBin gf_msg;
1535
1536 // If the geofence invalidates the route graph then recompute the routing graph now that the map has been updated
1537 if (update->invalidate_route_) {
1538
1539 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Rebuilding routing graph after is was invalidated by geofence");
1540
1541 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1542 lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_);
1543 current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
1544
1545 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence");
1546
1547 // Populate routing graph structure
1548 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message");
1549
1550 auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
1551
1552 gf_msg.routing_graph = readable_graph->routingGraphToMsg(participant_);
1553 gf_msg.has_routing_graph = true;
1554
1555 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message");
1556 }
1557
1558
1559 // Publish
1560 auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(update->id_, update->update_list_, update->remove_list_, update->lanelet_additions_));
1561 send_data->traffic_light_id_lookup_ = update->traffic_light_id_lookup_;
1562
1563 if (detected_map_msg_signal && updates_to_send.back() == update) // if last update
1564 {
1565 send_data->sim_ = *sim_;
1566 }
1567
1568 carma_wm::toBinMsg(send_data, &gf_msg);
1569 update_count_++; // Update the sequence count for the geofence messages
1570 gf_msg.seq_id = update_count_;
1571 gf_msg.invalidates_route=update->invalidate_route_;
1572 gf_msg.map_version = current_map_version_;
1573 map_update_pub_(gf_msg);
1574 }
1575
1576}
1577
1578void WMBroadcaster::removeGeofence(std::shared_ptr<Geofence> gf_ptr)
1579{
1580 std::lock_guard<std::mutex> guard(map_mutex_);
1581 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Removing inactive geofence from the map with geofence id: " << gf_ptr->id_);
1582
1583 // Process the geofence object to populate update remove lists
1584 if (gf_ptr->affected_parts_.empty())
1585 return;
1586
1587 removeGeofenceHelper(gf_ptr);
1588
1589 for (auto pair : gf_ptr->remove_list_) active_geofence_llt_ids_.erase(pair.first);
1590
1591 // publish
1592 autoware_lanelet2_msgs::msg::MapBin gf_msg_revert;
1593 auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, {}));
1594
1595 if (gf_ptr->invalidate_route_) { // If a geofence initially invalidated the route it stands to reason its removal should as well
1596
1597 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Rebuilding routing graph after is was invalidated by geofence removal");
1598
1599 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1600 lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_
1601 );
1602 current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
1603
1604 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence removal");
1605
1606 // Populate routing graph structure
1607 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message for geofence removal");
1608
1609 auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
1610
1611 gf_msg_revert.routing_graph = readable_graph->routingGraphToMsg(participant_);
1612 gf_msg_revert.has_routing_graph = true;
1613
1614 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message for geofence removal");
1615 }
1616
1617 carma_wm::toBinMsg(send_data, &gf_msg_revert);
1618 update_count_++; // Update the sequence count for geofence messages
1619 gf_msg_revert.seq_id = update_count_;
1620 gf_msg_revert.map_version = current_map_version_;
1621
1622 map_update_pub_(gf_msg_revert);
1623
1624
1625}
1626
1627carma_planning_msgs::msg::Route WMBroadcaster::getRoute()
1628{
1629 return current_route;
1630}
1631
1632void WMBroadcaster::routeCallbackMessage(carma_planning_msgs::msg::Route::UniquePtr route_msg)
1633{
1634 current_route = *route_msg;
1635 carma_v2x_msgs::msg::TrafficControlRequest cR;
1636 cR = controlRequestFromRoute(*route_msg);
1637 control_msg_pub_(cR);
1638
1639}
1640
1641carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRoute(const carma_planning_msgs::msg::Route& route_msg, std::shared_ptr<j2735_v2x_msgs::msg::Id64b> req_id_for_testing)
1642{
1643 lanelet::ConstLanelets path;
1644
1645 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1646 {
1647 // Return / log warning etc.
1648 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'current_map_' does not exist.");
1649 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
1650
1651 }
1652
1653 for(auto id : route_msg.route_path_lanelet_ids)
1654 {
1655 auto laneLayer = current_map_->laneletLayer.get(id);
1656 path.push_back(laneLayer);
1657 }
1658
1659 // update local copy
1660 route_path_ = path;
1661
1662 if(path.size() == 0) throw lanelet::InvalidObjectStateError(std::string("No lanelets available in path."));
1663
1664 /*logic to determine route bounds*/
1665 std::vector<lanelet::ConstLanelet> llt;
1666 std::vector<lanelet::BoundingBox2d> pathBox;
1667 double minX = std::numeric_limits<double>::max();
1668 double minY = std::numeric_limits<double>::max();
1669 double maxX = std::numeric_limits<double>::lowest();
1670 double maxY = std::numeric_limits<double>::lowest();
1671
1672 while (path.size() != 0) //Continue until there are no more lanelet elements in path
1673 {
1674 llt.push_back(path.back()); //Add a lanelet to the vector
1675
1676
1677 pathBox.push_back(lanelet::geometry::boundingBox2d(llt.back())); //Create a bounding box of the added lanelet and add it to the vector
1678
1679
1680 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x() < minX)
1681 minX = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x(); //minimum x-value
1682
1683
1684 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y() < minY)
1685 minY = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y(); //minimum y-value
1686
1687
1688
1689 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x() > maxX)
1690 maxX = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x(); //maximum x-value
1691
1692
1693 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y() > maxY)
1694 maxY = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y(); //maximum y-value
1695
1696
1697 path.pop_back(); //remove the added lanelet from path an reduce pack.size() by 1
1698 } //end of while loop
1699
1700
1701 std::string target_frame = base_map_georef_;
1702 if (target_frame.empty())
1703 {
1704 // Return / log warning etc.
1705 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'target_frame' is empty.");
1706 throw lanelet::InvalidObjectStateError(std::string("Base georeference map may not be loaded to the WMBroadcaster"));
1707
1708 }
1709
1710 // Convert the minimum point to latlon
1711 lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
1712 lanelet::BasicPoint3d localPoint;
1713
1714 localPoint.x()= minX;
1715 localPoint.y()= minY;
1716
1717 lanelet::GPSPoint gpsRoute = local_projector.reverse(localPoint); //If the appropriate library is included, the reverse() function can be used to convert from local xyz to lat/lon
1718
1719 // Create a local transverse mercator frame at the minimum point to allow us to get east,north oriented bounds
1720 std::string local_tmerc_enu_proj = "+proj=tmerc +datum=WGS84 +h_0=0 +lat_0=" + std::to_string(gpsRoute.lat) + " +lon_0=" + std::to_string(gpsRoute.lon);
1721
1722 // Create transform from map frame to local transform mercator frame at bounds min point
1723 PJ* tmerc_proj = proj_create_crs_to_crs(PJ_DEFAULT_CTX, target_frame.c_str(), local_tmerc_enu_proj.c_str(), nullptr);
1724
1725 if (tmerc_proj == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
1726
1727 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between request bounds frame and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1728 << " MapProjection: " << target_frame << " Message Projection: " << local_tmerc_enu_proj);
1729
1730 return {}; // Ignore geofence if it could not be projected into the map frame
1731
1732 }
1733
1734 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Top Left: ("<< minX <<", "<<maxY<<")");
1735 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Top Right: ("<< maxX <<", "<<maxY<<")");
1736 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Bottom Left: ("<< minX <<", "<<minY<<")");
1737 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Bottom Right: ("<< maxX <<", "<<minY<<")");
1738
1739 PJ_COORD pj_min {{minX, minY, 0, 0}}; // z is not currently used
1740 PJ_COORD pj_min_tmerc;
1741 PJ_COORD pj_max {{maxX, maxY, 0, 0}}; // z is not currently used
1742 PJ_COORD pj_max_tmerc;
1743 pj_min_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_min);
1744 pj_max_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_max);
1745
1746 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion: MinPoint ( "<< pj_min_tmerc.xyz.x <<", " << pj_min_tmerc.xyz.y <<" )");
1747 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion: MaxPoint ( "<< pj_max_tmerc.xyz.x <<", " << pj_max_tmerc.xyz.y <<" )");
1748
1749 carma_v2x_msgs::msg::TrafficControlRequest cR; /*Fill the latitude value in message cB with the value of lat */
1750 carma_v2x_msgs::msg::TrafficControlBounds cB; /*Fill the longitude value in message cB with the value of lon*/
1751
1752 cB.reflat = gpsRoute.lat;
1753 cB.reflon = gpsRoute.lon;
1754
1755 cB.offsets[0].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x; // Points in clockwise order min,min (lat,lon point) -> max,min -> max,max -> min,max
1756 cB.offsets[0].deltay = 0.0; // calculating the offsets
1757 cB.offsets[1].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x;
1758 cB.offsets[1].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
1759 cB.offsets[2].deltax = 0.0;
1760 cB.offsets[2].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
1761
1762 tcr_polygon_ = composeTCRStatus(localPoint, cB, local_projector); // TCR polygon can be visualized in UI
1763
1764 cB.oldest =rclcpp::Time(0.0,0.0, scheduler_.getClockType()); // TODO this needs to be set to 0 or an older value as otherwise this will filter out all controls
1765
1766 cR.choice = carma_v2x_msgs::msg::TrafficControlRequest::TCRV01;
1767
1768 // create 16 byte uuid
1769 boost::uuids::uuid uuid_id = boost::uuids::random_generator()();
1770 // take half as string
1771 std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
1772 std::string req_id_test = "12345678"; // TODO this is an extremely risky way of performing a unit test. This method needs to be refactored so that unit tests cannot side affect actual implementations
1773 generated_geofence_reqids_.insert(req_id_test);
1774 generated_geofence_reqids_.insert(reqid);
1775
1776
1777 // copy to reqid array
1778 boost::array<uint8_t, 16UL> req_id;
1779 std::copy(uuid_id.begin(),uuid_id.end(), req_id.begin());
1780 for (auto i = 0; i < 8; i ++)
1781 {
1782 cR.tcr_v01.reqid.id[i] = req_id[i];
1783 if (req_id_for_testing) req_id_for_testing->id[i] = req_id[i];
1784 }
1785
1786 cR.tcr_v01.bounds.push_back(cB);
1787
1788 return cR;
1789
1790}
1791
1792carma_v2x_msgs::msg::TrafficControlRequestPolygon WMBroadcaster::composeTCRStatus(const lanelet::BasicPoint3d& localPoint, const carma_v2x_msgs::msg::TrafficControlBounds& cB, const lanelet::projection::LocalFrameProjector& local_projector)
1793{
1794 carma_v2x_msgs::msg::TrafficControlRequestPolygon output;
1795 lanelet::BasicPoint3d local_point_tmp;
1796
1797 int i = -1;
1798 while (i < 3) // three offsets; offsets.size() doesn't return accurate size
1799 {
1800 if (i == -1)
1801 {
1802 local_point_tmp.x() = localPoint.x();
1803 local_point_tmp.y() = localPoint.y();
1804 }
1805 else
1806 {
1807 local_point_tmp.x() = localPoint.x() + cB.offsets[i].deltax;;
1808 local_point_tmp.y() = localPoint.y() + cB.offsets[i].deltay;;
1809 }
1810 lanelet::GPSPoint gps_vertex = local_projector.reverse(local_point_tmp);
1811
1812 carma_v2x_msgs::msg::Position3D gps_msg;
1813 gps_msg.elevation = gps_vertex.ele;
1814 gps_msg.latitude = gps_vertex.lat;
1815 gps_msg.longitude = gps_vertex.lon;
1816 output.polygon_list.push_back(gps_msg);
1817
1818 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "TCR Vertex Lat: "<< std::to_string(gps_vertex.lat));
1819 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "TCR Vertex Lon: "<<std::to_string(gps_vertex.lon));
1820
1821 i++;
1822 }
1823 return output;
1824}
1825
1826visualization_msgs::msg::Marker WMBroadcaster::composeTCMMarkerVisualizer(const std::vector<lanelet::Point3d>& input)
1827 {
1828
1829 // create the marker msgs
1830 visualization_msgs::msg::Marker marker;
1831 marker.header.frame_id = "map";
1832 marker.header.stamp = rclcpp::Time();
1833 marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
1834 marker.action = visualization_msgs::msg::Marker::ADD;
1835 marker.ns = "route_visualizer";
1836
1837 marker.scale.x = 0.65;
1838 marker.scale.y = 0.65;
1839 marker.scale.z = 0.65;
1840 marker.frame_locked = true;
1841
1842 if (!tcm_marker_array_.markers.empty())
1843 {
1844 marker.id = tcm_marker_array_.markers.back().id + 1;
1845 }
1846 else
1847 {
1848 marker.id = 0;
1849 }
1850 marker.color.r = 0.0F;
1851 marker.color.g = 1.0F;
1852 marker.color.b = 0.0F;
1853 marker.color.a = 1.0F;
1854
1855 for (int i = 0; i < input.size(); i++)
1856 {
1857 geometry_msgs::msg::Point temp_point;
1858 temp_point.x = input[i].x();
1859 temp_point.y = input[i].y();
1860 temp_point.z = 2; //to show up on top of the lanelet lines
1861
1862 marker.points.push_back(temp_point);
1863 }
1864
1865 return marker;
1866 }
1867
1868double WMBroadcaster::distToNearestActiveGeofence(const lanelet::BasicPoint2d& curr_pos)
1869{
1870 std::lock_guard<std::mutex> guard(map_mutex_);
1871
1872 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1873 {
1874 throw lanelet::InvalidObjectStateError(std::string("Lanelet map (current_map_) is not loaded to the WMBroadcaster"));
1875 }
1876
1877 // filter only the lanelets in the route
1878 std::vector<lanelet::Id> active_geofence_on_route;
1879 for (auto llt : route_path_)
1880 {
1881 if (active_geofence_llt_ids_.find(llt.id()) != active_geofence_llt_ids_.end())
1882 active_geofence_on_route.push_back(llt.id());
1883 }
1884 // Get the lanelet of this point
1885 auto curr_lanelet = lanelet::geometry::findNearest(current_map_->laneletLayer, curr_pos, 1)[0].second;
1886
1887 // Check if this point at least is actually within this lanelets
1888 if (!boost::geometry::within(curr_pos, curr_lanelet.polygon2d().basicPolygon()))
1889 throw std::invalid_argument("Given point is not within any lanelet");
1890
1891 // get route distance (downtrack + cross_track) distances to every lanelets by their ids
1892 std::vector<double> route_distances;
1893 // and take abs of cross_track to add them to get route distance
1894 for (auto id: active_geofence_on_route)
1895 {
1896 carma_wm::TrackPos tp = carma_wm::geometry::trackPos(current_map_->laneletLayer.get(id), curr_pos);
1897 // downtrack needs to be negative for lanelet to be in front of the point,
1898 // also we don't account for the lanelet that the vehicle is on
1899 if (tp.downtrack < 0 && id != curr_lanelet.id())
1900 {
1901 double dist = fabs(tp.downtrack) + fabs(tp.crosstrack);
1902 route_distances.push_back(dist);
1903 }
1904 }
1905 std::sort(route_distances.begin(), route_distances.end());
1906
1907 if (route_distances.size() != 0 ) return route_distances[0];
1908 else return 0.0;
1909
1910}
1911// helper function that detects the type of geofence and delegates
1912void WMBroadcaster::addGeofenceHelper(std::shared_ptr<Geofence> gf_ptr)
1913{
1914 // resetting the information inside geofence
1915 gf_ptr->remove_list_ = {};
1916 gf_ptr->update_list_ = {};
1917
1918 // add additional lanelets that need to be added
1919 for (auto llt : gf_ptr->lanelet_additions_)
1920 {
1921 current_map_->add(llt);
1922 }
1923
1924 // add trafficlight id mapping
1925 for (auto pair : gf_ptr->traffic_light_id_lookup_)
1926 {
1927 traffic_light_id_lookup_[pair.first] = pair.second;
1928 }
1929
1930 // Logic to determine what type of geofence
1931 addRegulatoryComponent(gf_ptr);
1932}
1933
1934// helper function that detects the type of geofence and delegates
1935void WMBroadcaster::removeGeofenceHelper(std::shared_ptr<Geofence> gf_ptr) const
1936{
1937 // Logic to determine what type of geofence
1938 // reset the info inside geofence
1939 gf_ptr->remove_list_ = {};
1940 gf_ptr->update_list_ = {};
1942 // as all changes are reverted back, we no longer need prev_regems
1943 gf_ptr->prev_regems_ = {};
1944}
1945
1946void WMBroadcaster::currentLocationCallback(geometry_msgs::msg::PoseStamped::UniquePtr current_pos)
1947{
1948 if (current_map_ && current_map_->laneletLayer.size() != 0) {
1949 carma_perception_msgs::msg::CheckActiveGeofence check = checkActiveGeofenceLogic(*current_pos);
1950 active_pub_(check);//Publish
1951 } else {
1952 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Could not check active geofence logic because map was not loaded");
1953 }
1954}
1955
1956bool WMBroadcaster::convertLightIdToInterGroupId(unsigned& intersection_id, unsigned& group_id, const lanelet::Id& lanelet_id)
1957{
1958 for (auto it = traffic_light_id_lookup_.begin(); it != traffic_light_id_lookup_.end(); ++it)
1959 {
1960 // Reverse of the logic for generating the lanelet_id. Reference function generate32BitId(const std::string& label)
1961 if (it -> second == lanelet_id)
1962 {
1963 group_id = (it -> first & 0xFF);
1964 intersection_id = (it -> first >> 8);
1965 return true;
1966 }
1967 }
1968 return false;
1969}
1970
1972{
1973 if (traffic_light_id_lookup_.empty())
1974 {
1975 return;
1976 }
1977 for(auto id : current_route.route_path_lanelet_ids)
1978 {
1979 bool convert_success = false;
1980 unsigned intersection_id = 0;
1981 unsigned group_id = 0;
1982 auto route_lanelet= current_map_->laneletLayer.get(id);
1983 auto traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
1984
1985 if (!traffic_lights.empty())
1986 {
1987 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id());
1988 convert_success = convertLightIdToInterGroupId(intersection_id,group_id, traffic_lights.front()->id());
1989 }
1990
1991 if (!convert_success)
1992 continue;
1993
1994 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light with Intersection id: " << intersection_id << " Group id:" << group_id);
1995 bool id_exists = false;
1996 for (int idx = 0; idx < upcoming_intersection_ids_.data.size(); idx +2)
1997 {
1998 if (upcoming_intersection_ids_.data[idx] == intersection_id && upcoming_intersection_ids_.data[idx + 1] == group_id) //check if already there
1999 {
2000 id_exists = true;
2001 break;
2002 }
2003 }
2004
2005 if (id_exists)
2006 continue;
2007
2008 upcoming_intersection_ids_.data.push_back(static_cast<int>(intersection_id));
2009 upcoming_intersection_ids_.data.push_back(static_cast<int>(group_id));
2010 }
2011}
2012carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped& current_pos)
2013{
2014
2015 if (!current_map_ || current_map_->laneletLayer.size() == 0)
2016 {
2017 throw lanelet::InvalidObjectStateError(std::string("Lanelet map 'current_map_' is not loaded to the WMBroadcaster"));
2018 }
2019
2020 // Store current position values to be compared to geofence boundary values
2021 double current_pos_x = current_pos.pose.position.x;
2022 double current_pos_y = current_pos.pose.position.y;
2023
2024
2025
2026
2027 lanelet::BasicPoint2d curr_pos;
2028 curr_pos.x() = current_pos_x;
2029 curr_pos.y() = current_pos_y;
2030
2031 carma_perception_msgs::msg::CheckActiveGeofence outgoing_geof; //message to publish
2032 double next_distance = 0 ; //Distance to next geofence
2033
2034 if (active_geofence_llt_ids_.size() <= 0 )
2035 {
2036 return outgoing_geof;
2037 }
2038
2039 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence llt ids are loaded to the WMBroadcaster");
2040
2041 // Obtain the closest lanelet to the vehicle's current position
2042 auto current_llt = lanelet::geometry::findNearest(current_map_->laneletLayer, curr_pos, 1)[0].second;
2043
2044
2045 /* determine whether or not the vehicle's current position is within an active geofence */
2046 if (boost::geometry::within(curr_pos, current_llt.polygon2d().basicPolygon()))
2047 {
2048 next_distance = distToNearestActiveGeofence(curr_pos);
2049 outgoing_geof.distance_to_next_geofence = next_distance;
2050
2051 for(auto id : active_geofence_llt_ids_)
2052 {
2053 if (id == current_llt.id())
2054 {
2055 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Vehicle is on Lanelet " << current_llt.id() << ", which has an active geofence");
2056 outgoing_geof.is_on_active_geofence = true;
2057 for (auto regem: current_llt.regulatoryElements())
2058 {
2059 // Assign active geofence fields based on the speed limit associated with this lanelet
2060 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
2061 {
2062 lanelet::DigitalSpeedLimitPtr speed = std::dynamic_pointer_cast<lanelet::DigitalSpeedLimit>
2063 (current_map_->regulatoryElementLayer.get(regem->id()));
2064 outgoing_geof.value = speed->speed_limit_.value();
2065 outgoing_geof.advisory_speed = speed->speed_limit_.value();
2066 outgoing_geof.reason = speed->getReason();
2067
2068 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a speed limit of " << speed->speed_limit_.value());
2069
2070 // Cannot overrule outgoing_geof.type if it is already set to LANE_CLOSED
2071 if(outgoing_geof.type != carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED)
2072 {
2073 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::SPEED_LIMIT;
2074 }
2075 }
2076
2077 // Assign active geofence fields based on the minimum gap associated with this lanelet (if it exists)
2078 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalMinimumGap::RuleName) == 0)
2079 {
2080 lanelet::DigitalMinimumGapPtr min_gap = std::dynamic_pointer_cast<lanelet::DigitalMinimumGap>
2081 (current_map_->regulatoryElementLayer.get(regem->id()));
2082 outgoing_geof.minimum_gap = min_gap->getMinimumGap();
2083 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a minimum gap of " << min_gap->getMinimumGap());
2084 }
2085
2086 // Assign active geofence fields based on whether the current lane is closed or is immediately adjacent to a closed lane
2087 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2088 {
2089 lanelet::RegionAccessRulePtr accessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2090 (current_map_->regulatoryElementLayer.get(regem->id()));
2091
2092 // Update the 'type' and 'reason' for this active geofence if the vehicle is in a closed lane
2093 if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2094 {
2095 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence is a closed lane.");
2096 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Closed lane reason: " << accessRuleReg->getReason());
2097 outgoing_geof.reason = accessRuleReg->getReason();
2098 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2099 }
2100 // Otherwise, update the 'type' and 'reason' for this active geofence if the vehicle is in a lane immediately adjacent to a closed lane with the same travel direction
2101 else
2102 {
2103 // Obtain all same-direction lanes sharing the right lane boundary (will include the current lanelet)
2104 auto right_boundary_lanelets = current_map_->laneletLayer.findUsages(current_llt.rightBound());
2105
2106 // Check if the adjacent right lane is closed
2107 if(right_boundary_lanelets.size() > 1)
2108 {
2109 for(auto lanelet : right_boundary_lanelets)
2110 {
2111 // Only check the adjacent right lanelet; ignore the current lanelet
2112 if(lanelet.id() != current_llt.id())
2113 {
2114 for (auto rightRegem: lanelet.regulatoryElements())
2115 {
2116 if(rightRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2117 {
2118 lanelet::RegionAccessRulePtr rightAccessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2119 (current_map_->regulatoryElementLayer.get(rightRegem->id()));
2120 if(!rightAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !rightAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2121 {
2122 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Right adjacent Lanelet " << lanelet.id() << " is CLOSED");
2123 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence");
2124 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << rightAccessRuleReg->getReason());
2125 outgoing_geof.reason = rightAccessRuleReg->getReason();
2126 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2127 }
2128 }
2129 }
2130 }
2131 }
2132 }
2133
2134 // Check if the adjacent left lane is closed
2135 auto left_boundary_lanelets = current_map_->laneletLayer.findUsages(current_llt.leftBound());
2136 if(left_boundary_lanelets.size() > 1)
2137 {
2138 for(auto lanelet : left_boundary_lanelets)
2139 {
2140 // Only check the adjacent left lanelet; ignore the current lanelet
2141 if(lanelet.id() != current_llt.id())
2142 {
2143 for (auto leftRegem: lanelet.regulatoryElements())
2144 {
2145 if(leftRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2146 {
2147 lanelet::RegionAccessRulePtr leftAccessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2148 (current_map_->regulatoryElementLayer.get(leftRegem->id()));
2149 if(!leftAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !leftAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2150 {
2151 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Left adjacent Lanelet " << lanelet.id() << " is CLOSED");
2152 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence");
2153 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << leftAccessRuleReg->getReason());
2154 outgoing_geof.reason = leftAccessRuleReg->getReason();
2155 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2156 }
2157 }
2158 }
2159 }
2160 }
2161 }
2162 }
2163 }
2164 }
2165 }
2166 }
2167 }
2168 return outgoing_geof;
2169}
2170
2171lanelet::LineString3d WMBroadcaster::createLinearInterpolatingLinestring(const lanelet::Point3d& front_pt, const lanelet::Point3d& back_pt, double increment_distance)
2172{
2173 double dx = back_pt.x() - front_pt.x();
2174 double dy = back_pt.y() - front_pt.y();
2175
2176 std::vector<lanelet::Point3d> points;
2177 double distance = std::sqrt(pow(dx, 2) + pow(dy,2));
2178 double cos = dx / distance;
2179 double sin = dy / distance;
2180 points.push_back(front_pt);
2181 double sum = increment_distance;
2182 while ( sum < distance)
2183 {
2184 points.push_back(lanelet::Point3d(lanelet::utils::getId(),front_pt.x() + sum * cos, front_pt.y() + sum * sin, 0.0));
2185 sum += increment_distance;
2186 }
2187 points.push_back(back_pt);
2188
2189 return lanelet::LineString3d(lanelet::utils::getId(), points);
2190}
2191
2192lanelet::Lanelet WMBroadcaster::createLinearInterpolatingLanelet(const lanelet::Point3d& left_front_pt, const lanelet::Point3d& right_front_pt, const lanelet::Point3d& left_back_pt, const lanelet::Point3d& right_back_pt, double increment_distance)
2193{
2194 return lanelet::Lanelet(lanelet::utils::getId(), createLinearInterpolatingLinestring(left_front_pt, left_back_pt, increment_distance), createLinearInterpolatingLinestring(right_front_pt, right_back_pt, increment_distance));
2195}
2196
2198{
2199 uint16_t map_msg_intersection_id = 0;
2200 uint16_t cur_signal_group_id = 0;
2201 std::vector<lanelet::CarmaTrafficSignalPtr> traffic_lights;
2202 lanelet::Lanelet route_lanelet;
2203 lanelet::Ids cur_route_lanelet_ids = current_route.route_path_lanelet_ids;
2204 bool isLightFound = false;
2205
2206 for(auto id : cur_route_lanelet_ids)
2207 {
2208 route_lanelet= current_map_->laneletLayer.get(id);
2209 traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
2210 if(!traffic_lights.empty())
2211 {
2212 isLightFound = true;
2213 break;
2214 }
2215 }
2216
2217 if(isLightFound && sim_)
2218 {
2219 for(auto itr = sim_->signal_group_to_traffic_light_id_.begin(); itr != sim_->signal_group_to_traffic_light_id_.end(); itr++)
2220 {
2221 if(itr->second == traffic_lights.front()->id())
2222 {
2223 cur_signal_group_id = itr->first;
2224 }
2225 }
2226 }
2227 else{
2228 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching Traffic lights along the route");
2229 }//END Traffic signals
2230
2231 auto intersections = route_lanelet.regulatoryElementsAs<lanelet::SignalizedIntersection>();
2232 if (intersections.empty())
2233 {
2234 // no match if any of the entry lanelet is not part of any intersection.
2235 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching intersection for current lanelet. lanelet id = " << route_lanelet.id());
2236 }
2237 else
2238 {
2239 //Currently, each lanelet has only one intersection
2240 lanelet::Id intersection_id = intersections.front()->id();
2241 if(intersection_id != lanelet::InvalId)
2242 {
2243 for(auto itr = sim_->intersection_id_to_regem_id_.begin(); itr != sim_->intersection_id_to_regem_id_.end(); itr++)
2244 {
2245 if(itr->second == intersection_id)
2246 {
2247 map_msg_intersection_id = itr->first;
2248 }
2249 }
2250 }
2251 } //END intersections
2252
2253 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "MAP msg: Intersection ID = " << map_msg_intersection_id << ", Signal Group ID =" << cur_signal_group_id );
2254 if(map_msg_intersection_id != 0 && cur_signal_group_id != 0)
2255 {
2256 upcoming_intersection_ids_.data.clear();
2257 upcoming_intersection_ids_.data.push_back(static_cast<int>(map_msg_intersection_id));
2258 upcoming_intersection_ids_.data.push_back(static_cast<int>(cur_signal_group_id));
2259 }
2260}
2261
2262void WMBroadcaster::pubTCMACK(j2735_v2x_msgs::msg::Id64b tcm_req_id, uint16_t msgnum, int ack_status, const std::string& ack_reason)
2263{
2264 carma_v2x_msgs::msg::MobilityOperation mom_msg;
2265 mom_msg.m_header.timestamp = scheduler_.now().nanoseconds()/1000000;
2266 mom_msg.m_header.sender_id = vehicle_id_;
2267 mom_msg.strategy = geofence_ack_strategy_;
2268 std::stringstream ss;
2269 for(size_t i=0; i < tcm_req_id.id.size(); i++)
2270 {
2271 ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned) tcm_req_id.id.at(i);
2272 }
2273 std::string tcmv01_req_id_hex = ss.str();
2274 ss.str("");
2275 ss << "traffic_control_id:" << tcmv01_req_id_hex << ", msgnum:"<< msgnum << ", acknowledgement:" << ack_status << ", reason:" << ack_reason;
2276 mom_msg.strategy_params = ss.str();
2277 for(int i = 0; i < ack_pub_times_; i++)
2278 {
2279 tcm_ack_pub_(mom_msg);
2280 }
2281}
2282
2283const uint8_t WorkZoneSection::OPEN = 0;
2284const uint8_t WorkZoneSection::CLOSED = 1;
2285const uint8_t WorkZoneSection::TAPERLEFT = 2;
2286const uint8_t WorkZoneSection::TAPERRIGHT = 3;
2287const uint8_t WorkZoneSection::OPENLEFT = 4;
2288const uint8_t WorkZoneSection::OPENRIGHT = 5;
2289const uint8_t WorkZoneSection::REVERSE = 6;
2290
2291
2292} // namespace carma_wm_ctrl
2293
Position in a track based coordinate system where the axis are downtrack and crosstrack....
Definition: TrackPos.hpp:35
std::unordered_set< boost::gregorian::greg_weekday, std::hash< int > > DayOfTheWeekSet
void onGeofenceActive(std::function< void(std::shared_ptr< Geofence >)> active_callback)
Method which allows the user to set a callback which will be triggered when a geofence becomes active...
rcl_clock_type_t getClockType()
Get the clock type of the clock being created by the timer factory.
rclcpp::Time now()
Get current time used by scheduler.
void addGeofence(std::shared_ptr< Geofence > gf_ptr)
Add a geofence to the scheduler. This will cause it to trigger an event when it becomes active or goe...
void onGeofenceInactive(std::function< void(std::shared_ptr< Geofence >)> inactive_callback)
Method which allows the user to set a callback which will be triggered when a geofence becomes in-act...
lanelet::LaneletMapPtr current_map_
lanelet::Lanelet createLinearInterpolatingLanelet(const lanelet::Point3d &left_front_pt, const lanelet::Point3d &right_front_pt, const lanelet::Point3d &left_back_pt, const lanelet::Point3d &right_back_pt, double increment_distance=0.25)
visualization_msgs::msg::MarkerArray tcm_marker_array_
void removeGeofenceHelper(std::shared_ptr< Geofence > gf_ptr) const
void publishLightId()
helps to populate upcoming_intersection_ids_ from local traffic lanelet ids
carma_v2x_msgs::msg::TrafficControlRequest controlRequestFromRoute(const carma_planning_msgs::msg::Route &route_msg, std::shared_ptr< j2735_v2x_msgs::msg::Id64b > req_id_for_testing=NULL)
Pulls vehicle information from CARMA Cloud at startup by providing its selected route in a TrafficCon...
void addRegionMinimumGap(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01, double min_gap, const std::vector< lanelet::Lanelet > &affected_llts, const std::vector< lanelet::Area > &affected_areas) const
Adds Minimum Gap to the map.
std::shared_ptr< Geofence > createWorkzoneGeometry(std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache, lanelet::Lanelet parallel_llt_front, lanelet::Lanelet parallel_llt_back, std::shared_ptr< std::vector< lanelet::Lanelet > > middle_opposite_lanelets)
Create workzone geofence. Create diagonal lanelets and a lanelet that houses opposing lane's trafficl...
lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d &gf_pts)
Gets the affected lanelet or areas based on the points in local frame.
bool shouldChangeTrafficSignal(const lanelet::ConstLaneletOrArea &el, const lanelet::RegulatoryElementConstPtr &regem, std::shared_ptr< carma_wm::SignalizedIntersectionManager > sim) const
This is a helper function that returns true if signal in the lanelet should be changed according to t...
std::vector< lanelet::Lanelet > splitLaneletWithRatio(std::vector< double > ratios, lanelet::Lanelet input_lanelet, double error_distance) const
Split given lanelet with given downtrack ratios relative to the lanelet. Newly created lanelet will h...
void setErrorDistance(double error_distance)
std::function< void(const carma_perception_msgs::msg::CheckActiveGeofence &)> PublishActiveGeofCallback
PublishCtrlRequestCallback control_msg_pub_
const std::string geofence_ack_strategy_
void baseMapCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg)
Callback to set the base map when it has been loaded.
visualization_msgs::msg::Marker composeTCMMarkerVisualizer(const std::vector< lanelet::Point3d > &input)
composeTCMMarkerVisualizer() compose TCM Marker visualization
lanelet::Lanelets splitOppositeLaneletWithPoint(std::shared_ptr< std::vector< lanelet::Lanelet > > opposite_llts, const lanelet::BasicPoint2d &input_pt, const lanelet::Lanelet &input_llt, double error_distance)
Split given lanelet's adjacent, OPPOSITE lanelet with same proportion as the given point's downtrack ...
carma_planning_msgs::msg::Route current_route
std::vector< std::string > combineParticipantsToVehicle(const std::vector< std::string > &input_participants) const
Combines a list of the given participants into a single "vehicle" type if participants cover all poss...
double distToNearestActiveGeofence(const lanelet::BasicPoint2d &curr_pos)
Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet...
void removeGeofence(std::shared_ptr< Geofence > gf_ptr)
Removes a geofence from the current map and publishes the ROS msg.
lanelet::LaneletMapPtr base_map_
void setConfigVehicleId(const std::string &vehicle_id)
Retrieve the vehicle ID from global vehicle parameters, and set instance memeber vehicle id.
lanelet::Points3d getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01 &geofence_msg)
Extract geofence points from geofence message using its given proj and datum fields.
std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache_
bool convertLightIdToInterGroupId(unsigned &intersection_id, unsigned &group_id, const lanelet::Id &lanelet_id)
helper for generating intersection and group Id of a traffic light from lanelet id
lanelet::Velocity config_limit
lanelet::routing::RoutingGraphPtr current_routing_graph_
void addGeofenceHelper(std::shared_ptr< Geofence > gf_ptr)
std::shared_ptr< Geofence > createWorkzoneGeofence(std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache)
Creates a single workzone geofence (in the vector) that includes all additional lanelets (housing tra...
lanelet::ConstLanelets route_path_
void setMaxLaneWidth(double max_lane_width)
Sets the max lane width in meters. Geofence points are associated to a lanelet if they are within thi...
void geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr geofence_msg)
Callback to add a geofence to the map. Currently only supports version 1 TrafficControlMessage.
void addGeofence(std::shared_ptr< Geofence > gf_ptr)
Adds a geofence to the current map and publishes the ROS msg.
void setConfigSpeedLimit(double cL)
Sets the configured speed limit.
void currentLocationCallback(geometry_msgs::msg::PoseStamped::UniquePtr current_pos)
void addPassingControlLineFromMsg(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01, const std::vector< lanelet::Lanelet > &affected_llts) const
void setVehicleParticipationType(std::string participant)
Set the Vehicle Participation Type.
carma_perception_msgs::msg::CheckActiveGeofence checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped &current_pos)
Returns a message indicating whether or not the vehicle is inside of an active geofence lanelet.
std::function< void(const autoware_lanelet2_msgs::msg::MapBin &)> PublishMapUpdateCallback
void routeCallbackMessage(carma_planning_msgs::msg::Route::UniquePtr route_msg)
Calls controlRequestFromRoute() and publishes the TrafficControlRequest Message returned after the co...
std::string getVehicleParticipationType()
Get the Vehicle Participation Type object.
std::vector< lanelet::Lanelet > splitLaneletWithPoint(const std::vector< lanelet::BasicPoint2d > &input_pts, const lanelet::Lanelet &input_llt, double error_distance)
Split given lanelet with same proportion as the given points' downtrack relative to the lanelet....
carma_v2x_msgs::msg::TrafficControlRequestPolygon composeTCRStatus(const lanelet::BasicPoint3d &localPoint, const carma_v2x_msgs::msg::TrafficControlBounds &cB, const lanelet::projection::LocalFrameProjector &local_projector)
composeTCRStatus() compose TCM Request visualization on UI
WMBroadcaster(const PublishMapCallback &map_pub, const PublishMapUpdateCallback &map_update_pub, const PublishCtrlRequestCallback &control_msg_pub, const PublishActiveGeofCallback &active_pub, std::shared_ptr< carma_ros2_utils::timers::TimerFactory > timer_factory, const PublishMobilityOperationCallback &tcm_ack_pub)
Constructor.
PublishMobilityOperationCallback tcm_ack_pub_
std_msgs::msg::Int32MultiArray upcoming_intersection_ids_
void externalMapMsgCallback(carma_v2x_msgs::msg::MapData::UniquePtr map_msg)
Callback to MAP.msg which contains intersections' static info such geometry and lane ids.
std::unordered_set< lanelet::Id > active_geofence_llt_ids_
std::unordered_map< uint32_t, lanelet::Id > traffic_light_id_lookup_
std::shared_ptr< carma_wm::SignalizedIntersectionManager > sim_
std::vector< std::string > participantsChecker(const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01) const
Generates participants list.
void addRegulatoryComponent(std::shared_ptr< Geofence > gf_ptr) const
std::vector< std::shared_ptr< Geofence > > geofenceFromMapMsg(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::MapData &map_msg)
Fills geofence object from MAP Data ROS Msg which contains intersections' static data such as geometr...
void scheduleGeofence(std::shared_ptr< carma_wm_ctrl::Geofence > gf_ptr_list)
carma_v2x_msgs::msg::TrafficControlRequestPolygon tcr_polygon_
PublishActiveGeofCallback active_pub_
void addScheduleFromMsg(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01)
Populates the schedules member of the geofence object from given TrafficControlMessageV01 message.
std::function< void(const carma_v2x_msgs::msg::MobilityOperation &)> PublishMobilityOperationCallback
std::vector< std::string > invertParticipants(const std::vector< std::string > &input_participants) const
Generates inverse participants list of the given participants.
void pubTCMACK(j2735_v2x_msgs::msg::Id64b tcm_req_id, uint16_t msgnum, int ack_status, const std::string &ack_reason)
Construct TCM acknowledgement object and populate it with params. Publish the object for a configured...
void setIntersectionCoordCorrection(const std::vector< int64_t > &intersection_ids_for_correction, const std::vector< double > &intersection_correction)
Sets the coordinate correction for intersection.
std::function< void(const autoware_lanelet2_msgs::msg::MapBin &)> PublishMapCallback
void updateUpcomingSGIntersectionIds()
populate upcoming_intersection_ids_ from local traffic lanelet ids
void preprocessWorkzoneGeometry(std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache, std::shared_ptr< std::vector< lanelet::Lanelet > > parallel_llts, std::shared_ptr< std::vector< lanelet::Lanelet > > opposite_llts)
Preprocess for workzone area. Parallel_llts will have front_parallel and back_parallel lanelets that ...
carma_planning_msgs::msg::Route getRoute()
Returns the most recently recieved route message.
uint32_t generate32BitId(const std::string &label)
helper for generating 32bit traffic light Id from TCM label field consisting workzone intersection/si...
std::unordered_set< std::string > generated_geofence_reqids_
void addRegionAccessRule(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01, const std::vector< lanelet::Lanelet > &affected_llts) const
Adds RegionAccessRule to the map.
void setConfigACKPubTimes(int ack_pub_times)
Sets the TCM Acknowledgement publish times.
PublishMapUpdateCallback map_update_pub_
lanelet::LineString3d createLinearInterpolatingLinestring(const lanelet::Point3d &front_pt, const lanelet::Point3d &back_pt, double increment_distance=0.25)
bool shouldChangeControlLine(const lanelet::ConstLaneletOrArea &el, const lanelet::RegulatoryElementConstPtr &regem, std::shared_ptr< Geofence > gf_ptr) const
This is a helper function that returns true if the provided regem is marked to be changed by the geof...
std::unordered_set< std::string > checked_geofence_ids_
void geofenceFromMsg(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &geofence_msg)
Fills geofence object from TrafficControlMessageV01 ROS Msg.
void addBackRegulatoryComponent(std::shared_ptr< Geofence > gf_ptr) const
std::function< void(const carma_v2x_msgs::msg::TrafficControlRequest &)> PublishCtrlRequestCallback
void geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_ref)
Callback to set the base map georeference (proj string)
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
TrackPos trackPos(const lanelet::ConstLanelet &lanelet, const lanelet::BasicPoint2d &point)
Returns the TrackPos, computed in 2d, of the provided point relative to the centerline of the provide...
Definition: Geometry.cpp:120
std::vector< lanelet::ConstLanelet > getLaneletsFromPoint(const lanelet::LaneletMapConstPtr &semantic_map, const lanelet::BasicPoint2d &point, const unsigned int n=10)
carma_wm::query namespace contains implementations for query functions (input and output read or writ...
lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d &gf_pts, const lanelet::LaneletMapPtr &lanelet_map, std::shared_ptr< const lanelet::routing::RoutingGraph > routing_graph, double max_lane_width)
Gets the affected lanelet or areas based on the points in the given map's frame.
std::vector< lanelet::ConstLanelet > nonConnectedAdjacentLeft(const lanelet::LaneletMapConstPtr &semantic_map, const lanelet::BasicPoint2d &input_point, const unsigned int n=10)
Given the cartesian point on the map, tries to get the opposite direction lanelet on the left This fu...
uint32_t get32BitId(uint16_t intersection_id, uint8_t signal_group_id)
Get 32bit id by concatenating 16bit id with 8bit signal_group_id.
const std::string MAP_MSG_TF_SIGNAL
Definition: Geofence.hpp:41
const std::string MAP_MSG_INTERSECTION
Definition: Geofence.hpp:40
const int WORKZONE_TCM_REQUIRED_SIZE
void fromBinMsg(const autoware_lanelet2_msgs::msg::MapBin &msg, std::shared_ptr< carma_wm::TrafficControl > gf_ptr, lanelet::LaneletMapPtr lanelet_map=nullptr)
void toBinMsg(std::shared_ptr< carma_wm::TrafficControl > gf_ptr, autoware_lanelet2_msgs::msg::MapBin *msg)
void ensureCompliance(lanelet::LaneletMapPtr map, lanelet::Velocity config_limit=80_mph)
Function modifies an existing map to make a best effort attempt at ensuring the map confroms to the e...
static const uint8_t TAPERRIGHT
static const uint8_t OPENLEFT
static const uint8_t CLOSED
static const uint8_t TAPERLEFT
static const uint8_t OPENRIGHT
static const uint8_t REVERSE