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

fix(out_of_lane): improve stop decision #866

Merged
merged 2 commits into from
Sep 25, 2023
Merged
Show file tree
Hide file tree
Changes from 1 commit
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
8 changes: 1 addition & 7 deletions planning/behavior_velocity_out_of_lane_module/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,7 @@ autoware_package()
pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)

ament_auto_add_library(${PROJECT_NAME} SHARED
src/debug.cpp
src/decisions.cpp
src/footprint.cpp
src/lanelets_selection.cpp
src/manager.cpp
src/overlapping_range.cpp
src/scene_out_of_lane.cpp
DIRECTORY src
)

ament_auto_package(INSTALL_TO_SHARE config)
15 changes: 7 additions & 8 deletions planning/behavior_velocity_out_of_lane_module/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -149,14 +149,13 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the
| `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 |
| `strict` | bool | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane |
| `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 /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 /ego | Type | Description |
| -------------------- | ------ | ---------------------------------------------------- |
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,7 @@

action: # action to insert in the path 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
strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego
# if false, ego stops just before entering a lane but may then be overlapping another lane.
precision: 0.1 # [m] precision when inserting a stop pose in the path
distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane
slowdown:
distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
Expand All @@ -34,6 +33,7 @@
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
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
// Copyright 2023 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_
#define CALCULATE_SLOWDOWN_POINTS_HPP_

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

#include <motion_utils/trajectory/trajectory.hpp>

#include <optional>
#include <vector>

namespace behavior_velocity_planner::out_of_lane
{

bool can_decelerate(
const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel)
{
if (ego_data.velocity < 0.5) return true;
const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength(
ego_data.path.points, ego_data.pose.position, point.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);
}

std::optional<PathPointWithLaneId> calculate_last_in_lane_pose(
const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint,
const PlannerParam & params)
{
const auto from_arc_length =
motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx);
const auto to_arc_length =
motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx);
PathPointWithLaneId interpolated_point;
for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) {
// TODO(Maxime): binary search
interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l);
const auto respect_decel_limit =
!params.skip_if_over_max_decel ||
can_decelerate(ego_data, interpolated_point, decision.velocity);
if (
respect_decel_limit && !boost::geometry::overlaps(
project_to_pose(footprint, interpolated_point.point.pose),
decision.lane_to_avoid.polygon2d().basicPolygon()))
return interpolated_point;
}
return std::nullopt;
}

/// @brief calculate the slowdown point to insert in the path
/// @param ego_data ego data (path, velocity, etc)
/// @param decisions decision (before which point to stop, what lane to avoid entering, etc)
/// @param params parameters
/// @return optional slowdown point to insert in the path
std::optional<SlowdownToInsert> calculate_slowdown_point(
const EgoData & ego_data, const std::vector<Slowdown> & decisions, PlannerParam params)
{
params.extra_front_offset += params.dist_buffer;
const auto base_footprint = make_base_footprint(params);

// 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, params);
if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose};
}
return std::nullopt;
}
} // namespace behavior_velocity_planner::out_of_lane
#endif // CALCULATE_SLOWDOWN_POINTS_HPP_
39 changes: 19 additions & 20 deletions planning/behavior_velocity_out_of_lane_module/src/decisions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,15 +27,15 @@ namespace behavior_velocity_planner::out_of_lane
double distance_along_path(const EgoData & ego_data, const size_t target_idx)
{
return motion_utils::calcSignedArcLength(
ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx);
ego_data.path.points, ego_data.pose.position, ego_data.first_path_idx + target_idx);
}

double time_along_path(const EgoData & ego_data, const size_t target_idx)
double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity)
{
const auto dist = distance_along_path(ego_data, target_idx);
const auto v = std::max(
ego_data.velocity,
ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps *
std::max(ego_data.velocity, min_velocity),
ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps *
0.5);
return dist / v;
}
Expand All @@ -55,8 +55,7 @@ bool object_is_incoming(

std::optional<std::pair<double, double>> object_time_to_range(
const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
const std::shared_ptr<route_handler::RouteHandler> route_handler, const double min_confidence,
const rclcpp::Logger & logger)
const std::shared_ptr<route_handler::RouteHandler> route_handler, const rclcpp::Logger & logger)
{
// skip the dynamic object if it is not in a lane preceding the overlapped lane
// lane changes are intentionally not considered
Expand All @@ -65,13 +64,12 @@ std::optional<std::pair<double, double>> object_time_to_range(
object.kinematics.initial_pose_with_covariance.pose.position.y);
if (!object_is_incoming(object_point, route_handler, range.lane)) return {};

const auto max_deviation = object.shape.dimensions.y * 2.0;
const auto max_deviation = object.shape.dimensions.y + range.inside_distance;

auto worst_enter_time = std::optional<double>();
auto worst_exit_time = std::optional<double>();

for (const auto & predicted_path : object.kinematics.predicted_paths) {
if (predicted_path.confidence < min_confidence) continue;
const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds();
const auto enter_point =
geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y());
Expand Down Expand Up @@ -119,11 +117,9 @@ std::optional<std::pair<double, double>> object_time_to_range(

const auto same_driving_direction_as_ego = enter_time < exit_time;
if (same_driving_direction_as_ego) {
RCLCPP_DEBUG(logger, " / SAME DIR \\\n");
worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time;
worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time;
} else {
RCLCPP_DEBUG(logger, " / OPPOSITE DIR \\\n");
worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time;
worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time;
}
Expand Down Expand Up @@ -205,8 +201,11 @@ std::optional<std::pair<double, double>> object_time_to_range(

bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params)
{
return std::min(range_times.object.enter_time, range_times.object.exit_time) <
params.time_threshold;
const auto enter_within_threshold =
range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold;
const auto exit_within_threshold =
range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold;
return enter_within_threshold || exit_within_threshold;
}

bool intervals_condition(
Expand Down Expand Up @@ -281,8 +280,10 @@ bool should_not_enter(
const rclcpp::Logger & logger)
{
RangeTimes range_times{};
range_times.ego.enter_time = time_along_path(inputs.ego_data, range.entering_path_idx);
range_times.ego.exit_time = time_along_path(inputs.ego_data, range.exiting_path_idx);
range_times.ego.enter_time =
time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity);
range_times.ego.exit_time =
time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity);
RCLCPP_DEBUG(
logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx,
range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time);
Expand All @@ -298,8 +299,7 @@ bool should_not_enter(
// skip objects that are already on the interval
const auto enter_exit_time =
params.objects_use_predicted_paths
? object_time_to_range(
object, range, inputs.route_handler, params.objects_min_confidence, logger)
? object_time_to_range(object, range, inputs.route_handler, logger)
: object_time_to_range(object, range, inputs, logger);
if (!enter_exit_time) {
RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n");
Expand Down Expand Up @@ -349,11 +349,10 @@ std::optional<Slowdown> calculate_decision(
{
std::optional<Slowdown> decision;
if (should_not_enter(range, inputs, params, logger)) {
const auto stop_before_range = params.strict ? find_most_preceding_range(range, inputs) : range;
decision.emplace();
decision->target_path_idx = inputs.ego_data.first_path_idx +
stop_before_range.entering_path_idx; // add offset from curr pose
decision->lane_to_avoid = stop_before_range.lane;
decision->target_path_idx =
inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose
decision->lane_to_avoid = range.lane;
const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx);
set_decision_velocity(decision, ego_dist_to_range, params);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@ double distance_along_path(const EgoData & ego_data, const size_t target_idx);
/// @brief estimate the time when ego will reach some target path index
/// @param [in] ego_data data related to the ego vehicle
/// @param [in] target_idx target ego path index
/// @param [in] min_velocity minimum ego velocity used to estimate the time
/// @return time taken by ego to reach the target [s]
double time_along_path(const EgoData & ego_data, const size_t target_idx);
/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit
Expand All @@ -57,14 +58,12 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx);
/// @param [in] object dynamic object
/// @param [in] range overlapping range
/// @param [in] route_handler route handler used to estimate the path of the dynamic object
/// @param [in] min_confidence minimum confidence to consider a predicted path
/// @param [in] logger ros logger
/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in
/// the opposite direction, time at enter > time at exit
std::optional<std::pair<double, double>> object_time_to_range(
const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range,
const std::shared_ptr<route_handler::RouteHandler> route_handler, const double min_confidence,
const rclcpp::Logger & logger);
const std::shared_ptr<route_handler::RouteHandler> route_handler, const rclcpp::Logger & logger);
/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit
/// points of an overlapping range
/// @param [in] object dynamic object
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
// Copyright 2023 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef FILTER_PREDICTED_OBJECTS_HPP_
#define FILTER_PREDICTED_OBJECTS_HPP_

#include "types.hpp"

#include <string>

namespace behavior_velocity_planner::out_of_lane
{
/// @brief filter predicted objects and their predicted paths
/// @param [in] objects predicted objects to filter
/// @param [in] ego_data ego data
/// @param [in] params parameters
/// @return filtered predicted objects
autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects(
const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data,
const PlannerParam & params)
{
autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects;
filtered_objects.header = objects.header;
for (const auto & object : objects.objects) {
const auto is_pedestrian =
std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
}) != object.classification.end();
if (is_pedestrian) continue;

auto filtered_object = object;
const auto is_invalid_predicted_path = [&](const auto & predicted_path) {
const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence;
const auto lat_offset_to_current_ego =
std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position));
const auto is_crossing_ego =
lat_offset_to_current_ego <=
object.shape.dimensions.y / 2.0 + std::max(
params.left_offset + params.extra_left_offset,
params.right_offset + params.extra_right_offset);
return is_low_confidence || is_crossing_ego;
};
if (params.objects_use_predicted_paths) {
auto & predicted_paths = filtered_object.kinematics.predicted_paths;
const auto new_end =
std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path);
predicted_paths.erase(new_end, predicted_paths.end());
}
if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty())
filtered_objects.objects.push_back(filtered_object);
}
return filtered_objects;
}

} // namespace behavior_velocity_planner::out_of_lane

#endif // FILTER_PREDICTED_OBJECTS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -58,9 +58,9 @@ std::vector<lanelet::BasicPolygon2d> calculate_path_footprints(
{
const auto base_footprint = make_base_footprint(params);
std::vector<lanelet::BasicPolygon2d> path_footprints;
path_footprints.reserve(ego_data.path->points.size());
for (auto i = ego_data.first_path_idx; i < ego_data.path->points.size(); ++i) {
const auto & path_pose = ego_data.path->points[i].point.pose;
path_footprints.reserve(ego_data.path.points.size());
for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) {
const auto & path_pose = ego_data.path.points[i].point.pose;
const auto angle = tf2::getYaw(path_pose.orientation);
const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle);
lanelet::BasicPolygon2d footprint;
Expand Down
Loading
Loading