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

#include <WMBroadcasterNode.hpp>

Inheritance diagram for carma_wm_ctrl::WMBroadcasterNode:

Collaboration diagram for carma_wm_ctrl::WMBroadcasterNode:

Public Member Functions
	WMBroadcasterNode (const rclcpp::NodeOptions &options)
	Constructor. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &)

void	publishMap (const autoware_lanelet2_msgs::msg::MapBin &map_msg)
	Callback to publish a map. More...

void	initializeWorker (std::weak_ptr< carma_ros2_utils::CarmaLifecycleNode > weak_node_pointer)
	Initializes the WMBroadcaster worker with reference to the CarmaLifecycleNode itself. More...

void	publishCtrlReq (const carma_v2x_msgs::msg::TrafficControlRequest &ctrlreq_msg) const
	Callback to publish TrafficControlRequest Messages. More...

void	publishMapUpdate (const autoware_lanelet2_msgs::msg::MapBin &geofence_msg) const
	Callback to publish map updates (geofences) More...

void	publishActiveGeofence (const carma_perception_msgs::msg::CheckActiveGeofence &active_geof_msg)
	Callback to publish active geofence information. More...

void	publishTCMACK (const carma_v2x_msgs::msg::MobilityOperation &mom_msg)
	Callback to publish traffic control acknowledgement information. More...

Private Member Functions
bool	spin_callback ()
	Spin callback, which will be called frequently based on the configured spin rate. More...

Private Attributes
Config	config_

carma_ros2_utils::PubPtr< autoware_lanelet2_msgs::msg::MapBin >	map_pub_

carma_ros2_utils::PubPtr< autoware_lanelet2_msgs::msg::MapBin >	map_update_pub_

carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::TrafficControlRequest >	control_msg_pub_

carma_ros2_utils::PubPtr< visualization_msgs::msg::MarkerArray >	tcm_visualizer_pub_

carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::TrafficControlRequestPolygon >	tcr_visualizer_pub_

carma_ros2_utils::PubPtr< std_msgs::msg::Int32MultiArray >	upcoming_intersection_ids_pub_

carma_ros2_utils::PubPtr< carma_perception_msgs::msg::CheckActiveGeofence >	active_pub_

carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation >	tcm_ack_pub_

carma_ros2_utils::SubPtr< autoware_lanelet2_msgs::msg::MapBin >	base_map_sub_

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::Route >	route_callmsg_sub_

carma_ros2_utils::SubPtr< std_msgs::msg::String >	georef_sub_

carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::TrafficControlMessage >	geofence_sub_

carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MapData >	incoming_map_sub_

carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped >	curr_location_sub_

rclcpp::TimerBase::SharedPtr	timer_

std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode >	ptr_

std::unique_ptr< carma_wm_ctrl::WMBroadcaster >	wmb_

Detailed Description

Node which provies exposes map publication and carma_wm update logic.

The WMBroadcasterNode 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 45 of file WMBroadcasterNode.hpp.

Constructor & Destructor Documentation

◆ WMBroadcasterNode()

carma_wm_ctrl::WMBroadcasterNode::WMBroadcasterNode ( const rclcpp::NodeOptions & options )

explicit

Constructor.

Definition at line 52 of file WMBroadcasterNode.cpp.

  : carma_ros2_utils::CarmaLifecycleNode(options)
{
  // Create initial config
  config_ = Config();
 
  config_.ack_pub_times = declare_parameter<int>("ack_pub_times", config_.ack_pub_times);
  config_.max_lane_width = declare_parameter<double>("max_lane_width", config_.max_lane_width);
  config_.traffic_control_request_period = declare_parameter<double>("traffic_control_request_period", config_.traffic_control_request_period);
  config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
  config_.participant = declare_parameter<std::string>("vehicle_participant_type", config_.participant);
  config_.participant = declare_parameter<double>("config_speed_limit", config_.config_limit);
 
  declare_parameter("intersection_ids_for_correction");
  declare_parameter("intersection_coord_correction");
};

References carma_wm_ctrl::Config::ack_pub_times, config_, carma_wm_ctrl::Config::config_limit, carma_wm_ctrl::Config::max_lane_width, carma_wm_ctrl::Config::participant, carma_wm_ctrl::Config::traffic_control_request_period, and carma_wm_ctrl::Config::vehicle_id.

Member Function Documentation

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn carma_wm_ctrl::WMBroadcasterNode::handle_on_activate ( const rclcpp_lifecycle::State & prev_state )

Definition at line 153 of file WMBroadcasterNode.cpp.

{
  // Timer setup
  timer_ = create_timer(get_clock(),
                          std::chrono::milliseconds((int)(config_.traffic_control_request_period * 1000)),
                          std::bind(&WMBroadcasterNode::spin_callback, this));
 
  //SUBSCRIBERS
 
  // Base Map Sub
  base_map_sub_ = create_subscription<autoware_lanelet2_msgs::msg::MapBin>("base_map", 1, std::bind(&WMBroadcaster::baseMapCallback, wmb_.get(), std_ph::_1));
 
  // Base Map Georeference Sub
  georef_sub_ = create_subscription<std_msgs::msg::String>("georeference", 1, std::bind(&WMBroadcaster::geoReferenceCallback, wmb_.get(), std_ph::_1));
 
  // Geofence Sub
  rclcpp::SubscriptionOptions geofence_sub_options;
  // NOTE: Currently, intra-process comms must be disabled for subscribers that are transient_local: https://github.com/ros2/rclcpp/issues/1753
  geofence_sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
  auto sub_qos_transient_local = rclcpp::QoS(rclcpp::KeepAll());
  sub_qos_transient_local.transient_local();
  geofence_sub_ = create_subscription<carma_v2x_msgs::msg::TrafficControlMessage>("geofence", sub_qos_transient_local, std::bind(&WMBroadcaster::geofenceCallback, wmb_.get(), std_ph::_1), geofence_sub_options);
 
  // External Map Msg Sub
  incoming_map_sub_ = create_subscription<carma_v2x_msgs::msg::MapData>("incoming_map", 20, std::bind(&WMBroadcaster::externalMapMsgCallback, wmb_.get(), std_ph::_1));
 
  //Route Message Sub
  route_callmsg_sub_ = create_subscription<carma_planning_msgs::msg::Route>("route", 1, std::bind(&WMBroadcaster::routeCallbackMessage, wmb_.get(), std_ph::_1));
 
  //Current Location Sub
  curr_location_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 1, std::bind(&WMBroadcaster::currentLocationCallback, wmb_.get(), std_ph::_1));
 
 
  return CallbackReturn::SUCCESS;
}

References base_map_sub_, carma_wm_ctrl::WMBroadcaster::baseMapCallback(), config_, curr_location_sub_, carma_wm_ctrl::WMBroadcaster::currentLocationCallback(), carma_wm_ctrl::WMBroadcaster::externalMapMsgCallback(), geofence_sub_, carma_wm_ctrl::WMBroadcaster::geofenceCallback(), georef_sub_, carma_wm_ctrl::WMBroadcaster::geoReferenceCallback(), incoming_map_sub_, route_callmsg_sub_, carma_wm_ctrl::WMBroadcaster::routeCallbackMessage(), spin_callback(), timer_, carma_wm_ctrl::Config::traffic_control_request_period, and wmb_.

Here is the call graph for this function:

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn carma_wm_ctrl::WMBroadcasterNode::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 77 of file WMBroadcasterNode.cpp.

{
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_mw_ctrl"),"Starting configuration!");
 
  // Reset config
  config_ = Config();
 
  initializeWorker(shared_from_this());
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_mw_ctrl"),"Done initializing worker!");
 
  get_parameter<int>("ack_pub_times", config_.ack_pub_times);
  get_parameter<double>("max_lane_width", config_.max_lane_width);
  get_parameter<double>("traffic_control_request_period", config_.traffic_control_request_period);
  get_parameter<std::string>("vehicle_id", config_.vehicle_id);
  get_parameter<std::string>("vehicle_participant_type", config_.participant);
  get_parameter<double>("config_speed_limit", config_.config_limit);
 
  wmb_->setConfigACKPubTimes(config_.ack_pub_times);
  wmb_->setMaxLaneWidth(config_.max_lane_width);
  wmb_->setConfigSpeedLimit(config_.config_limit);
  wmb_->setConfigVehicleId(config_.vehicle_id);
  wmb_->setVehicleParticipationType(config_.participant);
 
  rclcpp::Parameter intersection_coord_correction_param = get_parameter("intersection_coord_correction");
  config_.intersection_coord_correction = intersection_coord_correction_param.as_double_array();
 
  rclcpp::Parameter intersection_ids_for_correction_param = get_parameter("intersection_ids_for_correction");
  config_.intersection_ids_for_correction = intersection_ids_for_correction_param.as_integer_array();
 
  wmb_->setIntersectionCoordCorrection(config_.intersection_ids_for_correction, config_.intersection_coord_correction);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_mw_ctrl"),"Done loading parameters: " << config_);
 
  // PUBLISHERS
 
  // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753
  rclcpp::PublisherOptions intra_proc_disabled;
  intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object
 
  // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too
  auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepAll()); // A publisher with this QoS will store all messages that it has sent on the topic
  pub_qos_transient_local.transient_local();  // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers
                                         // NOTE: The subscriber's QoS must be set to transient_local() as well for earlier messages to be resent to the later-joiner.
 
  // Map Update Publisher
  map_update_pub_ = create_publisher<autoware_lanelet2_msgs::msg::MapBin>("map_update", pub_qos_transient_local, intra_proc_disabled);
 
  // Map Publisher
  map_pub_ = create_publisher<autoware_lanelet2_msgs::msg::MapBin>("semantic_map", pub_qos_transient_local, intra_proc_disabled);
 
  //Route Message Publisher
  control_msg_pub_= create_publisher<carma_v2x_msgs::msg::TrafficControlRequest>("outgoing_geofence_request", 1);
 
  //Check Active Geofence Publisher
  active_pub_ = create_publisher<carma_perception_msgs::msg::CheckActiveGeofence>("active_geofence", 200);
 
  //publish TCM acknowledgement after processing TCM
  tcm_ack_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityOperation>("outgoing_geofence_ack", 2 * config_.ack_pub_times );
 
  //TCM Visualizer pub
  tcm_visualizer_pub_= create_publisher<visualization_msgs::msg::MarkerArray>("tcm_visualizer",1);
 
  //TCR Visualizer pub (visualized on UI)
  tcr_visualizer_pub_ = create_publisher<carma_v2x_msgs::msg::TrafficControlRequestPolygon>("tcr_bounding_points",1);
 
  //Upcoming intersection and group id of traffic light
  upcoming_intersection_ids_pub_ = create_publisher<std_msgs::msg::Int32MultiArray>("intersection_signal_group_ids", 1);
 
  // Return success if everything initialized successfully
 
  return CallbackReturn::SUCCESS;
}

Here is the call graph for this function:

◆ initializeWorker()

void carma_wm_ctrl::WMBroadcasterNode::initializeWorker ( std::weak_ptr< carma_ros2_utils::CarmaLifecycleNode > weak_node_pointer )

Initializes the WMBroadcaster worker with reference to the CarmaLifecycleNode itself.

Parameters

weak_ptr to the node of type CarmaLifecycleNode that owns this worker

Definition at line 69 of file WMBroadcasterNode.cpp.

{
  wmb_ = std::make_unique<carma_wm_ctrl::WMBroadcaster>(std::bind(&WMBroadcasterNode::publishMap, this, std_ph::_1), std::bind(&WMBroadcasterNode::publishMapUpdate, this, std_ph::_1),
  std::bind(&WMBroadcasterNode::publishCtrlReq, this, std_ph::_1), std::bind(&WMBroadcasterNode::publishActiveGeofence, this, std_ph::_1),
    std::make_unique<carma_ros2_utils::timers::ROSTimerFactory>(weak_node_pointer),
    std::bind(&WMBroadcasterNode::publishTCMACK, this, std_ph::_1));
}

References publishActiveGeofence(), publishCtrlReq(), publishMap(), publishMapUpdate(), publishTCMACK(), and wmb_.

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publishActiveGeofence()

void carma_wm_ctrl::WMBroadcasterNode::publishActiveGeofence ( const carma_perception_msgs::msg::CheckActiveGeofence & active_geof_msg )

Callback to publish active geofence information.

Parameters

active_geof_msg The geofence information to publish

Definition at line 42 of file WMBroadcasterNode.cpp.

{
  active_pub_->publish(active_geof_msg);
}

References active_pub_.

Referenced by initializeWorker().

Here is the caller graph for this function:

◆ publishCtrlReq()

void carma_wm_ctrl::WMBroadcasterNode::publishCtrlReq ( const carma_v2x_msgs::msg::TrafficControlRequest & ctrlreq_msg ) const

Callback to publish TrafficControlRequest Messages.

Parameters

route_msg The TrafficControlRequest message to publish

Definition at line 37 of file WMBroadcasterNode.cpp.

{
  control_msg_pub_->publish(ctrlreq_msg);
}

References control_msg_pub_.

Referenced by initializeWorker().

Here is the caller graph for this function:

◆ publishMap()

void carma_wm_ctrl::WMBroadcasterNode::publishMap ( const autoware_lanelet2_msgs::msg::MapBin & map_msg )

Callback to publish a map.

Parameters

map_msg The map message to publish

Definition at line 27 of file WMBroadcasterNode.cpp.

{
  map_pub_->publish(map_msg);
}

References map_pub_.

Referenced by initializeWorker().

Here is the caller graph for this function:

◆ publishMapUpdate()

void carma_wm_ctrl::WMBroadcasterNode::publishMapUpdate ( const autoware_lanelet2_msgs::msg::MapBin & geofence_msg ) const

Callback to publish map updates (geofences)

Parameters

geofence_msg The geofence message to publish

Definition at line 32 of file WMBroadcasterNode.cpp.

{
  map_update_pub_->publish(geofence_msg);
}

References map_update_pub_.

Referenced by initializeWorker().

Here is the caller graph for this function:

◆ publishTCMACK()

void carma_wm_ctrl::WMBroadcasterNode::publishTCMACK ( const carma_v2x_msgs::msg::MobilityOperation & mom_msg )

Callback to publish traffic control acknowledgement information.

Parameters

mom_msg The acknowledgement information to publish

Definition at line 47 of file WMBroadcasterNode.cpp.

{
  tcm_ack_pub_->publish(mom_msg);
}

References tcm_ack_pub_.

Referenced by initializeWorker().

Here is the caller graph for this function:

◆ spin_callback()

bool carma_wm_ctrl::WMBroadcasterNode::spin_callback ( )

private

Spin callback, which will be called frequently based on the configured spin rate.

Definition at line 191 of file WMBroadcasterNode.cpp.

{
  tcm_visualizer_pub_->publish(wmb_->tcm_marker_array_);
  tcr_visualizer_pub_->publish(wmb_->tcr_polygon_);
  wmb_->publishLightId();
  //updating upcoming traffic signal group id and intersection id
  wmb_->updateUpcomingSGIntersectionIds();
  if (wmb_->upcoming_intersection_ids_.data.size() > 0)
    upcoming_intersection_ids_pub_->publish(wmb_->upcoming_intersection_ids_);
  if(wmb_->getRoute().route_path_lanelet_ids.size() > 0)
    wmb_->routeCallbackMessage(std::make_unique<carma_planning_msgs::msg::Route>(wmb_->getRoute()));
 
  return true;
}