refactor(obstacle_avoidance_planner): clean up the code

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

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -48,12 +48,12 @@ def launch_setup(context, *args, **kwargs):
         obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     obstacle_avoidance_planner_component = ComposableNode(
         package="obstacle_avoidance_planner",
-        plugin="ObstacleAvoidancePlanner",
+        plugin="obstacle_avoidance_planner::ObstacleAvoidancePlanner",
         name="obstacle_avoidance_planner",
         namespace="",
         remappings=[
-            ("~/input/objects", "/perception/object_recognition/objects"),
             ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/odometry", "/localization/kinematic_state"),
             ("~/output/path", "obstacle_avoidance_planner/trajectory"),
         ],
         parameters=[

planning/obstacle_avoidance_planner/CMakeLists.txt

@@ -1,40 +1,67 @@
 cmake_minimum_required(VERSION 3.14)
 project(obstacle_avoidance_planner)
 
+include(CheckCXXCompilerFlag)
+
+# For Eigen vectorization.
+check_cxx_compiler_flag("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE)
+if(COMPILER_SUPPORTS_MARCH_NATIVE)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
+  message(STATUS "Enabling MARCH NATIVE ")
+endif()
+
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(Eigen3 REQUIRED)
-find_package(OpenCV REQUIRED)
 
 ament_auto_add_library(obstacle_avoidance_planner SHARED
+  # node
+  src/node.cpp
+  # core algorithms
+  src/replan_checker.cpp
+  src/eb_path_smoother.cpp
+  src/mpt_optimizer.cpp
+  src/state_equation_generator.cpp
+  # debug marker
+  src/debug_marker.cpp
+  # vehicle model
   src/vehicle_model/vehicle_model_interface.cpp
   src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
-  src/eb_path_optimizer.cpp
-  src/mpt_optimizer.cpp
-  src/utils/utils.cpp
-  src/utils/debug_utils.cpp
-  src/node.cpp
+  # utils
+  src/utils/trajectory_utils.cpp
+  src/utils/geometry_utils.cpp
 )
 
 target_include_directories(obstacle_avoidance_planner
   SYSTEM PUBLIC
-    ${OpenCV_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIR}
 )
 
-target_link_libraries(obstacle_avoidance_planner
-  ${OpenCV_LIBRARIES}
-)
-
+# register node
 rclcpp_components_register_node(obstacle_avoidance_planner
-  PLUGIN "ObstacleAvoidancePlanner"
+  PLUGIN "obstacle_avoidance_planner::ObstacleAvoidancePlanner"
   EXECUTABLE obstacle_avoidance_planner_node
 )
 
+# unit test
+# if(BUILD_TESTING)
+#   ament_add_ros_isolated_gtest(test_obstacle_avoidance_planner
+#     test/test_mpt_optimizer.cpp
+#   )
+#   ament_target_dependencies(test_obstacle_avoidance_planner fake_test_node)
+#   target_link_libraries(
+#     test_obstacle_avoidance_planner obstacle_avoidance_planner)
+#
+#   find_package(autoware_testing REQUIRED)
+#   add_smoke_test(obstacle_avoidance_planner obstacle_avoidance_planner_node
+#     PARAM_FILENAMES "obstacle_avoidance_planner.param.yaml test_vehicle_info.param.yaml test_nearest_search.param.yaml"
+#   )
+# endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
     launch
-    config
     scripts
+    param
 )