refactor

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

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
 )
 

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp → planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/cv_utils.hpp

@@ -11,8 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_
-#define OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_
+#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_
+#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_
 
 #include "obstacle_avoidance_planner/common_structs.hpp"
 #include "obstacle_avoidance_planner/eb_path_optimizer.hpp"
@@ -118,4 +118,4 @@ bool isOutsideDrivableAreaFromCirclesFootprint(
   const double vehicle_circle_radius);
 }  // namespace cv_drivable_area_utils
 
-#endif  // OBSTACLE_AVOIDANCE_PLANNER__CV_UTILS_HPP_
+#endif  // OBSTACLE_AVOIDANCE_PLANNER__UTILS__CV_UTILS_HPP_

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug_visualization.hpp → planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/debug_utils.hpp

@@ -11,8 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_
-#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_
+#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_
+#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_
 
 #include "obstacle_avoidance_planner/common_structs.hpp"
 #include "opencv2/core.hpp"
@@ -33,7 +33,7 @@
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
 
-namespace debug_visualization
+namespace debug_utils
 {
 visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   DebugData & debug_data,
@@ -45,6 +45,6 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationWallMarker(
 
 nav_msgs::msg::OccupancyGrid getDebugCostmap(
   const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid);
-}  // namespace debug_visualization
+}  // namespace debug_utils
 
-#endif  // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_VISUALIZATION_HPP_
+#endif  // OBSTACLE_AVOIDANCE_PLANNER__UTILS__DEBUG_UTILS_HPP_

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils.hpp → planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_
-#define OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_
+#ifndef OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_
+#define OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_
 
 #include "eigen3/Eigen/Core"
 #include "interpolation/linear_interpolation.hpp"
@@ -344,4 +344,4 @@ namespace utils
 void logOSQPSolutionStatus(const int solution_status, const std::string & msg);
 }  // namespace utils
 
-#endif  // OBSTACLE_AVOIDANCE_PLANNER__UTILS_HPP_
+#endif  // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_

planning/obstacle_avoidance_planner/src/costmap_generator.cpp

@@ -14,8 +14,8 @@
 
 #include "obstacle_avoidance_planner/costmap_generator.hpp"
 
-#include "obstacle_avoidance_planner/cv_utils.hpp"
-#include "obstacle_avoidance_planner/utils.hpp"
+#include "obstacle_avoidance_planner/utils/cv_utils.hpp"
+#include "obstacle_avoidance_planner/utils/utils.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
 namespace

planning/obstacle_avoidance_planner/src/eb_path_optimizer.cpp

@@ -14,7 +14,7 @@
 
 #include "obstacle_avoidance_planner/eb_path_optimizer.hpp"
 
-#include "obstacle_avoidance_planner/utils.hpp"
+#include "obstacle_avoidance_planner/utils/utils.hpp"
 
 #include "geometry_msgs/msg/vector3.hpp"
 

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -15,7 +15,7 @@
 #include "obstacle_avoidance_planner/mpt_optimizer.hpp"
 
 #include "interpolation/spline_interpolation_points_2d.hpp"
-#include "obstacle_avoidance_planner/utils.hpp"
+#include "obstacle_avoidance_planner/utils/utils.hpp"
 #include "obstacle_avoidance_planner/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
 #include "tf2/utils.h"
 

planning/obstacle_avoidance_planner/src/node.cpp

@@ -16,9 +16,9 @@
 
 #include "interpolation/spline_interpolation_points_2d.hpp"
 #include "motion_utils/motion_utils.hpp"
-#include "obstacle_avoidance_planner/cv_utils.hpp"
-#include "obstacle_avoidance_planner/debug_visualization.hpp"
-#include "obstacle_avoidance_planner/utils.hpp"
+#include "obstacle_avoidance_planner/utils/cv_utils.hpp"
+#include "obstacle_avoidance_planner/utils/debug_utils.hpp"
+#include "obstacle_avoidance_planner/utils/utils.hpp"
 #include "rclcpp/time.hpp"
 #include "tf2/utils.h"
 #include "tier4_autoware_utils/ros/update_param.hpp"
@@ -1231,8 +1231,8 @@ void ObstacleAvoidancePlanner::publishDebugDataInOptimization(
   {  // publish markers
     if (is_publishing_debug_visualization_marker_) {
       stop_watch_.tic("getDebugVisualizationMarker");
-      const auto & debug_marker = debug_visualization::getDebugVisualizationMarker(
-        debug_data_, traj_points, vehicle_param_, false);
+      const auto & debug_marker =
+        debug_utils::getDebugVisualizationMarker(debug_data_, traj_points, vehicle_param_, false);
       debug_data_.msg_stream << "      getDebugVisualizationMarker:= "
                              << stop_watch_.toc("getDebugVisualizationMarker") << " [ms]\n";
 
@@ -1243,7 +1243,7 @@ void ObstacleAvoidancePlanner::publishDebugDataInOptimization(
 
       stop_watch_.tic("getDebugVisualizationWallMarker");
       const auto & debug_wall_marker =
-        debug_visualization::getDebugVisualizationWallMarker(debug_data_, vehicle_param_);
+        debug_utils::getDebugVisualizationWallMarker(debug_data_, vehicle_param_);
       debug_data_.msg_stream << "      getDebugVisualizationWallMarker:= "
                              << stop_watch_.toc("getDebugVisualizationWallMarker") << " [ms]\n";
 
@@ -1544,16 +1544,16 @@ void ObstacleAvoidancePlanner::publishDebugDataInMain(const Path & path) const
     stop_watch_.tic("publishClearanceMap");
 
     if (is_publishing_area_with_objects_) {  // false
-      debug_area_with_objects_pub_->publish(debug_visualization::getDebugCostmap(
-        debug_data_.area_with_objects_map, path.drivable_area));
+      debug_area_with_objects_pub_->publish(
+        debug_utils::getDebugCostmap(debug_data_.area_with_objects_map, path.drivable_area));
     }
     if (is_publishing_object_clearance_map_) {  // false
-      debug_object_clearance_map_pub_->publish(debug_visualization::getDebugCostmap(
-        debug_data_.only_object_clearance_map, path.drivable_area));
+      debug_object_clearance_map_pub_->publish(
+        debug_utils::getDebugCostmap(debug_data_.only_object_clearance_map, path.drivable_area));
     }
     if (is_publishing_clearance_map_) {  // true
       debug_clearance_map_pub_->publish(
-        debug_visualization::getDebugCostmap(debug_data_.clearance_map, path.drivable_area));
+        debug_utils::getDebugCostmap(debug_data_.clearance_map, path.drivable_area));
     }
     debug_data_.msg_stream << "    getDebugCostMap * 3:= " << stop_watch_.toc("publishClearanceMap")
                            << " [ms]\n";