From d56c191460a2939d76562df289e74b1d5588368b Mon Sep 17 00:00:00 2001 From: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Date: Tue, 19 Dec 2023 14:02:54 +0900 Subject: [PATCH] feat(obstacle_stop): upd obstacle hunting (#1068) * Adapted from PR #1458 Signed-off-by: Shigekazu Fukuta * Adapted from PR #1627 Signed-off-by: Shigekazu Fukuta * fix parameter name Signed-off-by: Shigekazu Fukuta * Adapted from PR #2647 Signed-off-by: Shigekazu Fukuta * ci(pre-commit): autofix * fix build error * ci(pre-commit): autofix * remove comment line * remove logic * Delete parameters other than those added this time * ci(pre-commit): autofix * add stop vehicle check * ci(pre-commit): autofix * fix stop velocity threshold * fix engage obstacle clear and stop threshold * ci(pre-commit): autofix --------- Signed-off-by: Shigekazu Fukuta Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/obstacle_stop_planner/README.md | 6 ++ .../config/obstacle_stop_planner.param.yaml | 2 + .../include/obstacle_stop_planner/node.hpp | 45 ++++++++- planning/obstacle_stop_planner/src/node.cpp | 99 +++++++++++++------ 4 files changed, 118 insertions(+), 34 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 95274b6a4829d..ed46c3f6bc154 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -39,6 +39,12 @@ When the deceleration section is inserted, the start point of the section is ins ## Modules +### Common Parameter + +| Parameter | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------------------------------------------- | +| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | + ### 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 c4be8d7c35789..a0acc76b357f3 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: + chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_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 0a4dc99493bc7..7ca2da6be2354 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include namespace motion_planning @@ -76,6 +77,8 @@ using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using vehicle_info_util::VehicleInfo; +using PointCloud = pcl::PointCloud; + struct StopPoint { TrajectoryPoint point{}; @@ -91,6 +94,17 @@ struct SlowDownSection double velocity; }; +struct ObstacleWithDetectionTime +{ + explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) + : detection_time(t), point(p) + { + } + + rclcpp::Time detection_time; + pcl::PointXYZ point; +}; + class ObstacleStopPlannerNode : public rclcpp::Node { public: @@ -101,7 +115,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node 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 chattering_threshold; // keep slow down or stop state if obstacle vanished [s] double max_yaw_deviation_rad; // maximum ego yaw deviation from trajectory [rad] (measures // against overlapping lanes) }; @@ -184,12 +198,12 @@ 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_slow_down_section_{boost::none}; + std::vector obstacle_history_{}; 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_; nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; nav_msgs::msg::Odometry::ConstSharedPtr prev_velocity_ptr_{nullptr}; @@ -305,6 +319,31 @@ class ObstacleStopPlannerNode : public rclcpp::Node void publishDebugData( const PlannerData & planner_data, const double current_acc, const double current_vel); + + void updateObstacleHistory(const rclcpp::Time & now, const double chattering_threshold) + { + for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > chattering_threshold; + + if (expired) { + itr = obstacle_history_.erase(itr); + continue; + } + + itr++; + } + } + + PointCloud::Ptr getOldPointCloudPtr() const + { + PointCloud::Ptr ret(new PointCloud); + + for (const auto & p : obstacle_history_) { + ret->push_back(p.point); + } + + return ret; + } }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 44de6e2cdf9cb..45a9757175f81 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -42,6 +42,8 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::findNearestIndex; using tier4_autoware_utils::getRPY; +using PointCloud = pcl::PointCloud; + namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -443,7 +445,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod auto & p = node_param_; p.enable_slow_down = declare_parameter("enable_slow_down", false); p.max_velocity = declare_parameter("max_velocity", 20.0); - p.hunting_threshold = declare_parameter("hunting_threshold", 0.5); + p.chattering_threshold = declare_parameter("chattering_threshold", 0.5); p.lowpass_gain = declare_parameter("lowpass_gain", 0.9); lpf_acc_ = std::make_shared(0.0, p.lowpass_gain); const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0); @@ -503,7 +505,6 @@ 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(); // Publishers path_pub_ = this->create_publisher("~/output/trajectory", 1); @@ -632,9 +633,7 @@ 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; - if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { + if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -662,6 +661,11 @@ void ObstacleStopPlannerNode::searchObstacle( return; } + const auto now = this->now(); + const bool is_stopping = (std::fabs(current_velocity_ptr->twist.twist.linear.x) < 0.001); + const double history_erase_sec = (is_stopping) ? node_param_.chattering_threshold : 0.0; + updateObstacleHistory(now, history_erase_sec); + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { // create one step circle center for vehicle const auto & p_front = decimate_trajectory.at(i).pose; @@ -721,37 +725,79 @@ void ObstacleStopPlannerNode::searchObstacle( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - planner_data.found_collision_points = withinPolygon( + const auto found_collision_points = withinPolygon( one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, next_center_point, slow_down_pointcloud_ptr, collision_pointcloud_ptr); - if (planner_data.found_collision_points) { - planner_data.decimate_trajectory_collision_index = i; + if (found_collision_points) { + pcl::PointXYZ nearest_collision_point; + rclcpp::Time nearest_collision_point_time; getNearestPoint( - *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, - &planner_data.nearest_collision_point_time); + *collision_pointcloud_ptr, p_front, &nearest_collision_point, + &nearest_collision_point_time); - debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); - debug_ptr_->pushPolygon( - one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - - planner_data.stop_require = planner_data.found_collision_points; - acc_controller_->insertAdaptiveCruiseVelocity( - decimate_trajectory, planner_data.decimate_trajectory_collision_index, - planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, - &planner_data.stop_require, &output, trajectory_header); + obstacle_history_.emplace_back(now, nearest_collision_point); break; } } } + for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) { + // create one step circle center for vehicle + const auto & p_front = decimate_trajectory.at(i).pose; + const auto & p_back = decimate_trajectory.at(i + 1).pose; + const auto prev_center_pose = getVehicleCenterFromBase(p_front, vehicle_info); + const Point2d prev_center_point(prev_center_pose.position.x, prev_center_pose.position.y); + const auto next_center_pose = getVehicleCenterFromBase(p_back, vehicle_info); + const Point2d next_center_point(next_center_pose.position.x, next_center_pose.position.y); + 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); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, decimate_trajectory.at(i).pose.position.z, + PolygonType::Vehicle); + + PointCloud::Ptr collision_pointcloud_ptr(new PointCloud); + collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; + + // check new collision points + planner_data.found_collision_points = withinPolygon( + one_step_move_vehicle_polygon, stop_param.stop_search_radius, prev_center_point, + next_center_point, getOldPointCloudPtr(), collision_pointcloud_ptr); + + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; + getNearestPoint( + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); + + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); + + planner_data.stop_require = planner_data.found_collision_points; + + acc_controller_->insertAdaptiveCruiseVelocity( + decimate_trajectory, planner_data.decimate_trajectory_collision_index, + planner_data.current_pose, planner_data.nearest_collision_point, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, + &planner_data.stop_require, &output, trajectory_header); + + if (!planner_data.stop_require) { + obstacle_history_.clear(); + } + + break; + } + } } void ObstacleStopPlannerNode::insertVelocity( TrajectoryPoints & output, PlannerData & planner_data, - const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const double current_vel, const StopParam & stop_param) + [[maybe_unused]] const std_msgs::msg::Header & trajectory_header, + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, + const StopParam & stop_param) { if (output.size() < 3) { return; @@ -799,17 +845,8 @@ void ObstacleStopPlannerNode::insertVelocity( index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, 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_ = slow_down_section; - } - insertSlowDownSection(slow_down_section, output); } - - last_detection_time_ = trajectory_header.stamp; } if (node_param_.enable_slow_down && latest_slow_down_section_) {