feat(obstacle_cruise_planner): implemented common obstacle stop (tier…

…4#1185) * feat(obstacle_cruise_planner): implemented common obstacle stop Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix some implementation Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * minor changes Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * use min_ego_accel_for_rss Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove unnecessary code Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix CI error Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix typo Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Sep 28, 2022 · 3ef1663 · 3ef1663
1 parent b6f7525
commit 3ef1663
Show file tree

Hide file tree

Showing 14 changed files with 604 additions and 740 deletions.
diff --git a/...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
@@ -6,7 +6,7 @@
       is_showing_debug_info: true
 
       # longitudinal info
-      idling_time: 4.0 # idling time to detect front vehicle starting deceleration [s]
+      idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s]
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
@@ -17,6 +17,7 @@
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
       obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+      obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
 
       cruise_obstacle_type:
         unknown: true
@@ -39,7 +40,7 @@
         pedestrian: true
 
     obstacle_filtering:
-      rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m]
+      rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m]
       detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion
       decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
 
@@ -48,6 +49,7 @@
       crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
       collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 
+      outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m]
       ego_obstacle_overlap_time_threshold : 1.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
       max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
 
@@ -65,14 +67,14 @@
       # use_predicted_obstacle_pose: false
 
       # PID gains to keep safe distance with the front vehicle
-      kp: 0.6
+      kp: 2.5
       ki: 0.0
-      kd: 0.5
+      kd: 2.3
 
       min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
 
-      output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
-      vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
+      output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+      vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
 
       min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
 
@@ -87,11 +89,11 @@
       max_trajectory_length: 200.0
       velocity_margin: 0.1 #[m/s]
 
-      # Parameters for safety distance limit time
+      # Parameters for safe distance
       t_dangerous: 0.5
 
       # For initial Motion
-      replan_vel_deviation: 5.0          # velocity deviation to replan initial velocity [m/s]
+      replan_vel_deviation: 5.0         # velocity deviation to replan initial velocity [m/s]
       engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
       engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
       engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.

diff --git a/planning/obstacle_cruise_planner/CMakeLists.txt b/planning/obstacle_cruise_planner/CMakeLists.txt
@@ -15,6 +15,7 @@ ament_auto_add_library(obstacle_cruise_planner_core SHARED
   src/optimization_based_planner/velocity_optimizer.cpp
   src/optimization_based_planner/optimization_based_planner.cpp
   src/pid_based_planner/pid_based_planner.cpp
+  src/planner_interface.cpp
 )
 
 rclcpp_components_register_node(obstacle_cruise_planner_core

diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -6,7 +6,7 @@
       is_showing_debug_info: true
 
       # longitudinal info
-      idling_time: 4.0 # idling time to detect front vehicle starting deceleration [s]
+      idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s]
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
@@ -17,6 +17,7 @@
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
       obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+      obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
 
       cruise_obstacle_type:
         unknown: true
@@ -39,7 +40,7 @@
         pedestrian: true
 
     obstacle_filtering:
-      rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m]
+      rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m]
       detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion
       decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
 
@@ -48,6 +49,7 @@
       crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
       collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 
+      outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m]
       ego_obstacle_overlap_time_threshold : 1.0 #  time threshold to decide cut-in obstacle for cruise or stop [s]
       max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
 
@@ -65,14 +67,14 @@
       # use_predicted_obstacle_pose: false
 
       # PID gains to keep safe distance with the front vehicle
-      kp: 0.6
+      kp: 2.5
       ki: 0.0
-      kd: 0.5
+      kd: 2.3
 
       min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
 
-      output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
-      vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
+      output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+      vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
 
       min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
 
@@ -87,11 +89,11 @@
       max_trajectory_length: 200.0
       velocity_margin: 0.1 #[m/s]
 
-      # Parameters for safety distance limit time
+      # Parameters for safe distance
       t_dangerous: 0.5
 
       # For initial Motion
-      replan_vel_deviation: 5.0          # velocity deviation to replan initial velocity [m/s]
+      replan_vel_deviation: 5.0         # velocity deviation to replan initial velocity [m/s]
       engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
       engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
       engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -79,6 +79,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     const PredictedObjects & predicted_objects, const Trajectory & traj,
     const geometry_msgs::msg::Pose & current_pose, const double current_vel,
     DebugData & debug_data);
+  void updateHasStopped(std::vector<TargetObstacle> & target_obstacles);
   geometry_msgs::msg::Point calcNearestCollisionPoint(
     const size_t & first_within_idx,
     const std::vector<geometry_msgs::msg::Point> & collision_points,
@@ -93,13 +94,19 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   void publishDebugData(const DebugData & debug_data) const;
   void publishCalculationTime(const double calculation_time) const;
 
+  bool isCruiseObstacle(const uint8_t label);
+  bool isStopObstacle(const uint8_t label);
+
   bool is_showing_debug_info_;
   double min_behavior_stop_margin_;
   double nearest_dist_deviation_threshold_;
   double nearest_yaw_deviation_threshold_;
   double obstacle_velocity_threshold_from_cruise_to_stop_;
   double obstacle_velocity_threshold_from_stop_to_cruise_;
 
+  std::vector<int> cruise_obstacle_types_;
+  std::vector<int> stop_obstacle_types_;
+
   // parameter callback result
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 
@@ -151,8 +158,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     double rough_detection_area_expand_width;
     double detection_area_expand_width;
     double decimate_trajectory_step_length;
+    // inside
     double crossing_obstacle_velocity_threshold;
     double collision_time_margin;
+    // outside
+    double outside_rough_detection_area_expand_width;
     double ego_obstacle_overlap_time_threshold;
     double max_prediction_time_for_collision_check;
     double crossing_obstacle_traj_angle_threshold;
@@ -161,6 +171,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   ObstacleFilteringParam obstacle_filtering_param_;
 
   bool need_to_clear_vel_limit_{false};
+
+  std::vector<TargetObstacle> prev_target_obstacles_;
 };
 }  // namespace motion_planning
 

diff --git a/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp
@@ -48,9 +48,9 @@ class OptimizationBasedPlanner : public PlannerInterface
     rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
-  Trajectory generateTrajectory(
-    const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,
-    DebugData & debug_data) override;
+  Trajectory generateCruiseTrajectory(
+    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
+    boost::optional<VelocityLimit> & vel_limit, DebugData & debug_data) override;
 
 private:
   struct TrajectoryData
@@ -71,26 +71,20 @@ class OptimizationBasedPlanner : public PlannerInterface
 
   // Member Functions
   std::vector<double> createTimeVector();
-
-  double getClosestStopDistance(
-    const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data);
-
   std::tuple<double, double> calcInitialMotion(
-    const double current_vel, const Trajectory & input_traj, const size_t input_closest,
-    const Trajectory & prev_traj, const double closest_stop_dist);
+    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
+    const size_t input_closest, const Trajectory & prev_traj);
 
   TrajectoryPoint calcInterpolatedTrajectoryPoint(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose);
-
-  bool checkHasReachedGoal(const Trajectory & traj, const size_t closest_idx, const double v0);
-
+  bool checkHasReachedGoal(
+    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj);
   TrajectoryData getTrajectoryData(
     const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose);
 
   TrajectoryData resampleTrajectoryData(
     const TrajectoryData & base_traj_data, const double resampling_s_interval,
-    const double max_traj_length, const double stop_dist);
-
+    const double max_traj_length);
   Trajectory resampleTrajectory(
     const std::vector<double> & base_index, const Trajectory & base_trajectory,
     const std::vector<double> & query_index, const bool use_spline_for_pose = false);
@@ -137,7 +131,6 @@ class OptimizationBasedPlanner : public PlannerInterface
     const rclcpp::Time & current_time, const Trajectory & traj, const size_t closest_idx,
     const std::vector<double> & time_vec, const SBoundaries & s_boundaries,
     const VelocityOptimizer::OptimizationResult & opt_result);
-
   // Calculation time watcher
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
 
@@ -150,7 +143,6 @@ class OptimizationBasedPlanner : public PlannerInterface
   rclcpp::Publisher<Trajectory>::SharedPtr boundary_pub_;
   rclcpp::Publisher<Trajectory>::SharedPtr optimized_sv_pub_;
   rclcpp::Publisher<Trajectory>::SharedPtr optimized_st_graph_pub_;
-  rclcpp::Publisher<Float32Stamped>::SharedPtr distance_to_closest_obj_pub_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_wall_marker_pub_;
 

diff --git a/...le_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/...le_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp
@@ -22,8 +22,6 @@
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
-#include "tier4_planning_msgs/msg/stop_reason_array.hpp"
-#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
 #include <boost/optional.hpp>
@@ -32,7 +30,6 @@
 #include <vector>
 
 using tier4_debug_msgs::msg::Float32MultiArrayStamped;
-using tier4_planning_msgs::msg::StopSpeedExceeded;
 
 class PIDBasedPlanner : public PlannerInterface
 {
@@ -54,30 +51,19 @@ class PIDBasedPlanner : public PlannerInterface
     double dist_to_obstacle;
   };
 
-  struct StopObstacleInfo
-  {
-    StopObstacleInfo(const TargetObstacle & obstacle_arg, const double dist_to_stop_arg)
-    : obstacle(obstacle_arg), dist_to_stop(dist_to_stop_arg)
-    {
-    }
-    TargetObstacle obstacle;
-    double dist_to_stop;
-  };
-
   PIDBasedPlanner(
     rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
-  Trajectory generateTrajectory(
-    const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,
-    DebugData & debug_data) override;
+  Trajectory generateCruiseTrajectory(
+    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
+    boost::optional<VelocityLimit> & vel_limit, DebugData & debug_data) override;
 
   void updateParam(const std::vector<rclcpp::Parameter> & parameters) override;
 
 private:
-  void calcObstaclesToCruiseAndStop(
+  void calcObstaclesToCruise(
     const ObstacleCruisePlannerData & planner_data,
-    boost::optional<StopObstacleInfo> & stop_obstacle_info,
     boost::optional<CruiseObstacleInfo> & cruise_obstacle_info);
   double calcDistanceToObstacle(
     const ObstacleCruisePlannerData & planner_data, const TargetObstacle & obstacle);
@@ -90,14 +76,6 @@ class PIDBasedPlanner : public PlannerInterface
     std::vector<TargetObstacle> & debug_obstacles_to_cruise,
     visualization_msgs::msg::MarkerArray & debug_walls_marker);
 
-  Trajectory planStop(
-    const ObstacleCruisePlannerData & planner_data,
-    const boost::optional<StopObstacleInfo> & stop_obstacle_info, DebugData & debug_data);
-  boost::optional<size_t> doStop(
-    const ObstacleCruisePlannerData & planner_data, const StopObstacleInfo & stop_obstacle_info,
-    std::vector<TargetObstacle> & debug_obstacles_to_stop,
-    visualization_msgs::msg::MarkerArray & debug_walls_marker) const;
-
   void publishDebugValues(const ObstacleCruisePlannerData & planner_data) const;
 
   size_t findExtendedNearestIndex(
@@ -138,8 +116,6 @@ class PIDBasedPlanner : public PlannerInterface
     stop_watch_;
 
   // publisher
-  rclcpp::Publisher<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reasons_pub_;
-  rclcpp::Publisher<StopSpeedExceeded>::SharedPtr stop_speed_exceeded_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_values_pub_;
 
   boost::optional<double> prev_target_vel_;