From d06add2b80f44934500d2bb1815a64122b949993 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 3 Aug 2022 14:35:00 +0900 Subject: [PATCH] feat(obstacle_stop_planner): prevent from obstacle hunting (#1458) * feat(obstacle_stop_planner): prevent from obstacle hunting Signed-off-by: satoshi-ota * docs(obstacle_stop_planner): add common parameter description Signed-off-by: satoshi-ota * fix(obstacle_stop_planner): fix bug Signed-off-by: satoshi-ota --- planning/obstacle_stop_planner/README.md | 10 +++++ .../config/obstacle_stop_planner.param.yaml | 2 + .../include/obstacle_stop_planner/node.hpp | 6 ++- planning/obstacle_stop_planner/src/node.cpp | 43 +++++++++++++++---- 4 files changed, 50 insertions(+), 11 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 9c8fc714fb9c1..2bbc22acc77be 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -46,6 +46,16 @@ When the deceleration section is inserted, the start point of the section is ins ## Modules +### Common Parameter + +| Parameter | Type | Description | +| ----------------------- | ------ | ---------------------------------------------------------------------------------------- | +| `enable_slow_down` | bool | enable slow down planner [-] | +| `max_velocity` | double | max velocity [m/s] | +| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] | +| `lowpass_gain` | double | low pass gain for calculating acceleration [-] | +| `max_yaw_deviation_deg` | double | # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes) | + ### Obstacle Stop Planner #### Role 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 7cb827feb0227..ff0281bf574eb 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + 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] 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 5d61c8ddf7a68..08647aae98826 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -185,12 +185,14 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; std::shared_ptr lpf_acc_{nullptr}; - boost::optional latest_slow_down_section_{}; + boost::optional latest_stop_point_{boost::none}; + boost::optional latest_slow_down_section_{boost::none}; tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - rclcpp::Time last_detection_time_; + rclcpp::Time last_detect_time_collision_point_; + rclcpp::Time last_detect_time_slowdown_point_; nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index e85dcdbb50d34..4f0f180d8c4b5 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -515,7 +515,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod acc_controller_ = std::make_unique( this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m); debug_ptr_ = std::make_shared(this, i.max_longitudinal_offset_m); - last_detection_time_ = this->now(); + last_detect_time_slowdown_point_ = this->now(); + last_detect_time_collision_point_ = this->now(); // Publishers path_pub_ = this->create_publisher("~/output/trajectory", 1); @@ -654,8 +655,9 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu current_vel, stop_param); const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; - const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() > - node_param_.hunting_threshold; + const auto no_hunting = + (rclcpp::Time(input_msg->header.stamp) - last_detect_time_slowdown_point_).seconds() > + node_param_.hunting_threshold; if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -723,6 +725,8 @@ void ObstacleStopPlannerNode::searchObstacle( debug_ptr_->pushObstaclePoint(planner_data.nearest_slow_down_point, PointType::SlowDown); debug_ptr_->pushPolygon( one_step_move_slow_down_range_polygon, p_front.position.z, PolygonType::SlowDown); + + last_detect_time_slowdown_point_ = trajectory_header.stamp; } } else { @@ -769,6 +773,10 @@ void ObstacleStopPlannerNode::searchObstacle( planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, &planner_data.stop_require, &output, trajectory_header); + if (planner_data.stop_require) { + last_detect_time_collision_point_ = trajectory_header.stamp; + } + break; } } @@ -781,6 +789,9 @@ void ObstacleStopPlannerNode::insertVelocity( const double current_acc, const double current_vel, const StopParam & stop_param) { const auto & base_link2front = vehicle_info.max_longitudinal_offset_m; + const auto no_hunting_collision_point = + (rclcpp::Time(trajectory_header.stamp) - last_detect_time_collision_point_).seconds() > + node_param_.hunting_threshold; if (planner_data.stop_require) { // insert stop point @@ -820,18 +831,29 @@ void ObstacleStopPlannerNode::insertVelocity( current_stop_pos.point.pose = ego_pos_on_path.get(); insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag); + latest_stop_point_ = current_stop_pos; debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); } } else { insertStopPoint(stop_point, output, planner_data.stop_reason_diag); + latest_stop_point_ = stop_point; debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); } } + } else if (!no_hunting_collision_point) { + if (latest_stop_point_) { + insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag); + debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::Stop); + } } + const auto no_hunting_slowdown_point = + (rclcpp::Time(trajectory_header.stamp) - last_detect_time_slowdown_point_).seconds() > + node_param_.hunting_threshold; + if (planner_data.slow_down_require) { // insert slow down point const auto traj_end_idx = output.size() - 1; @@ -856,16 +878,19 @@ void ObstacleStopPlannerNode::insertVelocity( current_vel); if ( - !latest_slow_down_section_ && - dist_baselink_to_obstacle + index_with_dist_remain.get().second < - vehicle_info.max_longitudinal_offset_m) { + (!latest_slow_down_section_ && + dist_baselink_to_obstacle + index_with_dist_remain.get().second < + vehicle_info.max_longitudinal_offset_m) || + !no_hunting_slowdown_point) { latest_slow_down_section_ = slow_down_section; } insertSlowDownSection(slow_down_section, output); } - - last_detection_time_ = trajectory_header.stamp; + } else if (!no_hunting_slowdown_point) { + if (latest_slow_down_section_) { + insertSlowDownSection(latest_slow_down_section_.get(), output); + } } if (node_param_.enable_slow_down && latest_slow_down_section_) { @@ -891,7 +916,7 @@ void ObstacleStopPlannerNode::insertVelocity( set_velocity_limit_ ? std::numeric_limits::max() : slow_down_param_.slow_down_vel; insertSlowDownSection(slow_down_section, output); - } else { + } else if (no_hunting_slowdown_point) { latest_slow_down_section_ = {}; } }