carma_wm::WMListenerWorker Class Reference

Backend logic class for WMListener. More...

#include <WMListenerWorker.hpp>

Collaboration diagram for carma_wm::WMListenerWorker:

Public Member Functions
	WMListenerWorker (rclcpp::Logger logger=rclcpp::get_logger("carma_wm"))
	Constructor. More...
WorldModelConstPtr	getWorldModel () const
	Constructor. More...
void	mapCallback (const autoware_lanelet2_msgs::msg::MapBin::SharedPtr map_msg)
	Callback for new map messages. Updates the underlying map. More...
void	mapUpdateCallback (autoware_lanelet2_msgs::msg::MapBin::SharedPtr geofence_msg)
	Callback for new map update messages (geofence). Updates the underlying map. More...
void	routeCallback (const carma_planning_msgs::msg::Route::SharedPtr route_msg)
	Callback for route message. More...
void	ros1ClockCallback (const rosgraph_msgs::msg::Clock::SharedPtr clock_msg)
	Callback for ROS1 clock message (used in Simulation runs) More...
void	simClockCallback (const rosgraph_msgs::msg::Clock::SharedPtr clock_msg)
	Callback for Simulation clock message (used in Simulation runs) More...
void	roadwayObjectListCallback (const carma_perception_msgs::msg::RoadwayObstacleList::SharedPtr msg)
	Callback for roadway objects msg. More...
void	setMapCallback (std::function< void()> callback)
	Allows user to set a callback to be triggered when a map update is received. More...
void	setRouteCallback (std::function< void()> callback)
	Allows user to set a callback to be triggered when a route update is received. More...
void	setConfigSpeedLimit (double config_lim)
	Allows user to set a callback to be triggered when a map update is received. More...
double	getConfigSpeedLimit () const
	Returns the current configured speed limit value. More...
void	setVehicleParticipationType (std::string participant)
	Allows user to set a callback to be triggered when a map update is received. More...
std::string	getVehicleParticipationType () const
	Returns the Vehicle Participation Type value. More...
bool	checkIfReRoutingNeeded () const
	Check if re-routing is needed and returns re-routing flag. More...
void	enableUpdatesWithoutRoute ()
	Enable updates without route and set route_node_flag_ as true. More...
LaneletRoutingGraphPtr	routingGraphFromMsg (const autoware_lanelet2_msgs::msg::RoutingGraph &msg, lanelet::LaneletMapPtr map) const
	Helper function to convert a routing graph message into a actual RoutingGraph object. More...
void	incomingSpatCallback (const carma_v2x_msgs::msg::SPAT::SharedPtr spat_msg)
	incoming spat message More...
void	isUsingSimTime (bool use_sim_time) const
	set true if simulation_mode is on More...
void	isSpatWallTime (bool use_real_time_spat_in_sim) const
	set true if incoming spat is based on wall clock More...
void	setWMSpatProcessingState (const SIGNAL_PHASE_PROCESSING &phase_type) const
	Activate World Model SPAT processor, which is turned off by default, with OFF (0), ON (1), FIXED (2) More...
SIGNAL_PHASE_PROCESSING	getWMSpatProcessingState () const
	Get World Model SPAT processor state from signalized intersection manager. More...

Private Member Functions
void	newRegemUpdateHelper (lanelet::Lanelet parent_llt, lanelet::RegulatoryElement *regem) const
	This is a helper function updates the parent_llt with specified regem. This function is needed as we need to dynamic_cast from general regem to specific type of regem based on the geofence. More...

Private Attributes
rclcpp::Logger	logger_
std::shared_ptr< CARMAWorldModel >	world_model_
std::function< void()>	map_callback_
std::function< void()>	route_callback_
double	config_speed_limit_
size_t	current_map_version_ = 0
std::queue< autoware_lanelet2_msgs::msg::MapBin::SharedPtr >	map_update_queue_
boost::optional< carma_planning_msgs::msg::Route >	delayed_route_msg_
bool	recompute_route_flag_ =false
bool	rerouting_flag_ =false
bool	route_node_flag_ =false
long	most_recent_update_msg_seq_ = -1

Detailed Description

Backend logic class for WMListener.

Definition at line 35 of file WMListenerWorker.hpp.

Constructor & Destructor Documentation

WMListenerWorker()

carma_wm::WMListenerWorker::WMListenerWorker ( rclcpp::Logger  logger = rclcpp::get_logger("carma_wm") )

explicit

Constructor.

Parameters

logger

Logger to use for all log output. Pass the calling node's get_child("carma_wm") logger so messages appear in MCAP. Defaults to a standalone logger for backward compatibility.

Definition at line 45 of file WMListenerWorker.cpp.

: logger_(logger)
{
  world_model_.reset(new CARMAWorldModel);
}

References world_model_.

Member Function Documentation

checkIfReRoutingNeeded()

bool carma_wm::WMListenerWorker::checkIfReRoutingNeeded

(

)

const

Check if re-routing is needed and returns re-routing flag.

Definition at line 110 of file WMListenerWorker.cpp.

{
  return rerouting_flag_;
}

References rerouting_flag_.

enableUpdatesWithoutRoute()

void carma_wm::WMListenerWorker::enableUpdatesWithoutRoute

(

)

Enable updates without route and set route_node_flag_ as true.

Definition at line 115 of file WMListenerWorker.cpp.

{
  route_node_flag_=true;
}

References route_node_flag_.

getConfigSpeedLimit()

double carma_wm::WMListenerWorker::getConfigSpeedLimit

(

)

const

Returns the current configured speed limit value.

Definition at line 725 of file WMListenerWorker.cpp.

{
  return config_speed_limit_;
}

References config_speed_limit_.

getVehicleParticipationType()

std::string carma_wm::WMListenerWorker::getVehicleParticipationType

(

)

const

Returns the Vehicle Participation Type value.

Definition at line 569 of file WMListenerWorker.cpp.

{
  return world_model_->getVehicleParticipationType();
}

References world_model_.

Referenced by routingGraphFromMsg().

Here is the caller graph for this function:

getWMSpatProcessingState()

SIGNAL_PHASE_PROCESSING carma_wm::WMListenerWorker::getWMSpatProcessingState

(

)

const

Get World Model SPAT processor state from signalized intersection manager.

Returns

World Model SPAT process state OFF (0), ON(1)

Definition at line 720 of file WMListenerWorker.cpp.

{
  return world_model_->sim_.spat_processor_state_;
}

References world_model_.

getWorldModel()

WorldModelConstPtr carma_wm::WMListenerWorker::getWorldModel

(

)

const

Constructor.

Definition at line 51 of file WMListenerWorker.cpp.

{
  return std::static_pointer_cast<const WorldModel>(world_model_);  // Cast pointer to const variant
}

References world_model_.

incomingSpatCallback()

void carma_wm::WMListenerWorker::incomingSpatCallback

(

const carma_v2x_msgs::msg::SPAT::SharedPtr

spat_msg

)

incoming spat message

Definition at line 105 of file WMListenerWorker.cpp.

{
  world_model_->sim_.processSpatFromMsg(*spat_msg, world_model_->getMutableMap());
}

References world_model_.

isSpatWallTime()

void carma_wm::WMListenerWorker::isSpatWallTime

(

bool

use_real_time_spat_in_sim

)

const

set true if incoming spat is based on wall clock

Definition at line 709 of file WMListenerWorker.cpp.

{
  world_model_->sim_.use_real_time_spat_in_sim_ = use_real_time_spat_in_sim;
}

References world_model_.

isUsingSimTime()

void carma_wm::WMListenerWorker::isUsingSimTime

(

bool

use_sim_time

)

const

set true if simulation_mode is on

Definition at line 705 of file WMListenerWorker.cpp.

{
  world_model_->sim_.use_sim_time_ = use_sim_time;
}

References world_model_.

mapCallback()

void carma_wm::WMListenerWorker::mapCallback

(

const autoware_lanelet2_msgs::msg::MapBin::SharedPtr

map_msg

)

Callback for new map messages. Updates the underlying map.

Parameters

map_msg

The new map messages to generate the map from

Definition at line 57 of file WMListenerWorker.cpp.

{
  current_map_version_ = map_msg->map_version;
 
  lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap);
 
  lanelet::utils::conversion::fromBinMsg(*map_msg, new_map);
 
  world_model_->setMap(new_map, current_map_version_);
 
  // After setting map evaluate the current update queue to apply any updates that arrived before the map
  bool more_updates_to_apply = true;
  while(!map_update_queue_.empty() && more_updates_to_apply) {
 
    auto update = map_update_queue_.front(); // Get first update
    map_update_queue_.pop(); // Remove update from queue
 
    if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the previous map
      RCLCPP_WARN_STREAM(logger_, "There were unapplied updates in carma_wm when a new map was received.");
      continue;
    }
    if (update->map_version == current_map_version_) { // Current update goes with current map
      mapUpdateCallback(update); // Apply the update
    } else {
      RCLCPP_INFO_STREAM(logger_, "Done applying updates for new map. However, more updates are waiting for a future map.");
      more_updates_to_apply = false; // If there is more updates queued that are not for this map version assume they are for a future map version
    }
 
  }
 
  // Call user defined map callback
  if (map_callback_)
  {
    map_callback_();
  }
 
  if (delayed_route_msg_) {
    if (delayed_route_msg_.get().map_version == current_map_version_) { // If there is a delayed route message to apply then do so
      routeCallback(std::make_unique<carma_planning_msgs::msg::Route>(delayed_route_msg_.get()));
    } else if (delayed_route_msg_.get().map_version < current_map_version_) {
      RCLCPP_WARN_STREAM(logger_, "Dropping delayed route message which was never applied as updated map was not recieved");
      delayed_route_msg_ = boost::none;
    } else {
      RCLCPP_INFO_STREAM(logger_, "There is a delayed route message still waiting to be applied in carma_wm");
    }
  }
}

References current_map_version_, delayed_route_msg_, carma_wm::fromBinMsg(), logger_, map_callback_, map_update_queue_, mapUpdateCallback(), routeCallback(), and world_model_.

Here is the call graph for this function:

mapUpdateCallback()

void carma_wm::WMListenerWorker::mapUpdateCallback

(

autoware_lanelet2_msgs::msg::MapBin::SharedPtr

geofence_msg

)

Callback for new map update messages (geofence). Updates the underlying map.

Parameters

geofence_msg

The new map update messages to generate the map edits from

Definition at line 148 of file WMListenerWorker.cpp.

{
  RCLCPP_INFO_STREAM(logger_, "Map Update Being Evaluated. SeqNum: " << geofence_msg->seq_id);
  if (rerouting_flag_) // no update should be applied if rerouting
  {
    RCLCPP_INFO_STREAM(logger_, "Currently new route is being processed. Queueing this update. Received seq: " << geofence_msg->seq_id << " prev seq: " << most_recent_update_msg_seq_);
    map_update_queue_.emplace(geofence_msg);
    return;
  }
  if (geofence_msg->seq_id <= most_recent_update_msg_seq_) {
    RCLCPP_DEBUG_STREAM(logger_, "Dropping map update which has already been processed. Received seq: " << geofence_msg->seq_id << " prev seq: " << most_recent_update_msg_seq_);
    return;
  } else if(!world_model_->getMap() || current_map_version_ < geofence_msg->map_version) { // If our current map version is older than the version target by this update
    RCLCPP_DEBUG_STREAM(logger_, "Update received for newer map version than available. Queueing update until map is available.");
    map_update_queue_.emplace(geofence_msg);
    return;
  } else if (current_map_version_ > geofence_msg->map_version) { // If this update is for an older map
    RCLCPP_WARN_STREAM(logger_, "Dropping old map update as newer map is already available.");
    return;
  } else if (most_recent_update_msg_seq_ + 1 < geofence_msg->seq_id) {
    RCLCPP_INFO_STREAM(logger_, "Queuing map update as we are waiting on an earlier update to be applied. most_recent_update_msg_seq_: " << most_recent_update_msg_seq_ << "geofence_msg->seq_id: " << geofence_msg->seq_id);
    map_update_queue_.emplace(geofence_msg);
    return;
  }
 
 
  if(geofence_msg->invalidates_route==true && world_model_->getRoute())
  {
    rerouting_flag_=true;
    recompute_route_flag_ = true;
 
    RCLCPP_DEBUG_STREAM(logger_, "Received notice that route has been invalidated in mapUpdateCallback");
 
    if(route_node_flag_!=true)
    {
     RCLCPP_INFO_STREAM(logger_, "Route is not yet available. Therefore queueing the update");
     map_update_queue_.emplace(geofence_msg);
     return;
    }
  }
 
  most_recent_update_msg_seq_ = geofence_msg->seq_id; // Update current sequence count
 
  auto gf_ptr = std::shared_ptr<carma_wm::TrafficControl>(new carma_wm::TrafficControl);
 
  // convert ros msg to geofence object
  carma_wm::fromBinMsg(*geofence_msg, gf_ptr, world_model_->getMutableMap());
 
  RCLCPP_INFO_STREAM(logger_, "Processing Map Update with Geofence Id:" << gf_ptr->id_);
 
  RCLCPP_DEBUG_STREAM(logger_, "Geofence id" << gf_ptr->id_ << " requests addition of lanelets size: " << gf_ptr->lanelet_additions_.size());
  for (auto llt : gf_ptr->lanelet_additions_)
  {
    // world model here should blindly accept the map update received
    RCLCPP_DEBUG_STREAM(logger_, "Adding new lanelet with id: " << llt.id());
    auto left = llt.leftBound3d(); //new lanelet coming in
 
    // updating incoming points' memory addresses with local ones of same ids
    // so that lanelet library can recognize they are same objects
    for (size_t i = 0; i < left.size(); i ++)
    {
      if (world_model_->getMutableMap()->pointLayer.exists(left[i].id())) //rewrite the memory address of new pts with that of local
      {
        llt.leftBound3d()[i] = world_model_->getMutableMap()->pointLayer.get(left[i].id());
      }
    }
    auto right = llt.rightBound3d(); //new lanelet coming in
    for (size_t i = 0; i < right.size(); i ++)
    {
      if (world_model_->getMutableMap()->pointLayer.exists(right[i].id())) //rewrite the memory address of new pts with that of local
      {
        llt.rightBound3d()[i] = world_model_->getMutableMap()->pointLayer.get(right[i].id());
      }
    }
 
    world_model_->getMutableMap()->add(llt);
  }
 
  RCLCPP_DEBUG_STREAM(logger_, "Geofence id" << gf_ptr->id_ << " sends record of traffic_lights_id size: " << gf_ptr->traffic_light_id_lookup_.size());
  for (auto const &[traffic_light_id, lanelet_id] : gf_ptr->traffic_light_id_lookup_)
  {
    RCLCPP_DEBUG_STREAM(logger_, "Adding new pair for traffic light ids: " << traffic_light_id << ", and lanelet::Id: " << lanelet_id);
    world_model_->setTrafficLightIds(traffic_light_id, lanelet_id);
  }
 
  RCLCPP_DEBUG_STREAM(logger_, "Geofence id" << gf_ptr->id_ << " sends record of intersections size: " << gf_ptr->sim_.intersection_id_to_regem_id_.size());
  if (gf_ptr->sim_.intersection_id_to_regem_id_.size() > 0)
  {
    world_model_->sim_ = gf_ptr->sim_;
    logSignalizedIntersectionManager(world_model_->sim_, logger_);
  }
 
  RCLCPP_DEBUG_STREAM(logger_, "Geofence id" << gf_ptr->id_ << " requests removal of size: " << gf_ptr->remove_list_.size());
  for (auto const &[lanelet_id, lanelet_to_remove] : gf_ptr->remove_list_)
  {
    auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id);
    // we can only check by id, if the element is there
    // this is only for speed optimization, as world model here should blindly accept the map update received
    auto regems_copy_to_check = parent_llt.regulatoryElements(); // save local copy as the regem can be deleted during iteration
    RCLCPP_DEBUG_STREAM(logger_, "Regems found in lanelet: " << regems_copy_to_check.size());
    for (auto regem: regems_copy_to_check)
    {
      // we can't use the deserialized element as its data address conflicts the one in this node
      if (lanelet_to_remove->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem);
    }
    RCLCPP_DEBUG_STREAM(logger_, "Regems left in lanelet after removal: " << parent_llt.regulatoryElements().size());
 
  }
 
  RCLCPP_INFO_STREAM(logger_, "Geofence id" << gf_ptr->id_ << " requests update of size: " << gf_ptr->update_list_.size());
 
  // we should extract general regem to specific type of regem the geofence specifies
  for (auto const &[lanelet_id, lanelet_to_update]: gf_ptr->update_list_)
  {
 
    auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id);
 
    auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(lanelet_to_update->id());
 
    // if this regem is already in the map.
    // This section is expected to be called to add back regulations which were previously removed by expired geofences.
    if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end())
    {
 
      RCLCPP_DEBUG_STREAM(logger_, "Reapplying previously existing element for lanelet id:" << parent_llt.id() << ", and regem id: " << regemptr_it->get()->id());
      // again we should use the element with correct data address to be consistent
      world_model_->getMutableMap()->update(parent_llt, *regemptr_it);
    }
    else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block.
    {
      RCLCPP_DEBUG_STREAM(logger_, "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << lanelet_to_update->id());
      newRegemUpdateHelper(parent_llt, lanelet_to_update.get());
    }
  }
 
  // set the Map to trigger a new route graph construction if rerouting was required by the updates and a new graph was not provided
  world_model_->setMap(world_model_->getMutableMap(), current_map_version_, recompute_route_flag_ && !geofence_msg->has_routing_graph );
 
  // If a new graph was provided then set that graph
  // recompute_route_flag_ not checked here to support the case of the first map or map version changing
  if (geofence_msg->has_routing_graph) {
 
    LaneletRoutingGraphPtr graph = routingGraphFromMsg(geofence_msg->routing_graph, world_model_->getMutableMap());
 
    if (!graph) {
      throw std::invalid_argument("Map updated provided routing graph which could not be applied to the current map.");
    }
 
    world_model_->setRoutingGraph(graph);
 
  }
 
  // no need to reroute again unless received invalidated msg again
  if (recompute_route_flag_)
    recompute_route_flag_ = false;
 
 
  RCLCPP_INFO_STREAM(logger_, "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_);
 
  // Call user defined map callback
  if (map_callback_)
  {
    RCLCPP_INFO_STREAM(logger_, "Calling user defined map update callback");
    map_callback_();
  }
}

References current_map_version_, carma_wm::fromBinMsg(), process_bag::i, logger_, carma_wm::logSignalizedIntersectionManager(), map_callback_, map_update_queue_, most_recent_update_msg_seq_, newRegemUpdateHelper(), recompute_route_flag_, rerouting_flag_, route_node_flag_, routingGraphFromMsg(), and world_model_.

Referenced by mapCallback(), and routeCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

newRegemUpdateHelper()

void carma_wm::WMListenerWorker::newRegemUpdateHelper ( lanelet::Lanelet  parent_llt, lanelet::RegulatoryElement *  regem  ) const

private

This is a helper function updates the parent_llt with specified regem. This function is needed as we need to dynamic_cast from general regem to specific type of regem based on the geofence.

Parameters

parent_llt	The Lanelet that need to register the regem
regem	lanelet::RegulatoryElement* which is the type that the serializer decodes from binary NOTE: Currently this function supports items in carma_wm::GeofenceType

Definition at line 323 of file WMListenerWorker.cpp.

{
  auto factory_regem = lanelet::RegulatoryElementFactory::create(regem->attribute(lanelet::AttributeName::Subtype).value(),
                                                            std::const_pointer_cast<lanelet::RegulatoryElementData>(regem->constData()));
 
  // we should extract general regem to specific type of regem the geofence specifies
  switch(resolveGeofenceType(regem->attribute(lanelet::AttributeName::Subtype).value()))
  {
    case GeofenceType::PASSING_CONTROL_LINE:
    {
      lanelet::PassingControlLinePtr control_line = std::dynamic_pointer_cast<lanelet::PassingControlLine>(factory_regem);
      if (control_line)
      {
        world_model_->getMutableMap()->update(parent_llt, control_line);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid control line");
      }
 
      break;
    }
    case GeofenceType::DIGITAL_SPEED_LIMIT:
    {
      lanelet::DigitalSpeedLimitPtr speed = std::dynamic_pointer_cast<lanelet::DigitalSpeedLimit>(factory_regem);
      if (speed)
      {
        world_model_->getMutableMap()->update(parent_llt, speed);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid speed limit");
      }
      break;
    }
    case GeofenceType::REGION_ACCESS_RULE:
    {
 
      lanelet::RegionAccessRulePtr rar = std::dynamic_pointer_cast<lanelet::RegionAccessRule>(factory_regem);
      if (rar)
      {
        world_model_->getMutableMap()->update(parent_llt, rar);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid region access rule");
      }
 
      break;
    }
    case GeofenceType::DIGITAL_MINIMUM_GAP:
    {
 
      lanelet::DigitalMinimumGapPtr min_gap = std::dynamic_pointer_cast<lanelet::DigitalMinimumGap>(factory_regem);
      if (min_gap)
      {
        world_model_->getMutableMap()->update(parent_llt, min_gap);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid minimum gap rule");
      }
 
      break;
    }
    case GeofenceType::DIRECTION_OF_TRAVEL:
    {
 
      lanelet::DirectionOfTravelPtr dot = std::dynamic_pointer_cast<lanelet::DirectionOfTravel>(factory_regem);
      if (dot)
      {
        world_model_->getMutableMap()->update(parent_llt, dot);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid direction of travel");
      }
 
      break;
    }
    case GeofenceType::STOP_RULE:
    {
 
      lanelet::StopRulePtr sr = std::dynamic_pointer_cast<lanelet::StopRule>(factory_regem);
      if (sr)
      {
        world_model_->getMutableMap()->update(parent_llt, sr);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid stop rule");
      }
      break;
    }
    case GeofenceType::CARMA_TRAFFIC_LIGHT:
    {
      lanelet::CarmaTrafficSignalPtr ctl = std::dynamic_pointer_cast<lanelet::CarmaTrafficSignal>(factory_regem);
      if (ctl)
      {
        world_model_->getMutableMap()->update(parent_llt, ctl);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid carma traffic signal");
      }
      break;
    }
    case GeofenceType::SIGNALIZED_INTERSECTION:
    {
      lanelet::SignalizedIntersectionPtr si = std::dynamic_pointer_cast<lanelet::SignalizedIntersection>(factory_regem);
      if (si)
      {
        world_model_->getMutableMap()->update(parent_llt, si);
      }
      else
      {
        std::invalid_argument("Dynamic Pointer cast failed on getting valid signalized intersection");
      }
 
      break;
    }
    default:
      RCLCPP_WARN_STREAM(logger_, "World Model instance received an unsupported geofence type in its map update callback!");
      break;
  }
}

References carma_wm::CARMA_TRAFFIC_LIGHT, carma_wm::DIGITAL_MINIMUM_GAP, carma_wm::DIGITAL_SPEED_LIMIT, carma_wm::DIRECTION_OF_TRAVEL, logger_, carma_wm::PASSING_CONTROL_LINE, carma_wm::REGION_ACCESS_RULE, carma_wm::resolveGeofenceType(), carma_wm::SIGNALIZED_INTERSECTION, carma_wm::STOP_RULE, and world_model_.

Referenced by mapUpdateCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

roadwayObjectListCallback()

void carma_wm::WMListenerWorker::roadwayObjectListCallback

(

const carma_perception_msgs::msg::RoadwayObstacleList::SharedPtr

msg

)

Callback for roadway objects msg.

Definition at line 574 of file WMListenerWorker.cpp.

{
  // this topic publishes only the objects that are on the road
  world_model_->setRoadwayObjects(msg->roadway_obstacles);
}

References world_model_.

ros1ClockCallback()

void carma_wm::WMListenerWorker::ros1ClockCallback

(

const rosgraph_msgs::msg::Clock::SharedPtr

clock_msg

)

Callback for ROS1 clock message (used in Simulation runs)

Definition at line 580 of file WMListenerWorker.cpp.

{
  world_model_->setRos1Clock(rclcpp::Time(clock_msg->clock));
}

References world_model_.

routeCallback()

void carma_wm::WMListenerWorker::routeCallback

(

const carma_planning_msgs::msg::Route::SharedPtr

route_msg

)

Callback for route message.

Definition at line 590 of file WMListenerWorker.cpp.

{
  if (route_msg->map_version < current_map_version_) {
    RCLCPP_WARN_STREAM(logger_, "Route message rejected as it is for an older map");
    rerouting_flag_ = false; // Clear any blockers on map updates as the route we were waiting for is no longer valid
    return;
  }
 
  if (route_msg->map_version > current_map_version_) {
    RCLCPP_WARN_STREAM(logger_, "Route message received for future map. Delaying application until map is recieved");
    delayed_route_msg_ = *route_msg;
    return;
  }
 
  bool route_invalidated_by_queued_map_update = false; // Flag to indicate whether this new route has been invalidated due to one of the applied queued map updates
  if(rerouting_flag_==true && route_msg->is_rerouted )
 
  {
 
    rerouting_flag_ = false; // Reset flag since the route node has finished re-routing
 
    // After setting map evaluate the current update queue to apply any updates that arrived before the map
    bool more_updates_to_apply = true;
    while(!map_update_queue_.empty() && more_updates_to_apply) {
 
      auto update = map_update_queue_.front(); // Get first update
      map_update_queue_.pop(); // Remove update from queue
 
      if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the current map
        RCLCPP_WARN_STREAM(logger_, "Apply from reroute: There were unapplied updates in carma_wm when a new map was recieved.");
        continue;
      }
      if (update->map_version == current_map_version_) { // Current update goes with current map which is also the map used by this route
        RCLCPP_DEBUG_STREAM(logger_, "Applying queued update after route was recieved. ");
 
        if (update->invalidates_route == true) {
          RCLCPP_DEBUG_STREAM(logger_, "Applied queued map update has invalidated the route.");
          route_invalidated_by_queued_map_update = true;
        }
 
        update->invalidates_route=false; // Do not trigger recomputation of routing graph in mapUpdateCallback; recomputation of routing graph will occur outside of this loop
 
        mapUpdateCallback(update); // Apply the update
      } else {
        RCLCPP_INFO_STREAM(logger_, "Apply from reroute: Done applying updates for new map. However, more updates are waiting for a future map.");
        more_updates_to_apply = false; // If there is more updates queued that are not for this map version assume they are for a future map version
      }
 
    }
 
  }
 
  if (!world_model_->getMap()) { // This check is a bit redundant but still useful from a debugging perspective as the alternative is a segfault
    RCLCPP_ERROR_STREAM(logger_, "WMListener received a route before a map was available. Dropping route message.");
    return;
  }
 
  // If one of the applied queued map updates invalidated the route, then the routing graph must be updated again for the route node
  if (route_invalidated_by_queued_map_update && route_node_flag_){
    RCLCPP_DEBUG_STREAM(logger_,"At least one applied queued map update has invalidated the route. Routing graph will be recomputed.");
    world_model_->setMap(world_model_->getMutableMap(), current_map_version_, route_invalidated_by_queued_map_update);
    RCLCPP_DEBUG_STREAM(logger_,"Finished recomputing the routing graph for the applied queued map update(s)");
 
    rerouting_flag_ = true; // Set flag to trigger a route update by the route node due to the updated routing graph
 
    return;
  }
  else {
    rerouting_flag_ = false; // Reset flag since no applied queued map updates invalidated the route for the route node
 
    auto path = lanelet::ConstLanelets();
    for(auto id : route_msg->shortest_path_lanelet_ids)
    {
      auto ll = world_model_->getMap()->laneletLayer.get(id);
      path.push_back(ll);
    }
 
    auto route_opt = path.size() == 1 ? world_model_->getMapRoutingGraph()->getRoute(path.front(), path.back())
                                : world_model_->getMapRoutingGraph()->getRouteVia(path.front(), lanelet::ConstLanelets(path.begin() + 1, path.end() - 1), path.back());
    if(route_opt.is_initialized()) {
      RCLCPP_DEBUG_STREAM(logger_, "Setting route in world model");
      auto ptr = std::make_shared<lanelet::routing::Route>(std::move(route_opt.get()));
      world_model_->setRoute(ptr);
    }
 
    world_model_->setRouteEndPoint({route_msg->end_point.x,route_msg->end_point.y,route_msg->end_point.z});
    world_model_->setRouteName(route_msg->route_name);
 
    // Call route_callback_
    if (route_callback_)
    {
      route_callback_();
    }
 
    return;
  }
}

References current_map_version_, delayed_route_msg_, logger_, map_update_queue_, mapUpdateCallback(), rerouting_flag_, route_callback_, route_node_flag_, and world_model_.

Referenced by mapCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

routingGraphFromMsg()

LaneletRoutingGraphPtr carma_wm::WMListenerWorker::routingGraphFromMsg	(	const autoware_lanelet2_msgs::msg::RoutingGraph &	msg,
		lanelet::LaneletMapPtr	map
	)		const

Helper function to convert a routing graph message into a actual RoutingGraph object.

Parameters

msg	The graph message to convert
map	The base map this graph applies to

Returns

nullptr if the graph could not be constructed or the provided graph does not match the map

Definition at line 450 of file WMListenerWorker.cpp.

                                                                                                                                             {
 
  if (msg.participant_type.compare(getVehicleParticipationType()) != 0) {
 
    RCLCPP_ERROR_STREAM(logger_,"Received routing graph does not have matching vehicle type for world model. WM Type: "
      << getVehicleParticipationType()
      << " graph type: " << msg.participant_type
    );
 
    return nullptr;
  }
 
  // Get the lists of passable lanelets and areas
  // Both these lists must be populated in the same order as the message to support later logic
  lanelet::ConstLanelets passable_lanelets;
  lanelet::ConstAreas passable_areas;
 
  passable_lanelets.reserve(msg.lanelet_vertices.size());
  passable_areas.reserve(msg.area_vertices.size());
 
  try {
 
    // All the passable lanelets and areas should be included as a vertext so just iterate over each and store
    for (auto vertex : msg.lanelet_vertices) {
      passable_lanelets.emplace_back(map->laneletLayer.get(vertex.lanelet_or_area));
    }
 
    for (auto vertex : msg.area_vertices) {
      passable_areas.emplace_back(map->areaLayer.get(vertex.lanelet_or_area));
    }
 
  } catch(const lanelet::NoSuchPrimitiveError& e) {
 
    RCLCPP_ERROR_STREAM(logger_, "Received routing graph specifies lanelets which do not match the current map version. Actual exception: " << e.what());
 
    return nullptr;
  }
 
 
  // Build the submap
  // This operation does increase in time as the number of lanelets and areas increase
  // however testing shows it to less than 1% of the total routing graph build time so this is a reasonable operation to keep
  auto passable_map = lanelet::utils::createConstSubmap(passable_lanelets, passable_areas);
 
  // This is the actual graph object which is used to initialize a RoutingGraph
  auto graph = std::make_unique<lanelet::routing::internal::RoutingGraphGraph>(msg.num_unique_routing_cost_ids);
 
  // Vertex must be added first then the edge can be added
  for (auto ll : passable_lanelets) {
    graph->addVertex(lanelet::routing::internal::VertexInfo{ll});
  }
 
  for (auto area : passable_areas) {
    graph->addVertex(lanelet::routing::internal::VertexInfo{area});
  }
 
  // Now we can add edges
  for (size_t i = 0; i < msg.lanelet_vertices.size(); ++i) {
 
    auto vertex = msg.lanelet_vertices[i];
    auto lanelet = passable_lanelets[i]; // passable_lanelets should be in the same order based on how its constructed
 
    for (size_t j = 0; j < vertex.lanelet_or_area_ids.size(); ++j) {
 
      lanelet::routing::RelationType relation;
 
      // Get relation
      switch (vertex.edge_relations[j])
      {
        case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_SUCCESSOR:
          relation = lanelet::routing::RelationType::Successor; break;
 
        case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_LEFT:
          relation = lanelet::routing::RelationType::Left; break;
 
        case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_RIGHT:
          relation = lanelet::routing::RelationType::Right; break;
 
        case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_ADJACENT_LEFT:
          relation = lanelet::routing::RelationType::AdjacentLeft; break;
 
        case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_ADJACENT_RIGHT:
          relation = lanelet::routing::RelationType::AdjacentRight; break;
 
        case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_CONFLICTING:
          relation = lanelet::routing::RelationType::Conflicting; break;
 
        case autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_AREA:
          relation = lanelet::routing::RelationType::Area; break;
 
        default: // Treat default as RELATION_NONE
          relation = lanelet::routing::RelationType::None; break;
      }
 
      try {
 
        // Create edge
        graph->addEdge(
          lanelet,
          map->laneletLayer.get(vertex.lanelet_or_area_ids[j]),
          lanelet::routing::internal::EdgeInfo{vertex.edge_routing_costs[j], vertex.edge_routing_cost_source_ids[j], relation}
        );
 
      }  catch(const lanelet::NoSuchPrimitiveError& e) {
 
        RCLCPP_ERROR_STREAM(logger_,"Received routing graph specifies lanelets which do not match the current map version. Not found lanelet or area: "
          << vertex.lanelet_or_area_ids[j] << " Actual exception: " << e.what());
 
        return nullptr;
      }
 
    }
  }
 
  // Build and return the final initialized routing graph
  return std::make_shared<lanelet::routing::RoutingGraph>(std::move(graph), std::move(passable_map));
 
}

References getVehicleParticipationType(), process_bag::i, sci_strategic_plugin::Left, logger_, and sci_strategic_plugin::Right.

Referenced by mapUpdateCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

setConfigSpeedLimit()

void carma_wm::WMListenerWorker::setConfigSpeedLimit

(

double

config_lim

)

Allows user to set a callback to be triggered when a map update is received.

Parameters

config_lim

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

Definition at line 698 of file WMListenerWorker.cpp.

{
  config_speed_limit_ = config_lim;
  //Function to load config_limit into CarmaWorldModel
  world_model_->setConfigSpeedLimit(config_speed_limit_);
}

References config_speed_limit_, and world_model_.

setMapCallback()

void carma_wm::WMListenerWorker::setMapCallback

(

std::function< void()>

callback

)

Allows user to set a callback to be triggered when a map update is received.

Parameters

callback

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

Definition at line 688 of file WMListenerWorker.cpp.

{
  map_callback_ = callback;
}

References map_callback_.

setRouteCallback()

void carma_wm::WMListenerWorker::setRouteCallback

(

std::function< void()>

callback

)

Allows user to set a callback to be triggered when a route update is received.

Parameters

callback

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

Definition at line 693 of file WMListenerWorker.cpp.

{
  route_callback_ = callback;
}

References route_callback_.

setVehicleParticipationType()

void carma_wm::WMListenerWorker::setVehicleParticipationType

(

std::string

participant

)

Allows user to set a callback to be triggered when a map update is received.

Parameters

participant

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

Definition at line 730 of file WMListenerWorker.cpp.

{
  //Function to load participation type into CarmaWorldModel
  world_model_->setVehicleParticipationType(participant);
}

References world_model_.

setWMSpatProcessingState()

void carma_wm::WMListenerWorker::setWMSpatProcessingState

(

const SIGNAL_PHASE_PROCESSING &

phase_type

)

const

Activate World Model SPAT processor, which is turned off by default, with OFF (0), ON (1), FIXED (2)

Definition at line 714 of file WMListenerWorker.cpp.

{
  world_model_->sim_.spat_processor_state_ = phase_type;
}

References world_model_.

simClockCallback()

void carma_wm::WMListenerWorker::simClockCallback

(

const rosgraph_msgs::msg::Clock::SharedPtr

clock_msg

)

Callback for Simulation clock message (used in Simulation runs)

Definition at line 585 of file WMListenerWorker.cpp.

{
  world_model_->setSimulationClock(rclcpp::Time(clock_msg->clock));
}

References world_model_.

Member Data Documentation

config_speed_limit_

double carma_wm::WMListenerWorker::config_speed_limit_

private

Definition at line 182 of file WMListenerWorker.hpp.

Referenced by getConfigSpeedLimit(), and setConfigSpeedLimit().

current_map_version_

size_t carma_wm::WMListenerWorker::current_map_version_ = 0

private

Definition at line 184 of file WMListenerWorker.hpp.

Referenced by mapCallback(), mapUpdateCallback(), and routeCallback().

delayed_route_msg_

boost::optional<carma_planning_msgs::msg::Route> carma_wm::WMListenerWorker::delayed_route_msg_

private

Definition at line 186 of file WMListenerWorker.hpp.

Referenced by mapCallback(), and routeCallback().

logger_

rclcpp::Logger carma_wm::WMListenerWorker::logger_

private

Definition at line 177 of file WMListenerWorker.hpp.

Referenced by mapCallback(), mapUpdateCallback(), newRegemUpdateHelper(), routeCallback(), and routingGraphFromMsg().

map_callback_

std::function<void()> carma_wm::WMListenerWorker::map_callback_

private

Definition at line 179 of file WMListenerWorker.hpp.

Referenced by mapCallback(), mapUpdateCallback(), and setMapCallback().

map_update_queue_

std::queue<autoware_lanelet2_msgs::msg::MapBin::SharedPtr> carma_wm::WMListenerWorker::map_update_queue_

private

Definition at line 185 of file WMListenerWorker.hpp.

Referenced by mapCallback(), mapUpdateCallback(), and routeCallback().

most_recent_update_msg_seq_

long carma_wm::WMListenerWorker::most_recent_update_msg_seq_ = -1

private

Definition at line 191 of file WMListenerWorker.hpp.

Referenced by mapUpdateCallback().

recompute_route_flag_

bool carma_wm::WMListenerWorker::recompute_route_flag_ =false

private

Definition at line 188 of file WMListenerWorker.hpp.

Referenced by mapUpdateCallback().

rerouting_flag_

bool carma_wm::WMListenerWorker::rerouting_flag_ =false

private

Definition at line 189 of file WMListenerWorker.hpp.

Referenced by checkIfReRoutingNeeded(), mapUpdateCallback(), and routeCallback().

route_callback_

std::function<void()> carma_wm::WMListenerWorker::route_callback_

private

Definition at line 180 of file WMListenerWorker.hpp.

Referenced by routeCallback(), and setRouteCallback().

route_node_flag_

bool carma_wm::WMListenerWorker::route_node_flag_ =false

private

Definition at line 190 of file WMListenerWorker.hpp.

Referenced by enableUpdatesWithoutRoute(), mapUpdateCallback(), and routeCallback().

world_model_

std::shared_ptr<CARMAWorldModel> carma_wm::WMListenerWorker::world_model_

private

Definition at line 178 of file WMListenerWorker.hpp.

Referenced by WMListenerWorker(), getVehicleParticipationType(), getWMSpatProcessingState(), getWorldModel(), incomingSpatCallback(), isSpatWallTime(), isUsingSimTime(), mapCallback(), mapUpdateCallback(), newRegemUpdateHelper(), roadwayObjectListCallback(), ros1ClockCallback(), routeCallback(), setConfigSpeedLimit(), setVehicleParticipationType(), setWMSpatProcessingState(), and simClockCallback().

---

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

• carma_wm/src/WMListenerWorker.hpp
• carma_wm/src/WMListenerWorker.cpp