Add extra tag for moving obstacle type

Signed-off-by: Daniel Sanchez <[email protected]>
tier4 · Oct 10, 2023 · f643aa2 · f643aa2
1 parent c508aa7
commit f643aa2
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 8 deletions.
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
@@ -266,20 +266,23 @@ class PlannerInterface
         node.declare_parameter<double>("slow_down.lpf_gain_dist_to_slow_down");
     }
 
-    ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const
+    ObstacleSpecificParams getObstacleParamByLabel(
+      const ObjectClassification & label_id, const bool & is_obstacle_moving) const
     {
-      const std::string label = types_map.at(label_id.label);
-      if (obstacle_to_param_struct_map.count(label) > 0) {
-        return obstacle_to_param_struct_map.at(label);
-      }
+      std::string label("default");
+      std::string moving_prefix("");
+      if (is_obstacle_moving) moving_prefix = "moving_";
+      if (types_map.count(label_id.label) > 0) label = types_map.at(label_id.label);
+      if (obstacle_to_param_struct_map.count(moving_prefix + label) > 0)
+        return obstacle_to_param_struct_map.at(moving_prefix + label);
       return obstacle_to_param_struct_map.at("default");
     }
 
     void onParam(const std::vector<rclcpp::Parameter> & parameters)
     {
       // obstacle type dependant parameters
       for (const auto & label : obstacle_labels) {
-        auto & param_by_obstacle_label = obstacle_to_param_struct_map[label];
+        auto & param_by_obstacle_label = obstacle_to_param_struct_map.at(label);
         tier4_autoware_utils::updateParam<double>(
           parameters, "slow_down." + label + ".max_lat_margin",
           param_by_obstacle_label.max_lat_margin);

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -665,8 +665,9 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
 double PlannerInterface::calculateSlowDownVelocity(
   const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output) const
 {
-  const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification);
-
+  const bool is_obstacle_moving = [&obstacle]() { return std::abs(obstacle.velocity) > 0.5; }();
+  const auto & p =
+    slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving);
   const double stable_precise_lat_dist = [&]() {
     if (prev_output) {
       return signal_processing::lowpassFilter(