From 4f9bade08cd95f52559e48122dbd10952e52e4f7 Mon Sep 17 00:00:00 2001 From: AhmedEbrahim Date: Fri, 7 Jul 2023 05:26:03 +0300 Subject: [PATCH] fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area Signed-off-by: AhmedEbrahim --- .../obstacle_avoidance_planner.param.yaml | 3 +++ .../obstacle_avoidance_planner/node.hpp | 4 ++++ .../obstacle_avoidance_planner/src/node.cpp | 21 ++++++++++++++++++- 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml index 624ba4defc8a0..5e6f845ccf0f2 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -18,6 +18,9 @@ # output output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] + + vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . + # This margin will be realized with delta_arc_length_for_mpt_points m precision. # replanning & trimming trajectory param outside algorithm replan: diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index efec8afadc56d..ffbb56ca6b9e8 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -127,6 +127,10 @@ class ObstacleAvoidancePlanner : public rclcpp::Node const PlannerData & planner_data, std::vector & traj_points) const; void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; void publishDebugMarkerOfOptimization(const std::vector & traj_points) const; + + private: + double vehicle_stop_margin_outside_drivable_area_; + }; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index c1d931dd54e20..adaf483147212 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -100,6 +100,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n time_keeper_ptr_->enable_calculation_time_info = declare_parameter("option.debug.enable_calculation_time_info"); + vehicle_stop_margin_outside_drivable_area_ = + declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); + // parameters for ego nearest search ego_nearest_param_ = EgoNearestParam(this); @@ -149,6 +152,10 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( parameters, "option.debug.enable_calculation_time_info", time_keeper_ptr_->enable_calculation_time_info); + updateParam( + parameters, "common.vehicle_stop_margin_outside_drivable_area", + vehicle_stop_margin_outside_drivable_area_); + // parameters for ego nearest search ego_nearest_param_.onParam(parameters); @@ -447,7 +454,19 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( if (first_outside_idx) { for (size_t i = *first_outside_idx; i < optimized_traj_points.size(); ++i) { - optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0; + size_t stop_idx = i; + const auto & op_target_point = motion_utils::calcLongitudinalOffsetPoint( + optimized_traj_points, optimized_traj_points.at(i).pose.position, -1.0 * vehicle_stop_margin_outside_drivable_area_); + if (op_target_point) { + const auto target_point = op_target_point.get(); + // confirm that target point doesn't overlap with the stop point outside drivable area + const auto dist = tier4_autoware_utils::calcDistance2d(optimized_traj_points.at(i), target_point); + const double overlap_threshold = 1e-3; + if (dist > overlap_threshold) { + stop_idx = motion_utils::findNearestSegmentIndex(optimized_traj_points, target_point); + } + } + optimized_traj_points.at(stop_idx).longitudinal_velocity_mps = 0.0; } }