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>
29#include <carma_perception_msgs/msg/check_active_geofence.hpp>
30#include <lanelet2_core/primitives/Polygon.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>
40#include <boost/date_time/date_defs.hpp>
45using std::placeholders::_1;
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)
60 static bool firstCall =
true;
65 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"WMBroadcaster::baseMapCallback called for first time with new map message");
69 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"WMBroadcaster::baseMapCallback called multiple times in the same node");
72 lanelet::LaneletMapPtr new_map(
new lanelet::LaneletMap);
73 lanelet::LaneletMapPtr new_map_to_change(
new lanelet::LaneletMap);
84 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Building routing graph for base map");
86 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
87 lanelet::traffic_rules::CarmaUSTrafficRules::Location,
participant_);
90 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done building routing graph for base map");
95 autoware_lanelet2_msgs::msg::MapBin compliant_map_msg;
98 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating routing graph message.");
102 compliant_map_msg.routing_graph = readable_graph->routingGraphToMsg(
participant_);
103 compliant_map_msg.has_routing_graph =
true;
105 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done creating routing graph message.");
120 carma_v2x_msgs::msg::TrafficControlSchedule msg_schedule = msg_v01.params.schedule;
121 double ns_to_sec = 1.0e9;
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};
132 if (msg_schedule.dow_exists) {
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) {
139 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Sunday);
142 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Monday);
145 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Tuesday);
148 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Wednesday);
151 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Thursday);
154 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Friday);
157 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Saturday);
160 throw std::invalid_argument(
"Unrecognized weekday value: " +
std::to_string(
i));
166 if (msg_schedule.between_exists) {
168 for (
auto daily_schedule : msg_schedule.between)
170 if (msg_schedule.repeat_exists) {
171 gf_ptr->schedules.push_back(
GeofenceSchedule({msg_schedule.start, clock_type},
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),
180 gf_ptr->schedules.push_back(
GeofenceSchedule({msg_schedule.start, clock_type},
182 rclcpp::Duration(daily_schedule.begin),
183 rclcpp::Duration(daily_schedule.duration),
184 rclcpp::Duration::from_nanoseconds(0.0),
185 rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin),
186 rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin),
193 if (msg_schedule.repeat_exists) {
194 gf_ptr->schedules.push_back(
GeofenceSchedule({msg_schedule.start, clock_type},
196 rclcpp::Duration::from_nanoseconds(0.0),
197 rclcpp::Duration::from_nanoseconds(86400.0e9),
198 rclcpp::Duration(msg_schedule.repeat.offset),
199 rclcpp::Duration(msg_schedule.repeat.span),
200 rclcpp::Duration(msg_schedule.repeat.period),
203 gf_ptr->schedules.push_back(
GeofenceSchedule({msg_schedule.start, clock_type},
205 rclcpp::Duration::from_nanoseconds(0.0),
206 rclcpp::Duration::from_nanoseconds(86400.0e9),
207 rclcpp::Duration::from_nanoseconds(0.0),
208 rclcpp::Duration::from_nanoseconds(86400.0e9),
209 rclcpp::Duration::from_nanoseconds(86400.0e9),
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;
223 auto sim_copy = std::make_shared<carma_wm::SignalizedIntersectionManager>(*
sim_);
227 if (*
sim_ == *sim_copy)
229 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
">>> Detected no change from previous, ignoring duplicate message! with gf id: " << gf_ptr->id_);
233 for (
auto intersection : intersections)
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())
241 update->affected_parts_.push_back(llt);
244 auto j2735_intersection_id =
sim_->regem_id_to_intersection_id_[intersection->id()];
245 for (
auto pt:
sim_->intersection_nodes_[j2735_intersection_id])
247 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"J2735 MAP msg road geometry points: x: " << pt.x() <<
", y: " << pt.y());
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);
253 for (
auto signal : traffic_signals)
255 auto update = std::make_shared<Geofence>();
256 update->id_ = boost::uuids::random_generator()();
258 update->regulatory_element_ = signal;
259 for (
auto llt : signal->getControlStartLanelets())
261 update->affected_parts_.push_back(llt);
263 updates_to_send.push_back(update);
266 return updates_to_send;
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;
275 std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin());
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_);
286 std::vector<lanelet::Lanelet> affected_llts;
287 std::vector<lanelet::Area> affected_areas;
290 for (
auto llt_or_area : gf_ptr->affected_parts_)
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()));
300 lanelet::Velocity sL;
303 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
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;
317 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital maximum speed limit is invalid. Value capped at max speed limit.");
323 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital speed limit is invalid. Value set to 0mph.");
327 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
330 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE)
333 sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPS());
337 std::string reason =
"";
338 if (msg_v01.package.label_exists)
339 reason = msg_v01.package.label;
345 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital speed limit is invalid. Value capped at max speed limit.");
350 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital speed limit is invalid. Value set to 0mph.");
353 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
356 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE)
360 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE)
363 double min_gap = (double)msg_detail.minhdwy;
367 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital min gap is invalid. Value set to 0 meter.");
373 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
375 gf_ptr->label_ = msg_v01.package.label;
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))
382 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
388 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==carma_v2x_msgs::msg::TrafficControlDetail::CLOSED)
393 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::RESTRICTED_CHOICE) {
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>());
409 auto gf_ptr =
createWorkzoneGeometry(work_zone_geofence_cache, parallel_llts->front(), parallel_llts->back(), middle_opposite_lanelets );
418 for (
auto pair : work_zone_geofence_cache)
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_);
422 work_zone_geofence_cache.clear();
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)
430 auto gf_ptr = std::make_shared<Geofence>();
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())
441 front_llt_diag.addRegulatoryElement(regem);
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())
453 back_llt_diag.addRegulatoryElement(regem);
460 std::vector<lanelet::Lanelet> middle_llts;
461 for (
int i = middle_opposite_lanelets->size() - 1;
i >= 0;
i--)
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())
466 middle_llt.addRegulatoryElement(regem);
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());
475 lanelet::LineString3d parallel_stop_ls(lanelet::utils::getId(), {parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back()});
477 std::vector<lanelet::Lanelet> controlled_taper_right;
479 controlled_taper_right.push_back(parallel_llt_front);
481 controlled_taper_right.push_back(front_llt_diag);
483 controlled_taper_right.insert(controlled_taper_right.end(), middle_llts.begin(), middle_llts.end());
485 controlled_taper_right.push_back(back_llt_diag);
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()}));
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());
497 std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts_with_stop_line = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
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()});
504 std::vector<lanelet::Lanelet> controlled_open_right;
506 controlled_open_right.insert(controlled_open_right.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());;
510 controlled_open_right.push_back(
current_map_->laneletLayer.get(llt.lanelet().get().id()));
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()}));
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());
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);
527 gf_ptr->lanelet_additions_.push_back(front_llt_diag);
529 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), middle_llts.begin(), middle_llts.end());
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);
535 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());;
543 carma_v2x_msgs::msg::TrafficControlMessageV01 participants_and_reason_only;
545 j2735_v2x_msgs::msg::TrafficControlVehClass participant;
547 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE;
549 participants_and_reason_only.params.vclasses.push_back(participant);
551 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BUS;
553 participants_and_reason_only.params.vclasses.push_back(participant);
555 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR;
557 participants_and_reason_only.params.vclasses.push_back(participant);
559 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK;
561 participants_and_reason_only.params.vclasses.push_back(participant);
563 participants_and_reason_only.package.label =
"SIG_WZ";
565 std::vector<lanelet::Lanelet> old_or_blocked_llts;
570 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
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()));
578 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
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()));
586 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
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()));
593 gf_ptr->affected_parts_.push_back(old_opposite_llts[0]);
594 old_or_blocked_llts.push_back(old_opposite_llts[0]);
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)
611 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map is not loaded to the WMBroadcaster"));
616 std::vector <lanelet::Lanelet> new_taper_right_llts;
621 double check_dist_tpr = lanelet::geometry::distance2d(taper_right_first_llt.centerline2d().front().basicPoint2d(), taper_right_first_pt);
625 if (new_taper_right_llts.size() == 1 && check_dist_tpr <=
error_distance_)
627 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating duplicate lanelet of 'previous lanelet' due to TAPERRIGHT using entire lanelet...");
629 if (previous_lanelets.empty())
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.");
637 auto prev_lanelet_to_copy =
current_map_->laneletLayer.get(previous_lanelets.front().id());
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());
648 std::vector <lanelet::Lanelet> new_open_right_llts;
653 double check_dist_opr = lanelet::geometry::distance2d(open_right_last_llt.centerline2d().back().basicPoint2d(), open_right_last_pt);
657 if (new_open_right_llts.size() == 1 && check_dist_opr <=
error_distance_)
659 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating duplicate lanelet of 'next lanelet' due to OPENRIGHT using entire lanelet...");
661 if (next_lanelets.empty())
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.");
669 auto next_lanelet_to_copy =
current_map_->laneletLayer.get(next_lanelets.front().id());
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());
682 auto reverse_back_llt = reverse_back_llts[0];
684 auto reverse_front_llt = reverse_front_llts[0];
686 if (reverse_back_llt.id() == reverse_front_llt.id())
688 std::vector<lanelet::Lanelet> temp_llts;
693 if (temp_llts.size() < 2)
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());
700 else if (temp_llts.size() == 2)
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()))
706 opposite_llts->push_back(temp_llts.back());
710 opposite_llts->push_back(temp_llts.front());
713 else if (temp_llts.size() == 3)
715 opposite_llts->insert(opposite_llts->end(), temp_llts.begin() + 1, temp_llts.end()- 1);
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");
724 std::vector<lanelet::Lanelet> temp_opposite_front_llts;
726 if (temp_opposite_front_llts.size() > 1)
728 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin() + 1, temp_opposite_front_llts.end());
732 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin(), temp_opposite_front_llts.end());
746 std::vector<lanelet::Lanelet> temp_opposite_back_llts;
748 if (temp_opposite_back_llts.size() > 1)
750 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end()- 1);
754 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end());
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");
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());
765 std::vector<lanelet::Lanelet> parallel_llts;
767 std::vector<double> ratios;
769 for (
auto pt : input_pts)
772 double point_downtrack_ratio = point_downtrack / llt_downtrack;
773 ratios.push_back(point_downtrack_ratio);
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;
792 if (opposing_llts.empty())
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");
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;
808 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map is not loaded to the WMBroadcaster"));
812 throw lanelet::InvalidInputError(std::string(
"Ratios list is empty! Cannot split"));
815 std::vector<lanelet::Lanelet> created_llts;
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());
820 ratios.push_back(1.0);
822 int left_ls_size = input_lanelet.leftBound2d().size();
823 int right_ls_size = input_lanelet.rightBound2d().size();
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 ++)
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));
834 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
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()));
842 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
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()));
847 left_next_pt_idx = left_ls_size - 1;
848 right_next_pt_idx = right_ls_size - 1;
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);
854 lanelet::LineString3d left_ls(lanelet::utils::getId(), left_pts,
current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().attributes());
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);
859 lanelet::LineString3d right_ls(lanelet::utils::getId(), right_pts);
861 lanelet::Lanelet new_llt (lanelet::utils::getId(), left_ls, right_ls,
current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().attributes());
863 for (
auto regem :
current_map_->laneletLayer.get(input_lanelet.id()).regulatoryElements())
865 new_llt.addRegulatoryElement(
current_map_->regulatoryElementLayer.get(regem->id()));
868 left_prev_pt_idx = left_next_pt_idx;
869 right_prev_pt_idx = right_next_pt_idx;
871 created_llts.push_back(new_llt);
874 if (left_prev_pt_idx == left_ls_size - 1 || right_prev_pt_idx == right_ls_size - 1)
881 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"splitLaneletWithRatio returning lanelets size: " << created_llts.size());
889 carma_v2x_msgs::msg::TrafficControlDetail msg_detail;
890 msg_detail = msg_v01.params.detail;
892 lanelet::LineStrings3d pcl_bounds;
893 if (msg_detail.lataffinity == carma_v2x_msgs::msg::TrafficControlDetail::LEFT)
895 for (
auto llt : affected_llts) pcl_bounds.push_back(llt.leftBound());
896 gf_ptr->pcl_affects_left_ =
true;
900 for (
auto llt : affected_llts) pcl_bounds.push_back(llt.rightBound());
901 gf_ptr->pcl_affects_right_ =
true;
905 std::vector<std::string> left_participants;
906 std::vector<std::string> right_participants;
910 if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
911 msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
913 left_participants = participants;
915 else if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
917 left_participants.push_back(lanelet::Participants::VehicleEmergency);
919 if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
920 msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
922 right_participants = participants;
924 else if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
926 right_participants.push_back(lanelet::Participants::VehicleEmergency);
929 gf_ptr->regulatory_element_ = std::make_shared<lanelet::PassingControlLine>(lanelet::PassingControlLine::buildData(
930 lanelet::utils::getId(), pcl_bounds, left_participants, right_participants));
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
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));
939 if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck ))
941 gf_ptr->invalidate_route_=
true;
945 gf_ptr->invalidate_route_=
false;
947 gf_ptr->regulatory_element_ = regulatory_element;
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
952 auto regulatory_element = std::make_shared<lanelet::DigitalMinimumGap>(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(),
955 gf_ptr->regulatory_element_ = regulatory_element;
960 std::vector<std::string> participants;
961 for (j2735_v2x_msgs::msg::TrafficControlVehClass participant : msg_v01.params.vclasses)
964 if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::ANY)
966 participants = {lanelet::Participants::Vehicle, lanelet::Participants::Pedestrian, lanelet::Participants::Bicycle};
969 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN)
971 participants.push_back(lanelet::Participants::Pedestrian);
973 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BICYCLE)
975 participants.push_back(lanelet::Participants::Bicycle);
977 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE ||
978 participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MOTORCYCLE)
980 participants.push_back(lanelet::Participants::VehicleMotorcycle);
982 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BUS)
984 participants.push_back(lanelet::Participants::VehicleBus);
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)
989 participants.push_back(lanelet::Participants::VehicleCar);
991 else if (8<= participant.vehicle_class && participant.vehicle_class <= 16)
993 participants.push_back(lanelet::Participants::VehicleTruck);
1002 std::vector<std::string> participants;
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())
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);
1013 return participants;
1018 std::vector<std::string> participants;
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())
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);
1030 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Not making any changes to the participants list");
1031 participants = input_participants;
1034 return participants;
1039 auto gf_ptr = std::make_shared<Geofence>();
1043 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Map is not available yet. Skipping MAP msg");
1048 bool up_to_date =
false;
1049 if (
sim_->intersection_id_to_regem_id_.size() == map_msg->intersections.size())
1053 for (
auto intersection : map_msg->intersections)
1055 if (
sim_->intersection_id_to_regem_id_.find(intersection.id.id) ==
sim_->intersection_id_to_regem_id_.end())
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()();
1074 carma_v2x_msgs::msg::TrafficControlMessageV01 traffic_control_msg;
1087 std::lock_guard<std::mutex> guard(
map_mutex_);
1088 std::stringstream reason_ss;
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());
1097 boost::uuids::uuid id;
1098 std::copy(geofence_msg->tcm_v01.id.id.begin(), geofence_msg->tcm_v01.id.id.end(),
id.begin());
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());
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());
1116 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid);
1122 auto gf_ptr = std::make_shared<Geofence>();
1124 gf_ptr->msg_ = geofence_msg->tcm_v01;
1132 reason_ss <<
"Successfully processed TCM.";
1135 catch(std::exception& ex)
1138 reason_ss <<
"Failed to process TCM. " << ex.what();
1146 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
1148 bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find(
"SIG_WZ") != std::string::npos;
1150 carma_v2x_msgs::msg::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail;
1153 if (detected_workzone_signal && msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1158 auto gf_ptr_speed = std::make_shared<Geofence>();
1159 gf_ptr_speed->schedules = gf_ptr->schedules;
1161 carma_v2x_msgs::msg::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_;
1163 std::reverse(duplicate_msg.geometry.nodes.begin() + 1, duplicate_msg.geometry.nodes.end());
1168 for (
auto& pt: duplicate_msg.geometry.nodes)
1175 duplicate_msg.geometry.nodes[0].x = first_x;
1176 duplicate_msg.geometry.nodes[0].y = first_y;
1178 gf_ptr_speed->msg_ = duplicate_msg;
1181 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1183 gf_ptr->label_ = gf_ptr->msg_.package.label;
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))
1190 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
1196 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now...");
1207 std::lock_guard<std::mutex> guard(
map_mutex_);
1208 sim_->setTargetFrame(geo_ref->data);
1214 sim_ = std::make_shared<carma_wm::SignalizedIntersectionManager>();
1222 if (intersection_correction.size() % 2 != 0 || intersection_ids_for_correction.size() != intersection_correction.size() / 2)
1224 throw std::invalid_argument(
"Some of intersection coordinate correction parameters are not fully set!");
1227 for (
auto i = 0;
i < intersection_correction.size();
i =
i + 2)
1229 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[
i/2]].first = intersection_correction[
i];
1230 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[
i/2]].second = intersection_correction[
i + 1];
1238 config_limit = lanelet::Velocity(cL * lanelet::units::MPH());
1261 auto pos1 = label.find(
"INT_ID:") + 7;
1262 auto pos2 = label.find(
"SG_ID:") + 6;
1264 uint16_t intersection_id = std::stoi(label.substr(pos1 , 4));
1265 uint8_t signal_id = std::stoi(label.substr(pos2 , 3));
1273 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Getting affected lanelets");
1276 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map is not loaded to the WMBroadcaster"));
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"));
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");
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);
1296 std::string universal_frame = datum;
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.");
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) {
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);
1311 PJ* target_to_map = proj_create_crs_to_crs(PJ_DEFAULT_CTX, projection.c_str(),
base_map_georef_.c_str(),
nullptr);
1313 if (target_to_map ==
nullptr) {
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_);
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);
1328 prev_pt.x = c_init.xyz.x;
1329 prev_pt.y = c_init.xyz.y;
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)
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);
1336 PJ_COORD
c {{prev_pt.x + pt.x, prev_pt.y + pt.y, 0, 0}};
1338 c_out = proj_trans(target_to_map, PJ_FWD,
c);
1340 gf_pts.push_back(lanelet::Point3d{
current_map_->pointLayer.uniqueId(), c_out.xyz.x, c_out.xyz.y});
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());
1367 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) != 0 || !el.isLanelet())
1372 lanelet::PassingControlLinePtr pcl = std::dynamic_pointer_cast<lanelet::PassingControlLine>(
current_map_->regulatoryElementLayer.get(regem->id()));
1374 bool should_change_pcl =
false;
1375 for (
auto control_line : pcl->controlLine())
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_))
1380 should_change_pcl =
true;
1384 return should_change_pcl;
1398 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::CarmaTrafficSignal::RuleName) != 0 || !el.isLanelet() || !
sim_)
1403 lanelet::CarmaTrafficSignalPtr traffic_signal = std::dynamic_pointer_cast<lanelet::CarmaTrafficSignal>(
current_map_->regulatoryElementLayer.get(regem->id()));
1404 uint8_t signal_id = 0;
1406 for (
auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it)
1408 if (regem->id() == it->second)
1410 signal_id = it->first;
1417 if (sim->signal_group_to_entry_lanelet_ids_[signal_id].find(el.id()) != sim->signal_group_to_entry_lanelet_ids_[signal_id].end())
1427 for (
auto el: gf_ptr->affected_parts_)
1429 for (
auto regem : el.regulatoryElements())
1435 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
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));
1448 for (
auto el: gf_ptr->affected_parts_)
1451 if (gf_ptr->regulatory_element_->id() != lanelet::InvalId)
1454 gf_ptr->update_list_.push_back(std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>(el.id(), gf_ptr->regulatory_element_));
1456 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Regulatory element with invalid id in geofence cannot be added to the map");
1468 for (
auto el: gf_ptr->affected_parts_)
1470 for (
auto regem : el.regulatoryElements())
1474 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1476 auto nonconst_regem =
current_map_->regulatoryElementLayer.get(regem->id());
1477 gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
1485 for (
auto pair : gf_ptr->prev_regems_)
1487 if (pair.second->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1490 gf_ptr->update_list_.push_back(pair);
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_);
1503 std::vector<std::shared_ptr<Geofence>> updates_to_send;
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)
1515 else if (detected_map_msg_signal)
1522 updates_to_send.push_back(gf_ptr);
1525 for (
auto update : updates_to_send)
1528 if (!update->gf_pts.empty())
1540 if (update->affected_parts_.empty())
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());
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.");
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.");
1564 if (!detected_map_msg_signal)
1569 autoware_lanelet2_msgs::msg::MapBin gf_msg;
1572 if (update->invalidate_route_) {
1574 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Rebuilding routing graph after is was invalidated by geofence");
1576 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1577 lanelet::traffic_rules::CarmaUSTrafficRules::Location,
participant_);
1580 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done rebuilding routing graph after is was invalidated by geofence");
1583 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating routing graph message");
1587 gf_msg.routing_graph = readable_graph->routingGraphToMsg(
participant_);
1588 gf_msg.has_routing_graph =
true;
1590 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done creating routing graph message");
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_;
1598 if (detected_map_msg_signal && updates_to_send.back() == update)
1600 send_data->sim_ = *
sim_;
1606 gf_msg.invalidates_route=update->invalidate_route_;
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_);
1619 if (gf_ptr->affected_parts_.empty())
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_, {}));
1630 if (gf_ptr->invalidate_route_) {
1632 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Rebuilding routing graph after is was invalidated by geofence removal");
1634 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1635 lanelet::traffic_rules::CarmaUSTrafficRules::Location,
participant_
1639 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done rebuilding routing graph after is was invalidated by geofence removal");
1642 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating routing graph message for geofence removal");
1646 gf_msg_revert.routing_graph = readable_graph->routingGraphToMsg(
participant_);
1647 gf_msg_revert.has_routing_graph =
true;
1649 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done creating routing graph message for geofence removal");
1670 carma_v2x_msgs::msg::TrafficControlRequest cR;
1678 lanelet::ConstLanelets path;
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"));
1688 for(
auto id : route_msg.route_path_lanelet_ids)
1691 path.push_back(laneLayer);
1697 if(path.size() == 0)
throw lanelet::InvalidObjectStateError(std::string(
"No lanelets available in path."));
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();
1707 while (path.size() != 0)
1709 llt.push_back(path.back());
1712 pathBox.push_back(lanelet::geometry::boundingBox2d(llt.back()));
1715 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x() < minX)
1716 minX = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x();
1719 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y() < minY)
1720 minY = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y();
1724 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x() > maxX)
1725 maxX = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x();
1728 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y() > maxY)
1729 maxY = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y();
1737 if (target_frame.empty())
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"));
1746 lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
1747 lanelet::BasicPoint3d localPoint;
1749 localPoint.x()= minX;
1750 localPoint.y()= minY;
1752 lanelet::GPSPoint gpsRoute = local_projector.reverse(localPoint);
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);
1758 PJ* tmerc_proj = proj_create_crs_to_crs(PJ_DEFAULT_CTX, target_frame.c_str(), local_tmerc_enu_proj.c_str(),
nullptr);
1760 if (tmerc_proj ==
nullptr) {
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);
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<<
")");
1774 PJ_COORD pj_min {{minX, minY, 0, 0}};
1775 PJ_COORD pj_min_tmerc;
1776 PJ_COORD pj_max {{maxX, maxY, 0, 0}};
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);
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 <<
" )");
1784 carma_v2x_msgs::msg::TrafficControlRequest cR;
1785 carma_v2x_msgs::msg::TrafficControlBounds cB;
1787 cB.reflat = gpsRoute.lat;
1788 cB.reflon = gpsRoute.lon;
1790 cB.offsets[0].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x;
1791 cB.offsets[0].deltay = 0.0;
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;
1801 cR.choice = carma_v2x_msgs::msg::TrafficControlRequest::TCRV01;
1804 boost::uuids::uuid uuid_id = boost::uuids::random_generator()();
1807 std::string req_id_test =
"12345678";
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 ++)
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];
1821 cR.tcr_v01.bounds.push_back(cB);
1827carma_v2x_msgs::msg::TrafficControlRequestPolygon
WMBroadcaster::composeTCRStatus(
const lanelet::BasicPoint3d& localPoint,
const carma_v2x_msgs::msg::TrafficControlBounds& cB,
const lanelet::projection::LocalFrameProjector& local_projector)
1829 carma_v2x_msgs::msg::TrafficControlRequestPolygon output;
1830 lanelet::BasicPoint3d local_point_tmp;
1837 local_point_tmp.x() = localPoint.x();
1838 local_point_tmp.y() = localPoint.y();
1842 local_point_tmp.x() = localPoint.x() + cB.offsets[
i].deltax;;
1843 local_point_tmp.y() = localPoint.y() + cB.offsets[
i].deltay;;
1845 lanelet::GPSPoint gps_vertex = local_projector.reverse(local_point_tmp);
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);
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));
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";
1871 marker.scale.x = 0.65;
1872 marker.scale.y = 0.65;
1873 marker.scale.z = 0.65;
1874 marker.frame_locked =
true;
1876 if (!marker_array.markers.empty())
1878 marker.id = marker_array.markers.back().id + 1;
1884 marker.color.r = 0.0F;
1885 marker.color.g = 1.0F;
1886 marker.color.b = 0.0F;
1887 marker.color.a = 1.0F;
1889 for (
int i = 0;
i < input.size();
i++)
1891 geometry_msgs::msg::Point temp_point;
1892 temp_point.x = input[
i].x();
1893 temp_point.y = input[
i].y();
1896 marker.points.push_back(temp_point);
1904 std::lock_guard<std::mutex> guard(
map_mutex_);
1908 throw lanelet::InvalidObjectStateError(std::string(
"Lanelet map (current_map_) is not loaded to the WMBroadcaster"));
1912 std::vector<lanelet::Id> active_geofence_on_route;
1916 active_geofence_on_route.push_back(llt.id());
1919 auto curr_lanelet = lanelet::geometry::findNearest(
current_map_->laneletLayer, curr_pos, 1)[0].second;
1922 if (!boost::geometry::within(curr_pos, curr_lanelet.polygon2d().basicPolygon()))
1923 throw std::invalid_argument(
"Given point is not within any lanelet");
1926 std::vector<double> route_distances;
1928 for (
auto id: active_geofence_on_route)
1933 if (tp.
downtrack < 0 &&
id != curr_lanelet.id())
1936 route_distances.push_back(dist);
1939 std::sort(route_distances.begin(), route_distances.end());
1941 if (route_distances.size() != 0 )
return route_distances[0];
1949 gf_ptr->remove_list_ = {};
1950 gf_ptr->update_list_ = {};
1953 for (
auto llt : gf_ptr->lanelet_additions_)
1959 for (
auto pair : gf_ptr->traffic_light_id_lookup_)
1973 gf_ptr->remove_list_ = {};
1974 gf_ptr->update_list_ = {};
1977 gf_ptr->prev_regems_ = {};
1986 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Could not check active geofence logic because map was not loaded");
1995 if (it -> second == lanelet_id)
1997 group_id = (it -> first & 0xFF);
1998 intersection_id = (it -> first >> 8);
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>();
2019 if (!traffic_lights.empty())
2021 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id());
2025 if (!convert_success)
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;
2051 throw lanelet::InvalidObjectStateError(std::string(
"Lanelet map 'current_map_' is not loaded to the WMBroadcaster"));
2055 double current_pos_x = current_pos.pose.position.x;
2056 double current_pos_y = current_pos.pose.position.y;
2061 lanelet::BasicPoint2d curr_pos;
2062 curr_pos.x() = current_pos_x;
2063 curr_pos.y() = current_pos_y;
2065 carma_perception_msgs::msg::CheckActiveGeofence outgoing_geof;
2066 double next_distance = 0 ;
2070 return outgoing_geof;
2073 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Active geofence llt ids are loaded to the WMBroadcaster");
2076 auto current_llt = lanelet::geometry::findNearest(
current_map_->laneletLayer, curr_pos, 1)[0].second;
2080 if (boost::geometry::within(curr_pos, current_llt.polygon2d().basicPolygon()))
2083 outgoing_geof.distance_to_next_geofence = next_distance;
2087 if (
id == current_llt.id())
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())
2094 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
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();
2102 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Active geofence has a speed limit of " << speed->speed_limit_.value());
2105 if(outgoing_geof.type != carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED)
2107 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::SPEED_LIMIT;
2112 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalMinimumGap::RuleName) == 0)
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());
2121 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2123 lanelet::RegionAccessRulePtr accessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2124 (
current_map_->regulatoryElementLayer.get(regem->id()));
2127 if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck))
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;
2138 auto right_boundary_lanelets =
current_map_->laneletLayer.findUsages(current_llt.rightBound());
2141 if(right_boundary_lanelets.size() > 1)
2143 for(
auto lanelet : right_boundary_lanelets)
2146 if(
lanelet.id() != current_llt.id())
2148 for (
auto rightRegem:
lanelet.regulatoryElements())
2150 if(rightRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
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))
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;
2169 auto left_boundary_lanelets =
current_map_->laneletLayer.findUsages(current_llt.leftBound());
2170 if(left_boundary_lanelets.size() > 1)
2172 for(
auto lanelet : left_boundary_lanelets)
2175 if(
lanelet.id() != current_llt.id())
2177 for (
auto leftRegem:
lanelet.regulatoryElements())
2179 if(leftRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
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))
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;
2202 return outgoing_geof;
2207 double dx = back_pt.x() - front_pt.x();
2208 double dy = back_pt.y() - front_pt.y();
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)
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;
2221 points.push_back(back_pt);
2223 return lanelet::LineString3d(lanelet::utils::getId(), points);
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;
2240 for(
auto id : cur_route_lanelet_ids)
2243 traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
2244 if(!traffic_lights.empty())
2246 isLightFound =
true;
2251 if(isLightFound &&
sim_)
2253 for(
auto itr =
sim_->signal_group_to_traffic_light_id_.begin(); itr !=
sim_->signal_group_to_traffic_light_id_.end(); itr++)
2255 if(itr->second == traffic_lights.front()->id())
2257 cur_signal_group_id = itr->first;
2262 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"NO matching Traffic lights along the route");
2265 auto intersections = route_lanelet.regulatoryElementsAs<lanelet::SignalizedIntersection>();
2266 if (intersections.empty())
2269 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"NO matching intersection for current lanelet. lanelet id = " << route_lanelet.id());
2274 lanelet::Id intersection_id = intersections.front()->id();
2275 if(intersection_id != lanelet::InvalId)
2277 for(
auto itr =
sim_->intersection_id_to_regem_id_.begin(); itr !=
sim_->intersection_id_to_regem_id_.end(); itr++)
2279 if(itr->second == intersection_id)
2281 map_msg_intersection_id = itr->first;
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)
2298 carma_v2x_msgs::msg::MobilityOperation mom_msg;
2299 mom_msg.m_header.timestamp =
scheduler_.
now().nanoseconds()/1000000;
2302 std::stringstream ss;
2303 for(
size_t i=0;
i < tcm_req_id.id.size();
i++)
2305 ss << std::setfill(
'0') << std::setw(2) << std::hex << (unsigned) tcm_req_id.id.at(
i);
2307 std::string tcmv01_req_id_hex = 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();
Position in a track based coordinate system where the axis are downtrack and crosstrack....
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 ®em, 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 ¤t_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....
std::string base_map_georef_
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.
PublishMapCallback map_pub_
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)
size_t current_map_version_
carma_v2x_msgs::msg::TrafficControlRequestPolygon tcr_polygon_
GeofenceScheduler scheduler_
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 ®em, 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
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...
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
const std::string MAP_MSG_INTERSECTION
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)
static const uint8_t OPEN
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