simple_planner v1.5.0
Loading...
Searching...
No Matches
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.
 
std::optional< std::string > updateForRegulatoryElements (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 supported 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
 
bool consider_stop_signs_ = true
 
bool consider_yield_signs_ = true
 
double offset_to_stop_line_ = 0.0
 
double ignore_stop_line_threshold_ = 0.5
 
bool consider_future_states_ = false
 
bool consider_objects_ = true
 
bool ignore_objects_behind_ego_ = true
 
double min_prediction_probability_ = 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

◆ PlannerState

Constructor & Destructor Documentation

◆ SimplePlannerNode()

simple_planner::SimplePlannerNode::SimplePlannerNode ( )

Creates a SimplePlannerNode node.

Definition at line 32 of file simple_planner.cpp.

32 : Node("simple_planner_node") {
33 this->declareAndLoadParameter("vehicle_frame_id", vehicle_frame_id_,
34 "Frame ID of local vehicle frame in which the trajectory is planned");
35 this->declareAndLoadParameter("trajectory_frame_id", trajectory_frame_id_, "Frame ID of published reference trajectory");
36 this->declareAndLoadParameter("fixed_over_time_frame_id", fixed_over_time_frame_id_,
37 "Frame ID of frame that is fixed over time for finding temporal transforms");
38 this->declareAndLoadParameter("frequency", freq_, "Frequency of reference planning cycle (Hz)");
39 this->declareAndLoadParameter("route_timeout", route_timeout_,
40 "Time after which a received route is considered invalid (s) (use -1 for no timeout)");
41 this->declareAndLoadParameter("ego_data_timeout", ego_data_timeout_,
42 "Time after which a received ego vehicle data is considered invalid (s) (use -1 for no timeout)");
43 this->declareAndLoadParameter("object_timeout", object_timeout_,
44 "Time after which a received object list is considered invalid (s) (use -1 for no timeout)");
45 this->declareAndLoadParameter("grid_map_timeout", grid_map_timeout_,
46 "Time after which a received grid map is considered invalid (s) (use -1 for no timeout)");
47 this->declareAndLoadParameter("trajectory_horizon", trajectory_horizon_, "Time horizon of the reference trajectory (s)");
48 this->declareAndLoadParameter("n_states", n_states_, "Number of states in the output trajectory");
49 this->declareAndLoadParameter("interpolation_type", interpolation_type_, "0: linear, 1: cubic spline");
51 "v_ref", v_ref_,
52 "Reference velocity (m/s); set for all states in the trajectory. Set to '-1.0' to use velocity from route.");
53 this->declareAndLoadParameter("a_decel", a_decel_,
54 "Desired deceleration for braking at stop lines or end of route (m/s^2) - must be < 0.0");
55 this->declareAndLoadParameter("a_max_decel", a_max_decel_,
56 "Maximum deceleration for safe-stop trajectories (m/s^2) - must be < 0.0 and <= a_decel");
57 this->declareAndLoadParameter("trigger_turn_signals", trigger_turn_signals_,
58 "True: planner will trigger turn signal services; false: planner will not request turn signals");
59 this->declareAndLoadParameter("consider_grid_map", consider_grid_map_,
60 "True: planner will consider grid map; false: planner will ignore grid map");
61 this->declareAndLoadParameter("grid_occupied_threshold", grid_occupied_threshold_,
62 "Minimum occupancy value that is considered as blocked within the grid map", true, false, false,
63 0.0, 100.0, 1.0);
64 this->declareAndLoadParameter("consider_out_of_grid", consider_out_of_grid_,
65 "True: path points falling outside the grid map are treated as blocked; false: points outside "
66 "the grid map are treated as free to drive");
67 this->declareAndLoadParameter("grid_longitudinal_safety_distance", grid_longitudinal_safety_distance_,
68 "Longitudinal ego-box margin for occupied-cell collision checks; negative values shrink the box "
69 "(m)",
70 true, false, false, -20.0, 20.0, 0.1);
71 this->declareAndLoadParameter("grid_lateral_safety_distance", grid_lateral_safety_distance_,
72 "Lateral ego-box margin for occupied-cell collision checks; negative values shrink the box (m)",
73 true, false, false, -10.0, 10.0, 0.1);
74 this->declareAndLoadParameter("consider_traffic_lights", consider_traffic_lights_,
75 "True: planner will consider traffic lights; false: planner will ignore traffic lights");
76 this->declareAndLoadParameter("consider_stop_signs", consider_stop_signs_,
77 "True: planner will consider stop signs; false: planner will ignore stop signs");
78 this->declareAndLoadParameter("consider_yield_signs", consider_yield_signs_,
79 "True: planner will consider yield signs; false: planner will ignore yield signs");
80 this->declareAndLoadParameter("offset_to_stop_line", offset_to_stop_line_,
81 "Additional distance to stop in front of a regulatory-element stop line (m) (default: "
82 "0.0 -> stops with front of vehicle at stop line)");
84 "ignore_stop_line_threshold", ignore_stop_line_threshold_,
85 "A stop line will be ignored if the front of the vehicle has already passed the stop line by more than this threshold (m)");
86 this->declareAndLoadParameter("consider_future_states", consider_future_states_,
87 "True: trajectory will consider forecast states of supported regulatory elements; false: "
88 "trajectory will only consider their current state");
89 this->declareAndLoadParameter("consider_objects", consider_objects_,
90 "True: planner will consider perceived objects on the route; false: planner will ignore objects");
91 this->declareAndLoadParameter("ignore_objects_behind_ego", ignore_objects_behind_ego_,
92 "Exclude objects with reference-point x < 0 in vehicle_frame_id");
93 this->declareAndLoadParameter("min_prediction_probability", min_prediction_probability_,
94 "Consider predictions with probability > this threshold; otherwise use the most likely one");
95 this->declareAndLoadParameter("object_longitudinal_safety_distance", object_longitudinal_safety_distance_,
96 "Longitudinal ego-box margin for object conflict detection; negative values shrink the box (m)",
97 true, false, false, -20.0, 20.0, 0.1);
98 this->declareAndLoadParameter("object_lateral_safety_distance", object_lateral_safety_distance_,
99 "Lateral ego-box margin for object conflict detection; negative values shrink the box (m)", true,
100 false, false, -10.0, 10.0, 0.1);
101 this->declareAndLoadParameter("object_interaction_time_window", object_interaction_time_window_,
102 "Maximum time offset for counting a spatial overlap as interaction (s)");
103 this->declareAndLoadParameter("object_velocity_reduction_step", object_velocity_reduction_step_,
104 "Velocity decrement per object-avoidance iteration (m/s)", true, false, false, 1e-3, 40.0, 1e-3);
105 this->declareAndLoadParameter("object_velocity_release_step", object_velocity_release_step_,
106 "Maximum velocity increase per cycle after hysteresis cleared object conflicts (m/s)", true,
107 false, false, 1e-3, 40.0, 1e-3);
108 this->declareAndLoadParameter("object_standstill_speed_threshold", object_standstill_speed_threshold_,
109 "Publish standstill if object avoidance would require a lower speed cap (m/s)", true, false,
110 false, 0.0, 10.0, 1e-3);
111 this->declareAndLoadParameter("object_velocity_release_hysteresis_cycles", object_velocity_release_hysteresis_cycles_,
112 "Number of conflict-free cycles required before increasing the remembered object speed cap", true,
113 false, false, 0.0, 100.0, 1.0);
114 this->declareAndLoadParameter("publish_object_interaction_markers", publish_object_interaction_markers_,
115 "Publish RViz markers for the conflict explaining the final speed reduction");
116 this->declareAndLoadParameter("lane_change_distance_factor", lane_change_distance_factor_,
117 "Factor multiplied with the current velocity to determine the lane change distance (m)");
118 this->declareAndLoadParameter("lane_change_min_distance_factor", lane_change_min_distance_factor_,
119 "Factor multiplied with the vehicle length to determine the minimum lane change distance (m)");
120
121 // check parameters
122 if (a_decel_ >= 0.0) {
123 RCLCPP_ERROR(this->get_logger(), "Invalid parameter: a_decel must be < 0.0");
124 exit(EXIT_FAILURE);
125 }
126 if (a_max_decel_ > a_decel_) {
127 RCLCPP_ERROR(this->get_logger(), "Invalid parameter: a_max_decel must be < 0.0 and <= a_decel");
128 exit(EXIT_FAILURE);
129 }
130
131 // diagnostics parameters
132 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.min_frequency",
134 "Minimum frequency for incoming ego-data messages", false);
135 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.max_frequency",
137 "Maximum frequency for incoming ego-data messages", false);
138 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.min_acceptable_timestamp_delta",
140 "Minimum acceptable timestamp delta for incoming ego-data messages", false);
141 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.ego_data.max_acceptable_timestamp_delta",
143 "Maximum acceptable timestamp delta for incoming ego-data messages", false);
144 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.min_frequency",
146 "Minimum frequency for incoming object-list messages", false);
147 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.max_frequency",
149 "Maximum frequency for incoming object-list messages", false);
150 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.min_acceptable_timestamp_delta",
152 "Minimum acceptable timestamp delta for incoming object-list messages", false);
153 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.object_list.max_acceptable_timestamp_delta",
155 "Maximum acceptable timestamp delta for incoming object-list messages", false);
156 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.min_frequency",
157 route_topic_diagnostic_config_.min_frequency, "Minimum frequency for incoming route messages",
158 false);
159 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.max_frequency",
160 route_topic_diagnostic_config_.max_frequency, "Maximum frequency for incoming route messages",
161 false);
162 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.min_acceptable_timestamp_delta",
164 "Minimum acceptable timestamp delta for incoming route messages", false);
165 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.route.max_acceptable_timestamp_delta",
167 "Maximum acceptable timestamp delta for incoming route messages", false);
168 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.min_frequency",
170 "Minimum frequency for incoming grid-map messages", false);
171 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.max_frequency",
173 "Maximum frequency for incoming grid-map messages", false);
174 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.min_acceptable_timestamp_delta",
176 "Minimum acceptable timestamp delta for incoming grid-map messages", false);
177 this->declareAndLoadParameter("diagnostic_updater.topic_diagnostics.grid_map.max_acceptable_timestamp_delta",
179 "Maximum acceptable timestamp delta for incoming grid-map messages", false);
180 this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.min_frequency",
181 diagnosed_publisher_config_.min_frequency, "Minimum frequency for published trajectory messages",
182 false);
183 this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.max_frequency",
184 diagnosed_publisher_config_.max_frequency, "Maximum frequency for published trajectory messages",
185 false);
186 this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.min_acceptable_timestamp_delta",
188 "Minimum acceptable timestamp delta for published trajectory messages", false);
189 this->declareAndLoadParameter("diagnostic_updater.diagnosed_publishers.trajectory.max_acceptable_timestamp_delta",
191 "Maximum acceptable timestamp delta for published trajectory messages", false);
192
193 this->setup();
194}
void setup()
Sets up subscribers, publishers, etc. to configure the node.
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.
Definition utils.hpp:7
TopicDiagnosticConfig grid_map_topic_diagnostic_config_
TopicDiagnosticConfig ego_data_topic_diagnostic_config_
TopicDiagnosticConfig object_list_topic_diagnostic_config_
TopicDiagnosticConfig diagnosed_publisher_config_
TopicDiagnosticConfig route_topic_diagnostic_config_
double max_acceptable_timestamp_delta
Maximum acceptable difference between message timestamp and receipt time (in seconds)
double min_frequency
Minimum acceptable frequency.
double max_frequency
Maximum acceptable frequency.
double min_acceptable_timestamp_delta
Minimum acceptable difference between message timestamp and receipt time (in seconds)

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_routeRoute transformed into vehicle frame.
[in,out]route_planMutable route planning result to extend.
[out]lane_change_indices_mapOutput lane-change windows to be merged later.

Definition at line 648 of file simple_planner.cpp.

650 {
651 if (tf_route.current_route_element_idx >= tf_route.route_elements.size()) {
652 const std::string msg = "Current route element index is outside the received local route";
653 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
654 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
655 return;
656 }
657
658 const bool destination_is_present = tf_route.destination_route_element_idx < tf_route.route_elements.size();
659 const size_t destination_or_route_end_idx =
660 destination_is_present ? tf_route.destination_route_element_idx : tf_route.route_elements.size();
661 if (destination_or_route_end_idx <= tf_route.current_route_element_idx) {
662 const std::string msg = "Received local route contains no remaining route elements";
663 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
664 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
665 return;
666 }
667
668 double t_total = 0.0;
669 const size_t remaining_route_elements = destination_or_route_end_idx - tf_route.current_route_element_idx;
670 RCLCPP_DEBUG(this->get_logger(), "Number of remaining route elements: %zu", remaining_route_elements);
671 health_.key_value_pairs.insert_or_assign("RemainingRouteElements", std::to_string(remaining_route_elements));
672 for (size_t j = tf_route.current_route_element_idx; j < destination_or_route_end_idx; ++j) {
673 const auto& route_element = tf_route.route_elements[j];
674 if (!route_element.is_enriched) {
675 RCLCPP_DEBUG(this->get_logger(), "Route element %zu is not enriched. Skipping.", j);
676 continue;
677 }
678
679 const auto& suggested_lane = route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
680 SimplePathPoint simple_path_point;
681 simple_path_point.position =
682 Eigen::Vector2d(suggested_lane.reference_pose.position.x, suggested_lane.reference_pose.position.y);
683 simple_path_point.s = route_element.s;
684 simple_path_point.v = v_ref_ < 0.0 ? suggested_lane.speed_limit / 3.6 : v_ref_;
685
686 if (!route_plan.path.points.empty()) {
687 const double v_average = (route_plan.path.points.back().v + simple_path_point.v) / 2.0;
688 const double dt = v_average != 0.0 ? (simple_path_point.s - route_plan.path.points.back().s) / v_average : 0.0;
689 if (dt <= 0.0) {
690 const std::string msg = "Negative time difference " + std::to_string(dt) +
691 " between points at s=" + std::to_string(route_plan.path.points.back().s) +
692 " and s=" + std::to_string(simple_path_point.s) + ". Could lead to unexpected behavior.";
693 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
694 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
695 }
696 t_total += dt;
697 }
698
699 if (j == tf_route.current_route_element_idx) {
700 route_plan.suggested_turn_signal = suggested_lane.suggested_turn_signal;
701 }
702
703 if (route_element.will_change_suggested_lane &&
704 !tryRegisterLaneChange(tf_route, j, lane_change_indices_map, route_plan.suggested_turn_signal)) {
705 break;
706 }
707
708 if (const auto stop_reason = updateForRegulatoryElements(tf_route, j, suggested_lane, simple_path_point, t_total,
709 route_plan.stop_at_end, route_plan.offset_to_stop_line)) {
710 route_plan.reason_to_stop = *stop_reason;
711 }
712
713 route_plan.path.points.push_back(simple_path_point);
714 if (destination_is_present && j + 1 == destination_or_route_end_idx) {
715 SimplePathPoint destination_point;
716 destination_point.position = Eigen::Vector2d(tf_route.destination.x, tf_route.destination.y);
717 destination_point.s = simple_path_point.s + (destination_point.position - simple_path_point.position).norm();
718 destination_point.v = simple_path_point.v;
719 route_plan.path.points.push_back(destination_point);
720 route_plan.reason_to_stop = "Reaching end of route";
721 route_plan.stop_at_end = true;
722 }
723 if (route_plan.stop_at_end || t_total >= 2.0 * trajectory_horizon_) {
724 break;
725 }
726 }
727
728 if (!destination_is_present && !route_plan.stop_at_end && t_total < 2.0 * trajectory_horizon_) {
729 const std::string msg = "Local enriched route ends before the planner horizon; trajectory will end at its last element";
730 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
731 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
732 }
733}
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 setHealth(const unsigned char status, const std::string &msg, const std::map< std::string, std::string > &key_value_pairs={})
Sets the health information.
Definition utils.hpp:180
struct simple_planner::SimplePlannerNode::DiagnosticStatus health_
std::optional< std::string > updateForRegulatoryElements(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 supported regulatory elements.
std::map< std::string, std::string > key_value_pairs

◆ 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_headerCurrent planning header propagated from the timer callback.
[in,out]base_path_pointsRoute path before time-based resampling.
[in,out]route_planMutable route planning result to be constrained by the grid map.

Definition at line 39 of file grid_map_handling.cpp.

41 {
42 if (!consider_grid_map_ || base_path_points.empty()) {
43 return;
44 }
45
46 std::optional<double> stop_s;
47 try {
48 stop_s = findFirstGridMapStopS(target_header, base_path_points);
49 } catch (const tf2::TransformException& ex) {
50 const std::string msg =
51 "Grid map transformation is not available: " + std::string(ex.what()) + ". Ignoring grid map for this planning cycle.";
52 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
53 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
54 return;
55 }
56
57 if (!stop_s.has_value()) {
58 return;
59 }
60
61 base_path_points = truncatePathAtS(base_path_points, *stop_s);
62 route_plan.stop_at_end = true;
63 route_plan.offset_to_stop_line = 0.0;
64 route_plan.reason_to_stop = "Grid map obstacle";
65
66 RCLCPP_INFO(this->get_logger(), "Applying stop for grid-map obstacle at last safe s=%f m", *stop_s);
67}
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.
static std::vector< SimplePathPoint > truncatePathAtS(const std::vector< SimplePathPoint > &path, double stop_s)
Returns a path ending exactly at the requested accumulated path coordinate.

◆ 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_signalSuggested signal derived from route semantics.

Definition at line 942 of file simple_planner.cpp.

942 {
943 if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE &&
944 left_turn_indicator_service_client_->service_is_ready() && right_turn_indicator_service_client_->service_is_ready() &&
945 hazard_lights_service_client_->service_is_ready()) {
946 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
947 request->data = false;
948 left_turn_indicator_service_client_->async_send_request(request);
949 right_turn_indicator_service_client_->async_send_request(request);
950 hazard_lights_service_client_->async_send_request(request);
951 } else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT &&
952 left_turn_indicator_service_client_->service_is_ready()) {
953 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
954 request->data = true;
955 left_turn_indicator_service_client_->async_send_request(request);
956 } else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT &&
957 right_turn_indicator_service_client_->service_is_ready()) {
958 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
959 request->data = true;
960 right_turn_indicator_service_client_->async_send_request(request);
961 } else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_HAZARD &&
962 hazard_lights_service_client_->service_is_ready()) {
963 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
964 request->data = true;
965 hazard_lights_service_client_->async_send_request(request);
966 } else {
967 std::string msg = "Cannot apply suggested turn signal " + std::to_string(suggested_turn_signal) +
968 " because the corresponding indicator service is not ready.";
969 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
970 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
971 }
972}
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr hazard_lights_service_client_
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr right_turn_indicator_service_client_
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr left_turn_indicator_service_client_

◆ 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_headerCurrent planning header propagated from the timer callback.
base_path_pointsRoute path before time-based resampling.
route_planMutable route plan to be constrained by dynamic objects.

Definition at line 227 of file object_handling.cpp.

229 {
230 if (!consider_objects_ || !object_list_init_ || route_plan.path.points.empty() || base_path_points.empty()) {
231 // Only forget the remembered speed cap if objects are disabled / never received; otherwise keep it
232 // across cycles in which the route path happens to be empty.
236 }
237 clearObjectInteractionMarkers(target_header);
238 return;
239 }
240
241 const rclcpp::Time stamp(target_header.stamp);
242 if (isMessageOutdated(object_list_.header, object_timeout_, stamp)) {
243 std::string msg =
244 "Object list is older than " + std::to_string(object_timeout_) + " seconds. Ignoring objects for this planning cycle.";
245 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
246 RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
247 resetObjectState(target_header);
248 return;
249 }
250 if (object_list_.objects.empty()) {
251 resetObjectState(target_header);
252 return;
253 }
254
255 perception_msgs::msg::ObjectList tf_object_list = object_list_;
256 if (requiresTransform(object_list_.header, target_header)) {
257 try {
258 tf_object_list = tf2_buffer_->transform(object_list_, target_header.frame_id, tf2_ros::fromMsg(target_header.stamp),
259 fixed_over_time_frame_id_, tf2::durationFromSec(1.0));
260 } catch (tf2::TransformException& ex) {
261 std::string msg =
262 "Object transformation is not available: " + std::string(ex.what()) + ". Ignoring objects for this planning cycle.";
263 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
264 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
265 resetObjectState(target_header);
266 return;
267 }
268 }
269
270 // Optionally ignore objects whose center is behind the ego vehicle (vehicle frame, +x ahead).
272 tf_object_list.objects.erase(std::remove_if(tf_object_list.objects.begin(), tf_object_list.objects.end(),
273 [](const auto& object) {
274 return perception_msgs::object_access::getCenterPosition(object.state).x < 0.0;
275 }),
276 tf_object_list.objects.end());
277 }
278
279 const std::vector<ObjectTrajectory> object_trajectories = buildObjectTrajectories(tf_object_list, stamp);
280 if (object_trajectories.empty()) {
281 resetObjectState(target_header);
282 return;
283 }
284
285 double initial_speed_cap = 0.0;
286 for (const auto& point : route_plan.path.points) initial_speed_cap = std::max(initial_speed_cap, point.v);
287 if (v_ref_ >= 0.0) initial_speed_cap = std::max(initial_speed_cap, v_ref_);
288
289 // Start the search at the remembered cap and allow a single release step upwards once enough
290 // conflict-free cycles have passed (hysteresis to avoid oscillating between speeds).
291 const rclcpp::Time iteration_begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
292 const double remembered_speed_cap = std::clamp(last_object_speed_cap_.value_or(initial_speed_cap), 0.0, initial_speed_cap);
293 double search_start_speed_cap = remembered_speed_cap;
294 bool attempted_release = false;
295 if (last_object_speed_cap_.has_value() && search_start_speed_cap < initial_speed_cap &&
297 search_start_speed_cap = std::min(search_start_speed_cap + object_velocity_release_step_, initial_speed_cap);
298 attempted_release = search_start_speed_cap > remembered_speed_cap + 1e-6;
299 }
300
301 // Step the speed cap down until the resampled path is conflict-free (or reaches zero).
302 double speed_cap = search_start_speed_cap;
303 size_t iteration_count = 0;
304 std::optional<ConflictSample> last_conflict;
305 while (speed_cap > 0.0) {
306 ++iteration_count;
307 std::vector<SimplePathPoint> candidate_path =
308 resamplePath(base_path_points, route_plan.stop_at_end, route_plan.offset_to_stop_line, &speed_cap);
309 const std::optional<ConflictSample> conflict = firstConflict(candidate_path, object_trajectories);
310 if (conflict.has_value()) {
311 last_conflict = conflict;
312 speed_cap = std::max(speed_cap - object_velocity_reduction_step_, 0.0);
313 continue;
314 }
315
316 const rclcpp::Time iteration_end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
318 speed_cap < initial_speed_cap ? last_conflict : std::optional<ConflictSample>{});
319 RCLCPP_DEBUG(this->get_logger(), "Object velocity iteration took %f ms (%zu iterations)",
320 (iteration_end - iteration_begin).seconds() * 1e3, iteration_count);
321
322 last_object_speed_cap_ = speed_cap < initial_speed_cap ? std::optional<double>(speed_cap) : std::nullopt;
323
324 // Hold a reduced cap for a few stable cycles before allowing the next release step upwards.
325 if (speed_cap >= initial_speed_cap - 1e-6 || speed_cap + 1e-6 < search_start_speed_cap || attempted_release) {
327 } else {
329 }
330
331 if (speed_cap <= object_standstill_speed_threshold_) {
332 health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
333 if (last_conflict.has_value()) {
334 health_.key_value_pairs.insert_or_assign("ReasonToStop", "Object conflict");
335 health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(last_conflict->object_id));
336 }
337 route_plan.path.points.clear();
338 const std::string msg = "Object avoidance speed cap " + std::to_string(speed_cap) +
339 " m/s is at or below standstill threshold " + std::to_string(object_standstill_speed_threshold_) +
340 " m/s. Publishing standstill.";
341 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
342 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
343 return;
344 }
345
346 route_plan.path.points = candidate_path;
347 if (speed_cap < initial_speed_cap) {
348 health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
349 if (last_conflict.has_value()) {
350 health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(last_conflict->object_id));
351 }
352 RCLCPP_DEBUG(this->get_logger(), "Reduced reference speed cap to %f m/s to avoid object conflict", speed_cap);
353 }
354 return;
355 }
356
357 // Speed cap reached zero while still conflicting: publish standstill.
358 route_plan.path.points = resamplePath(base_path_points, route_plan.stop_at_end, route_plan.offset_to_stop_line, &speed_cap);
359 const rclcpp::Time iteration_end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
360 RCLCPP_DEBUG(this->get_logger(), "Object velocity iteration took %f ms (%zu iterations)",
361 (iteration_end - iteration_begin).seconds() * 1e3, iteration_count);
362 last_object_speed_cap_ = speed_cap;
363 const std::optional<ConflictSample> standstill_conflict = firstConflict(route_plan.path.points, object_trajectories);
364 if (standstill_conflict.has_value()) {
365 health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
366 health_.key_value_pairs.insert_or_assign("ReasonToStop", "Object conflict");
367 health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(standstill_conflict->object_id));
368 last_conflict = standstill_conflict;
370 const std::string msg = "Reduced reference speed cap to 0.0 m/s; object " + std::to_string(standstill_conflict->object_id) +
371 " still conflicts at standstill. Publishing standstill.";
372 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
373 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
374 } else {
376 health_.key_value_pairs.insert_or_assign("ObjectSpeedCap", std::to_string(speed_cap));
377 if (last_conflict.has_value()) {
378 health_.key_value_pairs.insert_or_assign("ReasonToStop", "Object conflict");
379 health_.key_value_pairs.insert_or_assign("ObjectConflictId", std::to_string(last_conflict->object_id));
380 }
381 const std::string msg = "Reduced reference speed cap to 0.0 m/s to avoid object conflict. Publishing standstill.";
382 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
383 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
384 }
385 publishObjectInteractionMarkers(target_header, last_conflict);
386 route_plan.path.points.clear();
387}
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::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::unique_ptr< tf2_ros::Buffer > tf2_buffer_
perception_msgs::msg::ObjectList object_list_
std::optional< double > last_object_speed_cap_
void publishObjectInteractionMarkers(const std_msgs::msg::Header &target_header, const std::optional< ConflictSample > &conflict)
Publishes RViz markers for the current object interaction conflict.
void clearObjectInteractionMarkers(const std_msgs::msg::Header &target_header)
Deletes the currently published object interaction markers.
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.
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.
Definition utils.hpp:127
void resetObjectState(const std_msgs::msg::Header &target_header)
Resets the remembered object speed cap / hysteresis state and clears interaction markers.
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.

◆ 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_listObject list transformed into vehicle frame.
[in]stampCurrent 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.

97 {
98 std::vector<ObjectTrajectory> object_trajectories;
99 for (const auto& object : tf_object_list.objects) {
100 double object_width = 0.0;
101 double object_length = 0.0;
102 try {
103 object_width = perception_msgs::object_access::getWidth(object);
104 object_length = perception_msgs::object_access::getLength(object);
105 } catch (const std::exception&) {
106 object_width = 0.0;
107 object_length = 0.0;
108 }
109 if (!std::isfinite(object_width) || object_width < kMinObjectWidth) object_width = kMinObjectWidth;
110 if (!std::isfinite(object_length) || object_length < kMinObjectLength) object_length = kMinObjectLength;
111
112 auto add_static_object = [&]() {
113 ObjectTrajectory trajectory;
114 trajectory.id = object.id;
115 trajectory.is_static = true;
116 trajectory.samples.push_back(buildObjectSample(object.state, tf_object_list.header, stamp, object_length, object_width));
117 object_trajectories.push_back(trajectory);
118 };
119
120 // Select predictions strictly above the probability threshold, or fall back to the single most likely one.
121 std::vector<const perception_msgs::msg::ObjectStatePrediction*> selected_predictions;
122 for (const auto& prediction : object.state_predictions) {
123 if (prediction.probability > min_prediction_probability_) selected_predictions.push_back(&prediction);
124 }
125 if (selected_predictions.empty() && !object.state_predictions.empty()) {
126 selected_predictions.push_back(
127 &*std::max_element(object.state_predictions.begin(), object.state_predictions.end(),
128 [](const auto& lhs, const auto& rhs) { return lhs.probability < rhs.probability; }));
129 }
130
131 if (selected_predictions.empty()) {
132 add_static_object();
133 continue;
134 }
135
136 for (const auto* prediction : selected_predictions) {
137 if (prediction == nullptr || prediction->states.empty()) {
138 add_static_object();
139 continue;
140 }
141 ObjectTrajectory trajectory;
142 trajectory.id = object.id;
143 trajectory.is_static = false;
144 trajectory.samples.reserve(prediction->states.size());
145 for (const auto& state : prediction->states) {
146 trajectory.samples.push_back(buildObjectSample(state, tf_object_list.header, stamp, object_length, object_width));
147 }
148 object_trajectories.push_back(trajectory);
149 }
150 }
151 return object_trajectories;
152}
static constexpr double kMinObjectLength
static constexpr double kMinObjectWidth
TimedBox2D buildObjectSample(const perception_msgs::msg::ObjectState &state, const std_msgs::msg::Header &fallback_header, const rclcpp::Time &stamp, double length, double width)
Builds a timed bounding-box sample for a single object state.

◆ 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_headerVehicle-frame planning header for the generated route plan.
Returns
FollowRoutePlan Planned route path including stop and turn information.

Definition at line 613 of file simple_planner.cpp.

613 {
614 RCLCPP_DEBUG(this->get_logger(), "Default case: route is up to date, creating path from route.");
615
616 FollowRoutePlan route_plan;
617 route_plan.path.header = target_header;
618 route_planning_msgs::msg::Route tf_route = route_;
619 if (requiresTransform(route_.header, target_header)) {
620 try {
621 tf_route = tf2_buffer_->transform(route_, target_header.frame_id, tf2_ros::fromMsg(target_header.stamp),
622 fixed_over_time_frame_id_, tf2::durationFromSec(1.0));
623 } catch (tf2::TransformException& ex) {
624 std::string msg = "Route transformation is not available: " + std::string(ex.what()) + ".";
625 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
626 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
627 return route_plan;
628 }
629 }
630 route_plan.path.header = tf_route.header;
631
632 std::map<uint64_t, uint64_t> lane_change_indices_map;
633 appendRoutePoints(tf_route, route_plan, lane_change_indices_map);
634
635 std::vector<SimplePathPoint> merged_points = mergeLaneChangeSegments(tf_route, route_plan.path.points, lane_change_indices_map);
636 recalculateS(merged_points);
637 applyGridMapConstraints(target_header, merged_points, route_plan);
638 route_plan.path.points = resamplePath(merged_points, route_plan.stop_at_end, route_plan.offset_to_stop_line);
639 applyObjectConstraints(target_header, merged_points, route_plan);
641 applyIndicatorRequest(route_plan.suggested_turn_signal);
642 health_.key_value_pairs.insert_or_assign("SuggestedTurnSignal", turnSignalToString(route_plan.suggested_turn_signal));
643 }
644 if (route_plan.stop_at_end) health_.key_value_pairs.insert({"ReasonToStop", route_plan.reason_to_stop});
645 return route_plan;
646}
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.
static void recalculateS(std::vector< SimplePathPoint > &path)
Recomputes accumulated path distance from point positions.
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 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.
static std::string turnSignalToString(const uint8_t &turn_signal)
Converts a turn signal value to a string representation.
Definition utils.hpp:203
void applyIndicatorRequest(uint8_t suggested_turn_signal)
Requests the appropriate indicator state for the current route plan.
route_planning_msgs::msg::Route route_
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.

◆ 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_headerPlanning header for the generated safe-stop path.
Returns
SimplePath Safe-stop path in vehicle frame.

Definition at line 593 of file simple_planner.cpp.

593 {
594 double current_velocity = perception_msgs::object_access::getVelocityMagnitude(ego_data_);
595 safe_stop_distance_ = -0.5 * std::pow(current_velocity, 2) / a_max_decel_;
596
597 SimplePath safe_stop_path = transformPath(latest_path_, target_header);
598 trimPathBehindEgo(safe_stop_path);
599 if (safe_stop_path.points.empty()) {
600 RCLCPP_WARN(
601 this->get_logger(),
602 "No latest path available. Initialize safe stop along ego heading. Current velocity: %f m/s, safe stop distance: %f m",
603 current_velocity, *safe_stop_distance_);
605 } else {
606 RCLCPP_WARN(this->get_logger(), "Initialize safe stop along latest path. Current velocity: %f m/s, safe stop distance: %f m",
607 current_velocity, *safe_stop_distance_);
609 }
610 return latest_path_;
611}
static void trimPathBehindEgo(SimplePath &path)
Removes path points that lie behind the ego vehicle in vehicle frame.
std::optional< double > safe_stop_distance_
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.
Definition utils.hpp:140
perception_msgs::msg::EgoData ego_data_

◆ 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_headerOutput header for the generated trajectory.
Returns
trajectory_planning_msgs::msg::Trajectory Standstill trajectory message.

Definition at line 542 of file simple_planner.cpp.

543 {
544 int type_id = trajectory_planning_msgs::REFERENCE::TYPE_ID;
545 trajectory_planning_msgs::msg::Trajectory tra;
546 trajectory_planning_msgs::trajectory_access::initializeTrajectory(tra, type_id, 1);
547 tra.header = target_header;
548 trajectory_planning_msgs::trajectory_access::setStandstill(tra, true);
549 return tra;
550}

◆ 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]pathPath to be converted.
Returns
trajectory_planning_msgs::msg::Trajectory Generated trajectory message.

Definition at line 552 of file simple_planner.cpp.

552 {
553 SimplePath usable_path = path;
554 trimPathBehindEgo(usable_path);
555
556 if (usable_path.points.empty()) {
557 safe_stop_distance_.reset();
558 latest_path_.points.clear();
559 std::string msg = "No usable forward path remains. Publishing standstill trajectory.";
560 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
561 health_.key_value_pairs.insert_or_assign("PlannerState", plannerStateToString(PlannerState::Standstill));
562 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
563 return buildStandstillTrajectory(path.header);
564 }
565
566 latest_path_ = usable_path;
567 std::vector<SimplePathPoint> path_points = latest_path_.points;
568 const auto n_states_size = static_cast<size_t>(n_states_);
569 if (n_states_size < path_points.size()) {
570 path_points.resize(n_states_size);
571 }
572
573 int type_id = trajectory_planning_msgs::REFERENCE::TYPE_ID;
574 trajectory_planning_msgs::msg::Trajectory tra;
575 trajectory_planning_msgs::trajectory_access::initializeTrajectory(tra, type_id, n_states_);
576 tra.header = usable_path.header;
577
578 for (int i = 0; i < n_states_; i++) {
579 const size_t idx = static_cast<size_t>(i) < path_points.size() ? static_cast<size_t>(i) : path_points.size() - 1;
580 trajectory_planning_msgs::trajectory_access::setT(tra, dt_ * i, i);
581 trajectory_planning_msgs::trajectory_access::setX(tra, path_points[idx].position.x(), i);
582 trajectory_planning_msgs::trajectory_access::setY(tra, path_points[idx].position.y(), i);
583 trajectory_planning_msgs::trajectory_access::setV(tra, path_points[idx].v, i);
584 RCLCPP_DEBUG(this->get_logger(), "Debug: i: %d, t: %f, x: %f, y: %f, v: %f, s: %f", i, dt_ * i,
585 path_points[idx].position.x(), path_points[idx].position.y(), path_points[idx].v, path_points[idx].s);
586 }
587
588 trajectory_planning_msgs::trajectory_access::setStandstill(tra, false);
589 RCLCPP_DEBUG(this->get_logger(), "Standstill = %d", tra.standstill);
590 return tra;
591}
static std::string plannerStateToString(const PlannerState &state)
Converts a PlannerState enum to a string representation.
Definition utils.hpp:188
static trajectory_planning_msgs::msg::Trajectory buildStandstillTrajectory(const std_msgs::msg::Header &target_header)
Creates a standstill trajectory for the current planning cycle.
std::vector< SimplePathPoint > points

◆ 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_dataLatest ego state.
[in]safe_stop_distanceDistance available for the stop maneuver.
[in]target_headerTarget output header for the planning cycle.
Returns
Safe-stop path in the ego state frame.

Definition at line 995 of file simple_planner.cpp.

997 {
998 SimplePath safe_stop_path;
999 safe_stop_path.header = ego_data.header;
1000
1001 double v_ego = perception_msgs::object_access::getVelocityMagnitude(ego_data);
1002 geometry_msgs::msg::Point point = perception_msgs::object_access::getPosition(ego_data);
1003 double yaw = perception_msgs::object_access::getYaw(ego_data);
1004 Eigen::Vector2d start_position(point.x, point.y);
1005 Eigen::Vector2d heading(std::cos(yaw), std::sin(yaw));
1006
1007 RCLCPP_WARN(this->get_logger(), "Initializing minimal safe stop along ego heading. Frame: %s, yaw: %f rad",
1008 safe_stop_path.header.frame_id.c_str(), yaw);
1009 (void)target_header;
1010
1011 SimplePathPoint start_point(start_position, 0.0, v_ego);
1012 safe_stop_path.points.push_back(start_point);
1013 SimplePathPoint mid_point(start_position + heading * (safe_stop_distance / 2.0), safe_stop_distance / 2.0, v_ego);
1014 safe_stop_path.points.push_back(mid_point);
1015 SimplePathPoint end_point(start_position + heading * safe_stop_distance, safe_stop_distance, 0.0);
1016 safe_stop_path.points.push_back(end_point);
1017
1018 safe_stop_path.points = resamplePath(safe_stop_path.points, true, 0.0);
1019 return safe_stop_path;
1020}

◆ 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]pathRoute path to use as stop corridor.
[in]safe_stop_distanceDistance available for the stop maneuver.
Returns
Safe-stop path along the route.

Definition at line 980 of file simple_planner.cpp.

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

◆ clearObjectInteractionMarkers()

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

Deletes the currently published object interaction markers.

Parameters
[in]target_headerHeader used for the marker delete messages.

Definition at line 28 of file object_handling.cpp.

28 {
30 return;
31 }
32
33 visualization_msgs::msg::MarkerArray marker_array;
34 const std::array<std::string, 3> namespaces = {"object_interaction_ego_safety_box", "object_interaction_ego_box",
35 "object_interaction_object_box"};
36 int marker_id = 0;
37 for (const auto& marker_namespace : namespaces) {
38 visualization_msgs::msg::Marker marker;
39 marker.header = target_header;
40 marker.ns = marker_namespace;
41 marker.id = marker_id++;
42 marker.action = visualization_msgs::msg::Marker::DELETE;
43 marker_array.markers.push_back(marker);
44 }
45 object_interaction_marker_pub_->publish(marker_array);
46}
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr object_interaction_marker_pub_

◆ 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]statePlanner state to be executed.
[in]stampCurrent 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 regulatory elements 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 495 of file simple_planner.cpp.

495 {
496 rclcpp::Time begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
497
498 trajectory_planning_msgs::msg::Trajectory tra;
499 tra.header.stamp = stamp;
500 tra.header.frame_id = vehicle_frame_id_;
501
502 if (state != PlannerState::FollowRoute) {
503 resetObjectState(tra.header);
504 }
505
506 switch (state) {
508 tra = buildStandstillTrajectory(tra.header);
509 break;
511 if (!safe_stop_distance_.has_value()) {
513 } else {
514 RCLCPP_DEBUG(this->get_logger(), "Executing safe stop.");
516 }
517 break;
519 safe_stop_distance_.reset();
520 FollowRoutePlan route_plan = buildRoutePlan(tra.header);
521 tra = buildTrajectoryFromSimplePath(route_plan.path);
522 break;
523 }
525 throw std::runtime_error("createTrajectory called for non-publish state");
526 }
527
528 if (tra.header.frame_id != trajectory_frame_id_) {
529 try {
530 tra = tf2_buffer_->transform(tra, trajectory_frame_id_, tf2::durationFromSec(1.0));
531 } catch (tf2::TransformException& ex) {
532 throw std::runtime_error("Transformation into output frame '" + trajectory_frame_id_ +
533 "' is not available: " + std::string(ex.what()));
534 }
535 }
536
537 rclcpp::Time end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
538 RCLCPP_DEBUG(this->get_logger(), "Trajectory creation took %f ms", (end - begin).seconds() * 1e3);
539 return tra;
540}
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.
trajectory_planning_msgs::msg::Trajectory buildTrajectoryFromSimplePath(const SimplePath &path)
Builds a trajectory message from a simple path.
std_msgs::msg::Header header

◆ 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
TParameter value type.
Parameters
[in]nameParameter name.
[out]paramMember variable to store the parameter value.
[in]descriptionHuman-readable parameter description.
[in]add_to_auto_reconfigurable_paramsWhether parameter updates automatically update the member variable.
[in]is_requiredWhether the node should fail if the parameter is not set.
[in]read_onlyWhether the parameter is exposed as read-only.
[in]from_valueOptional lower bound for numeric parameters.
[in]to_valueOptional upper bound for numeric parameters.
[in]step_valueOptional step size for numeric parameters.
[in]additional_constraintsAdditional free-form constraint text for the parameter descriptor.

Definition at line 7 of file utils.hpp.

16 {
17 rcl_interfaces::msg::ParameterDescriptor param_desc;
18 param_desc.description = description;
19 param_desc.additional_constraints = additional_constraints;
20 param_desc.read_only = read_only;
21
22 auto type = rclcpp::ParameterValue(param).get_type();
23
24 if (from_value.has_value() && to_value.has_value()) {
25 if constexpr (std::is_integral_v<T>) {
26 rcl_interfaces::msg::IntegerRange range;
27 T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1);
28 range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
29 param_desc.integer_range = {range};
30 } else if constexpr (std::is_floating_point_v<T>) {
31 rcl_interfaces::msg::FloatingPointRange range;
32 T step = static_cast<T>(step_value.has_value() ? step_value.value() : 1.0);
33 range.set__from_value(static_cast<T>(from_value.value())).set__to_value(static_cast<T>(to_value.value())).set__step(step);
34 param_desc.floating_point_range = {range};
35 } else {
36 RCLCPP_WARN(this->get_logger(), "Parameter type of parameter '%s' does not support specifying a range", name.c_str());
37 }
38 }
39
40 this->declare_parameter(name, type, param_desc);
41
42 try {
43 param = this->get_parameter(name).get_value<T>();
44 std::stringstream ss;
45 ss << "Loaded parameter '" << name << "': ";
46 if constexpr (is_vector_v<T>) {
47 ss << "[";
48 for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
49 ss << "]";
50 } else {
51 ss << param;
52 }
53 RCLCPP_INFO_STREAM(this->get_logger(), ss.str());
54 } catch (rclcpp::exceptions::ParameterUninitializedException&) {
55 if (is_required) {
56 RCLCPP_FATAL_STREAM(this->get_logger(), "Missing required parameter '" << name << "', exiting");
57 exit(EXIT_FAILURE);
58 } else {
59 std::stringstream ss;
60 ss << "Missing parameter '" << name << "', using default value: ";
61 if constexpr (is_vector_v<T>) {
62 ss << "[";
63 for (const auto& element : param) ss << element << (&element != &param.back() ? ", " : "");
64 ss << "]";
65 } else {
66 ss << param;
67 }
68 RCLCPP_WARN_STREAM(this->get_logger(), ss.str());
69 this->set_parameters({rclcpp::Parameter(name, rclcpp::ParameterValue(param))});
70 }
71 }
72
73 if (add_to_auto_reconfigurable_params) {
74 std::function<void(const rclcpp::Parameter&)> setter = [&param](const rclcpp::Parameter& p) { param = p.get_value<T>(); };
75 auto_reconfigurable_params_.push_back(std::make_tuple(name, setter));
76 }
77}
std::vector< std::tuple< std::string, std::function< void(const rclcpp::Parameter &)> > > auto_reconfigurable_params_
Auto-reconfigurable parameters for dynamic reconfiguration.
constexpr bool is_vector_v

◆ 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]stampCurrent planning timestamp.
Returns
PlannerState The state to be handled for this cycle.

Definition at line 375 of file simple_planner.cpp.

375 {
376 // ego data missing -> no publish
377 if (!ego_data_init_) {
378 setHealth(diagnostic_msgs::msg::DiagnosticStatus::STALE, "No ego data received yet",
379 {{"PlannerState", plannerStateToString(PlannerState::NoPublish)}});
381 }
382
383 // ego data outdated -> no publish
384 if (isMessageOutdated(ego_data_.header, ego_data_timeout_, stamp)) {
385 ego_data_init_ = false;
386 std::string msg =
387 "EgoData is older than " + std::to_string(ego_data_timeout_) + " seconds. Skip publishing until fresh ego data arrives.";
388 RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
389 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
390 {{"PlannerState", plannerStateToString(PlannerState::NoPublish)}});
392 }
393
394 // no route received and no ongoing safe stop -> no publish
395 if (!route_init_ && !safe_stop_distance_.has_value()) {
396 setHealth(diagnostic_msgs::msg::DiagnosticStatus::STALE, "No route received and no ongoing safe stop",
397 {{"PlannerState", plannerStateToString(PlannerState::NoPublish)}});
399 }
400
401 // route outdated and vehicle in standstill -> standstill
402 if (route_init_ && isMessageOutdated(route_.header, route_timeout_, stamp)) {
403 if (perception_msgs::object_access::getStandstill(ego_data_)) {
404 route_init_ = false;
405 safe_stop_distance_.reset();
406 latest_path_.points.clear();
407 std::string msg = "Route is older than " + std::to_string(route_timeout_) +
408 " seconds and ego vehicle is stationary. Publishing standstill trajectory.";
409 RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
410 setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, msg,
413 }
414 route_init_ = false;
415
416 // route outdated and vehicle still moving -> safe stop
417 std::string msg = "Route is older than " + std::to_string(route_timeout_) +
418 " seconds but ego vehicle is still moving. Executing safe stop trajectory.";
419 RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
420 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
421 {{"PlannerState", plannerStateToString(PlannerState::SafeStop)}});
423 }
424
425 // route received, but empty -> standstill
426 if (route_init_ && route_.route_elements.empty()) {
427 route_init_ = false;
428 safe_stop_distance_.reset();
429 latest_path_.points.clear();
430 std::string msg = "Received route has no route elements. Publishing standstill trajectory.";
431 RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
432 setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, msg,
435 }
436
437 // no fresh route, but safe stop already started -> safe stop
438 if (!route_init_ && safe_stop_distance_.has_value()) {
439 if (perception_msgs::object_access::getStandstill(ego_data_)) {
440 safe_stop_distance_.reset();
441 latest_path_.points.clear();
442 std::string msg = "Safe stop finished. Ego vehicle is considered stationary. Publishing standstill trajectory.";
443 RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
444 setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, msg,
447 }
448 std::string msg = "No fresh route available, but safe stop already started. Executing safe stop trajectory.";
449 RCLCPP_DEBUG(this->get_logger(), "%s", msg.c_str());
450 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
451 {{"PlannerState", plannerStateToString(PlannerState::SafeStop)}});
453 }
454
455 // grid map enabled but missing, outdated, or invalid -> safe stop or standstill
456 if (consider_grid_map_ && !hasValidGridMap(stamp)) {
457 if (perception_msgs::object_access::getStandstill(ego_data_)) {
458 std::string msg =
459 "Grid map is missing, outdated, or invalid and ego vehicle is stationary. Publishing standstill trajectory.";
460 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
461 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
464 }
465
466 std::string msg = "Grid map is missing, outdated, or invalid. Executing safe stop trajectory.";
467 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
468 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg,
469 {{"PlannerState", plannerStateToString(PlannerState::SafeStop)}});
471 }
472
473 // fresh ego data and valid route available -> follow route
474 setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK, "Input information up to date. Following route.",
477}
bool hasValidGridMap(const rclcpp::Time &stamp) const
Checks whether a fresh, structurally valid grid map is currently available.

◆ 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
msgLatest ego data message.
[in]msgegoData

Definition at line 322 of file simple_planner.cpp.

322 {
323 if (ego_data_topic_diagnostic_ != nullptr) {
324 ego_data_topic_diagnostic_->tick(msg->header.stamp);
325 }
326 ego_data_ = *msg;
327
328 if (!ego_data_init_) {
329 ego_data_init_ = true;
330 RCLCPP_INFO(this->get_logger(), "Received first ego data message, initialized global variable");
331 }
332}
std::unique_ptr< diagnostic_updater::TopicDiagnostic > ego_data_topic_diagnostic_

◆ 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_headerCurrent planning header propagated from the timer callback.
[in]base_path_pointsRoute 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.

70 {
71 if (base_path_points.size() < 2) {
72 return std::nullopt;
73 }
74
75 const geometry_msgs::msg::TransformStamped tf =
76 tf2_buffer_->lookupTransform(grid_map_.header.frame_id, grid_map_.header.stamp, target_header.frame_id, target_header.stamp,
77 fixed_over_time_frame_id_, rclcpp::Duration::from_seconds(1.0));
78
79 std::vector<Eigen::Vector2d> grid_frame_path_points;
80 grid_frame_path_points.reserve(base_path_points.size());
81 for (const auto& point : base_path_points) {
82 geometry_msgs::msg::PointStamped point_msg;
83 geometry_msgs::msg::PointStamped transformed_point_msg;
84 point_msg.header = target_header;
85 point_msg.point.x = point.position.x();
86 point_msg.point.y = point.position.y();
87 point_msg.point.z = 0.0;
88 tf2::doTransform(point_msg, transformed_point_msg, tf);
89 grid_frame_path_points.emplace_back(transformed_point_msg.point.x, transformed_point_msg.point.y);
90 }
91
92 const double resolution = static_cast<double>(grid_map_.info.resolution);
93 const Eigen::Vector2d grid_origin(grid_map_.info.origin.position.x, grid_map_.info.origin.position.y);
94 const double grid_origin_yaw = tf2::getYaw(grid_map_.info.origin.orientation);
95 const double grid_length_x = static_cast<double>(grid_map_.info.width) * resolution;
96 const double grid_length_y = static_cast<double>(grid_map_.info.height) * resolution;
97 const Eigen::Vector2d ego_center_offset(ego_data_.state.reference_point.translation_to_geometric_center.x,
98 ego_data_.state.reference_point.translation_to_geometric_center.y);
99
100 // The target trajectory frame is ego-centered (normally base_link), so the ego reference point is its origin.
101 const Eigen::Vector2d ego_reference_position = Eigen::Vector2d::Zero();
102 double ego_path_s = 0.0;
103 double best_dist_sq = std::numeric_limits<double>::max();
104 for (size_t i = 0; i + 1 < base_path_points.size(); ++i) {
105 const Eigen::Vector2d segment = base_path_points[i + 1].position - base_path_points[i].position;
106 const double segment_length_sq = segment.squaredNorm();
107 if (segment_length_sq < 1e-9) {
108 continue;
109 }
110
111 const double alpha =
112 std::clamp((ego_reference_position - base_path_points[i].position).dot(segment) / segment_length_sq, 0.0, 1.0);
113 const Eigen::Vector2d projected = base_path_points[i].position + alpha * segment;
114 const double dist_sq = (projected - ego_reference_position).squaredNorm();
115 if (dist_sq >= best_dist_sq) {
116 continue;
117 }
118
119 best_dist_sq = dist_sq;
120 ego_path_s = base_path_points[i].s + alpha * (base_path_points[i + 1].s - base_path_points[i].s);
121 }
122
123 std::optional<double> last_safe_s;
124 for (size_t i = 0; i + 1 < base_path_points.size(); ++i) {
125 const Eigen::Vector2d segment = grid_frame_path_points[i + 1] - grid_frame_path_points[i];
126 const double segment_length = segment.norm();
127 if (segment_length <= 1e-3) {
128 continue;
129 }
130
131 const Eigen::Vector2d tangent = segment / segment_length;
132 const auto sample_count = static_cast<size_t>(std::ceil(segment_length / resolution));
133 for (size_t sample_idx = 0; sample_idx <= sample_count; ++sample_idx) {
134 const double sampled_distance = std::min(static_cast<double>(sample_idx) * resolution, segment_length);
135 const double alpha = sampled_distance / segment_length;
136 const double sample_s = base_path_points[i].s + alpha * (base_path_points[i + 1].s - base_path_points[i].s);
137 if (sample_s < ego_path_s) {
138 continue;
139 }
140
141 const Eigen::Vector2d reference_point = grid_frame_path_points[i] + alpha * segment;
142 const double yaw = wrap_angle_rad(std::atan2(tangent.y(), tangent.x()));
143 const Eigen::Vector2d ego_center = reference_point + rotate(ego_center_offset, yaw);
144 const OrientedBox2D ego_box = buildOrientedBox(ego_center, yaw, ego_data_.length, ego_data_.width);
145 const OrientedBox2D ego_safety_box =
147
148 const auto expanded_corners = getBoxCorners(ego_safety_box);
149 double min_local_x = std::numeric_limits<double>::max();
150 double max_local_x = std::numeric_limits<double>::lowest();
151 double min_local_y = std::numeric_limits<double>::max();
152 double max_local_y = std::numeric_limits<double>::lowest();
153 for (const auto& corner : expanded_corners) {
154 const Eigen::Vector2d local_corner = rotate(corner - grid_origin, -grid_origin_yaw);
155 min_local_x = std::min(min_local_x, local_corner.x());
156 max_local_x = std::max(max_local_x, local_corner.x());
157 min_local_y = std::min(min_local_y, local_corner.y());
158 max_local_y = std::max(max_local_y, local_corner.y());
159 }
160
161 const bool box_out_of_grid =
162 min_local_x < 0.0 || min_local_y < 0.0 || max_local_x >= grid_length_x || max_local_y >= grid_length_y;
163 if (box_out_of_grid && consider_out_of_grid_) {
164 return last_safe_s.value_or(ego_path_s);
165 }
166
167 const bool box_fully_outside =
168 max_local_x < 0.0 || max_local_y < 0.0 || min_local_x >= grid_length_x || min_local_y >= grid_length_y;
169 if (!box_fully_outside) {
170 const size_t min_cell_x = static_cast<size_t>(std::floor(std::max(min_local_x, 0.0) / resolution));
171 const size_t max_cell_x =
172 std::min(static_cast<size_t>(std::floor(max_local_x / resolution)), static_cast<size_t>(grid_map_.info.width) - 1);
173 const size_t min_cell_y = static_cast<size_t>(std::floor(std::max(min_local_y, 0.0) / resolution));
174 const size_t max_cell_y =
175 std::min(static_cast<size_t>(std::floor(max_local_y / resolution)), static_cast<size_t>(grid_map_.info.height) - 1);
176
177 for (size_t cell_y = min_cell_y; cell_y <= max_cell_y; ++cell_y) {
178 for (size_t cell_x = min_cell_x; cell_x <= max_cell_x; ++cell_x) {
179 const int8_t value = grid_map_.data[cell_y * grid_map_.info.width + cell_x];
180 if (value >= 0 && value < grid_occupied_threshold_) {
181 continue;
182 }
183
184 const Eigen::Vector2d cell_center_local((static_cast<double>(cell_x) + 0.5) * resolution,
185 (static_cast<double>(cell_y) + 0.5) * resolution);
186 const Eigen::Vector2d cell_center = grid_origin + rotate(cell_center_local, grid_origin_yaw);
187 const OrientedBox2D cell_box = buildOrientedBox(cell_center, grid_origin_yaw, resolution, resolution);
188 if (overlaps(ego_safety_box, cell_box)) {
189 return last_safe_s.value_or(ego_path_s);
190 }
191 }
192 }
193 }
194
195 last_safe_s = sample_s;
196 }
197 }
198
199 return std::nullopt;
200}
nav_msgs::msg::OccupancyGrid grid_map_
OrientedBox2D buildOrientedBox(const Eigen::Vector2d &center, double yaw, double length, double width)
Builds an oriented 2D bounding box from center pose and dimensions.
Eigen::Vector2d rotate(const Eigen::Vector2d &vec, double yaw)
Rotates a 2D vector by the given yaw angle.
double wrap_angle_rad(double angle_rad)
Wraps an angle to the range [-pi, pi].
bool overlaps(const OrientedBox2D &first_box, const OrientedBox2D &second_box)
Checks whether two oriented boxes overlap.
OrientedBox2D expandBoxWithSafetyMargins(const OrientedBox2D &box, double longitudinal_safety_distance, double lateral_safety_distance)
Expands a box by the given safety margins.
std::array< Eigen::Vector2d, 4 > getBoxCorners(const OrientedBox2D &box)
Returns the corners of an oriented box.

◆ 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_pathTime-equidistant ego path candidate.
[in]object_trajectoriesObject 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.

155 {
156 if (ego_path.empty()) return std::nullopt;
157
158 const auto& ego_offset_msg = ego_data_.state.reference_point.translation_to_geometric_center;
159 const Eigen::Vector2d ego_center_offset(ego_offset_msg.x, ego_offset_msg.y);
160
161 // Interpolates the ego bounding box along the (time-equidistant) path at relative time ego_t.
162 auto ego_box_at = [&](double ego_t) {
163 const size_t idx = std::min(static_cast<size_t>(std::floor(std::max(ego_t, 0.0) / dt_)), ego_path.size() - 1);
164 const size_t next_idx = std::min(idx + 1, ego_path.size() - 1);
165 const double segment_start_t = dt_ * static_cast<double>(idx);
166 const double alpha = next_idx > idx ? std::clamp((ego_t - segment_start_t) / dt_, 0.0, 1.0) : 0.0;
167 const Eigen::Vector2d position = ego_path[idx].position + alpha * (ego_path[next_idx].position - ego_path[idx].position);
168 Eigen::Vector2d heading = ego_path[next_idx].position - ego_path[idx].position;
169 if (heading.squaredNorm() < 1e-9 && idx > 0) heading = ego_path[idx].position - ego_path[idx - 1].position;
170 const double yaw = heading.squaredNorm() > 1e-9 ? wrap_angle_rad(std::atan2(heading.y(), heading.x())) : 0.0;
171 return buildOrientedBox(position + rotate(ego_center_offset, yaw), yaw, ego_data_.length, ego_data_.width);
172 };
173
174 const double check_dt = std::clamp(kObjectCollisionCheckDt, 0.01, dt_);
175 const size_t last_segment_idx = ego_path.size() > 1 ? ego_path.size() - 2 : 0;
176 for (size_t i = 0; i <= last_segment_idx; ++i) {
177 const double segment_start_t = dt_ * static_cast<double>(i);
178 if (segment_start_t > trajectory_horizon_) break;
179 const double segment_end_t = std::min(dt_ * static_cast<double>(i + 1), trajectory_horizon_);
180 const int steps = std::max(1, static_cast<int>(std::ceil((segment_end_t - segment_start_t) / check_dt)));
181
182 for (int step = 0; step <= steps; ++step) {
183 const double ego_t =
184 segment_start_t + (segment_end_t - segment_start_t) * static_cast<double>(step) / static_cast<double>(steps);
185 const OrientedBox2D ego_box = ego_box_at(ego_t);
186 const OrientedBox2D ego_safety_box =
188
189 for (const auto& object_trajectory : object_trajectories) {
190 if (object_trajectory.samples.empty()) continue;
191
192 if (object_trajectory.is_static) {
193 const auto& object_sample = object_trajectory.samples.front();
194 if (overlaps(ego_safety_box, object_sample.box)) {
195 return ConflictSample{object_trajectory.id, ego_box, object_sample.box};
196 }
197 continue;
198 }
199
200 for (size_t sample_idx = 0; sample_idx < object_trajectory.samples.size(); ++sample_idx) {
201 const auto& object_sample = object_trajectory.samples[sample_idx];
202 const TimedBox2D* next_sample =
203 sample_idx + 1 < object_trajectory.samples.size() ? &object_trajectory.samples[sample_idx + 1] : nullptr;
204
205 TimedBox2D timed_object_sample = object_sample;
206 if (next_sample != nullptr && ego_t >= object_sample.t - object_interaction_time_window_ &&
207 ego_t <= next_sample->t + object_interaction_time_window_) {
208 if (ego_t >= object_sample.t && ego_t <= next_sample->t) {
209 timed_object_sample = interpolateTimedBox(object_sample, *next_sample, ego_t);
210 } else if (std::abs(ego_t - next_sample->t) < std::abs(ego_t - object_sample.t)) {
211 timed_object_sample = *next_sample;
212 }
213 } else if (std::abs(ego_t - object_sample.t) > object_interaction_time_window_) {
214 continue;
215 }
216
217 if (overlaps(ego_safety_box, timed_object_sample.box)) {
218 return ConflictSample{object_trajectory.id, ego_box, timed_object_sample.box};
219 }
220 }
221 }
222 }
223 }
224 return std::nullopt;
225}
static constexpr double kObjectCollisionCheckDt
TimedBox2D interpolateTimedBox(const TimedBox2D &lhs, const TimedBox2D &rhs, double t)
Interpolates a timed box sample at the requested relative time.

◆ 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_idxRoute element index where the lane change starts.
[in]turn_idxRoute element index that indicates the lane-change turn.
[in]routeRoute in vehicle frame.
Returns
Lane-change path points, or an empty vector if the indices are invalid.

Definition at line 1022 of file simple_planner.cpp.

1024 {
1025 std::vector<SimplePathPoint> lane_change_path;
1026 const size_t end_idx = turn_idx + 1; // lane change should end at the next route element
1027 if (start_idx >= end_idx || end_idx >= route.route_elements.size()) {
1028 std::string msg = "Invalid lane change indices: start_idx=" + std::to_string(start_idx) +
1029 ", end_idx=" + std::to_string(end_idx) + ". Cannot generate lane change path.";
1030 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
1031 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
1032 return lane_change_path;
1033 }
1034
1035 int lane_change_direction = route_planning_msgs::route_access::getLaneChangeDirection(route.route_elements[turn_idx],
1036 route.route_elements[turn_idx + 1]);
1037
1038 // Interpolate between the two elements
1039 for (size_t i = start_idx; i <= end_idx; ++i) {
1040 if (i < route.current_route_element_idx) continue;
1041
1042 const auto& route_element = route.route_elements[i];
1043 const auto& suggested_lane = route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
1044 if (i > turn_idx) lane_change_direction = 0; // -> i == turn_idx + 1 == end_idx
1045 const auto& adjacent_lane = route_planning_msgs::route_access::getAdjacentLane(
1046 route_element, route.route_elements[i].suggested_lane_idx, lane_change_direction);
1047 Eigen::Vector2d suggested_lane_pos(suggested_lane.reference_pose.position.x, suggested_lane.reference_pose.position.y);
1048 Eigen::Vector2d adjacent_lane_pos(adjacent_lane.reference_pose.position.x, adjacent_lane.reference_pose.position.y);
1049 const double lane_change_progress = static_cast<double>(i - start_idx) / static_cast<double>(end_idx - start_idx);
1050 double alpha = 0.5 * (1.0 + std::cos(M_PI * lane_change_progress));
1051 Eigen::Vector2d interpolated_pos = alpha * suggested_lane_pos + (1.0 - alpha) * adjacent_lane_pos;
1052 lane_change_path.push_back(
1053 SimplePathPoint(interpolated_pos, route_element.s, suggested_lane.speed_limit / 3.6)); // convert km/h to m/s
1054 }
1055
1056 return lane_change_path;
1057}

◆ gridMapCallback()

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

Stores the latest occupancy grid map message.

Parameters
msgLatest occupancy grid map message.

Definition at line 363 of file simple_planner.cpp.

363 {
364 if (grid_map_topic_diagnostic_ != nullptr) {
365 grid_map_topic_diagnostic_->tick(msg->header.stamp);
366 }
367 grid_map_ = *msg;
368
369 if (!grid_map_init_) {
370 grid_map_init_ = true;
371 RCLCPP_INFO(this->get_logger(), "Received first grid map message, initialized global variable");
372 }
373}
std::unique_ptr< diagnostic_updater::TopicDiagnostic > grid_map_topic_diagnostic_

◆ 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]stampCurrent planning timestamp.
Returns
true if grid-map planning is possible.

Definition at line 20 of file grid_map_handling.cpp.

20 {
21 if (!grid_map_init_) {
22 return false;
23 }
24
25 const auto& origin = grid_map_.info.origin;
26 const double orientation_norm_sq = origin.orientation.x * origin.orientation.x + origin.orientation.y * origin.orientation.y +
27 origin.orientation.z * origin.orientation.z + origin.orientation.w * origin.orientation.w;
28 const bool has_valid_origin = std::isfinite(origin.position.x) && std::isfinite(origin.position.y) &&
29 std::isfinite(orientation_norm_sq) && orientation_norm_sq > 1e-12;
30 const bool has_valid_geometry = !grid_map_.header.frame_id.empty() && has_valid_origin &&
31 std::isfinite(grid_map_.info.resolution) && grid_map_.info.resolution > 0.0 &&
32 grid_map_.info.width > 0 && grid_map_.info.height > 0;
33 const size_t expected_cell_count = static_cast<size_t>(grid_map_.info.width) * static_cast<size_t>(grid_map_.info.height);
34 const bool has_complete_data = grid_map_.data.size() == expected_cell_count;
35
36 return has_valid_geometry && has_complete_data && !isMessageOutdated(grid_map_.header, grid_map_timeout_, stamp);
37}

◆ 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.

173 {
174 stat.summary(health_.status, health_.message);
175 for (const auto& [key, value] : health_.key_value_pairs) {
176 stat.add(key, value);
177 }
178}

◆ 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]headerHeader of the input message.
[in]timeoutTimeout in seconds. A value of -1.0 disables the timeout.
[in]stampCurrent planning timestamp.
Returns
true if the message is outdated.
false if the message is still valid.

Definition at line 479 of file simple_planner.cpp.

479 {
480 if (timeout == -1.0) {
481 return false;
482 }
483 return (stamp - rclcpp::Time(header.stamp)) > rclcpp::Duration::from_seconds(timeout);
484}

◆ 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_routeRoute transformed into vehicle frame and used to derive lane-change geometry.
[in]route_pointsBase route points before lane-change insertion.
[in]lane_change_indices_mapLane-change windows gathered during route processing.
Returns
std::vector<SimplePathPoint> Path points with lane changes inserted.

Definition at line 906 of file simple_planner.cpp.

909 {
910 size_t current = 0;
911 std::vector<SimplePathPoint> merged_points;
912 for (const auto& lane_change_indices : lane_change_indices_map) {
913 uint64_t lane_change_idx_route = lane_change_indices.first;
914 uint64_t start_idx_route = lane_change_indices.second;
915 size_t start_idx =
916 start_idx_route > tf_route.current_route_element_idx ? start_idx_route - tf_route.current_route_element_idx : 0;
917 size_t end_idx = lane_change_idx_route + 1 > tf_route.current_route_element_idx
918 ? lane_change_idx_route + 1 - tf_route.current_route_element_idx
919 : 0;
920
921 if (start_idx < current || current > route_points.size()) {
922 RCLCPP_WARN(this->get_logger(), "Skipping overlapping lane change window (%zu, %zu), current path index: %zu.", start_idx,
923 end_idx, current);
924 continue;
925 }
926
927 start_idx = std::min(start_idx, route_points.size());
928 const auto current_offset = static_cast<std::vector<SimplePathPoint>::difference_type>(current);
929 const auto start_offset = static_cast<std::vector<SimplePathPoint>::difference_type>(start_idx);
930 merged_points.insert(merged_points.end(), route_points.begin() + current_offset, route_points.begin() + start_offset);
931 std::vector<SimplePathPoint> lane_change_points = generateLaneChangePath(start_idx_route, lane_change_idx_route, tf_route);
932 merged_points.insert(merged_points.end(), lane_change_points.begin(), lane_change_points.end());
933 current = std::min(end_idx + 1, route_points.size());
934 }
935 if (current <= route_points.size()) {
936 const auto current_offset = static_cast<std::vector<SimplePathPoint>::difference_type>(current);
937 merged_points.insert(merged_points.end(), route_points.begin() + current_offset, route_points.end());
938 }
939 return merged_points;
940}
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.

◆ objectListCallback()

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

Stores the latest perceived object list including object predictions.

Parameters
msgLatest object list message.

Definition at line 334 of file simple_planner.cpp.

334 {
335 if (object_list_topic_diagnostic_ != nullptr) {
336 object_list_topic_diagnostic_->tick(msg->header.stamp);
337 }
338 object_list_ = *msg;
339
340 if (!object_list_init_) {
341 object_list_init_ = true;
342 RCLCPP_INFO(this->get_logger(), "Received first object list message, initialized global variable");
343 }
344}
std::unique_ptr< diagnostic_updater::TopicDiagnostic > object_list_topic_diagnostic_

◆ 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]parametersRequested parameter updates.
Returns
parameter change result

Definition at line 79 of file utils.hpp.

79 {
80 rcl_interfaces::msg::SetParametersResult result;
81 result.successful = false;
82
83 for (const auto& param : parameters) {
84 // check for specific parameter constraints
85 if (param.get_name() == "a_decel") {
86 if (param.as_double() >= 0.0) {
87 result.successful = false;
88 result.reason = "a_decel (" + std::to_string(param.as_double()) + ") must be < 0.0";
89 RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_decel': %s", result.reason.c_str());
90 break;
91 } else if (a_max_decel_ > param.as_double()) {
92 result.successful = false;
93 result.reason =
94 "a_max_decel (" + std::to_string(a_max_decel_) + ") must be <= a_decel (" + std::to_string(param.as_double()) + ")";
95 RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_decel': %s", result.reason.c_str());
96 break;
97 }
98 } else if (param.get_name() == "a_max_decel") {
99 if (param.as_double() >= 0.0) {
100 result.successful = false;
101 result.reason = "a_max_decel (" + std::to_string(param.as_double()) + ") must be < 0.0";
102 RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_max_decel': %s", result.reason.c_str());
103 break;
104 } else if (param.as_double() > a_decel_) {
105 result.successful = false;
106 result.reason =
107 "a_max_decel (" + std::to_string(param.as_double()) + ") must be <= a_decel (" + std::to_string(a_decel_) + ")";
108 RCLCPP_WARN(this->get_logger(), "Rejected parameter change for 'a_max_decel': %s", result.reason.c_str());
109 break;
110 }
111 }
112
113 // apply parameter change
114 for (auto& auto_reconfigurable_param : auto_reconfigurable_params_) {
115 if (param.get_name() == std::get<0>(auto_reconfigurable_param)) {
116 std::get<1>(auto_reconfigurable_param)(param);
117 RCLCPP_INFO(this->get_logger(), "Reconfigured parameter '%s'", param.get_name().c_str());
118 result.successful = true;
119 break;
120 }
121 }
122 }
123
124 return result;
125}

◆ 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.

188 {
189 switch (state) {
191 return "NoPublish";
193 return "Standstill";
195 return "SafeStop";
197 return "FollowRoute";
198 default:
199 return "Unknown";
200 }
201}

◆ 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_headerHeader used for the marker messages.
[in]conflictOptional conflict sample to visualize.

Definition at line 48 of file object_handling.cpp.

49 {
51 return;
52 }
53
54 if (!publish_object_interaction_markers_ || !conflict.has_value()) {
55 clearObjectInteractionMarkers(target_header);
56 return;
57 }
58
59 visualization_msgs::msg::MarkerArray marker_array;
60 auto make_box_marker = [&](const std::string& ns, int marker_id, const geometry_msgs::msg::Pose& pose, double length,
61 double width, double z, float r, float g, float b, float a) {
62 visualization_msgs::msg::Marker marker;
63 marker.header = target_header;
64 marker.ns = ns;
65 marker.id = marker_id;
66 marker.type = visualization_msgs::msg::Marker::CUBE;
67 marker.action = visualization_msgs::msg::Marker::ADD;
68 marker.pose = pose;
69 marker.pose.position.z = z;
70 marker.scale.x = length;
71 marker.scale.y = width;
72 marker.scale.z = 0.08;
73 marker.lifetime.sec = 0;
74 marker.lifetime.nanosec = 500000000;
75 marker.color.r = r;
76 marker.color.g = g;
77 marker.color.b = b;
78 marker.color.a = a;
79 marker_array.markers.push_back(marker);
80 };
81
82 const geometry_msgs::msg::Pose ego_pose = toPose(conflict->ego_box);
83 const OrientedBox2D ego_safety_box =
85 const double ego_length = 2.0 * conflict->ego_box.half_length;
86 const double ego_width = 2.0 * conflict->ego_box.half_width;
87 make_box_marker("object_interaction_ego_safety_box", 0, toPose(ego_safety_box), 2.0 * ego_safety_box.half_length,
88 2.0 * ego_safety_box.half_width, 0.12, 1.0F, 0.55F, 0.0F, 0.28F);
89 make_box_marker("object_interaction_ego_box", 1, ego_pose, ego_length, ego_width, 0.18, 1.0F, 0.0F, 0.0F, 0.55F);
90 make_box_marker("object_interaction_object_box", 2, toPose(conflict->object_box), 2.0 * conflict->object_box.half_length,
91 2.0 * conflict->object_box.half_width, 0.24, 0.0F, 0.45F, 1.0F, 0.55F);
92
93 object_interaction_marker_pub_->publish(marker_array);
94}
geometry_msgs::msg::Pose toPose(const OrientedBox2D &box)
Converts an oriented box center and heading to a ROS pose.

◆ publishTimerCallback()

void simple_planner::SimplePlannerNode::publishTimerCallback ( )
private

This callback is invoked every period seconds by the timer.

Definition at line 1227 of file simple_planner.cpp.

1227 {
1228 const rclcpp::Time stamp = now();
1229 PlannerState planner_state = determinePlannerState(stamp);
1230 if (planner_state == PlannerState::NoPublish) {
1231 std_msgs::msg::Header marker_header;
1232 marker_header.stamp = stamp;
1233 marker_header.frame_id = vehicle_frame_id_;
1234 clearObjectInteractionMarkers(marker_header);
1235 return;
1236 }
1237
1238 try {
1239 trajectory_planning_msgs::msg::Trajectory msg = createTrajectory(planner_state, stamp);
1240 diagnosed_publisher_->publish(msg);
1241 RCLCPP_DEBUG(this->get_logger(), "Published Trajectory!");
1242 } catch (const std::runtime_error& e) {
1243 std::string msg = "Error while creating trajectory: " + std::string(e.what());
1244 RCLCPP_ERROR(this->get_logger(), "%s", msg.c_str());
1245 setHealth(diagnostic_msgs::msg::DiagnosticStatus::ERROR, msg, {{"PlannerState", plannerStateToString(planner_state)}});
1246 }
1247}
std::unique_ptr< diagnostic_updater::DiagnosedPublisher< trajectory_planning_msgs::msg::Trajectory > > diagnosed_publisher_
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.

◆ recalculateS()

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

Recomputes accumulated path distance from point positions.

Parameters
[in,out]pathPath whose s values are updated in-place.

Definition at line 1100 of file simple_planner.cpp.

1100 {
1101 if (path.empty()) return;
1102
1103 path[0].s = 0.0;
1104 for (size_t i = 1; i < path.size(); ++i) {
1105 double ds = (path[i].position - path[i - 1].position).norm();
1106 path[i].s = path[i - 1].s + ds;
1107 }
1108}

◆ 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_headerHeader of the source data.
[in]target_headerRequested target frame and timestamp.
Returns
true if a spatial or temporal transform is required.

Definition at line 127 of file utils.hpp.

128 {
129 return source_header.frame_id != target_header.frame_id || source_header.stamp.sec != target_header.stamp.sec ||
130 source_header.stamp.nanosec != target_header.stamp.nanosec;
131}

◆ 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]pathInput path with accumulated s values.
[in]stop_at_endWhether the output should brake to a stop at the path end.
[in]offset_to_stop_lineAdditional offset used when computing the braking point.
[in]speed_capOptional upper velocity limit for all sampled points.
Returns
Time-resampled path points.

Definition at line 1110 of file simple_planner.cpp.

1113 {
1114 rclcpp::Time begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
1115 if (path.empty()) {
1116 std::string msg = "Route is empty. No resampling possible.";
1117 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
1118 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
1119 return path;
1120 }
1121
1122 std::vector<SimplePathPoint> resampled_path;
1123 double s = path[0].s;
1124
1125 tk::spline x_spline, y_spline;
1126 if (interpolation_type_ == InterpolationType::SPLINE && path.size() > 2) {
1127 std::vector<double> s_vector, x_vector, y_vector;
1128 for (size_t j = 0; j < path.size(); ++j) {
1129 s_vector.push_back(path[j].s);
1130 x_vector.push_back(path[j].position.x());
1131 y_vector.push_back(path[j].position.y());
1132 }
1133 x_spline.set_points(s_vector, x_vector);
1134 y_spline.set_points(s_vector, y_vector);
1135 }
1136
1137 while (s < path.back().s) {
1138 // find index of segment in route (not required if using linearInterpolation from utils)
1139 int idx = -1;
1140 for (size_t j = 0; j < path.size() - 1; ++j) {
1141 if (s >= path[j].s && s <= path[j + 1].s) {
1142 idx = static_cast<int>(j);
1143 break;
1144 }
1145 }
1146
1147 double v = v_ref_; // option 1: use predefined constant velocity
1148 if (v_ref_ < 0.0 && idx >= 0) { // option 2: use velocity from route if predefined velocity is negative
1149 v = path[idx].v + (path[idx + 1].v - path[idx].v) / (path[idx + 1].s - path[idx].s) * (s - path[idx].s);
1150 }
1151 if (speed_cap != nullptr) {
1152 v = std::min(v, std::max(*speed_cap, 0.0));
1153 }
1154
1155 double distance_to_stop = -0.5 * std::pow(v, 2) / a_decel_ + offset_to_stop_line;
1156 distance_to_stop = std::max(distance_to_stop, 0.0);
1157 double brake_point = path.back().s - distance_to_stop;
1158 double ds = v * dt_; // case 1: constant velocity
1159 if (s + ds > brake_point && stop_at_end) {
1160 if (s < brake_point) { // special case: braking point is between two states
1161 double ds_1 = brake_point - s; // distance with constant velocity to braking point
1162 double dt_1 = ds_1 / v; // time with constant velocity to braking point
1163 double dt_2 = dt_ - dt_1; // remaining time with deceleration
1164 double ds_2 = std::max(0.5 * a_decel_ * std::pow(dt_2, 2) + v * dt_2, 0.0);
1165 ds = ds_1 + ds_2;
1166 } else {
1167 v = std::sqrt(std::max(std::pow(v, 2) + 2 * a_decel_ * (s - brake_point), 0.0)); // case 2: deceleration (v(s))
1168 ds = 0.5 * a_decel_ * std::pow(dt_, 2) + v * dt_; // case 2: deceleration
1169 if (ds < 0.0) {
1170 v = 0.0;
1171 ds = 0.0;
1172 }
1173 }
1174 }
1175
1176 // interpolate point at s
1177 SimplePathPoint simple_path_point;
1178 if (path.size() == 1) {
1179 simple_path_point.position = path[0].position;
1180 } else if (interpolation_type_ == InterpolationType::SPLINE && path.size() > 2) { // spline interpolation
1181 simple_path_point.position.x() = x_spline(s);
1182 simple_path_point.position.y() = y_spline(s);
1183 } else if (
1184 (interpolation_type_ == InterpolationType::SPLINE && path.size() <= 2) ||
1187 LINEAR) { // linear interpolation // TODO: could be improved by using our linearInterpolation function -> no need for idx anymore
1188 simple_path_point.position.x() = path[idx].position.x() + (path[idx + 1].position.x() - path[idx].position.x()) /
1189 (path[idx + 1].s - path[idx].s) * (s - path[idx].s);
1190 simple_path_point.position.y() = path[idx].position.y() + (path[idx + 1].position.y() - path[idx].position.y()) /
1191 (path[idx + 1].s - path[idx].s) * (s - path[idx].s);
1192 } else { // unsupported interpolation type
1193 RCLCPP_ERROR(this->get_logger(), "Unsupported interpolation type value %d", interpolation_type_);
1194 throw std::runtime_error("Unsupported interpolation type value");
1195 }
1196 simple_path_point.s = s;
1197 simple_path_point.v = v;
1198 resampled_path.push_back(simple_path_point);
1199
1200 // increment s and v for next iteration
1201 if (v <= 1e-6 && ds <= 1e-6) break;
1202 s = s + ds;
1203 if (s == path.back().s && v == 0.0) break; // stop at end of route
1204 }
1205
1206 rclcpp::Time end = rclcpp::Clock(RCL_SYSTEM_TIME).now();
1207 RCLCPP_DEBUG(this->get_logger(), "Resampling route took %f ms", (end - begin).seconds() * 1e3);
1208
1209 if (stop_at_end) {
1210 SimplePathPoint stop_point = path.back();
1211 if (!resampled_path.empty() && (offset_to_stop_line > 0.0 || (speed_cap != nullptr && *speed_cap <= 1e-6))) {
1212 stop_point = resampled_path.back();
1213 }
1214 stop_point.v = 0.0;
1215 if (resampled_path.empty() || resampled_path.back().s < stop_point.s || resampled_path.back().v != 0.0) {
1216 resampled_path.push_back(stop_point);
1217 }
1218 }
1219
1220 return resampled_path;
1221}

◆ 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_headerHeader used for the cleared marker messages.

Definition at line 22 of file object_handling.cpp.

◆ 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
msgLatest route message.
[in]msgroute

Definition at line 351 of file simple_planner.cpp.

351 {
352 if (route_topic_diagnostic_ != nullptr) {
353 route_topic_diagnostic_->tick(msg->header.stamp);
354 }
355 route_ = *msg;
356 if (!route_init_) {
357 RCLCPP_INFO(this->get_logger(), "Received new route message, initialized global variable");
358 route_init_ = true;
359 safe_stop_distance_.reset();
360 }
361}
std::unique_ptr< diagnostic_updater::TopicDiagnostic > route_topic_diagnostic_

◆ 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.

182 {
183 health_.status = status;
184 health_.message = msg;
185 health_.key_value_pairs = key_value_pairs;
186}

◆ 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 200 of file simple_planner.cpp.

200 {
201 tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
202 tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
203
204 // calculate dt
206
207 // create a publisher for publishing output trajectory
208 pub_ = this->create_publisher<trajectory_planning_msgs::msg::Trajectory>("~/trajectory", 10);
209 RCLCPP_INFO(this->get_logger(), "Publishing to '%s'", pub_->get_topic_name());
211 this->create_publisher<visualization_msgs::msg::MarkerArray>("~/viz/object_interaction_markers", 10);
212 RCLCPP_INFO(this->get_logger(), "Publishing object interaction markers to '%s'",
213 object_interaction_marker_pub_->get_topic_name());
214
215 // create a timer for repeatedly invoking a callback to publish messages
216 publish_timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / freq_),
218 RCLCPP_INFO(this->get_logger(), "Publishing trajectory at '%f' hz", freq_);
219
220 // create subscriber for egoData
221 sub_egoData_ = this->create_subscription<perception_msgs::msg::EgoData>(
222 "~/ego_data", 10, std::bind(&SimplePlannerNode::egoDataCallback, this, std::placeholders::_1));
223 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_egoData_->get_topic_name());
224
225 sub_object_list_ = this->create_subscription<perception_msgs::msg::ObjectList>(
226 "~/object_list", 10, std::bind(&SimplePlannerNode::objectListCallback, this, std::placeholders::_1));
227 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_object_list_->get_topic_name());
228
229 // create subscriber for route
230 sub_route_ = this->create_subscription<route_planning_msgs::msg::Route>(
231 "~/route", 10, std::bind(&SimplePlannerNode::routeCallback, this, std::placeholders::_1));
232 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_route_->get_topic_name());
233
234 sub_grid_map_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
235 "~/grid_map", 10, std::bind(&SimplePlannerNode::gridMapCallback, this, std::placeholders::_1));
236 RCLCPP_INFO(this->get_logger(), "Subscribed to '%s'", sub_grid_map_->get_topic_name());
237
238 // create service clients for turn indicators and hazard lights
239 left_turn_indicator_service_client_ = this->create_client<std_srvs::srv::SetBool>("~/enable_left_turn_indicator");
240 RCLCPP_INFO(this->get_logger(), "Prepared service client for '%s'", left_turn_indicator_service_client_->get_service_name());
241 right_turn_indicator_service_client_ = this->create_client<std_srvs::srv::SetBool>("~/enable_right_turn_indicator");
242 RCLCPP_INFO(this->get_logger(), "Prepared service client for '%s'", right_turn_indicator_service_client_->get_service_name());
243 hazard_lights_service_client_ = this->create_client<std_srvs::srv::SetBool>("~/enable_hazard_lights");
244 RCLCPP_INFO(this->get_logger(), "Prepared service client for '%s'", hazard_lights_service_client_->get_service_name());
245
246 // create a callback for dynamic parameter configuration
248 this->add_on_set_parameters_callback(std::bind(&SimplePlannerNode::parametersCallback, this, std::placeholders::_1));
249
250 // Annotate message links for tracing: Trajectory is published periodically based on planning input subscriptions.
251 std::vector<const void*> link_subs = {static_cast<const void*>(sub_egoData_->get_subscription_handle().get()),
252 static_cast<const void*>(sub_object_list_->get_subscription_handle().get()),
253 static_cast<const void*>(sub_route_->get_subscription_handle().get()),
254 static_cast<const void*>(sub_grid_map_->get_subscription_handle().get())};
255 std::vector<const void*> link_pubs = {static_cast<const void*>(pub_->get_publisher_handle().get())};
256 TRACETOOLS_TRACEPOINT(message_link_periodic_async, link_subs.data(), link_subs.size(), link_pubs.data(), link_pubs.size());
257
258 // setup diagnostic updater
259 diagnostic_updater_.setHardwareID(this->get_name());
261
262 const int ego_data_topic_diagnostic_frequency_window_size =
263 std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * ego_data_topic_diagnostic_config_.min_frequency));
264 ego_data_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
265 "~/ego_data", diagnostic_updater_,
266 diagnostic_updater::FrequencyStatusParam(&ego_data_topic_diagnostic_config_.min_frequency,
268 ego_data_topic_diagnostic_frequency_window_size),
269 diagnostic_updater::TimeStampStatusParam(ego_data_topic_diagnostic_config_.min_acceptable_timestamp_delta,
271
272 const int route_topic_diagnostic_frequency_window_size =
273 std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * route_topic_diagnostic_config_.min_frequency));
274 route_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
275 "~/route", diagnostic_updater_,
276 diagnostic_updater::FrequencyStatusParam(&route_topic_diagnostic_config_.min_frequency,
278 route_topic_diagnostic_frequency_window_size),
279 diagnostic_updater::TimeStampStatusParam(route_topic_diagnostic_config_.min_acceptable_timestamp_delta,
281
282 if (consider_objects_) {
283 const int object_list_topic_diagnostic_frequency_window_size =
284 std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * object_list_topic_diagnostic_config_.min_frequency));
285 object_list_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
286 "~/object_list", diagnostic_updater_,
287 diagnostic_updater::FrequencyStatusParam(&object_list_topic_diagnostic_config_.min_frequency,
289 object_list_topic_diagnostic_frequency_window_size),
290 diagnostic_updater::TimeStampStatusParam(object_list_topic_diagnostic_config_.min_acceptable_timestamp_delta,
292 }
293
294 if (consider_grid_map_) {
295 const int grid_map_topic_diagnostic_frequency_window_size =
296 std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * grid_map_topic_diagnostic_config_.min_frequency));
297 grid_map_topic_diagnostic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
298 "~/grid_map", diagnostic_updater_,
299 diagnostic_updater::FrequencyStatusParam(&grid_map_topic_diagnostic_config_.min_frequency,
301 grid_map_topic_diagnostic_frequency_window_size),
302 diagnostic_updater::TimeStampStatusParam(grid_map_topic_diagnostic_config_.min_acceptable_timestamp_delta,
304 }
305
306 const int diagnosed_publisher_frequency_window_size =
307 std::ceil(5 / (diagnostic_updater_.getPeriod().seconds() * diagnosed_publisher_config_.min_frequency));
308 diagnosed_publisher_ = std::make_unique<diagnostic_updater::DiagnosedPublisher<trajectory_planning_msgs::msg::Trajectory>>(
310 diagnostic_updater::FrequencyStatusParam(&diagnosed_publisher_config_.min_frequency,
312 diagnosed_publisher_frequency_window_size),
313 diagnostic_updater::TimeStampStatusParam(diagnosed_publisher_config_.min_acceptable_timestamp_delta,
315}
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr pub_
diagnostic_updater::Updater diagnostic_updater_
Diagnostic updater.
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr sub_object_list_
void publishTimerCallback()
This callback is invoked every period seconds by the timer.
rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector< rclcpp::Parameter > &parameters)
Handles reconfiguration when a parameter value is changed.
Definition utils.hpp:79
void health(diagnostic_updater::DiagnosticStatusWrapper &stat)
Function called by diagnostic updater to populate diagnostics status.
Definition utils.hpp:173
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr sub_egoData_
void objectListCallback(const perception_msgs::msg::ObjectList::UniquePtr msg)
Stores the latest perceived object list including object predictions.
rclcpp::TimerBase::SharedPtr publish_timer_
OnSetParametersCallbackHandle::SharedPtr parameters_callback_
Callback handle for dynamic parameter reconfiguration.
void routeCallback(const route_planning_msgs::msg::Route::UniquePtr msg)
Stores the latest route message and extracts the route path.
rclcpp::Subscription< route_planning_msgs::msg::Route >::SharedPtr sub_route_
void egoDataCallback(const perception_msgs::msg::EgoData::UniquePtr msg)
Stores the latest ego data message.
rclcpp::Subscription< nav_msgs::msg::OccupancyGrid >::SharedPtr sub_grid_map_
void gridMapCallback(const nav_msgs::msg::OccupancyGrid::UniquePtr msg)
Stores the latest occupancy grid map message.

◆ 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]pathPath to transform.
[in]target_headerTarget 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.

140 {
141 SimplePath transformed_path;
142 transformed_path.header = target_header;
143
144 if (path.points.empty()) {
145 return transformed_path;
146 }
147 if (!requiresTransform(path.header, target_header)) {
148 return path;
149 }
150
151 geometry_msgs::msg::TransformStamped tf;
152 try {
153 tf = tf2_buffer_->lookupTransform(target_header.frame_id, target_header.stamp, path.header.frame_id, path.header.stamp,
154 fixed_over_time_frame_id_, rclcpp::Duration::from_seconds(1.0));
155 for (const auto& point : path.points) {
156 geometry_msgs::msg::PointStamped point_msg, transformed_point_msg;
157 point_msg.header = path.header;
158 point_msg.point.x = point.position.x();
159 point_msg.point.y = point.position.y();
160 point_msg.point.z = 0.0;
161 tf2::doTransform(point_msg, transformed_point_msg, tf);
162 SimplePathPoint transformed_point = point;
163 transformed_point.position = Eigen::Vector2d(transformed_point_msg.point.x, transformed_point_msg.point.y);
164 transformed_path.points.push_back(transformed_point);
165 }
166 } catch (tf2::TransformException& ex) {
167 RCLCPP_WARN(this->get_logger(), "Could not transform path: %s. Returning an empty path.", ex.what());
168 }
169
170 return transformed_path;
171}

◆ trimPathBehindEgo()

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

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

Parameters
[in,out]pathPath to be trimmed in-place.

Definition at line 974 of file simple_planner.cpp.

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

◆ 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]pathInput path with accumulated s values.
[in]stop_sAccumulated path coordinate of the new endpoint.
Returns
Truncated path including an endpoint at stop_s.

Definition at line 1059 of file simple_planner.cpp.

1059 {
1060 if (path.empty()) {
1061 return path;
1062 }
1063
1064 if (stop_s <= path.front().s) {
1065 SimplePathPoint stop_point = path.front();
1066 stop_point.s = stop_s;
1067 return {stop_point};
1068 }
1069
1070 if (stop_s >= path.back().s) {
1071 return path;
1072 }
1073
1074 const auto stop_it =
1075 std::lower_bound(path.begin(), path.end(), stop_s, [](const auto& point, double s) { return point.s < s; });
1076 if (stop_it == path.begin()) {
1077 return {*stop_it};
1078 }
1079 if (stop_it == path.end()) {
1080 return path;
1081 }
1082
1083 std::vector<SimplePathPoint> truncated_path(path.begin(), stop_it);
1084 if (std::abs(stop_it->s - stop_s) <= 1e-6) {
1085 truncated_path.push_back(*stop_it);
1086 return truncated_path;
1087 }
1088
1089 const auto& previous = *(stop_it - 1);
1090 const double segment_ds = stop_it->s - previous.s;
1091 const double alpha = segment_ds > 1e-9 ? std::clamp((stop_s - previous.s) / segment_ds, 0.0, 1.0) : 0.0;
1092 SimplePathPoint stop_point;
1093 stop_point.position = previous.position + alpha * (stop_it->position - previous.position);
1094 stop_point.s = stop_s;
1095 stop_point.v = previous.v + alpha * (stop_it->v - previous.v);
1096 truncated_path.push_back(stop_point);
1097 return truncated_path;
1098}

◆ 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_routeRoute transformed into vehicle frame.
[in]route_element_idxIndex of the current route element.
[out]lane_change_indices_mapOutput map of lane-change route indices.
[in,out]suggested_turn_signalTurn 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 735 of file simple_planner.cpp.

738 {
739 if (route_element_idx + 1 >= tf_route.route_elements.size()) {
740 std::string msg = "Route element " + std::to_string(route_element_idx) + " is the last element. Cannot change lane.";
741 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
742 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
743 return false;
744 }
745
746 const size_t i_end = route_element_idx + 1;
747 const auto& route_element = tf_route.route_elements[route_element_idx];
748 size_t current_lane_idx = route_element.suggested_lane_idx;
749 int lane_change_direction = 0;
750 try {
751 lane_change_direction =
752 route_planning_msgs::route_access::getLaneChangeDirection(route_element, tf_route.route_elements[route_element_idx + 1]);
753 } catch (const std::exception& ex) {
754 RCLCPP_WARN(this->get_logger(), "Could not determine lane change direction at route element %zu: %s", route_element_idx,
755 ex.what());
756 return false;
757 }
758 if (lane_change_direction < 0) {
759 suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT;
760 } else if (lane_change_direction > 0) {
761 suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT;
762 }
763
764 double ego_velocity = perception_msgs::object_access::getVelocityMagnitude(ego_data_);
765 double lane_change_distance =
767 RCLCPP_DEBUG(this->get_logger(), "Lane change direction: %d, lane change distance: %f", lane_change_direction,
768 lane_change_distance);
769 if (lane_change_direction == 0) {
770 RCLCPP_WARN(this->get_logger(),
771 "Route element %zu is marked as lane change, but suggested lane does not change. Ignoring lane change marker.",
772 route_element_idx);
773 return true;
774 }
775
776 double ds = 0.0;
777 size_t i_start = route_element_idx;
778 while (ds < lane_change_distance && i_start > 0) {
779 if (auto result = route_planning_msgs::route_access::getPrecedingLaneElementIdx(current_lane_idx,
780 tf_route.route_elements[i_start - 1])) {
781 current_lane_idx = *result;
782 } else {
783 std::string msg =
784 "No preceding lane element found for route element " + std::to_string(i_start) + ". Cannot extend lane change.";
785 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
786 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
787 break;
788 }
789 if (i_start >= 2 && tf_route.route_elements[i_start - 2].will_change_suggested_lane) {
790 std::string msg = "Found previous lane change in route element " + std::to_string(i_start - 2) +
791 ". Could not extend lane change over this element.";
792 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
793 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
794 break;
795 }
796 if (!route_planning_msgs::route_access::hasAdjacentLane(tf_route.route_elements[i_start - 1], current_lane_idx,
797 lane_change_direction)) {
798 std::string msg = "No adjacent lane found for route element " + std::to_string(i_start - 1) + ".";
799 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
800 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
801 break;
802 }
803 ds += std::abs(tf_route.route_elements[i_start].s - tf_route.route_elements[i_start - 1].s);
804 i_start--;
805 }
806
807 if (!tf_route.route_elements[i_start].is_enriched || !tf_route.route_elements[i_end].is_enriched) {
808 std::string msg =
809 "Not enough enriched route elements (" + std::to_string(i_start) + ", " + std::to_string(i_end) + ") for lane change.";
810 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
811 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
812 return false;
813 }
814
815 lane_change_indices_map[route_element_idx] = i_start;
816 return true;
817}

◆ 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.

203 {
204 switch (turn_signal) {
205 case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE:
206 return "None";
207 case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT:
208 return "Left";
209 case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT:
210 return "Right";
211 case route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_HAZARD:
212 return "Hazard";
213 default:
214 return "Unknown";
215 }
216}

◆ updateForRegulatoryElements()

std::optional< std::string > simple_planner::SimplePlannerNode::updateForRegulatoryElements ( 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 supported regulatory elements.

Parameters
[in]tf_routeRoute transformed into vehicle frame.
[in]route_element_idxIndex of the current route element.
[in]suggested_laneSuggested lane element of the current route element.
[in]simple_path_pointCurrent path point candidate.
[in]t_totalAccumulated travel time along the partial path.
[in,out]stop_at_endWhether the path should stop at its current end.
[in,out]offset_to_stop_lineEffective offset used for braking towards the stop line.
Returns
Stop reason when a regulatory element requires a valid stop, otherwise std::nullopt.

Definition at line 819 of file simple_planner.cpp.

826 {
827 const auto& route_element = tf_route.route_elements[route_element_idx];
828 const auto& reg_elems =
829 route_planning_msgs::route_access::getRegulatoryElementsOfLaneElement(suggested_lane, route_element.regulatory_elements);
830 for (size_t k = 0; k < reg_elems.size(); ++k) {
831 std::string regelem_type;
832 switch (reg_elems[k].type) {
833 case route_planning_msgs::msg::RegulatoryElement::TYPE_TRAFFIC_LIGHT:
834 if (!consider_traffic_lights_) continue;
835 regelem_type = "Traffic light";
836 break;
837 case route_planning_msgs::msg::RegulatoryElement::TYPE_STOP:
838 if (!consider_stop_signs_) continue;
839 regelem_type = "Stop sign";
840 break;
841 case route_planning_msgs::msg::RegulatoryElement::TYPE_YIELD:
842 if (!consider_yield_signs_) continue;
843 regelem_type = "Yield sign";
844 break;
845 default:
846 continue;
847 }
848
849 bool requires_stop = reg_elems[k].meta_value != route_planning_msgs::msg::RegulatoryElement::META_VALUE_MOVEMENT_ALLOWED;
850 const bool has_known_movement_state =
851 reg_elems[k].meta_value == route_planning_msgs::msg::RegulatoryElement::META_VALUE_MOVEMENT_ALLOWED ||
852 reg_elems[k].meta_value == route_planning_msgs::msg::RegulatoryElement::META_VALUE_MOVEMENT_RESTRICTED;
853 const bool use_future_state = reg_elems[k].has_validity_stamp && consider_future_states_ && has_known_movement_state;
854 if (!requires_stop && !use_future_state) continue;
855
856 offset_to_stop_line =
857 offset_to_stop_line_ + ego_data_.length / 2.0 + ego_data_.state.reference_point.translation_to_geometric_center.x;
858 double dt_offset_to_stop_line = 0.0;
859 if (simple_path_point.v != 0.0) {
860 dt_offset_to_stop_line = offset_to_stop_line / simple_path_point.v;
861 }
862 if (dt_offset_to_stop_line <= 0.0) {
863 std::string msg = "Negative time difference 'dt_offset_to_stop_line' (" + std::to_string(dt_offset_to_stop_line) +
864 " s) for " + regelem_type + " at route element " + std::to_string(route_element_idx) +
865 ". Could lead to unexpected behavior.";
866 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
867 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
868 }
869
870 if (use_future_state) {
871 double validity_duration =
872 rclcpp::Time(reg_elems[k].validity_stamp).seconds() - rclcpp::Time(route_.header.stamp).seconds();
873 if (validity_duration < (t_total - dt_offset_to_stop_line)) {
874 requires_stop = reg_elems[k].meta_value == route_planning_msgs::msg::RegulatoryElement::META_VALUE_MOVEMENT_ALLOWED;
875 }
876 }
877 if (!requires_stop) continue;
878
879 double distance_to_stop_point = tf_route.route_elements[route_element_idx].s -
880 tf_route.route_elements[tf_route.current_route_element_idx].s - offset_to_stop_line;
881 double v_ego = perception_msgs::object_access::getVelocityMagnitude(ego_data_);
882 double min_distance_to_stop = -0.5 * std::pow(v_ego, 2) / a_max_decel_;
883 if (distance_to_stop_point < 0.0 && std::abs(distance_to_stop_point) > ignore_stop_line_threshold_) {
884 std::string msg = regelem_type +
885 " stop point is behind ego vehicle (distance to stop point: " + std::to_string(distance_to_stop_point) +
886 " m). Ignoring regulatory element.";
887 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
888 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
889 continue;
890 }
891 if (distance_to_stop_point < min_distance_to_stop) {
892 std::string msg = regelem_type +
893 " requires stop, but distance to stop point is smaller than minimum distance to stop. Ignoring "
894 "regulatory element.";
895 setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
896 RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
897 continue;
898 }
899
900 stop_at_end = true;
901 return regelem_type + " indicates stop";
902 }
903 return std::nullopt;
904}

Member Data Documentation

◆ a_decel_

double simple_planner::SimplePlannerNode::a_decel_ = -0.5
private

Definition at line 577 of file simple_planner.hpp.

◆ a_max_decel_

double simple_planner::SimplePlannerNode::a_max_decel_ = -1.0
private

Definition at line 578 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 557 of file simple_planner.hpp.

◆ consider_future_states_

bool simple_planner::SimplePlannerNode::consider_future_states_ = false
private

Definition at line 590 of file simple_planner.hpp.

◆ consider_grid_map_

bool simple_planner::SimplePlannerNode::consider_grid_map_ = false
private

Definition at line 580 of file simple_planner.hpp.

◆ consider_objects_

bool simple_planner::SimplePlannerNode::consider_objects_ = true
private

Definition at line 591 of file simple_planner.hpp.

◆ consider_out_of_grid_

bool simple_planner::SimplePlannerNode::consider_out_of_grid_ = false
private

Definition at line 582 of file simple_planner.hpp.

◆ consider_stop_signs_

bool simple_planner::SimplePlannerNode::consider_stop_signs_ = true
private

Definition at line 586 of file simple_planner.hpp.

◆ consider_traffic_lights_

bool simple_planner::SimplePlannerNode::consider_traffic_lights_ = true
private

Definition at line 585 of file simple_planner.hpp.

◆ consider_yield_signs_

bool simple_planner::SimplePlannerNode::consider_yield_signs_ = true
private

Definition at line 587 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 647 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 648 of file simple_planner.hpp.

648{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 624 of file simple_planner.hpp.

624{this};

◆ dt_

double simple_planner::SimplePlannerNode::dt_
private

Definition at line 619 of file simple_planner.hpp.

◆ ego_data_

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

Definition at line 606 of file simple_planner.hpp.

◆ ego_data_init_

bool simple_planner::SimplePlannerNode::ego_data_init_ = false
private

Definition at line 611 of file simple_planner.hpp.

◆ ego_data_timeout_

double simple_planner::SimplePlannerNode::ego_data_timeout_ = 1.0
private

Definition at line 570 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 635 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 636 of file simple_planner.hpp.

636{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 567 of file simple_planner.hpp.

◆ freq_

double simple_planner::SimplePlannerNode::freq_ = 10.0
private

Definition at line 568 of file simple_planner.hpp.

◆ grid_lateral_safety_distance_

double simple_planner::SimplePlannerNode::grid_lateral_safety_distance_ = 0.0
private

Definition at line 584 of file simple_planner.hpp.

◆ grid_longitudinal_safety_distance_

double simple_planner::SimplePlannerNode::grid_longitudinal_safety_distance_ = 1.5
private

Definition at line 583 of file simple_planner.hpp.

◆ grid_map_

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

Definition at line 609 of file simple_planner.hpp.

◆ grid_map_init_

bool simple_planner::SimplePlannerNode::grid_map_init_ = false
private

Definition at line 614 of file simple_planner.hpp.

◆ grid_map_timeout_

double simple_planner::SimplePlannerNode::grid_map_timeout_ = 1.0
private

Definition at line 572 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 644 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 645 of file simple_planner.hpp.

645{9.09, 11.11, 0.0, 0.01};

◆ grid_occupied_threshold_

int simple_planner::SimplePlannerNode::grid_occupied_threshold_ = 50
private

Definition at line 581 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 552 of file simple_planner.hpp.

◆ health_

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

◆ ignore_objects_behind_ego_

bool simple_planner::SimplePlannerNode::ignore_objects_behind_ego_ = true
private

Definition at line 592 of file simple_planner.hpp.

◆ ignore_stop_line_threshold_

double simple_planner::SimplePlannerNode::ignore_stop_line_threshold_ = 0.5
private

Definition at line 589 of file simple_planner.hpp.

◆ interpolation_type_

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

Definition at line 575 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 603 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 604 of file simple_planner.hpp.

◆ last_object_speed_cap_

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

Definition at line 616 of file simple_planner.hpp.

◆ latest_path_

SimplePath simple_planner::SimplePlannerNode::latest_path_
private

Definition at line 617 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 550 of file simple_planner.hpp.

◆ min_prediction_probability_

double simple_planner::SimplePlannerNode::min_prediction_probability_ = 0.0
private

Definition at line 593 of file simple_planner.hpp.

◆ n_states_

int simple_planner::SimplePlannerNode::n_states_ = 51
private

Definition at line 574 of file simple_planner.hpp.

◆ object_conflict_free_cycles_

int simple_planner::SimplePlannerNode::object_conflict_free_cycles_ = 0
private

Definition at line 618 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 547 of file simple_planner.hpp.

◆ object_interaction_time_window_

double simple_planner::SimplePlannerNode::object_interaction_time_window_ = 0.5
private

Definition at line 596 of file simple_planner.hpp.

◆ object_lateral_safety_distance_

double simple_planner::SimplePlannerNode::object_lateral_safety_distance_ = 0.0
private

Definition at line 595 of file simple_planner.hpp.

◆ object_list_

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

Definition at line 607 of file simple_planner.hpp.

◆ object_list_init_

bool simple_planner::SimplePlannerNode::object_list_init_ = false
private

Definition at line 612 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 638 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 639 of file simple_planner.hpp.

639{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 594 of file simple_planner.hpp.

◆ object_standstill_speed_threshold_

double simple_planner::SimplePlannerNode::object_standstill_speed_threshold_ = 0.2
private

Definition at line 599 of file simple_planner.hpp.

◆ object_timeout_

double simple_planner::SimplePlannerNode::object_timeout_ = 1.0
private

Definition at line 571 of file simple_planner.hpp.

◆ object_velocity_reduction_step_

double simple_planner::SimplePlannerNode::object_velocity_reduction_step_ = 0.1
private

Definition at line 597 of file simple_planner.hpp.

◆ object_velocity_release_hysteresis_cycles_

int simple_planner::SimplePlannerNode::object_velocity_release_hysteresis_cycles_ = 3
private

Definition at line 600 of file simple_planner.hpp.

◆ object_velocity_release_step_

double simple_planner::SimplePlannerNode::object_velocity_release_step_ = 0.5
private

Definition at line 598 of file simple_planner.hpp.

◆ offset_to_stop_line_

double simple_planner::SimplePlannerNode::offset_to_stop_line_ = 0.0
private

Definition at line 588 of file simple_planner.hpp.

◆ parameters_callback_

OnSetParametersCallbackHandle::SharedPtr simple_planner::SimplePlannerNode::parameters_callback_
private

Callback handle for dynamic parameter reconfiguration.

Definition at line 562 of file simple_planner.hpp.

◆ pub_

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

Definition at line 546 of file simple_planner.hpp.

◆ publish_object_interaction_markers_

bool simple_planner::SimplePlannerNode::publish_object_interaction_markers_ = true
private

Definition at line 601 of file simple_planner.hpp.

◆ publish_timer_

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

Definition at line 549 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 551 of file simple_planner.hpp.

◆ route_

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

Definition at line 608 of file simple_planner.hpp.

◆ route_init_

bool simple_planner::SimplePlannerNode::route_init_ = false
private

Definition at line 613 of file simple_planner.hpp.

◆ route_timeout_

double simple_planner::SimplePlannerNode::route_timeout_ = 1.0
private

Definition at line 569 of file simple_planner.hpp.

◆ route_topic_diagnostic_

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

Definition at line 641 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 642 of file simple_planner.hpp.

642{18.18, 22.22, 0.0, 0.005};

◆ safe_stop_distance_

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

Definition at line 615 of file simple_planner.hpp.

◆ sub_egoData_

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

Definition at line 541 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 544 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 542 of file simple_planner.hpp.

◆ sub_route_

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

Definition at line 543 of file simple_planner.hpp.

◆ tf2_buffer_

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

Definition at line 538 of file simple_planner.hpp.

◆ tf2_listener_

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

Definition at line 539 of file simple_planner.hpp.

◆ trajectory_frame_id_

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

Definition at line 566 of file simple_planner.hpp.

◆ trajectory_horizon_

double simple_planner::SimplePlannerNode::trajectory_horizon_ = 10.0
private

Definition at line 573 of file simple_planner.hpp.

◆ trigger_turn_signals_

bool simple_planner::SimplePlannerNode::trigger_turn_signals_ = true
private

Definition at line 579 of file simple_planner.hpp.

◆ v_ref_

double simple_planner::SimplePlannerNode::v_ref_ = 13.89
private

Definition at line 576 of file simple_planner.hpp.

◆ vehicle_frame_id_

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

Definition at line 565 of file simple_planner.hpp.


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