Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(obstacle_stop): upd obstacle hunting #1068

Merged
merged 20 commits into from
Dec 19, 2023
Merged
Show file tree
Hide file tree
Changes from 17 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions planning/obstacle_stop_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@
#include <map>
#include <memory>
#include <mutex>
#include <utility>
#include <vector>

namespace motion_planning
Expand All @@ -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<pcl::PointXYZ>;

struct StopPoint
{
TrajectoryPoint point{};
Expand All @@ -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:
Expand All @@ -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)
};
Expand Down Expand Up @@ -184,12 +198,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node
std::unique_ptr<motion_planning::AdaptiveCruiseController> acc_controller_;
std::shared_ptr<ObstacleStopPlannerDebugNode> debug_ptr_;
std::shared_ptr<LowpassFilter1d> lpf_acc_{nullptr};
boost::optional<SlowDownSection> latest_slow_down_section_{};
boost::optional<SlowDownSection> latest_slow_down_section_{boost::none};
std::vector<ObstacleWithDetectionTime> 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};
Expand Down Expand Up @@ -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)
{
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

Expand Down
100 changes: 69 additions & 31 deletions planning/obstacle_stop_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ using tier4_autoware_utils::createPoint;
using tier4_autoware_utils::findNearestIndex;
using tier4_autoware_utils::getRPY;

using PointCloud = pcl::PointCloud<pcl::PointXYZ>;

namespace
{
rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr)
Expand Down Expand Up @@ -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<LowpassFilter1d>(0.0, p.lowpass_gain);
const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0);
Expand Down Expand Up @@ -503,7 +505,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
acc_controller_ = std::make_unique<motion_planning::AdaptiveCruiseController>(
this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m);
debug_ptr_ = std::make_shared<ObstacleStopPlannerDebugNode>(this, i.max_longitudinal_offset_m);
last_detection_time_ = this->now();

// Publishers
path_pub_ = this->create_publisher<Trajectory>("~/output/trajectory", 1);
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -662,6 +661,12 @@ void ObstacleStopPlannerNode::searchObstacle(
return;
}

const auto now = this->now();

if (current_velocity_ptr->twist.twist.linear.x == 0) {
yn-mrse marked this conversation as resolved.
Show resolved Hide resolved
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;
Expand Down Expand Up @@ -721,37 +726,79 @@ void ObstacleStopPlannerNode::searchObstacle(
new pcl::PointCloud<pcl::PointXYZ>);
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);

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, &nearest_collision_point,
&nearest_collision_point_time);

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<cv::Point2d> 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;
Expand Down Expand Up @@ -799,17 +846,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_) {
Expand Down
Loading