From efb2e3899b8706d7fddf336ea1d1c83f1f6b93a2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 28 Feb 2022 13:42:05 +0900 Subject: [PATCH] add path shape change detection for replan Signed-off-by: Takayuki Murooka --- .../obstacle_avoidance_planner.param.yaml | 19 ++--- .../obstacle_avoidance_planner/node.hpp | 8 +-- .../obstacle_avoidance_planner/src/node.cpp | 70 ++++++++++++++++--- 3 files changed, 76 insertions(+), 21 deletions(-) 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 f59386fd577ec..bc64558514a2b 100644 --- a/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml +++ b/planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml @@ -7,7 +7,7 @@ is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid - is_stopping_if_outside_drivable_area: true + is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area # show is_showing_debug_info: false @@ -16,7 +16,7 @@ # check if planned trajectory is outside drivable area drivability_check: # true: vehicle shape is considered as a set of circles - # false: vehicle shape is considered as footprint (= rectangle) + # false: vehicle shape is consideredas footprint (= rectangle) use_vehicle_circles: false # parameters only when use_vehicle_circles is true @@ -34,8 +34,8 @@ # other enable_avoidance: false # enable avoidance function - enable_pre_smoothing: true - skip_optimization: false + enable_pre_smoothing: true # enable EB + skip_optimization: false # skip MPT and EB reset_prev_optimization: false common: @@ -75,7 +75,7 @@ eb: common: num_joint_buffer_points: 3 # number of joint buffer points - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx + num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idxp delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points @@ -148,8 +148,8 @@ steer_rate_weight: 10.0 # weight for steering rate obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error - obstacle_avoid_yaw_error_weight: 100.0 # weight for yaw error - obstacle_avoid_steer_input_weight: 10000.0 # weight for yaw error + obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error + obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error near_objects_length: 30.0 # weight for yaw error terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point @@ -159,5 +159,6 @@ # replanning & trimming trajectory param outside algorithm replan: - min_ego_moving_dist_for_replan: 3.0 # minimum ego moving dist thres for replan [m] - min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan [second] + max_path_shape_change_dist: 2.0 # threshold of path shape change from behavior path [m] + max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] + max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] 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 3fe78b5253bd9..56f4e437853f3 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -120,10 +120,10 @@ class ObstacleAvoidancePlanner : public rclcpp::Node int vehicle_circle_radius_num_for_mpt_; double vehicle_circle_radius_ratio_for_mpt_; - // params outside logic - double min_ego_moving_dist_for_replan_; - double min_delta_time_sec_for_replan_; - double max_dist_for_extending_end_point_; + // params for replan + double max_path_shape_change_dist_for_replan_; + double max_ego_moving_dist_for_replan_; + double max_delta_time_sec_for_replan_; // logic std::unique_ptr costmap_generator_ptr_; diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index ae17a9b748d13..439eb02d6e69b 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -48,6 +48,48 @@ size_t searchExtendedZeroVelocityIndex( fine_points, vel_points.at(zero_vel_idx).pose.position); } +bool isPathShapeChanged( + const geometry_msgs::msg::Pose & ego_pose, + const std::vector & path_points, + const std::unique_ptr> & + prev_path_points, + const double max_path_shape_change_dist, const double delta_yaw_threshold) +{ + if (!prev_path_points) { + return false; + } + + // truncate prev points from ego pose to fixed end points + const auto opt_prev_begin_idx = tier4_autoware_utils::findNearestIndex( + *prev_path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); + const size_t prev_begin_idx = opt_prev_begin_idx ? *opt_prev_begin_idx : 0; + const std::vector truncated_prev_points{ + prev_path_points->begin() + prev_begin_idx, prev_path_points->end()}; + + // truncate points from ego pose to fixed end points + const auto opt_begin_idx = tier4_autoware_utils::findNearestIndex( + path_points, ego_pose, std::numeric_limits::max(), delta_yaw_threshold); + const size_t begin_idx = opt_begin_idx ? *opt_begin_idx : 0; + const std::vector truncated_points{ + path_points.begin() + begin_idx, path_points.end()}; + + // guard for lateral offset + if (truncated_prev_points.size() < 2 || truncated_points.size() < 2) { + return false; + } + + // calculate lateral deviations between truncated path_points and prev_path_points + for (const auto & prev_point : truncated_prev_points) { + const double dist = + tier4_autoware_utils::calcLateralOffset(truncated_points, prev_point.pose.position); + if (dist > max_path_shape_change_dist) { + return true; + } + } + + return false; +} + bool isPathGoalChanged( const double current_vel, const std::vector & path_points, @@ -431,10 +473,12 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n } { // replan - min_ego_moving_dist_for_replan_ = - declare_parameter("replan.min_ego_moving_dist_for_replan"); - min_delta_time_sec_for_replan_ = - declare_parameter("replan.min_delta_time_sec_for_replan"); + max_path_shape_change_dist_for_replan_ = + declare_parameter("replan.max_path_shape_change_dist"); + max_ego_moving_dist_for_replan_ = + declare_parameter("replan.max_ego_moving_dist_for_replan"); + max_delta_time_sec_for_replan_ = + declare_parameter("replan.max_delta_time_sec_for_replan"); } // TODO(murooka) tune this param when avoiding with obstacle_avoidance_planner @@ -718,9 +762,11 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::paramCallback { // replan updateParam( - parameters, "replan.min_ego_moving_dist_for_replan", min_ego_moving_dist_for_replan_); + parameters, "replan.max_path_shape_change_dist", max_path_shape_change_dist_for_replan_); + updateParam( + parameters, "replan.max_ego_moving_dist_for_replan", max_ego_moving_dist_for_replan_); updateParam( - parameters, "replan.min_delta_time_sec_for_replan", min_delta_time_sec_for_replan_); + parameters, "replan.max_delta_time_sec_for_replan", max_delta_time_sec_for_replan_); } resetPlanning(); @@ -872,6 +918,14 @@ bool ObstacleAvoidancePlanner::checkReplan( return true; } + if (isPathShapeChanged( + current_ego_pose_, path_points, prev_path_points_ptr_, + max_path_shape_change_dist_for_replan_, + traj_param_.delta_yaw_threshold_for_closest_point)) { + RCLCPP_INFO(get_logger(), "Replan since path shape was changed."); + return true; + } + if (isPathGoalChanged(current_twist_ptr_->twist.linear.x, path_points, prev_path_points_ptr_)) { RCLCPP_INFO(get_logger(), "Replan with resetting optimization since path goal was changed."); resetPrevOptimization(); @@ -881,7 +935,7 @@ bool ObstacleAvoidancePlanner::checkReplan( // For when ego pose is lost or new ego pose is designated in simulation const double delta_dist = tier4_autoware_utils::calcDistance2d(current_ego_pose_.position, prev_ego_pose_ptr_->position); - if (delta_dist > min_ego_moving_dist_for_replan_) { + if (delta_dist > max_ego_moving_dist_for_replan_) { RCLCPP_INFO( get_logger(), "Replan with resetting optimization since current ego pose is far from previous ego pose."); @@ -900,7 +954,7 @@ bool ObstacleAvoidancePlanner::checkReplan( } const double delta_time_sec = (this->now() - *latest_replanned_time_ptr_).seconds(); - if (delta_time_sec > min_delta_time_sec_for_replan_) { + if (delta_time_sec > max_delta_time_sec_for_replan_) { return true; } return false;