refactor(obstacle_avoidance_planner): node refactoring (autowarefound…

…ation#1829) * refactor Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * refactor Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * refactor Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * refactor Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * refactor Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix 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>
boyali · Oct 3, 2022 · c7eca63 · c7eca63
1 parent ad4941a
commit c7eca63
Show file tree

Hide file tree

Showing 18 changed files with 549 additions and 1,348 deletions.
diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt
@@ -10,12 +10,12 @@ find_package(OpenCV REQUIRED)
 ament_auto_add_library(obstacle_avoidance_planner SHARED
   src/vehicle_model/vehicle_model_interface.cpp
   src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
-  src/utils.cpp
   src/costmap_generator.cpp
-  src/debug_visualization.cpp
   src/eb_path_optimizer.cpp
   src/mpt_optimizer.cpp
-  src/cv_utils.cpp
+  src/utils/utils.cpp
+  src/utils/debug_utils.cpp
+  src/utils/cv_utils.cpp
   src/node.cpp
 )
 

diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/costmap_generator.hpp
@@ -36,19 +36,17 @@ class CostmapGenerator
   CVMaps getMaps(
     const bool enable_avoidance, const autoware_auto_planning_msgs::msg::Path & path,
     const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
-    const TrajectoryParam & traj_param, std::shared_ptr<DebugData> debug_data_ptr);
+    const TrajectoryParam & traj_param, DebugData & debug_data);
 
 private:
   mutable tier4_autoware_utils::StopWatch<
     std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
     stop_watch_;
 
   cv::Mat getAreaWithObjects(
-    const cv::Mat & drivable_area, const cv::Mat & objects_image,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    const cv::Mat & drivable_area, const cv::Mat & objects_image, DebugData & debug_data) const;
 
-  cv::Mat getClearanceMap(
-    const cv::Mat & drivable_area, std::shared_ptr<DebugData> debug_data_ptr) const;
+  cv::Mat getClearanceMap(const cv::Mat & drivable_area, DebugData & debug_data) const;
 
   cv::Mat drawObstaclesOnImage(
     const bool enable_avoidance,
@@ -57,10 +55,9 @@ class CostmapGenerator
     const nav_msgs::msg::MapMetaData & map_info, [[maybe_unused]] const cv::Mat & drivable_area,
     const cv::Mat & clearance_map, const TrajectoryParam & traj_param,
     std::vector<autoware_auto_perception_msgs::msg::PredictedObject> * debug_avoiding_objects,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    DebugData & debug_data) const;
 
   cv::Mat getDrivableAreaInCV(
-    const nav_msgs::msg::OccupancyGrid & occupancy_grid,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    const nav_msgs::msg::OccupancyGrid & occupancy_grid, DebugData & debug_data) const;
 };
 #endif  // OBSTACLE_AVOIDANCE_PLANNER__COSTMAP_GENERATOR_HPP_
diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/eb_path_optimizer.hpp
@@ -142,7 +142,7 @@ class EBPathOptimizer
   boost::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>>
   getOptimizedTrajectory(
     const autoware_auto_planning_msgs::msg::Path & path, const CandidatePoints & candidate_points,
-    std::shared_ptr<DebugData> debug_data_ptr);
+    DebugData & debug_data);
 
   void updateConstrain(
     const std::vector<geometry_msgs::msg::Point> & interpolated_points,
@@ -160,7 +160,7 @@ class EBPathOptimizer
   CandidatePoints getCandidatePoints(
     const geometry_msgs::msg::Pose & ego_pose,
     const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
-    const std::unique_ptr<Trajectories> & prev_trajs, std::shared_ptr<DebugData> debug_data_ptr);
+    const std::unique_ptr<Trajectories> & prev_trajs, DebugData & debug_data);
 
   CandidatePoints getDefaultCandidatePoints(
     const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points);
@@ -171,7 +171,7 @@ class EBPathOptimizer
   calculateTrajectory(
     const std::vector<geometry_msgs::msg::Point> & padded_interpolated_points,
     const std::vector<ConstrainRectangle> & constrain_rectangles, const int farthest_idx,
-    std::shared_ptr<DebugData> debug_data_ptr);
+    DebugData & debug_data);
 
 public:
   EBPathOptimizer(
@@ -181,7 +181,7 @@ class EBPathOptimizer
   boost::optional<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>> getEBTrajectory(
     const geometry_msgs::msg::Pose & ego_pose, const autoware_auto_planning_msgs::msg::Path & path,
     const std::unique_ptr<Trajectories> & prev_trajs, const double current_ego_vel,
-    std::shared_ptr<DebugData> debug_data_ptr);
+    DebugData & debug_data);
 };
 
 #endif  // OBSTACLE_AVOIDANCE_PLANNER__EB_PATH_OPTIMIZER_HPP_
diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/marker_helper.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/marker_helper.hpp
diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp
@@ -202,15 +202,15 @@ class MPTOptimizer
   std::vector<ReferencePoint> getReferencePoints(
     const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
     const std::unique_ptr<Trajectories> & prev_mpt_points, const bool enable_avoidance,
-    const CVMaps & maps, std::shared_ptr<DebugData> debug_data_ptr) const;
+    const CVMaps & maps, DebugData & debug_data) const;
 
   void calcPlanningFromEgo(std::vector<ReferencePoint> & ref_points) const;
 
   /*
   std::vector<ReferencePoint> convertToReferencePoints(
     const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
     const std::unique_ptr<Trajectories> & prev_mpt_points, const bool enable_avoidance,
-    const CVMaps & maps, std::shared_ptr<DebugData> debug_data_ptr) const;
+    const CVMaps & maps, DebugData & debug_data) const;
   */
 
   std::vector<ReferencePoint> getFixedReferencePoints(
@@ -219,12 +219,11 @@ class MPTOptimizer
 
   void calcBounds(
     std::vector<ReferencePoint> & ref_points, const bool enable_avoidance, const CVMaps & maps,
-    const std::unique_ptr<Trajectories> & prev_trajs,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    const std::unique_ptr<Trajectories> & prev_trajs, DebugData & debug_data) const;
 
   void calcVehicleBounds(
-    std::vector<ReferencePoint> & ref_points, const CVMaps & maps,
-    std::shared_ptr<DebugData> debug_data_ptr, const bool enable_avoidance) const;
+    std::vector<ReferencePoint> & ref_points, const CVMaps & maps, DebugData & debug_data,
+    const bool enable_avoidance) const;
 
   // void calcFixState(
   // std::vector<ReferencePoint> & ref_points,
@@ -246,13 +245,12 @@ class MPTOptimizer
    * Uex Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
    */
   MPTMatrix generateMPTMatrix(
-    const std::vector<ReferencePoint> & reference_points,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    const std::vector<ReferencePoint> & reference_points, DebugData & debug_data) const;
 
   ValueMatrix generateValueMatrix(
     const std::vector<ReferencePoint> & reference_points,
     const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    DebugData & debug_data) const;
 
   void addSteerWeightR(
     std::vector<Eigen::Triplet<double>> & Rex_triplet_vec,
@@ -261,19 +259,19 @@ class MPTOptimizer
   boost::optional<Eigen::VectorXd> executeOptimization(
     const std::unique_ptr<Trajectories> & prev_trajs, const bool enable_avoidance,
     const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat,
-    const std::vector<ReferencePoint> & ref_points, std::shared_ptr<DebugData> debug_data_ptr);
+    const std::vector<ReferencePoint> & ref_points, DebugData & debug_data);
 
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getMPTPoints(
     std::vector<ReferencePoint> & fixed_ref_points,
     std::vector<ReferencePoint> & non_fixed_ref_points, const Eigen::VectorXd & Uex,
-    const MPTMatrix & mpt_matrix, std::shared_ptr<DebugData> debug_data_ptr);
+    const MPTMatrix & mpt_matrix, DebugData & debug_data);
 
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getMPTFixedPoints(
     const std::vector<ReferencePoint> & ref_points) const;
 
   BoundsCandidates getBoundsCandidates(
     const bool enable_avoidance, const geometry_msgs::msg::Pose & avoiding_point,
-    const CVMaps & maps, std::shared_ptr<DebugData> debug_data_ptr) const;
+    const CVMaps & maps, DebugData & debug_data) const;
 
   CollisionType getCollisionType(
     const CVMaps & maps, const bool enable_avoidance,
@@ -286,13 +284,11 @@ class MPTOptimizer
 
   ObjectiveMatrix getObjectiveMatrix(
     const MPTMatrix & mpt_mat, const ValueMatrix & obj_mat,
-    [[maybe_unused]] const std::vector<ReferencePoint> & ref_points,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    [[maybe_unused]] const std::vector<ReferencePoint> & ref_points, DebugData & debug_data) const;
 
   ConstraintMatrix getConstraintMatrix(
     const bool enable_avoidance, const MPTMatrix & mpt_mat,
-    const std::vector<ReferencePoint> & ref_points,
-    std::shared_ptr<DebugData> debug_data_ptr) const;
+    const std::vector<ReferencePoint> & ref_points, DebugData & debug_data) const;
 
 public:
   MPTOptimizer(
@@ -305,7 +301,7 @@ class MPTOptimizer
     const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
     const std::unique_ptr<Trajectories> & prev_trajs, const CVMaps & maps,
     const geometry_msgs::msg::Pose & current_ego_pose, const double current_ego_vel,
-    std::shared_ptr<DebugData> debug_data_ptr);
+    DebugData & debug_data);
 };
 
 #endif  // OBSTACLE_AVOIDANCE_PLANNER__MPT_OPTIMIZER_HPP_