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(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(0.0),
197 rclcpp::Duration(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(0.0),
206 rclcpp::Duration(86400.0e9),
207 rclcpp::Duration(0.0),
208 rclcpp::Duration(86400.0e9),
209 rclcpp::Duration(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);
243 updates_to_send.push_back(update);
246 for (
auto signal : traffic_signals)
248 auto update = std::make_shared<Geofence>();
249 update->id_ = boost::uuids::random_generator()();
251 update->regulatory_element_ = signal;
252 for (
auto llt : signal->getControlStartLanelets())
254 update->affected_parts_.push_back(llt);
256 updates_to_send.push_back(update);
259 return updates_to_send;
264 bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find(
"SIG_WZ") != std::string::npos;
265 carma_v2x_msgs::msg::TrafficControlDetail msg_detail = msg_v01.params.detail;
268 std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin());
274 if (gf_ptr->affected_parts_.size() == 0) {
275 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"There is no applicable component in map for the new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
279 std::vector<lanelet::Lanelet> affected_llts;
280 std::vector<lanelet::Area> affected_areas;
283 for (
auto llt_or_area : gf_ptr->affected_parts_)
286 if (llt_or_area.isLanelet()) affected_llts.push_back(
current_map_->laneletLayer.get(llt_or_area.lanelet()->id()));
287 if (llt_or_area.isArea()) affected_areas.push_back(
current_map_->areaLayer.get(llt_or_area.area()->id()));
293 lanelet::Velocity sL;
296 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
299 sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPS());
300 std::string reason =
"";
301 if (msg_v01.package.label_exists)
302 reason = msg_v01.package.label;
310 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital maximum speed limit is invalid. Value capped at max speed limit.");
316 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital speed limit is invalid. Value set to 0mph.");
320 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
323 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE)
326 sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPS());
330 std::string reason =
"";
331 if (msg_v01.package.label_exists)
332 reason = msg_v01.package.label;
338 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital speed limit is invalid. Value capped at max speed limit.");
343 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital speed limit is invalid. Value set to 0mph.");
346 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
349 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE)
353 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE)
356 double min_gap = (double)msg_detail.minhdwy;
360 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Digital min gap is invalid. Value set to 0 meter.");
366 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
368 gf_ptr->label_ = msg_v01.package.label;
369 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
370 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
371 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
375 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
381 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==carma_v2x_msgs::msg::TrafficControlDetail::CLOSED)
386 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::RESTRICTED_CHOICE) {
395 std::shared_ptr<std::vector<lanelet::Lanelet>> parallel_llts = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
396 std::shared_ptr<std::vector<lanelet::Lanelet>> middle_opposite_lanelets = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
402 auto gf_ptr =
createWorkzoneGeometry(work_zone_geofence_cache, parallel_llts->front(), parallel_llts->back(), middle_opposite_lanelets );
411 for (
auto pair : work_zone_geofence_cache)
413 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Workzone geofence finished processing. Therefore following geofence id is being dropped from cache as it is processed as part of it: " << pair.second->id_);
415 work_zone_geofence_cache.clear();
420std::shared_ptr<Geofence>
WMBroadcaster::createWorkzoneGeometry(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache, lanelet::Lanelet parallel_llt_front, lanelet::Lanelet parallel_llt_back,
421 std::shared_ptr<std::vector<lanelet::Lanelet>> middle_opposite_lanelets)
423 auto gf_ptr = std::make_shared<Geofence>();
430 middle_opposite_lanelets->back().rightBound3d().back(), middle_opposite_lanelets->back().leftBound3d().back());
431 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Created diag front_llt_diag id:" << front_llt_diag.id());
432 for (
auto regem : middle_opposite_lanelets->back().regulatoryElements())
434 front_llt_diag.addRegulatoryElement(regem);
441 lanelet::Lanelet back_llt_diag =
createLinearInterpolatingLanelet(middle_opposite_lanelets->front().rightBound3d().front(), middle_opposite_lanelets->front().leftBound3d().front(),
442 parallel_llt_back.leftBound3d().front(), parallel_llt_back.rightBound3d().front());
443 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Created back_llt_diag diag id:" << back_llt_diag.id());
444 for (
auto regem : parallel_llt_back.regulatoryElements())
446 back_llt_diag.addRegulatoryElement(regem);
453 std::vector<lanelet::Lanelet> middle_llts;
454 for (
int i = middle_opposite_lanelets->size() - 1;
i >= 0;
i--)
456 lanelet::Lanelet middle_llt (lanelet::utils::getId(), (*(middle_opposite_lanelets.get()))[
i].rightBound3d().invert(), (*(middle_opposite_lanelets.get()))[
i].leftBound3d().invert());
457 for (
auto regem : (*(middle_opposite_lanelets.get()))[
i].regulatoryElements())
459 middle_llt.addRegulatoryElement(regem);
461 middle_llts.push_back(middle_llt);
462 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Created matching direction of middle_llt id:" << middle_llt.id());
468 lanelet::LineString3d parallel_stop_ls(lanelet::utils::getId(), {parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back()});
470 std::vector<lanelet::Lanelet> controlled_taper_right;
472 controlled_taper_right.push_back(parallel_llt_front);
474 controlled_taper_right.push_back(front_llt_diag);
476 controlled_taper_right.insert(controlled_taper_right.end(), middle_llts.begin(), middle_llts.end());
478 controlled_taper_right.push_back(back_llt_diag);
480 lanelet::CarmaTrafficSignalPtr tfl_parallel = std::make_shared<lanelet::CarmaTrafficSignal>(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {parallel_stop_ls}, {controlled_taper_right.front()}, {controlled_taper_right.back()}));
484 parallel_llt_front.addRegulatoryElement(tfl_parallel);
485 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Created TF_LIGHT of Id: " << tfl_parallel->id() <<
", to parallel_llt_front id:" << parallel_llt_front.id());
490 std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts_with_stop_line = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
495 lanelet::LineString3d opposite_stop_ls(lanelet::utils::getId(), {opposite_llts_with_stop_line->front().leftBound3d().back(), opposite_llts_with_stop_line->front().rightBound3d().back()});
497 std::vector<lanelet::Lanelet> controlled_open_right;
499 controlled_open_right.insert(controlled_open_right.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());;
503 controlled_open_right.push_back(
current_map_->laneletLayer.get(llt.lanelet().get().id()));
506 lanelet::CarmaTrafficSignalPtr tfl_opposite = std::make_shared<lanelet::CarmaTrafficSignal>(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {opposite_stop_ls}, {controlled_open_right.front()}, {controlled_open_right.back()}));
510 opposite_llts_with_stop_line->front().addRegulatoryElement(tfl_opposite);
511 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Created TF_LIGHT of Id: " << tfl_opposite->id() <<
", to opposite_llts_with_stop_line->front() id:" << opposite_llts_with_stop_line->front().id());
517 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Added parallel_llt_front id:" << parallel_llt_front.id());
518 gf_ptr->lanelet_additions_.push_back(parallel_llt_front);
520 gf_ptr->lanelet_additions_.push_back(front_llt_diag);
522 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), middle_llts.begin(), middle_llts.end());
524 gf_ptr->lanelet_additions_.push_back(back_llt_diag);
525 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Added parallel_llt_back id:" << parallel_llt_back.id());
526 gf_ptr->lanelet_additions_.push_back(parallel_llt_back);
528 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());;
536 carma_v2x_msgs::msg::TrafficControlMessageV01 participants_and_reason_only;
538 j2735_v2x_msgs::msg::TrafficControlVehClass participant;
540 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE;
542 participants_and_reason_only.params.vclasses.push_back(participant);
544 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BUS;
546 participants_and_reason_only.params.vclasses.push_back(participant);
548 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR;
550 participants_and_reason_only.params.vclasses.push_back(participant);
552 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK;
554 participants_and_reason_only.params.vclasses.push_back(participant);
556 participants_and_reason_only.package.label =
"SIG_WZ";
558 std::vector<lanelet::Lanelet> old_or_blocked_llts;
563 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
565 gf_ptr->affected_parts_.push_back(
current_map_->laneletLayer.get(llt.lanelet()->id()));
566 old_or_blocked_llts.push_back(
current_map_->laneletLayer.get(llt.lanelet()->id()));
571 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
573 gf_ptr->affected_parts_.push_back(
current_map_->laneletLayer.get(llt.lanelet()->id()));
574 old_or_blocked_llts.push_back(
current_map_->laneletLayer.get(llt.lanelet()->id()));
579 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
581 gf_ptr->affected_parts_.push_back(
current_map_->laneletLayer.get(llt.lanelet()->id()));
582 old_or_blocked_llts.push_back(
current_map_->laneletLayer.get(llt.lanelet()->id()));
586 gf_ptr->affected_parts_.push_back(old_opposite_llts[0]);
587 old_or_blocked_llts.push_back(old_opposite_llts[0]);
600void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_map<uint8_t, std::shared_ptr<Geofence>> work_zone_geofence_cache, std::shared_ptr<std::vector<lanelet::Lanelet>> parallel_llts, std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts)
604 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map is not loaded to the WMBroadcaster"));
609 std::vector <lanelet::Lanelet> new_taper_right_llts;
614 double check_dist_tpr = lanelet::geometry::distance2d(taper_right_first_llt.centerline2d().front().basicPoint2d(), taper_right_first_pt);
618 if (new_taper_right_llts.size() == 1 && check_dist_tpr <=
error_distance_)
620 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating duplicate lanelet of 'previous lanelet' due to TAPERRIGHT using entire lanelet...");
622 if (previous_lanelets.empty())
624 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Workzone area starts from lanelet with no previous lanelet (Id : " << work_zone_geofence_cache[
WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id()
625 <<
". This case is rare and not supported at the moment.");
630 auto prev_lanelet_to_copy =
current_map_->laneletLayer.get(previous_lanelets.front().id());
635 parallel_llts->insert(parallel_llts->end(), new_taper_right_llts.begin(), new_taper_right_llts.end());
636 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Finished TAPERRIGHT processing of size: " << new_taper_right_llts.size());
641 std::vector <lanelet::Lanelet> new_open_right_llts;
646 double check_dist_opr = lanelet::geometry::distance2d(open_right_last_llt.centerline2d().back().basicPoint2d(), open_right_last_pt);
650 if (new_open_right_llts.size() == 1 && check_dist_opr <=
error_distance_)
652 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating duplicate lanelet of 'next lanelet' due to OPENRIGHT using entire lanelet...");
654 if (next_lanelets.empty())
656 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Workzone area ends at lanelet with no following lanelet (Id : " << work_zone_geofence_cache[
WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id()
657 <<
". This case is rare and not supported at the moment.");
662 auto next_lanelet_to_copy =
current_map_->laneletLayer.get(next_lanelets.front().id());
667 parallel_llts->insert(parallel_llts->end(), new_open_right_llts.begin(), new_open_right_llts.end());
668 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Finished OPENRIGHT processing of size: " << new_open_right_llts.size());
675 auto reverse_back_llt = reverse_back_llts[0];
677 auto reverse_front_llt = reverse_front_llts[0];
679 if (reverse_back_llt.id() == reverse_front_llt.id())
681 std::vector<lanelet::Lanelet> temp_llts;
686 if (temp_llts.size() < 2)
689 opposite_llts->insert(opposite_llts->end(), temp_llts.begin(), temp_llts.end());
690 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() <<
", and parallel_llts.size()" << parallel_llts->size());
693 else if (temp_llts.size() == 2)
696 if (lanelet::geometry::distance2d(work_zone_geofence_cache[
WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d(), reverse_front_llt.centerline2d().back().basicPoint2d()) >
697 lanelet::geometry::distance2d(work_zone_geofence_cache[
WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(), reverse_front_llt.centerline2d().front().basicPoint2d()))
699 opposite_llts->push_back(temp_llts.back());
703 opposite_llts->push_back(temp_llts.front());
706 else if (temp_llts.size() == 3)
708 opposite_llts->insert(opposite_llts->end(), temp_llts.begin() + 1, temp_llts.end()- 1);
710 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Finished REVERSE processing of size: " << opposite_llts->size() <<
" from original of 1 REVERSE lanelet size");
717 std::vector<lanelet::Lanelet> temp_opposite_front_llts;
719 if (temp_opposite_front_llts.size() > 1)
721 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin() + 1, temp_opposite_front_llts.end());
725 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin(), temp_opposite_front_llts.end());
739 std::vector<lanelet::Lanelet> temp_opposite_back_llts;
741 if (temp_opposite_back_llts.size() > 1)
743 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end()- 1);
747 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end());
749 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Finished REVERSE processing of size: " << opposite_llts->size() <<
" from original of more than one REVERSE lanelet size");
752 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() <<
", and parallel_llts.size()" << parallel_llts->size());
758 std::vector<lanelet::Lanelet> parallel_llts;
760 std::vector<double> ratios;
762 for (
auto pt : input_pts)
765 double point_downtrack_ratio = point_downtrack / llt_downtrack;
766 ratios.push_back(point_downtrack_ratio);
771 parallel_llts.insert(parallel_llts.end(),new_parallel_llts.begin(), new_parallel_llts.end());
772 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"splitLaneletWithPoint returning lanelets size: " << parallel_llts.size());
773 return parallel_llts;
785 if (opposing_llts.empty())
787 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"WMBroadcaster was not able to find opposing lane for given point in geofence related to Work Zone! Returning");
791 auto new_llts_opposite =
splitLaneletWithRatio({1 - point_downtrack_ratio}, opposing_llts[0], error_distance);
792 opposite_llts->insert(opposite_llts->begin(),new_llts_opposite.begin(), new_llts_opposite.end());
793 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"splitOppositeLaneletWithPoint returning lanelets size: " << opposite_llts->size());
794 return opposing_llts;
801 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map is not loaded to the WMBroadcaster"));
805 throw lanelet::InvalidInputError(std::string(
"Ratios list is empty! Cannot split"));
808 std::vector<lanelet::Lanelet> created_llts;
810 std::sort(ratios.begin(), ratios.end());
811 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"splitLaneletWithRatio evaluating input ratios of size: " << ratios.size());
813 ratios.push_back(1.0);
815 int left_ls_size = input_lanelet.leftBound2d().size();
816 int right_ls_size = input_lanelet.rightBound2d().size();
818 int left_next_pt_idx = 0;
819 int left_prev_pt_idx = 0;
820 int right_next_pt_idx = 0;
821 int right_prev_pt_idx = 0;
822 for (
int i = 0 ;
i < ratios.size();
i ++)
824 left_next_pt_idx = std::round(ratios[
i] * (left_ls_size - 1));
825 right_next_pt_idx = std::round(ratios[
i] * (right_ls_size - 1));
827 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
831 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Ratio: " << ratios[
i] <<
", is too close to the lanelet's front boundary! Therefore, ignoring... Allowed error_distance: " << error_distance <<
", Distance: "
832 << lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
835 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
837 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Ratio: " << ratios[
i] <<
", is too close to the lanelet's back boundary! Therefore, ignoring... Allowed error_distance: " << error_distance <<
", Distance: "
838 << lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
840 left_next_pt_idx = left_ls_size - 1;
841 right_next_pt_idx = right_ls_size - 1;
844 std::vector<lanelet::Point3d> left_pts;
845 left_pts.insert(left_pts.end(),
current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_prev_pt_idx,
current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_next_pt_idx + 1);
847 lanelet::LineString3d left_ls(lanelet::utils::getId(), left_pts,
current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().attributes());
849 std::vector<lanelet::Point3d> right_pts;
850 right_pts.insert(right_pts.end(),
current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_prev_pt_idx,
current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_next_pt_idx + 1);
852 lanelet::LineString3d right_ls(lanelet::utils::getId(), right_pts);
854 lanelet::Lanelet new_llt (lanelet::utils::getId(), left_ls, right_ls,
current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().attributes());
856 for (
auto regem :
current_map_->laneletLayer.get(input_lanelet.id()).regulatoryElements())
858 new_llt.addRegulatoryElement(
current_map_->regulatoryElementLayer.get(regem->id()));
861 left_prev_pt_idx = left_next_pt_idx;
862 right_prev_pt_idx = right_next_pt_idx;
864 created_llts.push_back(new_llt);
867 if (left_prev_pt_idx == left_ls_size - 1 || right_prev_pt_idx == right_ls_size - 1)
874 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"splitLaneletWithRatio returning lanelets size: " << created_llts.size());
882 carma_v2x_msgs::msg::TrafficControlDetail msg_detail;
883 msg_detail = msg_v01.params.detail;
885 lanelet::LineStrings3d pcl_bounds;
886 if (msg_detail.lataffinity == carma_v2x_msgs::msg::TrafficControlDetail::LEFT)
888 for (
auto llt : affected_llts) pcl_bounds.push_back(llt.leftBound());
889 gf_ptr->pcl_affects_left_ =
true;
893 for (
auto llt : affected_llts) pcl_bounds.push_back(llt.rightBound());
894 gf_ptr->pcl_affects_right_ =
true;
898 std::vector<std::string> left_participants;
899 std::vector<std::string> right_participants;
903 if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
904 msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
906 left_participants = participants;
908 else if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
910 left_participants.push_back(lanelet::Participants::VehicleEmergency);
912 if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
913 msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
915 right_participants = participants;
917 else if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
919 right_participants.push_back(lanelet::Participants::VehicleEmergency);
922 gf_ptr->regulatory_element_ = std::make_shared<lanelet::PassingControlLine>(lanelet::PassingControlLine::buildData(
923 lanelet::utils::getId(), pcl_bounds, left_participants, right_participants));
926void WMBroadcaster::addRegionAccessRule(std::shared_ptr<Geofence> gf_ptr,
const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01,
const std::vector<lanelet::Lanelet>& affected_llts)
const
928 const std::string& reason = msg_v01.package.label;
929 gf_ptr->label_ = msg_v01.package.label;
930 auto regulatory_element = std::make_shared<lanelet::RegionAccessRule>(lanelet::RegionAccessRule::buildData(lanelet::utils::getId(),affected_llts,{},
invertParticipants(
participantsChecker(msg_v01)), reason));
932 if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck ))
934 gf_ptr->invalidate_route_=
true;
938 gf_ptr->invalidate_route_=
false;
940 gf_ptr->regulatory_element_ = regulatory_element;
943void WMBroadcaster::addRegionMinimumGap(std::shared_ptr<Geofence> gf_ptr,
const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01,
double min_gap,
const std::vector<lanelet::Lanelet>& affected_llts,
const std::vector<lanelet::Area>& affected_areas)
const
945 auto regulatory_element = std::make_shared<lanelet::DigitalMinimumGap>(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(),
948 gf_ptr->regulatory_element_ = regulatory_element;
953 std::vector<std::string> participants;
954 for (j2735_v2x_msgs::msg::TrafficControlVehClass participant : msg_v01.params.vclasses)
957 if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::ANY)
959 participants = {lanelet::Participants::Vehicle, lanelet::Participants::Pedestrian, lanelet::Participants::Bicycle};
962 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN)
964 participants.push_back(lanelet::Participants::Pedestrian);
966 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BICYCLE)
968 participants.push_back(lanelet::Participants::Bicycle);
970 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE ||
971 participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MOTORCYCLE)
973 participants.push_back(lanelet::Participants::VehicleMotorcycle);
975 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BUS)
977 participants.push_back(lanelet::Participants::VehicleBus);
979 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::LIGHT_TRUCK_VAN ||
980 participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR)
982 participants.push_back(lanelet::Participants::VehicleCar);
984 else if (8<= participant.vehicle_class && participant.vehicle_class <= 16)
986 participants.push_back(lanelet::Participants::VehicleTruck);
995 std::vector<std::string> participants;
997 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Pedestrian ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Pedestrian);
998 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Bicycle ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Bicycle);
999 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Vehicle ) == input_participants.end())
1001 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleMotorcycle);
1002 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleBus);
1003 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleCar);
1004 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleTruck);
1006 return participants;
1011 std::vector<std::string> participants;
1013 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)!= input_participants.end() &&
1014 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus) != input_participants.end() &&
1015 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar) != input_participants.end() &&
1016 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck) != input_participants.end())
1018 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Detected participants to cover all possible vehicle types");
1019 participants.emplace_back(lanelet::Participants::Vehicle);
1023 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Not making any changes to the participants list");
1024 participants = input_participants;
1027 return participants;
1032 auto gf_ptr = std::make_shared<Geofence>();
1036 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Map is not available yet. Skipping MAP msg");
1041 bool up_to_date =
false;
1042 if (
sim_->intersection_id_to_regem_id_.size() == map_msg->intersections.size())
1046 for (
auto intersection : map_msg->intersections)
1048 if (
sim_->intersection_id_to_regem_id_.find(intersection.id.id) ==
sim_->intersection_id_to_regem_id_.end())
1061 gf_ptr->map_msg_ = *map_msg;
1062 gf_ptr->msg_.package.label_exists =
true;
1063 gf_ptr->msg_.package.label =
"MAP_MSG";
1064 gf_ptr->id_ = boost::uuids::random_generator()();
1067 carma_v2x_msgs::msg::TrafficControlMessageV01 traffic_control_msg;
1080 std::lock_guard<std::mutex> guard(
map_mutex_);
1081 std::stringstream reason_ss;
1083 if (geofence_msg->choice != carma_v2x_msgs::msg::TrafficControlMessage::TCMV01) {
1084 reason_ss <<
"Dropping received geofence for unsupported TrafficControl version: " << geofence_msg->choice;
1085 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"), reason_ss.str());
1090 boost::uuids::uuid id;
1091 std::copy(geofence_msg->tcm_v01.id.id.begin(), geofence_msg->tcm_v01.id.id.end(),
id.begin());
1094 reason_ss <<
"Dropping received TrafficControl message with already handled id: " <<
boost::uuids::to_string(
id);
1095 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"), reason_ss.str());
1101 boost::array<uint8_t, 16UL> req_id;
1102 for (
auto i = 0;
i < 8;
i ++) req_id[
i] = geofence_msg->tcm_v01.reqid.id[
i];
1103 boost::uuids::uuid uuid_id;
1104 std::copy(req_id.begin(),req_id.end(), uuid_id.begin());
1109 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid);
1115 auto gf_ptr = std::make_shared<Geofence>();
1117 gf_ptr->msg_ = geofence_msg->tcm_v01;
1125 reason_ss <<
"Successfully processed TCM.";
1128 catch(std::exception& ex)
1131 reason_ss <<
"Failed to process TCM. " << ex.what();
1139 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
1141 bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find(
"SIG_WZ") != std::string::npos;
1143 carma_v2x_msgs::msg::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail;
1146 if (detected_workzone_signal && msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1151 auto gf_ptr_speed = std::make_shared<Geofence>();
1152 gf_ptr_speed->schedules = gf_ptr->schedules;
1154 carma_v2x_msgs::msg::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_;
1156 std::reverse(duplicate_msg.geometry.nodes.begin() + 1, duplicate_msg.geometry.nodes.end());
1161 for (
auto& pt: duplicate_msg.geometry.nodes)
1168 duplicate_msg.geometry.nodes[0].x = first_x;
1169 duplicate_msg.geometry.nodes[0].y = first_y;
1171 gf_ptr_speed->msg_ = duplicate_msg;
1174 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1176 gf_ptr->label_ = gf_ptr->msg_.package.label;
1177 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
1178 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
1179 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
1183 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
1189 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now...");
1200 std::lock_guard<std::mutex> guard(
map_mutex_);
1201 sim_->setTargetFrame(geo_ref->data);
1207 sim_ = std::make_shared<carma_wm::SignalizedIntersectionManager>();
1215 if (intersection_correction.size() % 2 != 0 || intersection_ids_for_correction.size() != intersection_correction.size() / 2)
1217 throw std::invalid_argument(
"Some of intersection coordinate correction parameters are not fully set!");
1220 for (
auto i = 0;
i < intersection_correction.size();
i =
i + 2)
1222 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[
i/2]].first = intersection_correction[
i];
1223 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[
i/2]].second = intersection_correction[
i + 1];
1231 config_limit = lanelet::Velocity(cL * lanelet::units::MPH());
1254 auto pos1 = label.find(
"INT_ID:") + 7;
1255 auto pos2 = label.find(
"SG_ID:") + 6;
1257 uint16_t intersection_id = std::stoi(label.substr(pos1 , 4));
1258 uint8_t signal_id = std::stoi(label.substr(pos2 , 3));
1266 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Getting affected lanelets");
1269 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map is not loaded to the WMBroadcaster"));
1272 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map has empty proj string loaded as georeference. Therefore, WMBroadcaster failed to\n ") +
1273 std::string(
"get transformation between the geofence and the map"));
1279 std::string projection = tcm_v01.geometry.proj;
1280 std::string datum = tcm_v01.geometry.datum;
1281 if (datum.empty()) {
1282 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Datum field not populated. Attempting to use WGS84");
1286 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Projection field: " << projection);
1287 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Datum field: " << datum);
1289 std::string universal_frame = datum;
1292 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Traffic Control heading provided: " << tcm_v01.geometry.heading <<
" System understanding is that this value will not affect the projection and is only provided for supporting derivative calculations.");
1295 PJ* universal_to_target = proj_create_crs_to_crs(PJ_DEFAULT_CTX, universal_frame.c_str(), projection.c_str(),
nullptr);
1296 if (universal_to_target ==
nullptr) {
1298 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1299 <<
" universal_frame: " << universal_frame <<
" projection: " << projection);
1304 PJ* target_to_map = proj_create_crs_to_crs(PJ_DEFAULT_CTX, projection.c_str(),
base_map_georef_.c_str(),
nullptr);
1306 if (target_to_map ==
nullptr) {
1308 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1309 <<
" target_to_map: " << target_to_map <<
" base_map_georef_: " <<
base_map_georef_);
1316 std::vector<lanelet::Point3d> gf_pts;
1317 carma_v2x_msgs::msg::PathNode prev_pt;
1318 PJ_COORD c_init_latlong{{tcm_v01.geometry.reflat, tcm_v01.geometry.reflon, tcm_v01.geometry.refelv}};
1319 PJ_COORD c_init = proj_trans(universal_to_target, PJ_FWD, c_init_latlong);
1321 prev_pt.x = c_init.xyz.x;
1322 prev_pt.y = c_init.xyz.y;
1324 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"In TCM's frame, initial Point X "<< prev_pt.x<<
" Before conversion: Point Y "<< prev_pt.y );
1325 for (
auto pt : tcm_v01.geometry.nodes)
1327 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Before conversion in TCM frame: Point X "<< pt.x <<
" Before conversion: Point Y "<< pt.y);
1329 PJ_COORD
c {{prev_pt.x + pt.x, prev_pt.y + pt.y, 0, 0}};
1331 c_out = proj_trans(target_to_map, PJ_FWD,
c);
1333 gf_pts.push_back(lanelet::Point3d{
current_map_->pointLayer.uniqueId(), c_out.xyz.x, c_out.xyz.y});
1337 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"After conversion in Map frame: Point X "<< gf_pts.back().x() <<
" After conversion: Point Y "<< gf_pts.back().y());
1360 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) != 0 || !el.isLanelet())
1365 lanelet::PassingControlLinePtr
pcl = std::dynamic_pointer_cast<lanelet::PassingControlLine>(
current_map_->regulatoryElementLayer.get(regem->id()));
1367 bool should_change_pcl =
false;
1368 for (
auto control_line :
pcl->controlLine())
1370 if ((control_line.id() == el.lanelet()->leftBound2d().id() && gf_ptr->pcl_affects_left_) ||
1371 (control_line.id() == el.lanelet()->rightBound2d().id() && gf_ptr->pcl_affects_right_))
1373 should_change_pcl =
true;
1377 return should_change_pcl;
1391 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::CarmaTrafficSignal::RuleName) != 0 || !el.isLanelet() || !
sim_)
1396 lanelet::CarmaTrafficSignalPtr traffic_signal = std::dynamic_pointer_cast<lanelet::CarmaTrafficSignal>(
current_map_->regulatoryElementLayer.get(regem->id()));
1397 uint8_t signal_id = 0;
1399 for (
auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it)
1401 if (regem->id() == it->second)
1403 signal_id = it->first;
1410 if (sim->signal_group_to_entry_lanelet_ids_[signal_id].find(el.id()) != sim->signal_group_to_entry_lanelet_ids_[signal_id].end())
1420 for (
auto el: gf_ptr->affected_parts_)
1422 for (
auto regem : el.regulatoryElements())
1428 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1430 lanelet::RegulatoryElementPtr nonconst_regem =
current_map_->regulatoryElementLayer.get(regem->id());
1431 gf_ptr->prev_regems_.push_back(std::make_pair(el.id(), nonconst_regem));
1432 gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
1441 for (
auto el: gf_ptr->affected_parts_)
1444 if (gf_ptr->regulatory_element_->id() != lanelet::InvalId)
1447 gf_ptr->update_list_.push_back(std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>(el.id(), gf_ptr->regulatory_element_));
1449 RCLCPP_WARN_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Regulatory element with invalid id in geofence cannot be added to the map");
1461 for (
auto el: gf_ptr->affected_parts_)
1463 for (
auto regem : el.regulatoryElements())
1467 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1469 auto nonconst_regem =
current_map_->regulatoryElementLayer.get(regem->id());
1470 gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
1478 for (
auto pair : gf_ptr->prev_regems_)
1480 if (pair.second->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1483 gf_ptr->update_list_.push_back(pair);
1491 std::lock_guard<std::mutex> guard(
map_mutex_);
1492 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Adding active geofence to the map with geofence id: " << gf_ptr->id_);
1496 std::vector<std::shared_ptr<Geofence>> updates_to_send;
1498 bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find(
"SIG_WZ") != std::string::npos;
1499 bool detected_map_msg_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find(
"MAP_MSG") != std::string::npos;
1500 if (detected_workzone_signal && gf_ptr->msg_.params.detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1508 else if (detected_map_msg_signal)
1515 updates_to_send.push_back(gf_ptr);
1518 for (
auto update : updates_to_send)
1523 if (update->affected_parts_.empty())
1529 if (!detected_map_msg_signal)
1534 autoware_lanelet2_msgs::msg::MapBin gf_msg;
1537 if (update->invalidate_route_) {
1539 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Rebuilding routing graph after is was invalidated by geofence");
1541 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1542 lanelet::traffic_rules::CarmaUSTrafficRules::Location,
participant_);
1545 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done rebuilding routing graph after is was invalidated by geofence");
1548 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating routing graph message");
1552 gf_msg.routing_graph = readable_graph->routingGraphToMsg(
participant_);
1553 gf_msg.has_routing_graph =
true;
1555 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done creating routing graph message");
1560 auto send_data = std::make_shared<carma_wm::TrafficControl>(
carma_wm::TrafficControl(update->id_, update->update_list_, update->remove_list_, update->lanelet_additions_));
1561 send_data->traffic_light_id_lookup_ = update->traffic_light_id_lookup_;
1563 if (detected_map_msg_signal && updates_to_send.back() == update)
1565 send_data->sim_ = *
sim_;
1571 gf_msg.invalidates_route=update->invalidate_route_;
1580 std::lock_guard<std::mutex> guard(
map_mutex_);
1581 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Removing inactive geofence from the map with geofence id: " << gf_ptr->id_);
1584 if (gf_ptr->affected_parts_.empty())
1592 autoware_lanelet2_msgs::msg::MapBin gf_msg_revert;
1593 auto send_data = std::make_shared<carma_wm::TrafficControl>(
carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, {}));
1595 if (gf_ptr->invalidate_route_) {
1597 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Rebuilding routing graph after is was invalidated by geofence removal");
1599 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1600 lanelet::traffic_rules::CarmaUSTrafficRules::Location,
participant_
1604 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done rebuilding routing graph after is was invalidated by geofence removal");
1607 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Creating routing graph message for geofence removal");
1611 gf_msg_revert.routing_graph = readable_graph->routingGraphToMsg(
participant_);
1612 gf_msg_revert.has_routing_graph =
true;
1614 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Done creating routing graph message for geofence removal");
1635 carma_v2x_msgs::msg::TrafficControlRequest cR;
1643 lanelet::ConstLanelets path;
1648 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Value 'current_map_' does not exist.");
1649 throw lanelet::InvalidObjectStateError(std::string(
"Base lanelet map is not loaded to the WMBroadcaster"));
1653 for(
auto id : route_msg.route_path_lanelet_ids)
1656 path.push_back(laneLayer);
1662 if(path.size() == 0)
throw lanelet::InvalidObjectStateError(std::string(
"No lanelets available in path."));
1665 std::vector<lanelet::ConstLanelet> llt;
1666 std::vector<lanelet::BoundingBox2d> pathBox;
1667 double minX = std::numeric_limits<double>::max();
1668 double minY = std::numeric_limits<double>::max();
1669 double maxX = std::numeric_limits<double>::lowest();
1670 double maxY = std::numeric_limits<double>::lowest();
1672 while (path.size() != 0)
1674 llt.push_back(path.back());
1677 pathBox.push_back(lanelet::geometry::boundingBox2d(llt.back()));
1680 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x() < minX)
1681 minX = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x();
1684 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y() < minY)
1685 minY = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y();
1689 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x() > maxX)
1690 maxX = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x();
1693 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y() > maxY)
1694 maxY = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y();
1702 if (target_frame.empty())
1705 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Value 'target_frame' is empty.");
1706 throw lanelet::InvalidObjectStateError(std::string(
"Base georeference map may not be loaded to the WMBroadcaster"));
1711 lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
1712 lanelet::BasicPoint3d localPoint;
1714 localPoint.x()= minX;
1715 localPoint.y()= minY;
1717 lanelet::GPSPoint gpsRoute = local_projector.reverse(localPoint);
1720 std::string local_tmerc_enu_proj =
"+proj=tmerc +datum=WGS84 +h_0=0 +lat_0=" +
std::to_string(gpsRoute.lat) +
" +lon_0=" +
std::to_string(gpsRoute.lon);
1723 PJ* tmerc_proj = proj_create_crs_to_crs(PJ_DEFAULT_CTX, target_frame.c_str(), local_tmerc_enu_proj.c_str(),
nullptr);
1725 if (tmerc_proj ==
nullptr) {
1727 RCLCPP_ERROR_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Failed to generate projection between request bounds frame and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1728 <<
" MapProjection: " << target_frame <<
" Message Projection: " << local_tmerc_enu_proj);
1734 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Before conversion: Top Left: ("<< minX <<
", "<<maxY<<
")");
1735 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Before conversion: Top Right: ("<< maxX <<
", "<<maxY<<
")");
1736 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Before conversion: Bottom Left: ("<< minX <<
", "<<minY<<
")");
1737 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Before conversion: Bottom Right: ("<< maxX <<
", "<<minY<<
")");
1739 PJ_COORD pj_min {{minX, minY, 0, 0}};
1740 PJ_COORD pj_min_tmerc;
1741 PJ_COORD pj_max {{maxX, maxY, 0, 0}};
1742 PJ_COORD pj_max_tmerc;
1743 pj_min_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_min);
1744 pj_max_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_max);
1746 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"After conversion: MinPoint ( "<< pj_min_tmerc.xyz.x <<
", " << pj_min_tmerc.xyz.y <<
" )");
1747 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"After conversion: MaxPoint ( "<< pj_max_tmerc.xyz.x <<
", " << pj_max_tmerc.xyz.y <<
" )");
1749 carma_v2x_msgs::msg::TrafficControlRequest cR;
1750 carma_v2x_msgs::msg::TrafficControlBounds cB;
1752 cB.reflat = gpsRoute.lat;
1753 cB.reflon = gpsRoute.lon;
1755 cB.offsets[0].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x;
1756 cB.offsets[0].deltay = 0.0;
1757 cB.offsets[1].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x;
1758 cB.offsets[1].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
1759 cB.offsets[2].deltax = 0.0;
1760 cB.offsets[2].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
1766 cR.choice = carma_v2x_msgs::msg::TrafficControlRequest::TCRV01;
1769 boost::uuids::uuid uuid_id = boost::uuids::random_generator()();
1772 std::string req_id_test =
"12345678";
1778 boost::array<uint8_t, 16UL> req_id;
1779 std::copy(uuid_id.begin(),uuid_id.end(), req_id.begin());
1780 for (
auto i = 0;
i < 8;
i ++)
1782 cR.tcr_v01.reqid.id[
i] = req_id[
i];
1783 if (req_id_for_testing) req_id_for_testing->id[
i] = req_id[
i];
1786 cR.tcr_v01.bounds.push_back(cB);
1792carma_v2x_msgs::msg::TrafficControlRequestPolygon
WMBroadcaster::composeTCRStatus(
const lanelet::BasicPoint3d& localPoint,
const carma_v2x_msgs::msg::TrafficControlBounds& cB,
const lanelet::projection::LocalFrameProjector& local_projector)
1794 carma_v2x_msgs::msg::TrafficControlRequestPolygon output;
1795 lanelet::BasicPoint3d local_point_tmp;
1802 local_point_tmp.x() = localPoint.x();
1803 local_point_tmp.y() = localPoint.y();
1807 local_point_tmp.x() = localPoint.x() + cB.offsets[
i].deltax;;
1808 local_point_tmp.y() = localPoint.y() + cB.offsets[
i].deltay;;
1810 lanelet::GPSPoint gps_vertex = local_projector.reverse(local_point_tmp);
1812 carma_v2x_msgs::msg::Position3D gps_msg;
1813 gps_msg.elevation = gps_vertex.ele;
1814 gps_msg.latitude = gps_vertex.lat;
1815 gps_msg.longitude = gps_vertex.lon;
1816 output.polygon_list.push_back(gps_msg);
1818 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"TCR Vertex Lat: "<<
std::to_string(gps_vertex.lat));
1819 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"TCR Vertex Lon: "<<
std::to_string(gps_vertex.lon));
1830 visualization_msgs::msg::Marker marker;
1831 marker.header.frame_id =
"map";
1832 marker.header.stamp = rclcpp::Time();
1833 marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
1834 marker.action = visualization_msgs::msg::Marker::ADD;
1835 marker.ns =
"route_visualizer";
1837 marker.scale.x = 0.65;
1838 marker.scale.y = 0.65;
1839 marker.scale.z = 0.65;
1840 marker.frame_locked =
true;
1850 marker.color.r = 0.0F;
1851 marker.color.g = 1.0F;
1852 marker.color.b = 0.0F;
1853 marker.color.a = 1.0F;
1855 for (
int i = 0;
i < input.size();
i++)
1857 geometry_msgs::msg::Point temp_point;
1858 temp_point.x = input[
i].x();
1859 temp_point.y = input[
i].y();
1862 marker.points.push_back(temp_point);
1870 std::lock_guard<std::mutex> guard(
map_mutex_);
1874 throw lanelet::InvalidObjectStateError(std::string(
"Lanelet map (current_map_) is not loaded to the WMBroadcaster"));
1878 std::vector<lanelet::Id> active_geofence_on_route;
1882 active_geofence_on_route.push_back(llt.id());
1885 auto curr_lanelet = lanelet::geometry::findNearest(
current_map_->laneletLayer, curr_pos, 1)[0].second;
1888 if (!boost::geometry::within(curr_pos, curr_lanelet.polygon2d().basicPolygon()))
1889 throw std::invalid_argument(
"Given point is not within any lanelet");
1892 std::vector<double> route_distances;
1894 for (
auto id: active_geofence_on_route)
1899 if (tp.
downtrack < 0 &&
id != curr_lanelet.id())
1902 route_distances.push_back(dist);
1905 std::sort(route_distances.begin(), route_distances.end());
1907 if (route_distances.size() != 0 )
return route_distances[0];
1915 gf_ptr->remove_list_ = {};
1916 gf_ptr->update_list_ = {};
1919 for (
auto llt : gf_ptr->lanelet_additions_)
1925 for (
auto pair : gf_ptr->traffic_light_id_lookup_)
1939 gf_ptr->remove_list_ = {};
1940 gf_ptr->update_list_ = {};
1943 gf_ptr->prev_regems_ = {};
1952 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Could not check active geofence logic because map was not loaded");
1961 if (it -> second == lanelet_id)
1963 group_id = (it -> first & 0xFF);
1964 intersection_id = (it -> first >> 8);
1979 bool convert_success =
false;
1980 unsigned intersection_id = 0;
1981 unsigned group_id = 0;
1982 auto route_lanelet=
current_map_->laneletLayer.get(
id);
1983 auto traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
1985 if (!traffic_lights.empty())
1987 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id());
1991 if (!convert_success)
1994 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Found Traffic Light with Intersection id: " << intersection_id <<
" Group id:" << group_id);
1995 bool id_exists =
false;
2017 throw lanelet::InvalidObjectStateError(std::string(
"Lanelet map 'current_map_' is not loaded to the WMBroadcaster"));
2021 double current_pos_x = current_pos.pose.position.x;
2022 double current_pos_y = current_pos.pose.position.y;
2027 lanelet::BasicPoint2d curr_pos;
2028 curr_pos.x() = current_pos_x;
2029 curr_pos.y() = current_pos_y;
2031 carma_perception_msgs::msg::CheckActiveGeofence outgoing_geof;
2032 double next_distance = 0 ;
2036 return outgoing_geof;
2039 RCLCPP_INFO_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Active geofence llt ids are loaded to the WMBroadcaster");
2042 auto current_llt = lanelet::geometry::findNearest(
current_map_->laneletLayer, curr_pos, 1)[0].second;
2046 if (boost::geometry::within(curr_pos, current_llt.polygon2d().basicPolygon()))
2049 outgoing_geof.distance_to_next_geofence = next_distance;
2053 if (
id == current_llt.id())
2055 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Vehicle is on Lanelet " << current_llt.id() <<
", which has an active geofence");
2056 outgoing_geof.is_on_active_geofence =
true;
2057 for (
auto regem: current_llt.regulatoryElements())
2060 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
2062 lanelet::DigitalSpeedLimitPtr speed = std::dynamic_pointer_cast<lanelet::DigitalSpeedLimit>
2063 (
current_map_->regulatoryElementLayer.get(regem->id()));
2064 outgoing_geof.value = speed->speed_limit_.value();
2065 outgoing_geof.advisory_speed = speed->speed_limit_.value();
2066 outgoing_geof.reason = speed->getReason();
2068 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Active geofence has a speed limit of " << speed->speed_limit_.value());
2071 if(outgoing_geof.type != carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED)
2073 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::SPEED_LIMIT;
2078 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalMinimumGap::RuleName) == 0)
2080 lanelet::DigitalMinimumGapPtr min_gap = std::dynamic_pointer_cast<lanelet::DigitalMinimumGap>
2081 (
current_map_->regulatoryElementLayer.get(regem->id()));
2082 outgoing_geof.minimum_gap = min_gap->getMinimumGap();
2083 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Active geofence has a minimum gap of " << min_gap->getMinimumGap());
2087 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2089 lanelet::RegionAccessRulePtr accessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2090 (
current_map_->regulatoryElementLayer.get(regem->id()));
2093 if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2095 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Active geofence is a closed lane.");
2096 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Closed lane reason: " << accessRuleReg->getReason());
2097 outgoing_geof.reason = accessRuleReg->getReason();
2098 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2104 auto right_boundary_lanelets =
current_map_->laneletLayer.findUsages(current_llt.rightBound());
2107 if(right_boundary_lanelets.size() > 1)
2109 for(
auto lanelet : right_boundary_lanelets)
2112 if(
lanelet.id() != current_llt.id())
2114 for (
auto rightRegem:
lanelet.regulatoryElements())
2116 if(rightRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2118 lanelet::RegionAccessRulePtr rightAccessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2119 (
current_map_->regulatoryElementLayer.get(rightRegem->id()));
2120 if(!rightAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !rightAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2122 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Right adjacent Lanelet " <<
lanelet.id() <<
" is CLOSED");
2123 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Assigning LANE_CLOSED type to active geofence");
2124 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Assigning reason " << rightAccessRuleReg->getReason());
2125 outgoing_geof.reason = rightAccessRuleReg->getReason();
2126 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2135 auto left_boundary_lanelets =
current_map_->laneletLayer.findUsages(current_llt.leftBound());
2136 if(left_boundary_lanelets.size() > 1)
2138 for(
auto lanelet : left_boundary_lanelets)
2141 if(
lanelet.id() != current_llt.id())
2143 for (
auto leftRegem:
lanelet.regulatoryElements())
2145 if(leftRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2147 lanelet::RegionAccessRulePtr leftAccessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2148 (
current_map_->regulatoryElementLayer.get(leftRegem->id()));
2149 if(!leftAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !leftAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2151 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Left adjacent Lanelet " <<
lanelet.id() <<
" is CLOSED");
2152 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Assigning LANE_CLOSED type to active geofence");
2153 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"Assigning reason " << leftAccessRuleReg->getReason());
2154 outgoing_geof.reason = leftAccessRuleReg->getReason();
2155 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2168 return outgoing_geof;
2173 double dx = back_pt.x() - front_pt.x();
2174 double dy = back_pt.y() - front_pt.y();
2176 std::vector<lanelet::Point3d> points;
2177 double distance = std::sqrt(pow(dx, 2) + pow(dy,2));
2178 double cos = dx / distance;
2179 double sin = dy / distance;
2180 points.push_back(front_pt);
2181 double sum = increment_distance;
2182 while ( sum < distance)
2184 points.push_back(lanelet::Point3d(lanelet::utils::getId(),front_pt.x() + sum * cos, front_pt.y() + sum * sin, 0.0));
2185 sum += increment_distance;
2187 points.push_back(back_pt);
2189 return lanelet::LineString3d(lanelet::utils::getId(), points);
2199 uint16_t map_msg_intersection_id = 0;
2200 uint16_t cur_signal_group_id = 0;
2201 std::vector<lanelet::CarmaTrafficSignalPtr> traffic_lights;
2202 lanelet::Lanelet route_lanelet;
2203 lanelet::Ids cur_route_lanelet_ids =
current_route.route_path_lanelet_ids;
2204 bool isLightFound =
false;
2206 for(
auto id : cur_route_lanelet_ids)
2209 traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
2210 if(!traffic_lights.empty())
2212 isLightFound =
true;
2217 if(isLightFound &&
sim_)
2219 for(
auto itr =
sim_->signal_group_to_traffic_light_id_.begin(); itr !=
sim_->signal_group_to_traffic_light_id_.end(); itr++)
2221 if(itr->second == traffic_lights.front()->id())
2223 cur_signal_group_id = itr->first;
2228 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"NO matching Traffic lights along the route");
2231 auto intersections = route_lanelet.regulatoryElementsAs<lanelet::SignalizedIntersection>();
2232 if (intersections.empty())
2235 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"NO matching intersection for current lanelet. lanelet id = " << route_lanelet.id());
2240 lanelet::Id intersection_id = intersections.front()->id();
2241 if(intersection_id != lanelet::InvalId)
2243 for(
auto itr =
sim_->intersection_id_to_regem_id_.begin(); itr !=
sim_->intersection_id_to_regem_id_.end(); itr++)
2245 if(itr->second == intersection_id)
2247 map_msg_intersection_id = itr->first;
2253 RCLCPP_DEBUG_STREAM(rclcpp::get_logger(
"carma_wm_ctrl"),
"MAP msg: Intersection ID = " << map_msg_intersection_id <<
", Signal Group ID =" << cur_signal_group_id );
2254 if(map_msg_intersection_id != 0 && cur_signal_group_id != 0)
2264 carma_v2x_msgs::msg::MobilityOperation mom_msg;
2265 mom_msg.m_header.timestamp =
scheduler_.
now().nanoseconds()/1000000;
2268 std::stringstream ss;
2269 for(
size_t i=0;
i < tcm_req_id.id.size();
i++)
2271 ss << std::setfill(
'0') << std::setw(2) << std::hex << (unsigned) tcm_req_id.id.at(
i);
2273 std::string tcmv01_req_id_hex = ss.str();
2275 ss <<
"traffic_control_id:" << tcmv01_req_id_hex <<
", msgnum:"<< msgnum <<
", acknowledgement:" << ack_status <<
", reason:" << ack_reason;
2276 mom_msg.strategy_params = ss.str();
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
PublishCtrlRequestCallback control_msg_pub_
const std::string geofence_ack_strategy_
void baseMapCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg)
Callback to set the base map when it has been loaded.
visualization_msgs::msg::Marker composeTCMMarkerVisualizer(const std::vector< lanelet::Point3d > &input)
composeTCMMarkerVisualizer() compose TCM Marker visualization
lanelet::Lanelets splitOppositeLaneletWithPoint(std::shared_ptr< std::vector< lanelet::Lanelet > > opposite_llts, const lanelet::BasicPoint2d &input_pt, const lanelet::Lanelet &input_llt, double error_distance)
Split given lanelet's adjacent, OPPOSITE lanelet with same proportion as the given point's downtrack ...
carma_planning_msgs::msg::Route current_route
std::vector< std::string > combineParticipantsToVehicle(const std::vector< std::string > &input_participants) const
Combines a list of the given participants into a single "vehicle" type if participants cover all poss...
double distToNearestActiveGeofence(const lanelet::BasicPoint2d &curr_pos)
Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet...
void removeGeofence(std::shared_ptr< Geofence > gf_ptr)
Removes a geofence from the current map and publishes the ROS msg.
lanelet::LaneletMapPtr base_map_
void setConfigVehicleId(const std::string &vehicle_id)
Retrieve the vehicle ID from global vehicle parameters, and set instance memeber vehicle id.
lanelet::Points3d getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01 &geofence_msg)
Extract geofence points from geofence message using its given proj and datum fields.
std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache_
bool convertLightIdToInterGroupId(unsigned &intersection_id, unsigned &group_id, const lanelet::Id &lanelet_id)
helper for generating intersection and group Id of a traffic light from lanelet id
lanelet::Velocity config_limit
lanelet::routing::RoutingGraphPtr current_routing_graph_
void addGeofenceHelper(std::shared_ptr< Geofence > gf_ptr)
std::shared_ptr< Geofence > createWorkzoneGeofence(std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache)
Creates a single workzone geofence (in the vector) that includes all additional lanelets (housing tra...
lanelet::ConstLanelets route_path_
void setMaxLaneWidth(double max_lane_width)
Sets the max lane width in meters. Geofence points are associated to a lanelet if they are within thi...
void geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr geofence_msg)
Callback to add a geofence to the map. Currently only supports version 1 TrafficControlMessage.
void addGeofence(std::shared_ptr< Geofence > gf_ptr)
Adds a geofence to the current map and publishes the ROS msg.
void setConfigSpeedLimit(double cL)
Sets the configured speed limit.
void currentLocationCallback(geometry_msgs::msg::PoseStamped::UniquePtr current_pos)
void addPassingControlLineFromMsg(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01, const std::vector< lanelet::Lanelet > &affected_llts) const
void setVehicleParticipationType(std::string participant)
Set the Vehicle Participation Type.
carma_perception_msgs::msg::CheckActiveGeofence checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped ¤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