From dffa14a2c2c21203672fb7cd276280f0cd4d3b49 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 21 Jan 2025 21:08:37 +0900 Subject: [PATCH] add Object struct and big refactoring Signed-off-by: Maxime CLEMENT --- .../src/collision.hpp | 40 +++---- .../src/debug.hpp | 113 +++++++++--------- .../src/decision.hpp | 14 +-- .../src/footprints.hpp | 24 ---- .../src/objects_filtering.hpp | 104 ++++++++++------ .../src/run_out_module.cpp | 16 +-- .../src/types.hpp | 22 ++-- 7 files changed, 164 insertions(+), 169 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/collision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/collision.hpp index 9105d8a42cff6..2411a2f94afc7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/collision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/collision.hpp @@ -91,24 +91,22 @@ inline FootprintIntersection calculate_footprint_intersection( inline std::vector> calculate_footprint_intersections_per_predicted_path( - const FootprintSegmentRtree & footprint_rtree, - const autoware_perception_msgs::msg::PredictedObject & object, + const FootprintSegmentRtree & footprint_rtree, const Object & object, const std::vector & ego_trajectory) { std::vector> intersections_per_predicted_path; - for (const auto & path : object.kinematics.predicted_paths) { + for (const auto & corner_footprint : object.corner_footprints) { std::vector intersections; - const auto time_step = rclcpp::Duration(path.time_step).seconds(); - const auto half_length = object.shape.dimensions.x * 0.5; - for (auto i = 0UL; i + 1 < path.path.size(); ++i) { - const auto object_polygon1 = universe_utils::toFootprint( - path.path[i], half_length, half_length, object.shape.dimensions.y); - const auto object_polygon2 = universe_utils::toFootprint( - path.path[i + 1], half_length, half_length, object.shape.dimensions.y); + for (auto i = 0UL; i + 1 < corner_footprint.front_left_ls.size(); ++i) { universe_utils::Segment2d segment; - for (auto corner_idx = 0UL; corner_idx < 4; ++corner_idx) { - segment.first = object_polygon1.outer()[corner_idx]; - segment.second = object_polygon2.outer()[corner_idx]; + for (auto & corner_ls : { + corner_footprint.front_left_ls, + corner_footprint.front_right_ls, + corner_footprint.rear_right_ls, + corner_footprint.rear_left_ls, + }) { + segment.first = corner_ls[i]; + segment.second = corner_ls[i + 1]; std::vector query_results; footprint_rtree.query( boost::geometry::index::intersects(segment), std::back_inserter(query_results)); @@ -118,7 +116,8 @@ calculate_footprint_intersections_per_predicted_path( if (intersection) { intersections.push_back(calculate_footprint_intersection( segment, *intersection, query_result.second, ego_trajectory, - {time_step * static_cast(i), time_step * (static_cast(i) + 1)})); + {corner_footprint.time_step * static_cast(i), + corner_footprint.time_step * (static_cast(i) + 1)})); } } } @@ -167,16 +166,12 @@ inline Collision calculate_collision( params}; } -inline std::vector calculate_collisions_per_object( - const std::vector & objects, - const run_out::FootprintSegmentRtree & ego_footprint_rtree, +inline void calculate_collisions( + std::vector & objects, const run_out::FootprintSegmentRtree & ego_footprint_rtree, const std::vector & ego_trajectory, const run_out::Parameters & params) { - std::vector collisions_per_object; - for (const auto & object : objects) { - collisions_per_object.emplace_back(); - collisions_per_object.back().object_uuid = universe_utils::toHexString(object.object_id); + for (auto & object : objects) { const auto intersections_per_predicted_path = run_out::calculate_footprint_intersections_per_predicted_path( ego_footprint_rtree, object, ego_trajectory); @@ -184,10 +179,9 @@ inline std::vector calculate_collisions_per_objec if (intersections.empty()) { continue; } - collisions_per_object.back().push_back(run_out::calculate_collision(intersections, params)); + object.collisions.push_back(run_out::calculate_collision(intersections, params)); } } - return collisions_per_object; } } // namespace autoware::motion_velocity_planner::run_out diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp index 8aa292e7f8259..7a5571618f3ae 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/debug.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include namespace autoware::motion_velocity_planner::run_out @@ -38,8 +38,7 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; inline MarkerArray make_debug_footprint_markers( - const run_out::TrajectoryCornerFootprint & ego, - const std::vector & objects) + const run_out::TrajectoryCornerFootprint & ego, const std::vector & objects) { MarkerArray markers; Marker m; @@ -76,66 +75,66 @@ inline MarkerArray make_debug_footprint_markers( m.ns = "objects_footprints"; m.color.r = 1.0; for (const auto & object : objects) { - const auto & o = object.corner_footprint; - for (auto i = 0UL; i + 1 < o.front_left_ls.size(); ++i) { - m.points.push_back( - universe_utils::createPoint(o.front_left_ls[i].x(), o.front_left_ls[i].y(), 0.0)); - m.points.push_back( - universe_utils::createPoint(o.front_left_ls[i + 1].x(), o.front_left_ls[i + 1].y(), 0.0)); - m.points.push_back( - universe_utils::createPoint(o.front_right_ls[i].x(), o.front_right_ls[i].y(), 0.0)); - m.points.push_back( - universe_utils::createPoint(o.front_right_ls[i + 1].x(), o.front_right_ls[i + 1].y(), 0.0)); - m.points.push_back( - universe_utils::createPoint(o.rear_left_ls[i].x(), o.rear_left_ls[i].y(), 0.0)); - m.points.push_back( - universe_utils::createPoint(o.rear_left_ls[i + 1].x(), o.rear_left_ls[i + 1].y(), 0.0)); - m.points.push_back( - universe_utils::createPoint(o.rear_right_ls[i].x(), o.rear_right_ls[i].y(), 0.0)); - m.points.push_back( - universe_utils::createPoint(o.rear_right_ls[i + 1].x(), o.rear_right_ls[i + 1].y(), 0.0)); - } + for (const auto & f : object.corner_footprints) + for (auto i = 0UL; i + 1 < f.front_left_ls.size(); ++i) { + m.points.push_back( + universe_utils::createPoint(f.front_left_ls[i].x(), f.front_left_ls[i].y(), 0.0)); + m.points.push_back( + universe_utils::createPoint(f.front_left_ls[i + 1].x(), f.front_left_ls[i + 1].y(), 0.0)); + m.points.push_back( + universe_utils::createPoint(f.front_right_ls[i].x(), f.front_right_ls[i].y(), 0.0)); + m.points.push_back(universe_utils::createPoint( + f.front_right_ls[i + 1].x(), f.front_right_ls[i + 1].y(), 0.0)); + m.points.push_back( + universe_utils::createPoint(f.rear_left_ls[i].x(), f.rear_left_ls[i].y(), 0.0)); + m.points.push_back( + universe_utils::createPoint(f.rear_left_ls[i + 1].x(), f.rear_left_ls[i + 1].y(), 0.0)); + m.points.push_back( + universe_utils::createPoint(f.rear_right_ls[i].x(), f.rear_right_ls[i].y(), 0.0)); + m.points.push_back( + universe_utils::createPoint(f.rear_right_ls[i + 1].x(), f.rear_right_ls[i + 1].y(), 0.0)); + } } markers.markers.push_back(m); return markers; } -inline MarkerArray make_debug_collisions_markers( - const std::vector & collisions_per_object) -{ - MarkerArray markers; - Marker m; - m.header.frame_id = "base_link"; - m.ns = "collisions"; - m.type = Marker::TEXT_VIEW_FACING; - m.color = universe_utils::createMarkerColor(1.0, 1.0, 0.0, 0.75); - m.scale = universe_utils::createMarkerScale(0.1, 0.0, 0.5); - std::stringstream ss; - ss << std::setprecision(4); - ss << std::fixed; - ss << std::left; - ss << std::setw(25) << "collision"; - ss << "|"; - ss << std::setw(20) << "ego_time_interval"; - ss << "|"; - ss << std::setw(20) << "object_time_interval"; - ss << "\n"; - for (const auto & collisions : collisions_per_object) { - for (const auto & collision : collisions) { - ss << std::setw(25) << magic_enum::enum_name(collision.type); - ss << "|"; - ss << std::setw(20) << collision.ego_time_interval; - ss << "|"; - ss << std::setw(20) << collision.object_time_interval; - ss << "\n"; - } - } - m.text = ss.str(); - std::cout << ss.str() << std::endl; - markers.markers.push_back(m); +// inline MarkerArray make_debug_collisions_markers( +// const std::vector & collisions_per_object) +// { +// MarkerArray markers; +// Marker m; +// m.header.frame_id = "base_link"; +// m.ns = "collisions"; +// m.type = Marker::TEXT_VIEW_FACING; +// m.color = universe_utils::createMarkerColor(1.0, 1.0, 0.0, 0.75); +// m.scale = universe_utils::createMarkerScale(0.1, 0.0, 0.5); +// std::stringstream ss; +// ss << std::setprecision(4); +// ss << std::fixed; +// ss << std::left; +// ss << std::setw(25) << "collision"; +// ss << "|"; +// ss << std::setw(20) << "ego_time_interval"; +// ss << "|"; +// ss << std::setw(20) << "object_time_interval"; +// ss << "\n"; +// for (const auto & collisions : collisions_per_object) { +// for (const auto & collision : collisions) { +// ss << std::setw(25) << magic_enum::enum_name(collision.type); +// ss << "|"; +// ss << std::setw(20) << collision.ego_time_interval; +// ss << "|"; +// ss << std::setw(20) << collision.object_time_interval; +// ss << "\n"; +// } +// } +// m.text = ss.str(); +// std::cout << ss.str() << std::endl; +// markers.markers.push_back(m); - return markers; -} +// return markers; +// } inline MarkerArray make_debug_decisions_markers(const ObjectDecisionsTracker & decisions_tracker) { diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/decision.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/decision.hpp index a3b052975e152..84beb464c6d9f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/decision.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_run_out_module/src/decision.hpp @@ -95,20 +95,18 @@ inline DecisionType calculate_decision_type( } inline void calculate_decisions( - ObjectDecisionsTracker & decisions_tracker, - const std::vector & collisions_per_object, const rclcpp::Time & now, - const Parameters & params) + ObjectDecisionsTracker & decisions_tracker, const std::vector & objects, + const rclcpp::Time & now, const Parameters & params) { - for (const auto & collisions : collisions_per_object) { + for (const auto & object : objects) { std::optional object_decision; - const auto & decision_history = decisions_tracker.history_per_object[collisions.object_uuid]; - for (const auto & collision : collisions) { + auto & decision_history = decisions_tracker.history_per_object[object.uuid]; + for (const auto & collision : object.collisions) { Decision d(collision, calculate_decision_type(collision.type, decision_history, now, params)); update_decision(object_decision, d); } if (object_decision) { - decisions_tracker.history_per_object[collisions.object_uuid].add_decision( - now, *object_decision); + decision_history.add_decision(now, *object_decision); } } decisions_tracker.remove_outdated(now, params.max_history_duration); 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 a382c2ec90316..45ae722641c8c 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 @@ -34,30 +34,6 @@ namespace autoware::motion_velocity_planner::run_out { -inline std::vector calculate_object_footprints( - const std::vector & objects) -{ - std::vector footprints; - // TODO(Maxime): handle objects with no shape if needed - for (const auto & object : objects) { - const auto uuid = universe_utils::toHexString(object.object_id); - const auto half_length = object.shape.dimensions.x * 0.5; - for (const auto & path : object.kinematics.predicted_paths) { - run_out::ObjectFootprint footprint(uuid); - for (const auto & p : path.path) { - const auto polygon = - universe_utils::toFootprint(p, half_length, half_length, object.shape.dimensions.y); - footprint.corner_footprint.front_left_ls.push_back(polygon.outer()[0]); - footprint.corner_footprint.front_right_ls.push_back(polygon.outer()[1]); - footprint.corner_footprint.rear_right_ls.push_back(polygon.outer()[2]); - footprint.corner_footprint.rear_left_ls.push_back(polygon.outer()[3]); - } - footprints.push_back(footprint); - } - } - return footprints; -} - inline run_out::TrajectoryCornerFootprint calculate_trajectory_corner_footprint( const std::vector & trajectory, autoware::vehicle_info_utils::VehicleInfo vehicle_info, const run_out::Parameters & params) 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 3054cc361b7ac..32e03351a4e2a 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 @@ -50,66 +50,94 @@ inline bool has_classification_label( return false; } +inline void classify( + Object & object, const autoware_perception_msgs::msg::PredictedObject & predicted_object, + const Parameters & params) +{ + if (has_classification_label(predicted_object, params.objects_parked_labels)) { + object.has_parked_label = true; + } + if (has_classification_label(predicted_object, params.objects_target_labels)) { + object.has_target_label = true; + } +} + +inline void calculate_current_footprint( + Object & object, const autoware_perception_msgs::msg::PredictedObject & predicted_object) +{ + const auto half_length = predicted_object.shape.dimensions.x * 0.5; + object.current_footprint = universe_utils::toFootprint( + predicted_object.kinematics.initial_pose_with_covariance.pose, half_length, half_length, + predicted_object.shape.dimensions.y); +} inline bool is_on_ego_trajectory( - const autoware_perception_msgs::msg::PredictedObject & object, - const TrajectoryCornerFootprint & ego_trajectory) + const Object & 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); + return boost::geometry::overlaps(object.current_footprint, ego_trajectory.polygon); } inline bool skip_object_condition( - const autoware_perception_msgs::msg::PredictedObject & object, - const std::optional & prev_decisions, - const TrajectoryCornerFootprint & ego_trajectory, const Parameters & params) + const Object & object, const std::optional & prev_decisions, + const Parameters & params) { + (void)params; + constexpr auto skip_object = true; 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 (!is_previous_target && object.is_on_ego_trajectory) { + return skip_object; } - if (!has_classification_label(object, params.objects_target_labels)) { - return true; + if (object.has_parked_label) { + return skip_object; } - return false; + if (!object.has_target_label) { + return skip_object; + } + return !skip_object; } inline void filter_predicted_paths( - autoware_perception_msgs::msg::PredictedObject & object, const Parameters & params) + Object & object, const autoware_perception_msgs::msg::PredictedObject & predicted_object, + const Parameters & params) { - (void)object; + // calculate footprint + for (const auto & path : predicted_object.kinematics.predicted_paths) { + ObjectCornerFootprint footprint; + footprint.time_step = rclcpp::Duration(path.time_step).seconds(); + const auto half_length = predicted_object.shape.dimensions.x * 0.5; + for (const auto & p : path.path) { + const auto object_polygon = universe_utils::toFootprint( + p, half_length, half_length, predicted_object.shape.dimensions.y); + footprint.front_left_ls.push_back(object_polygon.outer()[0]); + footprint.front_right_ls.push_back(object_polygon.outer()[1]); + footprint.rear_right_ls.push_back(object_polygon.outer()[2]); + footprint.rear_left_ls.push_back(object_polygon.outer()[3]); + } + object.corner_footprints.push_back(footprint); + } (void)params; } -inline std::vector filter_dynamic_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, - const ObjectDecisionsTracker & previous_decisions, - const TrajectoryCornerFootprint & ego_trajectory, const Parameters & params) +inline std::vector prepare_dynamic_objects( + const std::vector & objects, + const TrajectoryCornerFootprint & ego_trajectory, + const ObjectDecisionsTracker & previous_decisions, const Parameters & params) { - std::vector filtered_objects; - std::vector parked_objects; - for (const auto & object : objects.objects) { - auto filtered_object = object; - if ( - object.kinematics.initial_twist_with_covariance.twist.linear.x <= - params.objects_parked_velocity_threshold) { - if (has_classification_label(object, params.objects_parked_labels)) { - parked_objects.push_back(object); - } - continue; - } - 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)) { + std::vector filtered_objects; + for (const auto & object : objects) { + Object filtered_object; + filtered_object.uuid = universe_utils::toHexString(object.object_id); + classify(filtered_object, object, params); + calculate_current_footprint(filtered_object, object); + filtered_object.is_on_ego_trajectory = is_on_ego_trajectory(filtered_object, ego_trajectory); + const auto & previous_object_decisions = previous_decisions.get(filtered_object.uuid); + if (skip_object_condition(filtered_object, previous_object_decisions, params)) { continue; } - filter_predicted_paths(filtered_object, params); - if (!object.kinematics.predicted_paths.empty()) { + filter_predicted_paths(filtered_object, object, params); + if (!filtered_object.corner_footprints.empty()) { filtered_objects.push_back(filtered_object); } } 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 826231da1ba72..4e7824f0c3ae8 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 @@ -149,22 +149,19 @@ VelocityPlanningResult RunOutModule::plan( 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_); + auto filtered_objects = run_out::prepare_dynamic_objects( + planner_data->predicted_objects.objects, ego_footprint, decisions_tracker_, 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); // 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( - filtered_objects, footprint_rtree, ego_trajectory_points, params_); + run_out::calculate_collisions(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_); + run_out::calculate_decisions(decisions_tracker_, filtered_objects, now, params_); time_keeper_->end_track("calc_decisions()"); time_keeper_->start_track("calc_slowdowns()"); const auto result = run_out::calculate_slowdowns( @@ -178,9 +175,8 @@ VelocityPlanningResult RunOutModule::plan( virtual_wall_marker_creator.add_virtual_walls(run_out::create_virtual_walls( 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)); - debug_publisher_->publish(run_out::make_debug_collisions_markers(collisions_per_object)); + debug_publisher_->publish(run_out::make_debug_footprint_markers(ego_footprint, filtered_objects)); + // debug_publisher_->publish(run_out::make_debug_collisions_markers(filtered_objects)); debug_publisher_->publish(run_out::make_debug_decisions_markers(decisions_tracker_)); time_keeper_->end_track("publish_debug()"); time_keeper_->end_track("plan()"); 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 c96d769e7d7dd..bbb316df3b075 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 @@ -41,12 +41,9 @@ struct TrajectoryCornerFootprint universe_utils::Polygon2d polygon; }; -struct ObjectFootprint +struct ObjectCornerFootprint : TrajectoryCornerFootprint { - std::string uuid; - TrajectoryCornerFootprint corner_footprint; - - explicit ObjectFootprint(const std::string & id) { uuid = id; } + double time_step{}; }; struct FootprintIntersection @@ -131,10 +128,6 @@ struct Collision } } }; -struct CollisionsWithObject : std::vector -{ - std::string object_uuid; -}; enum DecisionType { stop, slowdown, nothing }; struct Decision @@ -232,6 +225,17 @@ struct UnavoidableCollision double comfortable_time_to_stop; }; +struct Object +{ + std::string uuid; + std::vector corner_footprints; // footprint of each predicted path + universe_utils::Polygon2d current_footprint; + bool is_on_ego_trajectory = false; + bool has_parked_label = false; + bool has_target_label = false; + std::vector collisions; // collisions with the ego trajectory +}; + } // namespace autoware::motion_velocity_planner::run_out #endif // TYPES_HPP_