Skip to content

Commit

Permalink
Merge pull request #827 from tier4/feat/approaching-stop-on-curve
Browse files Browse the repository at this point in the history
feat(obstacle_cruise_planner): approching stop on curve (autowarefoundation#4952)
  • Loading branch information
takayuki5168 authored Sep 22, 2023
2 parents c3ed832 + dc65861 commit 729cc28
Show file tree
Hide file tree
Showing 6 changed files with 162 additions and 19 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,11 @@
nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
min_behavior_stop_margin: 3.0 # [m]
suppress_sudden_obstacle_stop: false
stop_on_curve:
enable_approaching: true # false
additional_safe_distance_margin: 0.0 # [m]
min_safe_distance_margin: 3.0 # [m]
suppress_sudden_obstacle_stop: true

stop_obstacle_type:
unknown: true
Expand Down Expand Up @@ -54,7 +58,7 @@
pedestrian: false

slow_down_obstacle_type:
unknown: true
unknown: false
car: true
truck: true
bus: true
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -104,12 +104,15 @@ struct StopObstacle : public TargetObstacleInterface
{
StopObstacle(
const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity,
const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point)
const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape,
const double arg_lon_velocity, const double arg_lat_velocity,
const geometry_msgs::msg::Point arg_collision_point)
: TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
shape(arg_shape),
collision_point(arg_collision_point)
{
}
Shape shape;
geometry_msgs::msg::Point collision_point;
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
bool enable_debug_info_;
bool enable_calculation_time_info_;
double min_behavior_stop_margin_;
bool enable_approaching_on_curve_;
double additional_safe_distance_margin_on_curve_;
double min_safe_distance_margin_on_curve_;
bool suppress_sudden_obstacle_stop_;

std::vector<int> stop_obstacle_types_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,11 +53,16 @@ class PlannerInterface

void setParam(
const bool enable_debug_info, const bool enable_calculation_time_info,
const double min_behavior_stop_margin, const bool suppress_sudden_obstacle_stop)
const double min_behavior_stop_margin, const double enable_approaching_on_curve,
const double additional_safe_distance_margin_on_curve,
const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop)
{
enable_debug_info_ = enable_debug_info;
enable_calculation_time_info_ = enable_calculation_time_info;
min_behavior_stop_margin_ = min_behavior_stop_margin;
enable_approaching_on_curve_ = enable_approaching_on_curve;
additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve;
min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve;
suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop;
}

Expand Down Expand Up @@ -102,6 +107,9 @@ class PlannerInterface
bool enable_calculation_time_info_{false};
LongitudinalInfo longitudinal_info_;
double min_behavior_stop_margin_;
bool enable_approaching_on_curve_;
double additional_safe_distance_margin_on_curve_;
double min_safe_distance_margin_on_curve_;
bool suppress_sudden_obstacle_stop_;

// stop watch
Expand Down Expand Up @@ -194,6 +202,8 @@ class PlannerInterface
std::optional<geometry_msgs::msg::Pose> start_point{std::nullopt};
std::optional<geometry_msgs::msg::Pose> end_point{std::nullopt};
};
double calculateMarginFromObstacleOnCurve(
const PlannerData & planner_data, const StopObstacle & stop_obstacle) const;
double calculateSlowDownVelocity(
const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output) const;
std::optional<std::tuple<double, double, double>> calculateDistanceToSlowDownWithConstraints(
Expand Down
24 changes: 21 additions & 3 deletions planning/obstacle_cruise_planner/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -397,11 +397,18 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
}

min_behavior_stop_margin_ = declare_parameter<double>("common.min_behavior_stop_margin");
additional_safe_distance_margin_on_curve_ =
declare_parameter<double>("common.stop_on_curve.additional_safe_distance_margin");
enable_approaching_on_curve_ =
declare_parameter<bool>("common.stop_on_curve.enable_approaching");
min_safe_distance_margin_on_curve_ =
declare_parameter<double>("common.stop_on_curve.min_safe_distance_margin");
suppress_sudden_obstacle_stop_ =
declare_parameter<bool>("common.suppress_sudden_obstacle_stop");
planner_ptr_->setParam(
enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
suppress_sudden_obstacle_stop_);
enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);
}

{ // stop/cruise/slow down obstacle type
Expand Down Expand Up @@ -438,9 +445,20 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
parameters, "common.enable_debug_info", enable_debug_info_);
tier4_autoware_utils::updateParam<bool>(
parameters, "common.enable_calculation_time_info", enable_calculation_time_info_);

tier4_autoware_utils::updateParam<bool>(
parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_);
tier4_autoware_utils::updateParam<double>(
parameters, "common.stop_on_curve.additional_safe_distance_margin",
additional_safe_distance_margin_on_curve_);
tier4_autoware_utils::updateParam<double>(
parameters, "common.stop_on_curve.min_safe_distance_margin",
min_safe_distance_margin_on_curve_);

planner_ptr_->setParam(
enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
suppress_sudden_obstacle_stop_);
enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);

tier4_autoware_utils::updateParam<bool>(
parameters, "common.enable_slow_down_planning", enable_slow_down_planning_);
Expand Down Expand Up @@ -911,7 +929,7 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
}

const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,
return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape,
tangent_vel, normal_vel, *collision_point};
}

Expand Down
127 changes: 116 additions & 11 deletions planning/obstacle_cruise_planner/src/planner_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,10 @@
#include "obstacle_cruise_planner/planner_interface.hpp"

#include "motion_utils/distance/distance.hpp"
#include "motion_utils/marker/marker_helper.hpp"
#include "motion_utils/resample/resample.hpp"
#include "motion_utils/trajectory/tmp_conversion.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
#include "signal_processing/lowpass_filter_1d.hpp"

namespace
Expand Down Expand Up @@ -204,6 +208,19 @@ double calcDecelerationVelocityFromDistanceToTarget(
}
return current_velocity;
}

std::vector<TrajectoryPoint> resampleTrajectoryPoints(
const std::vector<TrajectoryPoint> & traj_points, const double interval)
{
const auto traj = motion_utils::convertToTrajectory(traj_points);
const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval);
return motion_utils::convertToTrajectoryPointArray(resampled_traj);
}

tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p)
{
return tier4_autoware_utils::Point2d{p.x, p.y};
}
} // namespace

std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
Expand Down Expand Up @@ -259,16 +276,19 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0);
const double dist_to_ego = -negative_dist_to_ego;

const double margin_from_obstacle =
calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle);

// If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin
// we set closest_obstacle_stop_distance to closest_behavior_stop_distance
const double margin_from_obstacle = [&]() {
const double margin_from_obstacle_considering_behavior_module = [&]() {
const size_t nearest_segment_idx =
findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
const auto closest_behavior_stop_idx =
motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1);

if (!closest_behavior_stop_idx) {
return longitudinal_info_.safe_distance_margin;
return margin_from_obstacle;
}

const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength(
Expand All @@ -282,29 +302,28 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
abs_ego_offset;
const double stop_dist_diff =
closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
if (stop_dist_diff < longitudinal_info_.safe_distance_margin) {
if (stop_dist_diff < margin_from_obstacle) {
// Use terminal margin (terminal_safe_distance_margin) for obstacle stop
return longitudinal_info_.terminal_safe_distance_margin;
}
} else {
const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego -
longitudinal_info_.safe_distance_margin -
abs_ego_offset;
const double closest_obstacle_stop_dist_from_ego =
closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset;
const double stop_dist_diff =
closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) {
if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) {
// Use shorter margin (min_behavior_stop_margin) for obstacle stop
return min_behavior_stop_margin_;
}
}
return longitudinal_info_.safe_distance_margin;
return margin_from_obstacle;
}();

const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() {
// Check feasibility to stop
if (suppress_sudden_obstacle_stop_) {
const double closest_obstacle_stop_dist =
closest_obstacle_dist - margin_from_obstacle - abs_ego_offset;
closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset;

// Calculate feasible stop margin (Check the feasibility)
const double feasible_stop_dist = calcMinimumDistanceToStop(
Expand All @@ -314,11 +333,12 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(

if (closest_obstacle_stop_dist < feasible_stop_dist) {
const auto feasible_margin_from_obstacle =
margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist);
margin_from_obstacle_considering_behavior_module -
(feasible_stop_dist - closest_obstacle_stop_dist);
return std::make_pair(feasible_margin_from_obstacle, true);
}
}
return std::make_pair(margin_from_obstacle, false);
return std::make_pair(margin_from_obstacle_considering_behavior_module, false);
}();

// Generate Output Trajectory
Expand Down Expand Up @@ -395,6 +415,91 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
return output_traj_points;
}

double PlannerInterface::calculateMarginFromObstacleOnCurve(
const PlannerData & planner_data, const StopObstacle & stop_obstacle) const
{
if (!enable_approaching_on_curve_) {
return longitudinal_info_.safe_distance_margin;
}

const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m);

// calculate short trajectory points towards obstacle
const size_t obj_segment_idx =
motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point);
std::vector<TrajectoryPoint> short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)};
double sum_short_traj_length{0.0};
for (int i = obj_segment_idx; 0 <= i; --i) {
short_traj_points.push_back(planner_data.traj_points.at(i));

if (
1 < short_traj_points.size() &&
longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) {
break;
}
sum_short_traj_length += tier4_autoware_utils::calcDistance2d(
planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1));
}
std::reverse(short_traj_points.begin(), short_traj_points.end());
if (short_traj_points.size() < 2) {
return longitudinal_info_.safe_distance_margin;
}

// calculate collision index between straight line from ego pose and object
const auto calculate_distance_from_straight_ego_path =
[&](const auto & ego_pose, const auto & object_polygon) {
const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose(
ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0);
const auto ego_straight_segment = tier4_autoware_utils::Segment2d{
convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)};
return boost::geometry::distance(ego_straight_segment, object_polygon);
};
const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5);
const auto object_polygon =
tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape);
const auto collision_idx = [&]() -> std::optional<size_t> {
for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) {
const double dist_to_obj = calculate_distance_from_straight_ego_path(
resampled_short_traj_points.at(i).pose, object_polygon);
if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) {
return i;
}
}
return std::nullopt;
}();
if (!collision_idx) {
return min_safe_distance_margin_on_curve_;
}
if (*collision_idx == 0) {
return longitudinal_info_.safe_distance_margin;
}

// calculate margin from obstacle
const double partial_segment_length = [&]() {
const double collision_segment_length = tier4_autoware_utils::calcDistance2d(
resampled_short_traj_points.at(*collision_idx - 1),
resampled_short_traj_points.at(*collision_idx));
const double prev_dist = calculate_distance_from_straight_ego_path(
resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon);
const double next_dist = calculate_distance_from_straight_ego_path(
resampled_short_traj_points.at(*collision_idx).pose, object_polygon);
return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) *
collision_segment_length;
}();

const double short_margin_from_obstacle =
partial_segment_length +
motion_utils::calcSignedArcLength(
resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) -
abs_ego_offset + additional_safe_distance_margin_on_curve_;

return std::min(
longitudinal_info_.safe_distance_margin,
std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle));
}

double PlannerInterface::calcDistanceToCollisionPoint(
const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point)
{
Expand Down

0 comments on commit 729cc28

Please sign in to comment.