Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
carma_wm_ctrl::WMBroadcaster Class Reference

Class which provies exposes map publication and carma_wm update logic. More...

#include <WMBroadcaster.hpp>

Collaboration diagram for carma_wm_ctrl::WMBroadcaster:
Collaboration graph

Public Types

using PublishMapCallback = std::function< void(const autoware_lanelet2_msgs::msg::MapBin &)>
 
using PublishMapUpdateCallback = std::function< void(const autoware_lanelet2_msgs::msg::MapBin &)>
 
using PublishCtrlRequestCallback = std::function< void(const carma_v2x_msgs::msg::TrafficControlRequest &)>
 
using PublishActiveGeofCallback = std::function< void(const carma_perception_msgs::msg::CheckActiveGeofence &)>
 
using PublishMobilityOperationCallback = std::function< void(const carma_v2x_msgs::msg::MobilityOperation &)>
 

Public Member Functions

 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. More...
 
void baseMapCallback (autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg)
 Callback to set the base map when it has been loaded. More...
 
void geoReferenceCallback (std_msgs::msg::String::UniquePtr geo_ref)
 Callback to set the base map georeference (proj string) More...
 
void geofenceCallback (carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr geofence_msg)
 Callback to add a geofence to the map. Currently only supports version 1 TrafficControlMessage. More...
 
void externalMapMsgCallback (carma_v2x_msgs::msg::MapData::UniquePtr map_msg)
 Callback to MAP.msg which contains intersections' static info such geometry and lane ids. More...
 
void addGeofence (std::shared_ptr< Geofence > gf_ptr)
 Adds a geofence to the current map and publishes the ROS msg. More...
 
void removeGeofence (std::shared_ptr< Geofence > gf_ptr)
 Removes a geofence from the current map and publishes the ROS msg. More...
 
void routeCallbackMessage (carma_planning_msgs::msg::Route::UniquePtr route_msg)
 Calls controlRequestFromRoute() and publishes the TrafficControlRequest Message returned after the completed operations. More...
 
visualization_msgs::msg::Marker composeTCMMarkerVisualizer (const std::vector< lanelet::Point3d > &input)
 composeTCMMarkerVisualizer() compose TCM Marker visualization More...
 
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 More...
 
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 TrafficControlRequest message that is published after a route is selected. During operation at ~10s intervals the vehicle will make another control request for the remainder of its route. More...
 
lanelet::Points3d getPointsInLocalFrame (const carma_v2x_msgs::msg::TrafficControlMessageV01 &geofence_msg)
 Extract geofence points from geofence message using its given proj and datum fields. More...
 
lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas (const lanelet::Points3d &gf_pts)
 Gets the affected lanelet or areas based on the points in local frame. More...
 
void setMaxLaneWidth (double max_lane_width)
 Sets the max lane width in meters. Geofence points are associated to a lanelet if they are within this distance to a lanelet as geofence points are guaranteed to apply to a single lane. More...
 
void setIntersectionCoordCorrection (const std::vector< int64_t > &intersection_ids_for_correction, const std::vector< double > &intersection_correction)
 Sets the coordinate correction for intersection. More...
 
void setConfigSpeedLimit (double cL)
 Sets the configured speed limit. More...
 
void setVehicleParticipationType (std::string participant)
 Set the Vehicle Participation Type. More...
 
std::string getVehicleParticipationType ()
 Get the Vehicle Participation Type object. More...
 
void geofenceFromMsg (std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &geofence_msg)
 Fills geofence object from TrafficControlMessageV01 ROS Msg. More...
 
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 geometry and signal_group. More...
 
double distToNearestActiveGeofence (const lanelet::BasicPoint2d &curr_pos)
 Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet. More...
 
void currentLocationCallback (geometry_msgs::msg::PoseStamped::UniquePtr current_pos)
 
carma_perception_msgs::msg::CheckActiveGeofence checkActiveGeofenceLogic (const geometry_msgs::msg::PoseStamped &current_pos)
 Returns a message indicating whether or not the vehicle is inside of an active geofence lanelet. More...
 
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. More...
 
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. More...
 
std::vector< std::string > participantsChecker (const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01) const
 Generates participants list. More...
 
std::vector< std::string > invertParticipants (const std::vector< std::string > &input_participants) const
 Generates inverse participants list of the given participants. More...
 
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 possible vehicle types. Returns the input with no change if it doesn't cover all. More...
 
carma_planning_msgs::msg::Route getRoute ()
 Returns the most recently recieved route message. More...
 
std::shared_ptr< GeofencecreateWorkzoneGeofence (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 traffic lights) and update_list that blocks old lanelets. Geofence will have minimum of 6 lanelet_additions_ (front parallel, front diagonal, middle lanelet(s), back diagonal, back parallel, 1 opposing lanelet with trafficlight). And regulatory_element_ of this geofence will be region_access_rule, where it blocks entire affected_parts of CLOSED, TAPERRIGHT, OPENRIGHT. It also blocks the opposing lane's lanelet that would have had the trafficlight, and new lanelet(s) would be added to replace it. More...
 
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 were created from splitting (if necessary) TAPERRIGHT and OPENRIGHT lanelets. Opposite_llts will have new lanelets split from REVERSE part with same direction as parallel lanelets that connect to diagonal ones. More...
 
std::shared_ptr< GeofencecreateWorkzoneGeometry (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 trafficlight. Fill lanelet_additions_ with every newly created lanelets New lanelets will have traffic light. Old lanelets (and those from CLOSED) will be blocked using update_list as region_access_rule. More...
 
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. Newly created lanelet will have old regulatory elements copied into each of them. From the front and back boundaries, it is deemed not necessary to split if the ratios are within error_distance from either of it. For example, if front and back points of 3 points given are both within error_distance, only middle point will be used to split into 2 lanelets. It will return duplicate of old lanelet (with different id) if no splitting was done. More...
 
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 relative to the lanelet. Newly created lanelet will have old regulatory elements copied into each of them. From the front and back boundaries, it is deemed not necessary to split if the ratios are within error_distance from either of it. It will return duplicate of old lanelet (with different id) if no splitting was done. More...
 
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 have old regulatory elements copied into each of them. From the front and back boundaries, it is deemed not necessary to split if the ratios are within error_distance from either of it. For example, if front and back points of 3 ratios given are both within error_distance, only middle ratio will be used to split, so 2 lanelets will be returned. It will return duplicate of old lanelet (with different id) if no splitting was done. More...
 
uint32_t generate32BitId (const std::string &label)
 helper for generating 32bit traffic light Id from TCM label field consisting workzone intersection/signal group ids More...
 
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 More...
 
void setErrorDistance (double error_distance)
 
void publishLightId ()
 helps to populate upcoming_intersection_ids_ from local traffic lanelet ids More...
 
void setConfigVehicleId (const std::string &vehicle_id)
 Retrieve the vehicle ID from global vehicle parameters, and set instance memeber vehicle id. More...
 
void setConfigACKPubTimes (int ack_pub_times)
 Sets the TCM Acknowledgement publish times. More...
 
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 number of times. More...
 
void updateUpcomingSGIntersectionIds ()
 populate upcoming_intersection_ids_ from local traffic lanelet ids More...
 

Public Attributes

visualization_msgs::msg::MarkerArray tcm_marker_array_
 
carma_v2x_msgs::msg::TrafficControlRequestPolygon tcr_polygon_
 
std_msgs::msg::Int32MultiArray upcoming_intersection_ids_
 

Private Types

enum class  AcknowledgementStatus { ACKNOWLEDGED = 1 , REJECTED = 2 }
 

Private Member Functions

void addRegulatoryComponent (std::shared_ptr< Geofence > gf_ptr) const
 
void addBackRegulatoryComponent (std::shared_ptr< Geofence > gf_ptr) const
 
void removeGeofenceHelper (std::shared_ptr< Geofence > gf_ptr) const
 
void addGeofenceHelper (std::shared_ptr< Geofence > gf_ptr)
 
bool shouldChangeControlLine (const lanelet::ConstLaneletOrArea &el, const lanelet::RegulatoryElementConstPtr &regem, std::shared_ptr< Geofence > gf_ptr) const
 This is a helper function that returns true if the provided regem is marked to be changed by the geofence as there are usually multiple passing control lines are in the lanelet. More...
 
bool shouldChangeTrafficSignal (const lanelet::ConstLaneletOrArea &el, const lanelet::RegulatoryElementConstPtr &regem, std::shared_ptr< carma_wm::SignalizedIntersectionManager > sim) const
 This is a helper function that returns true if signal in the lanelet should be changed according to the records of signalizer intersection manager Used in managing multiple signal_groups in a single entry lanelet for example. More...
 
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 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. More...
 
void scheduleGeofence (std::shared_ptr< carma_wm_ctrl::Geofence > gf_ptr_list)
 
lanelet::LineString3d createLinearInterpolatingLinestring (const lanelet::Point3d &front_pt, const lanelet::Point3d &back_pt, double increment_distance=0.25)
 
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)
 
std::unordered_set< lanelet::Lanelet > filterSuccessorLanelets (const std::unordered_set< lanelet::Lanelet > &possible_lanelets, const std::unordered_set< lanelet::Lanelet > &root_lanelets)
 

Private Attributes

double error_distance_ = 5
 
lanelet::ConstLanelets route_path_
 
std::unordered_set< lanelet::Id > active_geofence_llt_ids_
 
std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache_
 
std::unordered_map< uint32_t, lanelet::Id > traffic_light_id_lookup_
 
lanelet::LaneletMapPtr base_map_
 
lanelet::LaneletMapPtr current_map_
 
lanelet::routing::RoutingGraphPtr current_routing_graph_
 
lanelet::Velocity config_limit
 
std::string participant_ = lanelet::Participants::VehicleCar
 
std::unordered_set< std::string > checked_geofence_ids_
 
std::unordered_set< std::string > generated_geofence_reqids_
 
std::vector< lanelet::LaneletMapPtr > cached_maps_
 
std::mutex map_mutex_
 
PublishMapCallback map_pub_
 
PublishMapUpdateCallback map_update_pub_
 
PublishCtrlRequestCallback control_msg_pub_
 
PublishActiveGeofCallback active_pub_
 
GeofenceScheduler scheduler_
 
PublishMobilityOperationCallback tcm_ack_pub_
 
std::string base_map_georef_
 
double max_lane_width_
 
std::vector< carma_v2x_msgs::msg::TrafficControlMessageV01 > workzone_remaining_msgs_
 
bool workzone_geometry_published_ = false
 
size_t current_map_version_ = 0
 
carma_planning_msgs::msg::Route current_route
 
std::vector< autoware_lanelet2_msgs::msg::MapBin > map_update_message_queue_
 
size_t update_count_ = -1
 
std::shared_ptr< carma_wm::SignalizedIntersectionManagersim_
 
const std::string geofence_ack_strategy_ = "carma3/Geofence_Acknowledgement"
 
int ack_pub_times_ = 1
 
std::string vehicle_id_
 

Detailed Description

Class which provies exposes map publication and carma_wm update logic.

The WMBroadcaster handles updating the lanelet2 base map and publishing the new versions to the rest of the CARMA Platform ROS network. The broadcaster also provides functions for adding or removing geofences from the map and notifying the rest of the system.

Definition at line 72 of file WMBroadcaster.hpp.

Member Typedef Documentation

◆ PublishActiveGeofCallback

using carma_wm_ctrl::WMBroadcaster::PublishActiveGeofCallback = std::function<void(const carma_perception_msgs::msg::CheckActiveGeofence&)>

Definition at line 78 of file WMBroadcaster.hpp.

◆ PublishCtrlRequestCallback

using carma_wm_ctrl::WMBroadcaster::PublishCtrlRequestCallback = std::function<void(const carma_v2x_msgs::msg::TrafficControlRequest&)>

Definition at line 77 of file WMBroadcaster.hpp.

◆ PublishMapCallback

using carma_wm_ctrl::WMBroadcaster::PublishMapCallback = std::function<void(const autoware_lanelet2_msgs::msg::MapBin&)>

Definition at line 75 of file WMBroadcaster.hpp.

◆ PublishMapUpdateCallback

using carma_wm_ctrl::WMBroadcaster::PublishMapUpdateCallback = std::function<void(const autoware_lanelet2_msgs::msg::MapBin&)>

Definition at line 76 of file WMBroadcaster.hpp.

◆ PublishMobilityOperationCallback

using carma_wm_ctrl::WMBroadcaster::PublishMobilityOperationCallback = std::function<void(const carma_v2x_msgs::msg::MobilityOperation&)>

Definition at line 79 of file WMBroadcaster.hpp.

Member Enumeration Documentation

◆ AcknowledgementStatus

Enumerator
ACKNOWLEDGED 
REJECTED 

Definition at line 465 of file WMBroadcaster.hpp.

465 {
466 ACKNOWLEDGED = 1,
467 REJECTED = 2
468 };

Constructor & Destructor Documentation

◆ WMBroadcaster()

carma_wm_ctrl::WMBroadcaster::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.

Definition at line 47 of file WMBroadcaster.cpp.

49 : map_pub_(map_pub), map_update_pub_(map_update_pub), control_msg_pub_(control_msg_pub), active_pub_(active_pub), scheduler_(timer_factory), tcm_ack_pub_(tcm_ack_pub)
50{
53 std::bind(&WMBroadcaster::routeCallbackMessage, this, _1);
54};
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...
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...
PublishCtrlRequestCallback control_msg_pub_
void removeGeofence(std::shared_ptr< Geofence > gf_ptr)
Removes a geofence from the current map and publishes the ROS msg.
void addGeofence(std::shared_ptr< Geofence > gf_ptr)
Adds a geofence to the current map and publishes the ROS msg.
void routeCallbackMessage(carma_planning_msgs::msg::Route::UniquePtr route_msg)
Calls controlRequestFromRoute() and publishes the TrafficControlRequest Message returned after the co...
PublishMobilityOperationCallback tcm_ack_pub_
PublishActiveGeofCallback active_pub_
PublishMapUpdateCallback map_update_pub_

References addGeofence(), carma_wm_ctrl::GeofenceScheduler::onGeofenceActive(), carma_wm_ctrl::GeofenceScheduler::onGeofenceInactive(), removeGeofence(), routeCallbackMessage(), and scheduler_.

Here is the call graph for this function:

Member Function Documentation

◆ addBackRegulatoryComponent()

void carma_wm_ctrl::WMBroadcaster::addBackRegulatoryComponent ( std::shared_ptr< Geofence gf_ptr) const
private

Definition at line 1457 of file WMBroadcaster.cpp.

1458{
1459 // First loop is to remove the relation between element and regulatory element that this geofence added initially
1460
1461 for (auto el: gf_ptr->affected_parts_)
1462 {
1463 for (auto regem : el.regulatoryElements())
1464 {
1465 if (!shouldChangeControlLine(el, regem, gf_ptr)) continue;
1466
1467 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1468 {
1469 auto nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
1470 gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
1471 current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
1472 }
1473 }
1474 }
1475
1476 // As this gf received is the first gf that was sent in through addGeofence,
1477 // we have prev speed limit information inside it to put them back
1478 for (auto pair : gf_ptr->prev_regems_)
1479 {
1480 if (pair.second->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1481 {
1482 current_map_->update(current_map_->laneletLayer.get(pair.first), pair.second);
1483 gf_ptr->update_list_.push_back(pair);
1484 }
1485 }
1486}
lanelet::LaneletMapPtr current_map_
bool shouldChangeControlLine(const lanelet::ConstLaneletOrArea &el, const lanelet::RegulatoryElementConstPtr &regem, std::shared_ptr< Geofence > gf_ptr) const
This is a helper function that returns true if the provided regem is marked to be changed by the geof...

References current_map_, and shouldChangeControlLine().

Referenced by removeGeofenceHelper().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addGeofence()

void carma_wm_ctrl::WMBroadcaster::addGeofence ( std::shared_ptr< Geofence gf_ptr)

Adds a geofence to the current map and publishes the ROS msg.

Definition at line 1488 of file WMBroadcaster.cpp.

1489{
1490
1491 std::lock_guard<std::mutex> guard(map_mutex_);
1492 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Adding active geofence to the map with geofence id: " << gf_ptr->id_);
1493
1494 // if applying workzone geometry geofence, utilize workzone chache to create one
1495 // also multiple map updates can be sent from one geofence object
1496 std::vector<std::shared_ptr<Geofence>> updates_to_send;
1497
1498 bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos;
1499 bool detected_map_msg_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("MAP_MSG") != std::string::npos;
1500 if (detected_workzone_signal && gf_ptr->msg_.params.detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1501 {
1502 for (auto gf_cache_ptr : work_zone_geofence_cache_)
1503 {
1504 geofenceFromMsg(gf_cache_ptr.second, gf_cache_ptr.second->msg_);
1505 }
1506 updates_to_send.push_back(createWorkzoneGeofence(work_zone_geofence_cache_));
1507 }
1508 else if (detected_map_msg_signal)
1509 {
1510 updates_to_send = geofenceFromMapMsg(gf_ptr, gf_ptr->map_msg_);
1511 }
1512 else
1513 {
1514 geofenceFromMsg(gf_ptr, gf_ptr->msg_);
1515 updates_to_send.push_back(gf_ptr);
1516 }
1517
1518 for (auto update : updates_to_send)
1519 {
1520 // add marker to rviz
1521 tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(update->gf_pts)); // create visualizer in rviz
1522
1523 if (update->affected_parts_.empty())
1524 continue;
1525
1526 // Process the geofence object to populate update remove lists
1527 addGeofenceHelper(update);
1528
1529 if (!detected_map_msg_signal)
1530 {
1531 for (auto pair : update->update_list_) active_geofence_llt_ids_.insert(pair.first);
1532 }
1533
1534 autoware_lanelet2_msgs::msg::MapBin gf_msg;
1535
1536 // If the geofence invalidates the route graph then recompute the routing graph now that the map has been updated
1537 if (update->invalidate_route_) {
1538
1539 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Rebuilding routing graph after is was invalidated by geofence");
1540
1541 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1542 lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_);
1543 current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
1544
1545 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence");
1546
1547 // Populate routing graph structure
1548 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message");
1549
1550 auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
1551
1552 gf_msg.routing_graph = readable_graph->routingGraphToMsg(participant_);
1553 gf_msg.has_routing_graph = true;
1554
1555 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message");
1556 }
1557
1558
1559 // Publish
1560 auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(update->id_, update->update_list_, update->remove_list_, update->lanelet_additions_));
1561 send_data->traffic_light_id_lookup_ = update->traffic_light_id_lookup_;
1562
1563 if (detected_map_msg_signal && updates_to_send.back() == update) // if last update
1564 {
1565 send_data->sim_ = *sim_;
1566 }
1567
1568 carma_wm::toBinMsg(send_data, &gf_msg);
1569 update_count_++; // Update the sequence count for the geofence messages
1570 gf_msg.seq_id = update_count_;
1571 gf_msg.invalidates_route=update->invalidate_route_;
1572 gf_msg.map_version = current_map_version_;
1573 map_update_pub_(gf_msg);
1574 }
1575
1576}
visualization_msgs::msg::MarkerArray tcm_marker_array_
visualization_msgs::msg::Marker composeTCMMarkerVisualizer(const std::vector< lanelet::Point3d > &input)
composeTCMMarkerVisualizer() compose TCM Marker visualization
std::unordered_map< uint8_t, std::shared_ptr< Geofence > > work_zone_geofence_cache_
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...
std::unordered_set< lanelet::Id > active_geofence_llt_ids_
std::shared_ptr< carma_wm::SignalizedIntersectionManager > sim_
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 geofenceFromMsg(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &geofence_msg)
Fills geofence object from TrafficControlMessageV01 ROS Msg.
void toBinMsg(std::shared_ptr< carma_wm::TrafficControl > gf_ptr, autoware_lanelet2_msgs::msg::MapBin *msg)

References active_geofence_llt_ids_, addGeofenceHelper(), composeTCMMarkerVisualizer(), createWorkzoneGeofence(), current_map_, current_map_version_, current_routing_graph_, geofenceFromMapMsg(), geofenceFromMsg(), map_mutex_, map_update_pub_, participant_, sim_, tcm_marker_array_, carma_wm::toBinMsg(), update_count_, and work_zone_geofence_cache_.

Referenced by WMBroadcaster().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addGeofenceHelper()

void carma_wm_ctrl::WMBroadcaster::addGeofenceHelper ( std::shared_ptr< Geofence gf_ptr)
private

Definition at line 1912 of file WMBroadcaster.cpp.

1913{
1914 // resetting the information inside geofence
1915 gf_ptr->remove_list_ = {};
1916 gf_ptr->update_list_ = {};
1917
1918 // add additional lanelets that need to be added
1919 for (auto llt : gf_ptr->lanelet_additions_)
1920 {
1921 current_map_->add(llt);
1922 }
1923
1924 // add trafficlight id mapping
1925 for (auto pair : gf_ptr->traffic_light_id_lookup_)
1926 {
1927 traffic_light_id_lookup_[pair.first] = pair.second;
1928 }
1929
1930 // Logic to determine what type of geofence
1931 addRegulatoryComponent(gf_ptr);
1932}
std::unordered_map< uint32_t, lanelet::Id > traffic_light_id_lookup_
void addRegulatoryComponent(std::shared_ptr< Geofence > gf_ptr) const

References addRegulatoryComponent(), current_map_, and traffic_light_id_lookup_.

Referenced by addGeofence().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addPassingControlLineFromMsg()

void carma_wm_ctrl::WMBroadcaster::addPassingControlLineFromMsg ( std::shared_ptr< Geofence gf_ptr,
const carma_v2x_msgs::msg::TrafficControlMessageV01 &  msg_v01,
const std::vector< lanelet::Lanelet > &  affected_llts 
) const
private

Definition at line 880 of file WMBroadcaster.cpp.

881{
882 carma_v2x_msgs::msg::TrafficControlDetail msg_detail;
883 msg_detail = msg_v01.params.detail;
884 // Get affected bounds
885 lanelet::LineStrings3d pcl_bounds;
886 if (msg_detail.lataffinity == carma_v2x_msgs::msg::TrafficControlDetail::LEFT)
887 {
888 for (auto llt : affected_llts) pcl_bounds.push_back(llt.leftBound());
889 gf_ptr->pcl_affects_left_ = true;
890 }
891 else //right
892 {
893 for (auto llt : affected_llts) pcl_bounds.push_back(llt.rightBound());
894 gf_ptr->pcl_affects_right_ = true;
895 }
896
897 // Get specified participants
898 std::vector<std::string> left_participants;
899 std::vector<std::string> right_participants;
900 std::vector<std::string> participants=participantsChecker(msg_v01);
901
902 // Create the pcl depending on the allowed passing control direction, left, right, or both
903 if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
904 msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
905 {
906 left_participants = participants;
907 }
908 else if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
909 {
910 left_participants.push_back(lanelet::Participants::VehicleEmergency);
911 }
912 if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
913 msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
914 {
915 right_participants = participants;
916 }
917 else if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
918 {
919 right_participants.push_back(lanelet::Participants::VehicleEmergency);
920 }
921
922 gf_ptr->regulatory_element_ = std::make_shared<lanelet::PassingControlLine>(lanelet::PassingControlLine::buildData(
923 lanelet::utils::getId(), pcl_bounds, left_participants, right_participants));
924}
std::vector< std::string > participantsChecker(const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01) const
Generates participants list.

References participantsChecker().

Referenced by geofenceFromMsg().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addRegionAccessRule()

void carma_wm_ctrl::WMBroadcaster::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.

Parameters
gf_ptrgeofence pointer
msg_v01message type
afffected_lltsaffected lanelets

Definition at line 926 of file WMBroadcaster.cpp.

927{
928 const std::string& reason = msg_v01.package.label;
929 gf_ptr->label_ = msg_v01.package.label;
930 auto regulatory_element = std::make_shared<lanelet::RegionAccessRule>(lanelet::RegionAccessRule::buildData(lanelet::utils::getId(),affected_llts,{},invertParticipants(participantsChecker(msg_v01)), reason));
931
932 if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck ))
933 {
934 gf_ptr->invalidate_route_=true;
935 }
936 else
937 {
938 gf_ptr->invalidate_route_=false;
939 }
940 gf_ptr->regulatory_element_ = regulatory_element;
941}
std::vector< std::string > invertParticipants(const std::vector< std::string > &input_participants) const
Generates inverse participants list of the given participants.

References invertParticipants(), and participantsChecker().

Referenced by createWorkzoneGeometry(), and geofenceFromMsg().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addRegionMinimumGap()

void carma_wm_ctrl::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

Adds Minimum Gap to the map.

Parameters
gf_ptrgeofence pointer
doublemin_gap
afffected_lltsaffected lanelets
affected_areasaffected areas

Definition at line 943 of file WMBroadcaster.cpp.

944{
945 auto regulatory_element = std::make_shared<lanelet::DigitalMinimumGap>(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(),
946 min_gap, affected_llts, affected_areas,participantsChecker(msg_v01) ));
947
948 gf_ptr->regulatory_element_ = regulatory_element;
949}

References participantsChecker().

Referenced by geofenceFromMsg().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addRegulatoryComponent()

void carma_wm_ctrl::WMBroadcaster::addRegulatoryComponent ( std::shared_ptr< Geofence gf_ptr) const
private

Definition at line 1416 of file WMBroadcaster.cpp.

1417{
1418 // First loop is to save the relation between element and regulatory element
1419 // so that we can add back the old one after geofence deactivates
1420 for (auto el: gf_ptr->affected_parts_)
1421 {
1422 for (auto regem : el.regulatoryElements())
1423 {
1424 if (!shouldChangeControlLine(el, regem, gf_ptr) ||
1425 !shouldChangeTrafficSignal(el, regem, sim_))
1426 continue;
1427
1428 if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
1429 {
1430 lanelet::RegulatoryElementPtr nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
1431 gf_ptr->prev_regems_.push_back(std::make_pair(el.id(), nonconst_regem));
1432 gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
1433 current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
1434 }
1435 }
1436 }
1437
1438
1439 // this loop is also kept separately because previously we assumed
1440 // there was existing regem, but this handles changes to all of the elements
1441 for (auto el: gf_ptr->affected_parts_)
1442 {
1443 // update it with new regem
1444 if (gf_ptr->regulatory_element_->id() != lanelet::InvalId)
1445 {
1446 current_map_->update(current_map_->laneletLayer.get(el.id()), gf_ptr->regulatory_element_);
1447 gf_ptr->update_list_.push_back(std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>(el.id(), gf_ptr->regulatory_element_));
1448 } else {
1449 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Regulatory element with invalid id in geofence cannot be added to the map");
1450 }
1451 }
1452
1453
1454
1455}
bool shouldChangeTrafficSignal(const lanelet::ConstLaneletOrArea &el, const lanelet::RegulatoryElementConstPtr &regem, std::shared_ptr< carma_wm::SignalizedIntersectionManager > sim) const
This is a helper function that returns true if signal in the lanelet should be changed according to t...

References current_map_, shouldChangeControlLine(), shouldChangeTrafficSignal(), and sim_.

Referenced by addGeofenceHelper().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ addScheduleFromMsg()

void carma_wm_ctrl::WMBroadcaster::addScheduleFromMsg ( std::shared_ptr< Geofence gf_ptr,
const carma_v2x_msgs::msg::TrafficControlMessageV01 &  msg_v01 
)
private

Populates the schedules member of the geofence object from given TrafficControlMessageV01 message.

Parameters
gf_ptrgeofence pointer
msg_v01TrafficControlMessageV01 (geofence msg)

Definition at line 117 of file WMBroadcaster.cpp.

118{
119 // Handle schedule processing
120 carma_v2x_msgs::msg::TrafficControlSchedule msg_schedule = msg_v01.params.schedule;
121 double ns_to_sec = 1.0e9;
122 auto clock_type = scheduler_.getClockType();
123
124 rclcpp::Time end_time = {msg_schedule.end, clock_type};
125 if (!msg_schedule.end_exists) {
126 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "No end time for geofence, using rclcpp::Time::max()");
127 end_time = {rclcpp::Time::max(), clock_type}; // If there is no end time use the max time
128 }
129
130 // If the days of the week are specified then convert them to the boost format.
131 GeofenceSchedule::DayOfTheWeekSet week_day_set = { 0, 1, 2, 3, 4, 5, 6 }; // Default to all days 0==Sun to 6==Sat
132 if (msg_schedule.dow_exists) {
133 // sun (6), mon (5), tue (4), wed (3), thu (2), fri (1), sat (0)
134 week_day_set.clear();
135 for (size_t i = 0; i < msg_schedule.dow.dow.size(); i++) {
136 if (msg_schedule.dow.dow[i] == 1) {
137 switch(i) {
138 case 6: // sun
139 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Sunday);
140 break;
141 case 5: // mon
142 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Monday);
143 break;
144 case 4: // tue
145 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Tuesday);
146 break;
147 case 3: // wed
148 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Wednesday);
149 break;
150 case 2: // thur
151 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Thursday);
152 break;
153 case 1: // fri
154 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Friday);
155 break;
156 case 0: // sat
157 week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Saturday);
158 break;
159 default:
160 throw std::invalid_argument("Unrecognized weekday value: " + std::to_string(i));
161 }
162 }
163 }
164 }
165
166 if (msg_schedule.between_exists) {
167
168 for (auto daily_schedule : msg_schedule.between)
169 {
170 if (msg_schedule.repeat_exists) {
171 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
172 end_time,
173 rclcpp::Duration(daily_schedule.begin),
174 rclcpp::Duration(daily_schedule.duration),
175 rclcpp::Duration(msg_schedule.repeat.offset),
176 rclcpp::Duration(msg_schedule.repeat.span),
177 rclcpp::Duration(msg_schedule.repeat.period),
178 week_day_set));
179 } else {
180 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
181 end_time,
182 rclcpp::Duration(daily_schedule.begin),
183 rclcpp::Duration(daily_schedule.duration),
184 rclcpp::Duration(0.0), // No offset
185 rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // Compute schedule portion end time
186 rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // No repetition so same as portion end time
187 week_day_set));
188 }
189
190 }
191 }
192 else {
193 if (msg_schedule.repeat_exists) {
194 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
195 end_time,
196 rclcpp::Duration(0.0),
197 rclcpp::Duration(86400.0e9), // 24 hr daily application
198 rclcpp::Duration(msg_schedule.repeat.offset),
199 rclcpp::Duration(msg_schedule.repeat.span),
200 rclcpp::Duration(msg_schedule.repeat.period),
201 week_day_set));
202 } else {
203 gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
204 end_time,
205 rclcpp::Duration(0.0),
206 rclcpp::Duration(86400.0e9), // 24 hr daily application
207 rclcpp::Duration(0.0), // No offset
208 rclcpp::Duration(86400.0e9), // Applied for full lenth of 24 hrs
209 rclcpp::Duration(86400.0e9), // No repetition
210 week_day_set));
211 }
212
213 }
214}
std::unordered_set< boost::gregorian::greg_weekday, std::hash< int > > DayOfTheWeekSet
rcl_clock_type_t getClockType()
Get the clock type of the clock being created by the timer factory.
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21

References carma_wm_ctrl::GeofenceScheduler::getClockType(), process_bag::i, scheduler_, and carma_cooperative_perception::to_string().

Referenced by externalMapMsgCallback(), and geofenceCallback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ baseMapCallback()

void carma_wm_ctrl::WMBroadcaster::baseMapCallback ( autoware_lanelet2_msgs::msg::MapBin::UniquePtr  map_msg)

Callback to set the base map when it has been loaded.

Parameters
map_msgThe map message to use as the base map

Definition at line 56 of file WMBroadcaster.cpp.

57{
58 std::lock_guard<std::mutex> guard(map_mutex_);
59
60 static bool firstCall = true;
61 // This function should generally only ever be called one time so log a warning if it occurs multiple times
62 if (firstCall)
63 {
64 firstCall = false;
65 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called for first time with new map message");
66 }
67 else
68 {
69 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called multiple times in the same node");
70 }
71
72 lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap);
73 lanelet::LaneletMapPtr new_map_to_change(new lanelet::LaneletMap);
74
76 lanelet::utils::conversion::fromBinMsg(*map_msg, new_map_to_change);
77
78 base_map_ = new_map; // Store map
79 current_map_ = new_map_to_change; // broadcaster makes changes to this
80
81 lanelet::MapConformer::ensureCompliance(base_map_, config_limit); // Update map to ensure it complies with expectations
83
84 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Building routing graph for base map");
85
86 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
87 lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_);
88 current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
89
90 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done building routing graph for base map");
91
92 // Publish map
93 current_map_version_ += 1; // Increment the map version. It should always start from 1 for the first map
94
95 autoware_lanelet2_msgs::msg::MapBin compliant_map_msg;
96
97 // Populate the routing graph message
98 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message.");
99
100 auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
101
102 compliant_map_msg.routing_graph = readable_graph->routingGraphToMsg(participant_);
103 compliant_map_msg.has_routing_graph = true;
104
105 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message.");
106
108 compliant_map_msg.map_version = current_map_version_;
109 map_pub_(compliant_map_msg);
110};
lanelet::LaneletMapPtr base_map_
lanelet::Velocity config_limit
void fromBinMsg(const autoware_lanelet2_msgs::msg::MapBin &msg, std::shared_ptr< carma_wm::TrafficControl > gf_ptr, lanelet::LaneletMapPtr lanelet_map=nullptr)
void ensureCompliance(lanelet::LaneletMapPtr map, lanelet::Velocity config_limit=80_mph)
Function modifies an existing map to make a best effort attempt at ensuring the map confroms to the e...

References base_map_, config_limit, current_map_, current_map_version_, current_routing_graph_, lanelet::MapConformer::ensureCompliance(), carma_wm::fromBinMsg(), map_mutex_, map_pub_, participant_, and carma_wm::toBinMsg().

Referenced by carma_wm_ctrl::WMBroadcasterNode::handle_on_activate().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ checkActiveGeofenceLogic()

carma_perception_msgs::msg::CheckActiveGeofence carma_wm_ctrl::WMBroadcaster::checkActiveGeofenceLogic ( const geometry_msgs::msg::PoseStamped &  current_pos)

Returns a message indicating whether or not the vehicle is inside of an active geofence lanelet.

Parameters
current_posCurrent position of the vehicle
Returns
0 if vehicle is not on an active geofence

Definition at line 2012 of file WMBroadcaster.cpp.

2013{
2014
2015 if (!current_map_ || current_map_->laneletLayer.size() == 0)
2016 {
2017 throw lanelet::InvalidObjectStateError(std::string("Lanelet map 'current_map_' is not loaded to the WMBroadcaster"));
2018 }
2019
2020 // Store current position values to be compared to geofence boundary values
2021 double current_pos_x = current_pos.pose.position.x;
2022 double current_pos_y = current_pos.pose.position.y;
2023
2024
2025
2026
2027 lanelet::BasicPoint2d curr_pos;
2028 curr_pos.x() = current_pos_x;
2029 curr_pos.y() = current_pos_y;
2030
2031 carma_perception_msgs::msg::CheckActiveGeofence outgoing_geof; //message to publish
2032 double next_distance = 0 ; //Distance to next geofence
2033
2034 if (active_geofence_llt_ids_.size() <= 0 )
2035 {
2036 return outgoing_geof;
2037 }
2038
2039 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence llt ids are loaded to the WMBroadcaster");
2040
2041 // Obtain the closest lanelet to the vehicle's current position
2042 auto current_llt = lanelet::geometry::findNearest(current_map_->laneletLayer, curr_pos, 1)[0].second;
2043
2044
2045 /* determine whether or not the vehicle's current position is within an active geofence */
2046 if (boost::geometry::within(curr_pos, current_llt.polygon2d().basicPolygon()))
2047 {
2048 next_distance = distToNearestActiveGeofence(curr_pos);
2049 outgoing_geof.distance_to_next_geofence = next_distance;
2050
2051 for(auto id : active_geofence_llt_ids_)
2052 {
2053 if (id == current_llt.id())
2054 {
2055 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Vehicle is on Lanelet " << current_llt.id() << ", which has an active geofence");
2056 outgoing_geof.is_on_active_geofence = true;
2057 for (auto regem: current_llt.regulatoryElements())
2058 {
2059 // Assign active geofence fields based on the speed limit associated with this lanelet
2060 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
2061 {
2062 lanelet::DigitalSpeedLimitPtr speed = std::dynamic_pointer_cast<lanelet::DigitalSpeedLimit>
2063 (current_map_->regulatoryElementLayer.get(regem->id()));
2064 outgoing_geof.value = speed->speed_limit_.value();
2065 outgoing_geof.advisory_speed = speed->speed_limit_.value();
2066 outgoing_geof.reason = speed->getReason();
2067
2068 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a speed limit of " << speed->speed_limit_.value());
2069
2070 // Cannot overrule outgoing_geof.type if it is already set to LANE_CLOSED
2071 if(outgoing_geof.type != carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED)
2072 {
2073 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::SPEED_LIMIT;
2074 }
2075 }
2076
2077 // Assign active geofence fields based on the minimum gap associated with this lanelet (if it exists)
2078 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalMinimumGap::RuleName) == 0)
2079 {
2080 lanelet::DigitalMinimumGapPtr min_gap = std::dynamic_pointer_cast<lanelet::DigitalMinimumGap>
2081 (current_map_->regulatoryElementLayer.get(regem->id()));
2082 outgoing_geof.minimum_gap = min_gap->getMinimumGap();
2083 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a minimum gap of " << min_gap->getMinimumGap());
2084 }
2085
2086 // Assign active geofence fields based on whether the current lane is closed or is immediately adjacent to a closed lane
2087 if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2088 {
2089 lanelet::RegionAccessRulePtr accessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2090 (current_map_->regulatoryElementLayer.get(regem->id()));
2091
2092 // Update the 'type' and 'reason' for this active geofence if the vehicle is in a closed lane
2093 if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2094 {
2095 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence is a closed lane.");
2096 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Closed lane reason: " << accessRuleReg->getReason());
2097 outgoing_geof.reason = accessRuleReg->getReason();
2098 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2099 }
2100 // Otherwise, update the 'type' and 'reason' for this active geofence if the vehicle is in a lane immediately adjacent to a closed lane with the same travel direction
2101 else
2102 {
2103 // Obtain all same-direction lanes sharing the right lane boundary (will include the current lanelet)
2104 auto right_boundary_lanelets = current_map_->laneletLayer.findUsages(current_llt.rightBound());
2105
2106 // Check if the adjacent right lane is closed
2107 if(right_boundary_lanelets.size() > 1)
2108 {
2109 for(auto lanelet : right_boundary_lanelets)
2110 {
2111 // Only check the adjacent right lanelet; ignore the current lanelet
2112 if(lanelet.id() != current_llt.id())
2113 {
2114 for (auto rightRegem: lanelet.regulatoryElements())
2115 {
2116 if(rightRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2117 {
2118 lanelet::RegionAccessRulePtr rightAccessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2119 (current_map_->regulatoryElementLayer.get(rightRegem->id()));
2120 if(!rightAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !rightAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2121 {
2122 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Right adjacent Lanelet " << lanelet.id() << " is CLOSED");
2123 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence");
2124 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << rightAccessRuleReg->getReason());
2125 outgoing_geof.reason = rightAccessRuleReg->getReason();
2126 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2127 }
2128 }
2129 }
2130 }
2131 }
2132 }
2133
2134 // Check if the adjacent left lane is closed
2135 auto left_boundary_lanelets = current_map_->laneletLayer.findUsages(current_llt.leftBound());
2136 if(left_boundary_lanelets.size() > 1)
2137 {
2138 for(auto lanelet : left_boundary_lanelets)
2139 {
2140 // Only check the adjacent left lanelet; ignore the current lanelet
2141 if(lanelet.id() != current_llt.id())
2142 {
2143 for (auto leftRegem: lanelet.regulatoryElements())
2144 {
2145 if(leftRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
2146 {
2147 lanelet::RegionAccessRulePtr leftAccessRuleReg = std::dynamic_pointer_cast<lanelet::RegionAccessRule>
2148 (current_map_->regulatoryElementLayer.get(leftRegem->id()));
2149 if(!leftAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !leftAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
2150 {
2151 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Left adjacent Lanelet " << lanelet.id() << " is CLOSED");
2152 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence");
2153 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << leftAccessRuleReg->getReason());
2154 outgoing_geof.reason = leftAccessRuleReg->getReason();
2155 outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
2156 }
2157 }
2158 }
2159 }
2160 }
2161 }
2162 }
2163 }
2164 }
2165 }
2166 }
2167 }
2168 return outgoing_geof;
2169}
double distToNearestActiveGeofence(const lanelet::BasicPoint2d &curr_pos)
Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet...

References active_geofence_llt_ids_, current_map_, and distToNearestActiveGeofence().

Referenced by currentLocationCallback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ combineParticipantsToVehicle()

std::vector< std::string > carma_wm_ctrl::WMBroadcaster::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 possible vehicle types. Returns the input with no change if it doesn't cover all.

Parameters
std::vector<std::string>participants vector of strings

Definition at line 1009 of file WMBroadcaster.cpp.

1010{
1011 std::vector<std::string> participants;
1012
1013 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)!= input_participants.end() &&
1014 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus) != input_participants.end() &&
1015 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar) != input_participants.end() &&
1016 std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck) != input_participants.end())
1017 {
1018 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Detected participants to cover all possible vehicle types");
1019 participants.emplace_back(lanelet::Participants::Vehicle);
1020 }
1021 else
1022 {
1023 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Not making any changes to the participants list");
1024 participants = input_participants;
1025 }
1026
1027 return participants;
1028}

Referenced by participantsChecker().

Here is the caller graph for this function:

◆ composeTCMMarkerVisualizer()

visualization_msgs::msg::Marker carma_wm_ctrl::WMBroadcaster::composeTCMMarkerVisualizer ( const std::vector< lanelet::Point3d > &  input)

composeTCMMarkerVisualizer() compose TCM Marker visualization

Parameters
inputThe message containing tcm information

Definition at line 1826 of file WMBroadcaster.cpp.

1827 {
1828
1829 // create the marker msgs
1830 visualization_msgs::msg::Marker marker;
1831 marker.header.frame_id = "map";
1832 marker.header.stamp = rclcpp::Time();
1833 marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
1834 marker.action = visualization_msgs::msg::Marker::ADD;
1835 marker.ns = "route_visualizer";
1836
1837 marker.scale.x = 0.65;
1838 marker.scale.y = 0.65;
1839 marker.scale.z = 0.65;
1840 marker.frame_locked = true;
1841
1842 if (!tcm_marker_array_.markers.empty())
1843 {
1844 marker.id = tcm_marker_array_.markers.back().id + 1;
1845 }
1846 else
1847 {
1848 marker.id = 0;
1849 }
1850 marker.color.r = 0.0F;
1851 marker.color.g = 1.0F;
1852 marker.color.b = 0.0F;
1853 marker.color.a = 1.0F;
1854
1855 for (int i = 0; i < input.size(); i++)
1856 {
1857 geometry_msgs::msg::Point temp_point;
1858 temp_point.x = input[i].x();
1859 temp_point.y = input[i].y();
1860 temp_point.z = 2; //to show up on top of the lanelet lines
1861
1862 marker.points.push_back(temp_point);
1863 }
1864
1865 return marker;
1866 }

References process_bag::i, and tcm_marker_array_.

Referenced by addGeofence().

Here is the caller graph for this function:

◆ composeTCRStatus()

carma_v2x_msgs::msg::TrafficControlRequestPolygon carma_wm_ctrl::WMBroadcaster::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

Parameters
inputThe message containing tcr information

Definition at line 1792 of file WMBroadcaster.cpp.

1793{
1794 carma_v2x_msgs::msg::TrafficControlRequestPolygon output;
1795 lanelet::BasicPoint3d local_point_tmp;
1796
1797 int i = -1;
1798 while (i < 3) // three offsets; offsets.size() doesn't return accurate size
1799 {
1800 if (i == -1)
1801 {
1802 local_point_tmp.x() = localPoint.x();
1803 local_point_tmp.y() = localPoint.y();
1804 }
1805 else
1806 {
1807 local_point_tmp.x() = localPoint.x() + cB.offsets[i].deltax;;
1808 local_point_tmp.y() = localPoint.y() + cB.offsets[i].deltay;;
1809 }
1810 lanelet::GPSPoint gps_vertex = local_projector.reverse(local_point_tmp);
1811
1812 carma_v2x_msgs::msg::Position3D gps_msg;
1813 gps_msg.elevation = gps_vertex.ele;
1814 gps_msg.latitude = gps_vertex.lat;
1815 gps_msg.longitude = gps_vertex.lon;
1816 output.polygon_list.push_back(gps_msg);
1817
1818 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "TCR Vertex Lat: "<< std::to_string(gps_vertex.lat));
1819 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "TCR Vertex Lon: "<<std::to_string(gps_vertex.lon));
1820
1821 i++;
1822 }
1823 return output;
1824}

References process_bag::i, and carma_cooperative_perception::to_string().

Referenced by controlRequestFromRoute().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ controlRequestFromRoute()

carma_v2x_msgs::msg::TrafficControlRequest carma_wm_ctrl::WMBroadcaster::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 TrafficControlRequest message that is published after a route is selected. During operation at ~10s intervals the vehicle will make another control request for the remainder of its route.

Parameters
route_msgThe message containing route information pulled from routeCallbackMessage()
req_id_for_testingthis ptr is optional. it gives req_id for developer to test TrafficControlMessage as it needs it

Definition at line 1641 of file WMBroadcaster.cpp.

1642{
1643 lanelet::ConstLanelets path;
1644
1645 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1646 {
1647 // Return / log warning etc.
1648 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'current_map_' does not exist.");
1649 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
1650
1651 }
1652
1653 for(auto id : route_msg.route_path_lanelet_ids)
1654 {
1655 auto laneLayer = current_map_->laneletLayer.get(id);
1656 path.push_back(laneLayer);
1657 }
1658
1659 // update local copy
1660 route_path_ = path;
1661
1662 if(path.size() == 0) throw lanelet::InvalidObjectStateError(std::string("No lanelets available in path."));
1663
1664 /*logic to determine route bounds*/
1665 std::vector<lanelet::ConstLanelet> llt;
1666 std::vector<lanelet::BoundingBox2d> pathBox;
1667 double minX = std::numeric_limits<double>::max();
1668 double minY = std::numeric_limits<double>::max();
1669 double maxX = std::numeric_limits<double>::lowest();
1670 double maxY = std::numeric_limits<double>::lowest();
1671
1672 while (path.size() != 0) //Continue until there are no more lanelet elements in path
1673 {
1674 llt.push_back(path.back()); //Add a lanelet to the vector
1675
1676
1677 pathBox.push_back(lanelet::geometry::boundingBox2d(llt.back())); //Create a bounding box of the added lanelet and add it to the vector
1678
1679
1680 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x() < minX)
1681 minX = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x(); //minimum x-value
1682
1683
1684 if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y() < minY)
1685 minY = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y(); //minimum y-value
1686
1687
1688
1689 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x() > maxX)
1690 maxX = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x(); //maximum x-value
1691
1692
1693 if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y() > maxY)
1694 maxY = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y(); //maximum y-value
1695
1696
1697 path.pop_back(); //remove the added lanelet from path an reduce pack.size() by 1
1698 } //end of while loop
1699
1700
1701 std::string target_frame = base_map_georef_;
1702 if (target_frame.empty())
1703 {
1704 // Return / log warning etc.
1705 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'target_frame' is empty.");
1706 throw lanelet::InvalidObjectStateError(std::string("Base georeference map may not be loaded to the WMBroadcaster"));
1707
1708 }
1709
1710 // Convert the minimum point to latlon
1711 lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
1712 lanelet::BasicPoint3d localPoint;
1713
1714 localPoint.x()= minX;
1715 localPoint.y()= minY;
1716
1717 lanelet::GPSPoint gpsRoute = local_projector.reverse(localPoint); //If the appropriate library is included, the reverse() function can be used to convert from local xyz to lat/lon
1718
1719 // Create a local transverse mercator frame at the minimum point to allow us to get east,north oriented bounds
1720 std::string local_tmerc_enu_proj = "+proj=tmerc +datum=WGS84 +h_0=0 +lat_0=" + std::to_string(gpsRoute.lat) + " +lon_0=" + std::to_string(gpsRoute.lon);
1721
1722 // Create transform from map frame to local transform mercator frame at bounds min point
1723 PJ* tmerc_proj = proj_create_crs_to_crs(PJ_DEFAULT_CTX, target_frame.c_str(), local_tmerc_enu_proj.c_str(), nullptr);
1724
1725 if (tmerc_proj == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
1726
1727 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between request bounds frame and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1728 << " MapProjection: " << target_frame << " Message Projection: " << local_tmerc_enu_proj);
1729
1730 return {}; // Ignore geofence if it could not be projected into the map frame
1731
1732 }
1733
1734 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Top Left: ("<< minX <<", "<<maxY<<")");
1735 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Top Right: ("<< maxX <<", "<<maxY<<")");
1736 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Bottom Left: ("<< minX <<", "<<minY<<")");
1737 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Bottom Right: ("<< maxX <<", "<<minY<<")");
1738
1739 PJ_COORD pj_min {{minX, minY, 0, 0}}; // z is not currently used
1740 PJ_COORD pj_min_tmerc;
1741 PJ_COORD pj_max {{maxX, maxY, 0, 0}}; // z is not currently used
1742 PJ_COORD pj_max_tmerc;
1743 pj_min_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_min);
1744 pj_max_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_max);
1745
1746 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion: MinPoint ( "<< pj_min_tmerc.xyz.x <<", " << pj_min_tmerc.xyz.y <<" )");
1747 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion: MaxPoint ( "<< pj_max_tmerc.xyz.x <<", " << pj_max_tmerc.xyz.y <<" )");
1748
1749 carma_v2x_msgs::msg::TrafficControlRequest cR; /*Fill the latitude value in message cB with the value of lat */
1750 carma_v2x_msgs::msg::TrafficControlBounds cB; /*Fill the longitude value in message cB with the value of lon*/
1751
1752 cB.reflat = gpsRoute.lat;
1753 cB.reflon = gpsRoute.lon;
1754
1755 cB.offsets[0].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x; // Points in clockwise order min,min (lat,lon point) -> max,min -> max,max -> min,max
1756 cB.offsets[0].deltay = 0.0; // calculating the offsets
1757 cB.offsets[1].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x;
1758 cB.offsets[1].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
1759 cB.offsets[2].deltax = 0.0;
1760 cB.offsets[2].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
1761
1762 tcr_polygon_ = composeTCRStatus(localPoint, cB, local_projector); // TCR polygon can be visualized in UI
1763
1764 cB.oldest =rclcpp::Time(0.0,0.0, scheduler_.getClockType()); // TODO this needs to be set to 0 or an older value as otherwise this will filter out all controls
1765
1766 cR.choice = carma_v2x_msgs::msg::TrafficControlRequest::TCRV01;
1767
1768 // create 16 byte uuid
1769 boost::uuids::uuid uuid_id = boost::uuids::random_generator()();
1770 // take half as string
1771 std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
1772 std::string req_id_test = "12345678"; // TODO this is an extremely risky way of performing a unit test. This method needs to be refactored so that unit tests cannot side affect actual implementations
1773 generated_geofence_reqids_.insert(req_id_test);
1774 generated_geofence_reqids_.insert(reqid);
1775
1776
1777 // copy to reqid array
1778 boost::array<uint8_t, 16UL> req_id;
1779 std::copy(uuid_id.begin(),uuid_id.end(), req_id.begin());
1780 for (auto i = 0; i < 8; i ++)
1781 {
1782 cR.tcr_v01.reqid.id[i] = req_id[i];
1783 if (req_id_for_testing) req_id_for_testing->id[i] = req_id[i];
1784 }
1785
1786 cR.tcr_v01.bounds.push_back(cB);
1787
1788 return cR;
1789
1790}
lanelet::ConstLanelets route_path_
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
carma_v2x_msgs::msg::TrafficControlRequestPolygon tcr_polygon_
std::unordered_set< std::string > generated_geofence_reqids_

References base_map_georef_, composeTCRStatus(), current_map_, generated_geofence_reqids_, carma_wm_ctrl::GeofenceScheduler::getClockType(), process_bag::i, route_path_, scheduler_, tcr_polygon_, and carma_cooperative_perception::to_string().

Referenced by routeCallbackMessage().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ convertLightIdToInterGroupId()

bool carma_wm_ctrl::WMBroadcaster::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

Parameters
[in]trafficlanelet_id
[out]intersection_idand group_id
Returns
return true if conversion was successful

Definition at line 1956 of file WMBroadcaster.cpp.

1957{
1958 for (auto it = traffic_light_id_lookup_.begin(); it != traffic_light_id_lookup_.end(); ++it)
1959 {
1960 // Reverse of the logic for generating the lanelet_id. Reference function generate32BitId(const std::string& label)
1961 if (it -> second == lanelet_id)
1962 {
1963 group_id = (it -> first & 0xFF);
1964 intersection_id = (it -> first >> 8);
1965 return true;
1966 }
1967 }
1968 return false;
1969}

References traffic_light_id_lookup_.

Referenced by publishLightId().

Here is the caller graph for this function:

◆ createLinearInterpolatingLanelet()

lanelet::Lanelet carma_wm_ctrl::WMBroadcaster::createLinearInterpolatingLanelet ( const lanelet::Point3d &  left_front_pt,
const lanelet::Point3d &  right_front_pt,
const lanelet::Point3d &  left_back_pt,
const lanelet::Point3d &  right_back_pt,
double  increment_distance = 0.25 
)
private

Definition at line 2192 of file WMBroadcaster.cpp.

2193{
2194 return lanelet::Lanelet(lanelet::utils::getId(), createLinearInterpolatingLinestring(left_front_pt, left_back_pt, increment_distance), createLinearInterpolatingLinestring(right_front_pt, right_back_pt, increment_distance));
2195}
lanelet::LineString3d createLinearInterpolatingLinestring(const lanelet::Point3d &front_pt, const lanelet::Point3d &back_pt, double increment_distance=0.25)

References createLinearInterpolatingLinestring().

Referenced by createWorkzoneGeometry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ createLinearInterpolatingLinestring()

lanelet::LineString3d carma_wm_ctrl::WMBroadcaster::createLinearInterpolatingLinestring ( const lanelet::Point3d &  front_pt,
const lanelet::Point3d &  back_pt,
double  increment_distance = 0.25 
)
private

Definition at line 2171 of file WMBroadcaster.cpp.

2172{
2173 double dx = back_pt.x() - front_pt.x();
2174 double dy = back_pt.y() - front_pt.y();
2175
2176 std::vector<lanelet::Point3d> points;
2177 double distance = std::sqrt(pow(dx, 2) + pow(dy,2));
2178 double cos = dx / distance;
2179 double sin = dy / distance;
2180 points.push_back(front_pt);
2181 double sum = increment_distance;
2182 while ( sum < distance)
2183 {
2184 points.push_back(lanelet::Point3d(lanelet::utils::getId(),front_pt.x() + sum * cos, front_pt.y() + sum * sin, 0.0));
2185 sum += increment_distance;
2186 }
2187 points.push_back(back_pt);
2188
2189 return lanelet::LineString3d(lanelet::utils::getId(), points);
2190}

Referenced by createLinearInterpolatingLanelet().

Here is the caller graph for this function:

◆ createWorkzoneGeofence()

std::shared_ptr< Geofence > carma_wm_ctrl::WMBroadcaster::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 traffic lights) and update_list that blocks old lanelets. Geofence will have minimum of 6 lanelet_additions_ (front parallel, front diagonal, middle lanelet(s), back diagonal, back parallel, 1 opposing lanelet with trafficlight). And regulatory_element_ of this geofence will be region_access_rule, where it blocks entire affected_parts of CLOSED, TAPERRIGHT, OPENRIGHT. It also blocks the opposing lane's lanelet that would have had the trafficlight, and new lanelet(s) would be added to replace it.

Parameters
work_zone_geofence_cacheGeofence map with size of 4 corresponding to CLOSED, TAPERRIGHT, OPENRIGHT, REVERSE TrafficControlMessages. Each should have gf_pts, affected_parts, schedule, and id filled. TAPERRIGHT's id and schedule is used as all should have same schedule.
Exceptions
InvalidObjectStateErrorif no map is available
Returns
geofence housing all necessary workzone elements

Definition at line 393 of file WMBroadcaster.cpp.

394{
395 std::shared_ptr<std::vector<lanelet::Lanelet>> parallel_llts = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
396 std::shared_ptr<std::vector<lanelet::Lanelet>> middle_opposite_lanelets = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
397
398 // Split existing lanelets and filter into parallel_llts and middle_opposite_lanelets
399 preprocessWorkzoneGeometry(work_zone_geofence_cache, parallel_llts, middle_opposite_lanelets);
400
401 // Create geofence and rest of the required lanelets along with traffic light for completing workzone area
402 auto gf_ptr = createWorkzoneGeometry(work_zone_geofence_cache, parallel_llts->front(), parallel_llts->back(), middle_opposite_lanelets );
403
404 // copy static info from the existing workzone
405 gf_ptr->id_ = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->id_; //using taperright's id as the whole geofence's id
406
407 // schedule
408 gf_ptr->schedules = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->schedules; //using taperright's schedule as the whole geofence's schedule
409
410 // erase cache now that it is processed
411 for (auto pair : work_zone_geofence_cache)
412 {
413 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone geofence finished processing. Therefore following geofence id is being dropped from cache as it is processed as part of it: " << pair.second->id_);
414 }
415 work_zone_geofence_cache.clear();
416
417 return gf_ptr;
418}
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...
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 ...
static const uint8_t TAPERRIGHT

References createWorkzoneGeometry(), preprocessWorkzoneGeometry(), and carma_wm_ctrl::WorkZoneSection::TAPERRIGHT.

Referenced by addGeofence().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ createWorkzoneGeometry()

std::shared_ptr< Geofence > carma_wm_ctrl::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,
std::shared_ptr< std::vector< lanelet::Lanelet > >  middle_opposite_lanelets 
)

Create workzone geofence. Create diagonal lanelets and a lanelet that houses opposing lane's trafficlight. Fill lanelet_additions_ with every newly created lanelets New lanelets will have traffic light. Old lanelets (and those from CLOSED) will be blocked using update_list as region_access_rule.

Parameters
work_zone_geofence_cacheGeofence map with size of 4 corresponding to CLOSED, TAPERRIGHT, OPENRIGHT, REVERSE TrafficControlMessages. Each should have gf_pts, affected_parts.
parallel_llt_frontA lanelet whose end should connect to front diagonal lanelet
parallel_llt_backA lanelet whose start should connect to back diagonal lanelet
middle_opposite_laneletsA getInterGroupIdsByLightReg() connects to back diagonal (their directions are expected to be opposite of parallel ones)
Exceptions
InvalidObjectStateErrorif no map is available

Definition at line 420 of file WMBroadcaster.cpp.

422{
423 auto gf_ptr = std::make_shared<Geofence>();
424
426 //FRONT DIAGONAL LANELET
428
429 lanelet::Lanelet front_llt_diag = createLinearInterpolatingLanelet(parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back(),
430 middle_opposite_lanelets->back().rightBound3d().back(), middle_opposite_lanelets->back().leftBound3d().back());
431 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created diag front_llt_diag id:" << front_llt_diag.id());
432 for (auto regem : middle_opposite_lanelets->back().regulatoryElements()) //copy existing regem into the new llts
433 {
434 front_llt_diag.addRegulatoryElement(regem);
435 }
436
438 //BACK DIAGONAL LANELET
440
441 lanelet::Lanelet back_llt_diag = createLinearInterpolatingLanelet(middle_opposite_lanelets->front().rightBound3d().front(), middle_opposite_lanelets->front().leftBound3d().front(),
442 parallel_llt_back.leftBound3d().front(), parallel_llt_back.rightBound3d().front());
443 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created back_llt_diag diag id:" << back_llt_diag.id());
444 for (auto regem : parallel_llt_back.regulatoryElements()) //copy existing regem into the new llts
445 {
446 back_llt_diag.addRegulatoryElement(regem);
447 }
448
450 //MIDDLE LANELETS TO MATCH PARALLEL DIRECTION
452
453 std::vector<lanelet::Lanelet> middle_llts;
454 for (int i = middle_opposite_lanelets->size() - 1; i >= 0; i--) //do no use size_t as it might overflow
455 {
456 lanelet::Lanelet middle_llt (lanelet::utils::getId(), (*(middle_opposite_lanelets.get()))[i].rightBound3d().invert(), (*(middle_opposite_lanelets.get()))[i].leftBound3d().invert());
457 for (auto regem : (*(middle_opposite_lanelets.get()))[i].regulatoryElements())
458 {
459 middle_llt.addRegulatoryElement(regem); //copy existing regem into the new llts
460 }
461 middle_llts.push_back(middle_llt);
462 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created matching direction of middle_llt id:" << middle_llt.id());
463 }
464
466 //ADD TF_LIGHT TO PARALLEL LANELET
468 lanelet::LineString3d parallel_stop_ls(lanelet::utils::getId(), {parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back()});
469 // controlled lanelet:
470 std::vector<lanelet::Lanelet> controlled_taper_right;
471
472 controlled_taper_right.push_back(parallel_llt_front); //which has the light
473
474 controlled_taper_right.push_back(front_llt_diag);
475
476 controlled_taper_right.insert(controlled_taper_right.end(), middle_llts.begin(), middle_llts.end());
477
478 controlled_taper_right.push_back(back_llt_diag);
479
480 lanelet::CarmaTrafficSignalPtr tfl_parallel = std::make_shared<lanelet::CarmaTrafficSignal>(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {parallel_stop_ls}, {controlled_taper_right.front()}, {controlled_taper_right.back()}));
481
482 gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->label_),tfl_parallel->id()});
483
484 parallel_llt_front.addRegulatoryElement(tfl_parallel);
485 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created TF_LIGHT of Id: " << tfl_parallel->id() << ", to parallel_llt_front id:" << parallel_llt_front.id());
486
488 //ADD TF_LIGHT TO OPPOSITE LANELET
490 std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts_with_stop_line = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
491
492 auto old_opposite_llts = splitOppositeLaneletWithPoint(opposite_llts_with_stop_line, work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d(),
493 current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id()), error_distance_);
494
495 lanelet::LineString3d opposite_stop_ls(lanelet::utils::getId(), {opposite_llts_with_stop_line->front().leftBound3d().back(), opposite_llts_with_stop_line->front().rightBound3d().back()});
496
497 std::vector<lanelet::Lanelet> controlled_open_right;
498
499 controlled_open_right.insert(controlled_open_right.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());; //split lanelet one of which has light
500
501 for (auto llt : work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_)
502 {
503 controlled_open_right.push_back(current_map_->laneletLayer.get(llt.lanelet().get().id()));
504 }
505
506 lanelet::CarmaTrafficSignalPtr tfl_opposite = std::make_shared<lanelet::CarmaTrafficSignal>(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {opposite_stop_ls}, {controlled_open_right.front()}, {controlled_open_right.back()}));
507
508 gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->label_), tfl_opposite->id()});
509
510 opposite_llts_with_stop_line->front().addRegulatoryElement(tfl_opposite);
511 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created TF_LIGHT of Id: " << tfl_opposite->id() << ", to opposite_llts_with_stop_line->front() id:" << opposite_llts_with_stop_line->front().id());
512
514 //ADD ALL NEWLY CREATED LANELETS INTO GEOFENCE
515 //OBJECTS TO BE PROCESSED LATER BY SCHEDULER
517 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_front id:" << parallel_llt_front.id());
518 gf_ptr->lanelet_additions_.push_back(parallel_llt_front);
519
520 gf_ptr->lanelet_additions_.push_back(front_llt_diag);
521
522 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), middle_llts.begin(), middle_llts.end());
523
524 gf_ptr->lanelet_additions_.push_back(back_llt_diag);
525 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_back id:" << parallel_llt_back.id());
526 gf_ptr->lanelet_additions_.push_back(parallel_llt_back);
527
528 gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());;
529
531 // ADD REGION_ACCESS_RULE REGEM TO THE OUTDATED LANELETS
532 // AS WELL AS LANELETS BEING BLOCKED BY WORKZONE GEOFENCE
534
535 // fill information for paricipants and reason for blocking
536 carma_v2x_msgs::msg::TrafficControlMessageV01 participants_and_reason_only;
537
538 j2735_v2x_msgs::msg::TrafficControlVehClass participant; // sending all possible VEHICLE will be processed as they are not accessuble by regionAccessRule
539
540 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE;
541
542 participants_and_reason_only.params.vclasses.push_back(participant);
543
544 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BUS;
545
546 participants_and_reason_only.params.vclasses.push_back(participant);
547
548 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR;
549
550 participants_and_reason_only.params.vclasses.push_back(participant);
551
552 participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK;
553
554 participants_and_reason_only.params.vclasses.push_back(participant);
555
556 participants_and_reason_only.package.label = "SIG_WZ";
557
558 std::vector<lanelet::Lanelet> old_or_blocked_llts; // this is needed to addRegionAccessRule input signatures
559
560 // from all affected parts, remove duplicate entries
561 for (auto llt : work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_)
562 {
563 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
564 {
565 gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
566 old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
567 }
568 }
569 for (auto llt : work_zone_geofence_cache[WorkZoneSection::CLOSED]->affected_parts_)
570 {
571 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
572 {
573 gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
574 old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
575 }
576 }
577 for (auto llt : work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_)
578 {
579 if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
580 {
581 gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
582 old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
583 }
584 }
585
586 gf_ptr->affected_parts_.push_back(old_opposite_llts[0]); // block old lanelet in the opposing lanelet that will be replaced with split lanelets that have trafficlight
587 old_or_blocked_llts.push_back(old_opposite_llts[0]);
588
589 // actual regulatory element adder
590 addRegionAccessRule(gf_ptr, participants_and_reason_only, old_or_blocked_llts);
591
592 return gf_ptr;
593}
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)
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 ...
uint32_t generate32BitId(const std::string &label)
helper for generating 32bit traffic light Id from TCM label field consisting workzone intersection/si...
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.
static const uint8_t CLOSED
static const uint8_t OPENRIGHT
static const uint8_t REVERSE

References addRegionAccessRule(), carma_wm_ctrl::WorkZoneSection::CLOSED, createLinearInterpolatingLanelet(), current_map_, error_distance_, generate32BitId(), process_bag::i, carma_wm_ctrl::WorkZoneSection::OPENRIGHT, carma_wm_ctrl::WorkZoneSection::REVERSE, splitOppositeLaneletWithPoint(), and carma_wm_ctrl::WorkZoneSection::TAPERRIGHT.

Referenced by createWorkzoneGeofence().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ currentLocationCallback()

void carma_wm_ctrl::WMBroadcaster::currentLocationCallback ( geometry_msgs::msg::PoseStamped::UniquePtr  current_pos)

Definition at line 1946 of file WMBroadcaster.cpp.

1947{
1948 if (current_map_ && current_map_->laneletLayer.size() != 0) {
1949 carma_perception_msgs::msg::CheckActiveGeofence check = checkActiveGeofenceLogic(*current_pos);
1950 active_pub_(check);//Publish
1951 } else {
1952 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Could not check active geofence logic because map was not loaded");
1953 }
1954}
carma_perception_msgs::msg::CheckActiveGeofence checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped &current_pos)
Returns a message indicating whether or not the vehicle is inside of an active geofence lanelet.

References active_pub_, checkActiveGeofenceLogic(), and current_map_.

Referenced by carma_wm_ctrl::WMBroadcasterNode::handle_on_activate().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ distToNearestActiveGeofence()

double carma_wm_ctrl::WMBroadcaster::distToNearestActiveGeofence ( const lanelet::BasicPoint2d &  curr_pos)

Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet.

Parameters
curr_posCurrent position in local coordinates
Exceptions
InvalidObjectStateErrorif base_map is not set
std::invalid_argumentif curr_pos is not on the road
Returns
0 if there is no active geofence on the vehicle's route

Definition at line 1868 of file WMBroadcaster.cpp.

1869{
1870 std::lock_guard<std::mutex> guard(map_mutex_);
1871
1872 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1873 {
1874 throw lanelet::InvalidObjectStateError(std::string("Lanelet map (current_map_) is not loaded to the WMBroadcaster"));
1875 }
1876
1877 // filter only the lanelets in the route
1878 std::vector<lanelet::Id> active_geofence_on_route;
1879 for (auto llt : route_path_)
1880 {
1881 if (active_geofence_llt_ids_.find(llt.id()) != active_geofence_llt_ids_.end())
1882 active_geofence_on_route.push_back(llt.id());
1883 }
1884 // Get the lanelet of this point
1885 auto curr_lanelet = lanelet::geometry::findNearest(current_map_->laneletLayer, curr_pos, 1)[0].second;
1886
1887 // Check if this point at least is actually within this lanelets
1888 if (!boost::geometry::within(curr_pos, curr_lanelet.polygon2d().basicPolygon()))
1889 throw std::invalid_argument("Given point is not within any lanelet");
1890
1891 // get route distance (downtrack + cross_track) distances to every lanelets by their ids
1892 std::vector<double> route_distances;
1893 // and take abs of cross_track to add them to get route distance
1894 for (auto id: active_geofence_on_route)
1895 {
1896 carma_wm::TrackPos tp = carma_wm::geometry::trackPos(current_map_->laneletLayer.get(id), curr_pos);
1897 // downtrack needs to be negative for lanelet to be in front of the point,
1898 // also we don't account for the lanelet that the vehicle is on
1899 if (tp.downtrack < 0 && id != curr_lanelet.id())
1900 {
1901 double dist = fabs(tp.downtrack) + fabs(tp.crosstrack);
1902 route_distances.push_back(dist);
1903 }
1904 }
1905 std::sort(route_distances.begin(), route_distances.end());
1906
1907 if (route_distances.size() != 0 ) return route_distances[0];
1908 else return 0.0;
1909
1910}
Position in a track based coordinate system where the axis are downtrack and crosstrack....
Definition: TrackPos.hpp:35
TrackPos trackPos(const lanelet::ConstLanelet &lanelet, const lanelet::BasicPoint2d &point)
Returns the TrackPos, computed in 2d, of the provided point relative to the centerline of the provide...
Definition: Geometry.cpp:120

References active_geofence_llt_ids_, carma_wm::TrackPos::crosstrack, current_map_, carma_wm::TrackPos::downtrack, map_mutex_, route_path_, and carma_wm::geometry::trackPos().

Referenced by checkActiveGeofenceLogic().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ externalMapMsgCallback()

void carma_wm_ctrl::WMBroadcaster::externalMapMsgCallback ( carma_v2x_msgs::msg::MapData::UniquePtr  map_msg)

Callback to MAP.msg which contains intersections' static info such geometry and lane ids.

Parameters
map_msgThe ROS msg of the MAP.msg to process

Definition at line 1030 of file WMBroadcaster.cpp.

1031{
1032 auto gf_ptr = std::make_shared<Geofence>();
1033
1034 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1035 {
1036 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Map is not available yet. Skipping MAP msg");
1037 return;
1038 }
1039
1040 // check if we have seen this message already
1041 bool up_to_date = false;
1042 if (sim_->intersection_id_to_regem_id_.size() == map_msg->intersections.size())
1043 {
1044 up_to_date = true;
1045 // check id of the intersection only
1046 for (auto intersection : map_msg->intersections)
1047 {
1048 if (sim_->intersection_id_to_regem_id_.find(intersection.id.id) == sim_->intersection_id_to_regem_id_.end())
1049 {
1050 up_to_date = false;
1051 break;
1052 }
1053 }
1054 }
1055
1056 if(up_to_date)
1057 {
1058 return;
1059 }
1060
1061 gf_ptr->map_msg_ = *map_msg;
1062 gf_ptr->msg_.package.label_exists = true;
1063 gf_ptr->msg_.package.label = "MAP_MSG";
1064 gf_ptr->id_ = boost::uuids::random_generator()();
1065
1066 // create dummy traffic Control message to add instant activation schedule
1067 carma_v2x_msgs::msg::TrafficControlMessageV01 traffic_control_msg;
1068
1069 // process schedule from message
1070 addScheduleFromMsg(gf_ptr, traffic_control_msg);
1071
1072 scheduleGeofence(gf_ptr);
1073
1074}
void scheduleGeofence(std::shared_ptr< carma_wm_ctrl::Geofence > gf_ptr_list)
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.

References addScheduleFromMsg(), current_map_, scheduleGeofence(), and sim_.

Referenced by carma_wm_ctrl::WMBroadcasterNode::handle_on_activate().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ filterSuccessorLanelets()

std::unordered_set< lanelet::Lanelet > carma_wm_ctrl::WMBroadcaster::filterSuccessorLanelets ( const std::unordered_set< lanelet::Lanelet > &  possible_lanelets,
const std::unordered_set< lanelet::Lanelet > &  root_lanelets 
)
private

◆ generate32BitId()

uint32_t carma_wm_ctrl::WMBroadcaster::generate32BitId ( const std::string &  label)

helper for generating 32bit traffic light Id from TCM label field consisting workzone intersection/signal group ids

Definition at line 1252 of file WMBroadcaster.cpp.

1253{
1254 auto pos1 = label.find("INT_ID:") + 7;
1255 auto pos2 = label.find("SG_ID:") + 6;
1256
1257 uint16_t intersection_id = std::stoi(label.substr(pos1 , 4));
1258 uint8_t signal_id = std::stoi(label.substr(pos2 , 3));
1259
1260 return carma_wm::utils::get32BitId(intersection_id, signal_id);
1261}
uint32_t get32BitId(uint16_t intersection_id, uint8_t signal_group_id)
Get 32bit id by concatenating 16bit id with 8bit signal_group_id.

References carma_wm::utils::get32BitId().

Referenced by createWorkzoneGeometry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ geofenceCallback()

void carma_wm_ctrl::WMBroadcaster::geofenceCallback ( carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr  geofence_msg)

Callback to add a geofence to the map. Currently only supports version 1 TrafficControlMessage.

Parameters
geofence_msgThe ROS msg of the geofence to add.

Definition at line 1077 of file WMBroadcaster.cpp.

1078{
1079
1080 std::lock_guard<std::mutex> guard(map_mutex_);
1081 std::stringstream reason_ss;
1082 // quickly check if the id has been added
1083 if (geofence_msg->choice != carma_v2x_msgs::msg::TrafficControlMessage::TCMV01) {
1084 reason_ss << "Dropping received geofence for unsupported TrafficControl version: " << geofence_msg->choice;
1085 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str());
1086 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::REJECTED), reason_ss.str());
1087 return;
1088 }
1089
1090 boost::uuids::uuid id;
1091 std::copy(geofence_msg->tcm_v01.id.id.begin(), geofence_msg->tcm_v01.id.id.end(), id.begin());
1093 reason_ss.str("");
1094 reason_ss << "Dropping received TrafficControl message with already handled id: " << boost::uuids::to_string(id);
1095 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str());
1096 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str());
1097 return;
1098 }
1099
1100 // convert reqid to string check if it has been seen before
1101 boost::array<uint8_t, 16UL> req_id;
1102 for (auto i = 0; i < 8; i ++) req_id[i] = geofence_msg->tcm_v01.reqid.id[i];
1103 boost::uuids::uuid uuid_id;
1104 std::copy(req_id.begin(),req_id.end(), uuid_id.begin());
1105 std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
1106 // drop if the req has never been sent
1107 if (generated_geofence_reqids_.find(reqid) == generated_geofence_reqids_.end() && reqid.compare("00000000") != 0)
1108 {
1109 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid);
1110 return;
1111 }
1112
1114
1115 auto gf_ptr = std::make_shared<Geofence>();
1116
1117 gf_ptr->msg_ = geofence_msg->tcm_v01;
1118
1119 try
1120 {
1121 // process schedule from message
1122 addScheduleFromMsg(gf_ptr, geofence_msg->tcm_v01);
1123 scheduleGeofence(gf_ptr);
1124 reason_ss.str("");
1125 reason_ss << "Successfully processed TCM.";
1126 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str());
1127 }
1128 catch(std::exception& ex)
1129 {
1130 reason_ss.str("");
1131 reason_ss << "Failed to process TCM. " << ex.what();
1132 pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::REJECTED), reason_ss.str());
1133 throw; //rethrows the exception object
1134 }
1135};
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...
std::unordered_set< std::string > checked_geofence_ids_

References ACKNOWLEDGED, addScheduleFromMsg(), checked_geofence_ids_, generated_geofence_reqids_, process_bag::i, map_mutex_, pubTCMACK(), REJECTED, scheduleGeofence(), and carma_cooperative_perception::to_string().

Referenced by carma_wm_ctrl::WMBroadcasterNode::handle_on_activate().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ geofenceFromMapMsg()

std::vector< std::shared_ptr< Geofence > > carma_wm_ctrl::WMBroadcaster::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 geometry and signal_group.

Parameters
Geofenceobject to fill with information extracted from this msg
geofence_msgThe MAP ROS msg that contains intersection information
Exceptions
InvalidObjectStateErrorif base_map is not set or the base_map's georeference is empty

Definition at line 217 of file WMBroadcaster.cpp.

218{
219 std::vector<std::shared_ptr<Geofence>> updates_to_send;
220 std::vector<std::shared_ptr<lanelet::SignalizedIntersection>> intersections;
221 std::vector<std::shared_ptr<lanelet::CarmaTrafficSignal>> traffic_signals;
222
223 auto sim_copy = std::make_shared<carma_wm::SignalizedIntersectionManager>(*sim_);
224
225 sim_->createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, current_map_, current_routing_graph_);
226
227 if (*sim_ == *sim_copy) // if no change
228 {
229 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), ">>> Detected no change from previous, ignoring duplicate message! with gf id: " << gf_ptr->id_);
230 return {};
231 }
232
233 for (auto intersection : intersections)
234 {
235 auto update = std::make_shared<Geofence>();
236 update->id_ = boost::uuids::random_generator()();
238 update->regulatory_element_ = intersection;
239 for (auto llt : intersection->getEntryLanelets())
240 {
241 update->affected_parts_.push_back(llt);
242 }
243 updates_to_send.push_back(update);
244 }
245
246 for (auto signal : traffic_signals)
247 {
248 auto update = std::make_shared<Geofence>();
249 update->id_ = boost::uuids::random_generator()();
250 update->label_ = carma_wm_ctrl::MAP_MSG_TF_SIGNAL;
251 update->regulatory_element_ = signal;
252 for (auto llt : signal->getControlStartLanelets())
253 {
254 update->affected_parts_.push_back(llt);
255 }
256 updates_to_send.push_back(update);
257 }
258
259 return updates_to_send;
260}
const std::string MAP_MSG_TF_SIGNAL
Definition: Geofence.hpp:41
const std::string MAP_MSG_INTERSECTION
Definition: Geofence.hpp:40

References current_map_, current_routing_graph_, carma_wm_ctrl::MAP_MSG_INTERSECTION, carma_wm_ctrl::MAP_MSG_TF_SIGNAL, and sim_.

Referenced by addGeofence().

Here is the caller graph for this function:

◆ geofenceFromMsg()

void carma_wm_ctrl::WMBroadcaster::geofenceFromMsg ( std::shared_ptr< Geofence gf_ptr,
const carma_v2x_msgs::msg::TrafficControlMessageV01 &  geofence_msg 
)

Fills geofence object from TrafficControlMessageV01 ROS Msg.

Parameters
Geofenceobject to fill with information extracted from this msg and previously cached msgs that are relevant
geofence_msgThe ROS msg that contains geofence information
Exceptions
InvalidObjectStateErrorif base_map is not set or the base_map's georeference is empty

Definition at line 262 of file WMBroadcaster.cpp.

263{
264 bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find("SIG_WZ") != std::string::npos;
265 carma_v2x_msgs::msg::TrafficControlDetail msg_detail = msg_v01.params.detail;
266
267 // Get ID
268 std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin());
269
270 gf_ptr->gf_pts = getPointsInLocalFrame(msg_v01);
271
272 gf_ptr->affected_parts_ = getAffectedLaneletOrAreas(gf_ptr->gf_pts);
273
274 if (gf_ptr->affected_parts_.size() == 0) {
275 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "There is no applicable component in map for the new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
276 return; // Return empty geofence list
277 }
278
279 std::vector<lanelet::Lanelet> affected_llts;
280 std::vector<lanelet::Area> affected_areas;
281
282 // used for assigning them to the regem as parameters
283 for (auto llt_or_area : gf_ptr->affected_parts_)
284 {
285
286 if (llt_or_area.isLanelet()) affected_llts.push_back(current_map_->laneletLayer.get(llt_or_area.lanelet()->id()));
287 if (llt_or_area.isArea()) affected_areas.push_back(current_map_->areaLayer.get(llt_or_area.area()->id()));
288 }
289
290 // TODO: logic to determine what type of geofence goes here
291 // currently only converting portion of control message that is relevant to:
292 // - digital speed limit, passing control line, digital minimum gap, region access rule, and series of workzone related messages
293 lanelet::Velocity sL;
294
295
296 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
297 {
298 //Acquire speed limit information from TafficControlDetail msg
299 sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPS());
300 std::string reason = "";
301 if (msg_v01.package.label_exists)
302 reason = msg_v01.package.label;
303
304 if(config_limit > 0_mph && config_limit < 80_mph && config_limit < sL)//Accounting for the configured speed limit, input zero when not in use
305 sL = config_limit;
306 //Ensure Geofences do not provide invalid speed limit data (exceed predetermined maximum value)
307 // @SONAR_STOP@
308 if(sL > 80_mph )
309 {
310 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message
311 sL = 80_mph; //Cap the speed limit to the predetermined maximum value
312
313 }
314 if(sL < 0_mph)
315 {
316 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph.");
317 sL = 0_mph;
318 }// @SONAR_START@
319
320 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
321 sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason));
322 }
323 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE)
324 {
325 //Acquire speed limit information from TafficControlDetail msg
326 sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPS());
327 if(config_limit > 0_mph && config_limit < 80_mph)//Accounting for the configured speed limit, input zero when not in use
328 sL = config_limit;
329
330 std::string reason = "";
331 if (msg_v01.package.label_exists)
332 reason = msg_v01.package.label;
333
334 //Ensure Geofences do not provide invalid speed limit data
335 // @SONAR_STOP@
336 if(sL > 80_mph )
337 {
338 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value capped at max speed limit.");
339 sL = 80_mph;
340 }
341 if(sL < 0_mph)
342 {
343 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph.");
344 sL = 0_mph;
345 }// @SONAR_START@
346 gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
347 sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason));
348 }
349 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE)
350 {
351 addPassingControlLineFromMsg(gf_ptr, msg_v01, affected_llts);
352 }
353 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE)
354 {
355
356 double min_gap = (double)msg_detail.minhdwy;
357
358 if(min_gap < 0)
359 {
360 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital min gap is invalid. Value set to 0 meter.");
361 min_gap = 0;
362 }
363 addRegionMinimumGap(gf_ptr,msg_v01, min_gap, affected_llts, affected_areas);
364 }
365
366 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later
367 {
368 gf_ptr->label_ = msg_v01.package.label; // to extract intersection, and signal group id
369 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
370 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
371 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
372 {
373 work_zone_geofence_cache_[msg_detail.closed] = gf_ptr;
374 }
375 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
376 {
378 }
379 return;
380 }
381 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==carma_v2x_msgs::msg::TrafficControlDetail::CLOSED) // if stand-alone closed signal apart from Workzone
382 {
383 addRegionAccessRule(gf_ptr,msg_v01,affected_llts);
384 }
385
386 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::RESTRICTED_CHOICE) {
387 addRegionAccessRule(gf_ptr, msg_v01, affected_llts);
388 }
389
390 return;
391}
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.
lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d &gf_pts)
Gets the affected lanelet or areas based on the points in local frame.
lanelet::Points3d getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01 &geofence_msg)
Extract geofence points from geofence message using its given proj and datum fields.
void addPassingControlLineFromMsg(std::shared_ptr< Geofence > gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01 &msg_v01, const std::vector< lanelet::Lanelet > &affected_llts) const

References addPassingControlLineFromMsg(), addRegionAccessRule(), addRegionMinimumGap(), config_limit, current_map_, getAffectedLaneletOrAreas(), getPointsInLocalFrame(), participantsChecker(), carma_wm_ctrl::WorkZoneSection::REVERSE, and work_zone_geofence_cache_.

Referenced by addGeofence().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ geoReferenceCallback()

void carma_wm_ctrl::WMBroadcaster::geoReferenceCallback ( std_msgs::msg::String::UniquePtr  geo_ref)

Callback to set the base map georeference (proj string)

Parameters
georef_msgProj string that specifies the georeference of the map. It is used for transfering frames between that of geofence and that of the vehicle

Definition at line 1198 of file WMBroadcaster.cpp.

1199{
1200 std::lock_guard<std::mutex> guard(map_mutex_);
1201 sim_->setTargetFrame(geo_ref->data);
1202 base_map_georef_ = geo_ref->data;
1203}

References base_map_georef_, map_mutex_, and sim_.

Referenced by carma_wm_ctrl::WMBroadcasterNode::handle_on_activate().

Here is the caller graph for this function:

◆ getAffectedLaneletOrAreas()

lanelet::ConstLaneletOrAreas carma_wm_ctrl::WMBroadcaster::getAffectedLaneletOrAreas ( const lanelet::Points3d &  gf_pts)

Gets the affected lanelet or areas based on the points in local frame.

Parameters
geofence_msglanelet::Points3d in local frame NOTE:Currently this function only checks lanelets and will be expanded to areas in the future.

Definition at line 1344 of file WMBroadcaster.cpp.

1345{
1347}
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.

References current_map_, current_routing_graph_, carma_wm::query::getAffectedLaneletOrAreas(), and max_lane_width_.

Referenced by geofenceFromMsg().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getPointsInLocalFrame()

lanelet::Points3d carma_wm_ctrl::WMBroadcaster::getPointsInLocalFrame ( const carma_v2x_msgs::msg::TrafficControlMessageV01 &  geofence_msg)

Extract geofence points from geofence message using its given proj and datum fields.

Parameters
geofence_msgThe ROS msg that contains proj and any point that lie on the target lanelet or area
Exceptions
InvalidObjectStateErrorif base_map is not set or the base_map's georeference is empty
Returns
lanelet::Points3d in local frame

Definition at line 1264 of file WMBroadcaster.cpp.

1265{
1266 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Getting affected lanelets");
1267 if (!current_map_ || current_map_->laneletLayer.size() == 0)
1268 {
1269 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
1270 }
1271 if (base_map_georef_ == "")
1272 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map has empty proj string loaded as georeference. Therefore, WMBroadcaster failed to\n ") +
1273 std::string("get transformation between the geofence and the map"));
1274
1275 // This next section handles the geofence projection conversion
1276 // The datum field is used to identify the frame for the provided referance lat/lon.
1277 // This reference is then converted to the provided projection as a reference origin point
1278 // From the reference the message projection to map projection transformation is used to convert the nodes in the TrafficControlMessage
1279 std::string projection = tcm_v01.geometry.proj;
1280 std::string datum = tcm_v01.geometry.datum;
1281 if (datum.empty()) {
1282 RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field not populated. Attempting to use WGS84");
1283 datum = "WGS84";
1284 }
1285
1286 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Projection field: " << projection);
1287 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field: " << datum);
1288
1289 std::string universal_frame = datum; //lat/long included in TCM is in this datum
1290
1291
1292 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Traffic Control heading provided: " << tcm_v01.geometry.heading << " System understanding is that this value will not affect the projection and is only provided for supporting derivative calculations.");
1293
1294 // Create the resulting projection transformation
1295 PJ* universal_to_target = proj_create_crs_to_crs(PJ_DEFAULT_CTX, universal_frame.c_str(), projection.c_str(), nullptr);
1296 if (universal_to_target == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
1297
1298 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1299 << " universal_frame: " << universal_frame << " projection: " << projection);
1300
1301 return {}; // Ignore geofence if it could not be projected from universal to TCM frame
1302 }
1303
1304 PJ* target_to_map = proj_create_crs_to_crs(PJ_DEFAULT_CTX, projection.c_str(), base_map_georef_.c_str(), nullptr);
1305
1306 if (target_to_map == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
1307
1308 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX)
1309 << " target_to_map: " << target_to_map << " base_map_georef_: " << base_map_georef_);
1310
1311 return {}; // Ignore geofence if it could not be projected into the map frame
1312
1313 }
1314
1315 // convert all geofence points into our map's frame
1316 std::vector<lanelet::Point3d> gf_pts;
1317 carma_v2x_msgs::msg::PathNode prev_pt;
1318 PJ_COORD c_init_latlong{{tcm_v01.geometry.reflat, tcm_v01.geometry.reflon, tcm_v01.geometry.refelv}};
1319 PJ_COORD c_init = proj_trans(universal_to_target, PJ_FWD, c_init_latlong);
1320
1321 prev_pt.x = c_init.xyz.x;
1322 prev_pt.y = c_init.xyz.y;
1323
1324 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "In TCM's frame, initial Point X "<< prev_pt.x<<" Before conversion: Point Y "<< prev_pt.y );
1325 for (auto pt : tcm_v01.geometry.nodes)
1326 {
1327 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion in TCM frame: Point X "<< pt.x <<" Before conversion: Point Y "<< pt.y);
1328
1329 PJ_COORD c {{prev_pt.x + pt.x, prev_pt.y + pt.y, 0, 0}}; // z is not currently used
1330 PJ_COORD c_out;
1331 c_out = proj_trans(target_to_map, PJ_FWD, c);
1332
1333 gf_pts.push_back(lanelet::Point3d{current_map_->pointLayer.uniqueId(), c_out.xyz.x, c_out.xyz.y});
1334 prev_pt.x += pt.x;
1335 prev_pt.y += pt.y;
1336
1337 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion in Map frame: Point X "<< gf_pts.back().x() <<" After conversion: Point Y "<< gf_pts.back().y());
1338 }
1339
1340 // save the points converted to local map frame
1341 return gf_pts;
1342}

References base_map_georef_, process_traj_logs::c, and current_map_.

Referenced by geofenceFromMsg().

Here is the caller graph for this function:

◆ getRoute()

carma_planning_msgs::msg::Route carma_wm_ctrl::WMBroadcaster::getRoute ( )

Returns the most recently recieved route message.

Returns
The most recent route message.

Definition at line 1627 of file WMBroadcaster.cpp.

1628{
1629 return current_route;
1630}
carma_planning_msgs::msg::Route current_route

References current_route.

◆ getVehicleParticipationType()

std::string carma_wm_ctrl::WMBroadcaster::getVehicleParticipationType ( )

Get the Vehicle Participation Type object.

Returns
Current Vehicle Participation Type being used in this World Model Instance

Definition at line 1247 of file WMBroadcaster.cpp.

1248{
1249 return participant_;
1250}

References participant_.

◆ invertParticipants()

std::vector< std::string > carma_wm_ctrl::WMBroadcaster::invertParticipants ( const std::vector< std::string > &  input_participants) const

Generates inverse participants list of the given participants.

Parameters
std::vector<std::string>participants vector of strings

Definition at line 993 of file WMBroadcaster.cpp.

994{
995 std::vector<std::string> participants;
996
997 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Pedestrian ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Pedestrian);
998 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Bicycle ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Bicycle);
999 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Vehicle ) == input_participants.end())
1000 {
1001 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleMotorcycle);
1002 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleBus);
1003 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleCar);
1004 if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleTruck);
1005 }
1006 return participants;
1007}

Referenced by addRegionAccessRule().

Here is the caller graph for this function:

◆ participantsChecker()

std::vector< std::string > carma_wm_ctrl::WMBroadcaster::participantsChecker ( const carma_v2x_msgs::msg::TrafficControlMessageV01 &  msg_v01) const

Generates participants list.

Parameters
msg_v01message type

Definition at line 951 of file WMBroadcaster.cpp.

952{
953 std::vector<std::string> participants;
954 for (j2735_v2x_msgs::msg::TrafficControlVehClass participant : msg_v01.params.vclasses)
955 {
956 // Currently j2735_v2x_msgs::msg::TrafficControlVehClass::RAIL is not supported
957 if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::ANY)
958 {
959 participants = {lanelet::Participants::Vehicle, lanelet::Participants::Pedestrian, lanelet::Participants::Bicycle};
960 break;
961 }
962 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN)
963 {
964 participants.push_back(lanelet::Participants::Pedestrian);
965 }
966 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BICYCLE)
967 {
968 participants.push_back(lanelet::Participants::Bicycle);
969 }
970 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE ||
971 participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MOTORCYCLE)
972 {
973 participants.push_back(lanelet::Participants::VehicleMotorcycle);
974 }
975 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BUS)
976 {
977 participants.push_back(lanelet::Participants::VehicleBus);
978 }
979 else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::LIGHT_TRUCK_VAN ||
980 participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR)
981 {
982 participants.push_back(lanelet::Participants::VehicleCar);
983 }
984 else if (8<= participant.vehicle_class && participant.vehicle_class <= 16) // Truck enum definition range from 8-16 currently
985 {
986 participants.push_back(lanelet::Participants::VehicleTruck);
987 }
988 }
989 // combine to single vehicle type if possible, otherwise pass through
990 return combineParticipantsToVehicle(participants);
991}
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...

References combineParticipantsToVehicle().

Referenced by addPassingControlLineFromMsg(), addRegionAccessRule(), addRegionMinimumGap(), and geofenceFromMsg().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ preprocessWorkzoneGeometry()

void carma_wm_ctrl::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 
)

Preprocess for workzone area. Parallel_llts will have front_parallel and back_parallel lanelets that were created from splitting (if necessary) TAPERRIGHT and OPENRIGHT lanelets. Opposite_llts will have new lanelets split from REVERSE part with same direction as parallel lanelets that connect to diagonal ones.

Parameters
work_zone_geofence_cacheGeofence map with size of 4 corresponding to CLOSED, TAPERRIGHT, OPENRIGHT, REVERSE TrafficControlMessages. Each should have gf_pts, affected_parts.
parallel_lltslist holding front_parallel and back_parallel. These are created from splitting old ones in place such that they connect to diagonal ones
opposite_lltslist holding split and filtered lanelets from REVERSE part that connect to diagonal ones
Exceptions
InvalidObjectStateErrorif no map is available NOTE: REVERSE part's geofence points should have same direction as opposite lane's as that is what getAffectedLaneletOrArea requires.

PARALLEL FRONT (TAPERRIGHT side)

PARALLEL BACK (OPENRIGHT side)

HANDLE MID HERE

OPPOSITE FRONT (OPENRIGHT side)

Fill in the middle part of middle lanelets

OPPOSITE BACK (TAPERRIGHT side)

Definition at line 600 of file WMBroadcaster.cpp.

601{
602 if (!current_map_ || current_map_->laneletLayer.size() == 0)
603 {
604 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
605 }
609 std::vector <lanelet::Lanelet> new_taper_right_llts;
610 auto taper_right_first_pt = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->gf_pts.front().basicPoint2d();
611 auto taper_right_first_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id());
612
613 new_taper_right_llts = splitLaneletWithPoint({taper_right_first_pt}, taper_right_first_llt, error_distance_);
614 double check_dist_tpr = lanelet::geometry::distance2d(taper_right_first_llt.centerline2d().front().basicPoint2d(), taper_right_first_pt);
615
616 // if no splitting happened and taperright's first point is close to starting boundary, we need to create duplicate of previous lanelet of TAPERRIGHT's first lanelet
617 // to match the output expected of this function as if split happened
618 if (new_taper_right_llts.size() == 1 && check_dist_tpr <= error_distance_)
619 {
620 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating duplicate lanelet of 'previous lanelet' due to TAPERRIGHT using entire lanelet...");
621 auto previous_lanelets = current_routing_graph_->previous(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get());
622 if (previous_lanelets.empty()) //error if bad match
623 {
624 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone area starts from lanelet with no previous lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id()
625 << ". This case is rare and not supported at the moment.");
626 return;
627 }
628
629 // get previous lanelet of affected part of TAPERRIGHT (doesn't matter which previous, as the new lanelet will only be duplicate anyways)
630 auto prev_lanelet_to_copy = current_map_->laneletLayer.get(previous_lanelets.front().id());
631
632 // parallel_llts will have a copy of `prev_lanelet_to_copy` with new id to be used as part of workzone area
633 new_taper_right_llts = splitLaneletWithPoint({prev_lanelet_to_copy.centerline2d().back()}, prev_lanelet_to_copy, error_distance_);
634 }
635 parallel_llts->insert(parallel_llts->end(), new_taper_right_llts.begin(), new_taper_right_llts.end());
636 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished TAPERRIGHT processing of size: " << new_taper_right_llts.size());
637
641 std::vector <lanelet::Lanelet> new_open_right_llts;
642 auto open_right_last_pt = work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d();
643 auto open_right_last_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id());
644
645 new_open_right_llts = splitLaneletWithPoint({open_right_last_pt}, open_right_last_llt, error_distance_);
646 double check_dist_opr = lanelet::geometry::distance2d(open_right_last_llt.centerline2d().back().basicPoint2d(), open_right_last_pt);
647
648 // if no splitting happened, we need to create duplicate of next lanelet of OPENRIGHT's last lanelet
649 // to match the output expected of this function as if split happened
650 if (new_open_right_llts.size() == 1 && check_dist_opr <= error_distance_)
651 {
652 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating duplicate lanelet of 'next lanelet' due to OPENRIGHT using entire lanelet...");
653 auto next_lanelets = current_routing_graph_->following(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get());
654 if (next_lanelets.empty()) //error if bad match
655 {
656 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone area ends at lanelet with no following lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id()
657 << ". This case is rare and not supported at the moment.");
658 return;
659 }
660
661 // get next lanelet of affected part of OPENRIGHT (doesn't matter which next, as the new lanelet will only be duplicate anyways)
662 auto next_lanelet_to_copy = current_map_->laneletLayer.get(next_lanelets.front().id());
663
664 // parallel_llts will have a copy of `next_lanelet_to_copy` with new id to be used as part of workzone area
665 new_open_right_llts = splitLaneletWithPoint({next_lanelet_to_copy.centerline2d().back()}, next_lanelet_to_copy, error_distance_);
666 }
667 parallel_llts->insert(parallel_llts->end(), new_open_right_llts.begin(), new_open_right_llts.end());
668 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished OPENRIGHT processing of size: " << new_open_right_llts.size());
669
673
674 auto reverse_back_llts = carma_wm::query::getLaneletsFromPoint(current_map_, work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d());
675 auto reverse_back_llt = reverse_back_llts[0];
676 auto reverse_front_llts = carma_wm::query::getLaneletsFromPoint(current_map_, work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d());
677 auto reverse_front_llt = reverse_front_llts[0];
678
679 if (reverse_back_llt.id() == reverse_front_llt.id()) //means there is only 1 middle lanelet, which needs to be split into 3 lanelets
680 {
681 std::vector<lanelet::Lanelet> temp_llts;
682 temp_llts = splitLaneletWithPoint({work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(),
683 work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d()},
684 current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id()), error_distance_);
685
686 if (temp_llts.size() < 2) // if there is only 1 lanelet
687 {
688 // we found what we want, so return
689 opposite_llts->insert(opposite_llts->end(), temp_llts.begin(), temp_llts.end());
690 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size());
691 return;
692 }
693 else if (temp_llts.size() == 2) // determine which
694 {
695 // back gap is bigger than front's, we should lose front lanelet from the two
696 if (lanelet::geometry::distance2d(work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d(), reverse_front_llt.centerline2d().back().basicPoint2d()) >
697 lanelet::geometry::distance2d(work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(), reverse_front_llt.centerline2d().front().basicPoint2d()))
698 {
699 opposite_llts->push_back(temp_llts.back());
700 }
701 else
702 {
703 opposite_llts->push_back(temp_llts.front());
704 }
705 }
706 else if (temp_llts.size() == 3) // leave only middle lanelets from 3
707 {
708 opposite_llts->insert(opposite_llts->end(), temp_llts.begin() + 1, temp_llts.end()- 1);
709 }
710 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished REVERSE processing of size: " << opposite_llts->size() << " from original of 1 REVERSE lanelet size");
711 }
712 else //if there are two or more lanelets
713 {
715 auto reverse_first_pt = work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d();
716 auto reverse_first_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id());
717 std::vector<lanelet::Lanelet> temp_opposite_front_llts;
718 temp_opposite_front_llts = splitLaneletWithPoint( {reverse_first_pt}, reverse_first_llt, error_distance_);
719 if (temp_opposite_front_llts.size() > 1)
720 {
721 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin() + 1, temp_opposite_front_llts.end());
722 }
723 else
724 {
725 opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin(), temp_opposite_front_llts.end());
726 }
728 if (work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.size() > 2)
729 {
730 for (int i = 1; i < work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.size() - 1; i ++)
731 {
732 opposite_llts->push_back(current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_[i].id()));
733 }
734 }
736
737 auto reverse_last_pt = work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d();
738 auto reverse_last_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.back().lanelet().get().id());
739 std::vector<lanelet::Lanelet> temp_opposite_back_llts;
740 temp_opposite_back_llts = splitLaneletWithPoint({reverse_last_pt}, reverse_last_llt, error_distance_);
741 if (temp_opposite_back_llts.size() > 1)
742 {
743 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end()- 1);
744 }
745 else
746 {
747 opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end());
748 }
749 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished REVERSE processing of size: " << opposite_llts->size() << " from original of more than one REVERSE lanelet size");
750 }
751
752 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size());
753}
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::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...

References current_map_, current_routing_graph_, error_distance_, carma_wm::query::getLaneletsFromPoint(), process_bag::i, carma_wm_ctrl::WorkZoneSection::OPENRIGHT, carma_wm_ctrl::WorkZoneSection::REVERSE, splitLaneletWithPoint(), and carma_wm_ctrl::WorkZoneSection::TAPERRIGHT.

Referenced by createWorkzoneGeofence().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ publishLightId()

void carma_wm_ctrl::WMBroadcaster::publishLightId ( )

helps to populate upcoming_intersection_ids_ from local traffic lanelet ids

Definition at line 1971 of file WMBroadcaster.cpp.

1972{
1973 if (traffic_light_id_lookup_.empty())
1974 {
1975 return;
1976 }
1977 for(auto id : current_route.route_path_lanelet_ids)
1978 {
1979 bool convert_success = false;
1980 unsigned intersection_id = 0;
1981 unsigned group_id = 0;
1982 auto route_lanelet= current_map_->laneletLayer.get(id);
1983 auto traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
1984
1985 if (!traffic_lights.empty())
1986 {
1987 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id());
1988 convert_success = convertLightIdToInterGroupId(intersection_id,group_id, traffic_lights.front()->id());
1989 }
1990
1991 if (!convert_success)
1992 continue;
1993
1994 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light with Intersection id: " << intersection_id << " Group id:" << group_id);
1995 bool id_exists = false;
1996 for (int idx = 0; idx < upcoming_intersection_ids_.data.size(); idx +2)
1997 {
1998 if (upcoming_intersection_ids_.data[idx] == intersection_id && upcoming_intersection_ids_.data[idx + 1] == group_id) //check if already there
1999 {
2000 id_exists = true;
2001 break;
2002 }
2003 }
2004
2005 if (id_exists)
2006 continue;
2007
2008 upcoming_intersection_ids_.data.push_back(static_cast<int>(intersection_id));
2009 upcoming_intersection_ids_.data.push_back(static_cast<int>(group_id));
2010 }
2011}
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
std_msgs::msg::Int32MultiArray upcoming_intersection_ids_

References convertLightIdToInterGroupId(), current_map_, current_route, traffic_light_id_lookup_, and upcoming_intersection_ids_.

Here is the call graph for this function:

◆ pubTCMACK()

void carma_wm_ctrl::WMBroadcaster::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 number of times.

Definition at line 2262 of file WMBroadcaster.cpp.

2263{
2264 carma_v2x_msgs::msg::MobilityOperation mom_msg;
2265 mom_msg.m_header.timestamp = scheduler_.now().nanoseconds()/1000000;
2266 mom_msg.m_header.sender_id = vehicle_id_;
2267 mom_msg.strategy = geofence_ack_strategy_;
2268 std::stringstream ss;
2269 for(size_t i=0; i < tcm_req_id.id.size(); i++)
2270 {
2271 ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned) tcm_req_id.id.at(i);
2272 }
2273 std::string tcmv01_req_id_hex = ss.str();
2274 ss.str("");
2275 ss << "traffic_control_id:" << tcmv01_req_id_hex << ", msgnum:"<< msgnum << ", acknowledgement:" << ack_status << ", reason:" << ack_reason;
2276 mom_msg.strategy_params = ss.str();
2277 for(int i = 0; i < ack_pub_times_; i++)
2278 {
2279 tcm_ack_pub_(mom_msg);
2280 }
2281}
rclcpp::Time now()
Get current time used by scheduler.
const std::string geofence_ack_strategy_

References ack_pub_times_, geofence_ack_strategy_, process_bag::i, carma_wm_ctrl::GeofenceScheduler::now(), scheduler_, tcm_ack_pub_, and vehicle_id_.

Referenced by geofenceCallback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ removeGeofence()

void carma_wm_ctrl::WMBroadcaster::removeGeofence ( std::shared_ptr< Geofence gf_ptr)

Removes a geofence from the current map and publishes the ROS msg.

Definition at line 1578 of file WMBroadcaster.cpp.

1579{
1580 std::lock_guard<std::mutex> guard(map_mutex_);
1581 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Removing inactive geofence from the map with geofence id: " << gf_ptr->id_);
1582
1583 // Process the geofence object to populate update remove lists
1584 if (gf_ptr->affected_parts_.empty())
1585 return;
1586
1587 removeGeofenceHelper(gf_ptr);
1588
1589 for (auto pair : gf_ptr->remove_list_) active_geofence_llt_ids_.erase(pair.first);
1590
1591 // publish
1592 autoware_lanelet2_msgs::msg::MapBin gf_msg_revert;
1593 auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, {}));
1594
1595 if (gf_ptr->invalidate_route_) { // If a geofence initially invalidated the route it stands to reason its removal should as well
1596
1597 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Rebuilding routing graph after is was invalidated by geofence removal");
1598
1599 lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
1600 lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_
1601 );
1602 current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
1603
1604 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence removal");
1605
1606 // Populate routing graph structure
1607 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message for geofence removal");
1608
1609 auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
1610
1611 gf_msg_revert.routing_graph = readable_graph->routingGraphToMsg(participant_);
1612 gf_msg_revert.has_routing_graph = true;
1613
1614 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message for geofence removal");
1615 }
1616
1617 carma_wm::toBinMsg(send_data, &gf_msg_revert);
1618 update_count_++; // Update the sequence count for geofence messages
1619 gf_msg_revert.seq_id = update_count_;
1620 gf_msg_revert.map_version = current_map_version_;
1621
1622 map_update_pub_(gf_msg_revert);
1623
1624
1625}
void removeGeofenceHelper(std::shared_ptr< Geofence > gf_ptr) const

References active_geofence_llt_ids_, current_map_, current_map_version_, current_routing_graph_, map_mutex_, map_update_pub_, participant_, removeGeofenceHelper(), carma_wm::toBinMsg(), and update_count_.

Referenced by WMBroadcaster().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ removeGeofenceHelper()

void carma_wm_ctrl::WMBroadcaster::removeGeofenceHelper ( std::shared_ptr< Geofence gf_ptr) const
private

Definition at line 1935 of file WMBroadcaster.cpp.

1936{
1937 // Logic to determine what type of geofence
1938 // reset the info inside geofence
1939 gf_ptr->remove_list_ = {};
1940 gf_ptr->update_list_ = {};
1942 // as all changes are reverted back, we no longer need prev_regems
1943 gf_ptr->prev_regems_ = {};
1944}
void addBackRegulatoryComponent(std::shared_ptr< Geofence > gf_ptr) const

References addBackRegulatoryComponent().

Referenced by removeGeofence().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ routeCallbackMessage()

void carma_wm_ctrl::WMBroadcaster::routeCallbackMessage ( carma_planning_msgs::msg::Route::UniquePtr  route_msg)

Calls controlRequestFromRoute() and publishes the TrafficControlRequest Message returned after the completed operations.

Parameters
route_msgThe message containing route information

Definition at line 1632 of file WMBroadcaster.cpp.

1633{
1634 current_route = *route_msg;
1635 carma_v2x_msgs::msg::TrafficControlRequest cR;
1636 cR = controlRequestFromRoute(*route_msg);
1637 control_msg_pub_(cR);
1638
1639}
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...

References control_msg_pub_, controlRequestFromRoute(), and current_route.

Referenced by WMBroadcaster(), and carma_wm_ctrl::WMBroadcasterNode::handle_on_activate().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ scheduleGeofence()

void carma_wm_ctrl::WMBroadcaster::scheduleGeofence ( std::shared_ptr< carma_wm_ctrl::Geofence gf_ptr_list)
private

Definition at line 1137 of file WMBroadcaster.cpp.

1138{
1139 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
1140
1141 bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos;
1142
1143 carma_v2x_msgs::msg::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail;
1144
1145 // create workzone specific extra speed geofence
1146 if (detected_workzone_signal && msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
1147 {
1148 // duplicate the messages with inverted points to support new lanelets created from workzone
1149 // as carma-cloud currently does not support geofence points with direction opposite to that of the road
1150
1151 auto gf_ptr_speed = std::make_shared<Geofence>();
1152 gf_ptr_speed->schedules = gf_ptr->schedules;
1153
1154 carma_v2x_msgs::msg::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_;
1155
1156 std::reverse(duplicate_msg.geometry.nodes.begin() + 1, duplicate_msg.geometry.nodes.end());
1157 double first_x = 0;
1158 double first_y = 0;
1159
1160 // this fancy logic is needed as each node is expressed as an offset from the last one
1161 for (auto& pt: duplicate_msg.geometry.nodes)
1162 {
1163 first_x+= pt.x;
1164 first_y+= pt.y;
1165 pt.x = -1* pt.x;
1166 pt.y = -1* pt.y;
1167 }
1168 duplicate_msg.geometry.nodes[0].x = first_x;
1169 duplicate_msg.geometry.nodes[0].y = first_y;
1170
1171 gf_ptr_speed->msg_ = duplicate_msg;
1172 scheduler_.addGeofence(gf_ptr_speed);
1173 }
1174 if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later
1175 {
1176 gf_ptr->label_ = gf_ptr->msg_.package.label; // to extract intersection, and signal group id
1177 if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
1178 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
1179 msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
1180 {
1181 work_zone_geofence_cache_[msg_detail.closed] = gf_ptr;
1182 }
1183 else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
1184 {
1186 }
1188 {
1189 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now...");
1190 return;
1191 }
1192 }
1193
1194 scheduler_.addGeofence(gf_ptr); // Add the geofence to the scheduler
1195
1196}
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...
const int WORKZONE_TCM_REQUIRED_SIZE

References carma_wm_ctrl::GeofenceScheduler::addGeofence(), carma_wm_ctrl::WorkZoneSection::REVERSE, scheduler_, work_zone_geofence_cache_, and carma_wm_ctrl::WORKZONE_TCM_REQUIRED_SIZE.

Referenced by externalMapMsgCallback(), and geofenceCallback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setConfigACKPubTimes()

void carma_wm_ctrl::WMBroadcaster::setConfigACKPubTimes ( int  ack_pub_times)

Sets the TCM Acknowledgement publish times.

Parameters
ack_pub_timesthe number of times it publishes TCM Acknowledgement

Definition at line 1238 of file WMBroadcaster.cpp.

1238 {
1239 ack_pub_times_ = ack_pub_times;
1240}

References ack_pub_times_.

◆ setConfigSpeedLimit()

void carma_wm_ctrl::WMBroadcaster::setConfigSpeedLimit ( double  cL)

Sets the configured speed limit.

Definition at line 1228 of file WMBroadcaster.cpp.

1229{
1230 /*Logic to change config_lim to Velocity value config_limit*/
1231 config_limit = lanelet::Velocity(cL * lanelet::units::MPH());
1232}

References config_limit.

◆ setConfigVehicleId()

void carma_wm_ctrl::WMBroadcaster::setConfigVehicleId ( const std::string &  vehicle_id)

Retrieve the vehicle ID from global vehicle parameters, and set instance memeber vehicle id.

Parameters
vehicle_idVehicle ID from UniqueVehicleParams.yaml

Definition at line 1234 of file WMBroadcaster.cpp.

1234 {
1235 vehicle_id_ = vehicle_id;
1236}

References vehicle_id_.

◆ setErrorDistance()

void carma_wm_ctrl::WMBroadcaster::setErrorDistance ( double  error_distance)

Definition at line 595 of file WMBroadcaster.cpp.

596{
597 error_distance_ = error_distance;
598}

References error_distance_.

◆ setIntersectionCoordCorrection()

void carma_wm_ctrl::WMBroadcaster::setIntersectionCoordCorrection ( const std::vector< int64_t > &  intersection_ids_for_correction,
const std::vector< double > &  intersection_correction 
)

Sets the coordinate correction for intersection.

Parameters
listof intersection_ids corresponding to every two elements in correction list
listof intersection coord correction parameters in double in every 2 elements: delta_x, delta_y

Definition at line 1213 of file WMBroadcaster.cpp.

1214{
1215 if (intersection_correction.size() % 2 != 0 || intersection_ids_for_correction.size() != intersection_correction.size() / 2)
1216 {
1217 throw std::invalid_argument("Some of intersection coordinate correction parameters are not fully set!");
1218 }
1219
1220 for (auto i = 0; i < intersection_correction.size(); i = i + 2)
1221 {
1222 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].first = intersection_correction[i]; //x
1223 sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].second = intersection_correction[i + 1]; //y
1224 }
1225
1226}

References process_bag::i, and sim_.

◆ setMaxLaneWidth()

void carma_wm_ctrl::WMBroadcaster::setMaxLaneWidth ( double  max_lane_width)

Sets the max lane width in meters. Geofence points are associated to a lanelet if they are within this distance to a lanelet as geofence points are guaranteed to apply to a single lane.

Definition at line 1205 of file WMBroadcaster.cpp.

1206{
1207 sim_ = std::make_shared<carma_wm::SignalizedIntersectionManager>();
1208
1209 max_lane_width_ = max_lane_width;
1210 sim_->setMaxLaneWidth(max_lane_width_);
1211}

References max_lane_width_, and sim_.

◆ setVehicleParticipationType()

void carma_wm_ctrl::WMBroadcaster::setVehicleParticipationType ( std::string  participant)

Set the Vehicle Participation Type.

Parameters
participantvehicle participation type

Definition at line 1242 of file WMBroadcaster.cpp.

1243{
1244 participant_ = participant;
1245}

References participant_.

◆ shouldChangeControlLine()

bool carma_wm_ctrl::WMBroadcaster::shouldChangeControlLine ( const lanelet::ConstLaneletOrArea &  el,
const lanelet::RegulatoryElementConstPtr &  regem,
std::shared_ptr< Geofence gf_ptr 
) const
private

This is a helper function that returns true if the provided regem is marked to be changed by the geofence as there are usually multiple passing control lines are in the lanelet.

Parameters
geofence_msgThe ROS msg that contains geofence information
elThe LaneletOrArea that houses the regem
regemThe regulatoryElement that needs to be checked NOTE: Currently this function only works on lanelets. It returns true if the regem is not passingControlLine or if the pcl should be changed.

Definition at line 1357 of file WMBroadcaster.cpp.

1358{
1359 // should change if if the regem is not a passing control line or area, which is not supported by this logic
1360 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) != 0 || !el.isLanelet())
1361 {
1362 return true;
1363 }
1364
1365 lanelet::PassingControlLinePtr pcl = std::dynamic_pointer_cast<lanelet::PassingControlLine>(current_map_->regulatoryElementLayer.get(regem->id()));
1366 // if this geofence's pcl doesn't match with the lanelets current bound side, return false as we shouldn't change
1367 bool should_change_pcl = false;
1368 for (auto control_line : pcl->controlLine())
1369 {
1370 if ((control_line.id() == el.lanelet()->leftBound2d().id() && gf_ptr->pcl_affects_left_) ||
1371 (control_line.id() == el.lanelet()->rightBound2d().id() && gf_ptr->pcl_affects_right_))
1372 {
1373 should_change_pcl = true;
1374 break;
1375 }
1376 }
1377 return should_change_pcl;
1378}

References current_map_.

Referenced by addBackRegulatoryComponent(), and addRegulatoryComponent().

Here is the caller graph for this function:

◆ shouldChangeTrafficSignal()

bool carma_wm_ctrl::WMBroadcaster::shouldChangeTrafficSignal ( const lanelet::ConstLaneletOrArea &  el,
const lanelet::RegulatoryElementConstPtr &  regem,
std::shared_ptr< carma_wm::SignalizedIntersectionManager sim 
) const
private

This is a helper function that returns true if signal in the lanelet should be changed according to the records of signalizer intersection manager Used in managing multiple signal_groups in a single entry lanelet for example.

Parameters
elThe LaneletOrArea that houses the regem
regemThe regulatoryElement that needs to be checked
simThe signalized intersection manager that has records regems and corresponding lanelets NOTE: Currently this function only works on lanelets. It returns true if the regem is not CarmaTrafficSignal or if the signal should be changed.

Definition at line 1388 of file WMBroadcaster.cpp.

1389{
1390 // should change if the regem is not a CarmaTrafficSignal, which is not supported by this logic
1391 if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::CarmaTrafficSignal::RuleName) != 0 || !el.isLanelet() || !sim_)
1392 {
1393 return true;
1394 }
1395
1396 lanelet::CarmaTrafficSignalPtr traffic_signal = std::dynamic_pointer_cast<lanelet::CarmaTrafficSignal>(current_map_->regulatoryElementLayer.get(regem->id()));
1397 uint8_t signal_id = 0;
1398
1399 for (auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it)
1400 {
1401 if (regem->id() == it->second)
1402 {
1403 signal_id = it->first;
1404 }
1405 }
1406
1407 if (signal_id == 0) // doesn't exist in the record
1408 return true;
1409
1410 if (sim->signal_group_to_entry_lanelet_ids_[signal_id].find(el.id()) != sim->signal_group_to_entry_lanelet_ids_[signal_id].end())
1411 return false; // signal group's entry lane is still part of the intersection, so don't change
1412
1413 return true;
1414}

References current_map_, and sim_.

Referenced by addRegulatoryComponent().

Here is the caller graph for this function:

◆ splitLaneletWithPoint()

std::vector< lanelet::Lanelet > carma_wm_ctrl::WMBroadcaster::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. Newly created lanelet will have old regulatory elements copied into each of them. From the front and back boundaries, it is deemed not necessary to split if the ratios are within error_distance from either of it. For example, if front and back points of 3 points given are both within error_distance, only middle point will be used to split into 2 lanelets. It will return duplicate of old lanelet (with different id) if no splitting was done.

Parameters
input_ptsPoints whose downtrack ratio relative to given lanelet will be used to split the lanelet
input_lltA lanelet to split
error_distanceif within this distance (in meters) the point will be ignored
Returns
lanelets split form original one. lanelets sorted from front to back.
Exceptions
InvalidObjectStateErrorif no map is available. NOTE: this is requried to return mutable objects.

Definition at line 755 of file WMBroadcaster.cpp.

756{
757 // get ratio of this point and split
758 std::vector<lanelet::Lanelet> parallel_llts;
759 double llt_downtrack = carma_wm::geometry::trackPos(input_llt, input_llt.centerline().back().basicPoint2d()).downtrack;
760 std::vector<double> ratios;
761
762 for (auto pt : input_pts)
763 {
764 double point_downtrack = carma_wm::geometry::trackPos(input_llt, pt).downtrack;
765 double point_downtrack_ratio = point_downtrack / llt_downtrack;
766 ratios.push_back(point_downtrack_ratio);
767 }
768
769 auto new_parallel_llts = splitLaneletWithRatio(ratios, input_llt, error_distance);
770
771 parallel_llts.insert(parallel_llts.end(),new_parallel_llts.begin(), new_parallel_llts.end());
772 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithPoint returning lanelets size: " << parallel_llts.size());
773 return parallel_llts;
774}
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...

References carma_wm::TrackPos::downtrack, splitLaneletWithRatio(), and carma_wm::geometry::trackPos().

Referenced by preprocessWorkzoneGeometry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ splitLaneletWithRatio()

std::vector< lanelet::Lanelet > carma_wm_ctrl::WMBroadcaster::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 have old regulatory elements copied into each of them. From the front and back boundaries, it is deemed not necessary to split if the ratios are within error_distance from either of it. For example, if front and back points of 3 ratios given are both within error_distance, only middle ratio will be used to split, so 2 lanelets will be returned. It will return duplicate of old lanelet (with different id) if no splitting was done.

Parameters
ratiosRatios relative to given lanelet will be used to split the lanelet
input_lltA lanelet to split
error_distanceif within this distance (in meters) the ratio will be ignored as it is too small
Returns
parallel_llts return lanelets split form original one. lanelets sorted from front to back.
Exceptions
InvalidObjectStateErrorif no map is available. NOTE: this is requried to return mutable objects.

Definition at line 797 of file WMBroadcaster.cpp.

798{
799 if (!current_map_ || current_map_->laneletLayer.size() == 0)
800 {
801 throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
802 }
803 if (ratios.empty())
804 {
805 throw lanelet::InvalidInputError(std::string("Ratios list is empty! Cannot split"));
806 }
807
808 std::vector<lanelet::Lanelet> created_llts;
809
810 std::sort(ratios.begin(), ratios.end());
811 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio evaluating input ratios of size: " << ratios.size());
812
813 ratios.push_back(1.0); //needed to complete the loop
814
815 int left_ls_size = input_lanelet.leftBound2d().size();
816 int right_ls_size = input_lanelet.rightBound2d().size();
817
818 int left_next_pt_idx = 0;
819 int left_prev_pt_idx = 0;
820 int right_next_pt_idx = 0;
821 int right_prev_pt_idx = 0;
822 for (int i = 0 ; i < ratios.size(); i ++)
823 {
824 left_next_pt_idx = std::round(ratios[i] * (left_ls_size - 1));
825 right_next_pt_idx = std::round(ratios[i] * (right_ls_size - 1));
826 // check if edge ratios are too close to any boundaries. if so, skip
827 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
828 {
829 // assuming both linestrings have roughly the same number of points and
830 // assuming distance between 0th and index-th points are small enough we can approximate the curve between them as a line:
831 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ratio: " << ratios[i] << ", is too close to the lanelet's front boundary! Therefore, ignoring... Allowed error_distance: " << error_distance << ", Distance: "
832 << lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
833 continue;
834 }
835 if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
836 {
837 RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ratio: " << ratios[i] << ", is too close to the lanelet's back boundary! Therefore, ignoring... Allowed error_distance: " << error_distance << ", Distance: "
838 << lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
839
840 left_next_pt_idx = left_ls_size - 1;
841 right_next_pt_idx = right_ls_size - 1;
842 }
843 // create lanelet
844 std::vector<lanelet::Point3d> left_pts;
845 left_pts.insert(left_pts.end(), current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_prev_pt_idx, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().begin() + left_next_pt_idx + 1);
846
847 lanelet::LineString3d left_ls(lanelet::utils::getId(), left_pts, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().attributes());
848
849 std::vector<lanelet::Point3d> right_pts;
850 right_pts.insert(right_pts.end(), current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_prev_pt_idx, current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().begin() + right_next_pt_idx + 1);
851
852 lanelet::LineString3d right_ls(lanelet::utils::getId(), right_pts);
853
854 lanelet::Lanelet new_llt (lanelet::utils::getId(), left_ls, right_ls, current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().attributes());
855
856 for (auto regem : current_map_->laneletLayer.get(input_lanelet.id()).regulatoryElements()) //copy existing regem into new llts
857 {
858 new_llt.addRegulatoryElement(current_map_->regulatoryElementLayer.get(regem->id()));
859 }
860
861 left_prev_pt_idx = left_next_pt_idx;
862 right_prev_pt_idx = right_next_pt_idx;
863
864 created_llts.push_back(new_llt);
865
866 // Detected the end already. Exiting now
867 if (left_prev_pt_idx == left_ls_size - 1 || right_prev_pt_idx == right_ls_size - 1)
868 {
869 break;
870 }
871
872 }
873
874 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio returning lanelets size: " << created_llts.size());
875
876 return created_llts;
877}

References current_map_, and process_bag::i.

Referenced by splitLaneletWithPoint(), and splitOppositeLaneletWithPoint().

Here is the caller graph for this function:

◆ splitOppositeLaneletWithPoint()

lanelet::Lanelets carma_wm_ctrl::WMBroadcaster::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 relative to the lanelet. Newly created lanelet will have old regulatory elements copied into each of them. From the front and back boundaries, it is deemed not necessary to split if the ratios are within error_distance from either of it. It will return duplicate of old lanelet (with different id) if no splitting was done.

Parameters
opposite_lltsreturn lanelets split form original one. lanelets sorted from front to back.
input_ptA point whose downtrack ratio relative to given lanelet will be used to split the lanelet
input_lltA lanelet to split
error_distanceif within this distance (in meters) the point will be ignored
Returns
return original opposite lanelet(s if overlapping)
Exceptions
InvalidObjectStateErrorif no map is available. NOTE: this is requried to return mutable objects. NOTE: Opposite lanelet doesn't have to share points with current lanelet

Definition at line 776 of file WMBroadcaster.cpp.

777{
778 // get ratio of this point and split
779 auto point_downtrack = carma_wm::geometry::trackPos(input_llt, input_pt).downtrack;
780 auto point_downtrack_ratio = point_downtrack / carma_wm::geometry::trackPos(input_llt, input_llt.centerline().back().basicPoint2d()).downtrack;
781
782 // get opposing lanelets and split
783 auto opposing_llts = carma_wm::query::nonConnectedAdjacentLeft(current_map_, input_pt);
784
785 if (opposing_llts.empty())
786 {
787 RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster was not able to find opposing lane for given point in geofence related to Work Zone! Returning");
788 return {};
789 }
790
791 auto new_llts_opposite = splitLaneletWithRatio({1 - point_downtrack_ratio}, opposing_llts[0], error_distance);
792 opposite_llts->insert(opposite_llts->begin(),new_llts_opposite.begin(), new_llts_opposite.end());
793 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitOppositeLaneletWithPoint returning lanelets size: " << opposite_llts->size());
794 return opposing_llts;
795}
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...

References current_map_, carma_wm::TrackPos::downtrack, carma_wm::query::nonConnectedAdjacentLeft(), splitLaneletWithRatio(), and carma_wm::geometry::trackPos().

Referenced by createWorkzoneGeometry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ updateUpcomingSGIntersectionIds()

void carma_wm_ctrl::WMBroadcaster::updateUpcomingSGIntersectionIds ( )

populate upcoming_intersection_ids_ from local traffic lanelet ids

Definition at line 2197 of file WMBroadcaster.cpp.

2198{
2199 uint16_t map_msg_intersection_id = 0;
2200 uint16_t cur_signal_group_id = 0;
2201 std::vector<lanelet::CarmaTrafficSignalPtr> traffic_lights;
2202 lanelet::Lanelet route_lanelet;
2203 lanelet::Ids cur_route_lanelet_ids = current_route.route_path_lanelet_ids;
2204 bool isLightFound = false;
2205
2206 for(auto id : cur_route_lanelet_ids)
2207 {
2208 route_lanelet= current_map_->laneletLayer.get(id);
2209 traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
2210 if(!traffic_lights.empty())
2211 {
2212 isLightFound = true;
2213 break;
2214 }
2215 }
2216
2217 if(isLightFound && sim_)
2218 {
2219 for(auto itr = sim_->signal_group_to_traffic_light_id_.begin(); itr != sim_->signal_group_to_traffic_light_id_.end(); itr++)
2220 {
2221 if(itr->second == traffic_lights.front()->id())
2222 {
2223 cur_signal_group_id = itr->first;
2224 }
2225 }
2226 }
2227 else{
2228 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching Traffic lights along the route");
2229 }//END Traffic signals
2230
2231 auto intersections = route_lanelet.regulatoryElementsAs<lanelet::SignalizedIntersection>();
2232 if (intersections.empty())
2233 {
2234 // no match if any of the entry lanelet is not part of any intersection.
2235 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching intersection for current lanelet. lanelet id = " << route_lanelet.id());
2236 }
2237 else
2238 {
2239 //Currently, each lanelet has only one intersection
2240 lanelet::Id intersection_id = intersections.front()->id();
2241 if(intersection_id != lanelet::InvalId)
2242 {
2243 for(auto itr = sim_->intersection_id_to_regem_id_.begin(); itr != sim_->intersection_id_to_regem_id_.end(); itr++)
2244 {
2245 if(itr->second == intersection_id)
2246 {
2247 map_msg_intersection_id = itr->first;
2248 }
2249 }
2250 }
2251 } //END intersections
2252
2253 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "MAP msg: Intersection ID = " << map_msg_intersection_id << ", Signal Group ID =" << cur_signal_group_id );
2254 if(map_msg_intersection_id != 0 && cur_signal_group_id != 0)
2255 {
2256 upcoming_intersection_ids_.data.clear();
2257 upcoming_intersection_ids_.data.push_back(static_cast<int>(map_msg_intersection_id));
2258 upcoming_intersection_ids_.data.push_back(static_cast<int>(cur_signal_group_id));
2259 }
2260}

References current_map_, current_route, sim_, and upcoming_intersection_ids_.

Member Data Documentation

◆ ack_pub_times_

int carma_wm_ctrl::WMBroadcaster::ack_pub_times_ = 1
private

Definition at line 470 of file WMBroadcaster.hpp.

Referenced by pubTCMACK(), and setConfigACKPubTimes().

◆ active_geofence_llt_ids_

std::unordered_set<lanelet::Id> carma_wm_ctrl::WMBroadcaster::active_geofence_llt_ids_
private

◆ active_pub_

PublishActiveGeofCallback carma_wm_ctrl::WMBroadcaster::active_pub_
private

Definition at line 440 of file WMBroadcaster.hpp.

Referenced by currentLocationCallback().

◆ base_map_

lanelet::LaneletMapPtr carma_wm_ctrl::WMBroadcaster::base_map_
private

Definition at line 428 of file WMBroadcaster.hpp.

Referenced by baseMapCallback().

◆ base_map_georef_

std::string carma_wm_ctrl::WMBroadcaster::base_map_georef_
private

◆ cached_maps_

std::vector<lanelet::LaneletMapPtr> carma_wm_ctrl::WMBroadcaster::cached_maps_
private

Definition at line 435 of file WMBroadcaster.hpp.

◆ checked_geofence_ids_

std::unordered_set<std::string> carma_wm_ctrl::WMBroadcaster::checked_geofence_ids_
private

Definition at line 433 of file WMBroadcaster.hpp.

Referenced by geofenceCallback().

◆ config_limit

lanelet::Velocity carma_wm_ctrl::WMBroadcaster::config_limit
private

Definition at line 431 of file WMBroadcaster.hpp.

Referenced by baseMapCallback(), geofenceFromMsg(), and setConfigSpeedLimit().

◆ control_msg_pub_

PublishCtrlRequestCallback carma_wm_ctrl::WMBroadcaster::control_msg_pub_
private

Definition at line 439 of file WMBroadcaster.hpp.

Referenced by routeCallbackMessage().

◆ current_map_

◆ current_map_version_

size_t carma_wm_ctrl::WMBroadcaster::current_map_version_ = 0
private

Definition at line 451 of file WMBroadcaster.hpp.

Referenced by addGeofence(), baseMapCallback(), and removeGeofence().

◆ current_route

carma_planning_msgs::msg::Route carma_wm_ctrl::WMBroadcaster::current_route
private

◆ current_routing_graph_

lanelet::routing::RoutingGraphPtr carma_wm_ctrl::WMBroadcaster::current_routing_graph_
private

◆ error_distance_

double carma_wm_ctrl::WMBroadcaster::error_distance_ = 5
private

◆ generated_geofence_reqids_

std::unordered_set<std::string> carma_wm_ctrl::WMBroadcaster::generated_geofence_reqids_
private

Definition at line 434 of file WMBroadcaster.hpp.

Referenced by controlRequestFromRoute(), and geofenceCallback().

◆ geofence_ack_strategy_

const std::string carma_wm_ctrl::WMBroadcaster::geofence_ack_strategy_ = "carma3/Geofence_Acknowledgement"
private

Definition at line 469 of file WMBroadcaster.hpp.

Referenced by pubTCMACK().

◆ map_mutex_

std::mutex carma_wm_ctrl::WMBroadcaster::map_mutex_
private

◆ map_pub_

PublishMapCallback carma_wm_ctrl::WMBroadcaster::map_pub_
private

Definition at line 437 of file WMBroadcaster.hpp.

Referenced by baseMapCallback().

◆ map_update_message_queue_

std::vector<autoware_lanelet2_msgs::msg::MapBin> carma_wm_ctrl::WMBroadcaster::map_update_message_queue_
private

Queue which stores the map updates applied to the current map version as a sequence of diffs This queue is implemented as a vector because it gets reused by each new subscriber connection NOTE: This queue should be cleared each time the current_map_version changes

Definition at line 459 of file WMBroadcaster.hpp.

◆ map_update_pub_

PublishMapUpdateCallback carma_wm_ctrl::WMBroadcaster::map_update_pub_
private

Definition at line 438 of file WMBroadcaster.hpp.

Referenced by addGeofence(), and removeGeofence().

◆ max_lane_width_

double carma_wm_ctrl::WMBroadcaster::max_lane_width_
private

Definition at line 444 of file WMBroadcaster.hpp.

Referenced by getAffectedLaneletOrAreas(), and setMaxLaneWidth().

◆ participant_

std::string carma_wm_ctrl::WMBroadcaster::participant_ = lanelet::Participants::VehicleCar
private

◆ route_path_

lanelet::ConstLanelets carma_wm_ctrl::WMBroadcaster::route_path_
private

Definition at line 410 of file WMBroadcaster.hpp.

Referenced by controlRequestFromRoute(), and distToNearestActiveGeofence().

◆ scheduler_

GeofenceScheduler carma_wm_ctrl::WMBroadcaster::scheduler_
private

◆ sim_

◆ tcm_ack_pub_

PublishMobilityOperationCallback carma_wm_ctrl::WMBroadcaster::tcm_ack_pub_
private

Definition at line 442 of file WMBroadcaster.hpp.

Referenced by pubTCMACK().

◆ tcm_marker_array_

visualization_msgs::msg::MarkerArray carma_wm_ctrl::WMBroadcaster::tcm_marker_array_

Definition at line 404 of file WMBroadcaster.hpp.

Referenced by addGeofence(), and composeTCMMarkerVisualizer().

◆ tcr_polygon_

carma_v2x_msgs::msg::TrafficControlRequestPolygon carma_wm_ctrl::WMBroadcaster::tcr_polygon_

Definition at line 405 of file WMBroadcaster.hpp.

Referenced by controlRequestFromRoute().

◆ traffic_light_id_lookup_

std::unordered_map<uint32_t, lanelet::Id> carma_wm_ctrl::WMBroadcaster::traffic_light_id_lookup_
private

◆ upcoming_intersection_ids_

std_msgs::msg::Int32MultiArray carma_wm_ctrl::WMBroadcaster::upcoming_intersection_ids_

Definition at line 406 of file WMBroadcaster.hpp.

Referenced by publishLightId(), and updateUpcomingSGIntersectionIds().

◆ update_count_

size_t carma_wm_ctrl::WMBroadcaster::update_count_ = -1
private

Definition at line 461 of file WMBroadcaster.hpp.

Referenced by addGeofence(), and removeGeofence().

◆ vehicle_id_

std::string carma_wm_ctrl::WMBroadcaster::vehicle_id_
private

Definition at line 471 of file WMBroadcaster.hpp.

Referenced by pubTCMACK(), and setConfigVehicleId().

◆ work_zone_geofence_cache_

std::unordered_map<uint8_t, std::shared_ptr<Geofence> > carma_wm_ctrl::WMBroadcaster::work_zone_geofence_cache_
private

Definition at line 412 of file WMBroadcaster.hpp.

Referenced by addGeofence(), geofenceFromMsg(), and scheduleGeofence().

◆ workzone_geometry_published_

bool carma_wm_ctrl::WMBroadcaster::workzone_geometry_published_ = false
private

Definition at line 446 of file WMBroadcaster.hpp.

◆ workzone_remaining_msgs_

std::vector<carma_v2x_msgs::msg::TrafficControlMessageV01> carma_wm_ctrl::WMBroadcaster::workzone_remaining_msgs_
private

Definition at line 445 of file WMBroadcaster.hpp.


The documentation for this class was generated from the following files: