perf(dynamic_obstacle_avoidance): decrease the computation time with …

…time_keeper (autowarefoundation#7986) (#1427) * decrease computation cost * remove TODO * fix --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Jul 29, 2024 · ae7cbfe · ae7cbfe
1 parent 94c336f
commit ae7cbfe
Show file tree

Hide file tree

Showing 2 changed files with 200 additions and 123 deletions.
diff --git a/...oidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/...oidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp
@@ -16,7 +16,6 @@
 #define AUTOWARE__BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_
 
 #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
-#include "autoware/universe_utils/system/stop_watch.hpp"
 
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -206,20 +205,20 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
     std::optional<MinMaxValue> lat_offset_to_avoid{std::nullopt};
     bool is_collision_left{false};
     bool should_be_avoided{false};
-    std::vector<PathPointWithLaneId> ref_path_points_for_obj_poly;
+    std::vector<geometry_msgs::msg::Pose> ref_points_for_obj_poly;
     LatFeasiblePaths ego_lat_feasible_paths;
 
     // add additional information (not update to the latest data)
     void update(
       const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
       const bool arg_is_collision_left, const bool arg_should_be_avoided,
-      const std::vector<PathPointWithLaneId> & arg_ref_path_points_for_obj_poly)
+      const std::vector<geometry_msgs::msg::Pose> & arg_ref_points_for_obj_poly)
     {
       lon_offset_to_avoid = arg_lon_offset_to_avoid;
       lat_offset_to_avoid = arg_lat_offset_to_avoid;
       is_collision_left = arg_is_collision_left;
       should_be_avoided = arg_should_be_avoided;
-      ref_path_points_for_obj_poly = arg_ref_path_points_for_obj_poly;
+      ref_points_for_obj_poly = arg_ref_points_for_obj_poly;
     }
   };
 
@@ -317,12 +316,12 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
       const std::string & uuid, const MinMaxValue & lon_offset_to_avoid,
       const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left,
       const bool should_be_avoided,
-      const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly)
+      const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly)
     {
       if (object_map_.count(uuid) != 0) {
         object_map_.at(uuid).update(
           lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided,
-          ref_path_points_for_obj_poly);
+          ref_points_for_obj_poly);
       }
     }
 
@@ -409,24 +408,25 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
   std::optional<std::pair<size_t, size_t>> calcCollisionSection(
     const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & obj_path) const;
   LatLonOffset getLateralLongitudinalOffset(
-    const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
+    const std::vector<geometry_msgs::msg::Pose> & ego_points,
+    const geometry_msgs::msg::Pose & obj_pose, const size_t obj_seg_idx,
     const autoware_perception_msgs::msg::Shape & obj_shape) const;
   double calcValidLengthToAvoid(
     const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
     const autoware_perception_msgs::msg::Shape & obj_shape,
     const bool is_object_same_direction) const;
   MinMaxValue calcMinMaxLongitudinalOffsetToAvoid(
-    const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
+    const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly,
     const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
     const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape,
     const TimeWhileCollision & time_while_collision) const;
   std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoidRegulatedObject(
-    const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
+    const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly,
     const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel,
     const bool is_collision_left, const double obj_normal_vel,
     const std::optional<DynamicAvoidanceObject> & prev_object) const;
   std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoidUnregulatedObject(
-    const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
+    const std::vector<geometry_msgs::msg::Pose> & ref_points_for_obj_poly,
     const std::optional<DynamicAvoidanceObject> & prev_object,
     const DynamicAvoidanceObject & object) const;
   std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> getAdjacentLanes(
@@ -454,10 +454,6 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
   std::shared_ptr<DynamicAvoidanceParameters> parameters_;
 
   TargetObjectsManager target_objects_manager_;
-
-  mutable autoware::universe_utils::StopWatch<
-    std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
-    stop_watch_;
 };
 }  // namespace autoware::behavior_path_planner