feat(run_out): add motorcyles to run out module target obstacles (aut…

…owarefoundation#6690) * feat(run_out): add motorcyles to run out module target obstacles Signed-off-by: Daniel Sanchez <[email protected]> * check for obstacle type depending on params Signed-off-by: Daniel Sanchez <[email protected]> * update readme Signed-off-by: Daniel Sanchez <[email protected]> * pre-commit Signed-off-by: Daniel Sanchez <[email protected]> * include what you use Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
TaikiYamada4 · Mar 27, 2024 · e84aaee · e84aaee
1 parent a0bac37
commit e84aaee
Show file tree

Hide file tree

Showing 7 changed files with 28 additions and 19 deletions.
diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md
@@ -2,7 +2,7 @@
 
 ### Role
 
-`run_out` is the module that decelerates and stops for dynamic obstacles such as pedestrians and bicycles.
+`run_out` is the module that decelerates and stops for dynamic obstacles such as pedestrians, bicycles and motorcycles.
 
 ![brief](./docs/run_out_overview.svg)
 
@@ -179,17 +179,18 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
 
 ### Module Parameters
 
-| Parameter               | Type   | Description                                                                                                              |
-| ----------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------ |
-| `detection_method`      | string | [-] candidate: Object, ObjectWithoutPath, Points                                                                         |
-| `use_partition_lanelet` | bool   | [-] whether to use partition lanelet map data                                                                            |
-| `specify_decel_jerk`    | bool   | [-] whether to specify jerk when ego decelerates                                                                         |
-| `stop_margin`           | double | [m] the vehicle decelerates to be able to stop with this margin                                                          |
-| `passing_margin`        | double | [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin |
-| `deceleration_jerk`     | double | [m/s^3] ego decelerates with this jerk when stopping for obstacles                                                       |
-| `detection_distance`    | double | [m] ahead distance from ego to detect the obstacles                                                                      |
-| `detection_span`        | double | [m] calculate collision with this span to reduce calculation time                                                        |
-| `min_vel_ego_kmph`      | double | [km/h] min velocity to calculate time to collision                                                                       |
+| Parameter               | Type             | Description                                                                                                                                                                                          |
+| ----------------------- | ---------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `detection_method`      | string           | [-] candidate: Object, ObjectWithoutPath, Points                                                                                                                                                     |
+| `target_obstacle_types` | vector of string | [-] specifies which obstacle types will be considered by the module, if the obstacles classification type is not written here, it will be ignored. candidate: ["PEDESTRIAN", "BICYCLE","MOTORCYCLE"] |
+| `use_partition_lanelet` | bool             | [-] whether to use partition lanelet map data                                                                                                                                                        |
+| `specify_decel_jerk`    | bool             | [-] whether to specify jerk when ego decelerates                                                                                                                                                     |
+| `stop_margin`           | double           | [m] the vehicle decelerates to be able to stop with this margin                                                                                                                                      |
+| `passing_margin`        | double           | [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin                                                                             |
+| `deceleration_jerk`     | double           | [m/s^3] ego decelerates with this jerk when stopping for obstacles                                                                                                                                   |
+| `detection_distance`    | double           | [m] ahead distance from ego to detect the obstacles                                                                                                                                                  |
+| `detection_span`        | double           | [m] calculate collision with this span to reduce calculation time                                                                                                                                    |
+| `min_vel_ego_kmph`      | double           | [km/h] min velocity to calculate time to collision                                                                                                                                                   |
 
 | Parameter /detection_area | Type   | Description                                  |
 | ------------------------- | ------ | -------------------------------------------- |

diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml
@@ -2,6 +2,7 @@
   ros__parameters:
     run_out:
       detection_method: "Object"  # [-] candidate: Object, ObjectWithoutPath, Points
+      target_obstacle_types: ["PEDESTRIAN", "BICYCLE", "MOTORCYCLE"] # [-] Obstacle types considered by this module
       use_partition_lanelet: true # [-] whether to use partition lanelet map data
       suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet
       use_ego_cut_line: true      # [-] filter obstacles that pass the backside of ego: if a dynamic obstacle's predicted path intersects this line, it is ignored

diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml
@@ -28,6 +28,7 @@
   <depend>libboost-dev</depend>
   <depend>message_filters</depend>
   <depend>motion_utils</depend>
+  <depend>object_recognition_utils</depend>
   <depend>pcl_conversions</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp
@@ -18,6 +18,7 @@
 
 #include <string>
 #include <utility>
+#include <vector>
 
 namespace behavior_velocity_planner
 {
@@ -57,6 +58,8 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
   {
     auto & p = planner_param_.run_out;
     p.detection_method = getOrDeclareParameter<std::string>(node, ns + ".detection_method");
+    p.target_obstacle_types =
+      getOrDeclareParameter<std::vector<std::string>>(node, ns + ".target_obstacle_types");
     p.use_partition_lanelet = getOrDeclareParameter<bool>(node, ns + ".use_partition_lanelet");
     p.use_ego_cut_line = getOrDeclareParameter<bool>(node, ns + ".use_ego_cut_line");
     p.suppress_on_crosswalk = getOrDeclareParameter<bool>(node, ns + ".suppress_on_crosswalk");

diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp
@@ -332,21 +332,22 @@ std::vector<DynamicObstacle> RunOutModule::checkCollisionWithObstacles(
   std::vector<geometry_msgs::msg::Point> poly, const float travel_time,
   const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets) const
 {
+  using object_recognition_utils::convertLabelToString;
   const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly);
 
   // check collision for each objects
   std::vector<DynamicObstacle> obstacles_collision;
   for (const auto & obstacle : dynamic_obstacles) {
     // get classification that has highest probability
-    const auto classification = run_out_utils::getHighestProbLabel(obstacle.classifications);
-
-    // detect only pedestrian and bicycle
-    if (
-      classification != ObjectClassification::PEDESTRIAN &&
-      classification != ObjectClassification::BICYCLE) {
+    const auto classification =
+      convertLabelToString(run_out_utils::getHighestProbLabel(obstacle.classifications));
+    const auto & target_obstacle_types = planner_param_.run_out.target_obstacle_types;
+    // detect only pedestrians, bicycles or motorcycles
+    const auto it =
+      std::find(target_obstacle_types.begin(), target_obstacle_types.end(), classification);
+    if (it == target_obstacle_types.end()) {
       continue;
     }
-
     // calculate predicted obstacle pose for min velocity and max velocity
     const auto predicted_obstacle_pose_min_vel =
       calcPredictedObstaclePose(obstacle.predicted_paths, travel_time, obstacle.min_velocity_mps);

diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp
@@ -17,6 +17,7 @@
 
 #include "debug.hpp"
 #include "dynamic_obstacle.hpp"
+#include "object_recognition_utils/object_recognition_utils.hpp"
 #include "state_machine.hpp"
 #include "utils.hpp"
 

diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp
@@ -55,6 +55,7 @@ struct CommonParam
 struct RunOutParam
 {
   std::string detection_method;
+  std::vector<std::string> target_obstacle_types;
   bool use_partition_lanelet;
   bool suppress_on_crosswalk;
   bool specify_decel_jerk;