lanelet2_route_planning::Lanelet2RoutePlanning Class Reference

Lanelet2 route planning node. More...

#include <lanelet2_route_planning.hpp>

Inheritance diagram for lanelet2_route_planning::Lanelet2RoutePlanning:

Public Member Functions
	Lanelet2RoutePlanning ()
	Constructor.

Private Member Functions
template<typename T >
void	declareAndLoadParameter (const std::string &name, T &param, const std::string &description, const bool add_to_auto_reconfigurable_params=true, const bool is_required=false, const bool read_only=false, const std::optional< double > &from_value=std::nullopt, const std::optional< double > &to_value=std::nullopt, const std::optional< double > &step_value=std::nullopt, const std::string &additional_constraints="")
	Declares and loads a ROS parameter.
rcl_interfaces::msg::SetParametersResult	parametersCallback (const std::vector< rclcpp::Parameter > &parameters)
	Handles reconfiguration when a parameter value is changed.
void	setup ()
	Sets up subscribers, publishers, etc. to configure the node.
bool	checkMap (bool handle_update)
	Checks if map is loaded and handles map updates.
bool	buildRoutingGraph ()
	Builds the lanelet2 routing graph based on the current map.
void	egoDataCallback (const perception_msgs::msg::EgoData::SharedPtr msg)
	Callback for EgoData.
void	publishTimerCallback ()
	Callback to periodically publish the route.
void	globalRoutePublishTimerCallback ()
	Callback to periodically publish the adaptively sampled global route.
rclcpp_action::GoalResponse	actionHandleGoal (const rclcpp_action::GoalUUID &uuid, std::shared_ptr< const route_planning_msgs::action::PlanRoute::Goal > goal)
	Action goal callback: processes a route planning request.
rclcpp_action::CancelResponse	actionHandleCancel (const std::shared_ptr< rclcpp_action::ServerGoalHandle< route_planning_msgs::action::PlanRoute > > goal_handle)
	Action cancel callback: cancels a running action.
void	actionHandleAccepted (const std::shared_ptr< rclcpp_action::ServerGoalHandle< route_planning_msgs::action::PlanRoute > > goal_handle)
	Action accepted callback: starts action execution.
void	actionExecute (const std::shared_ptr< rclcpp_action::ServerGoalHandle< route_planning_msgs::action::PlanRoute > > goal_handle)
	Action execution: continually publishes route progress.
bool	planRoute (const geometry_msgs::msg::PointStamped &destination, const std::vector< geometry_msgs::msg::PointStamped > &intermediate_destinations={})
	Plans a lanelet route to the destination.
void	buildGlobalRouteMessage ()
	Builds a global ROS route message from the latest planned lanelet route.
void	buildEnrichedRouteMessage ()
	Builds an enriched ROS route message from the latest planned lanelet route.
void	publishHealth (const unsigned char status, const std::string &msg, const rclcpp::Time &now)
	Publishes health diagnostic.

Private Attributes
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > >	auto_reconfigurable_params_
	Auto-reconfigurable parameters for dynamic reconfiguration.
OnSetParametersCallbackHandle::SharedPtr	parameters_callback_
	Callback handle for dynamic parameter reconfiguration.
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr	subscriber_ego_data_
	Subscriber for ego data.
rclcpp::Publisher< route_planning_msgs::msg::Route >::SharedPtr	publisher_route_
	Publisher for enriched route.
rclcpp::Publisher< route_planning_msgs::msg::Route >::SharedPtr	publisher_global_route_
	Publisher for the adaptively sampled global route.
rclcpp::TimerBase::SharedPtr	publish_timer_
	Timer for publishing the enriched route.
rclcpp::TimerBase::SharedPtr	global_route_publish_timer_
	Timer for publishing the global route.
rclcpp_action::Server< route_planning_msgs::action::PlanRoute >::SharedPtr	action_server_
	Action server.
route_planning_msgs::action::PlanRoute::Feedback::SharedPtr	action_feedback_
	Latest action feedback.
route_planning_msgs::action::PlanRoute::Result::SharedPtr	action_result_
	Latest action result.
std::shared_ptr< rclcpp_action::ServerGoalHandle< route_planning_msgs::action::PlanRoute > >	action_goal_handle_
	Latest action goal handle.
rclcpp::CallbackGroup::SharedPtr	action_callback_group_
	Callback group for action server.
rclcpp::Time	action_start_time_
	Latest action start time to compute action duration.
std::unique_ptr< tf2_ros::Buffer >	tf_buffer_
	Transform buffer.
std::shared_ptr< tf2_ros::TransformListener >	tf_listener_
	Transform listener.
perception_msgs::msg::EgoData	latest_ego_data_
	Latest ego data in map frame.
std::unique_ptr< Lanelet2MapInterface >	ll2_interface_
	Lanelet2 map interface.
lanelet::routing::RoutingGraphUPtr	routing_graph_
	Lanelet routing graph for current map.
geometry_msgs::msg::Point	starting_point_
	Starting point in map frame.
geometry_msgs::msg::Point	destination_
	Destination point in map frame.
std::vector< geometry_msgs::msg::Point >	intermediate_destinations_
	Intermediate points in map frame.
lanelet::routing::Route	latest_route_
	Latest planned lanelet route.
bool	has_enriched_route_ = false
	Flag indicating that route has been enriched at least once.
bool	is_publishing_route_
	Controlling flag for route publication.
route_planning_msgs::msg::Route	latest_route_msg_
	Latest route message to publish.
route_planning_msgs::msg::Route	latest_full_route_msg_
	Latest complete minimal route used internally for progress tracking.
route_planning_msgs::msg::Route	latest_global_route_msg_
	Latest adaptively sampled non-enriched global route.
std::vector< Eigen::Vector2d >	latest_reference_line_
	Latest complete dense reference line in the map frame.
std::vector< size_t >	latest_lanelet_idx_by_reference_line_point_idx_
	Latest mapping between global route reference line and lanelet indices.
std::vector< std::vector< int > >	latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_
	Latest suggested turn signal distance ahead by route element by lane element.
std::string	ll2_map_server_name_ = "lanelet2_map_server"
	Name of lanelet2_map_server node (parameter)
double	publish_frequency_ = 10.0
	Frequency of route publication [Hz] (parameter)
double	action_feedback_frequency_ = 1.0
	Frequency of action feedback publication [Hz] (parameter)
double	publish_frequency_global_ = 1.0
	Frequency of global route publication [Hz] (parameter)
double	sampling_distance_ = 1.0
	Distance between resampled points along route [m] (parameter)
double	sampling_max_lateral_error_global_ = 0.5
	Maximum lateral error of the adaptively sampled global reference line [m] (parameter)
bool	project_destination_to_reference_line_ = true
	Whether to project destination to reference line (parameter)
double	destination_distance_threshold_ = 1.0
	Distance to destination where destination is considered reached [m] (parameter)
double	required_traveled_distance_proportion_ = 0.5
	Proportion of route length that must have been traveled before considering destination reached [0..1] (parameter)
double	enrich_route_ahead_ego_distance_ = 100.0
	Distance ahead of ego position where global route is enriched with more information [m] (negative=unlimited) (parameter)
double	enrich_route_behind_ego_distance_ = 10.0
	Distance behind ego position where global route is enriched with more information [m] (negative=unlimited) (parameter)
double	route_undershoot_distance_ = 0.0
	Undershoot route by this distance before ego position [m] (parameter)
double	route_overshoot_distance_ = 0.0
	Overshoot route by this distance behind destination [m] (parameter)
double	max_drivable_space_radius_ = 50.0
	Maximum distance to left/right drivable space bounds, if not otherwise restricted [m] (parameter)
int	max_num_threads_ = 0
	Maximum number of threads for parallel processing (0=max available) (parameter)
double	transform_timeout_ = 0.02
	How long to wait for a transform to be available [s] (parameter)
std::map< std::string, std::string >	health_kv_
	Health key-value pairs for diagnostic message.
std::mutex	health_kv_mutex_
	Mutex protecting health key-value pairs across threads.
rclcpp::Publisher< diagnostic_msgs::msg::DiagnosticArray >::SharedPtr	health_diagnostic_pub_
	Health diagnostic publisher.
diagnostic_updater::Updater	diagnostic_updater_
	Diagnostic updater for monitoring topic frequencies and timestamps.
std::unique_ptr< diagnostic_updater::TopicDiagnostic >	ego_data_diagnostic_
	Diagnostic to auto-diagnose ego data topic.
std::unique_ptr< diagnostic_updater::HeaderlessTopicDiagnostic >	route_timer_diagnostic_
	Diagnostic to auto-diagnose route timer frequency.
std::unique_ptr< diagnostic_updater::HeaderlessTopicDiagnostic >	global_route_timer_diagnostic_
	Diagnostic to auto-diagnose global route timer frequency.
TopicDiagnosticConfig	ego_data_diagnostic_config_ = {45.45, 55.55, 0.0, 0.002}
	Configuration for auto-diagnosed ego data topic.
TopicDiagnosticConfig	route_timer_diagnostic_config_ = {18.18, 22.22}
	Configuration for auto-diagnosed route timer.
TopicDiagnosticConfig	global_route_timer_diagnostic_config_ = {0.91, 1.11}
	Configuration for auto-diagnosed global route timer.

Detailed Description

Lanelet2 route planning node.

Definition at line 64 of file lanelet2_route_planning.hpp.

Constructor & Destructor Documentation

Lanelet2RoutePlanning()

lanelet2_route_planning::Lanelet2RoutePlanning::Lanelet2RoutePlanning

(

)

Constructor.

Definition at line 27 of file lanelet2_route_planning.cpp.

                                             : Node("lanelet2_route_planning"), diagnostic_updater_(this) {
  this->declareAndLoadParameter("ll2_map_server_name", ll2_map_server_name_, "Name of lanelet2_map_server node", false, false,
                                true);
  this->declareAndLoadParameter("publish_frequency", publish_frequency_, "Frequency of route publication [Hz]", true, false,
                                false, 0.1, 20.0);
  this->declareAndLoadParameter("publish_frequency_global", publish_frequency_global_,
                                "Frequency of global route publication [Hz]", true, false, false, 0.1, 20.0);
  this->declareAndLoadParameter("action_feedback_frequency", action_feedback_frequency_,
                                "Frequency of action feedback publication [Hz]", true, false, false, 0.1, 20.0);
  this->declareAndLoadParameter("sampling_distance", sampling_distance_, "Distance between resampled points along route [m]",
                                true, false, false, 0.1, 3.0);
  this->declareAndLoadParameter("sampling_max_lateral_error_global", sampling_max_lateral_error_global_,
                                "Maximum lateral error of the adaptively sampled global reference line [m]", true, false, false,
                                0.0, 100.0);
  this->declareAndLoadParameter("project_destination_to_reference_line", project_destination_to_reference_line_,
                                "Whether to project destination to reference line", true, false, false);
  this->declareAndLoadParameter("destination_distance_threshold", destination_distance_threshold_,
                                "Distance to destination where destination is considered reached [m]", true, false, false, 0.1,
                                200.0);
  this->declareAndLoadParameter(
      "required_traveled_distance_proportion", required_traveled_distance_proportion_,
      "Proportion of route length that must have been traveled before considering destination reached [0..1]", true, false, false,
      0.0, 1.0);
  this->declareAndLoadParameter(
      "enrich_route_ahead_ego_distance", enrich_route_ahead_ego_distance_,
      "Distance ahead of ego position where global route is enriched with more information [m] (negative=unlimited)", true, false,
      false, -1.0, 1000.0);
  this->declareAndLoadParameter(
      "enrich_route_behind_ego_distance", enrich_route_behind_ego_distance_,
      "Distance behind ego position where global route is enriched with more information [m] (negative=unlimited)", true, false,
      false, -1.0, 1000.0);
  this->declareAndLoadParameter("route_undershoot_distance", route_undershoot_distance_,
                                "Undershoot route by this distance before ego position [m]", true, false, false, 0.0, 50.0);
  this->declareAndLoadParameter("route_overshoot_distance", route_overshoot_distance_,
                                "Overshoot route by this distance behind destination [m]", true, false, false, 0.0, 100.0);
  this->declareAndLoadParameter("max_drivable_space_radius", max_drivable_space_radius_,
                                "Maximum distance to left/right drivable space bounds, if not otherwise restricted [m]", true,
                                false, false, 3.0, 100.0);
  this->declareAndLoadParameter("max_num_threads", max_num_threads_,
                                "Maximum number of threads for parallel processing (0=max available)", true, false, false, 0,
                                omp_get_max_threads(), 1);
  this->declareAndLoadParameter("transform_timeout", transform_timeout_, "How long to wait for a transform to be available [s]",
                                true, false, false, 0.0, 1.0);
  if (enrich_route_ahead_ego_distance_ < 0.0) {
    enrich_route_ahead_ego_distance_ = std::numeric_limits<double>::infinity();
  }
  if (enrich_route_behind_ego_distance_ < 0.0) {
    enrich_route_behind_ego_distance_ = std::numeric_limits<double>::infinity();
  }
  if (max_num_threads_ <= 0) {
    max_num_threads_ = omp_get_max_threads();
  }
  omp_set_num_threads(max_num_threads_);
 
  this->declareAndLoadParameter("diagnostic_updater.ego_data_diagnostic.min_frequency", ego_data_diagnostic_config_.min_frequency,
                                "Minimum frequency for incoming ego data topic", false, false, false);
  this->declareAndLoadParameter("diagnostic_updater.ego_data_diagnostic.max_frequency", ego_data_diagnostic_config_.max_frequency,
                                "Maximum frequency for incoming ego data topic", false, false, false);
  this->declareAndLoadParameter("diagnostic_updater.ego_data_diagnostic.min_acceptable_timestamp_delta",
                                ego_data_diagnostic_config_.min_acceptable_timestamp_delta,
                                "Minimum acceptable timestamp delta for incoming ego data topic", false, false, false);
  this->declareAndLoadParameter("diagnostic_updater.ego_data_diagnostic.max_acceptable_timestamp_delta",
                                ego_data_diagnostic_config_.max_acceptable_timestamp_delta,
                                "Maximum acceptable timestamp delta for incoming ego data topic", false, false, false);
  this->declareAndLoadParameter("diagnostic_updater.route_timer_diagnostic.min_frequency",
                                route_timer_diagnostic_config_.min_frequency, "Minimum frequency for route timer", false, false,
                                false);
  this->declareAndLoadParameter("diagnostic_updater.route_timer_diagnostic.max_frequency",
                                route_timer_diagnostic_config_.max_frequency, "Maximum frequency for route timer", false, false,
                                false);
  this->declareAndLoadParameter("diagnostic_updater.global_route_timer_diagnostic.min_frequency",
                                global_route_timer_diagnostic_config_.min_frequency, "Minimum frequency for global route timer",
                                false, false, false);
  this->declareAndLoadParameter("diagnostic_updater.global_route_timer_diagnostic.max_frequency",
                                global_route_timer_diagnostic_config_.max_frequency, "Maximum frequency for global route timer",
                                false, false, false);
 
  this->setup();
}

Member Function Documentation

actionExecute()

void lanelet2_route_planning::Lanelet2RoutePlanning::actionExecute ( const std::shared_ptr< rclcpp_action::ServerGoalHandle< route_planning_msgs::action::PlanRoute > > goal_handle )

private

Action execution: continually publishes route progress.

Periodically publishes route traveling progress. Checks for goal completion or cancellation.

Parameters

[in]

goal_handle

action goal handle

Definition at line 512 of file lanelet2_route_planning.cpp.

                                                                                                        {
  RCLCPP_INFO(this->get_logger(), "Executing action goal");
 
  {
    std::scoped_lock lock(health_kv_mutex_);
    health_kv_["action_status"] = "executing";
  }
 
  rclcpp::Rate feedback_rate(action_feedback_frequency_);
  bool has_reached_destination = false;
  while (goal_handle->is_executing() && !goal_handle->is_canceling() && !has_reached_destination) {
    // update feedback and result
    action_feedback_->distance_traveled = distanceTraveled(latest_full_route_msg_);
    action_feedback_->distance_remaining = distanceRemaining(latest_full_route_msg_);
    action_feedback_->time_traveled = this->now() - action_start_time_;
    action_feedback_->time_remaining =
        rclcpp::Duration::from_seconds(route_planning_msgs::route_access::estimateRemainingTime(latest_full_route_msg_));
    action_result_->distance_traveled = action_feedback_->distance_traveled;
    action_result_->time_traveled = action_feedback_->time_traveled;
 
    // check if destination reached (criteria: close to destination and some distance traveled)
    double distance_to_destination = (toEigen2d(egoPosition(latest_ego_data_)) - toEigen2d(destination_)).norm();
    double total_route_distance = action_feedback_->distance_traveled + action_feedback_->distance_remaining;
    bool is_close_to_destination = (distance_to_destination <= destination_distance_threshold_);
    bool has_traveled_sufficient_distance =
        (action_feedback_->distance_traveled >= required_traveled_distance_proportion_ * total_route_distance);
    has_reached_destination = (is_close_to_destination && has_traveled_sufficient_distance);
 
    // publish feedback
    goal_handle->publish_feedback(action_feedback_);
    if (!has_reached_destination) {
      feedback_rate.sleep();
    }
  }
 
  // prepare result
  action_result_->distance_traveled = action_feedback_->distance_traveled + action_feedback_->distance_remaining;
  action_result_->time_traveled = this->now() - action_start_time_;
  action_result_->destination_reached = has_reached_destination;
 
  // publish result
  if (goal_handle->is_canceling()) {
    goal_handle->canceled(action_result_);
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["action_status"] = "canceled";
    }
    RCLCPP_INFO(this->get_logger(), "Goal canceled");
  } else if (!goal_handle->is_executing()) {
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["action_status"] = "aborted";
    }
    RCLCPP_INFO(this->get_logger(), "Goal aborted");
  } else if (rclcpp::ok()) {
    is_publishing_route_ = false;  // stop publishing route
    goal_handle->succeed(action_result_);
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["action_status"] = "succeeded";
    }
    RCLCPP_INFO(this->get_logger(), "Goal succeeded");
  }
}

actionHandleAccepted()

void lanelet2_route_planning::Lanelet2RoutePlanning::actionHandleAccepted ( const std::shared_ptr< rclcpp_action::ServerGoalHandle< route_planning_msgs::action::PlanRoute > > goal_handle )

private

Action accepted callback: starts action execution.

Processes accepted action goal requests.

Initializes action feedback and starts action execution in a separate thread.

Parameters

[in]	goal_handle	action goal handle
	goal_handle	action goal handle

Definition at line 482 of file lanelet2_route_planning.cpp.

                                                                                                        {
  action_goal_handle_ = goal_handle;
 
  {
    std::scoped_lock lock(health_kv_mutex_);
    health_kv_["action_status"] = "accepted";
  }
 
  // initialize feedback and result
  action_start_time_ = this->now();
  action_feedback_ = std::make_shared<route_planning_msgs::action::PlanRoute::Feedback>();
  action_feedback_->distance_traveled = 0.0;
  action_feedback_->distance_remaining = 0.0;
  action_feedback_->distance_remaining = distanceRemaining(latest_full_route_msg_);
  action_feedback_->time_traveled = rclcpp::Duration::from_seconds(0.0);
  action_feedback_->time_remaining =
      rclcpp::Duration::from_seconds(route_planning_msgs::route_access::estimateRemainingTime(latest_full_route_msg_));
  action_result_ = std::make_shared<route_planning_msgs::action::PlanRoute::Result>();
  action_result_->distance_traveled = 0.0;
  action_result_->time_traveled = rclcpp::Duration::from_seconds(0.0);
  action_result_->destination_reached = false;
 
  // start publishing route
  is_publishing_route_ = true;
 
  // execute action in a separate thread to avoid blocking
  std::thread{std::bind(&Lanelet2RoutePlanning::actionExecute, this, std::placeholders::_1), goal_handle}.detach();
}

actionHandleCancel()

rclcpp_action::CancelResponse lanelet2_route_planning::Lanelet2RoutePlanning::actionHandleCancel ( const std::shared_ptr< rclcpp_action::ServerGoalHandle< route_planning_msgs::action::PlanRoute > > goal_handle )

private

Action cancel callback: cancels a running action.

Sets a flag to cancel action execution.

Parameters

[in]

goal_handle

action goal handle

Returns

cancel response

Definition at line 467 of file lanelet2_route_planning.cpp.

                                                                                                        {
  (void)goal_handle;
 
  RCLCPP_INFO(this->get_logger(), "Received request to cancel action goal");
  is_publishing_route_ = false;  // stop publishing route
 
  return rclcpp_action::CancelResponse::ACCEPT;
}

actionHandleGoal()

rclcpp_action::GoalResponse lanelet2_route_planning::Lanelet2RoutePlanning::actionHandleGoal ( const rclcpp_action::GoalUUID & uuid, std::shared_ptr< const route_planning_msgs::action::PlanRoute::Goal > goal )

private

Action goal callback: processes a route planning request.

Plans a global route to the destination before accepting the goal. Aborts any existing running action.

Parameters

[in]	uuid	unique goal identifier
[in]	goal	action goal

Returns

goal response

Definition at line 425 of file lanelet2_route_planning.cpp.

                                                                                                  {
  (void)uuid;
  (void)goal;
 
  const geometry_msgs::msg::PointStamped& destination = goal->destination;
  const std::vector<geometry_msgs::msg::PointStamped>& intermediate_destinations = goal->intermediate_destinations;
  RCLCPP_INFO(this->get_logger(), "Received request to plan route to destination (%.3f, %.3f, %.3f) in frame '%s'",
              destination.point.x, destination.point.y, destination.point.z, destination.header.frame_id.c_str());
 
  // check for and handle map updates
  if (!this->checkMap(true)) {
    RCLCPP_ERROR(this->get_logger(), "Cannot plan route, map not loaded by '%s'", ll2_map_server_name_.c_str());
    return rclcpp_action::GoalResponse::REJECT;
  }
 
  // plan route
  auto t0 = std::chrono::steady_clock::now();
  bool success = this->planRoute(destination, intermediate_destinations);
  auto t1 = std::chrono::steady_clock::now();
  auto dt = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - t0).count();
  if (!success) {
    RCLCPP_ERROR(this->get_logger(), "Failed to plan route to destination, rejecting request");
    return rclcpp_action::GoalResponse::REJECT;
  }
 
  // convert route to ROS message
  this->buildGlobalRouteMessage();
  RCLCPP_INFO(this->get_logger(), "Successfully planned route to destination (%.3fs)", dt);
 
  // abort current action if running
  if (action_goal_handle_ && action_goal_handle_->is_active()) {
    RCLCPP_WARN(this->get_logger(), "Existing action detected, aborting before accepting new goal");
    is_publishing_route_ = false;  // stop publishing route
    action_goal_handle_->abort(action_result_);
    action_goal_handle_.reset();
  }
 
  // accept action goal request
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

buildEnrichedRouteMessage()

void lanelet2_route_planning::Lanelet2RoutePlanning::buildEnrichedRouteMessage ( )

private

Builds an enriched ROS route message from the latest planned lanelet route.

The enriched route message is locally enriched around the ego position. The enriched part contains adjacent lanes, drivable space, and regulatory elements.

Definition at line 843 of file lanelet2_route_planning.cpp.

                                                      {
  if (latest_full_route_msg_.route_elements.empty() || latest_reference_line_.empty()) {
    return;
  }
 
  // find point of global reference line closest to and behind of ego position
  const Eigen::Vector2d ego_position = toEigen2d(egoPosition(latest_ego_data_));
  const size_t global_closest_point =
      matchPointToLineString(latest_reference_line_, ego_position, latest_full_route_msg_.current_route_element_idx, true, true);
  latest_full_route_msg_.current_route_element_idx = global_closest_point;
  const auto& full_route_elements = latest_full_route_msg_.route_elements;
  LocalRouteWindow local_route_window = extractLocalRouteWindow(
      latest_full_route_msg_, global_closest_point, enrich_route_behind_ego_distance_, enrich_route_ahead_ego_distance_);
  const size_t first_global_idx = local_route_window.first_global_idx;
  route_planning_msgs::msg::Route route_msg = std::move(local_route_window.route);
  std::vector<route_planning_msgs::msg::RouteElement>& route_elements = route_msg.route_elements;
  if (route_elements.empty()) {
    return;
  }
 
  latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_ =
      std::vector<std::vector<int>>(route_elements.size());
  const lanelet::routing::LaneletPath shortest_path = latest_route_.shortestPath();
 
// parallelize independent geometry, map, and regulatory-element extraction per local route element
#pragma omp parallel for
  for (size_t c = 0; c < route_elements.size(); ++c) {
    const size_t global_c = first_global_idx + c;
    route_planning_msgs::msg::RouteElement& route_element_msg = route_elements[c];
    route_planning_msgs::msg::LaneElement& lane_element_msg =
        route_element_msg.lane_elements[route_element_msg.suggested_lane_idx];
 
    // get current, previous and next centerline point
    const route_planning_msgs::msg::LaneElement prev_lane_element_msg =
        (global_c > 0) ? route_planning_msgs::route_access::getSuggestedLaneElement(full_route_elements[global_c - 1])
                       : lane_element_msg;
    const route_planning_msgs::msg::LaneElement next_lane_element_msg =
        (global_c + 1 < full_route_elements.size())
            ? route_planning_msgs::route_access::getSuggestedLaneElement(full_route_elements[global_c + 1])
            : lane_element_msg;
    const Eigen::Vector2d point = toEigen2d(lane_element_msg.reference_pose.position);
    const Eigen::Vector2d prev_point = toEigen2d(prev_lane_element_msg.reference_pose.position);
    const Eigen::Vector2d next_point = toEigen2d(next_lane_element_msg.reference_pose.position);
    const double point_z = lane_element_msg.reference_pose.position.z;  // assuming constant z across route elements
 
    // get lanelet corresponding to centerline point
    const lanelet::ConstLanelet& lanelet = shortest_path[latest_lanelet_idx_by_reference_line_point_idx_[global_c]];
 
    // identify lane changes
    const bool changes_lane_from_prev_point =
        changesLaneFromPointToPoint(prev_point, point, sampling_distance_);  // NOLINT(readability-suspicious-call-argument)
    const bool changes_lane_to_next_point = changesLaneFromPointToPoint(point, next_point, sampling_distance_);
 
    // determine neighboring points for projection
    Eigen::Vector2d prev_point_for_projection = changes_lane_from_prev_point ? point : prev_point;
    Eigen::Vector2d next_point_for_projection = changes_lane_to_next_point ? point : next_point;
 
    // get adjacent lanelets
    std::vector<lanelet::ConstLanelet> adjacent_left_lanelets = adjacentLeftOrRightLanelets(lanelet, routing_graph_, true);
    std::vector<lanelet::ConstLanelet> adjacent_right_lanelets = adjacentLeftOrRightLanelets(lanelet, routing_graph_, false);
    const int suggested_lane_idx = static_cast<int>(adjacent_left_lanelets.size());
    const int n_lanes = static_cast<int>(adjacent_left_lanelets.size() + 1 + adjacent_right_lanelets.size());
    if (latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_[c].empty()) {
      latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_[c] = std::vector<int>(n_lanes, -1);
    }
 
    // project centerline point to lanelet and adjacent lanelet centerlines and bounds
    auto lanelet_projected_points =
        projectPointToLaneletLines(point, prev_point_for_projection, next_point_for_projection,
                                   std::vector<lanelet::ConstLanelet>{lanelet}, this->get_logger())[0];
    auto adjacent_left_lanelets_projected_points = projectPointToLaneletLines(
        point, prev_point_for_projection, next_point_for_projection, adjacent_left_lanelets, this->get_logger());
    auto adjacent_right_lanelets_projected_points = projectPointToLaneletLines(
        point, prev_point_for_projection, next_point_for_projection, adjacent_right_lanelets, this->get_logger());
 
    // compute offset of lane element indices from current to next route element
    const lanelet::ConstLanelet& lanelet_of_next_point =
        (global_c + 1 < full_route_elements.size()) ? shortest_path[latest_lanelet_idx_by_reference_line_point_idx_[global_c + 1]]
                                                    : lanelet;
    int following_lane_idx_offset = 0;
    if (auto result = computeFollowingLaneIdxOffset(lanelet, lanelet_of_next_point, routing_graph_)) {
      following_lane_idx_offset = *result;
    } else {
      RCLCPP_ERROR(this->get_logger(),
                   "Failed to find following lane index offset for route element %ld on lanelet %ld, assuming no offset",
                   global_c, lanelet.id());
      following_lane_idx_offset = 0;
    }
 
    // extract drivable space
    Eigen::Vector2d drivable_space_left, drivable_space_right;
    std::tie(drivable_space_left, drivable_space_right) =
        extractDrivableSpace(ll2_interface_->getMapPtr()->lineStringLayer,
                             {prev_point_for_projection, point, next_point_for_projection}, max_drivable_space_radius_);
 
    // extract regulatory elements
    auto regulatory_element_extraction =
        extractRegulatoryElements(lanelet, adjacent_left_lanelets, adjacent_right_lanelets, {prev_point, point, next_point});
 
// enrich RouteElement with local route information
#pragma omp critical  // prevent race condition when accessing prev/next suggested lane element set by other threads
    {
      route_element_msg.lane_elements = {};
      route_element_msg.is_enriched = true;
      route_element_msg.left_boundary = toRos(to3d(drivable_space_left, point_z));
      route_element_msg.right_boundary = toRos(to3d(drivable_space_right, point_z));
      route_element_msg.regulatory_elements = regulatory_element_extraction.regulatory_element_msgs;
      route_element_msg.suggested_lane_idx = suggested_lane_idx;
      route_element_msg.will_change_suggested_lane = changes_lane_to_next_point;
      // route_element_msg.s already set in global route
      size_t lane_element_idx = route_element_msg.lane_elements.size();
 
      // create LaneElements for left adjacent lanes
      for (size_t a = 0; a < adjacent_left_lanelets_projected_points.size(); ++a) {
        route_planning_msgs::msg::LaneElement lane_element_msg;
        lane_element_msg.reference_pose.position =
            toRos(to3d(adjacent_left_lanelets_projected_points[a].centerline_point, point_z));
        // lane_element_msg.reference_pose.orientation computed in postprocessRouteMessage
        lane_element_msg.left_boundary.point = toRos(to3d(adjacent_left_lanelets_projected_points[a].left_bound_point, point_z));
        lane_element_msg.left_boundary.type = laneBoundaryType(adjacent_left_lanelets[a].leftBound2d());
        lane_element_msg.right_boundary.point =
            toRos(to3d(adjacent_left_lanelets_projected_points[a].right_bound_point, point_z));
        lane_element_msg.right_boundary.type = laneBoundaryType(adjacent_left_lanelets[a].rightBound2d());
        lane_element_msg.speed_limit =
            speedLimit(adjacent_left_lanelets[a], adjacent_left_lanelets_projected_points[a].centerline_point);
        lane_element_msg.regulatory_element_idcs = regulatory_element_extraction.adjacent_left_regulatory_element_idcs[a];
        const int computed_following_lane_idx =
            static_cast<int>(route_element_msg.lane_elements.size()) + following_lane_idx_offset;
        lane_element_msg.has_following_lane_idx = (computed_following_lane_idx >= 0 && computed_following_lane_idx < n_lanes);
        if (lane_element_msg.has_following_lane_idx) {
          lane_element_msg.following_lane_idx = computed_following_lane_idx;
        }
        std::tie(lane_element_msg.suggested_turn_signal,
                 latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_[c][lane_element_idx]) =
            suggestedTurnSignal(adjacent_left_lanelets[a], this->get_logger());
        route_element_msg.lane_elements.push_back(lane_element_msg);
        lane_element_idx = route_element_msg.lane_elements.size();
      }
 
      // create LaneElement for centerline lane
      route_planning_msgs::msg::LaneElement centerline_lane_element_msg;
      centerline_lane_element_msg.reference_pose.position = toRos(to3d(point, point_z));
      // centerline_lane_element_msg.reference_pose.orientation computed in postprocessRouteMessage
      centerline_lane_element_msg.left_boundary.point = toRos(to3d(lanelet_projected_points.left_bound_point, point_z));
      centerline_lane_element_msg.left_boundary.type = laneBoundaryType(lanelet.leftBound2d());
      centerline_lane_element_msg.right_boundary.point = toRos(to3d(lanelet_projected_points.right_bound_point, point_z));
      centerline_lane_element_msg.right_boundary.type = laneBoundaryType(lanelet.rightBound2d());
      centerline_lane_element_msg.speed_limit = speedLimit(lanelet, point);
      centerline_lane_element_msg.regulatory_element_idcs = regulatory_element_extraction.regulatory_element_idcs;
      const int computed_following_lane_idx =
          static_cast<int>(route_element_msg.lane_elements.size()) + following_lane_idx_offset;
      centerline_lane_element_msg.has_following_lane_idx =
          (computed_following_lane_idx >= 0 && computed_following_lane_idx < n_lanes);
      if (centerline_lane_element_msg.has_following_lane_idx) {
        centerline_lane_element_msg.following_lane_idx = computed_following_lane_idx;
      }
      std::tie(centerline_lane_element_msg.suggested_turn_signal,
               latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_[c][lane_element_idx]) =
          suggestedTurnSignal(lanelet, this->get_logger());
      route_element_msg.lane_elements.push_back(centerline_lane_element_msg);
      lane_element_idx = route_element_msg.lane_elements.size();
 
      // create LaneElements for right adjacent lanes
      for (size_t a = 0; a < adjacent_right_lanelets_projected_points.size(); ++a) {
        route_planning_msgs::msg::LaneElement lane_element_msg;
        lane_element_msg.reference_pose.position =
            toRos(to3d(adjacent_right_lanelets_projected_points[a].centerline_point, point_z));
        // lane_element_msg.reference_pose.orientation computed in postprocessRouteMessage
        lane_element_msg.left_boundary.point = toRos(to3d(adjacent_right_lanelets_projected_points[a].left_bound_point, point_z));
        lane_element_msg.left_boundary.type = laneBoundaryType(adjacent_right_lanelets[a].leftBound2d());
        lane_element_msg.right_boundary.point =
            toRos(to3d(adjacent_right_lanelets_projected_points[a].right_bound_point, point_z));
        lane_element_msg.right_boundary.type = laneBoundaryType(adjacent_right_lanelets[a].rightBound2d());
        lane_element_msg.speed_limit =
            speedLimit(adjacent_right_lanelets[a], adjacent_right_lanelets_projected_points[a].centerline_point);
        lane_element_msg.regulatory_element_idcs = regulatory_element_extraction.adjacent_right_regulatory_element_idcs[a];
        const int computed_following_lane_idx =
            static_cast<int>(route_element_msg.lane_elements.size()) + following_lane_idx_offset;
        lane_element_msg.has_following_lane_idx = (computed_following_lane_idx >= 0 && computed_following_lane_idx < n_lanes);
        if (lane_element_msg.has_following_lane_idx) {
          lane_element_msg.following_lane_idx = computed_following_lane_idx;
        }
        std::tie(lane_element_msg.suggested_turn_signal,
                 latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_[c][lane_element_idx]) =
            suggestedTurnSignal(adjacent_right_lanelets[a], this->get_logger());
        route_element_msg.lane_elements.push_back(lane_element_msg);
        lane_element_idx = route_element_msg.lane_elements.size();
      }
    }
  }
 
  route_msg.header.stamp = latest_ego_data_.header.stamp;
 
  // postprocess route message
  postprocessRouteMessage(route_msg, latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_);
 
  // save as latest route message
  latest_route_msg_ = route_msg;
  has_enriched_route_ = true;
}

buildGlobalRouteMessage()

void lanelet2_route_planning::Lanelet2RoutePlanning::buildGlobalRouteMessage ( )

private

Builds a global ROS route message from the latest planned lanelet route.

The global route message only contains a global reference line along a single lane of the shortest path of the route.

Definition at line 736 of file lanelet2_route_planning.cpp.

                                                    {
  // initialize the complete minimal route before deriving local and global views
  route_planning_msgs::msg::Route route_msg;
  route_msg.header.stamp = latest_ego_data_.header.stamp;
  route_msg.header.frame_id = ll2_interface_->map_frame_id_;
  route_msg.destination = destination_;
  route_msg.intermediate_destinations = intermediate_destinations_;
 
  // resample the shortest path to form the reference line
  const lanelet::routing::LaneletPath shortest_path = latest_route_.shortestPath();
  const auto resampling_result = resampleCenterlinesAlongPath(shortest_path, sampling_distance_, true);
  const std::vector<Eigen::Vector3d>& shortest_path_centerline_3d = resampling_result.centerline;
  const std::vector<Eigen::Vector2d> shortest_path_centerline = to2d(shortest_path_centerline_3d);
  latest_reference_line_ = shortest_path_centerline;
  latest_lanelet_idx_by_reference_line_point_idx_ = resampling_result.lanelet_idx_by_point;
 
  // create one minimal route element for each reference-line point
  double accumulated_distance = 0.0;
  for (size_t c = 0; c < shortest_path_centerline.size(); ++c) {
    const Eigen::Vector2d& point = shortest_path_centerline[c];
    const Eigen::Vector3d& point_3d = shortest_path_centerline_3d[c];
    const Eigen::Vector2d& prev_point = c > 0 ? shortest_path_centerline[c - 1] : point;
    const Eigen::Vector2d& next_point = c + 1 < shortest_path_centerline.size() ? shortest_path_centerline[c + 1] : point;
    const lanelet::ConstLanelet& lanelet = shortest_path[latest_lanelet_idx_by_reference_line_point_idx_[c]];
    const bool changes_lane_from_prev_point =
        changesLaneFromPointToPoint(prev_point, point, sampling_distance_);  // NOLINT(readability-suspicious-call-argument)
    const bool changes_lane_to_next_point = changesLaneFromPointToPoint(point, next_point, sampling_distance_);
    const Eigen::Vector2d orientation = tangentOfPointAlongLineString(point, changes_lane_from_prev_point ? point : prev_point,
                                                                      changes_lane_to_next_point ? point : next_point);
    accumulated_distance += (point - prev_point).norm();
    route_msg.route_elements.push_back(createMinimalRouteElement(toRos(point_3d), toRosQuaternion(orientation),
                                                                 accumulated_distance, changes_lane_to_next_point,
                                                                 speedLimit(lanelet, point)));
  }
 
  // optionally align route endpoints with the reference line
  if (project_destination_to_reference_line_) {
    starting_point_ = toRos(projectPointToLineString(toEigen(starting_point_), shortest_path_centerline_3d));
    for (auto& destination : intermediate_destinations_) {
      destination = toRos(projectPointToLineString(toEigen(destination), shortest_path_centerline_3d));
    }
    destination_ = toRos(projectPointToLineString(toEigen(destination_), shortest_path_centerline_3d));
  }
  route_msg.destination = destination_;
  route_msg.intermediate_destinations = intermediate_destinations_;
  // locate the route endpoints and current position in the complete route
  route_msg.starting_route_element_idx =
      matchPointToLineString(shortest_path_centerline, toEigen2d(starting_point_), 0, true, true);
  route_msg.current_route_element_idx = matchPointToLineString(shortest_path_centerline, toEigen2d(egoPosition(latest_ego_data_)),
                                                               route_msg.starting_route_element_idx, true, true);
  route_msg.destination_route_element_idx =
      indexOfLineStringPointClosestToPoint(shortest_path_centerline, toEigen2d(destination_), true, false);
 
  has_enriched_route_ = false;
  latest_route_msg_ = route_planning_msgs::msg::Route();
  latest_full_route_msg_ = route_planning_msgs::msg::Route();
  latest_global_route_msg_ = route_planning_msgs::msg::Route();
 
  const size_t start_idx = route_msg.starting_route_element_idx;
  const size_t destination_idx = route_msg.destination_route_element_idx;
  if (start_idx >= route_msg.route_elements.size() || destination_idx >= route_msg.route_elements.size() ||
      start_idx > destination_idx) {
    RCLCPP_WARN(this->get_logger(), "Invalid start or destination index, not building global route");
    return;
  }
 
  latest_full_route_msg_ = route_msg;
 
  // retain only the route segment from start to destination for the global route
  std::vector<Eigen::Vector2d> global_reference_line;
  global_reference_line.reserve(destination_idx - start_idx + 1);
  std::vector<size_t> break_after_indices;
  for (size_t c = start_idx; c <= destination_idx; ++c) {
    global_reference_line.push_back(shortest_path_centerline[c]);
    if (c < destination_idx && route_msg.route_elements[c].will_change_suggested_lane) {
      break_after_indices.push_back(c - start_idx);
    }
  }
  // adaptively sample each continuous global-route segment
  const std::vector<size_t> retained_indices =
      adaptivelySampleLineString(global_reference_line, sampling_max_lateral_error_global_, break_after_indices);
  if (retained_indices.empty()) {
    RCLCPP_WARN(this->get_logger(), "Global reference line is empty, not building global route");
    return;
  }
 
  latest_global_route_msg_ = route_msg;
  latest_global_route_msg_.route_elements.clear();
  latest_global_route_msg_.route_elements.reserve(retained_indices.size());
  for (const size_t retained_idx : retained_indices) {
    latest_global_route_msg_.route_elements.push_back(route_msg.route_elements[start_idx + retained_idx]);
  }
  // the global route contains one lane element per route element, which always continues in lane zero
  for (size_t i = 0; i + 1 < latest_global_route_msg_.route_elements.size(); ++i) {
    auto& lane_element = latest_global_route_msg_.route_elements[i].lane_elements[0];
    lane_element.has_following_lane_idx = true;
    lane_element.following_lane_idx = 0;
  }
  latest_global_route_msg_.route_elements.back().lane_elements[0].has_following_lane_idx = false;
 
  latest_global_route_msg_.starting_route_element_idx = 0;
  latest_global_route_msg_.destination_route_element_idx = latest_global_route_msg_.route_elements.size() - 1;
  latest_global_route_msg_.current_route_element_idx = 0;
  RCLCPP_INFO(this->get_logger(), "Built global route with %ld/%ld route elements", retained_indices.size(),
              global_reference_line.size());
}

buildRoutingGraph()

bool lanelet2_route_planning::Lanelet2RoutePlanning::buildRoutingGraph ( )

private

Builds the lanelet2 routing graph based on the current map.

Returns

whether successful

Definition at line 295 of file lanelet2_route_planning.cpp.

                                              {
  if (!this->checkMap(false)) {
    RCLCPP_ERROR(this->get_logger(), "Cannot build routing graph, map not loaded by '%s'", ll2_map_server_name_.c_str());
    return false;
  }
 
  // get map and traffic rules
  lanelet::LaneletMapConstPtr map = ll2_interface_->getMapPtr();
  lanelet::traffic_rules::TrafficRulesPtr traffic_rules = getTrafficRules();
 
  // build routing graph
  routing_graph_ = lanelet::routing::RoutingGraph::build(*map, *traffic_rules);
  lanelet::routing::Route::Errors errors = routing_graph_->checkValidity();
  if (!errors.empty()) {
    RCLCPP_ERROR(this->get_logger(), "Failed to build valid routing graph");
    for (size_t i = 0; i < errors.size(); ++i) {
      RCLCPP_ERROR_STREAM(this->get_logger(), errors[i]);
    }
    return false;
  }
 
  RCLCPP_INFO(this->get_logger(), "Successfully built routing graph");
  return true;
}

checkMap()

bool lanelet2_route_planning::Lanelet2RoutePlanning::checkMap ( bool handle_update )

private

Checks if map is loaded and handles map updates.

Parameters

[in]

handle_update

whether to handle map update

Returns

whether map is loaded (and updated, if supposed to handle update)

Definition at line 222 of file lanelet2_route_planning.cpp.

                                                       {
  bool map_status = ll2_interface_->map_loaded_;
  // update routing graph on map update
  if (handle_update && ll2_interface_->update_pending_ && ll2_interface_->map_loaded_) {
    if (this->buildRoutingGraph()) {
      ll2_interface_->update_pending_ = false;
    }
    map_status = map_status && !ll2_interface_->update_pending_;
  }
  {
    std::scoped_lock lock(health_kv_mutex_);
    health_kv_["map_loaded"] = map_status ? "true" : "false";
  }
  return map_status;
}

declareAndLoadParameter()

template<typename T >

void lanelet2_route_planning::Lanelet2RoutePlanning::declareAndLoadParameter ( const std::string & name, T & param, const std::string & description, const bool add_to_auto_reconfigurable_params = true, const bool is_required = false, const bool read_only = false, const std::optional< double > & from_value = std::nullopt, const std::optional< double > & to_value = std::nullopt, const std::optional< double > & step_value = std::nullopt, const std::string & additional_constraints = "" )

private

Declares and loads a ROS parameter.

Parameters

[in]	name	name
[in]	param	parameter variable to load into
[in]	description	description
[in]	add_to_auto_reconfigurable_params	enable reconfiguration of parameter
[in]	is_required	whether failure to load parameter will stop node
[in]	read_only	set parameter to read-only
[in]	from_value	parameter range minimum
[in]	to_value	parameter range maximum
[in]	step_value	parameter range step
[in]	additional_constraints	additional constraints description

Definition at line 108 of file lanelet2_route_planning.cpp.

                                                                                             {
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  param_desc.description = description;
  param_desc.additional_constraints = additional_constraints;
  param_desc.read_only = read_only;
 
  auto type = rclcpp::ParameterValue(param).get_type();
 
  if (from_value.has_value() && to_value.has_value()) {
    if constexpr (std::is_integral_v<T>) {
      rcl_interfaces::msg::IntegerRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.integer_range = {range};
    } else if constexpr (std::is_floating_point_v<T>) {
      rcl_interfaces::msg::FloatingPointRange range;
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value()));
      if (step_value.has_value()) range.set__step(static_cast<T>(step_value.value()));
      param_desc.floating_point_range = {range};
    } else {
      RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str());
    }
  }
 
  this->declare_parameter(name, type, param_desc);
 
  try {
    param = this->get_parameter(name).get_value<T>();
    std::stringstream ss;
    ss << "Loaded parameter '" << name << "': ";
    if constexpr (is_vector_v<T>) {
      ss << "[";
      for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
      ss << "]";
    } else {
      ss << param;
    }
    RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
  } catch (rclcpp::exceptions::ParameterUninitializedException&) {
    if (is_required) {
      RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
      exit(EXIT_FAILURE);
    } else {
      std::stringstream ss;
      ss << "Missing parameter '" << name << "', using default value: ";
      if constexpr (is_vector_v<T>) {
        ss << "[";
        for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
        ss << "]";
      } else {
        ss << param;
      }
      RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
      this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
    }
  }
 
  if (add_to_auto_reconfigurable_params) {
    std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
    auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
  }
}

egoDataCallback()

void lanelet2_route_planning::Lanelet2RoutePlanning::egoDataCallback ( const perception_msgs::msg::EgoData::SharedPtr msg )

private

Callback for EgoData.

Recomputes the local route if the node is currently publishing a route.

Parameters

[in]

msg

Definition at line 320 of file lanelet2_route_planning.cpp.

                                                                                        {
  ego_data_diagnostic_->tick(msg->header.stamp);
 
  if (!this->checkMap(false)) {
    return;
  }
 
  // transform ego data to map frame
  if (msg->header.frame_id != ll2_interface_->map_frame_id_) {
    try {
      latest_ego_data_ = tf_buffer_->transform(*msg, ll2_interface_->map_frame_id_, tf2::durationFromSec(transform_timeout_));
    } catch (tf2::TransformException& ex) {
      std::stringstream ss;
      ss << "Could not transform ego data from frame '" << msg->header.frame_id << "' to frame '" << ll2_interface_->map_frame_id_
         << "': " << ex.what();
      RCLCPP_ERROR_STREAM(this->get_logger(), ss.str());
      {
        std::scoped_lock lock(health_kv_mutex_);
        health_kv_["latest_error"] = ss.str();
      }
    }
  } else {
    latest_ego_data_ = *msg;
  }
 
  // recompute local route
  if (is_publishing_route_) {
    auto t0 = std::chrono::steady_clock::now();
    this->buildEnrichedRouteMessage();
    auto t1 = std::chrono::steady_clock::now();
    auto dt = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - t0).count();
    RCLCPP_DEBUG(this->get_logger(), "Recomputed route (%.3fs)", dt);
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["dt_buildEnrichedRouteMessage"] = std::to_string(dt);
    }
  }
}

globalRoutePublishTimerCallback()

void lanelet2_route_planning::Lanelet2RoutePlanning::globalRoutePublishTimerCallback ( )

private

Callback to periodically publish the adaptively sampled global route.

Definition at line 406 of file lanelet2_route_planning.cpp.

                                                            {
  global_route_timer_diagnostic_->tick();
  if (!is_publishing_route_ || latest_global_route_msg_.route_elements.empty()) {
    return;
  }
  std::vector<Eigen::Vector2d> reference_line;
  reference_line.reserve(latest_global_route_msg_.route_elements.size());
  for (const auto& route_element : latest_global_route_msg_.route_elements) {
    if (route_element.lane_elements.empty()) {
      return;
    }
    reference_line.push_back(toEigen2d(route_element.lane_elements.front().reference_pose.position));
  }
  latest_global_route_msg_.current_route_element_idx =
      matchPointToLineString(reference_line, toEigen2d(egoPosition(latest_ego_data_)), 0, true, true);
  latest_global_route_msg_.header.stamp = latest_ego_data_.header.stamp;
  publisher_global_route_->publish(latest_global_route_msg_);
}

parametersCallback()

rcl_interfaces::msg::SetParametersResult lanelet2_route_planning::Lanelet2RoutePlanning::parametersCallback ( const std::vector< rclcpp::Parameter > & parameters )

private

Handles reconfiguration when a parameter value is changed.

Parameters

[in]

parameters

parameters

Returns

parameter change result

Definition at line 180 of file lanelet2_route_planning.cpp.

                                                  {
  for (const auto& param : parameters) {
    for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
      if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
        std::get<1>(auto_reconfigurable_param)(param);
        RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str());
        break;
      }
    }
  }
 
  // parameter-specific reconfigurations
  for (const auto& param : parameters) {
    if (param.get_name() == "publish_frequency") {
      publish_timer_->cancel();
      publish_timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / publish_frequency_),
                                               std::bind(&Lanelet2RoutePlanning::publishTimerCallback, this));
    } else if (param.get_name() == "publish_frequency_global") {
      global_route_publish_timer_->cancel();
      global_route_publish_timer_ =
          this->create_wall_timer(std::chrono::duration<double>(1.0 / publish_frequency_global_),
                                  std::bind(&Lanelet2RoutePlanning::globalRoutePublishTimerCallback, this));
    }
  }
  if (enrich_route_ahead_ego_distance_ < 0.0) {
    enrich_route_ahead_ego_distance_ = std::numeric_limits<double>::infinity();
  }
  if (enrich_route_behind_ego_distance_ < 0.0) {
    enrich_route_behind_ego_distance_ = std::numeric_limits<double>::infinity();
  }
  if (max_num_threads_ <= 0) {
    max_num_threads_ = omp_get_max_threads();
  }
  omp_set_num_threads(max_num_threads_);
 
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
 
  return result;
}

planRoute()

bool lanelet2_route_planning::Lanelet2RoutePlanning::planRoute ( const geometry_msgs::msg::PointStamped & destination, const std::vector< geometry_msgs::msg::PointStamped > & intermediate_destinations = {} )

private

Plans a lanelet route to the destination.

Planned lanelet route is stored in latest_route_.

Parameters

[in]

destination

destination

Returns

whether route planning was successful

Definition at line 578 of file lanelet2_route_planning.cpp.

                                                                                                                {
  if (!this->checkMap(false)) {
    RCLCPP_ERROR(this->get_logger(), "Cannot plan route, map not loaded by '%s'", ll2_map_server_name_.c_str());
    return false;
  }
 
  // transform destination to map frame
  geometry_msgs::msg::PointStamped destination_map_stamped;
  if (destination.header.frame_id != ll2_interface_->map_frame_id_) {
    try {
      destination_map_stamped =
          tf_buffer_->transform(destination, ll2_interface_->map_frame_id_, tf2::durationFromSec(transform_timeout_));
    } catch (tf2::TransformException& ex) {
      std::stringstream ss;
      ss << "Could not transform destination from frame '" << destination.header.frame_id << "' to frame '"
         << ll2_interface_->map_frame_id_ << "': " << ex.what();
      RCLCPP_ERROR_STREAM(this->get_logger(), ss.str());
      {
        std::scoped_lock lock(health_kv_mutex_);
        health_kv_["latest_error"] = ss.str();
      }
      return false;
    }
  } else {
    destination_map_stamped = destination;
  }
  geometry_msgs::msg::Point& destination_map = destination_map_stamped.point;
 
  // transform intermediate destinations to map frame
  std::vector<geometry_msgs::msg::Point> intermediate_destinations_map;
  for (const auto& intermediate : intermediate_destinations) {
    geometry_msgs::msg::PointStamped intermediate_map_stamped;
    if (intermediate.header.frame_id != ll2_interface_->map_frame_id_) {
      try {
        intermediate_map_stamped =
            tf_buffer_->transform(intermediate, ll2_interface_->map_frame_id_, tf2::durationFromSec(transform_timeout_));
      } catch (tf2::TransformException& ex) {
        std::stringstream ss;
        ss << "Could not transform intermediate destination from frame '" << intermediate.header.frame_id << "' to frame '"
           << ll2_interface_->map_frame_id_ << "': " << ex.what();
        RCLCPP_ERROR_STREAM(this->get_logger(), ss.str());
        {
          std::scoped_lock lock(health_kv_mutex_);
          health_kv_["latest_error"] = ss.str();
        }
        return false;
      }
      intermediate_destinations_map.push_back(intermediate_map_stamped.point);
    } else {
      intermediate_destinations_map.push_back(intermediate.point);
    }
  }
 
  // get map and traffic rules
  lanelet::LaneletMapConstPtr map = ll2_interface_->getMapPtr();
  lanelet::traffic_rules::TrafficRulesPtr traffic_rules = getTrafficRules();
 
  // check validity of ego data
  const double timeout_ego_data = 1.0;
  if ((this->now() - latest_ego_data_.header.stamp).seconds() > timeout_ego_data) {
    RCLCPP_WARN(this->get_logger(), "Ego data is outdated by %.3fs > %.3fs",
                (this->now() - latest_ego_data_.header.stamp).seconds(), timeout_ego_data);
  }
  if (latest_ego_data_.header.frame_id != ll2_interface_->map_frame_id_) {
    std::stringstream ss;
    ss << "Ego data frame '" << latest_ego_data_.header.frame_id << "' does not match map frame '"
       << ll2_interface_->map_frame_id_ << "'";
    RCLCPP_ERROR_STREAM(this->get_logger(), ss.str());
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["latest_error"] = ss.str();
    }
    return false;
  }
 
  // project ego position to lanelet
  lanelet::ConstLanelet ego_ll;
  if (auto result = laneletAtPoint(toEigen2d(egoPosition(latest_ego_data_)), map)) {
    ego_ll = *result;
  } else {
    std::string msg = "Failed to find lanelet at ego position";
    RCLCPP_ERROR_STREAM(this->get_logger(), msg);
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["latest_error"] = msg;
    }
    return false;
  }
  Eigen::Vector2d ego_ll_position =
      projectPointToLineString(toEigen2d(egoPosition(latest_ego_data_)), toEigen(ego_ll.centerline2d().basicLineString()));
 
  // project destination to lanelet
  lanelet::ConstLanelet destination_ll;
  if (auto result = laneletAtPoint(toEigen2d(destination_map), map)) {
    destination_ll = *result;
  } else {
    std::string msg = "Failed to find lanelet at destination";
    RCLCPP_ERROR_STREAM(this->get_logger(), msg);
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["latest_error"] = msg;
    }
    return false;
  }
  Eigen::Vector2d destination_ll_position =
      projectPointToLineString(toEigen2d(destination_map), toEigen(destination_ll.centerline2d().basicLineString()));
 
  // project intermediate destinations to lanelets
  std::vector<lanelet::ConstLanelet> intermediate_destination_lls;
  std::vector<geometry_msgs::msg::Point> intermediate_destinations_on_route;
  for (const auto& intermediate : intermediate_destinations_map) {
    lanelet::ConstLanelet intermediate_ll;
    if (auto result = laneletAtPoint(toEigen2d(intermediate), map)) {
      intermediate_destination_lls.push_back(*result);
      intermediate_destinations_on_route.push_back(intermediate);
    } else {
      std::stringstream ss;
      ss << "Failed to find lanelet at intermediate point (" << intermediate.x << ", " << intermediate.y << "), skipping";
      RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
      {
        std::scoped_lock lock(health_kv_mutex_);
        health_kv_["latest_warning"] = ss.str();
      }
      continue;  // skip this intermediate if no lanelet found
    }
  }
 
  // undershoot/overshoot route endpoints to enable context before start position and behind destination
  lanelet::ConstLanelet undershot_ego_ll =
      followLaneletsAlongRoutingGraph(routing_graph_, ego_ll, ego_ll_position, -std::abs(route_undershoot_distance_));
  lanelet::ConstLanelet overshot_destination_ll =
      followLaneletsAlongRoutingGraph(routing_graph_, destination_ll, destination_ll_position, route_overshoot_distance_);
 
  // compute route from start to destination along intermediate destinations
  std::vector<lanelet::ConstLanelet> route_lanelets = {undershot_ego_ll};
  route_lanelets.insert(route_lanelets.end(), intermediate_destination_lls.begin(), intermediate_destination_lls.end());
  route_lanelets.push_back(overshot_destination_ll);
  std::optional<lanelet::routing::Route> planned_route = getRoute(routing_graph_, route_lanelets);
 
  if (planned_route) {
    starting_point_ = egoPosition(latest_ego_data_);
    destination_ = destination_map;
    intermediate_destinations_ = intermediate_destinations_on_route;
    latest_route_ = std::move(*planned_route);
    return true;
  } else {
    std::stringstream ss;
    ss << "Failed to plan route from lanelet " << ego_ll.id() << " to lanelet " << destination_ll.id();
    RCLCPP_ERROR_STREAM(this->get_logger(), ss.str());
    {
      std::scoped_lock lock(health_kv_mutex_);
      health_kv_["latest_error"] = ss.str();
    }
    return false;
  }
}

publishHealth()

void lanelet2_route_planning::Lanelet2RoutePlanning::publishHealth ( const unsigned char status, const std::string & msg, const rclcpp::Time & now )

private

Publishes health diagnostic.

Parameters

status	status code
msg	status message
now	current time for stamping

Definition at line 1044 of file lanelet2_route_planning.cpp.

                                                                                                                 {
  diagnostic_msgs::msg::DiagnosticArray diagnostics;
  diagnostics.header.stamp = now;
  auto& health = diagnostics.status.emplace_back();
  health.name = this->get_fully_qualified_name() + std::string(": health");
  health.hardware_id = "none";
  health.level = status;
  health.message = msg;
  std::map<std::string, std::string> health_kv_snapshot;
  {
    std::scoped_lock lock(health_kv_mutex_);
    health_kv_snapshot = health_kv_;
  }
  for (const auto& [key, value] : health_kv_snapshot) {
    auto& key_value = health.values.emplace_back();
    key_value.key = key;
    key_value.value = value;
  }
 
  health_diagnostic_pub_->publish(diagnostics);
}

publishTimerCallback()

void lanelet2_route_planning::Lanelet2RoutePlanning::publishTimerCallback ( )

private

Callback to periodically publish the route.

Definition at line 359 of file lanelet2_route_planning.cpp.

                                                 {
  route_timer_diagnostic_->tick();
  if (is_publishing_route_ && has_enriched_route_) {
    publisher_route_->publish(latest_route_msg_);
  }
 
  // check health
  unsigned char health_status = diagnostic_msgs::msg::DiagnosticStatus::OK;
  std::string health_msg = "OK";
  std::map<std::string, std::string> health_kv_snapshot;
  {
    std::scoped_lock lock(health_kv_mutex_);
    health_kv_snapshot = health_kv_;
  }
  if (health_kv_snapshot.count("map_loaded") > 0 && health_kv_snapshot["map_loaded"] == "false") {
    // map not loaded
    health_status = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
    health_msg = "Map not loaded";
  } else if (health_kv_snapshot.count("latest_error") > 0) {
    // at least one error
    health_status = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
    health_msg = health_kv_snapshot["latest_error"];
  } else if (health_kv_snapshot.count("latest_warning") > 0) {
    // at least one warning
    health_status = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    health_msg = health_kv_snapshot["latest_warning"];
  } else if (health_kv_snapshot.count("dt_buildEnrichedRouteMessage") > 0) {
    // enrichment cannot keep up with ego data
    double dt = std::stod(health_kv_snapshot["dt_buildEnrichedRouteMessage"]);
    double callback_freq = ego_data_diagnostic_config_.max_frequency;
    if (callback_freq > 0.0 && dt > 1.0 / callback_freq) {
      health_status = diagnostic_msgs::msg::DiagnosticStatus::WARN;
      health_msg = "Building enriched route takes longer than ego data frequency allows";
    }
  }
 
  // publish health diagnostics
  this->publishHealth(health_status, health_msg, this->now());
 
  // reset latest warning and error
  {
    std::scoped_lock lock(health_kv_mutex_);
    health_kv_.erase("latest_warning");
    health_kv_.erase("latest_error");
  }
}

setup()

void lanelet2_route_planning::Lanelet2RoutePlanning::setup ( )

private

Sets up subscribers, publishers, etc. to configure the node.

Definition at line 238 of file lanelet2_route_planning.cpp.

                                  {
  // map interface
  ll2_interface_ = std::make_unique<Lanelet2MapInterface>(*this, ll2_map_server_name_);
 
  // callback for dynamic parameter configuration
  parameters_callback_ =
      this->add_on_set_parameters_callback(std::bind(&Lanelet2RoutePlanning::parametersCallback, this, std::placeholders::_1));
 
  // tf transform listener
  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
  // publishers
  publisher_route_ = this->create_publisher<route_planning_msgs::msg::Route>("~/route", 1);
  publisher_global_route_ = this->create_publisher<route_planning_msgs::msg::Route>("~/global_route", 1);
  publish_timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / publish_frequency_),
                                           std::bind(&Lanelet2RoutePlanning::publishTimerCallback, this));
  global_route_publish_timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / publish_frequency_global_),
                                                        std::bind(&Lanelet2RoutePlanning::globalRoutePublishTimerCallback, this));
  is_publishing_route_ = false;
  RCLCPP_INFO(this->get_logger(), "Publishing enriched-only route to '%s'", publisher_route_->get_topic_name());
  RCLCPP_INFO(this->get_logger(), "Publishing non-enriched global route to '%s'", publisher_global_route_->get_topic_name());
 
  // subscribers
  subscriber_ego_data_ = this->create_subscription<perception_msgs::msg::EgoData>(
      "~/ego_data", 1, std::bind(&Lanelet2RoutePlanning::egoDataCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", subscriber_ego_data_->get_topic_name());
 
  // action server for handling action goal requests
  action_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
  action_server_ = rclcpp_action::create_server<route_planning_msgs::action::PlanRoute>(
      this, "~/plan_route",
      std::bind(&Lanelet2RoutePlanning::actionHandleGoal, this, std::placeholders::_1, std::placeholders::_2),
      std::bind(&Lanelet2RoutePlanning::actionHandleCancel, this, std::placeholders::_1),
      std::bind(&Lanelet2RoutePlanning::actionHandleAccepted, this, std::placeholders::_1),
      rcl_action_server_get_default_options(), action_callback_group_);
  RCLCPP_INFO(this->get_logger(), "Action server started");
 
  // set up health diagnostic publisher and diagnostic updater for monitoring topic frequencies and timestamps
  health_diagnostic_pub_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 1);
  diagnostic_updater_.setHardwareID("none");
  ego_data_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
      "~/ego_data", diagnostic_updater_,
      diagnostic_updater::FrequencyStatusParam(&ego_data_diagnostic_config_.min_frequency,
                                               &ego_data_diagnostic_config_.max_frequency, 0.0, 1),
      diagnostic_updater::TimeStampStatusParam(ego_data_diagnostic_config_.min_acceptable_timestamp_delta,
                                               ego_data_diagnostic_config_.max_acceptable_timestamp_delta));
  route_timer_diagnostic_ = std::make_unique<diagnostic_updater::HeaderlessTopicDiagnostic>(
      "route_timer", diagnostic_updater_,
      diagnostic_updater::FrequencyStatusParam(&route_timer_diagnostic_config_.min_frequency,
                                               &route_timer_diagnostic_config_.max_frequency, 0.0, 1));
  global_route_timer_diagnostic_ = std::make_unique<diagnostic_updater::HeaderlessTopicDiagnostic>(
      "global_route_timer", diagnostic_updater_,
      diagnostic_updater::FrequencyStatusParam(&global_route_timer_diagnostic_config_.min_frequency,
                                               &global_route_timer_diagnostic_config_.max_frequency, 0.0, 1));
}

Member Data Documentation

action_callback_group_

rclcpp::CallbackGroup::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::action_callback_group_

private

Callback group for action server.

Definition at line 280 of file lanelet2_route_planning.hpp.

action_feedback_

route_planning_msgs::action::PlanRoute::Feedback::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::action_feedback_

private

Latest action feedback.

Definition at line 265 of file lanelet2_route_planning.hpp.

action_feedback_frequency_

double lanelet2_route_planning::Lanelet2RoutePlanning::action_feedback_frequency_ = 1.0

private

Frequency of action feedback publication [Hz] (parameter)

Definition at line 398 of file lanelet2_route_planning.hpp.

action_goal_handle_

std::shared_ptr<rclcpp_action::ServerGoalHandle<route_planning_msgs::action::PlanRoute> > lanelet2_route_planning::Lanelet2RoutePlanning::action_goal_handle_

private

Latest action goal handle.

Definition at line 275 of file lanelet2_route_planning.hpp.

action_result_

route_planning_msgs::action::PlanRoute::Result::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::action_result_

private

Latest action result.

Definition at line 270 of file lanelet2_route_planning.hpp.

action_server_

rclcpp_action::Server<route_planning_msgs::action::PlanRoute>::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::action_server_

private

Action server.

Definition at line 260 of file lanelet2_route_planning.hpp.

action_start_time_

rclcpp::Time lanelet2_route_planning::Lanelet2RoutePlanning::action_start_time_

private

Latest action start time to compute action duration.

Definition at line 285 of file lanelet2_route_planning.hpp.

auto_reconfigurable_params_

std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)> > > lanelet2_route_planning::Lanelet2RoutePlanning::auto_reconfigurable_params_

private

Auto-reconfigurable parameters for dynamic reconfiguration.

Definition at line 225 of file lanelet2_route_planning.hpp.

destination_

geometry_msgs::msg::Point lanelet2_route_planning::Lanelet2RoutePlanning::destination_

private

Destination point in map frame.

This is affected by project_destination_to_reference_line_.

Definition at line 324 of file lanelet2_route_planning.hpp.

destination_distance_threshold_

double lanelet2_route_planning::Lanelet2RoutePlanning::destination_distance_threshold_ = 1.0

private

Distance to destination where destination is considered reached [m] (parameter)

Definition at line 423 of file lanelet2_route_planning.hpp.

diagnostic_updater_

diagnostic_updater::Updater lanelet2_route_planning::Lanelet2RoutePlanning::diagnostic_updater_

private

Diagnostic updater for monitoring topic frequencies and timestamps.

Definition at line 483 of file lanelet2_route_planning.hpp.

ego_data_diagnostic_

std::unique_ptr<diagnostic_updater::TopicDiagnostic> lanelet2_route_planning::Lanelet2RoutePlanning::ego_data_diagnostic_

private

Diagnostic to auto-diagnose ego data topic.

Definition at line 488 of file lanelet2_route_planning.hpp.

ego_data_diagnostic_config_

TopicDiagnosticConfig lanelet2_route_planning::Lanelet2RoutePlanning::ego_data_diagnostic_config_ = {45.45, 55.55, 0.0, 0.002}

private

Configuration for auto-diagnosed ego data topic.

Definition at line 503 of file lanelet2_route_planning.hpp.

{45.45, 55.55, 0.0, 0.002};

enrich_route_ahead_ego_distance_

double lanelet2_route_planning::Lanelet2RoutePlanning::enrich_route_ahead_ego_distance_ = 100.0

private

Distance ahead of ego position where global route is enriched with more information [m] (negative=unlimited) (parameter)

Definition at line 433 of file lanelet2_route_planning.hpp.

enrich_route_behind_ego_distance_

double lanelet2_route_planning::Lanelet2RoutePlanning::enrich_route_behind_ego_distance_ = 10.0

private

Distance behind ego position where global route is enriched with more information [m] (negative=unlimited) (parameter)

Definition at line 438 of file lanelet2_route_planning.hpp.

global_route_publish_timer_

rclcpp::TimerBase::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::global_route_publish_timer_

private

Timer for publishing the global route.

Definition at line 255 of file lanelet2_route_planning.hpp.

global_route_timer_diagnostic_

std::unique_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> lanelet2_route_planning::Lanelet2RoutePlanning::global_route_timer_diagnostic_

private

Diagnostic to auto-diagnose global route timer frequency.

Definition at line 498 of file lanelet2_route_planning.hpp.

global_route_timer_diagnostic_config_

TopicDiagnosticConfig lanelet2_route_planning::Lanelet2RoutePlanning::global_route_timer_diagnostic_config_ = {0.91, 1.11}

private

Configuration for auto-diagnosed global route timer.

Definition at line 513 of file lanelet2_route_planning.hpp.

{0.91, 1.11};

has_enriched_route_

bool lanelet2_route_planning::Lanelet2RoutePlanning::has_enriched_route_ = false

private

Flag indicating that route has been enriched at least once.

Useful because the initial non-enriched global route is invalid as traveled and remaining route are not yet split.

Definition at line 344 of file lanelet2_route_planning.hpp.

health_diagnostic_pub_

rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::health_diagnostic_pub_

private

Health diagnostic publisher.

Definition at line 478 of file lanelet2_route_planning.hpp.

health_kv_

std::map<std::string, std::string> lanelet2_route_planning::Lanelet2RoutePlanning::health_kv_

private

Health key-value pairs for diagnostic message.

Definition at line 468 of file lanelet2_route_planning.hpp.

health_kv_mutex_

std::mutex lanelet2_route_planning::Lanelet2RoutePlanning::health_kv_mutex_

mutableprivate

Mutex protecting health key-value pairs across threads.

Definition at line 473 of file lanelet2_route_planning.hpp.

intermediate_destinations_

std::vector<geometry_msgs::msg::Point> lanelet2_route_planning::Lanelet2RoutePlanning::intermediate_destinations_

private

Intermediate points in map frame.

Used for route planning with intermediate waypoints. These are affected by project_destination_to_reference_line_.

Definition at line 332 of file lanelet2_route_planning.hpp.

is_publishing_route_

bool lanelet2_route_planning::Lanelet2RoutePlanning::is_publishing_route_

private

Controlling flag for route publication.

Definition at line 349 of file lanelet2_route_planning.hpp.

latest_ego_data_

perception_msgs::msg::EgoData lanelet2_route_planning::Lanelet2RoutePlanning::latest_ego_data_

private

Latest ego data in map frame.

Definition at line 300 of file lanelet2_route_planning.hpp.

latest_full_route_msg_

route_planning_msgs::msg::Route lanelet2_route_planning::Lanelet2RoutePlanning::latest_full_route_msg_

private

Latest complete minimal route used internally for progress tracking.

Definition at line 359 of file lanelet2_route_planning.hpp.

latest_global_route_msg_

route_planning_msgs::msg::Route lanelet2_route_planning::Lanelet2RoutePlanning::latest_global_route_msg_

private

Latest adaptively sampled non-enriched global route.

Definition at line 362 of file lanelet2_route_planning.hpp.

latest_lanelet_idx_by_reference_line_point_idx_

std::vector<size_t> lanelet2_route_planning::Lanelet2RoutePlanning::latest_lanelet_idx_by_reference_line_point_idx_

private

Latest mapping between global route reference line and lanelet indices.

Used to easily find lanelet for a given point on the global reference line. Indexes into latest_route_.shortestPath().

Definition at line 375 of file lanelet2_route_planning.hpp.

latest_reference_line_

std::vector<Eigen::Vector2d> lanelet2_route_planning::Lanelet2RoutePlanning::latest_reference_line_

private

Latest complete dense reference line in the map frame.

Definition at line 367 of file lanelet2_route_planning.hpp.

latest_route_

lanelet::routing::Route lanelet2_route_planning::Lanelet2RoutePlanning::latest_route_

private

Latest planned lanelet route.

Definition at line 337 of file lanelet2_route_planning.hpp.

latest_route_msg_

route_planning_msgs::msg::Route lanelet2_route_planning::Lanelet2RoutePlanning::latest_route_msg_

private

Latest route message to publish.

Definition at line 354 of file lanelet2_route_planning.hpp.

latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_

std::vector<std::vector<int> > lanelet2_route_planning::Lanelet2RoutePlanning::latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_

private

Latest suggested turn signal distance ahead by route element by lane element.

Used to store the suggested turn signal distances ahead for each lane element, which can only be propagated to preceding lane elements during postprocessing.

Definition at line 383 of file lanelet2_route_planning.hpp.

ll2_interface_

std::unique_ptr<Lanelet2MapInterface> lanelet2_route_planning::Lanelet2RoutePlanning::ll2_interface_

private

Lanelet2 map interface.

Definition at line 305 of file lanelet2_route_planning.hpp.

ll2_map_server_name_

std::string lanelet2_route_planning::Lanelet2RoutePlanning::ll2_map_server_name_ = "lanelet2_map_server"

private

Name of lanelet2_map_server node (parameter)

Definition at line 388 of file lanelet2_route_planning.hpp.

max_drivable_space_radius_

double lanelet2_route_planning::Lanelet2RoutePlanning::max_drivable_space_radius_ = 50.0

private

Maximum distance to left/right drivable space bounds, if not otherwise restricted [m] (parameter)

Definition at line 453 of file lanelet2_route_planning.hpp.

max_num_threads_

int lanelet2_route_planning::Lanelet2RoutePlanning::max_num_threads_ = 0

private

Maximum number of threads for parallel processing (0=max available) (parameter)

Definition at line 458 of file lanelet2_route_planning.hpp.

parameters_callback_

OnSetParametersCallbackHandle::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::parameters_callback_

private

Callback handle for dynamic parameter reconfiguration.

Definition at line 230 of file lanelet2_route_planning.hpp.

project_destination_to_reference_line_

bool lanelet2_route_planning::Lanelet2RoutePlanning::project_destination_to_reference_line_ = true

private

Whether to project destination to reference line (parameter)

Definition at line 418 of file lanelet2_route_planning.hpp.

publish_frequency_

double lanelet2_route_planning::Lanelet2RoutePlanning::publish_frequency_ = 10.0

private

Frequency of route publication [Hz] (parameter)

Definition at line 393 of file lanelet2_route_planning.hpp.

publish_frequency_global_

double lanelet2_route_planning::Lanelet2RoutePlanning::publish_frequency_global_ = 1.0

private

Frequency of global route publication [Hz] (parameter)

Definition at line 403 of file lanelet2_route_planning.hpp.

publish_timer_

rclcpp::TimerBase::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::publish_timer_

private

Timer for publishing the enriched route.

Definition at line 250 of file lanelet2_route_planning.hpp.

publisher_global_route_

rclcpp::Publisher<route_planning_msgs::msg::Route>::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::publisher_global_route_

private

Publisher for the adaptively sampled global route.

Definition at line 245 of file lanelet2_route_planning.hpp.

publisher_route_

rclcpp::Publisher<route_planning_msgs::msg::Route>::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::publisher_route_

private

Publisher for enriched route.

Definition at line 240 of file lanelet2_route_planning.hpp.

required_traveled_distance_proportion_

double lanelet2_route_planning::Lanelet2RoutePlanning::required_traveled_distance_proportion_ = 0.5

private

Proportion of route length that must have been traveled before considering destination reached [0..1] (parameter)

Definition at line 428 of file lanelet2_route_planning.hpp.

route_overshoot_distance_

double lanelet2_route_planning::Lanelet2RoutePlanning::route_overshoot_distance_ = 0.0

private

Overshoot route by this distance behind destination [m] (parameter)

Definition at line 448 of file lanelet2_route_planning.hpp.

route_timer_diagnostic_

std::unique_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> lanelet2_route_planning::Lanelet2RoutePlanning::route_timer_diagnostic_

private

Diagnostic to auto-diagnose route timer frequency.

Definition at line 493 of file lanelet2_route_planning.hpp.

route_timer_diagnostic_config_

TopicDiagnosticConfig lanelet2_route_planning::Lanelet2RoutePlanning::route_timer_diagnostic_config_ = {18.18, 22.22}

private

Configuration for auto-diagnosed route timer.

Definition at line 508 of file lanelet2_route_planning.hpp.

{18.18, 22.22};

route_undershoot_distance_

double lanelet2_route_planning::Lanelet2RoutePlanning::route_undershoot_distance_ = 0.0

private

Undershoot route by this distance before ego position [m] (parameter)

Definition at line 443 of file lanelet2_route_planning.hpp.

routing_graph_

lanelet::routing::RoutingGraphUPtr lanelet2_route_planning::Lanelet2RoutePlanning::routing_graph_

private

Lanelet routing graph for current map.

Definition at line 310 of file lanelet2_route_planning.hpp.

sampling_distance_

double lanelet2_route_planning::Lanelet2RoutePlanning::sampling_distance_ = 1.0

private

Distance between resampled points along route [m] (parameter)

Definition at line 408 of file lanelet2_route_planning.hpp.

sampling_max_lateral_error_global_

double lanelet2_route_planning::Lanelet2RoutePlanning::sampling_max_lateral_error_global_ = 0.5

private

Maximum lateral error of the adaptively sampled global reference line [m] (parameter)

Definition at line 413 of file lanelet2_route_planning.hpp.

starting_point_

geometry_msgs::msg::Point lanelet2_route_planning::Lanelet2RoutePlanning::starting_point_

private

Starting point in map frame.

This is affected by project_destination_to_reference_line_.

Definition at line 317 of file lanelet2_route_planning.hpp.

subscriber_ego_data_

rclcpp::Subscription<perception_msgs::msg::EgoData>::SharedPtr lanelet2_route_planning::Lanelet2RoutePlanning::subscriber_ego_data_

private

Subscriber for ego data.

Definition at line 235 of file lanelet2_route_planning.hpp.

tf_buffer_

std::unique_ptr<tf2_ros::Buffer> lanelet2_route_planning::Lanelet2RoutePlanning::tf_buffer_

private

Transform buffer.

Definition at line 290 of file lanelet2_route_planning.hpp.

tf_listener_

std::shared_ptr<tf2_ros::TransformListener> lanelet2_route_planning::Lanelet2RoutePlanning::tf_listener_

private

Transform listener.

Definition at line 295 of file lanelet2_route_planning.hpp.

transform_timeout_

double lanelet2_route_planning::Lanelet2RoutePlanning::transform_timeout_ = 0.02

private

How long to wait for a transform to be available [s] (parameter)

Definition at line 463 of file lanelet2_route_planning.hpp.

---

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

• lanelet2_route_planning/include/lanelet2_route_planning/lanelet2_route_planning.hpp
• lanelet2_route_planning/src/lanelet2_route_planning.cpp