feat(obstacle_stop_planner): improve chattering prevention mechanism

planning/obstacle_stop_planner/README.md

@@ -31,11 +31,11 @@
 
 ### 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] |
+| Parameter              | Type   | Description                                                                               |
+| ---------------------- | ------ | ----------------------------------------------------------------------------------------- |
+| `enable_slow_down`     | bool   | enable slow down planner [-]                                                              |
+| `max_velocity`         | double | max velocity [m/s]                                                                        |
+| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] |
 
 ## Obstacle Stop Planner
 

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    hunting_threshold: 0.5                   # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
+    chattering_threshold: 0.5                # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s]
     max_velocity: 20.0                       # max velocity [m/s]
     enable_slow_down: False                  # whether to use slow down planner [-]
     voxel_grid_x: 0.05                       # voxel grid x parameter for filtering pointcloud [m]

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -58,6 +58,7 @@
 #include <map>
 #include <memory>
 #include <mutex>
+#include <utility>
 #include <vector>
 
 namespace motion_planning
@@ -91,6 +92,17 @@ using vehicle_info_util::VehicleInfo;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
 
+struct ObstacleWithDetectionTime
+{
+  explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p)
+  : detection_time(t), point(p)
+  {
+  }
+
+  rclcpp::Time detection_time;
+  pcl::PointXYZ point;
+};
+
 class ObstacleStopPlannerNode : public rclcpp::Node
 {
 public:
@@ -121,14 +133,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node
 
   std::unique_ptr<AdaptiveCruiseController> acc_controller_;
   std::shared_ptr<ObstacleStopPlannerDebugNode> debug_ptr_;
-  boost::optional<StopPoint> latest_stop_point_{boost::none};
   boost::optional<SlowDownSection> latest_slow_down_section_{boost::none};
+  std::vector<ObstacleWithDetectionTime> obstacle_history_{};
   tf2_ros::Buffer tf_buffer_{get_clock()};
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
   PredictedObjects::ConstSharedPtr object_ptr_{nullptr};
-  rclcpp::Time last_detect_time_collision_point_;
-  rclcpp::Time last_detect_time_slowdown_point_;
 
   Odometry::ConstSharedPtr current_velocity_ptr_{nullptr};
   AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_{nullptr};
@@ -202,6 +212,31 @@ class ObstacleStopPlannerNode : public rclcpp::Node
   void onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg);
 
   void onExpandStopRange(const ExpandStopRange::ConstSharedPtr input_msg);
+
+  void updateObstacleHistory(const rclcpp::Time & now)
+  {
+    for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) {
+      const auto expired = (now - itr->detection_time).seconds() > node_param_.chattering_threshold;
+
+      if (expired) {
+        itr = obstacle_history_.erase(itr);
+        continue;
+      }
+
+      itr++;
+    }
+  }
+
+  PointCloud::Ptr getOldPointCloudPtr() const
+  {
+    PointCloud::Ptr ret(new PointCloud);
+
+    for (const auto & p : obstacle_history_) {
+      ret->push_back(p.point);
+    }
+
+    return ret;
+  }
 };
 }  // namespace motion_planning
 

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp

@@ -63,7 +63,7 @@ struct NodeParam
   double max_velocity;
 
   // keep slow down or stop state if obstacle vanished [s]
-  double hunting_threshold;
+  double chattering_threshold;
 
   // dist threshold for ego's nearest index
   double ego_nearest_dist_threshold;

planning/obstacle_stop_planner/src/debug_marker.cpp

@@ -168,6 +168,7 @@ void ObstacleStopPlannerDebugNode::publish()
   slow_down_range_polygons_.clear();
   slow_down_polygons_.clear();
   stop_pose_ptr_ = nullptr;
+  target_stop_pose_ptr_ = nullptr;
   slow_down_start_pose_ptr_ = nullptr;
   slow_down_end_pose_ptr_ = nullptr;
   stop_obstacle_point_ptr_ = nullptr;

planning/obstacle_stop_planner/src/node.cpp

@@ -65,7 +65,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
     auto & p = node_param_;
     p.enable_slow_down = declare_parameter<bool>("enable_slow_down");
     p.max_velocity = declare_parameter<double>("max_velocity");
-    p.hunting_threshold = declare_parameter<double>("hunting_threshold");
+    p.chattering_threshold = declare_parameter<double>("chattering_threshold");
     p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
     p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
     p.voxel_grid_x = declare_parameter("voxel_grid_x", 0.05);
@@ -152,8 +152,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
   acc_controller_ = std::make_unique<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_detect_time_slowdown_point_ = this->now();
-  last_detect_time_collision_point_ = this->now();
 
   // Publishers
   pub_trajectory_ = this->create_publisher<Trajectory>("~/output/trajectory", 1);
@@ -307,10 +305,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m
     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_detect_time_slowdown_point_).seconds() >
-    node_param_.hunting_threshold;
-  if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) {
+  if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) {
     resetExternalVelocityLimit(current_acc, current_vel);
   }
 
@@ -335,6 +330,10 @@ void ObstacleStopPlannerNode::searchObstacle(
     return;
   }
 
+  const auto now = this->now();
+
+  updateObstacleHistory(now);
+
   for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) {
     // create one step circle center for vehicle
     const auto & p_front = decimate_trajectory.at(i).pose;
@@ -375,8 +374,6 @@ 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 {
@@ -395,52 +392,88 @@ void ObstacleStopPlannerNode::searchObstacle(
       PointCloud::Ptr collision_pointcloud_ptr(new PointCloud);
       collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header;
 
-      planner_data.found_collision_points = withinPolygon(
+      const auto found_collision_points = withinPolygon(
         one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point,
         next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr);
 
-      if (planner_data.found_collision_points) {
-        planner_data.decimate_trajectory_collision_index = i;
+      if (found_collision_points) {
+        pcl::PointXYZ nearest_collision_point;
+        rclcpp::Time nearest_collision_point_time;
+
         getNearestPoint(
-          *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point,
-          &planner_data.nearest_collision_point_time);
+          *collision_pointcloud_ptr, p_front, &nearest_collision_point,
+          &nearest_collision_point_time);
 
-        debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop);
-        debug_ptr_->pushPolygon(
-          one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision);
+        obstacle_history_.emplace_back(now, nearest_collision_point);
 
-        planner_data.stop_require = planner_data.found_collision_points;
+        break;
+      }
+    }
+  }
 
-        mutex_.lock();
-        const auto object_ptr = object_ptr_;
-        const auto current_velocity_ptr = current_velocity_ptr_;
-        mutex_.unlock();
+  for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) {
+    // create one step circle center for vehicle
+    const auto & p_front = decimate_trajectory.at(i).pose;
+    const auto & p_back = decimate_trajectory.at(i + 1).pose;
+    const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info);
+    const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y);
+    const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info);
+    const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y);
 
-        acc_controller_->insertAdaptiveCruiseVelocity(
-          decimate_trajectory, planner_data.decimate_trajectory_collision_index,
-          planner_data.current_pose, planner_data.nearest_collision_point,
-          planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr,
-          &planner_data.stop_require, &output, trajectory_header);
+    std::vector<cv::Point2d> one_step_move_vehicle_polygon;
+    // create one step polygon for vehicle
+    createOneStepPolygon(
+      p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.lateral_margin);
+    debug_ptr_->pushPolygon(
+      one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z,
+      PolygonType::Vehicle);
+
+    PointCloud::Ptr collision_pointcloud_ptr(new PointCloud);
+    collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header;
+
+    // check new collision points
+    planner_data.found_collision_points = withinPolygon(
+      one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point,
+      next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr);
+
+    if (planner_data.found_collision_points) {
+      planner_data.decimate_trajectory_collision_index = i;
+      getNearestPoint(
+        *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point,
+        &planner_data.nearest_collision_point_time);
+
+      debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop);
+      debug_ptr_->pushPolygon(
+        one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision);
 
-        if (planner_data.stop_require) {
-          last_detect_time_collision_point_ = trajectory_header.stamp;
-        }
+      planner_data.stop_require = planner_data.found_collision_points;
 
-        break;
+      mutex_.lock();
+      const auto object_ptr = object_ptr_;
+      const auto current_velocity_ptr = current_velocity_ptr_;
+      mutex_.unlock();
+
+      acc_controller_->insertAdaptiveCruiseVelocity(
+        decimate_trajectory, planner_data.decimate_trajectory_collision_index,
+        planner_data.current_pose, planner_data.nearest_collision_point,
+        planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr,
+        &planner_data.stop_require, &output, trajectory_header);
+
+      if (!planner_data.stop_require) {
+        obstacle_history_.clear();
       }
+
+      break;
     }
   }
 }
 
 void ObstacleStopPlannerNode::insertVelocity(
-  TrajectoryPoints & output, PlannerData & planner_data, const Header & trajectory_header,
-  const VehicleInfo & vehicle_info, const double current_acc, const double current_vel,
-  const StopParam & stop_param)
+  TrajectoryPoints & output, PlannerData & planner_data,
+  [[maybe_unused]] const Header & trajectory_header, const VehicleInfo & vehicle_info,
+  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
@@ -502,36 +535,20 @@ 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::TargetStop);
           debug_ptr_->pushPose(getPose(current_stop_pos.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::TargetStop);
         debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop);
       }
     }
-  } else if (!no_hunting_collision_point) {
-    if (latest_stop_point_) {
-      // update stop point index with the current trajectory
-      latest_stop_point_.get().index = findFirstNearestSegmentIndexWithSoftConstraints(
-        output, getPose(latest_stop_point_.get().point), node_param_.ego_nearest_dist_threshold,
-        node_param_.ego_nearest_yaw_threshold);
-      insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag);
-      debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::TargetStop);
-      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;
@@ -556,19 +573,14 @@ 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) ||
-        !no_hunting_slowdown_point) {
+        !latest_slow_down_section_ &&
+        dist_baselink_to_obstacle + index_with_dist_remain.get().second <
+          vehicle_info.max_longitudinal_offset_m) {
         latest_slow_down_section_ = slow_down_section;
       }
 
       insertSlowDownSection(slow_down_section, output);
     }
-  } 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_) {
@@ -604,7 +616,7 @@ void ObstacleStopPlannerNode::insertVelocity(
         set_velocity_limit_ ? std::numeric_limits<double>::max() : target_velocity;
 
       insertSlowDownSection(slow_down_section, output);
-    } else if (no_hunting_slowdown_point) {
+    } else {
       latest_slow_down_section_ = {};
     }
   }