feat(obstacle_stop_planner): prevent from obstacle hunting (#1458)

* feat(obstacle_stop_planner): prevent from obstacle hunting

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* docs(obstacle_stop_planner): add common parameter description

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(obstacle_stop_planner): fix bug

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/obstacle_stop_planner/README.md

@@ -46,6 +46,16 @@ When the deceleration section is inserted, the start point of the section is ins
 
 ## Modules
 
+### Common Parameter
+
+| Parameter               | Type   | Description                                                                              |
+| ----------------------- | ------ | ---------------------------------------------------------------------------------------- |
+| `enable_slow_down`      | bool   | enable slow down planner [-]                                                             |
+| `max_velocity`          | double | max velocity [m/s]                                                                       |
+| `hunting_threshold`     | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
+| `lowpass_gain`          | double | low pass gain for calculating acceleration [-]                                           |
+| `max_yaw_deviation_deg` | double | # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes)   |
+
 ### Obstacle Stop Planner
 
 #### Role

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -1,5 +1,7 @@
 /**:
   ros__parameters:
+    hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
+
     stop_planner:
       stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
       min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m]

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -185,12 +185,14 @@ class ObstacleStopPlannerNode : public rclcpp::Node
   std::unique_ptr<motion_planning::AdaptiveCruiseController> acc_controller_;
   std::shared_ptr<ObstacleStopPlannerDebugNode> debug_ptr_;
   std::shared_ptr<LowpassFilter1d> lpf_acc_{nullptr};
-  boost::optional<SlowDownSection> latest_slow_down_section_{};
+  boost::optional<StopPoint> latest_stop_point_{boost::none};
+  boost::optional<SlowDownSection> latest_slow_down_section_{boost::none};
   tf2_ros::Buffer tf_buffer_{get_clock()};
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
   sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
   PredictedObjects::ConstSharedPtr object_ptr_{nullptr};
-  rclcpp::Time last_detection_time_;
+  rclcpp::Time last_detect_time_collision_point_;
+  rclcpp::Time last_detect_time_slowdown_point_;
 
   nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr};
   nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr};

planning/obstacle_stop_planner/src/node.cpp

@@ -515,7 +515,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
   acc_controller_ = std::make_unique<motion_planning::AdaptiveCruiseController>(
     this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m);
   debug_ptr_ = std::make_shared<ObstacleStopPlannerDebugNode>(this, i.max_longitudinal_offset_m);
-  last_detection_time_ = this->now();
+  last_detect_time_slowdown_point_ = this->now();
+  last_detect_time_collision_point_ = this->now();
 
   // Publishers
   path_pub_ = this->create_publisher<Trajectory>("~/output/trajectory", 1);
@@ -652,8 +653,9 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu
     current_vel, stop_param);
 
   const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_;
-  const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() >
-                          node_param_.hunting_threshold;
+  const auto no_hunting =
+    (rclcpp::Time(input_msg->header.stamp) - last_detect_time_slowdown_point_).seconds() >
+    node_param_.hunting_threshold;
   if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) {
     resetExternalVelocityLimit(current_acc, current_vel);
   }
@@ -721,6 +723,8 @@ void ObstacleStopPlannerNode::searchObstacle(
         debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown);
         debug_ptr_->pushPolygon(
           one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown);
+
+        last_detect_time_slowdown_point_ = trajectory_header.stamp;
       }
 
     } else {
@@ -767,6 +771,10 @@ void ObstacleStopPlannerNode::searchObstacle(
           planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr,
           &planner_data.stop_require, &output, trajectory_header);
 
+        if (planner_data.stop_require) {
+          last_detect_time_collision_point_ = trajectory_header.stamp;
+        }
+
         break;
       }
     }
@@ -779,6 +787,9 @@ void ObstacleStopPlannerNode::insertVelocity(
   const double current_acc, const double current_vel, const StopParam & stop_param)
 {
   const auto & base_link2front = vehicle_info.max_longitudinal_offset_m;
+  const auto no_hunting_collision_point =
+    (rclcpp::Time(trajectory_header.stamp) - last_detect_time_collision_point_).seconds() >
+    node_param_.hunting_threshold;
 
   if (planner_data.stop_require) {
     // insert stop point
@@ -818,18 +829,29 @@ void ObstacleStopPlannerNode::insertVelocity(
           current_stop_pos.point.pose = ego_pos_on_path.get();
 
           insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag);
+          latest_stop_point_ = current_stop_pos;
 
           debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop);
         }
 
       } else {
         insertStopPoint(stop_point, output, planner_data.stop_reason_diag);
+        latest_stop_point_ = stop_point;
 
         debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop);
       }
     }
+  } else if (!no_hunting_collision_point) {
+    if (latest_stop_point_) {
+      insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag);
+      debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::Stop);
+    }
   }
 
+  const auto no_hunting_slowdown_point =
+    (rclcpp::Time(trajectory_header.stamp) - last_detect_time_slowdown_point_).seconds() >
+    node_param_.hunting_threshold;
+
   if (planner_data.slow_down_require) {
     // insert slow down point
     const auto traj_end_idx = output.size() - 1;
@@ -854,16 +876,19 @@ void ObstacleStopPlannerNode::insertVelocity(
         current_vel);
 
       if (
-        !latest_slow_down_section_ &&
-        dist_baselink_to_obstacle + index_with_dist_remain.get().second <
-          vehicle_info.max_longitudinal_offset_m) {
+        (!latest_slow_down_section_ &&
+         dist_baselink_to_obstacle + index_with_dist_remain.get().second <
+           vehicle_info.max_longitudinal_offset_m) ||
+        !no_hunting_slowdown_point) {
         latest_slow_down_section_ = slow_down_section;
       }
 
       insertSlowDownSection(slow_down_section, output);
     }
-
-    last_detection_time_ = trajectory_header.stamp;
+  } else if (!no_hunting_slowdown_point) {
+    if (latest_slow_down_section_) {
+      insertSlowDownSection(latest_slow_down_section_.get(), output);
+    }
   }
 
   if (node_param_.enable_slow_down && latest_slow_down_section_) {
@@ -889,7 +914,7 @@ void ObstacleStopPlannerNode::insertVelocity(
         set_velocity_limit_ ? std::numeric_limits<double>::max() : slow_down_param_.slow_down_vel;
 
       insertSlowDownSection(slow_down_section, output);
-    } else {
+    } else if (no_hunting_slowdown_point) {
       latest_slow_down_section_ = {};
     }
   }