17#include <tracetools/tracetools.h>
34 "Frame ID of local vehicle frame in which the trajectory is planned");
37 "Frame ID of frame that is fixed over time for finding temporal transforms");
40 "Time after which a received route is considered invalid (s) (use -1 for no timeout)");
42 "Time after which a received ego vehicle data is considered invalid (s) (use -1 for no timeout)");
44 "Time after which a received object list is considered invalid (s) (use -1 for no timeout)");
46 "Time after which a received grid map is considered invalid (s) (use -1 for no timeout)");
52 "Reference velocity (m/s); set for all states in the trajectory. Set to '-1.0' to use velocity from route.");
54 "Desired deceleration for braking at stop lines or end of route (m/s^2) - must be < 0.0");
56 "Maximum deceleration for safe-stop trajectories (m/s^2) - must be < 0.0 and <= a_decel");
58 "True: planner will trigger turn signal services; false: planner will not request turn signals");
60 "True: planner will consider grid map; false: planner will ignore grid map");
62 "Minimum occupancy value that is considered as blocked within the grid map",
true,
false,
false,
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");
68 "Longitudinal ego-box margin for occupied-cell collision checks; negative values shrink the box "
70 true,
false,
false, -20.0, 20.0, 0.1);
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);
75 "True: planner will consider traffic lights; false: planner will ignore traffic lights");
77 "True: planner will consider stop signs; false: planner will ignore stop signs");
79 "True: planner will consider yield signs; false: planner will ignore yield signs");
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)");
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)");
87 "True: trajectory will consider forecast states of supported regulatory elements; false: "
88 "trajectory will only consider their current state");
90 "True: planner will consider perceived objects on the route; false: planner will ignore objects");
92 "Exclude objects with reference-point x < 0 in vehicle_frame_id");
94 "Consider predictions with probability > this threshold; otherwise use the most likely one");
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);
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);
102 "Maximum time offset for counting a spatial overlap as interaction (s)");
104 "Velocity decrement per object-avoidance iteration (m/s)",
true,
false,
false, 1e-3, 40.0, 1e-3);
106 "Maximum velocity increase per cycle after hysteresis cleared object conflicts (m/s)",
true,
107 false,
false, 1e-3, 40.0, 1e-3);
109 "Publish standstill if object avoidance would require a lower speed cap (m/s)",
true,
false,
110 false, 0.0, 10.0, 1e-3);
112 "Number of conflict-free cycles required before increasing the remembered object speed cap",
true,
113 false,
false, 0.0, 100.0, 1.0);
115 "Publish RViz markers for the conflict explaining the final speed reduction");
117 "Factor multiplied with the current velocity to determine the lane change distance (m)");
119 "Factor multiplied with the vehicle length to determine the minimum lane change distance (m)");
123 RCLCPP_ERROR(this->get_logger(),
"Invalid parameter: a_decel must be < 0.0");
127 RCLCPP_ERROR(this->get_logger(),
"Invalid parameter: a_max_decel must be < 0.0 and <= a_decel");
134 "Minimum frequency for incoming ego-data messages",
false);
137 "Maximum frequency for incoming ego-data messages",
false);
140 "Minimum acceptable timestamp delta for incoming ego-data messages",
false);
143 "Maximum acceptable timestamp delta for incoming ego-data messages",
false);
146 "Minimum frequency for incoming object-list messages",
false);
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);
164 "Minimum acceptable timestamp delta for incoming route messages",
false);
167 "Maximum acceptable timestamp delta for incoming route messages",
false);
170 "Minimum frequency for incoming grid-map messages",
false);
173 "Maximum frequency for incoming grid-map messages",
false);
176 "Minimum acceptable timestamp delta for incoming grid-map messages",
false);
179 "Maximum acceptable timestamp delta for incoming grid-map messages",
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);
201 tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
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'",
218 RCLCPP_INFO(this->get_logger(),
"Publishing trajectory at '%f' hz",
freq_);
221 sub_egoData_ = this->create_subscription<perception_msgs::msg::EgoData>(
223 RCLCPP_INFO(this->get_logger(),
"Subscribed to '%s'",
sub_egoData_->get_topic_name());
225 sub_object_list_ = this->create_subscription<perception_msgs::msg::ObjectList>(
227 RCLCPP_INFO(this->get_logger(),
"Subscribed to '%s'",
sub_object_list_->get_topic_name());
230 sub_route_ = this->create_subscription<route_planning_msgs::msg::Route>(
232 RCLCPP_INFO(this->get_logger(),
"Subscribed to '%s'",
sub_route_->get_topic_name());
234 sub_grid_map_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
236 RCLCPP_INFO(this->get_logger(),
"Subscribed to '%s'",
sub_grid_map_->get_topic_name());
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());
262 const int ego_data_topic_diagnostic_frequency_window_size =
268 ego_data_topic_diagnostic_frequency_window_size),
272 const int route_topic_diagnostic_frequency_window_size =
278 route_topic_diagnostic_frequency_window_size),
283 const int object_list_topic_diagnostic_frequency_window_size =
289 object_list_topic_diagnostic_frequency_window_size),
295 const int grid_map_topic_diagnostic_frequency_window_size =
301 grid_map_topic_diagnostic_frequency_window_size),
306 const int diagnosed_publisher_frequency_window_size =
308 diagnosed_publisher_ = std::make_unique<diagnostic_updater::DiagnosedPublisher<trajectory_planning_msgs::msg::Trajectory>>(
312 diagnosed_publisher_frequency_window_size),
330 RCLCPP_INFO(this->get_logger(),
"Received first ego data message, initialized global variable");
342 RCLCPP_INFO(this->get_logger(),
"Received first object list message, initialized global variable");
357 RCLCPP_INFO(this->get_logger(),
"Received new route message, initialized global variable");
371 RCLCPP_INFO(this->get_logger(),
"Received first grid map message, initialized global variable");
378 setHealth(diagnostic_msgs::msg::DiagnosticStatus::STALE,
"No ego data received yet",
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,
396 setHealth(diagnostic_msgs::msg::DiagnosticStatus::STALE,
"No route received and no ongoing safe stop",
403 if (perception_msgs::object_access::getStandstill(
ego_data_)) {
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,
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,
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,
439 if (perception_msgs::object_access::getStandstill(
ego_data_)) {
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,
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,
457 if (perception_msgs::object_access::getStandstill(
ego_data_)) {
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,
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,
474 setHealth(diagnostic_msgs::msg::DiagnosticStatus::OK,
"Input information up to date. Following route.",
480 if (timeout == -1.0) {
483 return (stamp - rclcpp::Time(header.stamp)) > rclcpp::Duration::from_seconds(timeout);
496 rclcpp::Time begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
498 trajectory_planning_msgs::msg::Trajectory tra;
499 tra.header.stamp = stamp;
514 RCLCPP_DEBUG(this->get_logger(),
"Executing safe stop.");
525 throw std::runtime_error(
"createTrajectory called for non-publish state");
531 }
catch (tf2::TransformException& ex) {
533 "' is not available: " + std::string(ex.what()));
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);
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);
556 if (usable_path.
points.empty()) {
559 std::string msg =
"No usable forward path remains. Publishing standstill trajectory.";
560 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
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);
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;
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);
588 trajectory_planning_msgs::trajectory_access::setStandstill(tra,
false);
589 RCLCPP_DEBUG(this->get_logger(),
"Standstill = %d", tra.standstill);
594 double current_velocity = perception_msgs::object_access::getVelocityMagnitude(
ego_data_);
599 if (safe_stop_path.
points.empty()) {
602 "No latest path available. Initialize safe stop along ego heading. Current velocity: %f m/s, safe stop distance: %f m",
606 RCLCPP_WARN(this->get_logger(),
"Initialize safe stop along latest path. Current velocity: %f m/s, safe stop distance: %f m",
614 RCLCPP_DEBUG(this->get_logger(),
"Default case: route is up to date, creating path from route.");
618 route_planning_msgs::msg::Route tf_route =
route_;
621 tf_route =
tf2_buffer_->transform(
route_, target_header.frame_id, tf2_ros::fromMsg(target_header.stamp),
623 }
catch (tf2::TransformException& ex) {
624 std::string msg =
"Route transformation is not available: " + std::string(ex.what()) +
".";
626 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
632 std::map<uint64_t, uint64_t> lane_change_indices_map;
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";
654 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
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";
664 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
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);
679 const auto& suggested_lane = route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
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_;
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;
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.";
694 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
699 if (j == tf_route.current_route_element_idx) {
703 if (route_element.will_change_suggested_lane &&
713 route_plan.
path.
points.push_back(simple_path_point);
714 if (destination_is_present && j + 1 == destination_or_route_end_idx) {
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);
729 const std::string msg =
"Local enriched route ends before the planner horizon; trajectory will end at its last element";
731 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
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.";
742 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
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;
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,
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;
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.",
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;
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());
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());
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());
803 ds += std::abs(tf_route.route_elements[i_start].s - tf_route.route_elements[i_start - 1].s);
807 if (!tf_route.route_elements[i_start].is_enriched || !tf_route.route_elements[i_end].is_enriched) {
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());
815 lane_change_indices_map[route_element_idx] = i_start;
820 const route_planning_msgs::msg::Route& tf_route,
821 size_t route_element_idx,
822 const route_planning_msgs::msg::LaneElement& suggested_lane,
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:
835 regelem_type =
"Traffic light";
837 case route_planning_msgs::msg::RegulatoryElement::TYPE_STOP:
839 regelem_type =
"Stop sign";
841 case route_planning_msgs::msg::RegulatoryElement::TYPE_YIELD:
843 regelem_type =
"Yield sign";
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;
856 offset_to_stop_line =
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;
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.";
867 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
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;
877 if (!requires_stop)
continue;
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_;
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.";
888 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
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.";
896 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
901 return regelem_type +
" indicates stop";
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) {
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;
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
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,
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());
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());
939 return merged_points;
943 if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE &&
946 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
947 request->data =
false;
951 }
else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_LEFT &&
953 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
954 request->data =
true;
956 }
else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_RIGHT &&
958 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
959 request->data =
true;
961 }
else if (suggested_turn_signal == route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_HAZARD &&
963 auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
964 request->data =
true;
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());
975 while (!path.
points.empty() && path.
points[0].position.x() < 0.0) {
982 double start_s = safe_stop_path.
points[0].s;
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) {
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());
992 return safe_stop_path;
996 const double safe_stop_distance,
997 const std_msgs::msg::Header& target_header) {
999 safe_stop_path.
header = ego_data.header;
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));
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;
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);
1019 return safe_stop_path;
1024 const route_planning_msgs::msg::Route& route) {
1025 std::vector<SimplePathPoint> lane_change_path;
1026 const size_t end_idx = turn_idx + 1;
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());
1032 return lane_change_path;
1035 int lane_change_direction = route_planning_msgs::route_access::getLaneChangeDirection(route.route_elements[turn_idx],
1036 route.route_elements[turn_idx + 1]);
1039 for (
size_t i = start_idx; i <= end_idx; ++i) {
1040 if (i < route.current_route_element_idx)
continue;
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;
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));
1056 return lane_change_path;
1064 if (stop_s <= path.front().s) {
1066 stop_point.
s = stop_s;
1067 return {stop_point};
1070 if (stop_s >= path.back().s) {
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()) {
1079 if (stop_it == path.end()) {
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;
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;
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;
1101 if (path.empty())
return;
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;
1112 double offset_to_stop_line,
1113 const double* speed_cap) {
1114 rclcpp::Time begin = rclcpp::Clock(RCL_SYSTEM_TIME).now();
1116 std::string msg =
"Route is empty. No resampling possible.";
1117 RCLCPP_WARN(this->get_logger(),
"%s", msg.c_str());
1122 std::vector<SimplePathPoint> resampled_path;
1123 double s = path[0].s;
1125 tk::spline x_spline, y_spline;
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());
1133 x_spline.set_points(s_vector, x_vector);
1134 y_spline.set_points(s_vector, y_vector);
1137 while (s < path.back().s) {
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);
1149 v = path[idx].v + (path[idx + 1].v - path[idx].v) / (path[idx + 1].s - path[idx].s) * (s - path[idx].s);
1151 if (speed_cap !=
nullptr) {
1152 v = std::min(v, std::max(*speed_cap, 0.0));
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_;
1159 if (s + ds > brake_point && stop_at_end) {
1160 if (s < brake_point) {
1161 double ds_1 = brake_point - s;
1162 double dt_1 = ds_1 / v;
1163 double dt_2 =
dt_ - dt_1;
1164 double ds_2 = std::max(0.5 *
a_decel_ * std::pow(dt_2, 2) + v * dt_2, 0.0);
1167 v = std::sqrt(std::max(std::pow(v, 2) + 2 *
a_decel_ * (s - brake_point), 0.0));
1178 if (path.size() == 1) {
1179 simple_path_point.
position = path[0].position;
1181 simple_path_point.
position.x() = x_spline(s);
1182 simple_path_point.
position.y() = y_spline(s);
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);
1193 RCLCPP_ERROR(this->get_logger(),
"Unsupported interpolation type value %d",
interpolation_type_);
1194 throw std::runtime_error(
"Unsupported interpolation type value");
1196 simple_path_point.
s = s;
1197 simple_path_point.
v = v;
1198 resampled_path.push_back(simple_path_point);
1201 if (v <= 1e-6 && ds <= 1e-6)
break;
1203 if (s == path.back().s && v == 0.0)
break;
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);
1211 if (!resampled_path.empty() && (offset_to_stop_line > 0.0 || (speed_cap !=
nullptr && *speed_cap <= 1e-6))) {
1212 stop_point = resampled_path.back();
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);
1220 return resampled_path;
1228 const rclcpp::Time stamp = now();
1231 std_msgs::msg::Header marker_header;
1232 marker_header.stamp = stamp;
1239 trajectory_planning_msgs::msg::Trajectory msg =
createTrajectory(planner_state, stamp);
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());
1259 rclcpp::init(argc, argv);
1260 rclcpp::spin(std::make_shared<simple_planner::SimplePlannerNode>());
void setup()
Sets up subscribers, publishers, etc. to configure the node.
int object_velocity_release_hysteresis_cycles_
SimplePath buildSafeStopPath(const std_msgs::msg::Header &target_header)
Builds the initial safe-stop path for the current cycle.
double object_lateral_safety_distance_
std::unique_ptr< diagnostic_updater::TopicDiagnostic > ego_data_topic_diagnostic_
void declareAndLoadParameter(const std::string &name, T ¶m, 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.
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.
bool consider_traffic_lights_
double object_velocity_release_step_
std::optional< double > safe_stop_distance_
bool hasValidGridMap(const rclcpp::Time &stamp) const
Checks whether a fresh, structurally valid grid map is currently available.
bool consider_yield_signs_
double object_standstill_speed_threshold_
double ignore_stop_line_threshold_
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_
int grid_occupied_threshold_
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 ignore_objects_behind_ego_
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.
double min_prediction_probability_
std::unique_ptr< tf2_ros::Buffer > tf2_buffer_
double offset_to_stop_line_
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.
double object_interaction_time_window_
double lane_change_distance_factor_
std::unique_ptr< diagnostic_updater::TopicDiagnostic > grid_map_topic_diagnostic_
void publishTimerCallback()
This callback is invoked every period seconds by the timer.
double object_velocity_reduction_step_
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.
double trajectory_horizon_
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 > ¶meters)
Handles reconfiguration when a parameter value is changed.
TopicDiagnosticConfig object_list_topic_diagnostic_config_
void health(diagnostic_updater::DiagnosticStatusWrapper &stat)
Function called by diagnostic updater to populate diagnostics status.
bool consider_out_of_grid_
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.
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.
double grid_lateral_safety_distance_
uint8_t interpolation_type_
std::unique_ptr< diagnostic_updater::TopicDiagnostic > route_topic_diagnostic_
std::string vehicle_frame_id_
double grid_longitudinal_safety_distance_
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_
bool publish_object_interaction_markers_
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.
bool trigger_turn_signals_
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.
double lane_change_min_distance_factor_
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.
bool consider_future_states_
double object_longitudinal_safety_distance_
SimplePath transformPath(const SimplePath &path, const std_msgs::msg::Header &target_header)
Transforms a simple path into the requested target frame and timestamp.
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.
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.
std::string trajectory_frame_id_
bool consider_stop_signs_
std::string fixed_over_time_frame_id_
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
uint8_t suggested_turn_signal
double offset_to_stop_line
std::string reason_to_stop
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)