feat(behavior_velocity_run_out): cherry pick feature improvements

planning/behavior_velocity_run_out_module/README.md

@@ -188,15 +188,17 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
 | `margin_ahead`            | double | [m] ahead margin for detection area polygon  |
 | `margin_behind`           | double | [m] behind margin for detection area polygon |
 
-| Parameter /dynamic_obstacle | Type   | Description                                                                                                                   |
-| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- |
-| `min_vel_kmph`              | double | [km/h] minimum velocity for dynamic obstacles                                                                                 |
-| `max_vel_kmph`              | double | [km/h] maximum velocity for dynamic obstacles                                                                                 |
-| `diameter`                  | double | [m] diameter of obstacles. used for creating dynamic obstacles from points                                                    |
-| `height`                    | double | [m] height of obstacles. used for creating dynamic obstacles from points                                                      |
-| `max_prediction_time`       | double | [sec] create predicted path until this time                                                                                   |
-| `time_step`                 | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path                   |
-| `points_interval`           | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method |
+| Parameter /dynamic_obstacle          | Type   | Description                                                                                                                   |
+| ------------------------------------ | ------ | ----------------------------------------------------------------------------------------------------------------------------- |
+| `use_mandatory_area`                 | double | [-] whether to use mandatory detection area                                                                                   |
+| `assume_fixed_velocity.enable`       | double | [-] If enabled, the obstacle's velocity is assumed to be within the minimum and maximum velocity values specified below       |
+| `assume_fixed_velocity.min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles                                                                                 |
+| `assume_fixed_velocity.max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles                                                                                 |
+| `diameter`                           | double | [m] diameter of obstacles. used for creating dynamic obstacles from points                                                    |
+| `height`                             | double | [m] height of obstacles. used for creating dynamic obstacles from points                                                      |
+| `max_prediction_time`                | double | [sec] create predicted path until this time                                                                                   |
+| `time_step`                          | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path                   |
+| `points_interval`                    | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method |
 
 | Parameter /approaching | Type   | Description                                           |
 | ---------------------- | ------ | ----------------------------------------------------- |

planning/behavior_velocity_run_out_module/config/run_out.param.yaml

@@ -3,6 +3,7 @@
     run_out:
       detection_method: "Object"  # [-] candidate: Object, ObjectWithoutPath, Points
       use_partition_lanelet: true # [-] whether to use partition lanelet map data
+      suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet:
       specify_decel_jerk: false   # [-] whether to specify jerk when ego decelerates
       stop_margin: 2.5            # [m] the vehicle decelerates to be able to stop with this margin
       passing_margin: 1.1         # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin
@@ -21,14 +22,17 @@
 
       # parameter to create abstracted dynamic obstacles
       dynamic_obstacle:
-        use_mandatory_area: false  # [-] whether to use mandatory detection area
-        min_vel_kmph: 0.0          # [km/h] minimum velocity for dynamic obstacles
-        max_vel_kmph: 5.0          # [km/h] maximum velocity for dynamic obstacles
-        diameter: 0.1              # [m] diameter of obstacles. used for creating dynamic obstacles from points
-        height: 2.0                # [m] height of obstacles. used for creating dynamic obstacles from points
-        max_prediction_time: 10.0  # [sec] create predicted path until this time
-        time_step: 0.5             # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
-        points_interval: 0.1       # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
+        use_mandatory_area: false # [-] whether to use mandatory detection area
+        assume_fixed_velocity:
+          enable: false           # [-] If enabled, the obstacle's velocity is assumed to be within the minimum and maximum velocity values specified below
+          min_vel_kmph: 0.0       # [km/h] minimum velocity for dynamic obstacles
+          max_vel_kmph: 5.0       # [km/h] maximum velocity for dynamic obstacles
+        std_dev_multiplier: 1.96  # [-] min and max velocity of the obstacles are calculated from this value and covariance
+        diameter: 0.1             # [m] diameter of obstacles. used for creating dynamic obstacles from points
+        height: 2.0               # [m] height of obstacles. used for creating dynamic obstacles from points
+        max_prediction_time: 10.0 # [sec] create predicted path until this time
+        time_step: 0.5            # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
+        points_interval: 0.1      # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
 
       # approach if ego has stopped in the front of the obstacle for a certain amount of time
       approaching:

planning/behavior_velocity_run_out_module/package.xml

@@ -19,6 +19,7 @@
 
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>behavior_velocity_crosswalk_module</depend>
   <depend>behavior_velocity_planner_common</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>

planning/behavior_velocity_run_out_module/src/debug.hpp

@@ -33,14 +33,17 @@ class DebugValues
 public:
   enum class TYPE {
     CALCULATION_TIME = 0,
-    CALCULATION_TIME_COLLISION_CHECK = 1,
-    LATERAL_DIST = 2,
-    LONGITUDINAL_DIST_OBSTACLE = 3,
-    LONGITUDINAL_DIST_COLLISION = 4,
-    COLLISION_POS_FROM_EGO_FRONT = 5,
-    STOP_DISTANCE = 6,
-    NUM_OBSTACLES = 7,
-    LATERAL_PASS_DIST = 8,
+    CALCULATION_TIME_PATH_PROCESSING = 1,
+    CALCULATION_TIME_OBSTACLE_CREATION = 2,
+    CALCULATION_TIME_COLLISION_CHECK = 3,
+    CALCULATION_TIME_PATH_PLANNING = 4,
+    LATERAL_DIST = 5,
+    LONGITUDINAL_DIST_OBSTACLE = 6,
+    LONGITUDINAL_DIST_COLLISION = 7,
+    COLLISION_POS_FROM_EGO_FRONT = 8,
+    STOP_DISTANCE = 9,
+    NUM_OBSTACLES = 10,
+    LATERAL_PASS_DIST = 11,
     SIZE,  // this is the number of enum elements
   };
 

planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp

@@ -278,6 +278,31 @@ PointCloud2 concatPointCloud(
   return concat_points;
 }
 
+void calculateMinAndMaxVelFromCovariance(
+  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
+  const double std_dev_multiplier, run_out_utils::DynamicObstacle & dynamic_obstacle)
+{
+  const double x_velocity = std::abs(twist_with_covariance.twist.linear.x);
+  const double y_velocity = std::abs(twist_with_covariance.twist.linear.y);
+  const double x_variance = twist_with_covariance.covariance.at(0);
+  const double y_variance = twist_with_covariance.covariance.at(7);
+  const double x_std_dev = std::sqrt(x_variance);
+  const double y_std_dev = std::sqrt(y_variance);
+
+  // calculate the min and max velocity using the standard deviation of twist
+  // note that this assumes the covariance of x and y is zero
+  const double min_x = std::max(0.0, x_velocity - std_dev_multiplier * x_std_dev);
+  const double min_y = std::max(0.0, y_velocity - std_dev_multiplier * y_std_dev);
+  const double min_velocity = std::hypot(min_x, min_y);
+
+  const double max_x = x_velocity + std_dev_multiplier * x_std_dev;
+  const double max_y = y_velocity + std_dev_multiplier * y_std_dev;
+  const double max_velocity = std::hypot(max_x, max_y);
+
+  dynamic_obstacle.min_velocity_mps = min_velocity;
+  dynamic_obstacle.max_velocity_mps = max_velocity;
+}
+
 }  // namespace
 
 DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(
@@ -294,9 +319,14 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObsta
     DynamicObstacle dynamic_obstacle;
     dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
 
-    // TODO(Tomohito Ando): calculate velocity from covariance of predicted_object
-    dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
-    dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
+    if (param_.assume_fixed_velocity) {
+      dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph);
+      dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph);
+    } else {
+      calculateMinAndMaxVelFromCovariance(
+        predicted_object.kinematics.initial_twist_with_covariance, param_.std_dev_multiplier,
+        dynamic_obstacle);
+    }
     dynamic_obstacle.classifications = predicted_object.classification;
     dynamic_obstacle.shape = predicted_object.shape;
 

planning/behavior_velocity_run_out_module/src/manager.cpp

@@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     auto & p = planner_param_.run_out;
     p.detection_method = getOrDeclareParameter<std::string>(node, ns + ".detection_method");
     p.use_partition_lanelet = getOrDeclareParameter<bool>(node, ns + ".use_partition_lanelet");
+    p.suppress_on_crosswalk = getOrDeclareParameter<bool>(node, ns + ".suppress_on_crosswalk");
     p.specify_decel_jerk = getOrDeclareParameter<bool>(node, ns + ".specify_decel_jerk");
     p.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
     p.passing_margin = getOrDeclareParameter<double>(node, ns + ".passing_margin");
@@ -84,8 +85,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     auto & p = planner_param_.dynamic_obstacle;
     const std::string ns_do = ns + ".dynamic_obstacle";
     p.use_mandatory_area = getOrDeclareParameter<bool>(node, ns_do + ".use_mandatory_area");
-    p.min_vel_kmph = getOrDeclareParameter<double>(node, ns_do + ".min_vel_kmph");
-    p.max_vel_kmph = getOrDeclareParameter<double>(node, ns_do + ".max_vel_kmph");
+    p.assume_fixed_velocity =
+      getOrDeclareParameter<bool>(node, ns_do + ".assume_fixed_velocity.enable");
+    p.min_vel_kmph =
+      getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.min_vel_kmph");
+    p.max_vel_kmph =
+      getOrDeclareParameter<double>(node, ns_do + ".assume_fixed_velocity.max_vel_kmph");
+    p.std_dev_multiplier = getOrDeclareParameter<double>(node, ns_do + ".std_dev_multiplier");
     p.diameter = getOrDeclareParameter<double>(node, ns_do + ".diameter");
     p.height = getOrDeclareParameter<double>(node, ns_do + ".height");
     p.max_prediction_time = getOrDeclareParameter<double>(node, ns_do + ".max_prediction_time");