From 22f00a9e3f105c509e0ab090100aa2ee44dc05d0 Mon Sep 17 00:00:00 2001 From: asa-naki Date: Wed, 18 Sep 2024 17:13:35 +0900 Subject: [PATCH] revert: #1068 - d56c191460a2939d76562df289e74b1d5588368b. --- 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, 34 insertions(+), 118 deletions(-) diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index ed46c3f6bc154..95274b6a4829d 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -39,12 +39,6 @@ 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 a0acc76b357f3..c4be8d7c35789 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,7 +1,5 @@ /**: 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 7ca2da6be2354..0a4dc99493bc7 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -57,7 +57,6 @@ #include #include #include -#include #include namespace motion_planning @@ -77,8 +76,6 @@ 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{}; @@ -94,17 +91,6 @@ 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: @@ -115,7 +101,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 chattering_threshold; // keep slow down or stop state if obstacle vanished [s] + double hunting_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) }; @@ -198,12 +184,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::none}; - std::vector obstacle_history_{}; + boost::optional latest_slow_down_section_{}; 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}; @@ -319,31 +305,6 @@ 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 45a9757175f81..44de6e2cdf9cb 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -42,8 +42,6 @@ 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) @@ -445,7 +443,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.chattering_threshold = declare_parameter("chattering_threshold", 0.5); + p.hunting_threshold = declare_parameter("hunting_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); @@ -505,6 +503,7 @@ 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); @@ -633,7 +632,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_; - if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_) { + 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) { resetExternalVelocityLimit(current_acc, current_vel); } @@ -661,11 +662,6 @@ 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; @@ -725,79 +721,37 @@ void ObstacleStopPlannerNode::searchObstacle( new pcl::PointCloud); collision_pointcloud_ptr->header = obstacle_candidate_pointcloud_ptr->header; - const auto found_collision_points = withinPolygon( + planner_data.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 (found_collision_points) { - pcl::PointXYZ nearest_collision_point; - rclcpp::Time nearest_collision_point_time; + if (planner_data.found_collision_points) { + planner_data.decimate_trajectory_collision_index = i; getNearestPoint( - *collision_pointcloud_ptr, p_front, &nearest_collision_point, - &nearest_collision_point_time); - - 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); + *collision_pointcloud_ptr, p_front, &planner_data.nearest_collision_point, + &planner_data.nearest_collision_point_time); - planner_data.stop_require = planner_data.found_collision_points; + debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop); + debug_ptr_->pushPolygon( + one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); - 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); + 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; } - - break; } } } void ObstacleStopPlannerNode::insertVelocity( TrajectoryPoints & output, PlannerData & planner_data, - [[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) + 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; @@ -845,8 +799,17 @@ 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_) {