simple_planner v1.5.0
Loading...
Searching...
No Matches
simple_planner.cpp
Go to the documentation of this file.
1// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
2// SPDX-License-Identifier: Apache-2.0
3
4#include <algorithm>
5#include <array>
6#include <chrono>
7#include <cmath>
8#include <functional>
9#include <optional>
10#include <sstream>
11#include <thread>
12#include <vector>
13
14#include <Eigen/Dense>
15
16#include <tk/spline.h>
17#include <tracetools/tracetools.h>
18
21
26namespace simple_planner {
27
32SimplePlannerNode::SimplePlannerNode() : 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}
195
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}
316
322void SimplePlannerNode::egoDataCallback(const perception_msgs::msg::EgoData::UniquePtr msg) {
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}
333
334void SimplePlannerNode::objectListCallback(const perception_msgs::msg::ObjectList::UniquePtr msg) {
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}
345
351void SimplePlannerNode::routeCallback(const route_planning_msgs::msg::Route::UniquePtr msg) {
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}
362
363void SimplePlannerNode::gridMapCallback(const nav_msgs::msg::OccupancyGrid::UniquePtr msg) {
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}
374
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}
478
479bool SimplePlannerNode::isMessageOutdated(const std_msgs::msg::Header& header, double timeout, const rclcpp::Time& stamp) {
480 if (timeout == -1.0) {
481 return false;
482 }
483 return (stamp - rclcpp::Time(header.stamp)) > rclcpp::Duration::from_seconds(timeout);
484}
485
495trajectory_planning_msgs::msg::Trajectory SimplePlannerNode::createTrajectory(PlannerState state, const rclcpp::Time& stamp) {
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}
541
542trajectory_planning_msgs::msg::Trajectory SimplePlannerNode::buildStandstillTrajectory(
543 const std_msgs::msg::Header& target_header) {
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}
551
552trajectory_planning_msgs::msg::Trajectory SimplePlannerNode::buildTrajectoryFromSimplePath(const SimplePath& path) {
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);
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}
592
593SimplePath SimplePlannerNode::buildSafeStopPath(const std_msgs::msg::Header& target_header) {
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}
612
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);
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}
647
648void SimplePlannerNode::appendRoutePoints(const route_planning_msgs::msg::Route& tf_route,
649 FollowRoutePlan& route_plan,
650 std::map<uint64_t, uint64_t>& lane_change_indices_map) {
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}
734
735bool SimplePlannerNode::tryRegisterLaneChange(const route_planning_msgs::msg::Route& tf_route,
736 size_t route_element_idx,
737 std::map<uint64_t, uint64_t>& lane_change_indices_map,
738 uint8_t& suggested_turn_signal) {
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}
818
820 const route_planning_msgs::msg::Route& tf_route,
821 size_t route_element_idx,
822 const route_planning_msgs::msg::LaneElement& suggested_lane,
823 const SimplePathPoint& simple_path_point,
824 double t_total,
825 bool& stop_at_end,
826 double& offset_to_stop_line) {
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}
905
907 const route_planning_msgs::msg::Route& tf_route,
908 const std::vector<SimplePathPoint>& route_points,
909 const std::map<uint64_t, uint64_t>& lane_change_indices_map) {
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}
941
942void SimplePlannerNode::applyIndicatorRequest(uint8_t suggested_turn_signal) {
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}
973
975 while (!path.points.empty() && path.points[0].position.x() < 0.0) {
976 path.points.erase(path.points.begin());
977 }
978}
979
980SimplePath SimplePlannerNode::calculateSafeStopAlongRoute(const SimplePath& path, const double safe_stop_distance) {
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}
994
995SimplePath SimplePlannerNode::calculateSafeStopAlongEgoHeading(const perception_msgs::msg::EgoData& ego_data,
996 const double safe_stop_distance,
997 const std_msgs::msg::Header& target_header) {
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}
1021
1022std::vector<SimplePathPoint> SimplePlannerNode::generateLaneChangePath(size_t start_idx,
1023 size_t turn_idx,
1024 const route_planning_msgs::msg::Route& route) {
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}
1058
1059std::vector<SimplePathPoint> SimplePlannerNode::truncatePathAtS(const std::vector<SimplePathPoint>& path, double stop_s) {
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}
1099
1100void SimplePlannerNode::recalculateS(std::vector<SimplePathPoint>& path) {
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}
1109
1110std::vector<SimplePathPoint> SimplePlannerNode::resamplePath(const std::vector<SimplePathPoint>& path,
1111 bool stop_at_end,
1112 double offset_to_stop_line,
1113 const double* speed_cap) {
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}
1222
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}
1248
1249} // namespace simple_planner
1250
1258int main(int argc, char* argv[]) {
1259 rclcpp::init(argc, argv);
1260 rclcpp::spin(std::make_shared<simple_planner::SimplePlannerNode>());
1261 rclcpp::shutdown();
1262
1263 return 0;
1264}
void setup()
Sets up subscribers, publishers, etc. to configure the node.
SimplePath buildSafeStopPath(const std_msgs::msg::Header &target_header)
Builds the initial safe-stop path for the current cycle.
std::unique_ptr< diagnostic_updater::TopicDiagnostic > ego_data_topic_diagnostic_
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
static void trimPathBehindEgo(SimplePath &path)
Removes path points that lie behind the ego vehicle in vehicle frame.
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 > safe_stop_distance_
bool hasValidGridMap(const rclcpp::Time &stamp) const
Checks whether a fresh, structurally valid grid map is currently available.
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.
rclcpp::Publisher< trajectory_planning_msgs::msg::Trajectory >::SharedPtr pub_
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr hazard_lights_service_client_
static void recalculateS(std::vector< SimplePathPoint > &path)
Recomputes accumulated path distance from point positions.
diagnostic_updater::Updater diagnostic_updater_
Diagnostic updater.
std::shared_ptr< tf2_ros::TransformListener > tf2_listener_
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::unique_ptr< tf2_ros::Buffer > tf2_buffer_
rclcpp::Subscription< perception_msgs::msg::ObjectList >::SharedPtr sub_object_list_
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.
std::unique_ptr< diagnostic_updater::TopicDiagnostic > grid_map_topic_diagnostic_
void publishTimerCallback()
This callback is invoked every period seconds by the timer.
TopicDiagnosticConfig grid_map_topic_diagnostic_config_
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.
perception_msgs::msg::ObjectList object_list_
TopicDiagnosticConfig ego_data_topic_diagnostic_config_
std::unique_ptr< diagnostic_updater::DiagnosedPublisher< trajectory_planning_msgs::msg::Trajectory > > diagnosed_publisher_
FollowRoutePlan buildRoutePlan(const std_msgs::msg::Header &target_header)
Builds the complete route-following plan including stop and turn information.
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.
rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector< rclcpp::Parameter > &parameters)
Handles reconfiguration when a parameter value is changed.
Definition utils.hpp:79
TopicDiagnosticConfig object_list_topic_diagnostic_config_
void health(diagnostic_updater::DiagnosticStatusWrapper &stat)
Function called by diagnostic updater to populate diagnostics status.
Definition utils.hpp:173
nav_msgs::msg::OccupancyGrid grid_map_
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr right_turn_indicator_service_client_
static std::vector< SimplePathPoint > truncatePathAtS(const std::vector< SimplePathPoint > &path, double stop_s)
Returns a path ending exactly at the requested accumulated path coordinate.
std::unique_ptr< diagnostic_updater::TopicDiagnostic > object_list_topic_diagnostic_
static std::string plannerStateToString(const PlannerState &state)
Converts a PlannerState enum to a string representation.
Definition utils.hpp:188
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr object_interaction_marker_pub_
rclcpp::Subscription< perception_msgs::msg::EgoData >::SharedPtr sub_egoData_
static std::string turnSignalToString(const uint8_t &turn_signal)
Converts a turn signal value to a string representation.
Definition utils.hpp:203
std::unique_ptr< diagnostic_updater::TopicDiagnostic > route_topic_diagnostic_
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_
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.
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
TopicDiagnosticConfig diagnosed_publisher_config_
struct simple_planner::SimplePlannerNode::DiagnosticStatus health_
TopicDiagnosticConfig route_topic_diagnostic_config_
SimplePath calculateSafeStopAlongRoute(const SimplePath &path, const double safe_stop_distance)
Truncates and resamples an existing path to stop within the safe-stop distance.
void applyIndicatorRequest(uint8_t suggested_turn_signal)
Requests the appropriate indicator state for the current route plan.
void egoDataCallback(const perception_msgs::msg::EgoData::UniquePtr msg)
Stores the latest ego data message.
rclcpp::Client< std_srvs::srv::SetBool >::SharedPtr left_turn_indicator_service_client_
rclcpp::Subscription< nav_msgs::msg::OccupancyGrid >::SharedPtr sub_grid_map_
PlannerState determinePlannerState(const rclcpp::Time &stamp)
Determines the current planner state from input freshness and route availability.
void clearObjectInteractionMarkers(const std_msgs::msg::Header &target_header)
Deletes the currently published object interaction markers.
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
trajectory_planning_msgs::msg::Trajectory createTrajectory(PlannerState state, const rclcpp::Time &stamp)
Creates a trajectory for the already determined planner state.
route_planning_msgs::msg::Route route_
perception_msgs::msg::EgoData ego_data_
trajectory_planning_msgs::msg::Trajectory buildTrajectoryFromSimplePath(const SimplePath &path)
Builds a trajectory message from a simple path.
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
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.
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.
SimplePlannerNode()
Creates a SimplePlannerNode node.
static trajectory_planning_msgs::msg::Trajectory buildStandstillTrajectory(const std_msgs::msg::Header &target_header)
Creates a standstill trajectory for the current planning cycle.
void resetObjectState(const std_msgs::msg::Header &target_header)
Resets the remembered object speed cap / hysteresis state and clears interaction markers.
void gridMapCallback(const nav_msgs::msg::OccupancyGrid::UniquePtr msg)
Stores the latest occupancy grid map message.
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.
Namespace for simple_planner package.
int main(int argc, char *argv[])
Starts the simple planner ROS node.
std::vector< SimplePathPoint > points
std_msgs::msg::Header header
std::map< std::string, std::string > key_value_pairs
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)