Merge branch 'main' into refactor/start_planner_post_process

Signed-off-by: kyoichi-sugahara <[email protected]>

planning/behavior_velocity_run_out_module/README.md

@@ -217,6 +217,11 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
 | `max_jerk`                 | double | [m/s^3] minimum jerk deceleration for safe brake.             |
 | `max_acc`                  | double | [m/s^2] minimum accel deceleration for safe brake.            |
 
+| Parameter /ignore_momentary_detection | Type   | Description                                                       |
+| ------------------------------------- | ------ | ----------------------------------------------------------------- |
+| `enable`                              | bool   | [-] whether to ignore momentary detection                         |
+| `time_threshold`                      | double | [sec] ignores detections that persist for less than this duration |
+
 ### Future extensions / Unimplemented parts
 
 - Calculate obstacle's min velocity and max velocity from covariance

planning/behavior_velocity_run_out_module/config/run_out.param.yaml

@@ -48,3 +48,8 @@
         enable: true
         max_jerk: -0.7  # [m/s^3] minimum jerk deceleration for safe brake.
         max_acc : -2.0  # [m/s^2] minimum accel deceleration for safe brake.
+
+      # prevent abrupt stops caused by false positives in perception
+      ignore_momentary_detection:
+        enable: true
+        time_threshold: 0.15  # [sec] ignores detections that persist for less than this duration

planning/behavior_velocity_run_out_module/src/manager.cpp

@@ -119,6 +119,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     p.max_acc = getOrDeclareParameter<double>(node, ns_m + ".max_acc");
   }
 
+  {
+    auto & p = planner_param_.ignore_momentary_detection;
+    const std::string ns_param = ns + ".ignore_momentary_detection";
+    p.enable = getOrDeclareParameter<bool>(node, ns_param + ".enable");
+    p.time_threshold = getOrDeclareParameter<double>(node, ns_param + ".time_threshold");
+  }
+
   debug_ptr_ = std::make_shared<RunOutDebug>(node);
   setDynamicObstacleCreator(node, debug_ptr_);
 }

planning/behavior_velocity_run_out_module/src/scene.cpp

@@ -137,7 +137,7 @@ bool RunOutModule::modifyPathVelocity(
 }
 
 boost::optional<DynamicObstacle> RunOutModule::detectCollision(
-  const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path) const
+  const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path)
 {
   if (path.points.size() < 2) {
     RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points.");
@@ -181,13 +181,19 @@ boost::optional<DynamicObstacle> RunOutModule::detectCollision(
       continue;
     }
 
+    // ignore momentary obstacle detection to avoid sudden stopping by false positive
+    if (isMomentaryDetection()) {
+      return {};
+    }
+
     debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points);
     debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point);
 
     return obstacle_selected;
   }
 
   // no collision detected
+  first_detected_time_.reset();
   return {};
 }
 
@@ -812,4 +818,21 @@ void RunOutModule::publishDebugValue(
   debug_ptr_->publishDebugValue();
 }
 
+bool RunOutModule::isMomentaryDetection()
+{
+  if (!planner_param_.ignore_momentary_detection.enable) {
+    return false;
+  }
+
+  if (!first_detected_time_) {
+    first_detected_time_ = std::make_shared<rclcpp::Time>(clock_->now());
+    return true;
+  }
+
+  const auto now = clock_->now();
+  const double elapsed_time_since_detection = (now - *first_detected_time_).seconds();
+  RCLCPP_DEBUG_STREAM(logger_, "elapsed_time_since_detection: " << elapsed_time_since_detection);
+
+  return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold;
+}
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_run_out_module/src/scene.hpp

@@ -62,13 +62,13 @@ class RunOutModule : public SceneModuleInterface
   std::unique_ptr<DynamicObstacleCreator> dynamic_obstacle_creator_;
   std::shared_ptr<RunOutDebug> debug_ptr_;
   std::unique_ptr<run_out_utils::StateMachine> state_machine_;
+  std::shared_ptr<rclcpp::Time> first_detected_time_;
 
   // Function
   Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const;
 
   boost::optional<DynamicObstacle> detectCollision(
-    const std::vector<DynamicObstacle> & dynamic_obstacles,
-    const PathWithLaneId & path_points) const;
+    const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path_points);
 
   float calcCollisionPositionOfVehicleSide(
     const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const;
@@ -141,6 +141,8 @@ class RunOutModule : public SceneModuleInterface
     const PathWithLaneId & path, const std::vector<DynamicObstacle> extracted_obstacles,
     const boost::optional<DynamicObstacle> & dynamic_obstacle,
     const geometry_msgs::msg::Pose & current_pose) const;
+
+  bool isMomentaryDetection();
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_run_out_module/src/utils.hpp

@@ -126,6 +126,12 @@ struct DynamicObstacleParam
   float points_interval;      // [m]
 };
 
+struct IgnoreMomentaryDetection
+{
+  bool enable;
+  double time_threshold;
+};
+
 struct PlannerParam
 {
   CommonParam common;
@@ -138,6 +144,7 @@ struct PlannerParam
   DynamicObstacleParam dynamic_obstacle;
   SlowDownLimit slow_down_limit;
   Smoother smoother;
+  IgnoreMomentaryDetection ignore_momentary_detection;
 };
 
 enum class DetectionMethod {

sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml

@@ -1,14 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="input_imu_raw" default="imu_raw"/>
-  <arg name="input_pose" default="pose"/>
+  <arg name="input_odom" default="odom"/>
   <arg name="output_gyro_bias" default="gyro_bias"/>
   <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
   <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
 
   <node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
     <remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
-    <remap from="~/input/pose" to="$(var input_pose)"/>
+    <remap from="~/input/odom" to="$(var input_odom)"/>
     <remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>
     <param from="$(var gyro_bias_estimator_param_file)"/>
     <param from="$(var imu_corrector_param_file)"/>

sensing/imu_corrector/src/gyro_bias_estimator.cpp

@@ -43,9 +43,9 @@ GyroBiasEstimator::GyroBiasEstimator()
   imu_sub_ = create_subscription<Imu>(
     "~/input/imu_raw", rclcpp::SensorDataQoS(),
     [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); });
-  pose_sub_ = create_subscription<PoseWithCovarianceStamped>(
-    "~/input/pose", rclcpp::SensorDataQoS(),
-    [this](const PoseWithCovarianceStamped::ConstSharedPtr msg) { callback_pose(msg); });
+  odom_sub_ = create_subscription<Odometry>(
+    "~/input/odom", rclcpp::SensorDataQoS(),
+    [this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); });
   gyro_bias_pub_ = create_publisher<Vector3Stamped>("~/output/gyro_bias", rclcpp::SensorDataQoS());
 
   auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this);
@@ -88,12 +88,11 @@ void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr)
   }
 }
 
-void GyroBiasEstimator::callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr)
+void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr)
 {
-  // push pose_msg to queue
   geometry_msgs::msg::PoseStamped pose;
-  pose.header = pose_msg_ptr->header;
-  pose.pose = pose_msg_ptr->pose.pose;
+  pose.header = odom_msg_ptr->header;
+  pose.pose = odom_msg_ptr->pose.pose;
   pose_buf_.push_back(pose);
 }
 

sensing/imu_corrector/src/gyro_bias_estimator.hpp

@@ -22,6 +22,7 @@
 
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 
 #include <memory>
@@ -38,14 +39,15 @@ class GyroBiasEstimator : public rclcpp::Node
   using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
   using Vector3Stamped = geometry_msgs::msg::Vector3Stamped;
   using Vector3 = geometry_msgs::msg::Vector3;
+  using Odometry = nav_msgs::msg::Odometry;
 
 public:
   GyroBiasEstimator();
 
 private:
   void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
   void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr);
-  void callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr);
+  void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr);
   void timer_callback();
 
   static geometry_msgs::msg::Vector3 transform_vector3(
@@ -55,7 +57,7 @@ class GyroBiasEstimator : public rclcpp::Node
   const std::string output_frame_ = "base_link";
 
   rclcpp::Subscription<Imu>::SharedPtr imu_sub_;
-  rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr pose_sub_;
+  rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
   rclcpp::Publisher<Vector3Stamped>::SharedPtr gyro_bias_pub_;
   rclcpp::TimerBase::SharedPtr timer_;