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:

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	composeVisualizerMarkerFromPts (const visualization_msgs::msg::MarkerArray &marker_array, const std::vector< lanelet::Point3d > &input)
	Visualizes in Rviz geometry points related to MAP msg or TrafficControlMessage. 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< 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 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< 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 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_
visualization_msgs::msg::MarkerArray	j2735_map_msg_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::SignalizedIntersectionManager >	sim_
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

enum class carma_wm_ctrl::WMBroadcaster::AcknowledgementStatus

strongprivate

Enumerator
ACKNOWLEDGED
REJECTED

Definition at line 469 of file WMBroadcaster.hpp.

                                   {
    ACKNOWLEDGED = 1,
    REJECTED = 2
  };

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.

  : 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)
{
  scheduler_.onGeofenceActive(std::bind(&WMBroadcaster::addGeofence, this, _1));
  scheduler_.onGeofenceInactive(std::bind(&WMBroadcaster::removeGeofence, this, _1));
  std::bind(&WMBroadcaster::routeCallbackMessage, this, _1);
};

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 1464 of file WMBroadcaster.cpp.

{
  // First loop is to remove the relation between element and regulatory element that this geofence added initially
 
  for (auto el: gf_ptr->affected_parts_)
  {
    for (auto regem : el.regulatoryElements())
    {
      if (!shouldChangeControlLine(el, regem, gf_ptr)) continue;
 
      if (regem->attribute(lanelet::AttributeName::Subtype).value() ==  gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
      {
        auto nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
        gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
        current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
      }
    }
  }
 
  // As this gf received is the first gf that was sent in through addGeofence,
  // we have prev speed limit information inside it to put them back
  for (auto pair : gf_ptr->prev_regems_)
  {
    if (pair.second->attribute(lanelet::AttributeName::Subtype).value() ==  gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
    {
      current_map_->update(current_map_->laneletLayer.get(pair.first), pair.second);
      gf_ptr->update_list_.push_back(pair);
    }
  }
}

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 1495 of file WMBroadcaster.cpp.

{
 
  std::lock_guard<std::mutex> guard(map_mutex_);
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Adding active geofence to the map with geofence id: " << gf_ptr->id_);
 
  // if applying workzone geometry geofence, utilize workzone chache to create one
  // also multiple map updates can be sent from one geofence object
  std::vector<std::shared_ptr<Geofence>> updates_to_send;
 
  bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos;
  bool detected_map_msg_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("MAP_MSG") != std::string::npos;
  if (detected_workzone_signal && gf_ptr->msg_.params.detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
  {
    for (auto gf_cache_ptr : work_zone_geofence_cache_)
    {
      geofenceFromMsg(gf_cache_ptr.second, gf_cache_ptr.second->msg_);
    }
    updates_to_send.push_back(createWorkzoneGeofence(work_zone_geofence_cache_));
  }
  else if (detected_map_msg_signal)
  {
    updates_to_send = geofenceFromMapMsg(gf_ptr, gf_ptr->map_msg_);
  }
  else
  {
    geofenceFromMsg(gf_ptr, gf_ptr->msg_);
    updates_to_send.push_back(gf_ptr);
  }
 
  for (auto update : updates_to_send)
  {
    // add marker to rviz
    if (!update->gf_pts.empty())
    {
      if (update->label_ == carma_wm_ctrl::MAP_MSG_INTERSECTION)
      {
        j2735_map_msg_marker_array_.markers.push_back(composeVisualizerMarkerFromPts(j2735_map_msg_marker_array_, update->gf_pts));
      }
      else
      {
        tcm_marker_array_.markers.push_back(composeVisualizerMarkerFromPts(tcm_marker_array_, update->gf_pts));
      }
    }
 
    if (update->affected_parts_.empty())
      continue;
 
    // Process the geofence object to populate update remove lists
    addGeofenceHelper(update);
 
    if (!detected_map_msg_signal)
    {
      for (auto pair : update->update_list_) active_geofence_llt_ids_.insert(pair.first);
    }
 
    autoware_lanelet2_msgs::msg::MapBin gf_msg;
 
    // If the geofence invalidates the route graph then recompute the routing graph now that the map has been updated
    if (update->invalidate_route_) {
 
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Rebuilding routing graph after is was invalidated by geofence");
 
      lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
      lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_);
      current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
 
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence");
 
      // Populate routing graph structure
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message");
 
      auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
 
      gf_msg.routing_graph = readable_graph->routingGraphToMsg(participant_);
      gf_msg.has_routing_graph = true;
 
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message");
    }
 
 
    // Publish
    auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(update->id_, update->update_list_, update->remove_list_, update->lanelet_additions_));
    send_data->traffic_light_id_lookup_ = update->traffic_light_id_lookup_;
 
    if (detected_map_msg_signal && updates_to_send.back() == update) // if last update
    {
      send_data->sim_ = *sim_;
    }
 
    carma_wm::toBinMsg(send_data, &gf_msg);
    update_count_++; // Update the sequence count for the geofence messages
    gf_msg.seq_id = update_count_;
    gf_msg.invalidates_route=update->invalidate_route_;
    gf_msg.map_version = current_map_version_;
    map_update_pub_(gf_msg);
  }
 
}

References active_geofence_llt_ids_, addGeofenceHelper(), composeVisualizerMarkerFromPts(), createWorkzoneGeofence(), current_map_, current_map_version_, current_routing_graph_, geofenceFromMapMsg(), geofenceFromMsg(), j2735_map_msg_marker_array_, carma_wm_ctrl::MAP_MSG_INTERSECTION, 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 1928 of file WMBroadcaster.cpp.

{
  // resetting the information inside geofence
  gf_ptr->remove_list_ = {};
  gf_ptr->update_list_ = {};
 
  // add additional lanelets that need to be added
  for (auto llt : gf_ptr->lanelet_additions_)
  {
    current_map_->add(llt);
  }
 
  // add trafficlight id mapping
  for (auto pair : gf_ptr->traffic_light_id_lookup_)
  {
    traffic_light_id_lookup_[pair.first] = pair.second;
  }
 
  // Logic to determine what type of geofence
  addRegulatoryComponent(gf_ptr);
}

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 887 of file WMBroadcaster.cpp.

{
  carma_v2x_msgs::msg::TrafficControlDetail msg_detail;
  msg_detail = msg_v01.params.detail;
  // Get affected bounds
  lanelet::LineStrings3d pcl_bounds;
  if (msg_detail.lataffinity == carma_v2x_msgs::msg::TrafficControlDetail::LEFT)
  {
    for (auto llt : affected_llts) pcl_bounds.push_back(llt.leftBound());
    gf_ptr->pcl_affects_left_ = true;
  }
  else //right
  {
    for (auto llt : affected_llts) pcl_bounds.push_back(llt.rightBound());
    gf_ptr->pcl_affects_right_ = true;
  }
 
  // Get specified participants
  std::vector<std::string> left_participants;
  std::vector<std::string> right_participants;
  std::vector<std::string> participants=participantsChecker(msg_v01);
 
  // Create the pcl depending on the allowed passing control direction, left, right, or both
  if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
      msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
  {
    left_participants = participants;
  }
  else if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
  {
    left_participants.push_back(lanelet::Participants::VehicleEmergency);
  }
  if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED ||
      msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY)
  {
    right_participants = participants;
  }
  else if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY)
  {
    right_participants.push_back(lanelet::Participants::VehicleEmergency);
  }
 
  gf_ptr->regulatory_element_ = std::make_shared<lanelet::PassingControlLine>(lanelet::PassingControlLine::buildData(
    lanelet::utils::getId(), pcl_bounds, left_participants, right_participants));
}

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_ptr	geofence pointer
msg_v01	message type
afffected_llts	affected lanelets

Definition at line 933 of file WMBroadcaster.cpp.

{
  const std::string& reason = msg_v01.package.label;
  gf_ptr->label_ = msg_v01.package.label;
  auto regulatory_element = std::make_shared<lanelet::RegionAccessRule>(lanelet::RegionAccessRule::buildData(lanelet::utils::getId(),affected_llts,{},invertParticipants(participantsChecker(msg_v01)), reason));
 
  if(!regulatory_element->accessable(lanelet::Participants::VehicleCar) || !regulatory_element->accessable(lanelet::Participants::VehicleTruck ))
  {
    gf_ptr->invalidate_route_=true;
  }
  else
  {
    gf_ptr->invalidate_route_=false;
  }
  gf_ptr->regulatory_element_ = regulatory_element;
}

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_ptr	geofence pointer
double	min_gap
afffected_llts	affected lanelets
affected_areas	affected areas

Definition at line 950 of file WMBroadcaster.cpp.

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

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 1423 of file WMBroadcaster.cpp.

{
  // First loop is to save the relation between element and regulatory element
  // so that we can add back the old one after geofence deactivates
  for (auto el: gf_ptr->affected_parts_)
  {
    for (auto regem : el.regulatoryElements())
    {
      if (!shouldChangeControlLine(el, regem, gf_ptr) ||
        !shouldChangeTrafficSignal(el, regem, sim_))
        continue;
 
      if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value())
      {
        lanelet::RegulatoryElementPtr nonconst_regem = current_map_->regulatoryElementLayer.get(regem->id());
        gf_ptr->prev_regems_.push_back(std::make_pair(el.id(), nonconst_regem));
        gf_ptr->remove_list_.push_back(std::make_pair(el.id(), nonconst_regem));
        current_map_->remove(current_map_->laneletLayer.get(el.lanelet()->id()), nonconst_regem);
      }
    }
  }
 
 
  // this loop is also kept separately because previously we assumed
  // there was existing regem, but this handles changes to all of the elements
  for (auto el: gf_ptr->affected_parts_)
  {
    // update it with new regem
    if (gf_ptr->regulatory_element_->id() != lanelet::InvalId)
    {
      current_map_->update(current_map_->laneletLayer.get(el.id()), gf_ptr->regulatory_element_);
      gf_ptr->update_list_.push_back(std::pair<lanelet::Id, lanelet::RegulatoryElementPtr>(el.id(), gf_ptr->regulatory_element_));
    } else {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Regulatory element with invalid id in geofence cannot be added to the map");
    }
  }
 
 
 
}

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_ptr	geofence pointer
msg_v01	TrafficControlMessageV01 (geofence msg)

Definition at line 117 of file WMBroadcaster.cpp.

{
  // Handle schedule processing
  carma_v2x_msgs::msg::TrafficControlSchedule msg_schedule = msg_v01.params.schedule;
  double ns_to_sec = 1.0e9;
  auto clock_type = scheduler_.getClockType();
 
  rclcpp::Time end_time = {msg_schedule.end, clock_type};
  if (!msg_schedule.end_exists) {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "No end time for geofence, using rclcpp::Time::max()");
    end_time = {rclcpp::Time::max(), clock_type}; // If there is no end time use the max time
  }
 
  // If the days of the week are specified then convert them to the boost format.
  GeofenceSchedule::DayOfTheWeekSet week_day_set = { 0, 1, 2, 3, 4, 5, 6 }; // Default to all days  0==Sun to 6==Sat
  if (msg_schedule.dow_exists) {
   // sun (6), mon (5), tue (4), wed (3), thu (2), fri (1), sat (0)
   week_day_set.clear();
   for (size_t i = 0; i < msg_schedule.dow.dow.size(); i++) {
     if (msg_schedule.dow.dow[i] == 1) {
      switch(i) {
          case 6: // sun
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Sunday);
            break;
          case 5: // mon
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Monday);
            break;
          case 4: // tue
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Tuesday);
            break;
          case 3: // wed
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Wednesday);
            break;
          case 2: // thur
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Thursday);
            break;
          case 1: // fri
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Friday);
            break;
          case 0: // sat
            week_day_set.emplace(boost::gregorian::greg_weekday::weekday_enum::Saturday);
            break;
          default:
            throw std::invalid_argument("Unrecognized weekday value: " + std::to_string(i));
      }
     }
   }
  }
 
  if (msg_schedule.between_exists) {
 
    for (auto daily_schedule : msg_schedule.between)
    {
      if (msg_schedule.repeat_exists) {
        gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
                                    end_time,
                                    rclcpp::Duration(daily_schedule.begin),
                                    rclcpp::Duration(daily_schedule.duration),
                                    rclcpp::Duration(msg_schedule.repeat.offset),
                                    rclcpp::Duration(msg_schedule.repeat.span),
                                    rclcpp::Duration(msg_schedule.repeat.period),
                                    week_day_set));
      } else {
        gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
                                  end_time,
                                  rclcpp::Duration(daily_schedule.begin),
                                  rclcpp::Duration(daily_schedule.duration),
                                  rclcpp::Duration(0.0), // No offset
                                  rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin),   // Compute schedule portion end time
                                  rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin),   // No repetition so same as portion end time
                                  week_day_set));
      }
 
    }
  }
  else {
    if (msg_schedule.repeat_exists) {
      gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
                                  end_time,
                                  rclcpp::Duration(0.0),
                                  rclcpp::Duration(86400.0e9), // 24 hr daily application
                                  rclcpp::Duration(msg_schedule.repeat.offset),
                                  rclcpp::Duration(msg_schedule.repeat.span),
                                  rclcpp::Duration(msg_schedule.repeat.period),
                                  week_day_set));
    } else {
      gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type},
                                  end_time,
                                  rclcpp::Duration(0.0),
                                  rclcpp::Duration(86400.0e9), // 24 hr daily application
                                  rclcpp::Duration(0.0),     // No offset
                                  rclcpp::Duration(86400.0e9), // Applied for full lenth of 24 hrs
                                  rclcpp::Duration(86400.0e9), // No repetition
                                  week_day_set));
    }
 
  }
}

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_msg

The map message to use as the base map

Definition at line 56 of file WMBroadcaster.cpp.

{
  std::lock_guard<std::mutex> guard(map_mutex_);
 
  static bool firstCall = true;
  // This function should generally only ever be called one time so log a warning if it occurs multiple times
  if (firstCall)
  {
    firstCall = false;
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called for first time with new map message");
  }
  else
  {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called multiple times in the same node");
  }
 
  lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap);
  lanelet::LaneletMapPtr new_map_to_change(new lanelet::LaneletMap);
 
  lanelet::utils::conversion::fromBinMsg(*map_msg, new_map);
  lanelet::utils::conversion::fromBinMsg(*map_msg, new_map_to_change);
 
  base_map_ = new_map;  // Store map
  current_map_ = new_map_to_change; // broadcaster makes changes to this
 
  lanelet::MapConformer::ensureCompliance(base_map_, config_limit);     // Update map to ensure it complies with expectations
  lanelet::MapConformer::ensureCompliance(current_map_, config_limit);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Building routing graph for base map");
 
  lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
  lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_);
  current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done building routing graph for base map");
 
  // Publish map
  current_map_version_ += 1; // Increment the map version. It should always start from 1 for the first map
 
  autoware_lanelet2_msgs::msg::MapBin compliant_map_msg;
 
  // Populate the routing graph message
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message.");
 
  auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
 
  compliant_map_msg.routing_graph = readable_graph->routingGraphToMsg(participant_);
  compliant_map_msg.has_routing_graph = true;
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message.");
 
  lanelet::utils::conversion::toBinMsg(current_map_, &compliant_map_msg);
  compliant_map_msg.map_version = current_map_version_;
  map_pub_(compliant_map_msg);
};

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_pos

Current position of the vehicle

Returns

0 if vehicle is not on an active geofence

Definition at line 2028 of file WMBroadcaster.cpp.

{
 
  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    throw lanelet::InvalidObjectStateError(std::string("Lanelet map 'current_map_' is not loaded to the WMBroadcaster"));
  }
 
  // Store current position values to be compared to geofence boundary values
  double current_pos_x = current_pos.pose.position.x;
  double current_pos_y = current_pos.pose.position.y;
 
 
 
 
  lanelet::BasicPoint2d curr_pos;
  curr_pos.x() = current_pos_x;
  curr_pos.y() = current_pos_y;
 
  carma_perception_msgs::msg::CheckActiveGeofence outgoing_geof; //message to publish
  double next_distance = 0 ; //Distance to next geofence
 
  if (active_geofence_llt_ids_.size() <= 0 )
  {
    return outgoing_geof;
  }
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence llt ids are loaded to the WMBroadcaster");
 
  // Obtain the closest lanelet to the vehicle's current position
  auto current_llt = lanelet::geometry::findNearest(current_map_->laneletLayer, curr_pos, 1)[0].second;
 
 
  /* determine whether or not the vehicle's current position is within an active geofence */
  if (boost::geometry::within(curr_pos, current_llt.polygon2d().basicPolygon()))
  {
    next_distance = distToNearestActiveGeofence(curr_pos);
    outgoing_geof.distance_to_next_geofence = next_distance;
 
    for(auto id : active_geofence_llt_ids_)
    {
      if (id == current_llt.id())
      {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Vehicle is on Lanelet " << current_llt.id() << ", which has an active geofence");
        outgoing_geof.is_on_active_geofence = true;
        for (auto regem: current_llt.regulatoryElements())
        {
          // Assign active geofence fields based on the speed limit associated with this lanelet
          if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0)
          {
            lanelet::DigitalSpeedLimitPtr speed =  std::dynamic_pointer_cast<lanelet::DigitalSpeedLimit>
            (current_map_->regulatoryElementLayer.get(regem->id()));
            outgoing_geof.value = speed->speed_limit_.value();
            outgoing_geof.advisory_speed = speed->speed_limit_.value();
            outgoing_geof.reason = speed->getReason();
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a speed limit of " << speed->speed_limit_.value());
 
            // Cannot overrule outgoing_geof.type if it is already set to LANE_CLOSED
            if(outgoing_geof.type != carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED)
            {
              outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::SPEED_LIMIT;
            }
          }
 
          // Assign active geofence fields based on the minimum gap associated with this lanelet (if it exists)
          if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalMinimumGap::RuleName) == 0)
          {
            lanelet::DigitalMinimumGapPtr min_gap =  std::dynamic_pointer_cast<lanelet::DigitalMinimumGap>
            (current_map_->regulatoryElementLayer.get(regem->id()));
            outgoing_geof.minimum_gap = min_gap->getMinimumGap();
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a minimum gap of " << min_gap->getMinimumGap());
          }
 
          // Assign active geofence fields based on whether the current lane is closed or is immediately adjacent to a closed lane
          if(regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
          {
            lanelet::RegionAccessRulePtr accessRuleReg =  std::dynamic_pointer_cast<lanelet::RegionAccessRule>
            (current_map_->regulatoryElementLayer.get(regem->id()));
 
            // Update the 'type' and 'reason' for this active geofence if the vehicle is in a closed lane
            if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck))
            {
              RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence is a closed lane.");
              RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Closed lane reason: " << accessRuleReg->getReason());
              outgoing_geof.reason = accessRuleReg->getReason();
              outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
            }
            // 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
            else
            {
              // Obtain all same-direction lanes sharing the right lane boundary (will include the current lanelet)
              auto right_boundary_lanelets = current_map_->laneletLayer.findUsages(current_llt.rightBound());
 
              // Check if the adjacent right lane is closed
              if(right_boundary_lanelets.size() > 1)
              {
                for(auto lanelet : right_boundary_lanelets)
                {
                  // Only check the adjacent right lanelet; ignore the current lanelet
                  if(lanelet.id() != current_llt.id())
                  {
                    for (auto rightRegem: lanelet.regulatoryElements())
                    {
                      if(rightRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
                      {
                        lanelet::RegionAccessRulePtr rightAccessRuleReg =  std::dynamic_pointer_cast<lanelet::RegionAccessRule>
                        (current_map_->regulatoryElementLayer.get(rightRegem->id()));
                        if(!rightAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !rightAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
                        {
                          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Right adjacent Lanelet " << lanelet.id() << " is CLOSED");
                          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence");
                          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << rightAccessRuleReg->getReason());
                          outgoing_geof.reason = rightAccessRuleReg->getReason();
                          outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
                        }
                      }
                    }
                  }
                }
              }
 
              // Check if the adjacent left lane is closed
              auto left_boundary_lanelets = current_map_->laneletLayer.findUsages(current_llt.leftBound());
              if(left_boundary_lanelets.size() > 1)
              {
                for(auto lanelet : left_boundary_lanelets)
                {
                  // Only check the adjacent left lanelet; ignore the current lanelet
                  if(lanelet.id() != current_llt.id())
                  {
                    for (auto leftRegem: lanelet.regulatoryElements())
                    {
                      if(leftRegem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::RegionAccessRule::RuleName) == 0)
                      {
                        lanelet::RegionAccessRulePtr leftAccessRuleReg =  std::dynamic_pointer_cast<lanelet::RegionAccessRule>
                        (current_map_->regulatoryElementLayer.get(leftRegem->id()));
                        if(!leftAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !leftAccessRuleReg->accessable(lanelet::Participants::VehicleTruck))
                        {
                          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Left adjacent Lanelet " << lanelet.id() << " is CLOSED");
                          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence");
                          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << leftAccessRuleReg->getReason());
                          outgoing_geof.reason = leftAccessRuleReg->getReason();
                          outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED;
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
      }
    }
  }
  return outgoing_geof;
}

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 1016 of file WMBroadcaster.cpp.

{
  std::vector<std::string> participants;
 
  if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)!= input_participants.end() &&
      std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus) != input_participants.end() &&
      std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar) != input_participants.end() &&
      std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck) != input_participants.end())
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Detected participants to cover all possible vehicle types");
    participants.emplace_back(lanelet::Participants::Vehicle);
  }
  else
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Not making any changes to the participants list");
    participants = input_participants;
  }
 
  return participants;
}

Referenced by participantsChecker().

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

input

The message containing tcr information

Definition at line 1809 of file WMBroadcaster.cpp.

{
  carma_v2x_msgs::msg::TrafficControlRequestPolygon output;
  lanelet::BasicPoint3d local_point_tmp;
 
  int i = -1;
  while (i < 3) // three offsets; offsets.size() doesn't return accurate size
  {
    if (i == -1)
    {
      local_point_tmp.x() = localPoint.x();
      local_point_tmp.y() = localPoint.y();
    }
    else
    {
      local_point_tmp.x() = localPoint.x() + cB.offsets[i].deltax;;
      local_point_tmp.y() = localPoint.y() + cB.offsets[i].deltay;;
    }
    lanelet::GPSPoint gps_vertex = local_projector.reverse(local_point_tmp);
 
    carma_v2x_msgs::msg::Position3D gps_msg;
    gps_msg.elevation = gps_vertex.ele;
    gps_msg.latitude = gps_vertex.lat;
    gps_msg.longitude = gps_vertex.lon;
    output.polygon_list.push_back(gps_msg);
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "TCR Vertex Lat: "<< std::to_string(gps_vertex.lat));
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "TCR Vertex Lon: "<<std::to_string(gps_vertex.lon));
 
    i++;
  }
  return output;
}

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:

composeVisualizerMarkerFromPts()

visualization_msgs::msg::Marker carma_wm_ctrl::WMBroadcaster::composeVisualizerMarkerFromPts	(	const visualization_msgs::msg::MarkerArray &	marker_array,
		const std::vector< lanelet::Point3d > &	input
	)

Visualizes in Rviz geometry points related to MAP msg or TrafficControlMessage.

Parameters

marker_array	Previously visualized markers if available to continue marker id
input	geometry points to visualize

Returns

MarkerArray containing only the new visualization from the gf pts intended to be appended to the previous markers

Definition at line 1843 of file WMBroadcaster.cpp.

{
  // create the marker msgs
  visualization_msgs::msg::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = rclcpp::Time();
  marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.ns = "map_update_visualizer";
 
  marker.scale.x = 0.65;
  marker.scale.y = 0.65;
  marker.scale.z = 0.65;
  marker.frame_locked = true;
 
  if (!marker_array.markers.empty())
  {
    marker.id = marker_array.markers.back().id + 1;
  }
  else
  {
    marker.id = 0;
  }
  marker.color.r = 0.0F;
  marker.color.g = 1.0F;
  marker.color.b = 0.0F;
  marker.color.a = 1.0F;
 
  for (int i = 0; i < input.size(); i++)
  {
    geometry_msgs::msg::Point temp_point;
    temp_point.x = input[i].x();
    temp_point.y = input[i].y();
    temp_point.z = 2; //to show up on top of the lanelet lines
 
    marker.points.push_back(temp_point);
  }
 
  return marker;
 }

References process_bag::i.

Referenced by addGeofence().

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_msg	The message containing route information pulled from routeCallbackMessage()
req_id_for_testing	this ptr is optional. it gives req_id for developer to test TrafficControlMessage as it needs it

Definition at line 1658 of file WMBroadcaster.cpp.

{
  lanelet::ConstLanelets path;
 
  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
   // Return / log warning etc.
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'current_map_' does not exist.");
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
 
  }
 
  for(auto id : route_msg.route_path_lanelet_ids)
  {
    auto laneLayer = current_map_->laneletLayer.get(id);
    path.push_back(laneLayer);
  }
 
  // update local copy
  route_path_ = path;
 
  if(path.size() == 0) throw lanelet::InvalidObjectStateError(std::string("No lanelets available in path."));
 
   /*logic to determine route bounds*/
  std::vector<lanelet::ConstLanelet> llt;
  std::vector<lanelet::BoundingBox2d> pathBox;
  double minX = std::numeric_limits<double>::max();
  double minY = std::numeric_limits<double>::max();
  double maxX = std::numeric_limits<double>::lowest();
  double maxY = std::numeric_limits<double>::lowest();
 
  while (path.size() != 0) //Continue until there are no more lanelet elements in path
  {
      llt.push_back(path.back()); //Add a lanelet to the vector
 
 
      pathBox.push_back(lanelet::geometry::boundingBox2d(llt.back())); //Create a bounding box of the added lanelet and add it to the vector
 
 
      if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x() < minX)
        minX = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).x(); //minimum x-value
 
 
      if (pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y() < minY)
        minY = pathBox.back().corner(lanelet::BoundingBox2d::BottomLeft).y(); //minimum y-value
 
 
 
     if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x() > maxX)
        maxX = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).x(); //maximum x-value
 
 
     if (pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y() > maxY)
        maxY = pathBox.back().corner(lanelet::BoundingBox2d::TopRight).y(); //maximum y-value
 
 
      path.pop_back(); //remove the added lanelet from path an reduce pack.size() by 1
  } //end of while loop
 
 
  std::string target_frame = base_map_georef_;
  if (target_frame.empty())
  {
   // Return / log warning etc.
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'target_frame' is empty.");
    throw lanelet::InvalidObjectStateError(std::string("Base georeference map may not be loaded to the WMBroadcaster"));
 
  }
 
  // Convert the minimum point to latlon
  lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str());
  lanelet::BasicPoint3d localPoint;
 
  localPoint.x()= minX;
  localPoint.y()= minY;
 
  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
 
  // Create a local transverse mercator frame at the minimum point to allow us to get east,north oriented bounds
  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);
 
  // Create transform from map frame to local transform mercator frame at bounds min point
  PJ* tmerc_proj = proj_create_crs_to_crs(PJ_DEFAULT_CTX, target_frame.c_str(), local_tmerc_enu_proj.c_str(), nullptr);
 
  if (tmerc_proj == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
 
    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)
      << " MapProjection: " << target_frame << " Message Projection: " << local_tmerc_enu_proj);
 
    return {}; // Ignore geofence if it could not be projected into the map frame
 
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Top Left: ("<< minX <<", "<<maxY<<")");
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Top Right: ("<< maxX <<", "<<maxY<<")");
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Bottom Left: ("<< minX <<", "<<minY<<")");
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion: Bottom Right: ("<< maxX <<", "<<minY<<")");
 
  PJ_COORD pj_min {{minX, minY, 0, 0}}; // z is not currently used
  PJ_COORD pj_min_tmerc;
  PJ_COORD pj_max {{maxX, maxY, 0, 0}}; // z is not currently used
  PJ_COORD pj_max_tmerc;
  pj_min_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_min);
  pj_max_tmerc = proj_trans(tmerc_proj, PJ_FWD, pj_max);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion: MinPoint ( "<< pj_min_tmerc.xyz.x <<", " << pj_min_tmerc.xyz.y <<" )");
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion: MaxPoint ( "<< pj_max_tmerc.xyz.x <<", " << pj_max_tmerc.xyz.y <<" )");
 
  carma_v2x_msgs::msg::TrafficControlRequest cR; /*Fill the latitude value in message cB with the value of lat */
  carma_v2x_msgs::msg::TrafficControlBounds cB; /*Fill the longitude value in message cB with the value of lon*/
 
  cB.reflat = gpsRoute.lat;
  cB.reflon = gpsRoute.lon;
 
  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
  cB.offsets[0].deltay = 0.0;                                     // calculating the offsets
  cB.offsets[1].deltax = pj_max_tmerc.xyz.x - pj_min_tmerc.xyz.x;
  cB.offsets[1].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
  cB.offsets[2].deltax = 0.0;
  cB.offsets[2].deltay = pj_max_tmerc.xyz.y - pj_min_tmerc.xyz.y;
 
  tcr_polygon_ = composeTCRStatus(localPoint, cB, local_projector); // TCR polygon can be visualized in UI
 
  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
 
  cR.choice = carma_v2x_msgs::msg::TrafficControlRequest::TCRV01;
 
  // create 16 byte uuid
  boost::uuids::uuid uuid_id = boost::uuids::random_generator()();
  // take half as string
  std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
  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
  generated_geofence_reqids_.insert(req_id_test);
  generated_geofence_reqids_.insert(reqid);
 
 
  // copy to reqid array
  boost::array<uint8_t, 16UL> req_id;
  std::copy(uuid_id.begin(),uuid_id.end(), req_id.begin());
  for (auto i = 0; i < 8; i ++)
  {
    cR.tcr_v01.reqid.id[i] = req_id[i];
    if (req_id_for_testing) req_id_for_testing->id[i] = req_id[i];
  }
 
  cR.tcr_v01.bounds.push_back(cB);
 
  return cR;
 
}

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]	traffic	lanelet_id
[out]	intersection_id	and group_id

Returns

return true if conversion was successful

Definition at line 1972 of file WMBroadcaster.cpp.

{
  for (auto it = traffic_light_id_lookup_.begin(); it != traffic_light_id_lookup_.end(); ++it)
  {
    // Reverse of the logic for generating the lanelet_id. Reference function generate32BitId(const std::string& label)
    if (it -> second == lanelet_id)
    {
      group_id = (it -> first & 0xFF);
      intersection_id = (it -> first >> 8);
      return true;
    }
  }
  return false;
}

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 2208 of file WMBroadcaster.cpp.

{
  return lanelet::Lanelet(lanelet::utils::getId(), createLinearInterpolatingLinestring(left_front_pt, left_back_pt, increment_distance), createLinearInterpolatingLinestring(right_front_pt, right_back_pt, increment_distance));
}

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 2187 of file WMBroadcaster.cpp.

{
  double dx = back_pt.x() - front_pt.x();
  double dy = back_pt.y() - front_pt.y();
 
  std::vector<lanelet::Point3d> points;
  double distance = std::sqrt(pow(dx, 2) + pow(dy,2));
  double cos = dx / distance;
  double sin = dy / distance;
  points.push_back(front_pt);
  double sum = increment_distance;
  while ( sum < distance)
  {
    points.push_back(lanelet::Point3d(lanelet::utils::getId(),front_pt.x() + sum * cos, front_pt.y() + sum * sin, 0.0));
    sum += increment_distance;
  }
  points.push_back(back_pt);
 
  return lanelet::LineString3d(lanelet::utils::getId(), points);
}

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_cache

Geofence 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

InvalidObjectStateError

if no map is available

Returns

geofence housing all necessary workzone elements

Definition at line 400 of file WMBroadcaster.cpp.

{
  std::shared_ptr<std::vector<lanelet::Lanelet>> parallel_llts = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
  std::shared_ptr<std::vector<lanelet::Lanelet>> middle_opposite_lanelets = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
 
  // Split existing lanelets and filter into parallel_llts and middle_opposite_lanelets
  preprocessWorkzoneGeometry(work_zone_geofence_cache, parallel_llts, middle_opposite_lanelets);
 
  // Create geofence and rest of the required lanelets along with traffic light for completing workzone area
  auto gf_ptr = createWorkzoneGeometry(work_zone_geofence_cache, parallel_llts->front(), parallel_llts->back(), middle_opposite_lanelets );
 
  // copy static info from the existing workzone
  gf_ptr->id_ = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->id_; //using taperright's id as the whole geofence's id
 
  // schedule
  gf_ptr->schedules = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->schedules; //using taperright's schedule as the whole geofence's schedule
 
  // erase cache now that it is processed
  for (auto pair : work_zone_geofence_cache)
  {
    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_);
  }
  work_zone_geofence_cache.clear();
 
  return gf_ptr;
}

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_cache	Geofence map with size of 4 corresponding to CLOSED, TAPERRIGHT, OPENRIGHT, REVERSE TrafficControlMessages. Each should have gf_pts, affected_parts.
parallel_llt_front	A lanelet whose end should connect to front diagonal lanelet
parallel_llt_back	A lanelet whose start should connect to back diagonal lanelet
middle_opposite_lanelets	A getInterGroupIdsByLightReg() connects to back diagonal (their directions are expected to be opposite of parallel ones)

Exceptions

InvalidObjectStateError

if no map is available

Definition at line 427 of file WMBroadcaster.cpp.

{
  auto gf_ptr = std::make_shared<Geofence>();
 
  //FRONT DIAGONAL LANELET
 
  lanelet::Lanelet front_llt_diag = createLinearInterpolatingLanelet(parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back(),
                                                                        middle_opposite_lanelets->back().rightBound3d().back(), middle_opposite_lanelets->back().leftBound3d().back());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created diag front_llt_diag id:" << front_llt_diag.id());
  for (auto regem : middle_opposite_lanelets->back().regulatoryElements()) //copy existing regem into the new llts
  {
    front_llt_diag.addRegulatoryElement(regem);
  }
 
  //BACK DIAGONAL LANELET
 
  lanelet::Lanelet back_llt_diag = createLinearInterpolatingLanelet(middle_opposite_lanelets->front().rightBound3d().front(),  middle_opposite_lanelets->front().leftBound3d().front(),
                                                                     parallel_llt_back.leftBound3d().front(), parallel_llt_back.rightBound3d().front());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created back_llt_diag diag id:" << back_llt_diag.id());
  for (auto regem : parallel_llt_back.regulatoryElements()) //copy existing regem into the new llts
  {
    back_llt_diag.addRegulatoryElement(regem);
  }
 
  //MIDDLE LANELETS TO MATCH PARALLEL DIRECTION
 
  std::vector<lanelet::Lanelet> middle_llts;
  for (int i = middle_opposite_lanelets->size() - 1; i >= 0; i--) //do no use size_t as it might overflow
  {
    lanelet::Lanelet middle_llt (lanelet::utils::getId(), (*(middle_opposite_lanelets.get()))[i].rightBound3d().invert(), (*(middle_opposite_lanelets.get()))[i].leftBound3d().invert());
    for (auto regem : (*(middle_opposite_lanelets.get()))[i].regulatoryElements())
    {
      middle_llt.addRegulatoryElement(regem); //copy existing regem into the new llts
    }
    middle_llts.push_back(middle_llt);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created matching direction of middle_llt id:" << middle_llt.id());
  }
 
  //ADD TF_LIGHT TO PARALLEL LANELET
  lanelet::LineString3d parallel_stop_ls(lanelet::utils::getId(), {parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back()});
  // controlled lanelet:
  std::vector<lanelet::Lanelet> controlled_taper_right;
 
  controlled_taper_right.push_back(parallel_llt_front); //which has the light
 
  controlled_taper_right.push_back(front_llt_diag);
 
  controlled_taper_right.insert(controlled_taper_right.end(), middle_llts.begin(), middle_llts.end());
 
  controlled_taper_right.push_back(back_llt_diag);
 
  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()}));
 
  gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->label_),tfl_parallel->id()});
 
  parallel_llt_front.addRegulatoryElement(tfl_parallel);
  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());
 
  //ADD TF_LIGHT TO OPPOSITE LANELET
  std::shared_ptr<std::vector<lanelet::Lanelet>> opposite_llts_with_stop_line = std::make_shared<std::vector<lanelet::Lanelet>>(std::vector<lanelet::Lanelet>());
 
  auto old_opposite_llts = splitOppositeLaneletWithPoint(opposite_llts_with_stop_line, work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d(),
                        current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id()), error_distance_);
 
  lanelet::LineString3d opposite_stop_ls(lanelet::utils::getId(), {opposite_llts_with_stop_line->front().leftBound3d().back(), opposite_llts_with_stop_line->front().rightBound3d().back()});
 
  std::vector<lanelet::Lanelet> controlled_open_right;
 
  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
 
  for (auto llt : work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_)
  {
    controlled_open_right.push_back(current_map_->laneletLayer.get(llt.lanelet().get().id()));
  }
 
  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()}));
 
  gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->label_), tfl_opposite->id()});
 
  opposite_llts_with_stop_line->front().addRegulatoryElement(tfl_opposite);
  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());
 
  //ADD ALL NEWLY CREATED LANELETS INTO GEOFENCE
  //OBJECTS TO BE PROCESSED LATER BY SCHEDULER
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_front id:" << parallel_llt_front.id());
  gf_ptr->lanelet_additions_.push_back(parallel_llt_front);
 
  gf_ptr->lanelet_additions_.push_back(front_llt_diag);
 
  gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), middle_llts.begin(), middle_llts.end());
 
  gf_ptr->lanelet_additions_.push_back(back_llt_diag);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_back id:" << parallel_llt_back.id());
  gf_ptr->lanelet_additions_.push_back(parallel_llt_back);
 
  gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());;
 
  // ADD REGION_ACCESS_RULE REGEM TO THE OUTDATED LANELETS
  // AS WELL AS LANELETS BEING BLOCKED BY WORKZONE GEOFENCE
 
  // fill information for paricipants and reason for blocking
  carma_v2x_msgs::msg::TrafficControlMessageV01 participants_and_reason_only;
 
  j2735_v2x_msgs::msg::TrafficControlVehClass participant; // sending all possible VEHICLE will be processed as they are not accessuble by regionAccessRule
 
  participant.vehicle_class =  j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE;
 
  participants_and_reason_only.params.vclasses.push_back(participant);
 
  participant.vehicle_class =  j2735_v2x_msgs::msg::TrafficControlVehClass::BUS;
 
  participants_and_reason_only.params.vclasses.push_back(participant);
 
  participant.vehicle_class =  j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR;
 
  participants_and_reason_only.params.vclasses.push_back(participant);
 
  participant.vehicle_class =  j2735_v2x_msgs::msg::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK;
 
  participants_and_reason_only.params.vclasses.push_back(participant);
 
  participants_and_reason_only.package.label = "SIG_WZ";
 
  std::vector<lanelet::Lanelet> old_or_blocked_llts; // this is needed to addRegionAccessRule input signatures
 
  // from all affected parts, remove duplicate entries
  for (auto llt : work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_)
  {
    if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
    {
      gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
      old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
    }
  }
  for (auto llt : work_zone_geofence_cache[WorkZoneSection::CLOSED]->affected_parts_)
  {
    if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
    {
      gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
      old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
    }
  }
  for (auto llt : work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_)
  {
    if (std::find(old_or_blocked_llts.begin(), old_or_blocked_llts.end(), llt) == old_or_blocked_llts.end())
    {
      gf_ptr->affected_parts_.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
      old_or_blocked_llts.push_back(current_map_->laneletLayer.get(llt.lanelet()->id()));
    }
  }
 
  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
  old_or_blocked_llts.push_back(old_opposite_llts[0]);
 
  // actual regulatory element adder
  addRegionAccessRule(gf_ptr, participants_and_reason_only, old_or_blocked_llts);
 
  return gf_ptr;
}

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 1962 of file WMBroadcaster.cpp.

{
  if (current_map_ && current_map_->laneletLayer.size() != 0) {
    carma_perception_msgs::msg::CheckActiveGeofence check = checkActiveGeofenceLogic(*current_pos);
    active_pub_(check);//Publish
  } else {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Could not check active geofence logic because map was not loaded");
  }
}

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_pos

Current position in local coordinates

Exceptions

InvalidObjectStateError	if base_map is not set
std::invalid_argument	if curr_pos is not on the road

Returns

0 if there is no active geofence on the vehicle's route

Definition at line 1884 of file WMBroadcaster.cpp.

{
  std::lock_guard<std::mutex> guard(map_mutex_);
 
  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    throw lanelet::InvalidObjectStateError(std::string("Lanelet map (current_map_) is not loaded to the WMBroadcaster"));
  }
 
  // filter only the lanelets in the route
  std::vector<lanelet::Id> active_geofence_on_route;
  for (auto llt : route_path_)
  {
    if (active_geofence_llt_ids_.find(llt.id()) != active_geofence_llt_ids_.end())
      active_geofence_on_route.push_back(llt.id());
  }
  // Get the lanelet of this point
  auto curr_lanelet = lanelet::geometry::findNearest(current_map_->laneletLayer, curr_pos, 1)[0].second;
 
  // Check if this point at least is actually within this lanelets
  if (!boost::geometry::within(curr_pos, curr_lanelet.polygon2d().basicPolygon()))
    throw std::invalid_argument("Given point is not within any lanelet");
 
  // get route distance (downtrack + cross_track) distances to every lanelets by their ids
  std::vector<double> route_distances;
  // and take abs of cross_track to add them to get route distance
  for (auto id: active_geofence_on_route)
  {
    carma_wm::TrackPos tp = carma_wm::geometry::trackPos(current_map_->laneletLayer.get(id), curr_pos);
    // downtrack needs to be negative for lanelet to be in front of the point,
    // also we don't account for the lanelet that the vehicle is on
    if (tp.downtrack < 0 && id != curr_lanelet.id())
    {
      double dist = fabs(tp.downtrack) + fabs(tp.crosstrack);
      route_distances.push_back(dist);
    }
  }
  std::sort(route_distances.begin(), route_distances.end());
 
  if (route_distances.size() != 0 ) return route_distances[0];
  else return 0.0;
 
}

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_msg

The ROS msg of the MAP.msg to process

Definition at line 1037 of file WMBroadcaster.cpp.

{
  auto gf_ptr = std::make_shared<Geofence>();
 
  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Map is not available yet. Skipping MAP msg");
    return;
  }
 
  // check if we have seen this message already
  bool up_to_date = false;
  if (sim_->intersection_id_to_regem_id_.size() == map_msg->intersections.size())
  {
    up_to_date = true;
    // check id of the intersection only
    for (auto intersection : map_msg->intersections)
    {
      if (sim_->intersection_id_to_regem_id_.find(intersection.id.id) == sim_->intersection_id_to_regem_id_.end())
      {
        up_to_date = false;
        break;
      }
    }
  }
 
  if(up_to_date)
  {
    return;
  }
 
  gf_ptr->map_msg_ = *map_msg;
  gf_ptr->msg_.package.label_exists = true;
  gf_ptr->msg_.package.label = "MAP_MSG";
  gf_ptr->id_ = boost::uuids::random_generator()();
 
  // create dummy traffic Control message to add instant activation schedule
  carma_v2x_msgs::msg::TrafficControlMessageV01 traffic_control_msg;
 
  // process schedule from message
  addScheduleFromMsg(gf_ptr, traffic_control_msg);
 
  scheduleGeofence(gf_ptr);
 
}

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 1259 of file WMBroadcaster.cpp.

{
  auto pos1 = label.find("INT_ID:") + 7;
  auto pos2 = label.find("SG_ID:") + 6;
 
  uint16_t intersection_id = std::stoi(label.substr(pos1 , 4));
  uint8_t signal_id = std::stoi(label.substr(pos2 , 3));
 
  return carma_wm::utils::get32BitId(intersection_id, signal_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_msg

The ROS msg of the geofence to add.

Definition at line 1084 of file WMBroadcaster.cpp.

{
 
  std::lock_guard<std::mutex> guard(map_mutex_);
  std::stringstream reason_ss;
  // quickly check if the id has been added
  if (geofence_msg->choice != carma_v2x_msgs::msg::TrafficControlMessage::TCMV01) {
    reason_ss << "Dropping received geofence for unsupported TrafficControl version: " << geofence_msg->choice;
    RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str());
    pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::REJECTED), reason_ss.str());
    return;
  }
 
  boost::uuids::uuid id;
  std::copy(geofence_msg->tcm_v01.id.id.begin(), geofence_msg->tcm_v01.id.id.end(), id.begin());
  if (checked_geofence_ids_.find(boost::uuids::to_string(id)) != checked_geofence_ids_.end()) {
    reason_ss.str("");
    reason_ss << "Dropping received TrafficControl message with already handled id: " << boost::uuids::to_string(id);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str());
    pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str());
    return;
  }
 
  // convert reqid to string check if it has been seen before
  boost::array<uint8_t, 16UL> req_id;
  for (auto i = 0; i < 8; i ++) req_id[i] = geofence_msg->tcm_v01.reqid.id[i];
  boost::uuids::uuid uuid_id;
  std::copy(req_id.begin(),req_id.end(), uuid_id.begin());
  std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8);
  // drop if the req has never been sent
  if (generated_geofence_reqids_.find(reqid) == generated_geofence_reqids_.end() && reqid.compare("00000000") != 0)
  {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid);
    return;
  }
 
  checked_geofence_ids_.insert(boost::uuids::to_string(id));
 
  auto gf_ptr = std::make_shared<Geofence>();
 
  gf_ptr->msg_ = geofence_msg->tcm_v01;
 
  try
  {
    // process schedule from message
    addScheduleFromMsg(gf_ptr, geofence_msg->tcm_v01);
    scheduleGeofence(gf_ptr);
    reason_ss.str("");
    reason_ss << "Successfully processed TCM.";
    pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str());
  }
  catch(std::exception& ex)
  {
    reason_ss.str("");
    reason_ss << "Failed to process TCM. " << ex.what();
    pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast<int>(AcknowledgementStatus::REJECTED), reason_ss.str());
    throw; //rethrows the exception object
  }
};

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

Geofence	object to fill with information extracted from this msg
geofence_msg	The MAP ROS msg that contains intersection information

Exceptions

InvalidObjectStateError

if base_map is not set or the base_map's georeference is empty

Definition at line 217 of file WMBroadcaster.cpp.

{
  std::vector<std::shared_ptr<Geofence>> updates_to_send;
  std::vector<std::shared_ptr<lanelet::SignalizedIntersection>> intersections;
  std::vector<std::shared_ptr<lanelet::CarmaTrafficSignal>> traffic_signals;
 
  auto sim_copy = std::make_shared<carma_wm::SignalizedIntersectionManager>(*sim_);
 
  sim_->createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, current_map_, current_routing_graph_);
 
  if (*sim_ == *sim_copy) // if no change
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), ">>> Detected no change from previous, ignoring duplicate message! with gf id: " << gf_ptr->id_);
    return {};
  }
 
  for (auto intersection : intersections)
  {
    auto update = std::make_shared<Geofence>();
    update->id_ = boost::uuids::random_generator()();
    update->label_ = carma_wm_ctrl::MAP_MSG_INTERSECTION;
    update->regulatory_element_ = intersection;
    for (auto llt : intersection->getEntryLanelets())
    {
      update->affected_parts_.push_back(llt);
    }
    // For debug purpose we add the intersection geometry points to visualize later
    auto j2735_intersection_id = sim_->regem_id_to_intersection_id_[intersection->id()];
    for (auto pt: sim_->intersection_nodes_[j2735_intersection_id])
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "J2735 MAP msg road geometry points: x: " << pt.x() << ", y: " << pt.y());
    }
    update->gf_pts.insert(update->gf_pts.end(), sim_->intersection_nodes_[j2735_intersection_id].begin(), sim_->intersection_nodes_[j2735_intersection_id].end());
    updates_to_send.push_back(update);
  }
 
  for (auto signal : traffic_signals)
  {
    auto update = std::make_shared<Geofence>();
    update->id_ = boost::uuids::random_generator()();
    update->label_ = carma_wm_ctrl::MAP_MSG_TF_SIGNAL;
    update->regulatory_element_ = signal;
    for (auto llt : signal->getControlStartLanelets())
    {
      update->affected_parts_.push_back(llt);
    }
    updates_to_send.push_back(update);
  }
 
  return updates_to_send;
}

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

Geofence	object to fill with information extracted from this msg and previously cached msgs that are relevant
geofence_msg	The ROS msg that contains geofence information

Exceptions

InvalidObjectStateError

if base_map is not set or the base_map's georeference is empty

Definition at line 269 of file WMBroadcaster.cpp.

{
  bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find("SIG_WZ") != std::string::npos;
  carma_v2x_msgs::msg::TrafficControlDetail msg_detail = msg_v01.params.detail;
 
  // Get ID
  std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin());
 
  gf_ptr->gf_pts = getPointsInLocalFrame(msg_v01);
 
  gf_ptr->affected_parts_ = getAffectedLaneletOrAreas(gf_ptr->gf_pts);
 
  if (gf_ptr->affected_parts_.size() == 0) {
    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_);
    return; // Return empty geofence list
  }
 
  std::vector<lanelet::Lanelet> affected_llts;
  std::vector<lanelet::Area> affected_areas;
 
  // used for assigning them to the regem as parameters
  for (auto llt_or_area : gf_ptr->affected_parts_)
  {
 
    if (llt_or_area.isLanelet()) affected_llts.push_back(current_map_->laneletLayer.get(llt_or_area.lanelet()->id()));
    if (llt_or_area.isArea()) affected_areas.push_back(current_map_->areaLayer.get(llt_or_area.area()->id()));
  }
 
  // TODO: logic to determine what type of geofence goes here
  // currently only converting portion of control message that is relevant to:
  // - digital speed limit, passing control line, digital minimum gap, region access rule, and series of workzone related messages
  lanelet::Velocity sL;
 
 
  if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
  {
    //Acquire speed limit information from TafficControlDetail msg
    sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPS());
    std::string reason = "";
    if (msg_v01.package.label_exists)
      reason = msg_v01.package.label;
 
    if(config_limit > 0_mph && config_limit < 80_mph && config_limit < sL)//Accounting for the configured speed limit, input zero when not in use
        sL = config_limit;
    //Ensure Geofences do not provide invalid speed limit data (exceed predetermined maximum value)
    // @SONAR_STOP@
    if(sL > 80_mph )
    {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message
    sL = 80_mph; //Cap the speed limit to the predetermined maximum value
 
    }
    if(sL < 0_mph)
    {
          RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital  speed limit is invalid. Value set to 0mph.");
      sL = 0_mph;
    }// @SONAR_START@
 
    gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
                                        sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason));
  }
  if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE)
  {
    //Acquire speed limit information from TafficControlDetail msg
    sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPS());
    if(config_limit > 0_mph && config_limit < 80_mph)//Accounting for the configured speed limit, input zero when not in use
        sL = config_limit;
 
    std::string reason = "";
    if (msg_v01.package.label_exists)
      reason = msg_v01.package.label;
 
    //Ensure Geofences do not provide invalid speed limit data
    // @SONAR_STOP@
    if(sL > 80_mph )
    {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value capped at max speed limit.");
    sL = 80_mph;
    }
    if(sL < 0_mph)
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital  speed limit is invalid. Value set to 0mph.");
      sL = 0_mph;
    }// @SONAR_START@
    gf_ptr->regulatory_element_ = std::make_shared<lanelet::DigitalSpeedLimit>(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(),
                                        sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason));
  }
  if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE)
  {
    addPassingControlLineFromMsg(gf_ptr, msg_v01, affected_llts);
  }
  if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE)
  {
 
    double min_gap = (double)msg_detail.minhdwy;
 
    if(min_gap < 0)
    {
      RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital min gap is invalid. Value set to 0 meter.");
      min_gap = 0;
    }
    addRegionMinimumGap(gf_ptr,msg_v01, min_gap, affected_llts, affected_areas);
  }
 
  if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later
  {
    gf_ptr->label_ = msg_v01.package.label; // to extract intersection, and signal group id
    if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
                                                                                msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
                                                                                msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
    {
      work_zone_geofence_cache_[msg_detail.closed] = gf_ptr;
    }
    else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
    {
      work_zone_geofence_cache_[WorkZoneSection::REVERSE] = gf_ptr;
    }
    return;
  }
  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
  {
    addRegionAccessRule(gf_ptr,msg_v01,affected_llts);
  }
 
  if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::RESTRICTED_CHOICE) {
    addRegionAccessRule(gf_ptr, msg_v01, affected_llts);
  }
 
  return;
}

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_msg

Proj 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 1205 of file WMBroadcaster.cpp.

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

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_msg

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

Definition at line 1351 of file WMBroadcaster.cpp.

{
  return carma_wm::query::getAffectedLaneletOrAreas(gf_pts, current_map_, current_routing_graph_, max_lane_width_);
}

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_msg

The ROS msg that contains proj and any point that lie on the target lanelet or area

Exceptions

InvalidObjectStateError

if base_map is not set or the base_map's georeference is empty

Returns

lanelet::Points3d in local frame

Definition at line 1271 of file WMBroadcaster.cpp.

{
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Getting affected lanelets");
  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
  }
  if (base_map_georef_ == "")
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map has empty proj string loaded as georeference. Therefore, WMBroadcaster failed to\n ") +
                                          std::string("get transformation between the geofence and the map"));
 
  // This next section handles the geofence projection conversion
  // The datum field is used to identify the frame for the provided referance lat/lon.
  // This reference is then converted to the provided projection as a reference origin point
  // From the reference the message projection to map projection transformation is used to convert the nodes in the TrafficControlMessage
  std::string projection = tcm_v01.geometry.proj;
  std::string datum = tcm_v01.geometry.datum;
  if (datum.empty()) {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field not populated. Attempting to use WGS84");
    datum = "WGS84";
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Projection field: " << projection);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field: " << datum);
 
  std::string universal_frame = datum; //lat/long included in TCM is in this datum
 
 
  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.");
 
  // Create the resulting projection transformation
  PJ* universal_to_target = proj_create_crs_to_crs(PJ_DEFAULT_CTX, universal_frame.c_str(), projection.c_str(), nullptr);
  if (universal_to_target == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
 
    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)
      << " universal_frame: " << universal_frame << " projection: " << projection);
 
    return {}; // Ignore geofence if it could not be projected from universal to TCM frame
  }
 
  PJ* target_to_map = proj_create_crs_to_crs(PJ_DEFAULT_CTX, projection.c_str(), base_map_georef_.c_str(), nullptr);
 
  if (target_to_map == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection
 
    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)
      << " target_to_map: " << target_to_map << " base_map_georef_: " << base_map_georef_);
 
    return {}; // Ignore geofence if it could not be projected into the map frame
 
  }
 
  // convert all geofence points into our map's frame
  std::vector<lanelet::Point3d> gf_pts;
  carma_v2x_msgs::msg::PathNode prev_pt;
  PJ_COORD c_init_latlong{{tcm_v01.geometry.reflat, tcm_v01.geometry.reflon, tcm_v01.geometry.refelv}};
  PJ_COORD c_init = proj_trans(universal_to_target, PJ_FWD, c_init_latlong);
 
  prev_pt.x = c_init.xyz.x;
  prev_pt.y =  c_init.xyz.y;
 
  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 );
  for (auto pt : tcm_v01.geometry.nodes)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion in TCM frame: Point X "<< pt.x <<" Before conversion: Point Y "<< pt.y);
 
    PJ_COORD c {{prev_pt.x + pt.x, prev_pt.y + pt.y, 0, 0}}; // z is not currently used
    PJ_COORD c_out;
    c_out = proj_trans(target_to_map, PJ_FWD, c);
 
    gf_pts.push_back(lanelet::Point3d{current_map_->pointLayer.uniqueId(), c_out.xyz.x, c_out.xyz.y});
    prev_pt.x += pt.x;
    prev_pt.y += pt.y;
 
    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());
  }
 
  // save the points converted to local map frame
  return gf_pts;
}

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 1644 of file WMBroadcaster.cpp.

{
  return 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 1254 of file WMBroadcaster.cpp.

{
  return participant_;
}

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 1000 of file WMBroadcaster.cpp.

{
  std::vector<std::string> participants;
 
  if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Pedestrian ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Pedestrian);
  if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Bicycle ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Bicycle);
  if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Vehicle ) == input_participants.end())
  {
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleMotorcycle);
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleBus);
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleCar);
    if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck)== input_participants.end()) participants.emplace_back(lanelet::Participants::VehicleTruck);
  }
  return  participants;
}

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_v01

message type

Definition at line 958 of file WMBroadcaster.cpp.

{
  std::vector<std::string> participants;
  for (j2735_v2x_msgs::msg::TrafficControlVehClass participant : msg_v01.params.vclasses)
  {
    // Currently j2735_v2x_msgs::msg::TrafficControlVehClass::RAIL is not supported
    if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::ANY)
    {
      participants = {lanelet::Participants::Vehicle, lanelet::Participants::Pedestrian, lanelet::Participants::Bicycle};
      break;
    }
    else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN)
    {
      participants.push_back(lanelet::Participants::Pedestrian);
    }
    else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BICYCLE)
    {
      participants.push_back(lanelet::Participants::Bicycle);
    }
    else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE ||
              participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MOTORCYCLE)
    {
      participants.push_back(lanelet::Participants::VehicleMotorcycle);
    }
    else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BUS)
    {
      participants.push_back(lanelet::Participants::VehicleBus);
    }
    else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::LIGHT_TRUCK_VAN ||
            participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR)
    {
      participants.push_back(lanelet::Participants::VehicleCar);
    }
    else if (8<= participant.vehicle_class && participant.vehicle_class <= 16) // Truck enum definition range from 8-16 currently
    {
      participants.push_back(lanelet::Participants::VehicleTruck);
    }
  }
  // combine to single vehicle type if possible, otherwise pass through
  return  combineParticipantsToVehicle(participants);
}

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_cache	Geofence map with size of 4 corresponding to CLOSED, TAPERRIGHT, OPENRIGHT, REVERSE TrafficControlMessages. Each should have gf_pts, affected_parts.
parallel_llts	list holding front_parallel and back_parallel. These are created from splitting old ones in place such that they connect to diagonal ones
opposite_llts	list holding split and filtered lanelets from REVERSE part that connect to diagonal ones

Exceptions

InvalidObjectStateError

if 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 607 of file WMBroadcaster.cpp.

{
  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
  }
  std::vector <lanelet::Lanelet> new_taper_right_llts;
  auto taper_right_first_pt = work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->gf_pts.front().basicPoint2d();
  auto taper_right_first_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id());
 
  new_taper_right_llts = splitLaneletWithPoint({taper_right_first_pt}, taper_right_first_llt, error_distance_);
  double check_dist_tpr = lanelet::geometry::distance2d(taper_right_first_llt.centerline2d().front().basicPoint2d(), taper_right_first_pt);
 
  // 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
  // to match the output expected of this function as if split happened
  if (new_taper_right_llts.size() == 1 && check_dist_tpr <= error_distance_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating duplicate lanelet of 'previous lanelet' due to TAPERRIGHT using entire lanelet...");
    auto previous_lanelets = current_routing_graph_->previous(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get());
    if (previous_lanelets.empty()) //error if bad match
    {
      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()
                      << ". This case is rare and not supported at the moment.");
      return;
    }
 
    // get previous lanelet of affected part of TAPERRIGHT (doesn't matter which previous, as the new lanelet will only be duplicate anyways)
    auto prev_lanelet_to_copy  = current_map_->laneletLayer.get(previous_lanelets.front().id());
 
    // parallel_llts will have a copy of `prev_lanelet_to_copy` with new id to be used as part of workzone area
    new_taper_right_llts = splitLaneletWithPoint({prev_lanelet_to_copy.centerline2d().back()}, prev_lanelet_to_copy, error_distance_);
  }
  parallel_llts->insert(parallel_llts->end(), new_taper_right_llts.begin(), new_taper_right_llts.end());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished TAPERRIGHT processing of size: " << new_taper_right_llts.size());
 
  std::vector <lanelet::Lanelet> new_open_right_llts;
  auto open_right_last_pt = work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->gf_pts.back().basicPoint2d();
  auto open_right_last_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id());
 
  new_open_right_llts = splitLaneletWithPoint({open_right_last_pt}, open_right_last_llt, error_distance_);
  double check_dist_opr = lanelet::geometry::distance2d(open_right_last_llt.centerline2d().back().basicPoint2d(), open_right_last_pt);
 
  // if no splitting happened, we need to create duplicate of next lanelet of OPENRIGHT's last lanelet
  // to match the output expected of this function as if split happened
  if (new_open_right_llts.size() == 1 && check_dist_opr <= error_distance_)
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating duplicate lanelet of 'next lanelet' due to OPENRIGHT using entire lanelet...");
    auto next_lanelets = current_routing_graph_->following(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get());
    if (next_lanelets.empty()) //error if bad match
    {
      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()
                      << ". This case is rare and not supported at the moment.");
      return;
    }
 
    // get next lanelet of affected part of OPENRIGHT (doesn't matter which next, as the new lanelet will only be duplicate anyways)
    auto next_lanelet_to_copy  = current_map_->laneletLayer.get(next_lanelets.front().id());
 
    // parallel_llts will have a copy of `next_lanelet_to_copy` with new id to be used as part of workzone area
    new_open_right_llts = splitLaneletWithPoint({next_lanelet_to_copy.centerline2d().back()}, next_lanelet_to_copy, error_distance_);
  }
  parallel_llts->insert(parallel_llts->end(), new_open_right_llts.begin(), new_open_right_llts.end());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished OPENRIGHT processing of size: " << new_open_right_llts.size());
 
 
  auto reverse_back_llts = carma_wm::query::getLaneletsFromPoint(current_map_,  work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d());
  auto reverse_back_llt = reverse_back_llts[0];
  auto reverse_front_llts = carma_wm::query::getLaneletsFromPoint(current_map_,  work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d());
  auto reverse_front_llt = reverse_front_llts[0];
 
  if (reverse_back_llt.id() == reverse_front_llt.id()) //means there is only 1 middle lanelet, which needs to be split into 3 lanelets
  {
    std::vector<lanelet::Lanelet> temp_llts;
    temp_llts = splitLaneletWithPoint({work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(),
                                          work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d()},
                                          current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id()), error_distance_);
 
    if (temp_llts.size() < 2) // if there is only 1 lanelet
    {
      // we found what we want, so return
      opposite_llts->insert(opposite_llts->end(), temp_llts.begin(), temp_llts.end());
      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());
      return;
    }
    else if (temp_llts.size() == 2) // determine which
    {
      // back gap is bigger than front's, we should lose front lanelet from the two
      if (lanelet::geometry::distance2d(work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d(), reverse_front_llt.centerline2d().back().basicPoint2d()) >
        lanelet::geometry::distance2d(work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d(), reverse_front_llt.centerline2d().front().basicPoint2d()))
        {
          opposite_llts->push_back(temp_llts.back());
        }
        else
        {
          opposite_llts->push_back(temp_llts.front());
        }
    }
    else if (temp_llts.size() == 3) // leave only middle lanelets from 3
    {
      opposite_llts->insert(opposite_llts->end(), temp_llts.begin() + 1, temp_llts.end()- 1);
    }
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished REVERSE processing of size: " << opposite_llts->size() << " from original of 1 REVERSE lanelet size");
  }
  else //if there are two or more lanelets
  {
    auto reverse_first_pt = work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.front().basicPoint2d();
    auto reverse_first_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.front().lanelet().get().id());
    std::vector<lanelet::Lanelet> temp_opposite_front_llts;
    temp_opposite_front_llts = splitLaneletWithPoint( {reverse_first_pt}, reverse_first_llt, error_distance_);
    if (temp_opposite_front_llts.size() > 1)
    {
      opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin() + 1, temp_opposite_front_llts.end());
    }
    else
    {
      opposite_llts->insert(opposite_llts->end(), temp_opposite_front_llts.begin(), temp_opposite_front_llts.end());
    }
    if (work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.size() > 2)
    {
      for (int i = 1; i <  work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.size() - 1; i ++)
      {
        opposite_llts->push_back(current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_[i].id()));
      }
    }
 
    auto reverse_last_pt = work_zone_geofence_cache[WorkZoneSection::REVERSE]->gf_pts.back().basicPoint2d();
    auto reverse_last_llt = current_map_->laneletLayer.get(work_zone_geofence_cache[WorkZoneSection::REVERSE]->affected_parts_.back().lanelet().get().id());
    std::vector<lanelet::Lanelet> temp_opposite_back_llts;
    temp_opposite_back_llts = splitLaneletWithPoint({reverse_last_pt}, reverse_last_llt, error_distance_);
    if (temp_opposite_back_llts.size() > 1)
    {
      opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end()- 1);
    }
    else
    {
      opposite_llts->insert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end());
    }
    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");
  }
 
  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());
}

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 1987 of file WMBroadcaster.cpp.

{
  if (traffic_light_id_lookup_.empty())
  {
    return;
  }
  for(auto id : current_route.route_path_lanelet_ids)
  {
    bool convert_success = false;
    unsigned intersection_id = 0;
    unsigned group_id = 0;
    auto route_lanelet= current_map_->laneletLayer.get(id);
    auto traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
 
    if (!traffic_lights.empty())
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id());
      convert_success = convertLightIdToInterGroupId(intersection_id,group_id,  traffic_lights.front()->id());
    }
 
    if (!convert_success)
        continue;
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light with Intersection id: " << intersection_id << " Group id:" << group_id);
    bool id_exists = false;
    for (int idx = 0; idx < upcoming_intersection_ids_.data.size(); idx +2)
    {
      if (upcoming_intersection_ids_.data[idx] == intersection_id && upcoming_intersection_ids_.data[idx + 1] == group_id) //check if already there
      {
        id_exists = true;
        break;
      }
    }
 
    if (id_exists)
      continue;
 
    upcoming_intersection_ids_.data.push_back(static_cast<int>(intersection_id));
    upcoming_intersection_ids_.data.push_back(static_cast<int>(group_id));
  }
}

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 2278 of file WMBroadcaster.cpp.

{
  carma_v2x_msgs::msg::MobilityOperation mom_msg;
  mom_msg.m_header.timestamp = scheduler_.now().nanoseconds()/1000000;
  mom_msg.m_header.sender_id = vehicle_id_;
  mom_msg.strategy = geofence_ack_strategy_;
  std::stringstream ss;
  for(size_t i=0; i < tcm_req_id.id.size(); i++)
  {
    ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned) tcm_req_id.id.at(i);
  }
    std::string tcmv01_req_id_hex = ss.str();
  ss.str("");
  ss << "traffic_control_id:" << tcmv01_req_id_hex << ", msgnum:"<< msgnum << ", acknowledgement:" << ack_status << ", reason:" << ack_reason;
  mom_msg.strategy_params = ss.str();
  for(int i = 0; i < ack_pub_times_; i++)
  {
    tcm_ack_pub_(mom_msg);
  }
}

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 1595 of file WMBroadcaster.cpp.

{
  std::lock_guard<std::mutex> guard(map_mutex_);
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Removing inactive geofence from the map with geofence id: " << gf_ptr->id_);
 
  // Process the geofence object to populate update remove lists
  if (gf_ptr->affected_parts_.empty())
    return;
 
  removeGeofenceHelper(gf_ptr);
 
  for (auto pair : gf_ptr->remove_list_) active_geofence_llt_ids_.erase(pair.first);
 
  // publish
  autoware_lanelet2_msgs::msg::MapBin gf_msg_revert;
  auto send_data = std::make_shared<carma_wm::TrafficControl>(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, {}));
 
  if (gf_ptr->invalidate_route_) { // If a geofence initially invalidated the route it stands to reason its removal should as well
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Rebuilding routing graph after is was invalidated by geofence removal");
 
    lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create(
      lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_
    );
    current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car);
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence removal");
 
    // Populate routing graph structure
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message for geofence removal");
 
    auto readable_graph = std::static_pointer_cast<RoutingGraphAccessor>(current_routing_graph_);
 
    gf_msg_revert.routing_graph = readable_graph->routingGraphToMsg(participant_);
    gf_msg_revert.has_routing_graph = true;
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message for geofence removal");
  }
 
  carma_wm::toBinMsg(send_data, &gf_msg_revert);
  update_count_++; // Update the sequence count for geofence messages
  gf_msg_revert.seq_id = update_count_;
  gf_msg_revert.map_version = current_map_version_;
 
  map_update_pub_(gf_msg_revert);
 
 
}

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 1951 of file WMBroadcaster.cpp.

{
  // Logic to determine what type of geofence
  // reset the info inside geofence
  gf_ptr->remove_list_ = {};
  gf_ptr->update_list_ = {};
  addBackRegulatoryComponent(gf_ptr);
  // as all changes are reverted back, we no longer need prev_regems
  gf_ptr->prev_regems_ = {};
}

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_msg

The message containing route information

Definition at line 1649 of file WMBroadcaster.cpp.

{
 current_route = *route_msg;
 carma_v2x_msgs::msg::TrafficControlRequest cR;
 cR =  controlRequestFromRoute(*route_msg);
 control_msg_pub_(cR);
 
}

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 1144 of file WMBroadcaster.cpp.

{
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_);
 
  bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos;
 
  carma_v2x_msgs::msg::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail;
 
  // create workzone specific extra speed geofence
  if (detected_workzone_signal && msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE)
  {
    // duplicate the messages with inverted points to support new lanelets created from workzone
    // as carma-cloud currently does not support geofence points with direction opposite to that of the road
 
    auto gf_ptr_speed = std::make_shared<Geofence>();
    gf_ptr_speed->schedules = gf_ptr->schedules;
 
    carma_v2x_msgs::msg::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_;
 
    std::reverse(duplicate_msg.geometry.nodes.begin() + 1, duplicate_msg.geometry.nodes.end());
    double first_x = 0;
    double first_y = 0;
 
    // this fancy logic is needed as each node is expressed as an offset from the last one
    for (auto& pt: duplicate_msg.geometry.nodes)
    {
      first_x+= pt.x;
      first_y+= pt.y;
      pt.x = -1* pt.x;
      pt.y = -1* pt.y;
    }
    duplicate_msg.geometry.nodes[0].x = first_x;
    duplicate_msg.geometry.nodes[0].y = first_y;
 
    gf_ptr_speed->msg_ = duplicate_msg;
    scheduler_.addGeofence(gf_ptr_speed);
  }
  if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later
  {
    gf_ptr->label_ = gf_ptr->msg_.package.label; // to extract intersection, and signal group id
    if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED ||
                                                                                msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT ||
                                                                                msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT))
    {
      work_zone_geofence_cache_[msg_detail.closed] = gf_ptr;
    }
    else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE)
    {
      work_zone_geofence_cache_[WorkZoneSection::REVERSE] = gf_ptr;
    }
    if (work_zone_geofence_cache_.size() < WORKZONE_TCM_REQUIRED_SIZE)
    {
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now...");
      return;
    }
  }
 
  scheduler_.addGeofence(gf_ptr);  // Add the geofence to the scheduler
 
}

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_times

the number of times it publishes TCM Acknowledgement

Definition at line 1245 of file WMBroadcaster.cpp.

                                                         {
  ack_pub_times_ = ack_pub_times;
}

References ack_pub_times_.

setConfigSpeedLimit()

void carma_wm_ctrl::WMBroadcaster::setConfigSpeedLimit

(

double

cL

)

Sets the configured speed limit.

Definition at line 1235 of file WMBroadcaster.cpp.

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

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_id

Vehicle ID from UniqueVehicleParams.yaml

Definition at line 1241 of file WMBroadcaster.cpp.

                                                                 {
  vehicle_id_ = vehicle_id;
}

References vehicle_id_.

setErrorDistance()

void carma_wm_ctrl::WMBroadcaster::setErrorDistance

(

double

error_distance

)

Definition at line 602 of file WMBroadcaster.cpp.

{
  error_distance_ = error_distance;
}

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

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

Definition at line 1220 of file WMBroadcaster.cpp.

{
  if (intersection_correction.size() % 2 != 0 || intersection_ids_for_correction.size() != intersection_correction.size() / 2)
  {
    throw std::invalid_argument("Some of intersection coordinate correction parameters are not fully set!");
  }
 
  for (auto i = 0; i < intersection_correction.size(); i = i + 2)
  {
    sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].first =  intersection_correction[i]; //x
    sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].second = intersection_correction[i + 1]; //y
  }
 
}

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 1212 of file WMBroadcaster.cpp.

{
  sim_ = std::make_shared<carma_wm::SignalizedIntersectionManager>();
 
  max_lane_width_ = max_lane_width;
  sim_->setMaxLaneWidth(max_lane_width_);
}

References max_lane_width_, and sim_.

setVehicleParticipationType()

void carma_wm_ctrl::WMBroadcaster::setVehicleParticipationType

(

std::string

participant

)

Set the Vehicle Participation Type.

Parameters

participant

vehicle participation type

Definition at line 1249 of file WMBroadcaster.cpp.

{
  participant_ = participant;
}

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_msg	The ROS msg that contains geofence information
el	The LaneletOrArea that houses the regem
regem	The 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 1364 of file WMBroadcaster.cpp.

{
  // should change if if the regem is not a passing control line or area, which is not supported by this logic
  if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) != 0 || !el.isLanelet())
  {
    return true;
  }
 
  lanelet::PassingControlLinePtr pcl =  std::dynamic_pointer_cast<lanelet::PassingControlLine>(current_map_->regulatoryElementLayer.get(regem->id()));
  // if this geofence's pcl doesn't match with the lanelets current bound side, return false as we shouldn't change
  bool should_change_pcl = false;
  for (auto control_line : pcl->controlLine())
  {
    if ((control_line.id() == el.lanelet()->leftBound2d().id() && gf_ptr->pcl_affects_left_) ||
        (control_line.id() == el.lanelet()->rightBound2d().id() && gf_ptr->pcl_affects_right_))
    {
      should_change_pcl = true;
      break;
    }
  }
  return should_change_pcl;
}

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

el	The LaneletOrArea that houses the regem
regem	The regulatoryElement that needs to be checked
sim	The 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 1395 of file WMBroadcaster.cpp.

{
  // should change if the regem is not a CarmaTrafficSignal, which is not supported by this logic
  if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::CarmaTrafficSignal::RuleName) != 0 || !el.isLanelet() || !sim_)
  {
    return true;
  }
 
  lanelet::CarmaTrafficSignalPtr traffic_signal =  std::dynamic_pointer_cast<lanelet::CarmaTrafficSignal>(current_map_->regulatoryElementLayer.get(regem->id()));
  uint8_t signal_id = 0;
 
  for (auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it)
  {
    if (regem->id() == it->second)
    {
      signal_id = it->first;
    }
  }
 
  if (signal_id == 0) // doesn't exist in the record
    return true;
 
  if (sim->signal_group_to_entry_lanelet_ids_[signal_id].find(el.id()) != sim->signal_group_to_entry_lanelet_ids_[signal_id].end())
    return false; // signal group's entry lane is still part of the intersection, so don't change
 
  return true;
}

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_pts	Points whose downtrack ratio relative to given lanelet will be used to split the lanelet
input_llt	A lanelet to split
error_distance	if within this distance (in meters) the point will be ignored

Returns

lanelets split form original one. lanelets sorted from front to back.

Exceptions

InvalidObjectStateError

if no map is available. NOTE: this is requried to return mutable objects.

Definition at line 762 of file WMBroadcaster.cpp.

{
  // get ratio of this point and split
  std::vector<lanelet::Lanelet> parallel_llts;
  double llt_downtrack = carma_wm::geometry::trackPos(input_llt, input_llt.centerline().back().basicPoint2d()).downtrack;
  std::vector<double> ratios;
 
  for (auto pt : input_pts)
  {
    double point_downtrack = carma_wm::geometry::trackPos(input_llt, pt).downtrack;
    double point_downtrack_ratio = point_downtrack / llt_downtrack;
    ratios.push_back(point_downtrack_ratio);
  }
 
  auto new_parallel_llts = splitLaneletWithRatio(ratios, input_llt, error_distance);
 
  parallel_llts.insert(parallel_llts.end(),new_parallel_llts.begin(), new_parallel_llts.end());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithPoint returning lanelets size: " << parallel_llts.size());
  return parallel_llts;
}

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

ratios	Ratios relative to given lanelet will be used to split the lanelet
input_llt	A lanelet to split
error_distance	if 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

InvalidObjectStateError

if no map is available. NOTE: this is requried to return mutable objects.

Definition at line 804 of file WMBroadcaster.cpp.

{
  if (!current_map_ || current_map_->laneletLayer.size() == 0)
  {
    throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster"));
  }
  if (ratios.empty())
  {
    throw lanelet::InvalidInputError(std::string("Ratios list is empty! Cannot split"));
  }
 
  std::vector<lanelet::Lanelet> created_llts;
 
  std::sort(ratios.begin(), ratios.end());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio evaluating input ratios of size: " << ratios.size());
 
  ratios.push_back(1.0); //needed to complete the loop
 
  int left_ls_size = input_lanelet.leftBound2d().size();
  int right_ls_size = input_lanelet.rightBound2d().size();
 
  int left_next_pt_idx = 0;
  int left_prev_pt_idx = 0;
  int right_next_pt_idx = 0;
  int right_prev_pt_idx = 0;
  for (int i = 0 ; i < ratios.size(); i ++)
  {
    left_next_pt_idx = std::round(ratios[i] * (left_ls_size - 1));
    right_next_pt_idx = std::round(ratios[i] * (right_ls_size - 1));
    // check if edge ratios are too close to any boundaries. if so, skip
    if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
    {
      // assuming both linestrings have roughly the same number of points and
      // assuming distance between 0th and index-th points are small enough we can approximate the curve between them as a line:
      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: "
                        << lanelet::geometry::distance2d(input_lanelet.leftBound2d().front().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
      continue;
    }
    if (lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()) <= error_distance)
    {
      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: "
                  << lanelet::geometry::distance2d(input_lanelet.leftBound2d().back().basicPoint2d(), input_lanelet.leftBound2d()[left_next_pt_idx].basicPoint2d()));
 
      left_next_pt_idx = left_ls_size - 1;
      right_next_pt_idx = right_ls_size - 1;
    }
    // create lanelet
    std::vector<lanelet::Point3d> left_pts;
    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);
 
    lanelet::LineString3d left_ls(lanelet::utils::getId(), left_pts, current_map_->laneletLayer.get(input_lanelet.id()).leftBound3d().attributes());
 
    std::vector<lanelet::Point3d> right_pts;
    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);
 
    lanelet::LineString3d right_ls(lanelet::utils::getId(), right_pts);
 
    lanelet::Lanelet new_llt (lanelet::utils::getId(), left_ls, right_ls, current_map_->laneletLayer.get(input_lanelet.id()).rightBound3d().attributes());
 
    for (auto regem : current_map_->laneletLayer.get(input_lanelet.id()).regulatoryElements()) //copy existing regem into new llts
    {
      new_llt.addRegulatoryElement(current_map_->regulatoryElementLayer.get(regem->id()));
    }
 
    left_prev_pt_idx = left_next_pt_idx;
    right_prev_pt_idx = right_next_pt_idx;
 
    created_llts.push_back(new_llt);
 
    // Detected the end already. Exiting now
    if (left_prev_pt_idx == left_ls_size - 1 || right_prev_pt_idx == right_ls_size - 1)
    {
      break;
    }
 
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio returning lanelets size: " << created_llts.size());
 
  return created_llts;
}

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_llts	return lanelets split form original one. lanelets sorted from front to back.
input_pt	A point whose downtrack ratio relative to given lanelet will be used to split the lanelet
input_llt	A lanelet to split
error_distance	if within this distance (in meters) the point will be ignored

Returns

return original opposite lanelet(s if overlapping)

Exceptions

InvalidObjectStateError

if 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 783 of file WMBroadcaster.cpp.

{
  // get ratio of this point and split
  auto point_downtrack = carma_wm::geometry::trackPos(input_llt, input_pt).downtrack;
  auto point_downtrack_ratio = point_downtrack / carma_wm::geometry::trackPos(input_llt, input_llt.centerline().back().basicPoint2d()).downtrack;
 
  // get opposing lanelets and split
  auto opposing_llts = carma_wm::query::nonConnectedAdjacentLeft(current_map_, input_pt);
 
  if (opposing_llts.empty())
  {
    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");
    return {};
  }
 
  auto new_llts_opposite = splitLaneletWithRatio({1 - point_downtrack_ratio}, opposing_llts[0], error_distance);
  opposite_llts->insert(opposite_llts->begin(),new_llts_opposite.begin(), new_llts_opposite.end());
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitOppositeLaneletWithPoint returning lanelets size: " << opposite_llts->size());
  return opposing_llts;
}

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 2213 of file WMBroadcaster.cpp.

{
  uint16_t map_msg_intersection_id = 0;
  uint16_t cur_signal_group_id = 0;
  std::vector<lanelet::CarmaTrafficSignalPtr> traffic_lights;
  lanelet::Lanelet route_lanelet;
  lanelet::Ids cur_route_lanelet_ids = current_route.route_path_lanelet_ids;
  bool isLightFound = false;
 
  for(auto id : cur_route_lanelet_ids)
  {
    route_lanelet= current_map_->laneletLayer.get(id);
    traffic_lights = route_lanelet.regulatoryElementsAs<lanelet::CarmaTrafficSignal>();
    if(!traffic_lights.empty())
    {
      isLightFound  = true;
      break;
    }
  }
 
  if(isLightFound && sim_)
  {
    for(auto itr = sim_->signal_group_to_traffic_light_id_.begin(); itr != sim_->signal_group_to_traffic_light_id_.end(); itr++)
    {
      if(itr->second == traffic_lights.front()->id())
      {
        cur_signal_group_id = itr->first;
      }
    }
  }
  else{
     RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching Traffic lights along the route");
  }//END Traffic signals
 
  auto intersections = route_lanelet.regulatoryElementsAs<lanelet::SignalizedIntersection>();
  if (intersections.empty())
  {
    // no match if any of the entry lanelet is not part of any intersection.
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching intersection for current lanelet. lanelet id = " << route_lanelet.id());
  }
  else
  {
    //Currently, each lanelet has only one intersection
    lanelet::Id intersection_id = intersections.front()->id();
    if(intersection_id != lanelet::InvalId)
    {
      for(auto itr = sim_->intersection_id_to_regem_id_.begin(); itr != sim_->intersection_id_to_regem_id_.end(); itr++)
      {
        if(itr->second == intersection_id)
        {
          map_msg_intersection_id = itr->first;
        }
      }
    }
  } //END intersections
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "MAP msg: Intersection ID = " <<  map_msg_intersection_id << ", Signal Group ID =" << cur_signal_group_id );
  if(map_msg_intersection_id != 0 && cur_signal_group_id != 0)
  {
    upcoming_intersection_ids_.data.clear();
    upcoming_intersection_ids_.data.push_back(static_cast<int>(map_msg_intersection_id));
    upcoming_intersection_ids_.data.push_back(static_cast<int>(cur_signal_group_id));
  }
}

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 474 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

Definition at line 415 of file WMBroadcaster.hpp.

Referenced by addGeofence(), checkActiveGeofenceLogic(), distToNearestActiveGeofence(), and removeGeofence().

active_pub_

PublishActiveGeofCallback carma_wm_ctrl::WMBroadcaster::active_pub_

private

Definition at line 444 of file WMBroadcaster.hpp.

Referenced by currentLocationCallback().

base_map_

lanelet::LaneletMapPtr carma_wm_ctrl::WMBroadcaster::base_map_

private

Definition at line 432 of file WMBroadcaster.hpp.

Referenced by baseMapCallback().

base_map_georef_

std::string carma_wm_ctrl::WMBroadcaster::base_map_georef_

private

Definition at line 447 of file WMBroadcaster.hpp.

Referenced by controlRequestFromRoute(), geoReferenceCallback(), and getPointsInLocalFrame().

cached_maps_

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

private

Definition at line 439 of file WMBroadcaster.hpp.

checked_geofence_ids_

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

private

Definition at line 437 of file WMBroadcaster.hpp.

Referenced by geofenceCallback().

config_limit

lanelet::Velocity carma_wm_ctrl::WMBroadcaster::config_limit

private

Definition at line 435 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 443 of file WMBroadcaster.hpp.

Referenced by routeCallbackMessage().

current_map_

lanelet::LaneletMapPtr carma_wm_ctrl::WMBroadcaster::current_map_

private

Definition at line 433 of file WMBroadcaster.hpp.

Referenced by addBackRegulatoryComponent(), addGeofence(), addGeofenceHelper(), addRegulatoryComponent(), baseMapCallback(), checkActiveGeofenceLogic(), controlRequestFromRoute(), createWorkzoneGeometry(), currentLocationCallback(), distToNearestActiveGeofence(), externalMapMsgCallback(), geofenceFromMapMsg(), geofenceFromMsg(), getAffectedLaneletOrAreas(), getPointsInLocalFrame(), preprocessWorkzoneGeometry(), publishLightId(), removeGeofence(), shouldChangeControlLine(), shouldChangeTrafficSignal(), splitLaneletWithRatio(), splitOppositeLaneletWithPoint(), and updateUpcomingSGIntersectionIds().

current_map_version_

size_t carma_wm_ctrl::WMBroadcaster::current_map_version_ = 0

private

Definition at line 455 of file WMBroadcaster.hpp.

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

current_route

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

private

Definition at line 457 of file WMBroadcaster.hpp.

Referenced by getRoute(), publishLightId(), routeCallbackMessage(), and updateUpcomingSGIntersectionIds().

current_routing_graph_

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

private

Definition at line 434 of file WMBroadcaster.hpp.

Referenced by addGeofence(), baseMapCallback(), geofenceFromMapMsg(), getAffectedLaneletOrAreas(), preprocessWorkzoneGeometry(), and removeGeofence().

error_distance_

double carma_wm_ctrl::WMBroadcaster::error_distance_ = 5

private

Definition at line 413 of file WMBroadcaster.hpp.

Referenced by createWorkzoneGeometry(), preprocessWorkzoneGeometry(), and setErrorDistance().

generated_geofence_reqids_

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

private

Definition at line 438 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 473 of file WMBroadcaster.hpp.

Referenced by pubTCMACK().

j2735_map_msg_marker_array_

visualization_msgs::msg::MarkerArray carma_wm_ctrl::WMBroadcaster::j2735_map_msg_marker_array_

Definition at line 408 of file WMBroadcaster.hpp.

Referenced by addGeofence().

map_mutex_

std::mutex carma_wm_ctrl::WMBroadcaster::map_mutex_

private

Definition at line 440 of file WMBroadcaster.hpp.

Referenced by addGeofence(), baseMapCallback(), distToNearestActiveGeofence(), geofenceCallback(), geoReferenceCallback(), and removeGeofence().

map_pub_

PublishMapCallback carma_wm_ctrl::WMBroadcaster::map_pub_

private

Definition at line 441 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 463 of file WMBroadcaster.hpp.

map_update_pub_

PublishMapUpdateCallback carma_wm_ctrl::WMBroadcaster::map_update_pub_

private

Definition at line 442 of file WMBroadcaster.hpp.

Referenced by addGeofence(), and removeGeofence().

max_lane_width_

double carma_wm_ctrl::WMBroadcaster::max_lane_width_

private

Definition at line 448 of file WMBroadcaster.hpp.

Referenced by getAffectedLaneletOrAreas(), and setMaxLaneWidth().

participant_

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

private

Definition at line 436 of file WMBroadcaster.hpp.

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

route_path_

lanelet::ConstLanelets carma_wm_ctrl::WMBroadcaster::route_path_

private

Definition at line 414 of file WMBroadcaster.hpp.

Referenced by controlRequestFromRoute(), and distToNearestActiveGeofence().

scheduler_

GeofenceScheduler carma_wm_ctrl::WMBroadcaster::scheduler_

private

Definition at line 445 of file WMBroadcaster.hpp.

Referenced by WMBroadcaster(), addScheduleFromMsg(), controlRequestFromRoute(), pubTCMACK(), and scheduleGeofence().

sim_

std::shared_ptr<carma_wm::SignalizedIntersectionManager> carma_wm_ctrl::WMBroadcaster::sim_

private

Definition at line 467 of file WMBroadcaster.hpp.

Referenced by addGeofence(), addRegulatoryComponent(), externalMapMsgCallback(), geofenceFromMapMsg(), geoReferenceCallback(), setIntersectionCoordCorrection(), setMaxLaneWidth(), shouldChangeTrafficSignal(), and updateUpcomingSGIntersectionIds().

tcm_ack_pub_

PublishMobilityOperationCallback carma_wm_ctrl::WMBroadcaster::tcm_ack_pub_

private

Definition at line 446 of file WMBroadcaster.hpp.

Referenced by pubTCMACK().

tcm_marker_array_

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

Definition at line 407 of file WMBroadcaster.hpp.

Referenced by addGeofence().

tcr_polygon_

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

Definition at line 409 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

Definition at line 417 of file WMBroadcaster.hpp.

Referenced by addGeofenceHelper(), convertLightIdToInterGroupId(), and publishLightId().

upcoming_intersection_ids_

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

Definition at line 410 of file WMBroadcaster.hpp.

Referenced by publishLightId(), and updateUpcomingSGIntersectionIds().

update_count_

size_t carma_wm_ctrl::WMBroadcaster::update_count_ = -1

private

Definition at line 465 of file WMBroadcaster.hpp.

Referenced by addGeofence(), and removeGeofence().

vehicle_id_

std::string carma_wm_ctrl::WMBroadcaster::vehicle_id_

private

Definition at line 475 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 416 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 450 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 449 of file WMBroadcaster.hpp.

---

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

• carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp
• carma_wm_ctrl/src/WMBroadcaster.cpp