From f5730b8db4c8d4ce9d681f13489c80144de9c978 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 6 Sep 2022 09:20:40 +0900 Subject: [PATCH] refactor(obstacle_stop_planner): add params to config Signed-off-by: satoshi-ota --- .../obstacle_stop_planner/obstacle_stop_planner.param.yaml | 6 ++++-- .../config/obstacle_stop_planner.param.yaml | 6 ++++-- planning/obstacle_stop_planner/src/node.cpp | 4 ++-- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml index 9c96fb76fd69e..cf0ed41808395 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml @@ -13,7 +13,7 @@ detection_area: lateral_margin: 0.0 # margin of vehicle footprint [m] step_length: 1.0 # step length for pointcloud search range [m] - extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] slow_down_planner: @@ -41,4 +41,6 @@ jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] # others - consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] + velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] + acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index 6a1b7564dad0b..917f2e6c7e056 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -13,7 +13,7 @@ detection_area: lateral_margin: 0.0 # margin of vehicle footprint [m] step_length: 1.0 # step length for pointcloud search range [m] - extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] slow_down_planner: @@ -41,4 +41,6 @@ jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] # others - consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] + velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] + acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index b373d4652ecb5..e722f6017af19 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -525,9 +525,9 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.jerk_span = declare_parameter(ns + "constraints.jerk_span"); p.velocity_threshold_decel_complete = - declare_parameter(ns + "velocity_threshold_decel_complete", 0.2); + declare_parameter(ns + "velocity_threshold_decel_complete"); p.acceleration_threshold_decel_complete = - declare_parameter(ns + "acceleration_threshold_decel_complete", 0.1); + declare_parameter(ns + "acceleration_threshold_decel_complete"); // apply offset p.longitudinal_forward_margin += i.max_longitudinal_offset_m;