chore(behavior_velocity): parameterlize detection area length (tier4#…

…1346) Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
boyali · Oct 3, 2022 · 652f8d3 · 652f8d3
1 parent 3558ec7
commit 652f8d3
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 2 deletions.
diff --git a/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
@@ -115,6 +115,7 @@ struct PlannerParam
   bool use_moving_object_ray_cast;  // [-]
   bool use_partition_lanelet;       // [-]
   // parameters in yaml
+  double detection_area_offset;      // [m]
   double detection_area_length;      // [m]
   double detection_area_max_length;  // [m]
   double stuck_vehicle_vel;          // [m/s]

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp
@@ -70,6 +70,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
   pp.is_show_processing_time = node.declare_parameter(ns + ".debug.is_show_processing_time", false);
 
   // threshold
+  pp.detection_area_offset = node.declare_parameter(ns + ".threshold.detection_area_offset", 30.0);
   pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0);
   pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel", 1.0);
   pp.lateral_distance_thr = node.declare_parameter(ns + ".threshold.lateral_distance", 10.0);

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
@@ -93,12 +93,11 @@ bool OcclusionSpotModule::modifyPathVelocity(
     param_.v.v_ego = planner_data_->current_velocity->twist.linear.x;
     param_.v.a_ego = planner_data_->current_accel.get();
     param_.v.delay_time = planner_data_->system_delay;
-    const double detection_area_offset = 5.0;  // for visualization and stability
     param_.detection_area_max_length =
       planning_utils::calcJudgeLineDistWithJerkLimit(
         param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_accel, param_.v.non_effective_jerk,
         planner_data_->delay_response_time) +
-      detection_area_offset;
+      param_.detection_area_offset;  // To fill difference between planned and measured acc
   }
   const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose;
   PathWithLaneId clipped_path;