From 7fbeb1c8e4aecbd80c2945a807a7acd190a66a8d Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 29 Aug 2022 16:07:18 +0900 Subject: [PATCH] refactor(obstacle_stop_planner): update parameter name for readability Signed-off-by: satoshi-ota --- .../config/obstacle_stop_planner.param.yaml | 51 ++-- .../include/obstacle_stop_planner/node.hpp | 87 +------ .../obstacle_stop_planner/planner_data.hpp | 233 ++++++++++++++++++ planning/obstacle_stop_planner/src/node.cpp | 114 ++++----- 4 files changed, 323 insertions(+), 162 deletions(-) create mode 100644 planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp 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 6fe30b216118e..b27e17324f6b1 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,28 +1,37 @@ /**: ros__parameters: - hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] stop_planner: - stop_margin: 5.0 # stop margin distance from obstacle on the path [m] - min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [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 - expand_stop_range: 0.0 # margin of vehicle footprint [m] - hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] + # params for margin + lateral_margin: 0.0 # margin of vehicle footprint [m] + max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_longitudinal_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m] + + # params for trajectory pre-process + 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 + + # others + hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] slow_down_planner: - # slow down planner parameters - forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] - backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m] - expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] - max_slow_down_vel: 1.38 # max slow down velocity [m/s] - min_slow_down_vel: 0.28 # min slow down velocity [m/s] + # params for margin + lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] + longitudinal_backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m] + longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s] + + # params for velocity + max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s] + min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s] + slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s] + + # params for deceleration constraints (use this param if consider_constraints is True) + jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss] + jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] + jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] - # slow down constraint parameters - consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel - forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s] - forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s] - jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss] - jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] - jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] - slow_down_vel: 1.38 # target slow down velocity [m/s] + # others + consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 7d63be90db608..1294e6dccd5a1 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -17,6 +17,7 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" #include "obstacle_stop_planner/debug_marker.hpp" +#include "obstacle_stop_planner/planner_data.hpp" #include #include @@ -76,97 +77,11 @@ using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using vehicle_info_util::VehicleInfo; -struct StopPoint -{ - TrajectoryPoint point{}; - size_t index; -}; - -struct SlowDownSection -{ - TrajectoryPoint start_point{}; - TrajectoryPoint end_point{}; - size_t slow_down_start_idx; - size_t slow_down_end_idx; - double velocity; -}; - class ObstacleStopPlannerNode : public rclcpp::Node { public: explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options); - struct NodeParam - { - bool enable_slow_down; // set True, slow down for obstacle beside the path - double max_velocity; // max velocity [m/s] - double lowpass_gain; // smoothing calculated current acceleration [-] - double hunting_threshold; // keep slow down or stop state if obstacle vanished [s] - double ego_nearest_dist_threshold; // dist threshold for ego's nearest index - double ego_nearest_yaw_threshold; // yaw threshold for ego's nearest index - }; - - struct StopParam - { - double stop_margin; // stop margin distance from obstacle on the path [m] - double min_behavior_stop_margin; // margin distance, any other stop point is inserted [m] - double expand_stop_range; // margin of vehicle footprint [m] - double extend_distance; // trajectory extend_distance [m] - double step_length; // step length for pointcloud search range [m] - double stop_search_radius; // search radius for obstacle point cloud [m] - double hold_stop_margin_distance; // keep stopping if the ego is in this margin [m] - }; - - struct SlowDownParam - { - double normal_min_jerk; // min jerk limit for mild stop [m/sss] - double normal_min_acc; // min deceleration limit for mild stop [m/ss] - double limit_min_jerk; // min jerk limit [m/sss] - double limit_min_acc; // min deceleration limit [m/ss] - double forward_margin; // slow down margin(vehicle front -> obstacle) [m] - double backward_margin; // slow down margin(obstacle vehicle rear) [m] - double expand_slow_down_range; // lateral range of detection area [m] - double max_slow_down_vel; // maximum speed in slow down section [m/s] - double min_slow_down_vel; // minimum velocity in slow down section [m/s] - bool consider_constraints; // set "True", decel point is planned under jerk/dec constraints - double slow_down_vel; // target slow down velocity [m/s] - double forward_margin_min; // min margin for relaxing slow down margin [m/s] - double forward_margin_span; // fineness param for relaxing slow down margin [m/s] - double slow_down_min_jerk; // min slow down jerk constraint [m/sss] - double jerk_start; // init jerk used for deceleration planning [m/sss] - double jerk_span; // fineness param for planning deceleration jerk [m/sss] - double vel_threshold_reset_velocity_limit_; // velocity threshold, - // check complete deceleration [m/s] - double dec_threshold_reset_velocity_limit_; // acceleration threshold, - // check complete deceleration [m/ss] - double slow_down_search_radius; // search radius for slow down obstacle point cloud [m] - }; - - struct PlannerData - { - diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag{}; - - geometry_msgs::msg::Pose current_pose{}; - - pcl::PointXYZ nearest_collision_point; - pcl::PointXYZ nearest_slow_down_point; - pcl::PointXYZ lateral_nearest_slow_down_point; - rclcpp::Time nearest_collision_point_time{}; - double lateral_deviation{0.0}; - - size_t trajectory_trim_index{}; - size_t decimate_trajectory_collision_index{}; - size_t decimate_trajectory_slow_down_index{}; - std::map decimate_trajectory_index_map{}; // key: decimate index - // value: original index - - bool found_collision_points{false}; - bool found_slow_down_points{false}; - bool stop_require{false}; - bool slow_down_require{false}; - bool enable_adaptive_cruise{false}; - }; - private: /* * ROS diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp new file mode 100644 index 0000000000000..10ba5d6b331cc --- /dev/null +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -0,0 +1,233 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ +#define OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ + +#include + +#include + +#include + +namespace motion_planning +{ + +using diagnostic_msgs::msg::DiagnosticStatus; + +struct StopPoint +{ + TrajectoryPoint point{}; + + size_t index; +}; + +struct SlowDownSection +{ + TrajectoryPoint start_point{}; + + TrajectoryPoint end_point{}; + + size_t slow_down_start_idx; + + size_t slow_down_end_idx; + + double velocity; +}; + +struct NodeParam +{ + // set True, slow down for obstacle beside the path + bool enable_slow_down; + + // max velocity [m/s] + double max_velocity; + + // smoothing calculated current acceleration [-] + double lowpass_gain; + + // keep slow down or stop state if obstacle vanished [s] + double hunting_threshold; + + // dist threshold for ego's nearest index + double ego_nearest_dist_threshold; + + // yaw threshold for ego's nearest index + double ego_nearest_yaw_threshold; +}; + +struct StopParam +{ + // ================= + // params for margin + // ================= + + // stop margin distance from obstacle on the path [m] + double max_longitudinal_margin; + + // margin distance, any other stop point is inserted [m] + double min_longitudinal_margin; + + // margin of vehicle footprint [m] + double lateral_margin; + + // ================================= + // params for trajectory pre-process + // ================================= + + // trajectory extend_distance [m] + double extend_distance; + + // step length for pointcloud search range [m] + double step_length; + + // ====== + // others + // ====== + + // search radius for obstacle point cloud [m] + double stop_search_radius; + + // keep stopping if the ego is in this margin [m] + double hold_stop_margin_distance; +}; + +struct SlowDownParam +{ + // ================= + // params for margin + // ================= + + // slow down margin(vehicle front -> obstacle) [m] + double longitudinal_forward_margin; + + // slow down margin(obstacle vehicle rear) [m] + double longitudinal_backward_margin; + + // lateral margin for slow down area [m] + double lateral_margin; + + // OPTIONAL (use this param if consider_constraints is True) + // min margin for relaxing slow down margin [m/s] + double min_longitudinal_forward_margin; + + // OPTIONAL (use this param if consider_constraints is True) + // fineness param for relaxing slow down margin [m/s] + double longitudinal_margin_span; + + // =================== + // params for velocity + // =================== + + // target slow down velocity [m/s] + double slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is False) + // maximum speed in slow down section [m/s] + double max_slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is False) + // minimum velocity in slow down section [m/s] + double min_slow_down_velocity; + + // OPTIONAL (use this param if consider_constraints is True) + // velocity threshold whether the vehicle complete deceleration [m/s] + double velocity_threshold_decel_complete; + + // OPTIONAL (use this param if consider_constraints is True) + // acceleration threshold whether the vehicle complete deceleration [m/ss] + double acceleration_threshold_decel_complete; + + // =================================== + // params for deceleration constraints + // =================================== + + // OPTIONAL (use this param if consider_constraints is True) + // min jerk limit for mild stop [m/sss] + double normal_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // min deceleration limit for mild stop [m/ss] + double normal_min_acc; + + // OPTIONAL (use this param if consider_constraints is True) + // min jerk limit [m/sss] + double limit_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // min deceleration limit [m/ss] + double limit_min_acc; + + // OPTIONAL (use this param if consider_constraints is True) + // min slow down jerk constraint [m/sss] + double slow_down_min_jerk; + + // OPTIONAL (use this param if consider_constraints is True) + // init jerk used for deceleration planning [m/sss] + double jerk_start; + + // OPTIONAL (use this param if consider_constraints is True) + // fineness param for planning deceleration jerk [m/sss] + double jerk_span; + + // ====== + // others + // ====== + + // set "True", decel point is planned under jerk/dec constraints + bool consider_constraints; + + // search radius for slow down obstacle point cloud [m] + double slow_down_search_radius; +}; + +struct PlannerData +{ + DiagnosticStatus stop_reason_diag{}; + + geometry_msgs::msg::Pose current_pose{}; + + pcl::PointXYZ nearest_collision_point; + + pcl::PointXYZ nearest_slow_down_point; + + pcl::PointXYZ lateral_nearest_slow_down_point; + + rclcpp::Time nearest_collision_point_time{}; + + double lateral_deviation{0.0}; + + size_t trajectory_trim_index{}; + + size_t decimate_trajectory_collision_index{}; + + size_t decimate_trajectory_slow_down_index{}; + + // key: decimate index, value: original index + std::map decimate_trajectory_index_map{}; + + bool found_collision_points{false}; + + bool found_slow_down_points{false}; + + bool stop_require{false}; + + bool slow_down_require{false}; + + bool enable_adaptive_cruise{false}; +}; + +} // namespace motion_planning + +#endif // OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_ diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index f6d367b25e9b5..1f2c8598cdd5a 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -233,15 +233,15 @@ boost::optional calcDecelDistWithJerkAndAccConstraints( return {}; } boost::optional> calcFeasibleMarginAndVelocity( - const ObstacleStopPlannerNode::SlowDownParam & slow_down_param, - const double dist_baselink_to_obstacle, const double current_vel, const double current_acc) + const SlowDownParam & slow_down_param, const double dist_baselink_to_obstacle, + const double current_vel, const double current_acc) { const auto & p = slow_down_param; const auto & logger = rclcpp::get_logger("calcFeasibleMarginAndVelocity"); constexpr double epsilon = 1e-4; - if (current_vel < p.slow_down_vel + epsilon) { - return std::make_pair(p.forward_margin, p.slow_down_vel); + if (current_vel < p.slow_down_velocity + epsilon) { + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); } for (double planning_jerk = p.jerk_start; planning_jerk > p.slow_down_min_jerk - epsilon; @@ -252,17 +252,18 @@ boost::optional> calcFeasibleMarginAndVelocity( const auto planning_dec = planning_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, p.slow_down_vel, current_acc, planning_dec, jerk_acc, jerk_dec); + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { continue; } - if (stop_dist.get() + p.forward_margin < dist_baselink_to_obstacle) { + if (stop_dist.get() + p.longitudinal_forward_margin < dist_baselink_to_obstacle) { RCLCPP_DEBUG( logger, "[found plan] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt:%-6.2f", - stop_dist.get(), planning_jerk, p.forward_margin, p.slow_down_vel, current_vel); - return std::make_pair(p.forward_margin, p.slow_down_vel); + stop_dist.get(), planning_jerk, p.longitudinal_forward_margin, p.slow_down_velocity, + current_vel); + return std::make_pair(p.longitudinal_forward_margin, p.slow_down_velocity); } } @@ -273,18 +274,18 @@ boost::optional> calcFeasibleMarginAndVelocity( const auto planning_dec = p.slow_down_min_jerk > p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; const auto stop_dist = calcDecelDistWithJerkAndAccConstraints( - current_vel, p.slow_down_vel, current_acc, planning_dec, jerk_acc, jerk_dec); + current_vel, p.slow_down_velocity, current_acc, planning_dec, jerk_acc, jerk_dec); if (!stop_dist) { return {}; } - if (stop_dist.get() + p.forward_margin_min < dist_baselink_to_obstacle) { + if (stop_dist.get() + p.min_longitudinal_forward_margin < dist_baselink_to_obstacle) { const auto planning_margin = dist_baselink_to_obstacle - stop_dist.get(); RCLCPP_DEBUG( logger, "[relax margin] dist:%-6.2f jerk:%-6.2f margin:%-6.2f v0:%-6.2f vt%-6.2f", - stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_vel, current_vel); - return std::make_pair(planning_margin, p.slow_down_vel); + stop_dist.get(), p.slow_down_min_jerk, planning_margin, p.slow_down_velocity, current_vel); + return std::make_pair(planning_margin, p.slow_down_velocity); } } @@ -465,16 +466,16 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod { auto & p = stop_param_; const std::string ns = "stop_planner."; - p.stop_margin = declare_parameter(ns + "stop_margin", 5.0); - p.min_behavior_stop_margin = declare_parameter(ns + "min_behavior_stop_margin", 2.0); - p.expand_stop_range = declare_parameter(ns + "expand_stop_range", 0.0); + p.max_longitudinal_margin = declare_parameter(ns + "max_longitudinal_margin", 5.0); + p.min_longitudinal_margin = declare_parameter(ns + "min_longitudinal_margin", 2.0); + p.lateral_margin = declare_parameter(ns + "lateral_margin", 0.0); p.extend_distance = declare_parameter(ns + "extend_distance", 0.0); p.step_length = declare_parameter(ns + "step_length", 1.0); - p.stop_margin += i.max_longitudinal_offset_m; - p.min_behavior_stop_margin += i.max_longitudinal_offset_m; + p.max_longitudinal_margin += i.max_longitudinal_offset_m; + p.min_longitudinal_margin += i.max_longitudinal_offset_m; p.stop_search_radius = p.step_length + - std::hypot(i.vehicle_width_m / 2.0 + p.expand_stop_range, i.vehicle_length_m / 2.0); + std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0); p.hold_stop_margin_distance = declare_parameter(ns + "hold_stop_margin_distance", 0.0); } @@ -487,29 +488,30 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.limit_min_jerk = declare_parameter("limit.min_jerk", -1.5); p.limit_min_acc = declare_parameter("limit.min_acc", -2.5); // slow down planner specific parameters - p.forward_margin = declare_parameter(ns + "forward_margin", 5.0); - p.backward_margin = declare_parameter(ns + "backward_margin", 5.0); - p.expand_slow_down_range = declare_parameter(ns + "expand_slow_down_range", 1.0); - p.max_slow_down_vel = declare_parameter(ns + "max_slow_down_vel", 4.0); - p.min_slow_down_vel = declare_parameter(ns + "min_slow_down_vel", 2.0); + p.longitudinal_forward_margin = declare_parameter(ns + "longitudinal_forward_margin", 5.0); + p.longitudinal_backward_margin = declare_parameter(ns + "longitudinal_backward_margin", 5.0); + p.lateral_margin = declare_parameter(ns + "lateral_margin", 1.0); + p.max_slow_down_velocity = declare_parameter(ns + "max_slow_down_velocity", 4.0); + p.min_slow_down_velocity = declare_parameter(ns + "min_slow_down_velocity", 2.0); // consider jerk/dec constraints in slow down p.consider_constraints = declare_parameter(ns + "consider_constraints", false); - p.forward_margin_min = declare_parameter(ns + "forward_margin_min", 1.0); - p.forward_margin_span = declare_parameter(ns + "forward_margin_span", -0.1); + p.min_longitudinal_forward_margin = + declare_parameter(ns + "min_longitudinal_forward_margin", 1.0); + p.longitudinal_margin_span = declare_parameter(ns + "longitudinal_margin_span", -0.1); p.slow_down_min_jerk = declare_parameter(ns + "jerk_min_slow_down", -0.3); p.jerk_start = declare_parameter(ns + "jerk_start", -0.1); p.jerk_span = declare_parameter(ns + "jerk_span", -0.01); - p.slow_down_vel = declare_parameter(ns + "slow_down_vel", 1.39); - p.vel_threshold_reset_velocity_limit_ = - declare_parameter(ns + "vel_threshold_reset_velocity_limit_", 0.2); - p.dec_threshold_reset_velocity_limit_ = - declare_parameter(ns + "dec_threshold_reset_velocity_limit_", 0.1); - p.forward_margin += i.max_longitudinal_offset_m; - p.forward_margin_min += i.wheel_base_m + i.front_overhang_m; - p.backward_margin += i.rear_overhang_m; + p.slow_down_velocity = declare_parameter(ns + "slow_down_velocity", 1.39); + p.velocity_threshold_decel_complete = + declare_parameter(ns + "velocity_threshold_decel_complete", 0.2); + p.acceleration_threshold_decel_complete = + declare_parameter(ns + "acceleration_threshold_decel_complete", 0.1); + p.longitudinal_forward_margin += i.max_longitudinal_offset_m; + p.min_longitudinal_forward_margin += i.wheel_base_m + i.front_overhang_m; + p.longitudinal_backward_margin += i.rear_overhang_m; p.slow_down_search_radius = stop_param_.step_length + - std::hypot(i.vehicle_width_m / 2.0 + p.expand_slow_down_range, i.vehicle_length_m / 2.0); + std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0); } // Initializer @@ -700,7 +702,7 @@ void ObstacleStopPlannerNode::searchObstacle( // create one step polygon for slow_down range createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range_polygon, vehicle_info, - slow_down_param_.expand_slow_down_range); + slow_down_param_.lateral_margin); debug_ptr_->pushPolygon( one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDownRange); @@ -738,7 +740,7 @@ void ObstacleStopPlannerNode::searchObstacle( std::vector one_step_move_vehicle_polygon; // create one step polygon for vehicle createOneStepPolygon( - p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.expand_stop_range); + p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.lateral_margin); debug_ptr_->pushPolygon( one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, PolygonType::Vehicle); @@ -942,8 +944,8 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_section.start_point = output.front(); slow_down_section.slow_down_end_idx = end_insert_point_with_idx.get().first; slow_down_section.end_point = end_insert_point_with_idx.get().second; - slow_down_section.velocity = - set_velocity_limit_ ? std::numeric_limits::max() : slow_down_param_.slow_down_vel; + slow_down_section.velocity = set_velocity_limit_ ? std::numeric_limits::max() + : slow_down_param_.slow_down_velocity; insertSlowDownSection(slow_down_section, output); } else if (no_hunting_slowdown_point) { @@ -996,10 +998,10 @@ void ObstacleStopPlannerNode::externalExpandStopRangeCallback( std::lock_guard lock(mutex_); const auto & i = vehicle_info_; - stop_param_.expand_stop_range = input_msg->expand_stop_range; + stop_param_.lateral_margin = input_msg->expand_stop_range; stop_param_.stop_search_radius = stop_param_.step_length + - std::hypot(i.vehicle_width_m / 2.0 + stop_param_.expand_stop_range, i.vehicle_length_m / 2.0); + std::hypot(i.vehicle_width_m / 2.0 + stop_param_.lateral_margin, i.vehicle_length_m / 2.0); } void ObstacleStopPlannerNode::insertStopPoint( @@ -1042,9 +1044,9 @@ StopPoint ObstacleStopPlannerNode::searchInsertPoint( const StopParam & stop_param) { const auto max_dist_stop_point = - createTargetPoint(idx, stop_param.stop_margin, base_trajectory, dist_remain); + createTargetPoint(idx, stop_param.max_longitudinal_margin, base_trajectory, dist_remain); const auto min_dist_stop_point = - createTargetPoint(idx, stop_param.min_behavior_stop_margin, base_trajectory, dist_remain); + createTargetPoint(idx, stop_param.min_longitudinal_margin, base_trajectory, dist_remain); // check if stop point is already inserted by behavior planner bool is_inserted_already_stop_point = false; @@ -1099,7 +1101,7 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( } const auto no_need_velocity_limit = - dist_baselink_to_obstacle + dist_remain > slow_down_param_.forward_margin; + dist_baselink_to_obstacle + dist_remain > slow_down_param_.longitudinal_forward_margin; if (set_velocity_limit_ && no_need_velocity_limit) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -1107,9 +1109,10 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( const auto use_velocity_limit = relax_target_vel || set_velocity_limit_; const auto update_forward_margin_from_vehicle = - use_velocity_limit ? slow_down_param_.forward_margin_min - dist_remain + use_velocity_limit ? slow_down_param_.min_longitudinal_forward_margin - dist_remain : margin_with_vel.get().first - dist_remain; - const auto update_backward_margin_from_vehicle = slow_down_param_.backward_margin + dist_remain; + const auto update_backward_margin_from_vehicle = + slow_down_param_.longitudinal_backward_margin + dist_remain; const auto velocity = use_velocity_limit ? std::numeric_limits::max() : margin_with_vel.get().second; @@ -1118,14 +1121,16 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, velocity); } else { - const auto update_forward_margin_from_vehicle = slow_down_param_.forward_margin - dist_remain; - const auto update_backward_margin_from_vehicle = slow_down_param_.backward_margin + dist_remain; + const auto update_forward_margin_from_vehicle = + slow_down_param_.longitudinal_forward_margin - dist_remain; + const auto update_backward_margin_from_vehicle = + slow_down_param_.longitudinal_backward_margin + dist_remain; const auto velocity = - slow_down_param_.min_slow_down_vel + - (slow_down_param_.max_slow_down_vel - slow_down_param_.min_slow_down_vel) * + slow_down_param_.min_slow_down_velocity + + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * std::max(lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) / - slow_down_param_.expand_slow_down_range; + slow_down_param_.lateral_margin; return createSlowDownSectionFromMargin( idx, base_trajectory, update_forward_margin_from_vehicle, update_backward_margin_from_vehicle, @@ -1565,7 +1570,7 @@ void ObstacleStopPlannerNode::setExternalVelocityLimit() const auto & p = slow_down_param_; auto slow_down_limit_vel = std::make_shared(); slow_down_limit_vel->stamp = this->now(); - slow_down_limit_vel->max_velocity = p.slow_down_vel; + slow_down_limit_vel->max_velocity = p.slow_down_velocity; slow_down_limit_vel->constraints.min_acceleration = p.slow_down_min_jerk < p.normal_min_jerk ? p.limit_min_acc : p.normal_min_acc; slow_down_limit_vel->constraints.max_jerk = std::abs(p.slow_down_min_jerk); @@ -1584,11 +1589,10 @@ void ObstacleStopPlannerNode::setExternalVelocityLimit() void ObstacleStopPlannerNode::resetExternalVelocityLimit( const double current_acc, const double current_vel) { - const auto reach_target_vel = - current_vel < - slow_down_param_.slow_down_vel + slow_down_param_.vel_threshold_reset_velocity_limit_; + const auto reach_target_vel = current_vel < slow_down_param_.slow_down_velocity + + slow_down_param_.velocity_threshold_decel_complete; const auto constant_vel = - std::abs(current_acc) < slow_down_param_.dec_threshold_reset_velocity_limit_; + std::abs(current_acc) < slow_down_param_.acceleration_threshold_decel_complete; const auto no_undershoot = reach_target_vel && constant_vel; if (!no_undershoot) {