simple_planner::SimplePlannerNode Class Reference

#include <simple_planner.hpp>

Inheritance diagram for simple_planner::SimplePlannerNode:

Classes
struct	DiagnosticStatus
	Diagnostic status indicating node health. More...
struct	FollowRoutePlan

Public Member Functions
	SimplePlannerNode ()
	Creates a SimplePlannerNode node.

Private Types
enum	InterpolationType { LINEAR = 0 , SPLINE = 1 }
enum class	PlannerState { NoPublish , Standstill , SafeStop , FollowRoute }

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 a ROS parameter, loads its value and optionally registers it for runtime updates.
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.
void	egoDataCallback (const perception_msgs::msg::EgoData::UniquePtr msg)
	Stores the latest ego data message.
void	objectListCallback (const perception_msgs::msg::ObjectList::UniquePtr msg)
	Stores the latest perceived object list including object predictions.
void	routeCallback (const route_planning_msgs::msg::Route::UniquePtr msg)
	Stores the latest route message and extracts the route path.
void	gridMapCallback (const nav_msgs::msg::OccupancyGrid::UniquePtr msg)
	Stores the latest occupancy grid map message.
void	publishTimerCallback ()
	This callback is invoked every period seconds by the timer.
PlannerState	determinePlannerState (const rclcpp::Time &stamp)
	Determines the current planner state from input freshness and route availability.
trajectory_planning_msgs::msg::Trajectory	createTrajectory (PlannerState state, const rclcpp::Time &stamp)
	Creates a trajectory for the already determined planner state.
trajectory_planning_msgs::msg::Trajectory	buildTrajectoryFromSimplePath (const SimplePath &path)
	Builds a trajectory message from a simple path.
void	clearObjectInteractionMarkers (const std_msgs::msg::Header &target_header)
	Deletes the currently published object interaction markers.
void	publishObjectInteractionMarkers (const std_msgs::msg::Header &target_header, const std::optional< ConflictSample > &conflict)
	Publishes RViz markers for the current object interaction conflict.
SimplePath	buildSafeStopPath (const std_msgs::msg::Header &target_header)
	Builds the initial safe-stop path for the current cycle.
FollowRoutePlan	buildRoutePlan (const std_msgs::msg::Header &target_header)
	Builds the complete route-following plan including stop and turn information.
bool	hasValidGridMap (const rclcpp::Time &stamp) const
	Checks whether a fresh, structurally valid grid map is currently available.
void	applyObjectConstraints (const std_msgs::msg::Header &target_header, const std::vector< SimplePathPoint > &base_path_points, FollowRoutePlan &route_plan)
	Applies trajectory-based object conflict constraints to a follow-route plan.
void	applyGridMapConstraints (const std_msgs::msg::Header &target_header, std::vector< SimplePathPoint > &base_path_points, FollowRoutePlan &route_plan)
	Applies occupancy-grid-based stop constraints to the base route path.
std::optional< double >	findFirstGridMapStopS (const std_msgs::msg::Header &target_header, const std::vector< SimplePathPoint > &base_path_points)
	Finds the last safe grid-map sample before the first blocked pose.
std::vector< ObjectTrajectory >	buildObjectTrajectories (const perception_msgs::msg::ObjectList &tf_object_list, const rclcpp::Time &stamp) const
	Reduces the perceived object list (in vehicle frame) to timed bounding-box trajectories.
std::optional< ConflictSample >	firstConflict (const std::vector< SimplePathPoint > &ego_path, const std::vector< ObjectTrajectory > &object_trajectories) const
	Returns the first conflict between the (time-sampled) ego path and any object trajectory.
void	resetObjectState (const std_msgs::msg::Header &target_header)
	Resets the remembered object speed cap / hysteresis state and clears interaction markers.
void	appendRoutePoints (const route_planning_msgs::msg::Route &tf_route, FollowRoutePlan &route_plan, std::map< uint64_t, uint64_t > &lane_change_indices_map)
	Appends route-derived path points and stop metadata for the follow-route case.
bool	tryRegisterLaneChange (const route_planning_msgs::msg::Route &tf_route, size_t route_element_idx, std::map< uint64_t, uint64_t > &lane_change_indices_map, uint8_t &suggested_turn_signal)
	Detects and stores the start/end window of a lane change.
void	updateForTrafficLights (const route_planning_msgs::msg::Route &tf_route, size_t route_element_idx, const route_planning_msgs::msg::LaneElement &suggested_lane, const SimplePathPoint &simple_path_point, double t_total, bool &stop_at_end, double &offset_to_stop_line)
	Updates stop-at-end and stop-line offset state for traffic-light regulatory elements.
std::vector< SimplePathPoint >	mergeLaneChangeSegments (const route_planning_msgs::msg::Route &tf_route, const std::vector< SimplePathPoint > &route_points, const std::map< uint64_t, uint64_t > &lane_change_indices_map)
	Merges interpolated lane-change segments into the base route path.
void	applyIndicatorRequest (uint8_t suggested_turn_signal)
	Requests the appropriate indicator state for the current route plan.
std::vector< SimplePathPoint >	resamplePath (const std::vector< SimplePathPoint > &path, bool stop_at_end, double offset_to_stop_line=0.0, const double *speed_cap=nullptr)
	Resamples a path into trajectory time steps and applies optional stopping behavior.
std::vector< SimplePathPoint >	generateLaneChangePath (size_t start_idx, size_t turn_idx, const route_planning_msgs::msg::Route &route)
	Generates interpolated points for a lane-change section of the route.
SimplePath	calculateSafeStopAlongEgoHeading (const perception_msgs::msg::EgoData &ego_data, const double safe_stop_distance, const std_msgs::msg::Header &target_header)
	Creates a minimal safe-stop path along the current ego heading.
SimplePath	calculateSafeStopAlongRoute (const SimplePath &path, const double safe_stop_distance)
	Truncates and resamples an existing path to stop within the safe-stop distance.
SimplePath	transformPath (const SimplePath &path, const std_msgs::msg::Header &target_header)
	Transforms a simple path into the requested target frame and timestamp.
void	health (diagnostic_updater::DiagnosticStatusWrapper &stat)
	Function called by diagnostic updater to populate diagnostics status.
void	setHealth (const unsigned char status, const std::string &msg, const std::map< std::string, std::string > &key_value_pairs={})
	Sets the health information.

Static Private Member Functions
static bool	isMessageOutdated (const std_msgs::msg::Header &header, double timeout, const rclcpp::Time &stamp)
	Checks whether an input message is older than the configured timeout.
static trajectory_planning_msgs::msg::Trajectory	buildStandstillTrajectory (const std_msgs::msg::Header &target_header)
	Creates a standstill trajectory for the current planning cycle.
static void	trimPathBehindEgo (SimplePath &path)
	Removes path points that lie behind the ego vehicle in vehicle frame.
static std::vector< SimplePathPoint >	truncatePathAtS (const std::vector< SimplePathPoint > &path, double stop_s)
	Returns a path ending exactly at the requested accumulated path coordinate.
static void	recalculateS (std::vector< SimplePathPoint > &path)
	Recomputes accumulated path distance from point positions.
static bool	requiresTransform (const std_msgs::msg::Header &source_header, const std_msgs::msg::Header &target_header)
	Checks whether a transform between two stamped frames is required.
static std::string	plannerStateToString (const PlannerState &state)
	Converts a PlannerState enum to a string representation.
static std::string	turnSignalToString (const uint8_t &turn_signal)
	Converts a turn signal value to a string representation.

Private Attributes
std::unique_ptr< tf2_ros::Buffer >	tf2_buffer_
std::shared_ptr< tf2_ros::TransformListener >	tf2_listener_
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr	sub_egoData_
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr	sub_object_list_
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr	sub_route_
rclcpp::Subscription< nav_msgs::msg::OccupancyGrid >::SharedPtr	sub_grid_map_
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr	pub_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	object_interaction_marker_pub_
rclcpp::TimerBase::SharedPtr	publish_timer_
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr	left_turn_indicator_service_client_
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr	right_turn_indicator_service_client_
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr	hazard_lights_service_client_
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.
std::string	vehicle_frame_id_ = "base_link"
std::string	trajectory_frame_id_ = "base_link"
std::string	fixed_over_time_frame_id_ = "map"
double	freq_ = 10.0
double	route_timeout_ = 1.0
double	ego_data_timeout_ = 1.0
double	object_timeout_ = 1.0
double	grid_map_timeout_ = 1.0
double	trajectory_horizon_ = 10.0
int	n_states_ = 51
uint8_t	interpolation_type_ = InterpolationType::SPLINE
double	v_ref_ = 13.89
double	a_decel_ = -0.5
double	a_max_decel_ = -1.0
bool	trigger_turn_signals_ = true
bool	consider_grid_map_ = false
int	grid_occupied_threshold_ = 50
bool	consider_out_of_grid_ = false
double	grid_longitudinal_safety_distance_ = 1.5
double	grid_lateral_safety_distance_ = 0.0
bool	consider_traffic_lights_ = true
double	offset_to_stop_line_ = 0.0
double	ignore_stop_line_threshold_ = 0.5
bool	consider_future_states_ = false
bool	consider_objects_ = true
double	min_prediction_prob_ = 0.0
double	object_longitudinal_safety_distance_ = 1.5
double	object_lateral_safety_distance_ = 0.0
double	object_interaction_time_window_ = 0.5
double	object_velocity_reduction_step_ = 0.1
double	object_velocity_release_step_ = 0.5
double	object_standstill_speed_threshold_ = 0.2
int	object_velocity_release_hysteresis_cycles_ = 3
bool	publish_object_interaction_markers_ = true
double	lane_change_distance_factor_ = 6.0
double	lane_change_min_distance_factor_ = 2.0
perception_msgs::msg::EgoData	ego_data_
perception_msgs::msg::ObjectList	object_list_
route_planning_msgs::msg::Route	route_
nav_msgs::msg::OccupancyGrid	grid_map_
bool	ego_data_init_ = false
bool	object_list_init_ = false
bool	route_init_ = false
bool	grid_map_init_ = false
std::optional< double >	safe_stop_distance_
std::optional< double >	last_object_speed_cap_
SimplePath	latest_path_
int	object_conflict_free_cycles_ = 0
double	dt_
diagnostic_updater::Updater	diagnostic_updater_ {this}
	Diagnostic updater.
struct simple_planner::SimplePlannerNode::DiagnosticStatus	health_
std::unique_ptr< diagnostic_updater::TopicDiagnostic >	ego_data_topic_diagnostic_
TopicDiagnosticConfig	ego_data_topic_diagnostic_config_ {45.45, 55.55, 0.0, 0.002}
std::unique_ptr< diagnostic_updater::TopicDiagnostic >	object_list_topic_diagnostic_
TopicDiagnosticConfig	object_list_topic_diagnostic_config_ {9.09, 11.11, 0.0, 0.01}
std::unique_ptr< diagnostic_updater::TopicDiagnostic >	route_topic_diagnostic_
TopicDiagnosticConfig	route_topic_diagnostic_config_ {18.18, 22.22, 0.0, 0.005}
std::unique_ptr< diagnostic_updater::TopicDiagnostic >	grid_map_topic_diagnostic_
TopicDiagnosticConfig	grid_map_topic_diagnostic_config_ {9.09, 11.11, 0.0, 0.01}
std::unique_ptr< diagnostic_updater::DiagnosedPublisher< trajectory_planning_msgs::msg::Trajectory > >	diagnosed_publisher_
TopicDiagnosticConfig	diagnosed_publisher_config_ {9.09, 11.11, 0.0, 0.01}

Static Private Attributes
static constexpr double	kObjectCollisionCheckDt = 0.05
static constexpr double	kMinObjectWidth = 0.8
static constexpr double	kMinObjectLength = 1.2

Detailed Description

Definition at line 101 of file simple_planner.hpp.

Member Enumeration Documentation

InterpolationType

enum simple_planner::SimplePlannerNode::InterpolationType

private

Enumerator
LINEAR
SPLINE

Definition at line 106 of file simple_planner.hpp.

{ LINEAR = 0, SPLINE = 1 };

PlannerState

enum class simple_planner::SimplePlannerNode::PlannerState

strongprivate

Enumerator
NoPublish
Standstill
SafeStop
FollowRoute

Definition at line 107 of file simple_planner.hpp.

{ NoPublish, Standstill, SafeStop, FollowRoute };

Constructor & Destructor Documentation

SimplePlannerNode()

simple_planner::SimplePlannerNode::SimplePlannerNode

(

)

Creates a SimplePlannerNode node.

Definition at line 32 of file simple_planner.cpp.

                                     : Node("simple_planner_node") {
  this->declareAndLoadParameter("vehicle_frame_id", vehicle_frame_id_,
                                "Frame ID of local vehicle frame in which the trajectory is planned");
  this->declareAndLoadParameter("trajectory_frame_id", trajectory_frame_id_, "Frame ID of published reference trajectory");
  this->declareAndLoadParameter("fixed_over_time_frame_id", fixed_over_time_frame_id_,
                                "Frame ID of frame that is fixed over time for finding temporal transforms");
  this->declareAndLoadParameter("frequency", freq_, "Frequency of reference planning cycle (Hz)");
  this->declareAndLoadParameter("route_timeout", route_timeout_,
                                "Time after which a received route is considered invalid (s) (use -1 for no timeout)");
  this->declareAndLoadParameter("ego_data_timeout", ego_data_timeout_,
                                "Time after which a received ego vehicle data is considered invalid (s) (use -1 for no timeout)");
  this->declareAndLoadParameter("object_timeout", object_timeout_,
                                "Time after which a received object list is considered invalid (s) (use -1 for no timeout)");
  this->declareAndLoadParameter("grid_map_timeout", grid_map_timeout_,
                                "Time after which a received grid map is considered invalid (s) (use -1 for no timeout)");
  this->declareAndLoadParameter("trajectory_horizon", trajectory_horizon_, "Time horizon of the reference trajectory (s)");
  this->declareAndLoadParameter("n_states", n_states_, "Number of states in the output trajectory");
  this->declareAndLoadParameter("interpolation_type", interpolation_type_, "0: linear, 1: cubic spline");
  this->declareAndLoadParameter(
      "v_ref", v_ref_,
      "Reference velocity (m/s); set for all states in the trajectory. Set to '-1.0' to use velocity from route.");
  this->declareAndLoadParameter("a_decel", a_decel_,
                                "Desired deceleration for braking at stop lines or end of route (m/s^2) - must be < 0.0");
  this->declareAndLoadParameter("a_max_decel", a_max_decel_,
                                "Maximum deceleration for safe-stop trajectories (m/s^2) - must be < 0.0 and <= a_decel");
  this->declareAndLoadParameter("trigger_turn_signals", trigger_turn_signals_,
                                "True: planner will trigger turn signal services; false: planner will not request turn signals");
  this->declareAndLoadParameter("consider_grid_map", consider_grid_map_,
                                "True: planner will consider grid map; false: planner will ignore grid map");
  this->declareAndLoadParameter("grid_occupied_threshold", grid_occupied_threshold_,
                                "Minimum occupancy value that is considered as blocked within the grid map", true, false, false,
                                0.0, 100.0, 1.0);
  this->declareAndLoadParameter("consider_out_of_grid", consider_out_of_grid_,
                                "True: path points falling outside the grid map are treated as blocked; false: points outside "
                                "the grid map are treated as free to drive");
  this->declareAndLoadParameter("grid_longitudinal_safety_distance", grid_longitudinal_safety_distance_,
                                "Longitudinal ego-box margin for occupied-cell collision checks; negative values shrink the box "
                                "(m)",
                                true, false, false, -20.0, 20.0, 0.1);
  this->declareAndLoadParameter("grid_lateral_safety_distance", grid_lateral_safety_distance_,
                                "Lateral ego-box margin for occupied-cell collision checks; negative values shrink the box (m)",
                                true, false, false, -10.0, 10.0, 0.1);
  this->declareAndLoadParameter("consider_traffic_lights", consider_traffic_lights_,
                                "True: planner will consider traffic lights; false: planner will ignore traffic lights");
  this->declareAndLoadParameter("offset_to_stop_line", offset_to_stop_line_,
                                "Additional distance to stop in front of a stop line (m) (default: 0.0 -> stops with "
                                "front of vehicle at stop line)");
  this->declareAndLoadParameter(
      "ignore_stop_line_threshold", ignore_stop_line_threshold_,
      "A stop line will be ignored if the front of the vehicle has already passed the stop line by more than this threshold (m)");
  this->declareAndLoadParameter("consider_future_states", consider_future_states_,
                                "True: trajectory will consider forecast of traffic light states; false: trajectory will only "
                                "consider current traffic light state");
  this->declareAndLoadParameter("consider_objects", consider_objects_,
                                "True: planner will consider perceived objects on the route; false: planner will ignore objects");
  this->declareAndLoadParameter("min_prediction_prob", min_prediction_prob_,
                                "Minimum probability for considering an object prediction branch");
  this->declareAndLoadParameter("object_longitudinal_safety_distance", object_longitudinal_safety_distance_,
                                "Longitudinal ego-box margin for object conflict detection; negative values shrink the box (m)",
                                true, false, false, -20.0, 20.0, 0.1);
  this->declareAndLoadParameter("object_lateral_safety_distance", object_lateral_safety_distance_,
                                "Lateral ego-box margin for object conflict detection; negative values shrink the box (m)", true,
                                false, false, -10.0, 10.0, 0.1);
  this->declareAndLoadParameter("object_interaction_time_window", object_interaction_time_window_,
                                "Maximum time offset for counting a spatial overlap as interaction (s)");
  this->declareAndLoadParameter("object_velocity_reduction_step", object_velocity_reduction_step_,
                                "Velocity decrement per object-avoidance iteration (m/s)", true, false, false, 1e-3, 40.0, 1e-3);
  this->declareAndLoadParameter("object_velocity_release_step", object_velocity_release_step_,
                                "Maximum velocity increase per cycle after hysteresis cleared object conflicts (m/s)", true,
                                false, false, 1e-3, 40.0, 1e-3);
  this->declareAndLoadParameter("object_standstill_speed_threshold", object_standstill_speed_threshold_,
                                "Publish standstill if object avoidance would require a lower speed cap (m/s)", true, false,
                                false, 0.0, 10.0, 1e-3);
  this->declareAndLoadParameter("object_velocity_release_hysteresis_cycles", object_velocity_release_hysteresis_cycles_,
                                "Number of conflict-free cycles required before increasing the remembered object speed cap", true,
                                false, false, 0.0, 100.0, 1.0);
  this->declareAndLoadParameter("publish_object_interaction_markers", publish_object_interaction_markers_,
                                "Publish RViz markers for the conflict explaining the final speed reduction");
  this->declareAndLoadParameter("lane_change_distance_factor", lane_change_distance_factor_,
                                "Factor multiplied with the current velocity to determine the lane change distance (m)");
  this->declareAndLoadParameter("lane_change_min_distance_factor", lane_change_min_distance_factor_,
                                "Factor multiplied with the vehicle length to determine the minimum lane change distance (m)");
 
  // check parameters
  if (a_decel_ >= 0.0) {
    RCLCPP_ERROR(this->get_logger(), "Invalid parameter: a_decel must be < 0.0");
    exit(EXIT_FAILURE);
  }
  if (a_max_decel_ > a_decel_) {
    RCLCPP_ERROR(this->get_logger(), "Invalid parameter: a_max_decel must be < 0.0 and <= a_decel");
    exit(EXIT_FAILURE);
  }
 
  // diagnostics parameters
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.min_frequency",
                                ego_data_topic_diagnostic_config_.min_frequency,
                                "Minimum frequency for incoming ego-data messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.max_frequency",
                                ego_data_topic_diagnostic_config_.max_frequency,
                                "Maximum frequency for incoming ego-data messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.min_acceptable_timestamp_delta",
                                ego_data_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                "Minimum acceptable timestamp delta for incoming ego-data messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.max_acceptable_timestamp_delta",
                                ego_data_topic_diagnostic_config_.max_acceptable_timestamp_delta,
                                "Maximum acceptable timestamp delta for incoming ego-data messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.min_frequency",
                                object_list_topic_diagnostic_config_.min_frequency,
                                "Minimum frequency for incoming object-list messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.max_frequency",
                                object_list_topic_diagnostic_config_.max_frequency,
                                "Maximum frequency for incoming object-list messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.min_acceptable_timestamp_delta",
                                object_list_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                "Minimum acceptable timestamp delta for incoming object-list messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.max_acceptable_timestamp_delta",
                                object_list_topic_diagnostic_config_.max_acceptable_timestamp_delta,
                                "Maximum acceptable timestamp delta for incoming object-list messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.min_frequency",
                                route_topic_diagnostic_config_.min_frequency, "Minimum frequency for incoming route messages",
                                false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.max_frequency",
                                route_topic_diagnostic_config_.max_frequency, "Maximum frequency for incoming route messages",
                                false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.min_acceptable_timestamp_delta",
                                route_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                "Minimum acceptable timestamp delta for incoming route messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.max_acceptable_timestamp_delta",
                                route_topic_diagnostic_config_.max_acceptable_timestamp_delta,
                                "Maximum acceptable timestamp delta for incoming route messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.min_frequency",
                                grid_map_topic_diagnostic_config_.min_frequency,
                                "Minimum frequency for incoming grid-map messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.max_frequency",
                                grid_map_topic_diagnostic_config_.max_frequency,
                                "Maximum frequency for incoming grid-map messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.min_acceptable_timestamp_delta",
                                grid_map_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                "Minimum acceptable timestamp delta for incoming grid-map messages", false);
  this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.max_acceptable_timestamp_delta",
                                grid_map_topic_diagnostic_config_.max_acceptable_timestamp_delta,
                                "Maximum acceptable timestamp delta for incoming grid-map messages", false);
  this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.min_frequency",
                                diagnosed_publisher_config_.min_frequency, "Minimum frequency for published trajectory messages",
                                false);
  this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.max_frequency",
                                diagnosed_publisher_config_.max_frequency, "Maximum frequency for published trajectory messages",
                                false);
  this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.min_acceptable_timestamp_delta",
                                diagnosed_publisher_config_.min_acceptable_timestamp_delta,
                                "Minimum acceptable timestamp delta for published trajectory messages", false);
  this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.max_acceptable_timestamp_delta",
                                diagnosed_publisher_config_.max_acceptable_timestamp_delta,
                                "Maximum acceptable timestamp delta for published trajectory messages", false);
 
  this->setup();
}

Member Function Documentation

appendRoutePoints()

void simple_planner::SimplePlannerNode::appendRoutePoints ( const route_planning_msgs::msg::Route & tf_route, FollowRoutePlan & route_plan, std::map< uint64_t, uint64_t > & lane_change_indices_map )

private

Appends route-derived path points and stop metadata for the follow-route case.

Parameters

[in]	tf_route	Route transformed into vehicle frame.
[in,out]	route_plan	Mutable route planning result to extend.
[out]	lane_change_indices_map	Output lane-change windows to be merged later.

Definition at line 643 of file simple_planner.cpp.

                                                                                               {
  double t_total = 0.0;
  RCLCPP_DEBUG(this->get_logger(), "Number of remaining route elements: %zu",
               tf_route.destination_route_element_idx - tf_route.current_route_element_idx);
  health_.key_value_pairs.insert(
      {"RemainingRouteElements", std::to_string(tf_route.destination_route_element_idx - tf_route.current_route_element_idx)});
  for (size_t j = tf_route.current_route_element_idx; j < tf_route.destination_route_element_idx; ++j) {
    const auto& route_element = tf_route.route_elements[j];
    if (!route_element.is_enriched) {
      RCLCPP_DEBUG(this->get_logger(), "Route element %zu is not enriched. Skipping.", j);
      continue;
    }
 
    const auto& suggested_lane = route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
    SimplePathPoint simple_path_point;
    simple_path_point.position =
        Eigen::Vector2d(suggested_lane.reference_pose.position.x, suggested_lane.reference_pose.position.y);
    simple_path_point.s = route_element.s;
    simple_path_point.v = v_ref_;
    if (v_ref_ < 0.0) {
      simple_path_point.v = suggested_lane.speed_limit / 3.6;
    }
 
    if (!route_plan.path.points.empty()) {
      double v_average = (route_plan.path.points.back().v + simple_path_point.v) / 2.0;
      double dt = 0.0;
      if (v_average != 0.0) {
        dt = (simple_path_point.s - route_plan.path.points.back().s) / v_average;
      }
      if (dt <= 0.0) {
        std::string msg = "Negative time difference " + std::to_string(dt) +
                          " between points at s=" + std::to_string(route_plan.path.points.back().s) +
                          " and s=" + std::to_string(simple_path_point.s) + ". Could lead to unexpected behavior.";
        setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
        RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      }
      t_total += dt;
    }
 
    if (j == tf_route.current_route_element_idx) {
      route_plan.suggested_turn_signal = suggested_lane.suggested_turn_signal;
    }
 
    if (route_element.will_change_suggested_lane &&
        !tryRegisterLaneChange(tf_route, j, lane_change_indices_map, route_plan.suggested_turn_signal)) {
      break;
    }
 
    if (consider_traffic_lights_) {
      updateForTrafficLights(tf_route, j, suggested_lane, simple_path_point, t_total, route_plan.stop_at_end,
                             route_plan.offset_to_stop_line);
      if (route_plan.stop_at_end) route_plan.reason_to_stop = "Traffic light indicates stop";
    }
 
    route_plan.path.points.push_back(simple_path_point);
 
    if (j == tf_route.destination_route_element_idx - 1) {
      SimplePathPoint destination_point;
      destination_point.position = Eigen::Vector2d(tf_route.destination.x, tf_route.destination.y);
      destination_point.s = simple_path_point.s + (destination_point.position - simple_path_point.position).norm();
      destination_point.v = simple_path_point.v;
      route_plan.path.points.push_back(destination_point);
      route_plan.reason_to_stop = "Reaching end of route";
      route_plan.stop_at_end = true;
    }
    if (route_plan.stop_at_end || t_total >= 2.0 * trajectory_horizon_) {
      break;
    }
  }
}

applyGridMapConstraints()

void simple_planner::SimplePlannerNode::applyGridMapConstraints ( const std_msgs::msg::Header & target_header, std::vector< SimplePathPoint > & base_path_points, FollowRoutePlan & route_plan )

private

Applies occupancy-grid-based stop constraints to the base route path.

Parameters

[in]	target_header	Current planning header propagated from the timer callback.
[in,out]	base_path_points	Route path before time-based resampling.
[in,out]	route_plan	Mutable route planning result to be constrained by the grid map.

Definition at line 39 of file grid_map_handling.cpp.

                                                                             {
  if (!consider_grid_map_ || base_path_points.empty()) {
    return;
  }
 
  std::optional<double> stop_s;
  try {
    stop_s = findFirstGridMapStopS(target_header, base_path_points);
  } catch (const tf2::TransformException& ex) {
    const std::string msg =
        "Grid map transformation is not available: " + std::string(ex.what()) + ". Ignoring grid map for this planning cycle.";
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    return;
  }
 
  if (!stop_s.has_value()) {
    return;
  }
 
  base_path_points = truncatePathAtS(base_path_points, *stop_s);
  route_plan.stop_at_end = true;
  route_plan.offset_to_stop_line = 0.0;
  route_plan.reason_to_stop = "Grid map obstacle";
 
  RCLCPP_INFO(this->get_logger(), "Applying stop for grid-map obstacle at last safe s=%f m", *stop_s);
}

applyIndicatorRequest()

void simple_planner::SimplePlannerNode::applyIndicatorRequest ( uint8_t suggested_turn_signal )

private

Requests the appropriate indicator state for the current route plan.

Parameters

[in]

suggested_turn_signal

Suggested signal derived from route semantics.

Definition at line 911 of file simple_planner.cpp.

                                                                           {
  if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE &&
      left_turn_indicator_service_client_->service_is_ready() && right_turn_indicator_service_client_->service_is_ready() &&
      hazard_lights_service_client_->service_is_ready()) {
    auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
    request->data = false;
    left_turn_indicator_service_client_->async_send_request(request);
    right_turn_indicator_service_client_->async_send_request(request);
    hazard_lights_service_client_->async_send_request(request);
  } else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT &&
             left_turn_indicator_service_client_->service_is_ready()) {
    auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
    request->data = true;
    left_turn_indicator_service_client_->async_send_request(request);
  } else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT &&
             right_turn_indicator_service_client_->service_is_ready()) {
    auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
    request->data = true;
    right_turn_indicator_service_client_->async_send_request(request);
  } else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_HAZARD &&
             hazard_lights_service_client_->service_is_ready()) {
    auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
    request->data = true;
    hazard_lights_service_client_->async_send_request(request);
  } else {
    std::string msg = "Cannot apply suggested turn signal " + std::to_string(suggested_turn_signal) +
                      " because the corresponding indicator service is not ready.";
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
  }
}

applyObjectConstraints()

void simple_planner::SimplePlannerNode::applyObjectConstraints ( const std_msgs::msg::Header & target_header, const std::vector< SimplePathPoint > & base_path_points, FollowRoutePlan & route_plan )

private

Applies trajectory-based object conflict constraints to a follow-route plan.

The object list is transformed into vehicle frame and checked against the already planned ego trajectory. If conflicts are detected, the path is resampled repeatedly with a lower speed cap until it is conflict-free or the speed cap reaches zero.

Parameters

target_header	Current planning header propagated from the timer callback.
base_path_points	Route path before time-based resampling.
route_plan	Mutable route plan to be constrained by dynamic objects.

Definition at line 227 of file object_handling.cpp.

                                                                            {
  if (!consider_objects_ || !object_list_init_ || route_plan.path.points.empty() || base_path_points.empty()) {
    // Only forget the remembered speed cap if objects are disabled / never received; otherwise keep it
    // across cycles in which the route path happens to be empty.
    if (!consider_objects_ || !object_list_init_) {
      last_object_speed_cap_.reset();
      object_conflict_free_cycles_ = 0;
    }
    clearObjectInteractionMarkers(target_header);
    return;
  }
 
  const rclcpp::Time stamp(target_header.stamp);
  if (isMessageOutdated(object_list_.header, object_timeout_, stamp)) {
    std::string msg =
        "Object list is older than " + std::to_string(object_timeout_) + " seconds. Ignoring objects for this planning cycle.";
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
    resetObjectState(target_header);
    return;
  }
  if (object_list_.objects.empty()) {
    resetObjectState(target_header);
    return;
  }
 
  perception_msgs::msg::ObjectList tf_object_list = object_list_;
  if (requiresTransform(object_list_.header, target_header)) {
    try {
      tf_object_list = tf2_buffer_->transform(object_list_, target_header.frame_id, tf2_ros::fromMsg(target_header.stamp),
                                              fixed_over_time_frame_id_, tf2::durationFromSec(1.0));
    } catch (tf2::TransformException& ex) {
      std::string msg =
          "Object transformation is not available: " + std::string(ex.what()) + ". Ignoring objects for this planning cycle.";
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      resetObjectState(target_header);
      return;
    }
  }
 
  // Ignore objects whose center is behind the ego vehicle (vehicle frame, +x ahead).
  tf_object_list.objects.erase(
      std::remove_if(tf_object_list.objects.begin(), tf_object_list.objects.end(),
                     [](const auto& object) { return perception_msgs::object_access::getCenterPosition(object.state).x < 0.0; }),
      tf_object_list.objects.end());
 
  const std::vector<ObjectTrajectory> object_trajectories = buildObjectTrajectories(tf_object_list, stamp);
  if (object_trajectories.empty()) {
    resetObjectState(target_header);
    return;
  }
 
  double initial_speed_cap = 0.0;
  for (const auto& point : route_plan.path.points) initial_speed_cap = std::max(initial_speed_cap, point.v);
  if (v_ref_ >= 0.0) initial_speed_cap = std::max(initial_speed_cap, v_ref_);
 
  // Start the search at the remembered cap and allow a single release step upwards once enough
  // conflict-free cycles have passed (hysteresis to avoid oscillating between speeds).
  const rclcpp::Time iteration_begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
  const double remembered_speed_cap = std::clamp(last_object_speed_cap_.value_or(initial_speed_cap), 0.0, initial_speed_cap);
  double search_start_speed_cap = remembered_speed_cap;
  bool attempted_release = false;
  if (last_object_speed_cap_.has_value() && search_start_speed_cap < initial_speed_cap &&
      object_conflict_free_cycles_ >= object_velocity_release_hysteresis_cycles_) {
    search_start_speed_cap = std::min(search_start_speed_cap + object_velocity_release_step_, initial_speed_cap);
    attempted_release = search_start_speed_cap > remembered_speed_cap + 1e-6;
  }
 
  // Step the speed cap down until the resampled path is conflict-free (or reaches zero).
  double speed_cap = search_start_speed_cap;
  size_t iteration_count = 0;
  std::optional<ConflictSample> last_conflict;
  while (speed_cap > 0.0) {
    ++iteration_count;
    std::vector<SimplePathPoint> candidate_path =
        resamplePath(base_path_points, route_plan.stop_at_end, route_plan.offset_to_stop_line, &speed_cap);
    const std::optional<ConflictSample> conflict = firstConflict(candidate_path, object_trajectories);
    if (conflict.has_value()) {
      last_conflict = conflict;
      speed_cap = std::max(speed_cap - object_velocity_reduction_step_, 0.0);
      continue;
    }
 
    const rclcpp::Time iteration_end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
    publishObjectInteractionMarkers(target_header,
                                    speed_cap < initial_speed_cap ? last_conflict : std::optional<ConflictSample>{});
    RCLCPP_DEBUG(this->get_logger(), "Object velocity iteration took %f ms (%zu iterations)",
                 (iteration_end - iteration_begin).seconds() * 1e3, iteration_count);
 
    last_object_speed_cap_ = speed_cap < initial_speed_cap ? std::optional<double>(speed_cap) : std::nullopt;
 
    // Hold a reduced cap for a few stable cycles before allowing the next release step upwards.
    if (speed_cap >= initial_speed_cap - 1e-6 || speed_cap + 1e-6 < search_start_speed_cap || attempted_release) {
      object_conflict_free_cycles_ = 0;
    } else {
      object_conflict_free_cycles_ = std::min(object_conflict_free_cycles_ + 1, object_velocity_release_hysteresis_cycles_);
    }
 
    if (speed_cap <= object_standstill_speed_threshold_) {
      health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
      if (last_conflict.has_value()) {
        health_.key_value_pairs.insert_or_assign("ReasonToStop", "Object conflict");
        health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(last_conflict->object_id));
      }
      route_plan.path.points.clear();
      const std::string msg = "Object avoidance speed cap " + std::to_string(speed_cap) +
                              " m/s is at or below standstill threshold " + std::to_string(object_standstill_speed_threshold_) +
                              " m/s. Publishing standstill.";
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      return;
    }
 
    route_plan.path.points = candidate_path;
    if (speed_cap < initial_speed_cap) {
      health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
      if (last_conflict.has_value()) {
        health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(last_conflict->object_id));
      }
      RCLCPP_DEBUG(this->get_logger(), "Reduced reference speed cap to %f m/s to avoid object conflict", speed_cap);
    }
    return;
  }
 
  // Speed cap reached zero while still conflicting: publish standstill.
  route_plan.path.points = resamplePath(base_path_points, route_plan.stop_at_end, route_plan.offset_to_stop_line, &speed_cap);
  const rclcpp::Time iteration_end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
  RCLCPP_DEBUG(this->get_logger(), "Object velocity iteration took %f ms (%zu iterations)",
               (iteration_end - iteration_begin).seconds() * 1e3, iteration_count);
  last_object_speed_cap_ = speed_cap;
  const std::optional<ConflictSample> standstill_conflict = firstConflict(route_plan.path.points, object_trajectories);
  if (standstill_conflict.has_value()) {
    health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
    health_.key_value_pairs.insert_or_assign("ReasonToStop", "Object conflict");
    health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(standstill_conflict->object_id));
    last_conflict = standstill_conflict;
    object_conflict_free_cycles_ = 0;
    const std::string msg = "Reduced reference speed cap to 0.0 m/s; object " + std::to_string(standstill_conflict->object_id) +
                            " still conflicts at standstill. Publishing standstill.";
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
  } else {
    object_conflict_free_cycles_ = std::min(object_conflict_free_cycles_ + 1, object_velocity_release_hysteresis_cycles_);
    health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
    if (last_conflict.has_value()) {
      health_.key_value_pairs.insert_or_assign("ReasonToStop", "Object conflict");
      health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(last_conflict->object_id));
    }
    const std::string msg = "Reduced reference speed cap to 0.0 m/s to avoid object conflict. Publishing standstill.";
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
  }
  publishObjectInteractionMarkers(target_header, last_conflict);
  route_plan.path.points.clear();
}

buildObjectTrajectories()

std::vector< ObjectTrajectory > simple_planner::SimplePlannerNode::buildObjectTrajectories ( const perception_msgs::msg::ObjectList & tf_object_list, const rclcpp::Time & stamp ) const

private

Reduces the perceived object list (in vehicle frame) to timed bounding-box trajectories.

Static objects (without usable prediction) become a single-sample trajectory; otherwise the predictions above the probability threshold (or the most likely one) are sampled.

Parameters

[in]	tf_object_list	Object list transformed into vehicle frame.
[in]	stamp	Current planning timestamp used to compute relative sample times.

Returns

Timed bounding-box trajectories for all relevant objects.

Definition at line 96 of file object_handling.cpp.

                                                                                                        {
  std::vector<ObjectTrajectory> object_trajectories;
  for (const auto& object : tf_object_list.objects) {
    double object_width = 0.0;
    double object_length = 0.0;
    try {
      object_width = perception_msgs::object_access::getWidth(object);
      object_length = perception_msgs::object_access::getLength(object);
    } catch (const std::exception&) {
      object_width = 0.0;
      object_length = 0.0;
    }
    if (!std::isfinite(object_width) || object_width < kMinObjectWidth) object_width = kMinObjectWidth;
    if (!std::isfinite(object_length) || object_length < kMinObjectLength) object_length = kMinObjectLength;
 
    auto add_static_object = [&]() {
      ObjectTrajectory trajectory;
      trajectory.id = object.id;
      trajectory.is_static = true;
      trajectory.samples.push_back(buildObjectSample(object.state, tf_object_list.header, stamp, object_length, object_width));
      object_trajectories.push_back(trajectory);
    };
 
    // Select all sufficiently likely predictions, or fall back to the single most likely one.
    std::vector<const perception_msgs::msg::ObjectStatePrediction*> selected_predictions;
    for (const auto& prediction : object.state_predictions) {
      if (prediction.probability >= min_prediction_prob_) selected_predictions.push_back(&prediction);
    }
    if (selected_predictions.empty() && !object.state_predictions.empty()) {
      selected_predictions.push_back(
          &*std::max_element(object.state_predictions.begin(), object.state_predictions.end(),
                             [](const auto& lhs, const auto& rhs) { return lhs.probability < rhs.probability; }));
    }
 
    if (selected_predictions.empty()) {
      add_static_object();
      continue;
    }
 
    for (const auto* prediction : selected_predictions) {
      if (prediction == nullptr || prediction->states.empty()) {
        add_static_object();
        continue;
      }
      ObjectTrajectory trajectory;
      trajectory.id = object.id;
      trajectory.is_static = false;
      trajectory.samples.reserve(prediction->states.size());
      for (const auto& state : prediction->states) {
        trajectory.samples.push_back(buildObjectSample(state, tf_object_list.header, stamp, object_length, object_width));
      }
      object_trajectories.push_back(trajectory);
    }
  }
  return object_trajectories;
}

buildRoutePlan()

SimplePlannerNode::FollowRoutePlan simple_planner::SimplePlannerNode::buildRoutePlan ( const std_msgs::msg::Header & target_header )

private

Builds the complete route-following plan including stop and turn information.

Parameters

[in]

target_header

Vehicle-frame planning header for the generated route plan.

Returns

FollowRoutePlan Planned route path including stop and turn information.

Definition at line 608 of file simple_planner.cpp.

                                                                                                         {
  RCLCPP_DEBUG(this->get_logger(), "Default case: route is up to date, creating path from route.");
 
  FollowRoutePlan route_plan;
  route_plan.path.header = target_header;
  route_planning_msgs::msg::Route tf_route = route_;
  if (requiresTransform(route_.header, target_header)) {
    try {
      tf_route = tf2_buffer_->transform(route_, target_header.frame_id, tf2_ros::fromMsg(target_header.stamp),
                                        fixed_over_time_frame_id_, tf2::durationFromSec(1.0));
    } catch (tf2::TransformException& ex) {
      std::string msg = "Route transformation is not available: " + std::string(ex.what()) + ".";
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      return route_plan;
    }
  }
  route_plan.path.header = tf_route.header;
 
  std::map<uint64_t, uint64_t> lane_change_indices_map;
  appendRoutePoints(tf_route, route_plan, lane_change_indices_map);
 
  std::vector<SimplePathPoint> merged_points = mergeLaneChangeSegments(tf_route, route_plan.path.points, lane_change_indices_map);
  recalculateS(merged_points);
  applyGridMapConstraints(target_header, merged_points, route_plan);
  route_plan.path.points = resamplePath(merged_points, route_plan.stop_at_end, route_plan.offset_to_stop_line);
  applyObjectConstraints(target_header, merged_points, route_plan);
  if (trigger_turn_signals_) {
    applyIndicatorRequest(route_plan.suggested_turn_signal);
    health_.key_value_pairs.insert_or_assign("SuggestedTurnSignal", turnSignalToString(route_plan.suggested_turn_signal));
  }
  if (route_plan.stop_at_end) health_.key_value_pairs.insert({"ReasonToStop", route_plan.reason_to_stop});
  return route_plan;
}

buildSafeStopPath()

SimplePath simple_planner::SimplePlannerNode::buildSafeStopPath ( const std_msgs::msg::Header & target_header )

private

Builds the initial safe-stop path for the current cycle.

Parameters

[in]

target_header

Planning header for the generated safe-stop path.

Returns

SimplePath Safe-stop path in vehicle frame.

Definition at line 588 of file simple_planner.cpp.

                                                                                      {
  double current_velocity = perception_msgs::object_access::getVelocityMagnitude(ego_data_);
  safe_stop_distance_ = -0.5 * std::pow(current_velocity, 2) / a_max_decel_;
 
  SimplePath safe_stop_path = transformPath(latest_path_, target_header);
  trimPathBehindEgo(safe_stop_path);
  if (safe_stop_path.points.empty()) {
    RCLCPP_WARN(
        this->get_logger(),
        "No latest path available. Initialize safe stop along ego heading. Current velocity: %f m/s, safe stop distance: %f m",
        current_velocity, *safe_stop_distance_);
    latest_path_ = transformPath(calculateSafeStopAlongEgoHeading(ego_data_, *safe_stop_distance_, target_header), target_header);
  } else {
    RCLCPP_WARN(this->get_logger(), "Initialize safe stop along latest path. Current velocity: %f m/s, safe stop distance: %f m",
                current_velocity, *safe_stop_distance_);
    latest_path_ = calculateSafeStopAlongRoute(safe_stop_path, *safe_stop_distance_);
  }
  return latest_path_;
}

buildStandstillTrajectory()

trajectory_planning_msgs::msg::Trajectory simple_planner::SimplePlannerNode::buildStandstillTrajectory ( const std_msgs::msg::Header & target_header )

staticprivate

Creates a standstill trajectory for the current planning cycle.

Parameters

[in]

target_header

Output header for the generated trajectory.

Returns

trajectory_planning_msgs::msg::Trajectory Standstill trajectory message.

Definition at line 537 of file simple_planner.cpp.

                                            {
  int type_id = trajectory_planning_msgs::REFERENCE::TYPE_ID;
  trajectory_planning_msgs::msg::Trajectory tra;
  trajectory_planning_msgs::trajectory_access::initializeTrajectory(tra, type_id, 1);
  tra.header = target_header;
  trajectory_planning_msgs::trajectory_access::setStandstill(tra, true);
  return tra;
}

buildTrajectoryFromSimplePath()

trajectory_planning_msgs::msg::Trajectory simple_planner::SimplePlannerNode::buildTrajectoryFromSimplePath ( const SimplePath & path )

private

Builds a trajectory message from a simple path.

This also trims points behind the ego vehicle and falls back to standstill if no usable forward path remains.

Parameters

[in]

path

Path to be converted.

Returns

trajectory_planning_msgs::msg::Trajectory Generated trajectory message.

Definition at line 547 of file simple_planner.cpp.

                                                                                                             {
  SimplePath usable_path = path;
  trimPathBehindEgo(usable_path);
 
  if (usable_path.points.empty()) {
    safe_stop_distance_.reset();
    latest_path_.points.clear();
    std::string msg = "No usable forward path remains. Publishing standstill trajectory.";
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    health_.key_value_pairs.insert_or_assign("PlannerState", plannerStateToString(PlannerState::Standstill));
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    return buildStandstillTrajectory(path.header);
  }
 
  latest_path_ = usable_path;
  std::vector<SimplePathPoint> path_points = latest_path_.points;
  const auto n_states_size = static_cast<size_t>(n_states_);
  if (n_states_size < path_points.size()) {
    path_points.resize(n_states_size);
  }
 
  int type_id = trajectory_planning_msgs::REFERENCE::TYPE_ID;
  trajectory_planning_msgs::msg::Trajectory tra;
  trajectory_planning_msgs::trajectory_access::initializeTrajectory(tra, type_id, n_states_);
  tra.header = usable_path.header;
 
  for (int i = 0; i < n_states_; i++) {
    const size_t idx = static_cast<size_t>(i) < path_points.size() ? static_cast<size_t>(i) : path_points.size() - 1;
    trajectory_planning_msgs::trajectory_access::setT(tra, dt_ * i, i);
    trajectory_planning_msgs::trajectory_access::setX(tra, path_points[idx].position.x(), i);
    trajectory_planning_msgs::trajectory_access::setY(tra, path_points[idx].position.y(), i);
    trajectory_planning_msgs::trajectory_access::setV(tra, path_points[idx].v, i);
    RCLCPP_DEBUG(this->get_logger(), "Debug: i: %d,  t: %f,  x: %f,  y: %f,  v: %f, s: %f", i, dt_ * i,
                 path_points[idx].position.x(), path_points[idx].position.y(), path_points[idx].v, path_points[idx].s);
  }
 
  trajectory_planning_msgs::trajectory_access::setStandstill(tra, false);
  RCLCPP_DEBUG(this->get_logger(), "Standstill = %d", tra.standstill);
  return tra;
}

calculateSafeStopAlongEgoHeading()

SimplePath simple_planner::SimplePlannerNode::calculateSafeStopAlongEgoHeading ( const perception_msgs::msg::EgoData & ego_data, const double safe_stop_distance, const std_msgs::msg::Header & target_header )

private

Creates a minimal safe-stop path along the current ego heading.

Parameters

[in]	ego_data	Latest ego state.
[in]	safe_stop_distance	Distance available for the stop maneuver.
[in]	target_header	Target output header for the planning cycle.

Returns

Safe-stop path in the ego state frame.

Definition at line 964 of file simple_planner.cpp.

                                                                                                       {
  SimplePath safe_stop_path;
  safe_stop_path.header = ego_data.header;
 
  double v_ego = perception_msgs::object_access::getVelocityMagnitude(ego_data);
  geometry_msgs::msg::Point point = perception_msgs::object_access::getPosition(ego_data);
  double yaw = perception_msgs::object_access::getYaw(ego_data);
  Eigen::Vector2d start_position(point.x, point.y);
  Eigen::Vector2d heading(std::cos(yaw), std::sin(yaw));
 
  RCLCPP_WARN(this->get_logger(), "Initializing minimal safe stop along ego heading. Frame: %s, yaw: %f rad",
              safe_stop_path.header.frame_id.c_str(), yaw);
  (void)target_header;
 
  SimplePathPoint start_point(start_position, 0.0, v_ego);
  safe_stop_path.points.push_back(start_point);
  SimplePathPoint mid_point(start_position + heading * (safe_stop_distance / 2.0), safe_stop_distance / 2.0, v_ego);
  safe_stop_path.points.push_back(mid_point);
  SimplePathPoint end_point(start_position + heading * safe_stop_distance, safe_stop_distance, 0.0);
  safe_stop_path.points.push_back(end_point);
 
  safe_stop_path.points = resamplePath(safe_stop_path.points, true, 0.0);
  return safe_stop_path;
}

calculateSafeStopAlongRoute()

SimplePath simple_planner::SimplePlannerNode::calculateSafeStopAlongRoute ( const SimplePath & path, const double safe_stop_distance )

private

Truncates and resamples an existing path to stop within the safe-stop distance.

Parameters

[in]	path	Route path to use as stop corridor.
[in]	safe_stop_distance	Distance available for the stop maneuver.

Returns

Safe-stop path along the route.

Definition at line 949 of file simple_planner.cpp.

                                                                                                                 {
  SimplePath safe_stop_path = path;
  double start_s = safe_stop_path.points[0].s;  // start s value of path
  for (size_t i = 0; i < safe_stop_path.points.size(); i++) {
    if (safe_stop_path.points[i].s > start_s + safe_stop_distance) {
      // remove all points after the point where the safe stop distance is reached
      const auto erase_offset = static_cast<std::vector<SimplePathPoint>::difference_type>(i);
      safe_stop_path.points.erase(safe_stop_path.points.begin() + erase_offset, safe_stop_path.points.end());
      break;
    }
  }
  safe_stop_path.points = resamplePath(safe_stop_path.points, true, 0.0);
  return safe_stop_path;
}

clearObjectInteractionMarkers()

void simple_planner::SimplePlannerNode::clearObjectInteractionMarkers ( const std_msgs::msg::Header & target_header )

private

Deletes the currently published object interaction markers.

Parameters

[in]

target_header

Header used for the marker delete messages.

Definition at line 28 of file object_handling.cpp.

                                                                                            {
  if (!object_interaction_marker_pub_) {
    return;
  }
 
  visualization_msgs::msg::MarkerArray marker_array;
  const std::array<std::string, 3> namespaces = {"object_interaction_ego_safety_box", "object_interaction_ego_box",
                                                 "object_interaction_object_box"};
  int marker_id = 0;
  for (const auto& marker_namespace : namespaces) {
    visualization_msgs::msg::Marker marker;
    marker.header = target_header;
    marker.ns = marker_namespace;
    marker.id = marker_id++;
    marker.action = visualization_msgs::msg::Marker::DELETE;
    marker_array.markers.push_back(marker);
  }
  object_interaction_marker_pub_->publish(marker_array);
}

createTrajectory()

trajectory_planning_msgs::msg::Trajectory simple_planner::SimplePlannerNode::createTrajectory ( PlannerState state, const rclcpp::Time & stamp )

private

Creates a trajectory for the already determined planner state.

Main function of this node. Creates a reference trajectory based on the current route, vehicle state, and planning parameters.

Parameters

[in]	state	Planner state to be executed.
[in]	stamp	Current planning timestamp propagated from the timer callback.

Returns

trajectory_planning_msgs::msg::Trajectory The generated output trajectory.

This function generates a trajectory message by processing the current route, transforming it to the appropriate frame, handling special cases (such as route timeouts or empty routes), and considering traffic lights and lane changes. The resulting trajectory is resampled over time and trimmed to fit the configured number of states.

Returns

trajectory_planning_msgs::msg::Trajectory The generated trajectory message.

Definition at line 490 of file simple_planner.cpp.

                                                                                                                     {
  rclcpp::Time begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
 
  trajectory_planning_msgs::msg::Trajectory tra;
  tra.header.stamp = stamp;
  tra.header.frame_id = vehicle_frame_id_;
 
  if (state != PlannerState::FollowRoute) {
    resetObjectState(tra.header);
  }
 
  switch (state) {
    case PlannerState::Standstill:
      tra = buildStandstillTrajectory(tra.header);
      break;
    case PlannerState::SafeStop:
      if (!safe_stop_distance_.has_value()) {
        tra = buildTrajectoryFromSimplePath(buildSafeStopPath(tra.header));
      } else {
        RCLCPP_DEBUG(this->get_logger(), "Executing safe stop.");
        tra = buildTrajectoryFromSimplePath(transformPath(latest_path_, tra.header));
      }
      break;
    case PlannerState::FollowRoute: {
      safe_stop_distance_.reset();
      FollowRoutePlan route_plan = buildRoutePlan(tra.header);
      tra = buildTrajectoryFromSimplePath(route_plan.path);
      break;
    }
    case PlannerState::NoPublish:
      throw std::runtime_error("createTrajectory called for non-publish state");
  }
 
  if (tra.header.frame_id != trajectory_frame_id_) {
    try {
      tra = tf2_buffer_->transform(tra, trajectory_frame_id_, tf2::durationFromSec(1.0));
    } catch (tf2::TransformException& ex) {
      throw std::runtime_error("Transformation into output frame '" + trajectory_frame_id_ +
                               "' is not available: " + std::string(ex.what()));
    }
  }
 
  rclcpp::Time end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
  RCLCPP_DEBUG(this->get_logger(), "Trajectory creation took %f ms", (end - begin).seconds() * 1e3);
  return tra;
}

declareAndLoadParameter()

template<typename T >

void simple_planner::SimplePlannerNode::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 a ROS parameter, loads its value and optionally registers it for runtime updates.

Template Parameters

T	Parameter value type.

Parameters

[in]	name	Parameter name.
[out]	param	Member variable to store the parameter value.
[in]	description	Human-readable parameter description.
[in]	add_to_auto_reconfigurable_params	Whether parameter updates automatically update the member variable.
[in]	is_required	Whether the node should fail if the parameter is not set.
[in]	read_only	Whether the parameter is exposed as read-only.
[in]	from_value	Optional lower bound for numeric parameters.
[in]	to_value	Optional upper bound for numeric parameters.
[in]	step_value	Optional step size for numeric parameters.
[in]	additional_constraints	Additional free-form constraint text for the parameter descriptor.

Definition at line 7 of file utils.hpp.

                                                                                         {
  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;
      T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1);
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
      param_desc.integer_range = {range};
    } else if constexpr (std::is_floating_point_v<T>) {
      rcl_interfaces::msg::FloatingPointRange range;
      T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1.0);
      range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
      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));
  }
}

determinePlannerState()

SimplePlannerNode::PlannerState simple_planner::SimplePlannerNode::determinePlannerState ( const rclcpp::Time & stamp )

private

Determines the current planner state from input freshness and route availability.

Parameters

[in]

stamp

Current planning timestamp.

Returns

PlannerState The state to be handled for this cycle.

Definition at line 370 of file simple_planner.cpp.

                                                                                              {
  // ego data missing -> no publish
  if (!ego_data_init_) {
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::STALE, "No ego data received yet",
              {{"PlannerState", plannerStateToString(PlannerState::NoPublish)}});
    return PlannerState::NoPublish;
  }
 
  // ego data outdated -> no publish
  if (isMessageOutdated(ego_data_.header, ego_data_timeout_, stamp)) {
    ego_data_init_ = false;
    std::string msg =
        "EgoData is older than " + std::to_string(ego_data_timeout_) + " seconds. Skip publishing until fresh ego data arrives.";
    RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
              {{"PlannerState", plannerStateToString(PlannerState::NoPublish)}});
    return PlannerState::NoPublish;
  }
 
  // no route received and no ongoing safe stop -> no publish
  if (!route_init_ && !safe_stop_distance_.has_value()) {
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::STALE, "No route received and no ongoing safe stop",
              {{"PlannerState", plannerStateToString(PlannerState::NoPublish)}});
    return PlannerState::NoPublish;
  }
 
  // route outdated and vehicle in standstill -> standstill
  if (route_init_ && isMessageOutdated(route_.header, route_timeout_, stamp)) {
    if (perception_msgs::object_access::getStandstill(ego_data_)) {
      route_init_ = false;
      safe_stop_distance_.reset();
      latest_path_.points.clear();
      std::string msg = "Route is older than " + std::to_string(route_timeout_) +
                        " seconds and ego vehicle is stationary. Publishing standstill trajectory.";
      RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, msg,
                {{"PlannerState", plannerStateToString(PlannerState::Standstill)}});
      return PlannerState::Standstill;
    }
    route_init_ = false;
 
    // route outdated and vehicle still moving -> safe stop
    std::string msg = "Route is older than " + std::to_string(route_timeout_) +
                      " seconds but ego vehicle is still moving. Executing safe stop trajectory.";
    RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
              {{"PlannerState", plannerStateToString(PlannerState::SafeStop)}});
    return PlannerState::SafeStop;
  }
 
  // route received, but empty -> standstill
  if (route_init_ && route_.route_elements.empty()) {
    route_init_ = false;
    safe_stop_distance_.reset();
    latest_path_.points.clear();
    std::string msg = "Received route has no route elements. Publishing standstill trajectory.";
    RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, msg,
              {{"PlannerState", plannerStateToString(PlannerState::Standstill)}});
    return PlannerState::Standstill;
  }
 
  // no fresh route, but safe stop already started -> safe stop
  if (!route_init_ && safe_stop_distance_.has_value()) {
    if (perception_msgs::object_access::getStandstill(ego_data_)) {
      safe_stop_distance_.reset();
      latest_path_.points.clear();
      std::string msg = "Safe stop finished. Ego vehicle is considered stationary. Publishing standstill trajectory.";
      RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, msg,
                {{"PlannerState", plannerStateToString(PlannerState::Standstill)}});
      return PlannerState::Standstill;
    }
    std::string msg = "No fresh route available, but safe stop already started. Executing safe stop trajectory.";
    RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
              {{"PlannerState", plannerStateToString(PlannerState::SafeStop)}});
    return PlannerState::SafeStop;
  }
 
  // grid map enabled but missing, outdated, or invalid -> safe stop or standstill
  if (consider_grid_map_ && !hasValidGridMap(stamp)) {
    if (perception_msgs::object_access::getStandstill(ego_data_)) {
      std::string msg =
          "Grid map is missing, outdated, or invalid and ego vehicle is stationary. Publishing standstill trajectory.";
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
                {{"PlannerState", plannerStateToString(PlannerState::Standstill)}});
      return PlannerState::Standstill;
    }
 
    std::string msg = "Grid map is missing, outdated, or invalid. Executing safe stop trajectory.";
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
              {{"PlannerState", plannerStateToString(PlannerState::SafeStop)}});
    return PlannerState::SafeStop;
  }
 
  // fresh ego data and valid route available -> follow route
  setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, "Input information up to date. Following route.",
            {{"PlannerState", plannerStateToString(PlannerState::FollowRoute)}});
  return PlannerState::FollowRoute;
}

egoDataCallback()

void simple_planner::SimplePlannerNode::egoDataCallback ( const perception_msgs::msg::EgoData::UniquePtr msg )

private

Stores the latest ego data message.

This callback is invoked when the subscriber receives a new egoData message.

Parameters

	msg	Latest ego data message.
[in]	msg	egoData

Definition at line 316 of file simple_planner.cpp.

                                                                                    {
  if (ego_data_topic_diagnostic_ != nullptr) {
    ego_data_topic_diagnostic_->tick(msg->header.stamp);
  }
  ego_data_ = *msg;
 
  if (!ego_data_init_) {
    ego_data_init_ = true;
    RCLCPP_INFO(this->get_logger(), "Received first ego data message, initialized global variable");
  }
}

findFirstGridMapStopS()

std::optional< double > simple_planner::SimplePlannerNode::findFirstGridMapStopS ( const std_msgs::msg::Header & target_header, const std::vector< SimplePathPoint > & base_path_points )

private

Finds the last safe grid-map sample before the first blocked pose.

Parameters

[in]	target_header	Current planning header propagated from the timer callback.
[in]	base_path_points	Route path before time-based resampling.

Returns

Safe path coordinate at which to stop if a blocked pose is found.

Definition at line 69 of file grid_map_handling.cpp.

                                                                                                                   {
  if (base_path_points.size() < 2) {
    return std::nullopt;
  }
 
  const geometry_msgs::msg::TransformStamped tf =
      tf2_buffer_->lookupTransform(grid_map_.header.frame_id, grid_map_.header.stamp, target_header.frame_id, target_header.stamp,
                                   fixed_over_time_frame_id_, rclcpp::Duration::from_seconds(1.0));
 
  std::vector<Eigen::Vector2d> grid_frame_path_points;
  grid_frame_path_points.reserve(base_path_points.size());
  for (const auto& point : base_path_points) {
    geometry_msgs::msg::PointStamped point_msg;
    geometry_msgs::msg::PointStamped transformed_point_msg;
    point_msg.header = target_header;
    point_msg.point.x = point.position.x();
    point_msg.point.y = point.position.y();
    point_msg.point.z = 0.0;
    tf2::doTransform(point_msg, transformed_point_msg, tf);
    grid_frame_path_points.emplace_back(transformed_point_msg.point.x, transformed_point_msg.point.y);
  }
 
  const double resolution = static_cast<double>(grid_map_.info.resolution);
  const Eigen::Vector2d grid_origin(grid_map_.info.origin.position.x, grid_map_.info.origin.position.y);
  const double grid_origin_yaw = tf2::getYaw(grid_map_.info.origin.orientation);
  const double grid_length_x = static_cast<double>(grid_map_.info.width) * resolution;
  const double grid_length_y = static_cast<double>(grid_map_.info.height) * resolution;
  const Eigen::Vector2d ego_center_offset(ego_data_.state.reference_point.translation_to_geometric_center.x,
                                          ego_data_.state.reference_point.translation_to_geometric_center.y);
 
  // The target trajectory frame is ego-centered (normally base_link), so the ego reference point is its origin.
  const Eigen::Vector2d ego_reference_position = Eigen::Vector2d::Zero();
  double ego_path_s = 0.0;
  double best_dist_sq = std::numeric_limits<double>::max();
  for (size_t i = 0; i + 1 < base_path_points.size(); ++i) {
    const Eigen::Vector2d segment = base_path_points[i + 1].position - base_path_points[i].position;
    const double segment_length_sq = segment.squaredNorm();
    if (segment_length_sq < 1e-9) {
      continue;
    }
 
    const double alpha =
        std::clamp((ego_reference_position - base_path_points[i].position).dot(segment) / segment_length_sq, 0.0, 1.0);
    const Eigen::Vector2d projected = base_path_points[i].position + alpha * segment;
    const double dist_sq = (projected - ego_reference_position).squaredNorm();
    if (dist_sq >= best_dist_sq) {
      continue;
    }
 
    best_dist_sq = dist_sq;
    ego_path_s = base_path_points[i].s + alpha * (base_path_points[i + 1].s - base_path_points[i].s);
  }
 
  std::optional<double> last_safe_s;
  for (size_t i = 0; i + 1 < base_path_points.size(); ++i) {
    const Eigen::Vector2d segment = grid_frame_path_points[i + 1] - grid_frame_path_points[i];
    const double segment_length = segment.norm();
    if (segment_length <= 1e-3) {
      continue;
    }
 
    const Eigen::Vector2d tangent = segment / segment_length;
    const auto sample_count = static_cast<size_t>(std::ceil(segment_length / resolution));
    for (size_t sample_idx = 0; sample_idx <= sample_count; ++sample_idx) {
      const double sampled_distance = std::min(static_cast<double>(sample_idx) * resolution, segment_length);
      const double alpha = sampled_distance / segment_length;
      const double sample_s = base_path_points[i].s + alpha * (base_path_points[i + 1].s - base_path_points[i].s);
      if (sample_s < ego_path_s) {
        continue;
      }
 
      const Eigen::Vector2d reference_point = grid_frame_path_points[i] + alpha * segment;
      const double yaw = wrap_angle_rad(std::atan2(tangent.y(), tangent.x()));
      const Eigen::Vector2d ego_center = reference_point + rotate(ego_center_offset, yaw);
      const OrientedBox2D ego_box = buildOrientedBox(ego_center, yaw, ego_data_.length, ego_data_.width);
      const OrientedBox2D ego_safety_box =
          expandBoxWithSafetyMargins(ego_box, grid_longitudinal_safety_distance_, grid_lateral_safety_distance_);
 
      const auto expanded_corners = getBoxCorners(ego_safety_box);
      double min_local_x = std::numeric_limits<double>::max();
      double max_local_x = std::numeric_limits<double>::lowest();
      double min_local_y = std::numeric_limits<double>::max();
      double max_local_y = std::numeric_limits<double>::lowest();
      for (const auto& corner : expanded_corners) {
        const Eigen::Vector2d local_corner = rotate(corner - grid_origin, -grid_origin_yaw);
        min_local_x = std::min(min_local_x, local_corner.x());
        max_local_x = std::max(max_local_x, local_corner.x());
        min_local_y = std::min(min_local_y, local_corner.y());
        max_local_y = std::max(max_local_y, local_corner.y());
      }
 
      const bool box_out_of_grid =
          min_local_x < 0.0 || min_local_y < 0.0 || max_local_x >= grid_length_x || max_local_y >= grid_length_y;
      if (box_out_of_grid && consider_out_of_grid_) {
        return last_safe_s.value_or(ego_path_s);
      }
 
      const bool box_fully_outside =
          max_local_x < 0.0 || max_local_y < 0.0 || min_local_x >= grid_length_x || min_local_y >= grid_length_y;
      if (!box_fully_outside) {
        const size_t min_cell_x = static_cast<size_t>(std::floor(std::max(min_local_x, 0.0) / resolution));
        const size_t max_cell_x =
            std::min(static_cast<size_t>(std::floor(max_local_x / resolution)), static_cast<size_t>(grid_map_.info.width) - 1);
        const size_t min_cell_y = static_cast<size_t>(std::floor(std::max(min_local_y, 0.0) / resolution));
        const size_t max_cell_y =
            std::min(static_cast<size_t>(std::floor(max_local_y / resolution)), static_cast<size_t>(grid_map_.info.height) - 1);
 
        for (size_t cell_y = min_cell_y; cell_y <= max_cell_y; ++cell_y) {
          for (size_t cell_x = min_cell_x; cell_x <= max_cell_x; ++cell_x) {
            const int8_t value = grid_map_.data[cell_y * grid_map_.info.width + cell_x];
            if (value >= 0 && value < grid_occupied_threshold_) {
              continue;
            }
 
            const Eigen::Vector2d cell_center_local((static_cast<double>(cell_x) + 0.5) * resolution,
                                                    (static_cast<double>(cell_y) + 0.5) * resolution);
            const Eigen::Vector2d cell_center = grid_origin + rotate(cell_center_local, grid_origin_yaw);
            const OrientedBox2D cell_box = buildOrientedBox(cell_center, grid_origin_yaw, resolution, resolution);
            if (overlaps(ego_safety_box, cell_box)) {
              return last_safe_s.value_or(ego_path_s);
            }
          }
        }
      }
 
      last_safe_s = sample_s;
    }
  }
 
  return std::nullopt;
}

firstConflict()

std::optional< ConflictSample > simple_planner::SimplePlannerNode::firstConflict ( const std::vector< SimplePathPoint > & ego_path, const std::vector< ObjectTrajectory > & object_trajectories ) const

private

Returns the first conflict between the (time-sampled) ego path and any object trajectory.

Parameters

[in]	ego_path	Time-equidistant ego path candidate.
[in]	object_trajectories	Object trajectories to check against.

Returns

The first detected conflict, or std::nullopt if the ego path is conflict-free.

Definition at line 154 of file object_handling.cpp.

                                                                                                                             {
  if (ego_path.empty()) return std::nullopt;
 
  const auto& ego_offset_msg = ego_data_.state.reference_point.translation_to_geometric_center;
  const Eigen::Vector2d ego_center_offset(ego_offset_msg.x, ego_offset_msg.y);
 
  // Interpolates the ego bounding box along the (time-equidistant) path at relative time ego_t.
  auto ego_box_at = [&](double ego_t) {
    const size_t idx = std::min(static_cast<size_t>(std::floor(std::max(ego_t, 0.0) / dt_)), ego_path.size() - 1);
    const size_t next_idx = std::min(idx + 1, ego_path.size() - 1);
    const double segment_start_t = dt_ * static_cast<double>(idx);
    const double alpha = next_idx > idx ? std::clamp((ego_t - segment_start_t) / dt_, 0.0, 1.0) : 0.0;
    const Eigen::Vector2d position = ego_path[idx].position + alpha * (ego_path[next_idx].position - ego_path[idx].position);
    Eigen::Vector2d heading = ego_path[next_idx].position - ego_path[idx].position;
    if (heading.squaredNorm() < 1e-9 && idx > 0) heading = ego_path[idx].position - ego_path[idx - 1].position;
    const double yaw = heading.squaredNorm() > 1e-9 ? wrap_angle_rad(std::atan2(heading.y(), heading.x())) : 0.0;
    return buildOrientedBox(position + rotate(ego_center_offset, yaw), yaw, ego_data_.length, ego_data_.width);
  };
 
  const double check_dt = std::clamp(kObjectCollisionCheckDt, 0.01, dt_);
  const size_t last_segment_idx = ego_path.size() > 1 ? ego_path.size() - 2 : 0;
  for (size_t i = 0; i <= last_segment_idx; ++i) {
    const double segment_start_t = dt_ * static_cast<double>(i);
    if (segment_start_t > trajectory_horizon_) break;
    const double segment_end_t = std::min(dt_ * static_cast<double>(i + 1), trajectory_horizon_);
    const int steps = std::max(1, static_cast<int>(std::ceil((segment_end_t - segment_start_t) / check_dt)));
 
    for (int step = 0; step <= steps; ++step) {
      const double ego_t =
          segment_start_t + (segment_end_t - segment_start_t) * static_cast<double>(step) / static_cast<double>(steps);
      const OrientedBox2D ego_box = ego_box_at(ego_t);
      const OrientedBox2D ego_safety_box =
          expandBoxWithSafetyMargins(ego_box, object_longitudinal_safety_distance_, object_lateral_safety_distance_);
 
      for (const auto& object_trajectory : object_trajectories) {
        if (object_trajectory.samples.empty()) continue;
 
        if (object_trajectory.is_static) {
          const auto& object_sample = object_trajectory.samples.front();
          if (overlaps(ego_safety_box, object_sample.box)) {
            return ConflictSample{object_trajectory.id, ego_box, object_sample.box};
          }
          continue;
        }
 
        for (size_t sample_idx = 0; sample_idx < object_trajectory.samples.size(); ++sample_idx) {
          const auto& object_sample = object_trajectory.samples[sample_idx];
          const TimedBox2D* next_sample =
              sample_idx + 1 < object_trajectory.samples.size() ? &object_trajectory.samples[sample_idx + 1] : nullptr;
 
          TimedBox2D timed_object_sample = object_sample;
          if (next_sample != nullptr && ego_t >= object_sample.t - object_interaction_time_window_ &&
              ego_t <= next_sample->t + object_interaction_time_window_) {
            if (ego_t >= object_sample.t && ego_t <= next_sample->t) {
              timed_object_sample = interpolateTimedBox(object_sample, *next_sample, ego_t);
            } else if (std::abs(ego_t - next_sample->t) < std::abs(ego_t - object_sample.t)) {
              timed_object_sample = *next_sample;
            }
          } else if (std::abs(ego_t - object_sample.t) > object_interaction_time_window_) {
            continue;
          }
 
          if (overlaps(ego_safety_box, timed_object_sample.box)) {
            return ConflictSample{object_trajectory.id, ego_box, timed_object_sample.box};
          }
        }
      }
    }
  }
  return std::nullopt;
}

generateLaneChangePath()

std::vector< SimplePathPoint > simple_planner::SimplePlannerNode::generateLaneChangePath ( size_t start_idx, size_t turn_idx, const route_planning_msgs::msg::Route & route )

private

Generates interpolated points for a lane-change section of the route.

Parameters

[in]	start_idx	Route element index where the lane change starts.
[in]	turn_idx	Route element index that indicates the lane-change turn.
[in]	route	Route in vehicle frame.

Returns

Lane-change path points, or an empty vector if the indices are invalid.

Definition at line 991 of file simple_planner.cpp.

                                                                                                                 {
  std::vector<SimplePathPoint> lane_change_path;
  const size_t end_idx = turn_idx + 1;  // lane change should end at the next route element
  if (start_idx >= end_idx || end_idx >= route.route_elements.size()) {
    std::string msg = "Invalid lane change indices: start_idx=" + std::to_string(start_idx) +
                      ", end_idx=" + std::to_string(end_idx) + ". Cannot generate lane change path.";
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    return lane_change_path;
  }
 
  int lane_change_direction = route_planning_msgs::route_access::getLaneChangeDirection(route.route_elements[turn_idx],
                                                                                        route.route_elements[turn_idx + 1]);
 
  // Interpolate between the two elements
  for (size_t i = start_idx; i <= end_idx; ++i) {
    if (i < route.current_route_element_idx) continue;
 
    const auto& route_element = route.route_elements[i];
    const auto& suggested_lane = route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
    if (i > turn_idx) lane_change_direction = 0;  // -> i == turn_idx + 1 == end_idx
    const auto& adjacent_lane = route_planning_msgs::route_access::getAdjacentLane(
        route_element, route.route_elements[i].suggested_lane_idx, lane_change_direction);
    Eigen::Vector2d suggested_lane_pos(suggested_lane.reference_pose.position.x, suggested_lane.reference_pose.position.y);
    Eigen::Vector2d adjacent_lane_pos(adjacent_lane.reference_pose.position.x, adjacent_lane.reference_pose.position.y);
    const double lane_change_progress = static_cast<double>(i - start_idx) / static_cast<double>(end_idx - start_idx);
    double alpha = 0.5 * (1.0 + std::cos(M_PI * lane_change_progress));
    Eigen::Vector2d interpolated_pos = alpha * suggested_lane_pos + (1.0 - alpha) * adjacent_lane_pos;
    lane_change_path.push_back(
        SimplePathPoint(interpolated_pos, route_element.s, suggested_lane.speed_limit / 3.6));  // convert km/h to m/s
  }
 
  return lane_change_path;
}

gridMapCallback()

void simple_planner::SimplePlannerNode::gridMapCallback ( const nav_msgs::msg::OccupancyGrid::UniquePtr msg )

private

Stores the latest occupancy grid map message.

Parameters

msg	Latest occupancy grid map message.

Definition at line 358 of file simple_planner.cpp.

                                                                                   {
  if (grid_map_topic_diagnostic_ != nullptr) {
    grid_map_topic_diagnostic_->tick(msg->header.stamp);
  }
  grid_map_ = *msg;
 
  if (!grid_map_init_) {
    grid_map_init_ = true;
    RCLCPP_INFO(this->get_logger(), "Received first grid map message, initialized global variable");
  }
}

hasValidGridMap()

bool simple_planner::SimplePlannerNode::hasValidGridMap ( const rclcpp::Time & stamp ) const

private

Checks whether a fresh, structurally valid grid map is currently available.

Parameters

[in]

stamp

Current planning timestamp.

Returns

true if grid-map planning is possible.

Definition at line 20 of file grid_map_handling.cpp.

                                                                     {
  if (!grid_map_init_) {
    return false;
  }
 
  const auto& origin = grid_map_.info.origin;
  const double orientation_norm_sq = origin.orientation.x * origin.orientation.x + origin.orientation.y * origin.orientation.y +
                                     origin.orientation.z * origin.orientation.z + origin.orientation.w * origin.orientation.w;
  const bool has_valid_origin = std::isfinite(origin.position.x) && std::isfinite(origin.position.y) &&
                                std::isfinite(orientation_norm_sq) && orientation_norm_sq > 1e-12;
  const bool has_valid_geometry = !grid_map_.header.frame_id.empty() && has_valid_origin &&
                                  std::isfinite(grid_map_.info.resolution) && grid_map_.info.resolution > 0.0 &&
                                  grid_map_.info.width > 0 && grid_map_.info.height > 0;
  const size_t expected_cell_count = static_cast<size_t>(grid_map_.info.width) * static_cast<size_t>(grid_map_.info.height);
  const bool has_complete_data = grid_map_.data.size() == expected_cell_count;
 
  return has_valid_geometry && has_complete_data && !isMessageOutdated(grid_map_.header, grid_map_timeout_, stamp);
}

health()

void simple_planner::SimplePlannerNode::health ( diagnostic_updater::DiagnosticStatusWrapper & stat )

private

Function called by diagnostic updater to populate diagnostics status.

Definition at line 173 of file utils.hpp.

                                                                              {
  stat.summary(health_.status, health_.message);
  for (const auto& [key, value] : health_.key_value_pairs) {
    stat.add(key, value);
  }
}

isMessageOutdated()

bool simple_planner::SimplePlannerNode::isMessageOutdated ( const std_msgs::msg::Header & header, double timeout, const rclcpp::Time & stamp )

staticprivate

Checks whether an input message is older than the configured timeout.

Parameters

[in]	header	Header of the input message.
[in]	timeout	Timeout in seconds. A value of -1.0 disables the timeout.
[in]	stamp	Current planning timestamp.

Returns

true if the message is outdated.

false if the message is still valid.

Definition at line 474 of file simple_planner.cpp.

                                                                                                                  {
  if (timeout == -1.0) {
    return false;
  }
  return (stamp - rclcpp::Time(header.stamp)) > rclcpp::Duration::from_seconds(timeout);
}

mergeLaneChangeSegments()

std::vector< SimplePathPoint > simple_planner::SimplePlannerNode::mergeLaneChangeSegments ( const route_planning_msgs::msg::Route & tf_route, const std::vector< SimplePathPoint > & route_points, const std::map< uint64_t, uint64_t > & lane_change_indices_map )

private

Merges interpolated lane-change segments into the base route path.

The base route points are the already filtered route-following path, while tf_route is still needed to reconstruct the lane geometry for the interpolated lane-change segments.

Parameters

[in]	tf_route	Route transformed into vehicle frame and used to derive lane-change geometry.
[in]	route_points	Base route points before lane-change insertion.
[in]	lane_change_indices_map	Lane-change windows gathered during route processing.

Returns

std::vector<SimplePathPoint> Path points with lane changes inserted.

Definition at line 875 of file simple_planner.cpp.

                                                               {
  size_t current = 0;
  std::vector<SimplePathPoint> merged_points;
  for (const auto& lane_change_indices : lane_change_indices_map) {
    uint64_t lane_change_idx_route = lane_change_indices.first;
    uint64_t start_idx_route = lane_change_indices.second;
    size_t start_idx =
        start_idx_route > tf_route.current_route_element_idx ? start_idx_route - tf_route.current_route_element_idx : 0;
    size_t end_idx = lane_change_idx_route + 1 > tf_route.current_route_element_idx
                         ? lane_change_idx_route + 1 - tf_route.current_route_element_idx
                         : 0;
 
    if (start_idx < current || current > route_points.size()) {
      RCLCPP_WARN(this->get_logger(), "Skipping overlapping lane change window (%zu, %zu), current path index: %zu.", start_idx,
                  end_idx, current);
      continue;
    }
 
    start_idx = std::min(start_idx, route_points.size());
    const auto current_offset = static_cast<std::vector<SimplePathPoint>::difference_type>(current);
    const auto start_offset = static_cast<std::vector<SimplePathPoint>::difference_type>(start_idx);
    merged_points.insert(merged_points.end(), route_points.begin() + current_offset, route_points.begin() + start_offset);
    std::vector<SimplePathPoint> lane_change_points = generateLaneChangePath(start_idx_route, lane_change_idx_route, tf_route);
    merged_points.insert(merged_points.end(), lane_change_points.begin(), lane_change_points.end());
    current = std::min(end_idx + 1, route_points.size());
  }
  if (current <= route_points.size()) {
    const auto current_offset = static_cast<std::vector<SimplePathPoint>::difference_type>(current);
    merged_points.insert(merged_points.end(), route_points.begin() + current_offset, route_points.end());
  }
  return merged_points;
}

objectListCallback()

void simple_planner::SimplePlannerNode::objectListCallback ( const perception_msgs::msg::ObjectList::UniquePtr msg )

private

Stores the latest perceived object list including object predictions.

Parameters

msg	Latest object list message.

Definition at line 328 of file simple_planner.cpp.

                                                                                          {
  if (object_list_topic_diagnostic_ != nullptr) {
    object_list_topic_diagnostic_->tick(msg->header.stamp);
  }
  object_list_ = *msg;
 
  if (!object_list_init_) {
    object_list_init_ = true;
    RCLCPP_INFO(this->get_logger(), "Received first object list message, initialized global variable");
  }
}

parametersCallback()

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

private

Handles reconfiguration when a parameter value is changed.

Parameters

[in]

parameters

Requested parameter updates.

Returns

parameter change result

Definition at line 79 of file utils.hpp.

                                                                                                                       {
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = false;
 
  for (const auto& param : parameters) {
    // check for specific parameter constraints
    if (param.get_name() == "a_decel") {
      if (param.as_double() >= 0.0) {
        result.successful = false;
        result.reason = "a_decel (" + std::to_string(param.as_double()) + ") must be < 0.0";
        RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_decel': %s", result.reason.c_str());
        break;
      } else if (a_max_decel_ > param.as_double()) {
        result.successful = false;
        result.reason =
            "a_max_decel (" + std::to_string(a_max_decel_) + ") must be <= a_decel (" + std::to_string(param.as_double()) + ")";
        RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_decel': %s", result.reason.c_str());
        break;
      }
    } else if (param.get_name() == "a_max_decel") {
      if (param.as_double() >= 0.0) {
        result.successful = false;
        result.reason = "a_max_decel (" + std::to_string(param.as_double()) + ") must be < 0.0";
        RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_max_decel': %s", result.reason.c_str());
        break;
      } else if (param.as_double() > a_decel_) {
        result.successful = false;
        result.reason =
            "a_max_decel (" + std::to_string(param.as_double()) + ") must be <= a_decel (" + std::to_string(a_decel_) + ")";
        RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_max_decel': %s", result.reason.c_str());
        break;
      }
    }
 
    // apply parameter change
    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());
        result.successful = true;
        break;
      }
    }
  }
 
  return result;
}

plannerStateToString()

std::string simple_planner::SimplePlannerNode::plannerStateToString ( const PlannerState & state )

staticprivate

Converts a PlannerState enum to a string representation.

Parameters

state

Returns

std::string

Definition at line 188 of file utils.hpp.

                                                                                            {
  switch (state) {
    case PlannerState::NoPublish:
      return "NoPublish";
    case PlannerState::Standstill:
      return "Standstill";
    case PlannerState::SafeStop:
      return "SafeStop";
    case PlannerState::FollowRoute:
      return "FollowRoute";
    default:
      return "Unknown";
  }
}

publishObjectInteractionMarkers()

void simple_planner::SimplePlannerNode::publishObjectInteractionMarkers ( const std_msgs::msg::Header & target_header, const std::optional< ConflictSample > & conflict )

private

Publishes RViz markers for the current object interaction conflict.

If marker publishing is disabled or no conflict exists, existing markers are cleared.

Parameters

[in]	target_header	Header used for the marker messages.
[in]	conflict	Optional conflict sample to visualize.

Definition at line 48 of file object_handling.cpp.

                                                                                                     {
  if (!object_interaction_marker_pub_) {
    return;
  }
 
  if (!publish_object_interaction_markers_ || !conflict.has_value()) {
    clearObjectInteractionMarkers(target_header);
    return;
  }
 
  visualization_msgs::msg::MarkerArray marker_array;
  auto make_box_marker = [&](const std::string& ns, int marker_id, const geometry_msgs::msg::Pose& pose, double length,
                             double width, double z, float r, float g, float b, float a) {
    visualization_msgs::msg::Marker marker;
    marker.header = target_header;
    marker.ns = ns;
    marker.id = marker_id;
    marker.type = visualization_msgs::msg::Marker::CUBE;
    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.pose = pose;
    marker.pose.position.z = z;
    marker.scale.x = length;
    marker.scale.y = width;
    marker.scale.z = 0.08;
    marker.lifetime.sec = 0;
    marker.lifetime.nanosec = 500000000;
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b;
    marker.color.a = a;
    marker_array.markers.push_back(marker);
  };
 
  const geometry_msgs::msg::Pose ego_pose = toPose(conflict->ego_box);
  const OrientedBox2D ego_safety_box =
      expandBoxWithSafetyMargins(conflict->ego_box, object_longitudinal_safety_distance_, object_lateral_safety_distance_);
  const double ego_length = 2.0 * conflict->ego_box.half_length;
  const double ego_width = 2.0 * conflict->ego_box.half_width;
  make_box_marker("object_interaction_ego_safety_box", 0, toPose(ego_safety_box), 2.0 * ego_safety_box.half_length,
                  2.0 * ego_safety_box.half_width, 0.12, 1.0F, 0.55F, 0.0F, 0.28F);
  make_box_marker("object_interaction_ego_box", 1, ego_pose, ego_length, ego_width, 0.18, 1.0F, 0.0F, 0.0F, 0.55F);
  make_box_marker("object_interaction_object_box", 2, toPose(conflict->object_box), 2.0 * conflict->object_box.half_length,
                  2.0 * conflict->object_box.half_width, 0.24, 0.0F, 0.45F, 1.0F, 0.55F);
 
  object_interaction_marker_pub_->publish(marker_array);
}

publishTimerCallback()

void simple_planner::SimplePlannerNode::publishTimerCallback ( )

private

This callback is invoked every period seconds by the timer.

Definition at line 1193 of file simple_planner.cpp.

                                             {
  const rclcpp::Time stamp = now();
  PlannerState planner_state = determinePlannerState(stamp);
  if (planner_state == PlannerState::NoPublish) {
    std_msgs::msg::Header marker_header;
    marker_header.stamp = stamp;
    marker_header.frame_id = vehicle_frame_id_;
    clearObjectInteractionMarkers(marker_header);
    return;
  }
 
  try {
    trajectory_planning_msgs::msg::Trajectory msg = createTrajectory(planner_state, stamp);
    diagnosed_publisher_->publish(msg);
    RCLCPP_DEBUG(this->get_logger(), "Published Trajectory!");
  } catch (const std::runtime_error& e) {
    std::string msg = "Error while creating trajectory: " + std::string(e.what());
    RCLCPP_ERROR(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::ERROR, msg, {{"PlannerState", plannerStateToString(planner_state)}});
  }
}

recalculateS()

void simple_planner::SimplePlannerNode::recalculateS ( std::vector< SimplePathPoint > & path )

staticprivate

Recomputes accumulated path distance from point positions.

Parameters

[in,out]

path

Path whose s values are updated in-place.

Definition at line 1069 of file simple_planner.cpp.

                                                                     {
  if (path.empty()) return;
 
  path[0].s = 0.0;
  for (size_t i = 1; i < path.size(); ++i) {
    double ds = (path[i].position - path[i - 1].position).norm();
    path[i].s = path[i - 1].s + ds;
  }
}

requiresTransform()

bool simple_planner::SimplePlannerNode::requiresTransform ( const std_msgs::msg::Header & source_header, const std_msgs::msg::Header & target_header )

staticprivate

Checks whether a transform between two stamped frames is required.

A transform is required if either the frame IDs or timestamps differ. The timestamp comparison ensures that temporal transforms are still applied when source and target use the same moving frame.

Parameters

[in]	source_header	Header of the source data.
[in]	target_header	Requested target frame and timestamp.

Returns

true if a spatial or temporal transform is required.

Definition at line 127 of file utils.hpp.

                                                                                  {
  return source_header.frame_id != target_header.frame_id || source_header.stamp.sec != target_header.stamp.sec ||
         source_header.stamp.nanosec != target_header.stamp.nanosec;
}

resamplePath()

std::vector< SimplePathPoint > simple_planner::SimplePlannerNode::resamplePath ( const std::vector< SimplePathPoint > & path, bool stop_at_end, double offset_to_stop_line = 0.0, const double * speed_cap = nullptr )

private

Resamples a path into trajectory time steps and applies optional stopping behavior.

Parameters

[in]	path	Input path with accumulated s values.
[in]	stop_at_end	Whether the output should brake to a stop at the path end.
[in]	offset_to_stop_line	Additional offset used when computing the braking point.
[in]	speed_cap	Optional upper velocity limit for all sampled points.

Returns

Time-resampled path points.

Definition at line 1079 of file simple_planner.cpp.

                                                                                      {
  rclcpp::Time begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
  if (path.empty()) {
    std::string msg = "Route is empty. No resampling possible.";
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    return path;
  }
 
  std::vector<SimplePathPoint> resampled_path;
  double s = path[0].s;
 
  tk::spline x_spline, y_spline;
  if (interpolation_type_ == InterpolationType::SPLINE && path.size() > 2) {
    std::vector<double> s_vector, x_vector, y_vector;
    for (size_t j = 0; j < path.size(); ++j) {
      s_vector.push_back(path[j].s);
      x_vector.push_back(path[j].position.x());
      y_vector.push_back(path[j].position.y());
    }
    x_spline.set_points(s_vector, x_vector);
    y_spline.set_points(s_vector, y_vector);
  }
 
  while (s < path.back().s) {
    // find index of segment in route (not required if using linearInterpolation from utils)
    int idx = -1;
    for (size_t j = 0; j < path.size() - 1; ++j) {
      if (s >= path[j].s && s <= path[j + 1].s) {
        idx = static_cast<int>(j);
        break;
      }
    }
 
    double v = v_ref_;               // option 1: use predefined constant velocity
    if (v_ref_ < 0.0 && idx >= 0) {  // option 2: use velocity from route if predefined velocity is negative
      v = path[idx].v + (path[idx + 1].v - path[idx].v) / (path[idx + 1].s - path[idx].s) * (s - path[idx].s);
    }
    if (speed_cap != nullptr) {
      v = std::min(v, std::max(*speed_cap, 0.0));
    }
 
    double distance_to_stop = -0.5 * std::pow(v, 2) / a_decel_ + offset_to_stop_line;
    distance_to_stop = std::max(distance_to_stop, 0.0);
    double brake_point = path.back().s - distance_to_stop;
    double ds = v * dt_;  // case 1: constant velocity
    if (s + ds > brake_point && stop_at_end) {
      if (s < brake_point) {            // special case: braking point is between two states
        double ds_1 = brake_point - s;  // distance with constant velocity to braking point
        double dt_1 = ds_1 / v;         // time with constant velocity to braking point
        double dt_2 = dt_ - dt_1;       // remaining time with deceleration
        double ds_2 = std::max(0.5 * a_decel_ * std::pow(dt_2, 2) + v * dt_2, 0.0);
        ds = ds_1 + ds_2;
      } else {
        v = std::sqrt(std::max(std::pow(v, 2) + 2 * a_decel_ * (s - brake_point), 0.0));  // case 2: deceleration (v(s))
        ds = 0.5 * a_decel_ * std::pow(dt_, 2) + v * dt_;                                 // case 2: deceleration
        if (ds < 0.0) ds = path.back().s - s;  // only add rest of route instead of driving backwards
      }
    }
 
    // interpolate point at s
    SimplePathPoint simple_path_point;
    if (path.size() == 1) {
      simple_path_point.position = path[0].position;
    } else if (interpolation_type_ == InterpolationType::SPLINE && path.size() > 2) {  // spline interpolation
      simple_path_point.position.x() = x_spline(s);
      simple_path_point.position.y() = y_spline(s);
    } else if (
        (interpolation_type_ == InterpolationType::SPLINE && path.size() <= 2) ||
        interpolation_type_ ==
            InterpolationType::
                LINEAR) {  // linear interpolation // TODO: could be improved by using our linearInterpolation function -> no need for idx anymore
      simple_path_point.position.x() = path[idx].position.x() + (path[idx + 1].position.x() - path[idx].position.x()) /
                                                                    (path[idx + 1].s - path[idx].s) * (s - path[idx].s);
      simple_path_point.position.y() = path[idx].position.y() + (path[idx + 1].position.y() - path[idx].position.y()) /
                                                                    (path[idx + 1].s - path[idx].s) * (s - path[idx].s);
    } else {  // unsupported interpolation type
      RCLCPP_ERROR(this->get_logger(), "Unsupported interpolation type value %d", interpolation_type_);
      throw std::runtime_error("Unsupported interpolation type value");
    }
    simple_path_point.s = s;
    simple_path_point.v = v;
    resampled_path.push_back(simple_path_point);
 
    // increment s and v for next iteration
    if (v <= 1e-6 && ds <= 1e-6) break;
    s = s + ds;
    if (s == path.back().s && v == 0.0) break;  // stop at end of route
  }
 
  rclcpp::Time end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
  RCLCPP_DEBUG(this->get_logger(), "Resampling route took %f ms", (end - begin).seconds() * 1e3);
 
  if (stop_at_end) {
    SimplePathPoint stop_point = path.back();
    if (!resampled_path.empty() && (offset_to_stop_line > 0.0 || (speed_cap != nullptr && *speed_cap <= 1e-6))) {
      stop_point = resampled_path.back();
    }
    stop_point.v = 0.0;
    if (resampled_path.empty() || resampled_path.back().s < stop_point.s || resampled_path.back().v != 0.0) {
      resampled_path.push_back(stop_point);
    }
  }
 
  return resampled_path;
}

resetObjectState()

void simple_planner::SimplePlannerNode::resetObjectState ( const std_msgs::msg::Header & target_header )

private

Resets the remembered object speed cap / hysteresis state and clears interaction markers.

Parameters

[in]

target_header

Header used for the cleared marker messages.

Definition at line 22 of file object_handling.cpp.

                                                                               {
  last_object_speed_cap_.reset();
  object_conflict_free_cycles_ = 0;
  clearObjectInteractionMarkers(target_header);
}

routeCallback()

void simple_planner::SimplePlannerNode::routeCallback ( const route_planning_msgs::msg::Route::UniquePtr msg )

private

Stores the latest route message and extracts the route path.

This callback is invoked when the subscriber receives a new route message.

Parameters

	msg	Latest route message.
[in]	msg	route

Definition at line 345 of file simple_planner.cpp.

                                                                                    {
  if (route_topic_diagnostic_ != nullptr) {
    route_topic_diagnostic_->tick(msg->header.stamp);
  }
  route_ = *msg;
 
  if (!route_init_) {
    RCLCPP_INFO(this->get_logger(), "Received new route message, initialized global variable");
    route_init_ = true;
    safe_stop_distance_.reset();
  }
}

setHealth()

void simple_planner::SimplePlannerNode::setHealth ( const unsigned char status, const std::string & msg, const std::map< std::string, std::string > & key_value_pairs = {} )

private

Sets the health information.

Definition at line 180 of file utils.hpp.

                                                                                       {
  health_.status = status;
  health_.message = msg;
  health_.key_value_pairs = key_value_pairs;
}

setup()

void simple_planner::SimplePlannerNode::setup ( )

private

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

Sets up subscribers, publishers, and more.

Definition at line 194 of file simple_planner.cpp.

                              {
  tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
 
  // calculate dt
  dt_ = trajectory_horizon_ / (n_states_ - 1);
 
  // create a publisher for publishing output trajectory
  pub_ = this->create_publisher<trajectory_planning_msgs::msg::Trajectory>("~/trajectory", 10);
  RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", pub_->get_topic_name());
  object_interaction_marker_pub_ =
      this->create_publisher<visualization_msgs::msg::MarkerArray>("~/viz/object_interaction_markers", 10);
  RCLCPP_INFO(this->get_logger(), "Publishing object interaction markers to '%s'",
              object_interaction_marker_pub_->get_topic_name());
 
  // create a timer for repeatedly invoking a callback to publish messages
  publish_timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / freq_),
                                           std::bind(&SimplePlannerNode::publishTimerCallback, this));
  RCLCPP_INFO(this->get_logger(), "Publishing trajectory at '%f' hz", freq_);
 
  // create subscriber for egoData
  sub_egoData_ = this->create_subscription<perception_msgs::msg::EgoData>(
      "~/ego_data", 10, std::bind(&SimplePlannerNode::egoDataCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_egoData_->get_topic_name());
 
  sub_object_list_ = this->create_subscription<perception_msgs::msg::ObjectList>(
      "~/object_list", 10, std::bind(&SimplePlannerNode::objectListCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_object_list_->get_topic_name());
 
  // create subscriber for route
  sub_route_ = this->create_subscription<route_planning_msgs::msg::Route>(
      "~/route", 10, std::bind(&SimplePlannerNode::routeCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_route_->get_topic_name());
 
  sub_grid_map_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
      "~/grid_map", 10, std::bind(&SimplePlannerNode::gridMapCallback, this, std::placeholders::_1));
  RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_grid_map_->get_topic_name());
 
  // create service clients for turn indicators and hazard lights
  left_turn_indicator_service_client_ = this->create_client<std_srvs::srv::SetBool>("~/enable_left_turn_indicator");
  RCLCPP_INFO(this->get_logger(), "Prepared service client for '%s'", left_turn_indicator_service_client_->get_service_name());
  right_turn_indicator_service_client_ = this->create_client<std_srvs::srv::SetBool>("~/enable_right_turn_indicator");
  RCLCPP_INFO(this->get_logger(), "Prepared service client for '%s'", right_turn_indicator_service_client_->get_service_name());
  hazard_lights_service_client_ = this->create_client<std_srvs::srv::SetBool>("~/enable_hazard_lights");
  RCLCPP_INFO(this->get_logger(), "Prepared service client for '%s'", hazard_lights_service_client_->get_service_name());
 
  // create a callback for dynamic parameter configuration
  parameters_callback_ =
      this->add_on_set_parameters_callback(std::bind(&SimplePlannerNode::parametersCallback, this, std::placeholders::_1));
 
  // Annotate message links for tracing: Trajectory is published periodically based on planning input subscriptions.
  std::vector<const void*> link_subs = {static_cast<const void*>(sub_egoData_->get_subscription_handle().get()),
                                        static_cast<const void*>(sub_object_list_->get_subscription_handle().get()),
                                        static_cast<const void*>(sub_route_->get_subscription_handle().get()),
                                        static_cast<const void*>(sub_grid_map_->get_subscription_handle().get())};
  std::vector<const void*> link_pubs = {static_cast<const void*>(pub_->get_publisher_handle().get())};
  TRACETOOLS_TRACEPOINT(message_link_periodic_async, link_subs.data(), link_subs.size(), link_pubs.data(), link_pubs.size());
 
  // setup diagnostic updater
  diagnostic_updater_.setHardwareID(this->get_name());
  diagnostic_updater_.add("Health", this, &SimplePlannerNode::health);
 
  const int ego_data_topic_diagnostic_frequency_window_size =
      std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * ego_data_topic_diagnostic_config_.min_frequency));
  ego_data_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
      "~/ego_data", diagnostic_updater_,
      diagnostic_updater::FrequencyStatusParam(&ego_data_topic_diagnostic_config_.min_frequency,
                                               &ego_data_topic_diagnostic_config_.max_frequency, 0.0,
                                               ego_data_topic_diagnostic_frequency_window_size),
      diagnostic_updater::TimeStampStatusParam(ego_data_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                               ego_data_topic_diagnostic_config_.max_acceptable_timestamp_delta));
 
  const int route_topic_diagnostic_frequency_window_size =
      std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * route_topic_diagnostic_config_.min_frequency));
  route_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
      "~/route", diagnostic_updater_,
      diagnostic_updater::FrequencyStatusParam(&route_topic_diagnostic_config_.min_frequency,
                                               &route_topic_diagnostic_config_.max_frequency, 0.0,
                                               route_topic_diagnostic_frequency_window_size),
      diagnostic_updater::TimeStampStatusParam(route_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                               route_topic_diagnostic_config_.max_acceptable_timestamp_delta));
 
  if (consider_objects_) {
    const int object_list_topic_diagnostic_frequency_window_size =
        std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * object_list_topic_diagnostic_config_.min_frequency));
    object_list_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
        "~/object_list", diagnostic_updater_,
        diagnostic_updater::FrequencyStatusParam(&object_list_topic_diagnostic_config_.min_frequency,
                                                 &object_list_topic_diagnostic_config_.max_frequency, 0.0,
                                                 object_list_topic_diagnostic_frequency_window_size),
        diagnostic_updater::TimeStampStatusParam(object_list_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                                 object_list_topic_diagnostic_config_.max_acceptable_timestamp_delta));
  }
 
  if (consider_grid_map_) {
    const int grid_map_topic_diagnostic_frequency_window_size =
        std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * grid_map_topic_diagnostic_config_.min_frequency));
    grid_map_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
        "~/grid_map", diagnostic_updater_,
        diagnostic_updater::FrequencyStatusParam(&grid_map_topic_diagnostic_config_.min_frequency,
                                                 &grid_map_topic_diagnostic_config_.max_frequency, 0.0,
                                                 grid_map_topic_diagnostic_frequency_window_size),
        diagnostic_updater::TimeStampStatusParam(grid_map_topic_diagnostic_config_.min_acceptable_timestamp_delta,
                                                 grid_map_topic_diagnostic_config_.max_acceptable_timestamp_delta));
  }
 
  const int diagnosed_publisher_frequency_window_size =
      std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * diagnosed_publisher_config_.min_frequency));
  diagnosed_publisher_ = std::make_unique<diagnostic_updater::DiagnosedPublisher<trajectory_planning_msgs::msg::Trajectory>>(
      pub_, diagnostic_updater_,
      diagnostic_updater::FrequencyStatusParam(&diagnosed_publisher_config_.min_frequency,
                                               &diagnosed_publisher_config_.max_frequency, 0.0,
                                               diagnosed_publisher_frequency_window_size),
      diagnostic_updater::TimeStampStatusParam(diagnosed_publisher_config_.min_acceptable_timestamp_delta,
                                               diagnosed_publisher_config_.max_acceptable_timestamp_delta));
}

transformPath()

SimplePath simple_planner::SimplePlannerNode::transformPath ( const SimplePath & path, const std_msgs::msg::Header & target_header )

private

Transforms a simple path into the requested target frame and timestamp.

Parameters

[in]	path	Path to transform.
[in]	target_header	Target frame and timestamp.

Returns

Transformed path, or an empty path in the target frame if the transform fails.

Definition at line 140 of file utils.hpp.

                                                                                                          {
  SimplePath transformed_path;
  transformed_path.header = target_header;
 
  if (path.points.empty()) {
    return transformed_path;
  }
  if (!requiresTransform(path.header, target_header)) {
    return path;
  }
 
  geometry_msgs::msg::TransformStamped tf;
  try {
    tf = tf2_buffer_->lookupTransform(target_header.frame_id, target_header.stamp, path.header.frame_id, path.header.stamp,
                                      fixed_over_time_frame_id_, rclcpp::Duration::from_seconds(1.0));
    for (const auto& point : path.points) {
      geometry_msgs::msg::PointStamped point_msg, transformed_point_msg;
      point_msg.header = path.header;
      point_msg.point.x = point.position.x();
      point_msg.point.y = point.position.y();
      point_msg.point.z = 0.0;
      tf2::doTransform(point_msg, transformed_point_msg, tf);
      SimplePathPoint transformed_point = point;
      transformed_point.position = Eigen::Vector2d(transformed_point_msg.point.x, transformed_point_msg.point.y);
      transformed_path.points.push_back(transformed_point);
    }
  } catch (tf2::TransformException& ex) {
    RCLCPP_WARN(this->get_logger(), "Could not transform path: %s. Returning an empty path.", ex.what());
  }
 
  return transformed_path;
}

trimPathBehindEgo()

void simple_planner::SimplePlannerNode::trimPathBehindEgo ( SimplePath & path )

staticprivate

Removes path points that lie behind the ego vehicle in vehicle frame.

Parameters

[in,out]

path

Path to be trimmed in-place.

Definition at line 943 of file simple_planner.cpp.

                                                          {
  while (!path.points.empty() && path.points[0].position.x() < 0.0) {
    path.points.erase(path.points.begin());
  }
}

truncatePathAtS()

std::vector< SimplePathPoint > simple_planner::SimplePlannerNode::truncatePathAtS ( const std::vector< SimplePathPoint > & path, double stop_s )

staticprivate

Returns a path ending exactly at the requested accumulated path coordinate.

If stop_s lies between two path points, the final point is linearly interpolated.

Parameters

[in]	path	Input path with accumulated s values.
[in]	stop_s	Accumulated path coordinate of the new endpoint.

Returns

Truncated path including an endpoint at stop_s.

Definition at line 1028 of file simple_planner.cpp.

                                                                                                                   {
  if (path.empty()) {
    return path;
  }
 
  if (stop_s <= path.front().s) {
    SimplePathPoint stop_point = path.front();
    stop_point.s = stop_s;
    return {stop_point};
  }
 
  if (stop_s >= path.back().s) {
    return path;
  }
 
  const auto stop_it =
      std::lower_bound(path.begin(), path.end(), stop_s, [](const auto& point, double s) { return point.s < s; });
  if (stop_it == path.begin()) {
    return {*stop_it};
  }
  if (stop_it == path.end()) {
    return path;
  }
 
  std::vector<SimplePathPoint> truncated_path(path.begin(), stop_it);
  if (std::abs(stop_it->s - stop_s) <= 1e-6) {
    truncated_path.push_back(*stop_it);
    return truncated_path;
  }
 
  const auto& previous = *(stop_it - 1);
  const double segment_ds = stop_it->s - previous.s;
  const double alpha = segment_ds > 1e-9 ? std::clamp((stop_s - previous.s) / segment_ds, 0.0, 1.0) : 0.0;
  SimplePathPoint stop_point;
  stop_point.position = previous.position + alpha * (stop_it->position - previous.position);
  stop_point.s = stop_s;
  stop_point.v = previous.v + alpha * (stop_it->v - previous.v);
  truncated_path.push_back(stop_point);
  return truncated_path;
}

tryRegisterLaneChange()

bool simple_planner::SimplePlannerNode::tryRegisterLaneChange ( const route_planning_msgs::msg::Route & tf_route, size_t route_element_idx, std::map< uint64_t, uint64_t > & lane_change_indices_map, uint8_t & suggested_turn_signal )

private

Detects and stores the start/end window of a lane change.

Parameters

[in]	tf_route	Route transformed into vehicle frame.
[in]	route_element_idx	Index of the current route element.
[out]	lane_change_indices_map	Output map of lane-change route indices.
[in,out]	suggested_turn_signal	Turn signal selected for the current route plan.

Returns

true if the lane change could be registered.

false if the route data is insufficient and processing should stop.

Definition at line 716 of file simple_planner.cpp.

                                                                              {
  if (route_element_idx + 1 >= tf_route.route_elements.size()) {
    std::string msg = "Route element " + std::to_string(route_element_idx) + " is the last element. Cannot change lane.";
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    return false;
  }
 
  const size_t i_end = route_element_idx + 1;
  const auto& route_element = tf_route.route_elements[route_element_idx];
  size_t current_lane_idx = route_element.suggested_lane_idx;
  int lane_change_direction = 0;
  try {
    lane_change_direction =
        route_planning_msgs::route_access::getLaneChangeDirection(route_element, tf_route.route_elements[route_element_idx + 1]);
  } catch (const std::exception& ex) {
    RCLCPP_WARN(this->get_logger(), "Could not determine lane change direction at route element %zu: %s", route_element_idx,
                ex.what());
    return false;
  }
  if (lane_change_direction < 0) {
    suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT;
  } else if (lane_change_direction > 0) {
    suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT;
  }
 
  double ego_velocity = perception_msgs::object_access::getVelocityMagnitude(ego_data_);
  double lane_change_distance =
      std::max(lane_change_min_distance_factor_ * ego_data_.length, lane_change_distance_factor_ * ego_velocity);
  RCLCPP_DEBUG(this->get_logger(), "Lane change direction: %d, lane change distance: %f", lane_change_direction,
               lane_change_distance);
  if (lane_change_direction == 0) {
    RCLCPP_WARN(this->get_logger(),
                "Route element %zu is marked as lane change, but suggested lane does not change. Ignoring lane change marker.",
                route_element_idx);
    return true;
  }
 
  double ds = 0.0;
  size_t i_start = route_element_idx;
  while (ds < lane_change_distance && i_start > 0) {
    if (auto result = route_planning_msgs::route_access::getPrecedingLaneElementIdx(current_lane_idx,
                                                                                    tf_route.route_elements[i_start - 1])) {
      current_lane_idx = *result;
    } else {
      std::string msg =
          "No preceding lane element found for route element " + std::to_string(i_start) + ". Cannot extend lane change.";
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      break;
    }
    if (i_start >= 2 && tf_route.route_elements[i_start - 2].will_change_suggested_lane) {
      std::string msg = "Found previous lane change in route element " + std::to_string(i_start - 2) +
                        ". Could not extend lane change over this element.";
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      break;
    }
    if (!route_planning_msgs::route_access::hasAdjacentLane(tf_route.route_elements[i_start - 1], current_lane_idx,
                                                            lane_change_direction)) {
      std::string msg = "No adjacent lane found for route element " + std::to_string(i_start - 1) + ".";
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      break;
    }
    ds += std::abs(tf_route.route_elements[i_start].s - tf_route.route_elements[i_start - 1].s);
    i_start--;
  }
 
  if (!tf_route.route_elements[i_start].is_enriched || !tf_route.route_elements[i_end].is_enriched) {
    std::string msg =
        "Not enough enriched route elements (" + std::to_string(i_start) + ", " + std::to_string(i_end) + ") for lane change.";
    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
    return false;
  }
 
  lane_change_indices_map[route_element_idx] = i_start;
  return true;
}

turnSignalToString()

std::string simple_planner::SimplePlannerNode::turnSignalToString ( const uint8_t & turn_signal )

staticprivate

Converts a turn signal value to a string representation.

Parameters

turn_signal

Returns

std::string

Definition at line 203 of file utils.hpp.

                                                                          {
  switch (turn_signal) {
    case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE:
      return "None";
    case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT:
      return "Left";
    case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT:
      return "Right";
    case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_HAZARD:
      return "Hazard";
    default:
      return "Unknown";
  }
}

updateForTrafficLights()

void simple_planner::SimplePlannerNode::updateForTrafficLights ( const route_planning_msgs::msg::Route & tf_route, size_t route_element_idx, const route_planning_msgs::msg::LaneElement & suggested_lane, const SimplePathPoint & simple_path_point, double t_total, bool & stop_at_end, double & offset_to_stop_line )

private

Updates stop-at-end and stop-line offset state for traffic-light regulatory elements.

Parameters

[in]	tf_route	Route transformed into vehicle frame.
[in]	route_element_idx	Index of the current route element.
[in]	suggested_lane	Suggested lane element of the current route element.
[in]	simple_path_point	Current path point candidate.
[in]	t_total	Accumulated travel time along the partial path.
[in,out]	stop_at_end	Whether the path should stop at its current end.
[in,out]	offset_to_stop_line	Effective offset used for braking towards the stop line.

Definition at line 800 of file simple_planner.cpp.

                                                                            {
  const auto& route_element = tf_route.route_elements[route_element_idx];
  const auto& reg_elems =
      route_planning_msgs::route_access::getRegulatoryElementsOfLaneElement(suggested_lane, route_element.regulatory_elements);
  for (size_t k = 0; k < reg_elems.size(); ++k) {
    if (reg_elems[k].type != route_planning_msgs::msg::RegulatoryElement::TYPE_TRAFFIC_LIGHT) {
      continue;
    }
    if (reg_elems[k].meta_value == route_planning_msgs::msg::RegulatoryElement::META_VALUE_MOVEMENT_ALLOWED &&
        !consider_future_states_) {
      continue;
    }
 
    offset_to_stop_line =
        offset_to_stop_line_ + ego_data_.length / 2.0 + ego_data_.state.reference_point.translation_to_geometric_center.x;
    double dt_offset_to_stop_line = 0.0;
    if (simple_path_point.v != 0.0) {
      dt_offset_to_stop_line = offset_to_stop_line / simple_path_point.v;
    }
    if (dt_offset_to_stop_line <= 0.0) {
      std::string msg = "Negative time difference 'dt_offset_to_stop_line' (" + std::to_string(dt_offset_to_stop_line) +
                        " s) for traffic light at route element " + std::to_string(route_element_idx) +
                        ". Could lead to unexpected behavior.";
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
    }
 
    if (reg_elems[k].has_validity_stamp && consider_future_states_) {
      double validity_duration =
          rclcpp::Time(reg_elems[k].validity_stamp).seconds() - rclcpp::Time(route_.header.stamp).seconds();
      if (validity_duration < (t_total - dt_offset_to_stop_line)) {
        if (reg_elems[k].meta_value == route_planning_msgs::msg::RegulatoryElement::META_VALUE_MOVEMENT_ALLOWED) {
          stop_at_end = true;
        } else {
          continue;
        }
      } else {
        if (reg_elems[k].meta_value == route_planning_msgs::msg::RegulatoryElement::META_VALUE_MOVEMENT_ALLOWED) {
          continue;
        } else {
          stop_at_end = true;
        }
      }
    } else {
      stop_at_end = true;
    }
 
    double distance_to_stop_point = tf_route.route_elements[route_element_idx].s -
                                    tf_route.route_elements[tf_route.current_route_element_idx].s - offset_to_stop_line;
    double v_ego = perception_msgs::object_access::getVelocityMagnitude(ego_data_);
    double min_distance_to_stop = -0.5 * std::pow(v_ego, 2) / a_max_decel_;
    if (distance_to_stop_point < 0.0 && std::abs(distance_to_stop_point) > ignore_stop_line_threshold_) {
      std::string msg =
          "Traffic light stop point is behind ego vehicle (distance to stop point: " + std::to_string(distance_to_stop_point) +
          " m). Ignoring traffic light.";
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      stop_at_end = false;
    } else if ((distance_to_stop_point < min_distance_to_stop) && stop_at_end) {
      std::string msg =
          "Traffic light requires stop, but distance to stop point is smaller than minimum distance to stop. Ignoring traffic "
          "light.";
      setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
      RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
      stop_at_end = false;
    }
  }
}

Member Data Documentation

a_decel_

double simple_planner::SimplePlannerNode::a_decel_ = -0.5

private

Definition at line 576 of file simple_planner.hpp.

a_max_decel_

double simple_planner::SimplePlannerNode::a_max_decel_ = -1.0

private

Definition at line 577 of file simple_planner.hpp.

auto_reconfigurable_params_

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

private

Auto-reconfigurable parameters for dynamic reconfiguration.

Definition at line 556 of file simple_planner.hpp.

consider_future_states_

bool simple_planner::SimplePlannerNode::consider_future_states_ = false

private

Definition at line 587 of file simple_planner.hpp.

consider_grid_map_

bool simple_planner::SimplePlannerNode::consider_grid_map_ = false

private

Definition at line 579 of file simple_planner.hpp.

consider_objects_

bool simple_planner::SimplePlannerNode::consider_objects_ = true

private

Definition at line 588 of file simple_planner.hpp.

consider_out_of_grid_

bool simple_planner::SimplePlannerNode::consider_out_of_grid_ = false

private

Definition at line 581 of file simple_planner.hpp.

consider_traffic_lights_

bool simple_planner::SimplePlannerNode::consider_traffic_lights_ = true

private

Definition at line 584 of file simple_planner.hpp.

diagnosed_publisher_

std::unique_ptr<diagnostic_updater::DiagnosedPublisher<trajectory_planning_msgs::msg::Trajectory> > simple_planner::SimplePlannerNode::diagnosed_publisher_

private

Definition at line 643 of file simple_planner.hpp.

diagnosed_publisher_config_

TopicDiagnosticConfig simple_planner::SimplePlannerNode::diagnosed_publisher_config_ {9.09, 11.11, 0.0, 0.01}

private

Definition at line 644 of file simple_planner.hpp.

{9.09, 11.11, 0.0, 0.01};

diagnostic_updater_

diagnostic_updater::Updater simple_planner::SimplePlannerNode::diagnostic_updater_ {this}

private

Diagnostic updater.

Definition at line 620 of file simple_planner.hpp.

{this};

dt_

double simple_planner::SimplePlannerNode::dt_

private

Definition at line 615 of file simple_planner.hpp.

ego_data_

perception_msgs::msg::EgoData simple_planner::SimplePlannerNode::ego_data_

private

Definition at line 602 of file simple_planner.hpp.

ego_data_init_

bool simple_planner::SimplePlannerNode::ego_data_init_ = false

private

Definition at line 607 of file simple_planner.hpp.

ego_data_timeout_

double simple_planner::SimplePlannerNode::ego_data_timeout_ = 1.0

private

Definition at line 569 of file simple_planner.hpp.

ego_data_topic_diagnostic_

std::unique_ptr<diagnostic_updater::TopicDiagnostic> simple_planner::SimplePlannerNode::ego_data_topic_diagnostic_

private

Definition at line 631 of file simple_planner.hpp.

ego_data_topic_diagnostic_config_

TopicDiagnosticConfig simple_planner::SimplePlannerNode::ego_data_topic_diagnostic_config_ {45.45, 55.55, 0.0, 0.002}

private

Definition at line 632 of file simple_planner.hpp.

{45.45, 55.55, 0.0, 0.002};

fixed_over_time_frame_id_

std::string simple_planner::SimplePlannerNode::fixed_over_time_frame_id_ = "map"

private

Definition at line 566 of file simple_planner.hpp.

freq_

double simple_planner::SimplePlannerNode::freq_ = 10.0

private

Definition at line 567 of file simple_planner.hpp.

grid_lateral_safety_distance_

double simple_planner::SimplePlannerNode::grid_lateral_safety_distance_ = 0.0

private

Definition at line 583 of file simple_planner.hpp.

grid_longitudinal_safety_distance_

double simple_planner::SimplePlannerNode::grid_longitudinal_safety_distance_ = 1.5

private

Definition at line 582 of file simple_planner.hpp.

grid_map_

nav_msgs::msg::OccupancyGrid simple_planner::SimplePlannerNode::grid_map_

private

Definition at line 605 of file simple_planner.hpp.

grid_map_init_

bool simple_planner::SimplePlannerNode::grid_map_init_ = false

private

Definition at line 610 of file simple_planner.hpp.

grid_map_timeout_

double simple_planner::SimplePlannerNode::grid_map_timeout_ = 1.0

private

Definition at line 571 of file simple_planner.hpp.

grid_map_topic_diagnostic_

std::unique_ptr<diagnostic_updater::TopicDiagnostic> simple_planner::SimplePlannerNode::grid_map_topic_diagnostic_

private

Definition at line 640 of file simple_planner.hpp.

grid_map_topic_diagnostic_config_

TopicDiagnosticConfig simple_planner::SimplePlannerNode::grid_map_topic_diagnostic_config_ {9.09, 11.11, 0.0, 0.01}

private

Definition at line 641 of file simple_planner.hpp.

{9.09, 11.11, 0.0, 0.01};

grid_occupied_threshold_

int simple_planner::SimplePlannerNode::grid_occupied_threshold_ = 50

private

Definition at line 580 of file simple_planner.hpp.

hazard_lights_service_client_

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr simple_planner::SimplePlannerNode::hazard_lights_service_client_

private

Definition at line 551 of file simple_planner.hpp.

health_

struct simple_planner::SimplePlannerNode::DiagnosticStatus simple_planner::SimplePlannerNode::health_

private

ignore_stop_line_threshold_

double simple_planner::SimplePlannerNode::ignore_stop_line_threshold_ = 0.5

private

Definition at line 586 of file simple_planner.hpp.

interpolation_type_

uint8_t simple_planner::SimplePlannerNode::interpolation_type_ = InterpolationType::SPLINE

private

Definition at line 574 of file simple_planner.hpp.

kMinObjectLength

double simple_planner::SimplePlannerNode::kMinObjectLength = 1.2

staticconstexprprivate

Definition at line 120 of file simple_planner.hpp.

kMinObjectWidth

double simple_planner::SimplePlannerNode::kMinObjectWidth = 0.8

staticconstexprprivate

Definition at line 119 of file simple_planner.hpp.

kObjectCollisionCheckDt

double simple_planner::SimplePlannerNode::kObjectCollisionCheckDt = 0.05

staticconstexprprivate

Definition at line 118 of file simple_planner.hpp.

lane_change_distance_factor_

double simple_planner::SimplePlannerNode::lane_change_distance_factor_ = 6.0

private

Definition at line 599 of file simple_planner.hpp.

lane_change_min_distance_factor_

double simple_planner::SimplePlannerNode::lane_change_min_distance_factor_ = 2.0

private

Definition at line 600 of file simple_planner.hpp.

last_object_speed_cap_

std::optional<double> simple_planner::SimplePlannerNode::last_object_speed_cap_

private

Definition at line 612 of file simple_planner.hpp.

latest_path_

SimplePath simple_planner::SimplePlannerNode::latest_path_

private

Definition at line 613 of file simple_planner.hpp.

left_turn_indicator_service_client_

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr simple_planner::SimplePlannerNode::left_turn_indicator_service_client_

private

Definition at line 549 of file simple_planner.hpp.

min_prediction_prob_

double simple_planner::SimplePlannerNode::min_prediction_prob_ = 0.0

private

Definition at line 589 of file simple_planner.hpp.

n_states_

int simple_planner::SimplePlannerNode::n_states_ = 51

private

Definition at line 573 of file simple_planner.hpp.

object_conflict_free_cycles_

int simple_planner::SimplePlannerNode::object_conflict_free_cycles_ = 0

private

Definition at line 614 of file simple_planner.hpp.

object_interaction_marker_pub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr simple_planner::SimplePlannerNode::object_interaction_marker_pub_

private

Definition at line 546 of file simple_planner.hpp.

object_interaction_time_window_

double simple_planner::SimplePlannerNode::object_interaction_time_window_ = 0.5

private

Definition at line 592 of file simple_planner.hpp.

object_lateral_safety_distance_

double simple_planner::SimplePlannerNode::object_lateral_safety_distance_ = 0.0

private

Definition at line 591 of file simple_planner.hpp.

object_list_

perception_msgs::msg::ObjectList simple_planner::SimplePlannerNode::object_list_

private

Definition at line 603 of file simple_planner.hpp.

object_list_init_

bool simple_planner::SimplePlannerNode::object_list_init_ = false

private

Definition at line 608 of file simple_planner.hpp.

object_list_topic_diagnostic_

std::unique_ptr<diagnostic_updater::TopicDiagnostic> simple_planner::SimplePlannerNode::object_list_topic_diagnostic_

private

Definition at line 634 of file simple_planner.hpp.

object_list_topic_diagnostic_config_

TopicDiagnosticConfig simple_planner::SimplePlannerNode::object_list_topic_diagnostic_config_ {9.09, 11.11, 0.0, 0.01}

private

Definition at line 635 of file simple_planner.hpp.

{9.09, 11.11, 0.0, 0.01};

object_longitudinal_safety_distance_

double simple_planner::SimplePlannerNode::object_longitudinal_safety_distance_ = 1.5

private

Definition at line 590 of file simple_planner.hpp.

object_standstill_speed_threshold_

double simple_planner::SimplePlannerNode::object_standstill_speed_threshold_ = 0.2

private

Definition at line 595 of file simple_planner.hpp.

object_timeout_

double simple_planner::SimplePlannerNode::object_timeout_ = 1.0

private

Definition at line 570 of file simple_planner.hpp.

object_velocity_reduction_step_

double simple_planner::SimplePlannerNode::object_velocity_reduction_step_ = 0.1

private

Definition at line 593 of file simple_planner.hpp.

object_velocity_release_hysteresis_cycles_

int simple_planner::SimplePlannerNode::object_velocity_release_hysteresis_cycles_ = 3

private

Definition at line 596 of file simple_planner.hpp.

object_velocity_release_step_

double simple_planner::SimplePlannerNode::object_velocity_release_step_ = 0.5

private

Definition at line 594 of file simple_planner.hpp.

offset_to_stop_line_

double simple_planner::SimplePlannerNode::offset_to_stop_line_ = 0.0

private

Definition at line 585 of file simple_planner.hpp.

parameters_callback_

OnSetParametersCallbackHandle::SharedPtr simple_planner::SimplePlannerNode::parameters_callback_

private

Callback handle for dynamic parameter reconfiguration.

Definition at line 561 of file simple_planner.hpp.

pub_

rclcpp::Publisher<trajectory_planning_msgs::msg::Trajectory>::SharedPtr simple_planner::SimplePlannerNode::pub_

private

Definition at line 545 of file simple_planner.hpp.

publish_object_interaction_markers_

bool simple_planner::SimplePlannerNode::publish_object_interaction_markers_ = true

private

Definition at line 597 of file simple_planner.hpp.

publish_timer_

rclcpp::TimerBase::SharedPtr simple_planner::SimplePlannerNode::publish_timer_

private

Definition at line 548 of file simple_planner.hpp.

right_turn_indicator_service_client_

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr simple_planner::SimplePlannerNode::right_turn_indicator_service_client_

private

Definition at line 550 of file simple_planner.hpp.

route_

route_planning_msgs::msg::Route simple_planner::SimplePlannerNode::route_

private

Definition at line 604 of file simple_planner.hpp.

route_init_

bool simple_planner::SimplePlannerNode::route_init_ = false

private

Definition at line 609 of file simple_planner.hpp.

route_timeout_

double simple_planner::SimplePlannerNode::route_timeout_ = 1.0

private

Definition at line 568 of file simple_planner.hpp.

route_topic_diagnostic_

std::unique_ptr<diagnostic_updater::TopicDiagnostic> simple_planner::SimplePlannerNode::route_topic_diagnostic_

private

Definition at line 637 of file simple_planner.hpp.

route_topic_diagnostic_config_

TopicDiagnosticConfig simple_planner::SimplePlannerNode::route_topic_diagnostic_config_ {18.18, 22.22, 0.0, 0.005}

private

Definition at line 638 of file simple_planner.hpp.

{18.18, 22.22, 0.0, 0.005};

safe_stop_distance_

std::optional<double> simple_planner::SimplePlannerNode::safe_stop_distance_

private

Definition at line 611 of file simple_planner.hpp.

sub_egoData_

rclcpp::Subscription<perception_msgs::msg::EgoData>::SharedPtr simple_planner::SimplePlannerNode::sub_egoData_

private

Definition at line 540 of file simple_planner.hpp.

sub_grid_map_

rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr simple_planner::SimplePlannerNode::sub_grid_map_

private

Definition at line 543 of file simple_planner.hpp.

sub_object_list_

rclcpp::Subscription<perception_msgs::msg::ObjectList>::SharedPtr simple_planner::SimplePlannerNode::sub_object_list_

private

Definition at line 541 of file simple_planner.hpp.

sub_route_

rclcpp::Subscription<route_planning_msgs::msg::Route>::SharedPtr simple_planner::SimplePlannerNode::sub_route_

private

Definition at line 542 of file simple_planner.hpp.

tf2_buffer_

std::unique_ptr<tf2_ros::Buffer> simple_planner::SimplePlannerNode::tf2_buffer_

private

Definition at line 537 of file simple_planner.hpp.

tf2_listener_

std::shared_ptr<tf2_ros::TransformListener> simple_planner::SimplePlannerNode::tf2_listener_

private

Definition at line 538 of file simple_planner.hpp.

trajectory_frame_id_

std::string simple_planner::SimplePlannerNode::trajectory_frame_id_ = "base_link"

private

Definition at line 565 of file simple_planner.hpp.

trajectory_horizon_

double simple_planner::SimplePlannerNode::trajectory_horizon_ = 10.0

private

Definition at line 572 of file simple_planner.hpp.

trigger_turn_signals_

bool simple_planner::SimplePlannerNode::trigger_turn_signals_ = true

private

Definition at line 578 of file simple_planner.hpp.

v_ref_

double simple_planner::SimplePlannerNode::v_ref_ = 13.89

private

Definition at line 575 of file simple_planner.hpp.

vehicle_frame_id_

std::string simple_planner::SimplePlannerNode::vehicle_frame_id_ = "base_link"

private

Definition at line 564 of file simple_planner.hpp.

---

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

• simple_planner/include/simple_planner/simple_planner.hpp
• simple_planner/include/simple_planner/utils.hpp
• simple_planner/src/grid_map_handling.cpp
• simple_planner/src/object_handling.cpp
• simple_planner/src/simple_planner.cpp