refactor(common): extern template for motion_utils / remove tier4_autoware_utils.hpp / remove motion_utis.hpp (autowarefoundation#5027)

Author: soblin

common/motion_utils/CMakeLists.txt

@@ -7,13 +7,14 @@ autoware_package()
 find_package(Boost REQUIRED)
 
 ament_auto_add_library(motion_utils SHARED
-  src/motion_utils.cpp
   src/distance/distance.cpp
   src/marker/marker_helper.cpp
   src/marker/virtual_wall_marker_creator.cpp
   src/resample/resample.cpp
+  src/trajectory/trajectory.cpp
   src/trajectory/interpolation.cpp
   src/trajectory/path_with_lane_id.cpp
+  src/trajectory/tmp_conversion.cpp
   src/vehicle/vehicle_state_checker.cpp
 )
 

common/motion_utils/README.md

@@ -109,3 +109,9 @@ const size_t ego_nearest_seg_idx = findFirstNearestSegmentIndex(points, ego_pose
 const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold);
 const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx);
 ```
+
+## For developers
+
+Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time.
+
+`motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing.

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -15,13 +15,11 @@
 #ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 
-#include "motion_utils/resample/resample_utils.hpp"
+#include <rclcpp/time.hpp>
 
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include <functional>
 #include <string>
-#include <vector>
 
 namespace motion_utils
 {

common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -15,14 +15,14 @@
 #ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
 #define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
 
-#include "motion_utils/marker/marker_helper.hpp"
+#include <rclcpp/time.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <string>
 #include <unordered_map>
 #include <vector>
-
 namespace motion_utils
 {
 

common/motion_utils/include/motion_utils/motion_utils.hpp

@@ -15,17 +15,6 @@
 #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
 #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
 
-#include "interpolation/interpolation_utils.hpp"
-#include "interpolation/linear_interpolation.hpp"
-#include "interpolation/spline_interpolation.hpp"
-#include "interpolation/zero_order_hold.hpp"
-#include "motion_utils/trajectory/trajectory.hpp"
-#include "tier4_autoware_utils/geometry/geometry.hpp"
-#include "tier4_autoware_utils/geometry/pose_deviation.hpp"
-#include "tier4_autoware_utils/math/constants.hpp"
-
-#include <rclcpp/rclcpp.hpp>
-
 #include "autoware_auto_planning_msgs/msg/path.hpp"
 #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"

common/motion_utils/include/motion_utils/resample/resample.hpp

@@ -18,7 +18,6 @@
 #include <motion_utils/constants.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
-#include <tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp>
 
 #include <vector>
 

common/motion_utils/include/motion_utils/resample/resample_utils.hpp

@@ -15,90 +15,24 @@
 #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
 #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
 
-#include "motion_utils/trajectory/trajectory.hpp"
-#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp"
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
+#include <geometry_msgs/msg/point.hpp>
 
 #include <boost/optional.hpp>
 
-#include <algorithm>
 #include <utility>
-#include <vector>
-
 namespace motion_utils
 {
-inline boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id)
-{
-  size_t start_idx = 0;  // NOTE: to prevent from maybe-uninitialized error
-  size_t end_idx = 0;    // NOTE: to prevent from maybe-uninitialized error
-
-  bool found_first_idx = false;
-  for (size_t i = 0; i < path.points.size(); ++i) {
-    const auto & p = path.points.at(i);
-    for (const auto & id : p.lane_ids) {
-      if (id == target_lane_id) {
-        if (!found_first_idx) {
-          start_idx = i;
-          found_first_idx = true;
-        }
-        end_idx = i;
-      }
-    }
-  }
-
-  if (found_first_idx) {
-    // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded.
-    start_idx = static_cast<size_t>(std::max(0, static_cast<int>(start_idx) - 1));
-    end_idx = std::min(path.points.size() - 1, end_idx + 1);
+boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);
 
-    return std::make_pair(start_idx, end_idx);
-  }
-
-  return {};
-}
-
-inline size_t findNearestIndexFromLaneId(
+size_t findNearestIndexFromLaneId(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const geometry_msgs::msg::Point & pos, const int64_t lane_id)
-{
-  const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id);
-  if (opt_range) {
-    const size_t start_idx = opt_range->first;
-    const size_t end_idx = opt_range->second;
-
-    validateNonEmpty(path.points);
+  const geometry_msgs::msg::Point & pos, const int64_t lane_id);
 
-    const auto sub_points = std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId>{
-      path.points.begin() + start_idx, path.points.begin() + end_idx + 1};
-    validateNonEmpty(sub_points);
-
-    return start_idx + findNearestIndex(sub_points, pos);
-  }
-
-  return findNearestIndex(path.points, pos);
-}
-
-inline size_t findNearestSegmentIndexFromLaneId(
+size_t findNearestSegmentIndexFromLaneId(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const geometry_msgs::msg::Point & pos, const int64_t lane_id)
-{
-  const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id);
-
-  if (nearest_idx == 0) {
-    return 0;
-  }
-  if (nearest_idx == path.points.size() - 1) {
-    return path.points.size() - 2;
-  }
-
-  const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos);
-
-  if (signed_length <= 0) {
-    return nearest_idx - 1;
-  }
-
-  return nearest_idx;
-}
+  const geometry_msgs::msg::Point & pos, const int64_t lane_id);
 
 // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle
 // center follow the input path

common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp

@@ -15,15 +15,9 @@
 #ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_
 #define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_
 
-#include "tier4_autoware_utils/geometry/geometry.hpp"
-#include "tier4_autoware_utils/geometry/pose_deviation.hpp"
-
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 
-#include <boost/optional.hpp>
-
-#include <algorithm>
 #include <vector>
 
 namespace motion_utils
@@ -39,30 +33,15 @@ namespace motion_utils
  * @todo Decide how to handle the situation that we need to use the trajectory with the size of
  * points larger than the capacity. (Tier IV)
  */
-inline autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory)
-{
-  autoware_auto_planning_msgs::msg::Trajectory output{};
-  for (const auto & pt : trajectory) {
-    output.points.push_back(pt);
-    if (output.points.size() >= output.CAPACITY) {
-      break;
-    }
-  }
-  return output;
-}
+autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory);
 
 /**
  * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to
  * std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>.
  */
-inline std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> convertToTrajectoryPointArray(
-  const autoware_auto_planning_msgs::msg::Trajectory & trajectory)
-{
-  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> output(trajectory.points.size());
-  std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin());
-  return output;
-}
+std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> convertToTrajectoryPointArray(
+  const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
 
 }  // namespace motion_utils