Add parameter to enable/disable obstacle detection

tier4 · Sep 18, 2024 · b552c02 · b552c02
1 parent 2914909
commit b552c02
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 2 deletions.
diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
@@ -24,3 +24,4 @@
       jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
       jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
       slow_down_vel: 1.38 # target slow down velocity [m/s]
+    enable_obstacle_detection: false # setting for obstacle detection
diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
@@ -104,6 +104,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node
     double hunting_threshold;      // keep slow down or stop state if obstacle vanished [s]
     double max_yaw_deviation_rad;  // maximum ego yaw deviation from trajectory [rad] (measures
                                    // against overlapping lanes)
+    bool enable_obstacle_detection;  // setting for obstacle detection
   };
 
   struct StopParam

diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
@@ -448,6 +448,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
     lpf_acc_ = std::make_shared<LowpassFilter1d>(0.0, p.lowpass_gain);
     const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0);
     p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad(max_yaw_deviation_deg);
+    p.enable_obstacle_detection = declare_parameter("enable_obstacle_detection", false);
   }
 
   {
@@ -753,7 +754,7 @@ void ObstacleStopPlannerNode::insertVelocity(
   const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info,
   const double current_acc, const double current_vel, const StopParam & stop_param)
 {
-  if (planner_data.stop_require && false) {
+  if (planner_data.stop_require && node_param_.enable_obstacle_detection) {
     // insert stop point
     const auto traj_end_idx = output.size() - 1;
     const auto idx = planner_data.decimate_trajectory_index_map.at(
@@ -772,7 +773,7 @@ void ObstacleStopPlannerNode::insertVelocity(
     }
   }
 
-  if (planner_data.slow_down_require && false) {
+  if (planner_data.slow_down_require && node_param_.enable_obstacle_detection) {
     // insert slow down point
     const auto traj_end_idx = output.size() - 1;
     const auto idx = planner_data.decimate_trajectory_index_map.at(