From cf5b4a99e9578bb12cf54f0579ee35c163db994a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 27 Dec 2024 14:57:51 +0900 Subject: [PATCH] modify modules Signed-off-by: Takayuki Murooka --- .../src/dynamic_obstacle_stop_module.cpp | 2 +- .../src/object_filtering.cpp | 20 ++++++++------- .../src/object_filtering.hpp | 4 ++- .../test/test_object_filtering.cpp | 6 +++-- .../src/obstacle_velocity_limiter.cpp | 2 +- .../src/obstacle_velocity_limiter.hpp | 2 +- .../src/obstacle_velocity_limiter_module.cpp | 6 ++--- .../src/obstacles.cpp | 14 ++++++----- .../src/obstacles.hpp | 4 +-- .../test/test_collision_distance.cpp | 9 ++++--- .../src/filter_predicted_objects.cpp | 25 +++++++++++-------- .../planner_data.hpp | 4 +-- 12 files changed, 56 insertions(+), 42 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 40aa985ee42e2..7fd5834ba2cfa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -131,7 +131,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( ? 0.0 : params_.hysteresis; const auto dynamic_obstacles = dynamic_obstacle_stop::filter_predicted_objects( - planner_data->predicted_objects, ego_data, params_, hysteresis); + planner_data->objects, ego_data, params_, hysteresis); const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 2ab3c09e64edd..0726fdba02de0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -96,20 +96,22 @@ bool is_unavoidable( }; std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; - for (const auto & object : objects.objects) { - const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= - params.minimum_object_velocity; + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; + const auto is_not_too_slow = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity; if ( - is_vehicle(object) && is_not_too_slow && - is_in_range(object, ego_data.trajectory, params, hysteresis) && - is_not_too_close(object, ego_data, params.ego_longitudinal_offset) && + is_vehicle(predicted_object) && is_not_too_slow && + is_in_range(predicted_object, ego_data.trajectory, params, hysteresis) && + is_not_too_close(predicted_object, ego_data, params.ego_longitudinal_offset) && (!params.ignore_unavoidable_collisions || - !is_unavoidable(object, ego_data.pose, ego_data.earliest_stop_pose, params))) - filtered_objects.push_back(object); + !is_unavoidable(predicted_object, ego_data.pose, ego_data.earliest_stop_pose, params))) + filtered_objects.push_back(predicted_object); } return filtered_objects; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index d20d6354066c6..6024cde019489 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -17,6 +17,8 @@ #include "types.hpp" +#include + #include #include @@ -74,7 +76,7 @@ bool is_unavoidable( /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 3fa179903c23f..5993f81c7a8f1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -14,6 +14,7 @@ #include "../src/object_filtering.hpp" +#include #include #include @@ -26,6 +27,8 @@ #include #include +#include + TEST(TestObjectFiltering, isVehicle) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_vehicle; @@ -202,8 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable) TEST(TestObjectFiltering, filterPredictedObjects) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; - autoware_perception_msgs::msg::PredictedObjects objects; - autoware_perception_msgs::msg::PredictedObject object; + std::vector objects; autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; double hysteresis{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index e501b756af6a1..9be7f52bca99a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index d7d2de63b33f8..b0cdb1f802e0f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel); /// @brief create footprint polygons from projection lines diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 532cc834c19ac..1923f023552e3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -170,7 +170,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("obstacles"); obstacle_masks.negative_masks = obstacle_velocity_limiter::createPolygonMasks( - planner_data->predicted_objects, obstacle_params_.dynamic_obstacles_buffer, + planner_data->objects, obstacle_params_.dynamic_obstacles_buffer, obstacle_params_.dynamic_obstacles_min_vel); if (obstacle_params_.ignore_on_path) obstacle_masks.negative_masks.push_back(obstacle_velocity_limiter::createTrajectoryFootprint( @@ -189,8 +189,8 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( obstacle_masks.positive_mask = obstacle_velocity_limiter::createEnvelopePolygon(footprint_polygons); obstacle_velocity_limiter::addSensorObstacles( - obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud, obstacle_masks, - obstacle_params_); + obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud.pointcloud, + obstacle_masks, obstacle_params_); } const auto obstacles_us = stopwatch.toc("obstacles"); autoware::motion_utils::VirtualWalls virtual_walls; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp index 47215ee5cb0c4..187f028dc5969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp @@ -25,6 +25,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { @@ -58,17 +59,18 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity) + const std::vector & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; - for (const auto & object : objects.objects) { + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); if (min_velocity <= obj_vel_norm) { polygons.push_back(createObjectPolygon( - object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + predicted_object.kinematics.initial_pose_with_covariance.pose, + predicted_object.shape.dimensions, buffer)); } } return polygons; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index 7e4704ba6818c..a98d92994f497 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -18,6 +18,7 @@ #include "parameters.hpp" #include "types.hpp" +#include #include #include @@ -163,8 +164,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity); + const std::vector & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object /// @param[out] obstacles Obstacles object in which to add the sensor obstacles diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index ac07c62f62cf7..023ae83774917 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -15,6 +15,7 @@ #include "../src/distance.hpp" #include "../src/obstacles.hpp" #include "../src/types.hpp" +#include "autoware/motion_velocity_planner_common/planner_data.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -27,6 +28,7 @@ #include #include +#include const auto point_in_polygon = [](const auto x, const auto y, const auto & polygon) { return std::find_if(polygon.outer().begin(), polygon.outer().end(), [=](const auto & pt) { @@ -387,11 +389,12 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { + using autoware::motion_velocity_planner::PlannerData; using autoware::motion_velocity_planner::obstacle_velocity_limiter::createObjectPolygons; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; - PredictedObjects objects; + std::vector objects; auto polygons = createObjectPolygons(objects, 0.0, 0.0); EXPECT_TRUE(polygons.empty()); @@ -404,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; - objects.objects.push_back(object1); + objects.push_back(PlannerData::Object(object1)); polygons = createObjectPolygons(objects, 0.0, 1.0); EXPECT_TRUE(polygons.empty()); @@ -431,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; - objects.objects.push_back(object2); + objects.push_back(PlannerData::Object(object2)); polygons = createObjectPolygons(objects, 0.0, 2.0); ASSERT_EQ(polygons.size(), 1ul); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index f9ba7f4af9877..a6c8368c20571 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -106,23 +106,26 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data.predicted_objects.header; - for (const auto & object : planner_data.predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects_header; + for (const auto & object : planner_data.objects) { + const auto & predicted_object = object.predicted_object; const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); + std::find_if( + predicted_object.classification.begin(), predicted_object.classification.end(), + [](const auto & c) { + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != predicted_object.classification.end(); if (is_pedestrian) continue; const auto is_coming_from_behind = motion_utils::calcSignedArcLength( ego_data.trajectory_points, 0UL, - object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; + predicted_object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; if (params.objects_ignore_behind_ego && is_coming_from_behind) { continue; } - auto filtered_object = object; + auto filtered_object = predicted_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 no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); @@ -130,10 +133,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const auto lat_offset_to_current_ego = std::abs(motion_utils::calcLateralOffset(no_overlap_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); + lat_offset_to_current_ego <= predicted_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; }; auto & predicted_paths = filtered_object.kinematics.predicted_paths; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp index f0afc9a07a328..f67b6f03e0a0b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp @@ -107,7 +107,7 @@ struct PlannerData void process_predicted_objects( const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) { - predicted_object_header = predicted_objects.header; + predicted_objects_header = predicted_objects.header; for (const auto & predicted_object : predicted_objects.objects) { objects.push_back(Object(predicted_object)); } @@ -116,7 +116,7 @@ struct PlannerData // msgs from callbacks that are used for data-ready nav_msgs::msg::Odometry current_odometry; geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; - std_msgs::msg::Header predicted_object_header; + std_msgs::msg::Header predicted_objects_header; std::vector objects; Pointcloud no_ground_pointcloud; nav_msgs::msg::OccupancyGrid occupancy_grid;