Class which provies automated subscription and threading support for the world model. More...

#include <WMListener.hpp>

Collaboration diagram for carma_wm::WMListener:

Public Member Functions
	WMListener (rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_, bool multi_thread=false)
	Constructor which can be used to specify threading behavior of this class. More...

	~WMListener ()
	Destructor. More...

WorldModelConstPtr	getWorldModel ()
	Returns a pointer to an intialized world model instance. More...

void	setMapCallback (std::function< void()> callback)
	Allows user to set a callback to be triggered when a map update is received NOTE: If operating in multi-threaded mode the world model will remain locked until the user function completes. More...

void	setRouteCallback (std::function< void()> callback)
	Allows user to set a callback to be triggered when a route update is received NOTE: If operating in multi-threaded mode the world model will remain locked until the user function completes. More...

std::unique_lock< std::mutex >	getLock (bool pre_locked=true)
	Returns a unique_lock which can be used to lock world model updates until the user finishes a desired operation. This function should be used when multiple queries are needed and this object is operating in multi-threaded mode. More...

void	setConfigSpeedLimit (double config_lim) const
	Allows user to set a callback to be triggered when a route update is received NOTE: If operating in multi-threaded mode the world model will remain locked until the user function completes. More...

void	enableUpdatesWithoutRouteWL ()
	Use to allow updates to occur even if they invalidate the current route. This is only meant to be used by components which generate the route. More...

bool	checkIfReRoutingNeededWL ()
	Returns true if a map update has been processed which requires rerouting. This method is meant to be used by routing components. More...

Private Member Functions
void	mapUpdateCallback (autoware_lanelet2_msgs::msg::MapBin::SharedPtr geofence_msg)

Private Attributes
carma_ros2_utils::SubPtr< carma_perception_msgs::msg::RoadwayObstacleList >	roadway_objects_sub_

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

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr	node_base_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	node_logging_

rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr	node_topics_

rclcpp::node_interfaces::NodeParametersInterface::SharedPtr	node_params_

std::unique_ptr< WMListenerWorker >	worker_

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

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

carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::SPAT >	traffic_spat_sub_

carma_ros2_utils::SubPtr< rosgraph_msgs::msg::Clock >	sim_clock_sub_

carma_ros2_utils::SubPtr< rosgraph_msgs::msg::Clock >	ros1_clock_sub_

const bool	multi_threaded_

std::mutex	mw_mutex_

Detailed Description

Class which provies automated subscription and threading support for the world model.

Users should generally create and cache a single instance of this class within a node. They can then retrieve a pointer to an initialized WorldModel object for doing queries. By default this class follows the threading model of the host node, but it can operate in the background if specified in the constructor. When used in a multi-threading case users can ensure threadsafe operation though usage of the getLock function

Definition at line 46 of file WMListener.hpp.

Constructor & Destructor Documentation

◆ WMListener()

carma_wm::WMListener::WMListener	(	rclcpp::node_interfaces::NodeBaseInterface::SharedPtr	node_base,
		rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	node_logging,
		rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr	node_topics,
		rclcpp::node_interfaces::NodeParametersInterface::SharedPtr	node_params_,
		bool	multi_thread = `false`
	)

Constructor which can be used to specify threading behavior of this class.

By default this object follows node threading behavior (ie. waiting for ros::spin()) If the object is operating in multi-threaded mode a ros::AsyncSpinner is used to implement a background thread.

Parameters

multi_thread If true this object will subscribe using background threads. Defaults to false

Definition at line 25 of file WMListener.cpp.

    :node_base_(node_base),node_logging_(node_logging),node_topics_(node_topics),worker_(std::unique_ptr<WMListenerWorker>(new WMListenerWorker)), multi_threaded_(multi_thread)
{
 
  RCLCPP_DEBUG_STREAM(node_logging_->get_logger(), "WMListener: Creating world model listener");
 
  //Declare parameter if it doesn't exist
  rclcpp::Parameter config_speed_limit_param("config_speed_limit");
  if(!node_params_->get_parameter("config_speed_limit", config_speed_limit_param)){
    rclcpp::ParameterValue config_speed_limit_param_value;
    config_speed_limit_param_value = node_params_->declare_parameter("config_speed_limit", rclcpp::ParameterValue (0.0));
  }
 
  rclcpp::Parameter participant_param("vehicle_participant_type");
  if(!node_params_->get_parameter("vehicle_participant_type", participant_param)){
    rclcpp::ParameterValue participant_param_value;
    participant_param_value = node_params_->declare_parameter("vehicle_participant_type", rclcpp::ParameterValue(""));
  }
 
  //Declare parameter if it doesn't exist
  rclcpp::Parameter use_sim_time_param("use_sim_time");
  if(!node_params_->get_parameter("use_sim_time", use_sim_time_param)){
    rclcpp::ParameterValue use_sim_time_param_value;
    use_sim_time_param_value = node_params_->declare_parameter("use_sim_time", rclcpp::ParameterValue (false));
  }
 
  rclcpp::Parameter is_spat_wall_time_param("is_spat_wall_time");
  if(!node_params_->get_parameter("is_spat_wall_time", is_spat_wall_time_param)){
    rclcpp::ParameterValue is_spat_wall_time_param_value;
    is_spat_wall_time_param_value = node_params_->declare_parameter("is_spat_wall_time", rclcpp::ParameterValue (true));
  }
 
  // Get params
  config_speed_limit_param = node_params_->get_parameter("config_speed_limit");
  participant_param = node_params_->get_parameter("vehicle_participant_type");
  use_sim_time_param = node_params_->get_parameter("use_sim_time");
  is_spat_wall_time_param = node_params_->get_parameter("is_spat_wall_time");
 
  RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded config speed limit: " << config_speed_limit_param.as_double());
  RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded vehicle participant type: " << participant_param.as_string());
  RCLCPP_INFO_STREAM(node_logging->get_logger(), "Is using simulation time? : " << use_sim_time_param.as_bool());
  RCLCPP_INFO_STREAM(node_logging->get_logger(), "Is SPaT using wall time? : " << is_spat_wall_time_param.as_bool());
 
 
  setConfigSpeedLimit(config_speed_limit_param.as_double());
  worker_->setVehicleParticipationType(participant_param.as_string());
  worker_->isUsingSimTime(use_sim_time_param.as_bool());
  worker_->isSpatWallTime(is_spat_wall_time_param.as_bool());
 
  rclcpp::SubscriptionOptions map_update_options;
  rclcpp::SubscriptionOptions map_options;
  rclcpp::SubscriptionOptions route_options;
  rclcpp::SubscriptionOptions roadway_objects_options;
  rclcpp::SubscriptionOptions traffic_spat_options;
  rclcpp::SubscriptionOptions ros1_clock_options;
  rclcpp::SubscriptionOptions sim_clock_options;
 
  if(multi_threaded_)
  {
    RCLCPP_DEBUG_STREAM(node_logging_->get_logger(), "WMListener: Using a multi-threaded subscription");
 
    map_update_options.callback_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
    map_options.callback_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
    route_options.callback_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
    roadway_objects_options.callback_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
    traffic_spat_options.callback_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
    ros1_clock_options.callback_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
    sim_clock_options.callback_group = node_base_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
  }
 
  // Setup subscribers
  route_sub_ = rclcpp::create_subscription<carma_planning_msgs::msg::Route>(node_topics_, "route", 1,
                                  [this](const carma_planning_msgs::msg::Route::SharedPtr msg)
                                  {
                                    this->worker_->routeCallback(msg);
                                  }
                                  , route_options);
 
  ros1_clock_sub_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(node_topics_, "/clock", 1,
                                  [this](const rosgraph_msgs::msg::Clock::SharedPtr msg)
                                  {
                                    this->worker_->ros1ClockCallback(msg);
                                  }
                                  , ros1_clock_options);
 
  sim_clock_sub_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(node_topics_, "/sim_clock", 1,
                                  [this](const rosgraph_msgs::msg::Clock::SharedPtr msg)
                                  {
                                    this->worker_->simClockCallback(msg);
                                  }
                                  , sim_clock_options);
 
  roadway_objects_sub_ = rclcpp::create_subscription<carma_perception_msgs::msg::RoadwayObstacleList>(node_topics_, "roadway_objects", 1,
                                  [this](const carma_perception_msgs::msg::RoadwayObstacleList::SharedPtr msg)
                                  {
                                    this->worker_->roadwayObjectListCallback(msg);
                                  }
                                  , roadway_objects_options);
 
  traffic_spat_sub_ = rclcpp::create_subscription<carma_v2x_msgs::msg::SPAT>(node_topics_, "incoming_spat", 1,
                                  [this](const carma_v2x_msgs::msg::SPAT::SharedPtr msg)
                                  {
                                    this->worker_->incomingSpatCallback(msg);
                                  }
                                  , traffic_spat_options);
 
  // NOTE: Currently, intra-process comms must be disabled for subscribers that are transient_local: https://github.com/ros2/rclcpp/issues/1753
  map_update_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for the map update subscriber
  auto map_update_sub_qos = rclcpp::QoS(rclcpp::KeepLast(10)); // Set the queue size for the map update subscriber
  map_update_sub_qos.transient_local();  // If it is possible that this node is a late-joiner to its topic, it must be set to transient_local to receive earlier messages that were missed.
                                         // NOTE: The publisher's QoS must be set to transisent_local() as well for earlier messages to be resent to this later-joiner.
 
  // Create map update subscriber that will receive earlier messages that were missed ONLY if the publisher is transient_local too
  map_update_sub_ = rclcpp::create_subscription<autoware_lanelet2_msgs::msg::MapBin>(node_topics_, "map_update", map_update_sub_qos,
                  [this](const autoware_lanelet2_msgs::msg::MapBin::SharedPtr msg)
                  {
                    this->mapUpdateCallback(msg);
                  }
                  , map_update_options);
 
  map_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for the semantic map subscriber
  auto map_sub_qos = rclcpp::QoS(rclcpp::KeepLast(2)); // Set the queue size for the semantic map subscriber
  map_sub_qos.transient_local();  // If it is possible that this node is a late-joiner to its topic, it must be set to transient_local to receive earlier messages that were missed.
                                  // NOTE: The publisher's QoS must be set to transisent_local() as well for earlier messages to be resent to this later-joiner.
 
  // Create semantic mab subscriber that will receive earlier messages that were missed ONLY if the publisher is transient_local too
  map_sub_ = rclcpp::create_subscription<autoware_lanelet2_msgs::msg::MapBin>(node_topics_, "semantic_map", map_sub_qos,
                  [this](const autoware_lanelet2_msgs::msg::MapBin::SharedPtr msg)
                  {
                    this->worker_->mapCallback(msg);
                  }
                  , map_options);
}

References map_sub_, map_update_sub_, mapUpdateCallback(), multi_threaded_, node_base_, node_logging_, node_params_, node_topics_, roadway_objects_sub_, ros1_clock_sub_, route_sub_, setConfigSpeedLimit(), sim_clock_sub_, traffic_spat_sub_, and worker_.

Here is the call graph for this function:

◆ ~WMListener()

carma_wm::WMListener::~WMListener ( )

Destructor.

Definition at line 170 of file WMListener.cpp.

170{}

Member Function Documentation

◆ checkIfReRoutingNeededWL()

bool carma_wm::WMListener::checkIfReRoutingNeededWL ( )

Returns true if a map update has been processed which requires rerouting. This method is meant to be used by routing components.

Returns: True is rerouting is needed

Definition at line 178 of file WMListener.cpp.

{
  const std::lock_guard<std::mutex> lock(mw_mutex_);
  return worker_->checkIfReRoutingNeeded();
}

References mw_mutex_, and worker_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ enableUpdatesWithoutRouteWL()

void carma_wm::WMListener::enableUpdatesWithoutRouteWL ( )

Use to allow updates to occur even if they invalidate the current route. This is only meant to be used by components which generate the route.

Definition at line 172 of file WMListener.cpp.

{
  const std::lock_guard<std::mutex> lock(mw_mutex_);
  worker_->enableUpdatesWithoutRoute();
}

References mw_mutex_, and worker_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ getLock()

std::unique_lock< std::mutex > carma_wm::WMListener::getLock ( bool pre_locked = true )

Returns a unique_lock which can be used to lock world model updates until the user finishes a desired operation. This function should be used when multiple queries are needed and this object is operating in multi-threaded mode.

Parameters

pre_locked If true the returned lock will already be locked. If false the lock will be deferred with std::defer_lock Default is true

Returns: std::unique_lock for thread safe access to world model data

Definition at line 211 of file WMListener.cpp.

{
  if (pre_locked)
  {
    std::unique_lock<std::mutex> lock(mw_mutex_);  // Create movable lock
    return lock;
  }
  std::unique_lock<std::mutex> lock(mw_mutex_, std::defer_lock);  // Create movable deferred lock
  return lock;
}

References mw_mutex_.

◆ getWorldModel()

WorldModelConstPtr carma_wm::WMListener::getWorldModel ( )

Returns a pointer to an intialized world model instance.

Returns: Const pointer to a world model object

Definition at line 184 of file WMListener.cpp.

{
  const std::lock_guard<std::mutex> lock(mw_mutex_);
  return worker_->getWorldModel();
}

References mw_mutex_, and worker_.

Referenced by plan_delegator::PlanDelegator::handle_on_configure(), and route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ mapUpdateCallback()

void carma_wm::WMListener::mapUpdateCallback ( autoware_lanelet2_msgs::msg::MapBin::SharedPtr geofence_msg )

private

Definition at line 190 of file WMListener.cpp.

{
  const std::lock_guard<std::mutex> lock(mw_mutex_);
 
  RCLCPP_INFO_STREAM(node_logging_->get_logger(), "New Map Update Received. SeqNum: " << geofence_msg->seq_id);
 
  worker_->mapUpdateCallback(geofence_msg);
}

References mw_mutex_, node_logging_, and worker_.

Referenced by WMListener().

Here is the caller graph for this function:

◆ setConfigSpeedLimit()

void carma_wm::WMListener::setConfigSpeedLimit ( double config_lim ) const

Allows user to set a callback to be triggered when a route update is received NOTE: If operating in multi-threaded mode the world model will remain locked until the user function completes.

Parameters

config_lim A function that populate the configurable speed limit value after the world model is updated with a new route

Definition at line 222 of file WMListener.cpp.

{
  worker_->setConfigSpeedLimit(config_lim);
}

References worker_.

Referenced by WMListener().

Here is the caller graph for this function:

◆ setMapCallback()

void carma_wm::WMListener::setMapCallback ( std::function< void()> callback )

Allows user to set a callback to be triggered when a map update is received NOTE: If operating in multi-threaded mode the world model will remain locked until the user function completes.

Parameters

callback A callback function that will be triggered after the world model receives a new map update

Definition at line 199 of file WMListener.cpp.

{
  const std::lock_guard<std::mutex> lock(mw_mutex_);
  worker_->setMapCallback(callback);
}

References callback(), mw_mutex_, and worker_.

Here is the call graph for this function:

◆ setRouteCallback()

void carma_wm::WMListener::setRouteCallback ( std::function< void()> callback )

Allows user to set a callback to be triggered when a route update is received NOTE: If operating in multi-threaded mode the world model will remain locked until the user function completes.

Parameters

callback A callback function that will be triggered after the world model is updated with a new route

Definition at line 205 of file WMListener.cpp.

{
  const std::lock_guard<std::mutex> lock(mw_mutex_);
  worker_->setRouteCallback(callback);
}

References callback(), mw_mutex_, and worker_.

Here is the call graph for this function:

Referenced by WMListener(), checkIfReRoutingNeededWL(), enableUpdatesWithoutRouteWL(), getWorldModel(), mapUpdateCallback(), setConfigSpeedLimit(), setMapCallback(), and setRouteCallback().

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

carma_wm/include/carma_wm/WMListener.hpp
carma_wm/src/WMListener.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ WMListener()

◆ ~WMListener()

Member Function Documentation

◆ checkIfReRoutingNeededWL()

◆ enableUpdatesWithoutRouteWL()

◆ getLock()

◆ getWorldModel()

◆ mapUpdateCallback()

◆ setConfigSpeedLimit()

◆ setMapCallback()

◆ setRouteCallback()

Member Data Documentation

◆ map_sub_

◆ map_update_sub_

◆ multi_threaded_

◆ mw_mutex_

◆ node_base_

◆ node_logging_

◆ node_params_

◆ node_topics_

◆ roadway_objects_sub_

◆ ros1_clock_sub_

◆ route_sub_

◆ sim_clock_sub_

◆ traffic_spat_sub_

◆ worker_