diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index a76b8a9b814f4..2ae4146ce09be 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -31,11 +31,11 @@ ### 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] | +| Parameter | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------------------------------------------- | +| `enable_slow_down` | bool | enable slow down planner [-] | +| `max_velocity` | double | max velocity [m/s] | +| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] | ## Obstacle Stop Planner 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 de07e9e41b7c1..e9445f4e920e7 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] + chattering_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] max_velocity: 20.0 # max velocity [m/s] enable_slow_down: False # whether to use slow down planner [-] voxel_grid_x: 0.05 # voxel grid x parameter for filtering pointcloud [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 236d7338bcdd7..c461e112e40e7 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include namespace motion_planning @@ -91,6 +92,17 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; +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: @@ -121,14 +133,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; - boost::optional latest_stop_point_{boost::none}; 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_}; PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - rclcpp::Time last_detect_time_collision_point_; - rclcpp::Time last_detect_time_slowdown_point_; Odometry::ConstSharedPtr current_velocity_ptr_{nullptr}; AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_{nullptr}; @@ -202,6 +212,31 @@ class ObstacleStopPlannerNode : public rclcpp::Node void onDynamicObjects(const PredictedObjects::ConstSharedPtr input_msg); void onExpandStopRange(const ExpandStopRange::ConstSharedPtr input_msg); + + void updateObstacleHistory(const rclcpp::Time & now) + { + for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) { + const auto expired = (now - itr->detection_time).seconds() > node_param_.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/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 58895a9d40c78..4f88972d7206e 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -63,7 +63,7 @@ struct NodeParam double max_velocity; // keep slow down or stop state if obstacle vanished [s] - double hunting_threshold; + double chattering_threshold; // dist threshold for ego's nearest index double ego_nearest_dist_threshold; diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 683bbdf9e6113..425577504867e 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -168,6 +168,7 @@ void ObstacleStopPlannerDebugNode::publish() slow_down_range_polygons_.clear(); slow_down_polygons_.clear(); stop_pose_ptr_ = nullptr; + target_stop_pose_ptr_ = nullptr; slow_down_start_pose_ptr_ = nullptr; slow_down_end_pose_ptr_ = nullptr; stop_obstacle_point_ptr_ = nullptr; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 0a29beec48840..5e2c031925786 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -65,7 +65,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod auto & p = node_param_; p.enable_slow_down = declare_parameter("enable_slow_down"); p.max_velocity = declare_parameter("max_velocity"); - p.hunting_threshold = declare_parameter("hunting_threshold"); + p.chattering_threshold = declare_parameter("chattering_threshold"); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); p.voxel_grid_x = declare_parameter("voxel_grid_x", 0.05); @@ -152,8 +152,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_detect_time_slowdown_point_ = this->now(); - last_detect_time_collision_point_ = this->now(); // Publishers pub_trajectory_ = this->create_publisher("~/output/trajectory", 1); @@ -307,10 +305,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m 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_detect_time_slowdown_point_).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); } @@ -335,6 +330,10 @@ void ObstacleStopPlannerNode::searchObstacle( return; } + const auto now = this->now(); + + updateObstacleHistory(now); + 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; @@ -375,8 +374,6 @@ 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 { @@ -395,52 +392,88 @@ void ObstacleStopPlannerNode::searchObstacle( PointCloud::Ptr collision_pointcloud_ptr(new 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); + obstacle_history_.emplace_back(now, nearest_collision_point); - planner_data.stop_require = planner_data.found_collision_points; + break; + } + } + } - mutex_.lock(); - const auto object_ptr = object_ptr_; - const auto current_velocity_ptr = current_velocity_ptr_; - mutex_.unlock(); + 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); - 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); + 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.lateral_margin); + 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); - if (planner_data.stop_require) { - last_detect_time_collision_point_ = trajectory_header.stamp; - } + planner_data.stop_require = planner_data.found_collision_points; - break; + mutex_.lock(); + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; + mutex_.unlock(); + + 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 Header & trajectory_header, - const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, - const StopParam & stop_param) + TrajectoryPoints & output, PlannerData & planner_data, + [[maybe_unused]] const Header & trajectory_header, const VehicleInfo & vehicle_info, + 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 @@ -502,7 +535,6 @@ 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::TargetStop); debug_ptr_->pushPose(getPose(current_stop_pos.point), PoseType::Stop); @@ -510,28 +542,13 @@ void ObstacleStopPlannerNode::insertVelocity( } else { insertStopPoint(stop_point, output, planner_data.stop_reason_diag); - latest_stop_point_ = stop_point; debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop); debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop); } } - } else if (!no_hunting_collision_point) { - if (latest_stop_point_) { - // update stop point index with the current trajectory - latest_stop_point_.get().index = findFirstNearestSegmentIndexWithSoftConstraints( - output, getPose(latest_stop_point_.get().point), node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag); - debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::TargetStop); - 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; @@ -556,19 +573,14 @@ 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) || - !no_hunting_slowdown_point) { + !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); } - } 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_) { @@ -604,7 +616,7 @@ void ObstacleStopPlannerNode::insertVelocity( set_velocity_limit_ ? std::numeric_limits::max() : target_velocity; insertSlowDownSection(slow_down_section, output); - } else if (no_hunting_slowdown_point) { + } else { latest_slow_down_section_ = {}; } }