carma_wm::WMListenerWorker Class Reference

Backend logic class for WMListener. More...

#include <WMListenerWorker.hpp>

Collaboration diagram for carma_wm::WMListenerWorker:

Public Member Functions
	WMListenerWorker ()
	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)
	set true if simulation_mode is on More...
void	isSpatWallTime (bool is_spat_wall_time)
	set true if incoming spat is based on wall clock 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
std::shared_ptr< CARMAWorldModel >	world_model_
bool	use_sim_time_
bool	is_spat_wall_time_
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 34 of file WMListenerWorker.hpp.

Constructor & Destructor Documentation

WMListenerWorker()

carma_wm::WMListenerWorker::WMListenerWorker

(

)

Constructor.

Definition at line 45 of file WMListenerWorker.cpp.

{
  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 109 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 114 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 713 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 568 of file WMListenerWorker.cpp.

{
  return world_model_->getVehicleParticipationType();
}

References world_model_.

Referenced by routingGraphFromMsg().

Here is the caller graph for this function:

getWorldModel()

WorldModelConstPtr carma_wm::WMListenerWorker::getWorldModel

(

)

const

Constructor.

Definition at line 50 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 104 of file WMListenerWorker.cpp.

{
  world_model_->processSpatFromMsg(*spat_msg, use_sim_time_, is_spat_wall_time_);
}

References is_spat_wall_time_, use_sim_time_, and world_model_.

isSpatWallTime()

void carma_wm::WMListenerWorker::isSpatWallTime

(

bool

is_spat_wall_time

)

set true if incoming spat is based on wall clock

Definition at line 708 of file WMListenerWorker.cpp.

{
  is_spat_wall_time_ = is_spat_wall_time;
}

References is_spat_wall_time_.

isUsingSimTime()

void carma_wm::WMListenerWorker::isUsingSimTime

(

bool

use_sim_time

)

set true if simulation_mode is on

Definition at line 704 of file WMListenerWorker.cpp.

{
  use_sim_time_ = use_sim_time;
}

References use_sim_time_.

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 56 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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Dropping delayed route message which was never applied as updated map was not recieved");
      delayed_route_msg_ = boost::none;
    } else {
      RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "There is a delayed route message still waiting to be applied in carma_wm");
    }
  }
}

References current_map_version_, delayed_route_msg_, carma_wm::fromBinMsg(), 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 147 of file WMListenerWorker.cpp.

{
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Map Update Being Evaluated. SeqNum: " << geofence_msg->seq_id);
  if (rerouting_flag_) // no update should be applied if rerouting
  {
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Received notice that route has been invalidated in mapUpdateCallback");
 
    if(route_node_flag_!=true)
    {
     RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Processing Map Update with Geofence Id:" << gf_ptr->id_);
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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_);
  }
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems left in lanelet after removal: " << parent_llt.regulatoryElements().size());
 
  }
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_);
 
  // Call user defined map callback
  if (map_callback_)
  {
    RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Calling user defined map update callback");
    map_callback_();
  }
}

References current_map_version_, carma_wm::fromBinMsg(), process_bag::i, 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 322 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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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, 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 573 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 579 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 589 of file WMListenerWorker.cpp.

{
  if (route_msg->map_version < current_map_version_) {
    RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Applying queued update after route was recieved. ");
 
        if (update->invalidates_route == true) {
          RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"),"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(rclcpp::get_logger("carma_wm::WMListenerWorker"),"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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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_, 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 449 of file WMListenerWorker.cpp.

                                                                                                                                             {
 
  if (msg.participant_type.compare(getVehicleParticipationType()) != 0) {
 
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"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(rclcpp::get_logger("carma_wm::WMListenerWorker"), "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(rclcpp::get_logger("carma_wm::WMListenerWorker"),"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, 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 697 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 687 of file WMListenerWorker.cpp.

{
  map_callback_ = callback;
}

References callback(), and map_callback_.

Here is the call graph for this function:

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 692 of file WMListenerWorker.cpp.

{
  route_callback_ = callback;
}

References callback(), and route_callback_.

Here is the call graph for this function:

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 718 of file WMListenerWorker.cpp.

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

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 584 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 166 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 168 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 170 of file WMListenerWorker.hpp.

Referenced by mapCallback(), and routeCallback().

is_spat_wall_time_

bool carma_wm::WMListenerWorker::is_spat_wall_time_

private

Definition at line 162 of file WMListenerWorker.hpp.

Referenced by incomingSpatCallback(), and isSpatWallTime().

map_callback_

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

private

Definition at line 163 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 169 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 175 of file WMListenerWorker.hpp.

Referenced by mapUpdateCallback().

recompute_route_flag_

bool carma_wm::WMListenerWorker::recompute_route_flag_ =false

private

Definition at line 172 of file WMListenerWorker.hpp.

Referenced by mapUpdateCallback().

rerouting_flag_

bool carma_wm::WMListenerWorker::rerouting_flag_ =false

private

Definition at line 173 of file WMListenerWorker.hpp.

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

route_callback_

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

private

Definition at line 164 of file WMListenerWorker.hpp.

Referenced by routeCallback(), and setRouteCallback().

route_node_flag_

bool carma_wm::WMListenerWorker::route_node_flag_ =false

private

Definition at line 174 of file WMListenerWorker.hpp.

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

use_sim_time_

bool carma_wm::WMListenerWorker::use_sim_time_

private

Definition at line 161 of file WMListenerWorker.hpp.

Referenced by incomingSpatCallback(), and isUsingSimTime().

world_model_

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

private

Definition at line 160 of file WMListenerWorker.hpp.

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

---

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

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