simple_planner.hpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#pragma once
 
#include <map>
#include <optional>
 
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>
 
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
 
#include <nav_msgs/msg/occupancy_grid.hpp>
 
#include <perception_msgs/msg/ego_data.hpp>
#include <perception_msgs/msg/object_list.hpp>
#include <perception_msgs_utils/object_access.hpp>
 
#include <rclcpp/rclcpp.hpp>
 
#include <std_srvs/srv/set_bool.hpp>
 
#include <route_planning_msgs/msg/route.hpp>
#include <route_planning_msgs_utils/route_access.hpp>
 
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_route_planning_msgs/tf2_route_planning_msgs.hpp>
#include <tf2_trajectory_planning_msgs/tf2_trajectory_planning_msgs.hpp>
 
#include <trajectory_planning_msgs/msg/trajectory.hpp>
#include <trajectory_planning_msgs_utils/trajectory_access.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
 
#include <simple_planner/object_geometry.hpp>
 
namespace simple_planner {
 
// only required for parameter handling
template <typename C>
struct is_vector : std::false_type {};
template <typename T, typename A>
struct is_vector<std::vector<T, A>> : std::true_type {};
template <typename C>
inline constexpr bool is_vector_v = is_vector<C>::value;
 
struct TopicDiagnosticConfig {
  double min_frequency;
 
  double max_frequency;
 
  double min_acceptable_timestamp_delta;
 
  double max_acceptable_timestamp_delta;
};
 
struct SimplePathPoint {
  Eigen::Vector2d position;
  double s;
  double v;
 
  explicit SimplePathPoint(const Eigen::Vector2d& pos, double s = -1.0, double v = -1.0) : position(pos), s(s), v(v) {}
 
  SimplePathPoint() = default;
};
 
struct SimplePath {
  std_msgs::msg::Header header;
  std::vector<SimplePathPoint> points;
};
 
class SimplePlannerNode : public rclcpp::Node {
 public:
  SimplePlannerNode();
 
 private:
  enum InterpolationType { LINEAR = 0, SPLINE = 1 };
  enum class PlannerState { NoPublish, Standstill, SafeStop, FollowRoute };
 
  struct FollowRoutePlan {
    SimplePath path;
    bool stop_at_end = false;
    std::string reason_to_stop;
    double offset_to_stop_line = 0.0;
    uint8_t suggested_turn_signal = route_planning_msgs::msg::LaneElement::SUGGESTED_TURN_SIGNAL_NONE;
  };
 
  // Internal object-handling tuning values (fixed, intentionally not exposed as parameters)
  static constexpr double kObjectCollisionCheckDt = 0.05;  // maximum time step for swept collision checks (s)
  static constexpr double kMinObjectWidth = 0.8;           // minimum object width if dimensions are missing/too small (m)
  static constexpr double kMinObjectLength = 1.2;          // minimum object length if dimensions are missing/too small (m)
 
  template <typename T>
  void declareAndLoadParameter(const std::string& name,
                               T& param,
                               const std::string& description,
                               const bool add_to_auto_reconfigurable_params = true,
                               const bool is_required = false,
                               const bool read_only = false,
                               const std::optional<double>& from_value = std::nullopt,
                               const std::optional<double>& to_value = std::nullopt,
                               const std::optional<double>& step_value = std::nullopt,
                               const std::string& additional_constraints = "");
 
  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter>& parameters);
 
  void setup();
 
  void egoDataCallback(const perception_msgs::msg::EgoData::UniquePtr msg);
 
  void objectListCallback(const perception_msgs::msg::ObjectList::UniquePtr msg);
 
  void routeCallback(const route_planning_msgs::msg::Route::UniquePtr msg);
 
  void gridMapCallback(const nav_msgs::msg::OccupancyGrid::UniquePtr msg);
 
  void publishTimerCallback();
 
  PlannerState determinePlannerState(const rclcpp::Time& stamp);
 
  static bool isMessageOutdated(const std_msgs::msg::Header& header, double timeout, const rclcpp::Time& stamp);
 
  trajectory_planning_msgs::msg::Trajectory createTrajectory(PlannerState state, const rclcpp::Time& stamp);
 
  static trajectory_planning_msgs::msg::Trajectory buildStandstillTrajectory(const std_msgs::msg::Header& target_header);
 
  trajectory_planning_msgs::msg::Trajectory buildTrajectoryFromSimplePath(const SimplePath& path);
 
  void clearObjectInteractionMarkers(const std_msgs::msg::Header& target_header);
 
  void publishObjectInteractionMarkers(const std_msgs::msg::Header& target_header, const std::optional<ConflictSample>& conflict);
 
  SimplePath buildSafeStopPath(const std_msgs::msg::Header& target_header);
 
  FollowRoutePlan buildRoutePlan(const std_msgs::msg::Header& target_header);
 
  bool hasValidGridMap(const rclcpp::Time& stamp) const;
 
  void applyObjectConstraints(const std_msgs::msg::Header& target_header,
                              const std::vector<SimplePathPoint>& base_path_points,
                              FollowRoutePlan& route_plan);
 
  void applyGridMapConstraints(const std_msgs::msg::Header& target_header,
                               std::vector<SimplePathPoint>& base_path_points,
                               FollowRoutePlan& route_plan);
 
  std::optional<double> findFirstGridMapStopS(const std_msgs::msg::Header& target_header,
                                              const std::vector<SimplePathPoint>& base_path_points);
 
  std::vector<ObjectTrajectory> buildObjectTrajectories(const perception_msgs::msg::ObjectList& tf_object_list,
                                                        const rclcpp::Time& stamp) const;
 
  std::optional<ConflictSample> firstConflict(const std::vector<SimplePathPoint>& ego_path,
                                              const std::vector<ObjectTrajectory>& object_trajectories) const;
 
  void resetObjectState(const std_msgs::msg::Header& target_header);
 
  void appendRoutePoints(const route_planning_msgs::msg::Route& tf_route,
                         FollowRoutePlan& route_plan,
                         std::map<uint64_t, uint64_t>& lane_change_indices_map);
 
  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);
 
  void updateForTrafficLights(const route_planning_msgs::msg::Route& tf_route,
                              size_t route_element_idx,
                              const route_planning_msgs::msg::LaneElement& suggested_lane,
                              const SimplePathPoint& simple_path_point,
                              double t_total,
                              bool& stop_at_end,
                              double& offset_to_stop_line);
 
  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);
 
  void applyIndicatorRequest(uint8_t suggested_turn_signal);
 
  static void trimPathBehindEgo(SimplePath& path);
 
  static std::vector<SimplePathPoint> truncatePathAtS(const std::vector<SimplePathPoint>& path, double stop_s);
 
  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);
 
  std::vector<SimplePathPoint> generateLaneChangePath(size_t start_idx,
                                                      size_t turn_idx,
                                                      const route_planning_msgs::msg::Route& route);
 
  static void recalculateS(std::vector<SimplePathPoint>& path);
 
  SimplePath calculateSafeStopAlongEgoHeading(const perception_msgs::msg::EgoData& ego_data,
                                              const double safe_stop_distance,
                                              const std_msgs::msg::Header& target_header);
 
  SimplePath calculateSafeStopAlongRoute(const SimplePath& path, const double safe_stop_distance);
 
  static bool requiresTransform(const std_msgs::msg::Header& source_header, const std_msgs::msg::Header& target_header);
 
  SimplePath transformPath(const SimplePath& path, const std_msgs::msg::Header& target_header);
 
  void health(diagnostic_updater::DiagnosticStatusWrapper& stat);
 
  void setHealth(const unsigned char status,
                 const std::string& msg,
                 const std::map<std::string, std::string>& key_value_pairs = {});
 
  static std::string plannerStateToString(const PlannerState& state);
 
  static std::string turnSignalToString(const uint8_t& turn_signal);
 
  std::unique_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
 
  rclcpp::Subscription<perception_msgs::msg::EgoData>::SharedPtr sub_egoData_;
  rclcpp::Subscription<perception_msgs::msg::ObjectList>::SharedPtr sub_object_list_;
  rclcpp::Subscription<route_planning_msgs::msg::Route>::SharedPtr sub_route_;
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_grid_map_;
 
  rclcpp::Publisher<trajectory_planning_msgs::msg::Trajectory>::SharedPtr pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr object_interaction_marker_pub_;
 
  rclcpp::TimerBase::SharedPtr publish_timer_;
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr left_turn_indicator_service_client_;
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr right_turn_indicator_service_client_;
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr hazard_lights_service_client_;
 
  std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)>>> auto_reconfigurable_params_;
 
  OnSetParametersCallbackHandle::SharedPtr parameters_callback_;
 
  // Parameters
  std::string vehicle_frame_id_ = "base_link";
  std::string trajectory_frame_id_ = "base_link";
  std::string fixed_over_time_frame_id_ = "map";
  double freq_ = 10.0;
  double route_timeout_ = 1.0;
  double ego_data_timeout_ = 1.0;
  double object_timeout_ = 1.0;
  double grid_map_timeout_ = 1.0;
  double trajectory_horizon_ = 10.0;
  int n_states_ = 51;
  uint8_t interpolation_type_ = InterpolationType::SPLINE;
  double v_ref_ = 13.89;
  double a_decel_ = -0.5;
  double a_max_decel_ = -1.0;
  bool trigger_turn_signals_ = true;
  bool consider_grid_map_ = false;
  int grid_occupied_threshold_ = 50;
  bool consider_out_of_grid_ = false;
  double grid_longitudinal_safety_distance_ = 1.5;
  double grid_lateral_safety_distance_ = 0.0;
  bool consider_traffic_lights_ = true;
  double offset_to_stop_line_ = 0.0;
  double ignore_stop_line_threshold_ = 0.5;
  bool consider_future_states_ = false;
  bool consider_objects_ = true;
  double min_prediction_prob_ = 0.0;
  double object_longitudinal_safety_distance_ = 1.5;
  double object_lateral_safety_distance_ = 0.0;
  double object_interaction_time_window_ = 0.5;
  double object_velocity_reduction_step_ = 0.1;
  double object_velocity_release_step_ = 0.5;
  double object_standstill_speed_threshold_ = 0.2;
  int object_velocity_release_hysteresis_cycles_ = 3;
  bool publish_object_interaction_markers_ = true;
 
  double lane_change_distance_factor_ = 6.0;
  double lane_change_min_distance_factor_ = 2.0;
 
  perception_msgs::msg::EgoData ego_data_;
  perception_msgs::msg::ObjectList object_list_;
  route_planning_msgs::msg::Route route_;
  nav_msgs::msg::OccupancyGrid grid_map_;
 
  bool ego_data_init_ = false;
  bool object_list_init_ = false;
  bool route_init_ = false;
  bool grid_map_init_ = false;
  std::optional<double> safe_stop_distance_;
  std::optional<double> last_object_speed_cap_;
  SimplePath latest_path_;
  int object_conflict_free_cycles_ = 0;
  double dt_;
 
  diagnostic_updater::Updater diagnostic_updater_{this};
 
  struct DiagnosticStatus {
    unsigned char status = diagnostic_msgs::msg::DiagnosticStatus::STALE;
    std::string message;
    std::map<std::string, std::string> key_value_pairs;
  } health_;
 
  std::unique_ptr<diagnostic_updater::TopicDiagnostic> ego_data_topic_diagnostic_;
  TopicDiagnosticConfig ego_data_topic_diagnostic_config_{45.45, 55.55, 0.0, 0.002};
 
  std::unique_ptr<diagnostic_updater::TopicDiagnostic> object_list_topic_diagnostic_;
  TopicDiagnosticConfig object_list_topic_diagnostic_config_{9.09, 11.11, 0.0, 0.01};
 
  std::unique_ptr<diagnostic_updater::TopicDiagnostic> route_topic_diagnostic_;
  TopicDiagnosticConfig route_topic_diagnostic_config_{18.18, 22.22, 0.0, 0.005};
 
  std::unique_ptr<diagnostic_updater::TopicDiagnostic> grid_map_topic_diagnostic_;
  TopicDiagnosticConfig grid_map_topic_diagnostic_config_{9.09, 11.11, 0.0, 0.01};
 
  std::unique_ptr<diagnostic_updater::DiagnosedPublisher<trajectory_planning_msgs::msg::Trajectory>> diagnosed_publisher_;
  TopicDiagnosticConfig diagnosed_publisher_config_{9.09, 11.11, 0.0, 0.01};
};
 
}  // namespace simple_planner