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