refactor(obstacle_avoidance_planner): clean up the code (autowarefoun…

…dation#2796)

* update obstacle avoidance planner, static centerline optimizer, tier4_planning_launch

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

* update velocity on joint and correct trajectory z

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

* update

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

* minor change

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

* pre-commit

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

---------

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

common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp

@@ -84,6 +84,7 @@ class SplineInterpolationPoints2d
   void calcSplineCoefficientsInner(const std::vector<geometry_msgs::msg::Point> & points);
   SplineInterpolation spline_x_;
   SplineInterpolation spline_y_;
+  SplineInterpolation spline_z_;
 
   std::vector<double> base_s_vec_;
 };

common/interpolation/src/spline_interpolation_points_2d.cpp

@@ -35,12 +35,13 @@ std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vec
   return dist_v;
 }
 
-std::array<std::vector<double>, 3> getBaseValues(
+std::array<std::vector<double>, 4> getBaseValues(
   const std::vector<geometry_msgs::msg::Point> & points)
 {
   // calculate x, y
   std::vector<double> base_x;
   std::vector<double> base_y;
+  std::vector<double> base_z;
   for (size_t i = 0; i < points.size(); i++) {
     const auto & current_pos = points.at(i);
     if (i > 0) {
@@ -53,16 +54,17 @@ std::array<std::vector<double>, 3> getBaseValues(
     }
     base_x.push_back(current_pos.x);
     base_y.push_back(current_pos.y);
+    base_z.push_back(current_pos.z);
   }
 
   // calculate base_keys, base_values
-  if (base_x.size() < 2 || base_y.size() < 2) {
+  if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) {
     throw std::logic_error("The numbef of unique points is not enough.");
   }
 
   const std::vector<double> base_s = calcEuclidDist(base_x, base_y);
 
-  return {base_s, base_x, base_y};
+  return {base_s, base_x, base_y, base_z};
 }
 }  // namespace
 
@@ -137,10 +139,12 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin
 
   const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0);
   const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0);
+  const double z = spline_z_.getSplineInterpolatedValues({whole_s}).at(0);
 
   geometry_msgs::msg::Point geom_point;
   geom_point.x = x;
   geom_point.y = y;
+  geom_point.z = z;
   return geom_point;
 }
 
@@ -226,8 +230,10 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
   base_s_vec_ = base.at(0);
   const auto & base_x_vec = base.at(1);
   const auto & base_y_vec = base.at(2);
+  const auto & base_z_vec = base.at(3);
 
   // calculate spline coefficients
   spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec);
   spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec);
+  spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec);
 }

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,52 @@
 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
 )
 
 ament_auto_package(
   INSTALL_TO_SHARE
     launch
-    config
     scripts
+    config
 )