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.

Sign up for GitHub

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(out_of_lane): redesign to improve accuracy and performance #1475

Closed
wants to merge 1 commit into from
+1,271 −1,421
Closed

feat(out_of_lane): redesign to improve accuracy and performance #1475

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 9 additions & 0 deletions common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2504,6 +2504,15 @@ extern template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg:
const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
const double threshold = 0.0);

/// @brief calculate the time_from_start fields of the given trajectory points
/// @details this function assumes constant longitudinal velocity between points
/// @param trajectory trajectory for which to calculate the time_from_start
/// @param current_ego_point current ego position
/// @param min_velocity minimum velocity used for a trajectory point
void calculate_time_from_start(
std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f);

} // namespace autoware::motion_utils

#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_
23 changes: 23 additions & 0 deletions common/autoware_motion_utils/src/trajectory/trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -599,4 +599,27 @@ template bool isTargetPointFront<std::vector<autoware_planning_msgs::msg::Trajec
const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point,
const double threshold);

void calculate_time_from_start(
std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
const geometry_msgs::msg::Point & current_ego_point, const float min_velocity)
{
const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point);
if (nearest_segment_idx + 1 == trajectory.size()) {
return;
}
for (auto & p : trajectory) {
p.time_from_start = rclcpp::Duration::from_seconds(0);
}
// TODO(Maxime): some points can have very low velocities which introduce huge time errors
// Temporary solution: use a minimum velocity
for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) {
const auto & from = trajectory[idx - 1];
auto & to = trajectory[idx];
const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps);
if (velocity != 0.0) {
const auto t = universe_utils::calcDistance2d(from, to) / velocity;
to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start;
}
}
}
} // namespace autoware::motion_utils
37 changes: 14 additions & 23 deletions ...g/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,33 +129,24 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the
| -------------------- | ------ | ---------------------------------------------------------------- |
| `time_threshold` | double | [s] consider objects that will reach an overlap within this time |

| Parameter /intervals | Type | Description |
| --------------------- | ------ | ------------------------------------------------------- |
| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer |
| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer |

| Parameter /ttc | Type | Description |
| -------------- | ------ | ------------------------------------------------------------------------------------------------------ |
| `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap |

| Parameter /objects | Type | Description |
| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value |
| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered |
| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in |

| Parameter /overlap | Type | Description |
| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- |
| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered |
| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) |

| Parameter /action | Type | Description |
| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- |
| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed |
| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane |
| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap |
| `slowdown.velocity` | double | [m] slow down velocity |
| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap |
| Parameter /objects | Type | Description |
| ------------------------------- | ------ | --------------------------------------------------------------------- |
| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value |
| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered |

| Parameter /overlap | Type | Description |
| ------------------ | ------ | --------------------------------------------------------------------- |
| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered |

| Parameter /action | Type | Description |
| ----------------------------- | ------ | --------------------------------------------------------------------- |
| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap |
| `slowdown.velocity` | double | [m] slow down velocity |
| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap |

| Parameter /ego | Type | Description |
| -------------------- | ------ | ---------------------------------------------------- |
Expand Down
25 changes: 8 additions & 17 deletions ...elocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,32 +1,23 @@
/**:
ros__parameters:
out_of_lane: # module to stop or slowdown before overlapping another lane with other objects
mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc"
mode: ttc # mode used to consider a conflict with an object. "threshold", or "ttc"
skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane
ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored
max_arc_length: 100.0 # [m] maximum trajectory arc length that is checked for out_of_lane collisions

threshold:
time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time
intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego
ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer
objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer
ttc:
threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap

objects:
minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored
use_predicted_paths: true # if true, use the predicted paths to estimate future positions.
# if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value.
distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights
ignore_behind_ego: true # if true, objects behind the ego vehicle are ignored

overlap:
minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered
extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times)

action: # action to insert in the trajectory if an object causes a conflict at an overlap
skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed
precision: 0.1 # [m] precision when inserting a stop pose in the trajectory
longitudinal_distance_buffer: 1.5 # [m] safety distance buffer to keep in front of the ego vehicle
lateral_distance_buffer: 1.0 # [m] safety distance buffer to keep on the side of the ego vehicle
Expand All @@ -38,8 +29,8 @@
distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap

ego:
min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego
extra_front_offset: 0.0 # [m] extra front distance
extra_rear_offset: 0.0 # [m] extra rear distance
extra_right_offset: 0.0 # [m] extra right distance
extra_left_offset: 0.0 # [m] extra left distance
# extra footprint offsets to calculate out of lane collisions
extra_front_offset: 0.0 # [m] extra footprint front distance
extra_rear_offset: 0.0 # [m] extra footprint rear distance
extra_right_offset: 0.0 # [m] extra footprint right distance
extra_left_offset: 0.0 # [m] extra footprint left distance
131 changes: 87 additions & 44 deletions ...ity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,74 +15,117 @@
#include "calculate_slowdown_points.hpp"

#include "footprint.hpp"
#include "types.hpp"

#include <autoware/motion_utils/trajectory/interpolation.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/geometry/boost_geometry.hpp>

#include <boost/geometry/algorithms/overlaps.hpp>
#include <autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <boost/geometry/algorithms/disjoint.hpp>

#include <lanelet2_core/Forward.h>
#include <lanelet2_core/geometry/Polygon.h>

#include <algorithm>
#include <optional>
#include <vector>

namespace autoware::motion_velocity_planner::out_of_lane
{

bool can_decelerate(
const EgoData & ego_data, const TrajectoryPoint & point, const double target_vel)
std::optional<geometry_msgs::msg::Pose> calculate_last_avoiding_pose(
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
const universe_utils::Polygon2d & footprint, const lanelet::BasicPolygons2d & polygons_to_avoid,
const double min_arc_length, const double max_arc_length, const double precision)
{
// TODO(Maxime): use the library function
const auto dist_ahead_of_ego = autoware::motion_utils::calcSignedArcLength(
ego_data.trajectory_points, ego_data.pose.position, point.pose.position);
const auto acc_to_target_vel =
(ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego);
return acc_to_target_vel < std::abs(ego_data.max_decel);
geometry_msgs::msg::Pose interpolated_pose{};
bool is_avoiding_pose = false;

auto from = min_arc_length;
auto to = max_arc_length;
while (to - from > precision) {
auto l = from + 0.5 * (to - from);
interpolated_pose = motion_utils::calcInterpolatedPose(trajectory, l);
const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose);
is_avoiding_pose =
std::all_of(polygons_to_avoid.begin(), polygons_to_avoid.end(), [&](const auto & polygon) {
return boost::geometry::disjoint(interpolated_footprint, polygon);
});
if (is_avoiding_pose) {
from = l;
} else {
to = l;
}
}
if (is_avoiding_pose) {
return interpolated_pose;
}
return std::nullopt;
}

std::optional<TrajectoryPoint> calculate_last_in_lane_pose(
const EgoData & ego_data, const Slowdown & decision,
const autoware::universe_utils::Polygon2d & footprint,
const std::optional<SlowdownToInsert> & prev_slowdown_point, const PlannerParam & params)
std::optional<geometry_msgs::msg::Pose> calculate_pose_ahead_of_collision(
const EgoData & ego_data, const OutOfLanePoint & point_to_avoid,
const universe_utils::Polygon2d & footprint, const double precision)
{
const auto from_arc_length = autoware::motion_utils::calcSignedArcLength(
ego_data.trajectory_points, 0, ego_data.first_trajectory_idx);
const auto to_arc_length = autoware::motion_utils::calcSignedArcLength(
ego_data.trajectory_points, 0, decision.target_trajectory_idx);
TrajectoryPoint interpolated_point;
for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) {
// TODO(Maxime): binary search
interpolated_point.pose =
autoware::motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l);
const auto respect_decel_limit =
!params.skip_if_over_max_decel || prev_slowdown_point ||
can_decelerate(ego_data, interpolated_point, decision.velocity);
const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.pose);
const auto is_overlap_lane = boost::geometry::overlaps(
interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon());
const auto is_overlap_extra_lane =
prev_slowdown_point &&
boost::geometry::overlaps(
interpolated_footprint,
prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon());
if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane)
return interpolated_point;
const auto first_avoid_arc_length = motion_utils::calcSignedArcLength(
ego_data.trajectory_points, 0UL, point_to_avoid.trajectory_index);
for (auto l = first_avoid_arc_length - precision; l >= ego_data.min_stop_arc_length;
l -= precision) {
const auto interpolated_pose =
motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l);
const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose);
if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) {
return interpolated_pose;
}
}
return std::nullopt;
}

std::optional<SlowdownToInsert> calculate_slowdown_point(
const EgoData & ego_data, const std::vector<Slowdown> & decisions,
const std::optional<SlowdownToInsert> & prev_slowdown_point, PlannerParam params)
std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params)
{
const auto point_to_avoid_it = std::find_if(
out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(),
[&](const auto & p) { return p.to_avoid; });
if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) {
return std::nullopt;
}
const auto raw_footprint = make_base_footprint(params, true); // ignore extra footprint offsets
const auto base_footprint = make_base_footprint(params);
params.extra_front_offset += params.lon_dist_buffer;
params.extra_right_offset += params.lat_dist_buffer;
params.extra_left_offset += params.lat_dist_buffer;
const auto base_footprint = make_base_footprint(params);
const auto expanded_footprint = make_base_footprint(params); // with added distance buffers
lanelet::BasicPolygons2d polygons_to_avoid;
for (const auto & ll : point_to_avoid_it->overlapped_lanelets) {
polygons_to_avoid.push_back(ll.polygon2d().basicPolygon());
}
// points are ordered by trajectory index so the first one has the smallest index and arc length
const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index;
const auto first_outside_arc_length =
motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx);

std::optional<geometry_msgs::msg::Pose> slowdown_point;
// search for the first slowdown decision for which a stop point can be inserted
for (const auto & decision : decisions) {
const auto last_in_lane_pose =
calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params);
if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose};
// we first try to use the expanded footprint (distance buffers + extra footprint offsets)
for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) {
slowdown_point = calculate_last_avoiding_pose(
ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length,
first_outside_arc_length, params.precision);
if (slowdown_point) {
break;
}
}
return std::nullopt;
// fallback to simply stopping ahead of the collision to avoid (regardless of being out of lane or
// not)
if (!slowdown_point) {
slowdown_point = calculate_pose_ahead_of_collision(
ego_data, *point_to_avoid_it, expanded_footprint, params.precision);
}
return slowdown_point;
}
} // namespace autoware::motion_velocity_planner::out_of_lane
Loading
Loading