feat(obstacle_cruise_planner): add new package (autowarefoundation#570)

* feat(obstacle_velocity_planner): add obstacle_velocity_planner Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * udpate yaml Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update dependency Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix maybe-unused false positive error Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * Tier IV -> TIER IV Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix some reviews Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix some reviews Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * minor change Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * minor changes Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * use obstacle_stop by default Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix compile error Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * obstacle_velocity -> adaptive_cruise Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix for autoware meta repository Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix compile error on CI Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add min_ego_accel_for_rss Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix CI error Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * rename to obstacle_cruise_planner Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix tier4_planning_launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix humble CI Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
SoohyeokPark-MORAI · Jun 15, 2022 · 167fead · 167fead
1 parent 89c51d6
commit 167fead
Show file tree

Hide file tree

Showing 14 changed files with 421 additions and 0 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
@@ -10,13 +10,20 @@
       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]
+<<<<<<< HEAD
+=======
+      min_strong_accel: -3.0 # obstacle stop requiring deceleration less than this value will be ignored [m/ss]
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
       lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
 
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
+<<<<<<< HEAD
       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]
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
       cruise_obstacle_type:
         unknown: true
@@ -76,7 +83,14 @@
 
       min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
 
+<<<<<<< HEAD
     optimization_based_planner:
+=======
+      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]
+
+    optimization_based_planner:
+      limit_min_accel: -3.0
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
       resampling_s_interval: 2.0
       dense_resampling_time_interval: 0.1
       sparse_resampling_time_interval: 1.0
@@ -86,8 +100,28 @@
       velocity_margin: 0.1 #[m/s]
 
       # Parameters for safe distance
+<<<<<<< HEAD
+      t_dangerous: 0.5
+
+=======
+      safe_distance_margin: 5.0
       t_dangerous: 0.5
 
+      # Parameters for collision detection
+      collision_time_threshold: 10.0
+      object_zero_velocity_threshold: 3.0 #[m/s]
+      object_low_velocity_threshold: 3.0 #[m/s]
+      external_velocity_limit: 20.0 #[m/s]
+      delta_yaw_threshold_of_object_and_ego: 90.0 # [deg]
+      delta_yaw_threshold_of_nearest_index: 60.0 # [deg]
+      collision_duration_threshold_of_object_and_ego: 1.0 # [s]
+
+      # Switch for each functions
+      enable_adaptive_cruise: true
+      use_object_acceleration: false
+      use_hd_map: true
+
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
       # For initial Motion
       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)

diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -10,13 +10,20 @@
       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]
+<<<<<<< HEAD
+=======
+      min_strong_accel: -3.0 # obstacle stop requiring deceleration less than this value will be ignored [m/ss]
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
       lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
 
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
+<<<<<<< HEAD
       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]
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
       cruise_obstacle_type:
         unknown: true
@@ -76,7 +83,14 @@
 
       min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
 
+<<<<<<< HEAD
     optimization_based_planner:
+=======
+      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]
+
+    optimization_based_planner:
+      limit_min_accel: -3.0
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
       resampling_s_interval: 2.0
       dense_resampling_time_interval: 0.1
       sparse_resampling_time_interval: 1.0
@@ -85,11 +99,34 @@
       max_trajectory_length: 200.0
       velocity_margin: 0.1 #[m/s]
 
+<<<<<<< HEAD
       # Parameters for safety distance limit time
       t_dangerous: 0.5
 
       # For initial Motion
       replan_vel_deviation: 5.0          # velocity deviation to replan initial velocity [m/s]
+=======
+      # Parameters for safe distance
+      safe_distance_margin: 5.0
+      t_dangerous: 0.5
+
+      # Parameters for collision detection
+      collision_time_threshold: 10.0
+      object_zero_velocity_threshold: 3.0 #[m/s]
+      object_low_velocity_threshold: 3.0 #[m/s]
+      external_velocity_limit: 20.0 #[m/s]
+      delta_yaw_threshold_of_object_and_ego: 90.0 # [deg]
+      delta_yaw_threshold_of_nearest_index: 60.0 # [deg]
+      collision_duration_threshold_of_object_and_ego: 1.0 # [s]
+
+      # Switch for each functions
+      enable_adaptive_cruise: true
+      use_object_acceleration: false
+      use_hd_map: true
+
+      # For initial Motion
+      replan_vel_deviation: 5.0         # velocity deviation to replan initial velocity [m/s]
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
       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/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
@@ -79,14 +79,37 @@ struct ObstacleCruisePlannerData
 
 struct LongitudinalInfo
 {
+<<<<<<< HEAD
+=======
+  LongitudinalInfo(
+    const double arg_max_accel, const double arg_min_accel, const double arg_max_jerk,
+    const double arg_min_jerk, const double arg_min_strong_accel, const double arg_idling_time,
+    const double arg_min_ego_accel_for_rss, const double arg_min_object_accel_for_rss,
+    const double arg_safe_distance_margin)
+  : max_accel(arg_max_accel),
+    min_accel(arg_min_accel),
+    max_jerk(arg_max_jerk),
+    min_jerk(arg_min_jerk),
+    min_strong_accel(arg_min_strong_accel),
+    idling_time(arg_idling_time),
+    min_ego_accel_for_rss(arg_min_ego_accel_for_rss),
+    min_object_accel_for_rss(arg_min_object_accel_for_rss),
+    safe_distance_margin(arg_safe_distance_margin)
+  {
+  }
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
   double max_accel;
   double min_accel;
   double max_jerk;
   double min_jerk;
+<<<<<<< HEAD
   double limit_max_accel;
   double limit_min_accel;
   double limit_max_jerk;
   double limit_min_jerk;
+=======
+  double min_strong_accel;
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
   double idling_time;
   double min_ego_accel_for_rss;
   double min_object_accel_for_rss;

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -97,7 +97,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   double min_behavior_stop_margin_;
   double nearest_dist_deviation_threshold_;
   double nearest_yaw_deviation_threshold_;
+<<<<<<< HEAD
   double obstacle_velocity_threshold_from_cruise_to_stop_;
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
   // parameter callback result
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;

diff --git a/...tacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/box2d.hpp b/...tacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/box2d.hpp
@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+<<<<<<< HEAD
 /******************************************************************************
  * Copyright 2017 The Apollo Authors. All Rights Reserved.
  *
@@ -28,6 +29,8 @@
  * limitations under the License.
  *****************************************************************************/
 
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 #ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__BOX2D_HPP_
 #define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__BOX2D_HPP_
 

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
@@ -116,13 +116,23 @@ class OptimizationBasedPlanner : public PlannerInterface
     const TargetObstacle & object, const rclcpp::Time & obj_base_time,
     const PredictedPath & predicted_path);
 
+<<<<<<< HEAD
+=======
+  bool checkIsFrontObject(const TargetObstacle & object, const Trajectory & traj);
+
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
   boost::optional<PredictedPath> resampledPredictedPath(
     const TargetObstacle & object, const rclcpp::Time & obj_base_time,
     const rclcpp::Time & current_time, const std::vector<double> & resolutions,
     const double horizon);
 
   boost::optional<double> getDistanceToCollisionPoint(
+<<<<<<< HEAD
     const TrajectoryData & ego_traj_data, const ObjectData & obj_data);
+=======
+    const TrajectoryData & ego_traj_data, const ObjectData & obj_data,
+    const double delta_yaw_threshold);
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
   boost::optional<size_t> getCollisionIdx(
     const TrajectoryData & ego_traj, const Box2d & obj_box, const size_t start_idx,
@@ -162,9 +172,25 @@ class OptimizationBasedPlanner : public PlannerInterface
   double sparse_resampling_time_interval_;
   double dense_time_horizon_;
   double max_time_horizon_;
+<<<<<<< HEAD
 
   double t_dangerous_;
   double velocity_margin_;
+=======
+  double limit_min_accel_;
+
+  double delta_yaw_threshold_of_nearest_index_;
+  double delta_yaw_threshold_of_object_and_ego_;
+  double object_zero_velocity_threshold_;
+  double object_low_velocity_threshold_;
+  double external_velocity_limit_;
+  double collision_time_threshold_;
+  double safe_distance_margin_;
+  double t_dangerous_;
+  double velocity_margin_;
+  bool enable_adaptive_cruise_;
+  bool use_object_acceleration_;
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
   double replan_vel_deviation_;
   double engage_velocity_;

diff --git a/...planner/include/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp b/...planner/include/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp
@@ -31,10 +31,14 @@ class VelocityOptimizer
     double v_max;
     double a_max;
     double a_min;
+<<<<<<< HEAD
     double limit_a_max;
     double limit_a_min;
     double limit_j_max;
     double limit_j_min;
+=======
+    double limit_a_min;
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
     double j_max;
     double j_min;
     double t_dangerous;

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
@@ -109,6 +109,7 @@ class PIDBasedPlanner : public PlannerInterface
     return tier4_autoware_utils::findNearestIndex(traj.points, pose.position);
   }
 
+<<<<<<< HEAD
   size_t findExtendedNearestSegmentIndex(
     const Trajectory traj, const geometry_msgs::msg::Pose & pose) const
   {
@@ -120,10 +121,16 @@ class PIDBasedPlanner : public PlannerInterface
     return tier4_autoware_utils::findNearestSegmentIndex(traj.points, pose.position);
   }
 
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
   // ROS parameters
   double min_accel_during_cruise_;
   double vel_to_acc_weight_;
   double min_cruise_target_vel_;
+<<<<<<< HEAD
+=======
+  double obstacle_velocity_threshold_from_cruise_to_stop_;
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
   // bool use_predicted_obstacle_pose_;
 
   // pid controller

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
@@ -99,15 +99,22 @@ class PlannerInterface
 
   void setParams(
     const bool is_showing_debug_info, const double min_behavior_stop_margin,
+<<<<<<< HEAD
     const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold,
     const double obstacle_velocity_threshold_from_cruise_to_stop)
+=======
+    const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold)
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
   {
     is_showing_debug_info_ = is_showing_debug_info;
     min_behavior_stop_margin_ = min_behavior_stop_margin;
     nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold;
     nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold;
+<<<<<<< HEAD
     obstacle_velocity_threshold_from_cruise_to_stop_ =
       obstacle_velocity_threshold_from_cruise_to_stop;
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
   }
 
   /*
@@ -136,10 +143,13 @@ class PlannerInterface
     tier4_autoware_utils::updateParam<double>(parameters, "common.min_accel", i.min_accel);
     tier4_autoware_utils::updateParam<double>(parameters, "common.max_jerk", i.max_jerk);
     tier4_autoware_utils::updateParam<double>(parameters, "common.min_jerk", i.min_jerk);
+<<<<<<< HEAD
     tier4_autoware_utils::updateParam<double>(parameters, "limit.max_accel", i.limit_max_accel);
     tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", i.limit_min_accel);
     tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", i.limit_max_jerk);
     tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", i.limit_min_jerk);
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
     tier4_autoware_utils::updateParam<double>(
       parameters, "common.min_ego_accel_for_rss", i.min_ego_accel_for_rss);
     tier4_autoware_utils::updateParam<double>(
@@ -174,7 +184,10 @@ class PlannerInterface
   double min_behavior_stop_margin_;
   double nearest_dist_deviation_threshold_;
   double nearest_yaw_deviation_threshold_;
+<<<<<<< HEAD
   double obstacle_velocity_threshold_from_cruise_to_stop_;
+=======
+>>>>>>> 5687eedd6... feat(obstacle_cruise_planner): add new package (#570)
 
   // Vehicle Parameters
   vehicle_info_util::VehicleInfo vehicle_info_;