feat(motion_velocity_planner): introduce Object/Pointcloud structure …

…in PlannerData (#9812)

* feat: new object/pointcloud struct in motion velocity planner

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update planner_data

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* modify modules

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

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");
 

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -96,20 +96,22 @@ bool is_unavoidable(
 };
 
 std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
-  const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data,
+  const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
   const PlannerParam & params, const double hysteresis)
 {
   std::vector<autoware_perception_msgs::msg::PredictedObject> 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;
 }

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp

@@ -17,6 +17,8 @@
 
 #include "types.hpp"
 
+#include <autoware/motion_velocity_planner_common/planner_data.hpp>
+
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 
 #include <vector>
@@ -74,7 +76,7 @@ bool is_unavoidable(
 /// @param hysteresis [m] extra distance threshold used for filtering
 /// @return filtered predicted objects
 std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
-  const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data,
+  const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
   const PlannerParam & params, const double hysteresis);
 
 }  // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop

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 <autoware/motion_velocity_planner_common/planner_data.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
 #include <autoware_perception_msgs/msg/detail/object_classification__struct.hpp>
@@ -26,6 +27,8 @@
 #include <gtest/gtest.h>
 #include <lanelet2_core/geometry/LineString.h>
 
+#include <vector>
+
 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<autoware::motion_velocity_planner::PlannerData::Object> objects;
   autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data;
   autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params;
   double hysteresis{};

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<PlannerData::Object> & dynamic_obstacles, const double buffer,
   const double min_vel)
 {
   return createObjectPolygons(dynamic_obstacles, buffer, min_vel);

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<PlannerData::Object> & dynamic_obstacles, const double buffer,
   const double min_vel);
 
 /// @brief create footprint polygons from projection lines

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;

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp

@@ -25,6 +25,7 @@
 #include <tf2/utils.h>
 
 #include <algorithm>
+#include <vector>
 
 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<PlannerData::Object> & 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;

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 <autoware/motion_velocity_planner_common/planner_data.hpp>
 #include <autoware/universe_utils/ros/transform_listener.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
@@ -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<PlannerData::Object> & 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

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 <autoware_perception_msgs/msg/predicted_object.hpp>
@@ -27,6 +28,7 @@
 
 #include <algorithm>
 #include <limits>
+#include <vector>
 
 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<PlannerData::Object> 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);

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -106,34 +106,37 @@ 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);
       if (no_overlap_path.size() <= 1) return true;
       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;

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp

@@ -39,6 +39,7 @@
 #include <map>
 #include <memory>
 #include <optional>
+#include <vector>
 
 namespace autoware::motion_velocity_planner
 {
@@ -54,11 +55,72 @@ struct PlannerData
   {
   }
 
+  struct Object
+  {
+  public:
+    Object() = default;
+    explicit Object(const autoware_perception_msgs::msg::PredictedObject & arg_predicted_object)
+    : predicted_object(arg_predicted_object)
+    {
+    }
+
+    autoware_perception_msgs::msg::PredictedObject predicted_object;
+    // double get_lon_vel_relative_to_traj()
+    // {
+    //   if (!lon_vel_relative_to_traj) {
+    //     lon_vel_relative_to_traj = 0.0;
+    //   }
+    //   return *lon_vel_relative_to_traj;
+    // }
+    // double get_lat_vel_relative_to_traj()
+    // {
+    //   if (!lat_vel_relative_to_traj) {
+    //     lat_vel_relative_to_traj = 0.0;
+    //   }
+    //   return *lat_vel_relative_to_traj;
+    // }
+
+  private:
+    // TODO(murooka) implement the following variables and their functions.
+    // std::optional<double> dist_to_traj_poly{std::nullopt};
+    // std::optional<double> dist_to_traj_lateral{std::nullopt};
+    // std::optional<double> dist_from_ego_longitudinal{std::nullopt};
+    // std::optional<double> lon_vel_relative_to_traj{std::nullopt};
+    // std::optional<double> lat_vel_relative_to_traj{std::nullopt};
+  };
+
+  struct Pointcloud
+  {
+  public:
+    Pointcloud() = default;
+    explicit Pointcloud(const pcl::PointCloud<pcl::PointXYZ> & arg_pointcloud)
+    : pointcloud(arg_pointcloud)
+    {
+    }
+
+    pcl::PointCloud<pcl::PointXYZ> pointcloud;
+
+  private:
+    // NOTE: clustered result will be added.
+  };
+
+  void process_predicted_objects(
+    const autoware_perception_msgs::msg::PredictedObjects & predicted_objects)
+  {
+    predicted_objects_header = predicted_objects.header;
+
+    objects.clear();
+    for (const auto & predicted_object : predicted_objects.objects) {
+      objects.push_back(Object(predicted_object));
+    }
+  }
+
   // msgs from callbacks that are used for data-ready
   nav_msgs::msg::Odometry current_odometry;
   geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration;
-  autoware_perception_msgs::msg::PredictedObjects predicted_objects;
-  pcl::PointCloud<pcl::PointXYZ> no_ground_pointcloud;
+  std_msgs::msg::Header predicted_objects_header;
+  std::vector<Object> objects;
+  Pointcloud no_ground_pointcloud;
   nav_msgs::msg::OccupancyGrid occupancy_grid;
   std::shared_ptr<route_handler::RouteHandler> route_handler;
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -161,13 +161,14 @@ bool MotionVelocityPlannerNode::update_planner_data(
 
   const auto predicted_objects_ptr = sub_predicted_objects_.takeData();
   if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects"))
-    planner_data_.predicted_objects = *predicted_objects_ptr;
+    planner_data_.process_predicted_objects(*predicted_objects_ptr);
   processing_times["update_planner_data.pred_obj"] = sw.toc(true);
 
   const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData();
   if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) {
     const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr);
-    if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud;
+    if (no_ground_pointcloud)
+      planner_data_.no_ground_pointcloud = PlannerData::Pointcloud(*no_ground_pointcloud);
   }
   processing_times["update_planner_data.pcd"] = sw.toc(true);