Skip to content

Commit

Permalink
feat(obstacle_stop): upd obstacle hunting (#1068)
Browse files Browse the repository at this point in the history 
* Adapted from PR #1458

Signed-off-by: Shigekazu Fukuta <[email protected]>

* Adapted from PR #1627

Signed-off-by: Shigekazu Fukuta <[email protected]>

* fix parameter name

Signed-off-by: Shigekazu Fukuta <[email protected]>

* Adapted from PR autowarefoundation#2647

Signed-off-by: Shigekazu Fukuta <[email protected]>

* 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 <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@sfukuta @pre-commit-ci
sfukuta and pre-commit-ci[bot] authored Dec 19, 2023
1 parent 861c01e commit 65e903c
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 34 deletions.
6 changes: 6 additions & 0 deletions planning/obstacle_stop_planner/README.md
View file
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
2 changes: 2 additions & 0 deletions planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
View file
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
45 changes: 42 additions & 3 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
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, 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

Expand Down
99 changes: 68 additions & 31 deletions planning/obstacle_stop_planner/src/node.cpp
View file
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,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;
Expand Down Expand Up @@ -721,37 +725,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);
*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<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 +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_) {
Expand Down

0 comments on commit 65e903c

Please sign in to comment.