From a87d818ee2ecb9adf66daaa578477780672a6f5c Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 21 Jan 2025 15:51:44 +0900 Subject: [PATCH] Ignore objects on trajectory Signed-off-by: Maxime CLEMENT --- .../src/footprints.hpp | 29 +++++++++++++- .../src/objects_filtering.hpp | 39 ++++++++++++++++--- .../src/parameters.hpp | 9 ++--- .../src/run_out_module.cpp | 17 ++++---- .../src/slowdown.hpp | 1 + .../src/types.hpp | 9 +++++ 6 files changed, 85 insertions(+), 19 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/footprints.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/footprints.hpp index 6f0fa36f807f8..a382c2ec90316 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/footprints.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/footprints.hpp @@ -17,13 +17,18 @@ #include "types.hpp" +#include #include #include #include #include +#include +#include + #include +#include #include @@ -53,7 +58,7 @@ inline std::vector calculate_object_footprints( return footprints; } -run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint( +inline run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint( const std::vector & trajectory, autoware::vehicle_info_utils::VehicleInfo vehicle_info, const run_out::Parameters & params) { @@ -82,6 +87,28 @@ run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint( footprint.rear_left_ls.emplace_back( base_link.x() + rotated_rear_left_offset.x(), base_link.y() + rotated_rear_left_offset.y()); } + + universe_utils::Polygon2d polygon_front; + for (const auto & p : footprint.front_left_ls) { + polygon_front.outer().push_back(p); + } + std::for_each( + footprint.front_right_ls.rbegin(), footprint.front_right_ls.rend(), + [&](const auto & p) { polygon_front.outer().push_back(p); }); + boost::geometry::correct(polygon_front); + universe_utils::Polygon2d polygon_rear; + for (const auto & p : footprint.rear_left_ls) { + polygon_rear.outer().push_back(p); + } + std::for_each( + footprint.rear_right_ls.rbegin(), footprint.rear_right_ls.rend(), + [&](const auto & p) { polygon_rear.outer().push_back(p); }); + boost::geometry::correct(polygon_rear); + universe_utils::MultiPolygon2d unions; + boost::geometry::union_(polygon_front, polygon_rear, unions); + if (!unions.empty()) { + footprint.polygon = unions.front(); + } return footprint; } } // namespace autoware::motion_velocity_planner::run_out diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/objects_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/objects_filtering.hpp index 51e344d502bf8..3054cc361b7ac 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/objects_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/objects_filtering.hpp @@ -16,13 +16,18 @@ #define OBJECTS_FILTERING_HPP_ #include "parameters.hpp" +#include "types.hpp" +#include +#include #include #include #include #include +#include + #include #include @@ -45,13 +50,33 @@ inline bool has_classification_label( return false; } +inline bool is_on_ego_trajectory( + const autoware_perception_msgs::msg::PredictedObject & object, + const TrajectoryCornerFootprint & ego_trajectory) +{ + const auto half_length = object.shape.dimensions.x; + const auto object_polygon = universe_utils::toFootprint( + object.kinematics.initial_pose_with_covariance.pose, half_length, half_length, + object.shape.dimensions.y); + return boost::geometry::overlaps(object_polygon, ego_trajectory.polygon); +} + inline bool skip_object_condition( - const autoware_perception_msgs::msg::PredictedObject & object, const Parameters & params) + const autoware_perception_msgs::msg::PredictedObject & object, + const std::optional & prev_decisions, + const TrajectoryCornerFootprint & ego_trajectory, const Parameters & params) { + const auto & is_previous_target = + prev_decisions && (prev_decisions->decisions.back().type == stop || + (prev_decisions->decisions.back().collision.has_value() && + prev_decisions->decisions.back().collision->type == collision)); + if (!is_previous_target && is_on_ego_trajectory(object, ego_trajectory)) { + return true; + } if (!has_classification_label(object, params.objects_target_labels)) { - return false; + return true; } - return true; + return false; } inline void filter_predicted_paths( @@ -62,7 +87,9 @@ inline void filter_predicted_paths( } inline std::vector filter_dynamic_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const Parameters & params) + const autoware_perception_msgs::msg::PredictedObjects & objects, + const ObjectDecisionsTracker & previous_decisions, + const TrajectoryCornerFootprint & ego_trajectory, const Parameters & params) { std::vector filtered_objects; std::vector parked_objects; @@ -76,7 +103,9 @@ inline std::vector filter_dynami } continue; } - if (skip_object_condition(filtered_object, params)) { + const auto & previous_object_decisions = + previous_decisions.get(universe_utils::toHexString(object.object_id)); + if (skip_object_condition(filtered_object, previous_object_decisions, ego_trajectory, params)) { continue; } filter_predicted_paths(filtered_object, params); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/parameters.hpp index 2c18afaa72e2c..5dc37eafcbb4a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/parameters.hpp @@ -54,10 +54,9 @@ struct Parameters void initialize(rclcpp::Node & node, const std::string & ns) { using universe_utils::getOrDeclareParameter; - enable_passing_collisions = - getOrDeclareParameter(node, ns + ".enable_passing_collisions"); + enable_passing_collisions = getOrDeclareParameter(node, ns + ".passing.enable"); passing_collisions_time_margin = - getOrDeclareParameter(node, ns + ".passing_collisions_time_margin"); + getOrDeclareParameter(node, ns + ".passing.time_margin"); stop_off_time_buffer = getOrDeclareParameter(node, ns + ".stop_off_time_buffer"); stop_on_time_buffer = getOrDeclareParameter(node, ns + ".stop_on_time_buffer"); stop_distance_buffer = getOrDeclareParameter(node, ns + ".stop_distance_buffer"); @@ -78,8 +77,8 @@ struct Parameters using universe_utils::updateParam; updateParam(params, ns + ".stop_on_time_buffer", stop_on_time_buffer); updateParam(params, ns + ".stop_off_time_buffer", stop_off_time_buffer); - updateParam(params, ns + ".enable_passing_collisions", enable_passing_collisions); - updateParam(params, ns + ".passing_collisions_time_margin", passing_collisions_time_margin); + updateParam(params, ns + ".passing.enable", enable_passing_collisions); + updateParam(params, ns + ".passing.time_margin", passing_collisions_time_margin); updateParam(params, ns + ".stop_off_time_buffer", stop_off_time_buffer); updateParam(params, ns + ".stop_on_time_buffer", stop_on_time_buffer); updateParam(params, ns + ".stop_distance_buffer", stop_distance_buffer); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp index ad154ec7ba849..826231da1ba72 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/run_out_module.cpp @@ -144,23 +144,24 @@ VelocityPlanningResult RunOutModule::plan( { const auto now = clock_->now(); time_keeper_->start_track("plan()"); - time_keeper_->start_track("filter_objects()"); - const auto filtered_objects = - run_out::filter_dynamic_objects(planner_data->predicted_objects, params_); - time_keeper_->end_track("filter_objects()"); time_keeper_->start_track("calc_ego_footprint()"); const auto ego_footprint = calculate_trajectory_corner_footprint( ego_trajectory_points, planner_data->vehicle_info_, params_); time_keeper_->end_track("calc_ego_footprint()"); + time_keeper_->start_track("filter_objects()"); + const auto filtered_objects = run_out::filter_dynamic_objects( + planner_data->predicted_objects, decisions_tracker_, ego_footprint, params_); + time_keeper_->end_track("filter_objects()"); time_keeper_->start_track("calc_obj_footprint()"); - const auto object_footprints = run_out::calculate_object_footprints(filtered_objects); + const auto object_footprints = + run_out::calculate_object_footprints(filtered_objects); // TODO(Maxime): delete if not used time_keeper_->end_track("calc_obj_footprint()"); time_keeper_->start_track("calc_rtree()"); const auto footprint_rtree = run_out::prepare_trajectory_footprint_rtree(ego_footprint); time_keeper_->end_track("calc_rtree()"); time_keeper_->start_track("calc_collisions()"); auto collisions_per_object = run_out::calculate_collisions_per_object( - planner_data->predicted_objects.objects, footprint_rtree, ego_trajectory_points, params_); + filtered_objects, footprint_rtree, ego_trajectory_points, params_); time_keeper_->end_track("calc_collisions()"); time_keeper_->start_track("calc_decisions()"); run_out::calculate_decisions(decisions_tracker_, collisions_per_object, now, params_); @@ -168,14 +169,14 @@ VelocityPlanningResult RunOutModule::plan( time_keeper_->start_track("calc_slowdowns()"); const auto result = run_out::calculate_slowdowns( decisions_tracker_, ego_trajectory_points, - planner_data->vehicle_info_.front_overhang_m + params_.stop_distance_buffer); + params_.stop_distance_buffer + planner_data->vehicle_info_.max_longitudinal_offset_m); check_unavoidable_collision(calculate_comfortable_time_to_stop( ego_trajectory_points, planner_data->calculate_min_deceleration_distance(0.0))); time_keeper_->end_track("calc_slowdowns()"); time_keeper_->start_track("publish_debug()"); virtual_wall_marker_creator.add_virtual_walls(run_out::create_virtual_walls( - result, ego_trajectory_points, planner_data->vehicle_info_.front_overhang_m)); + result, ego_trajectory_points, planner_data->vehicle_info_.max_longitudinal_offset_m)); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(now)); debug_publisher_->publish( run_out::make_debug_footprint_markers(ego_footprint, object_footprints)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/slowdown.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/slowdown.hpp index 03b438d9a30c5..d8eef443b2389 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/slowdown.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/slowdown.hpp @@ -51,6 +51,7 @@ inline geometry_msgs::msg::Point interpolated_point_at_time( return universe_utils::calcInterpolatedPoint( prev_it->pose.position, std::next(prev_it)->pose.position, ratio); } + inline VelocityPlanningResult calculate_slowdowns( ObjectDecisionsTracker & decision_tracker, const std::vector & trajectory, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/types.hpp index 466fd0f741620..c96d769e7d7dd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/types.hpp @@ -38,6 +38,7 @@ struct TrajectoryCornerFootprint universe_utils::LineString2d front_right_ls; universe_utils::LineString2d rear_left_ls; universe_utils::LineString2d rear_right_ls; + universe_utils::Polygon2d polygon; }; struct ObjectFootprint @@ -215,6 +216,14 @@ struct ObjectDecisionsTracker } } } + + std::optional get(const std::string & object) const + { + if (history_per_object.count(object) < 1) { + return std::nullopt; + } + return history_per_object.at(object); + } }; struct UnavoidableCollision