Merge pull request #1090 from HansOersted/beta/v0.20.0

refactor(motion_utils): cherry picked from a merged PR

common/interpolation/include/interpolation/spherical_linear_interpolation.hpp

@@ -39,6 +39,10 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
   const std::vector<double> & base_keys,
   const std::vector<geometry_msgs::msg::Quaternion> & base_values,
   const std::vector<double> & query_keys);
+
+geometry_msgs::msg::Quaternion lerpOrientation(
+  const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
+  const double ratio);
 }  // namespace interpolation
 
 #endif  // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_

common/interpolation/src/spherical_linear_interpolation.cpp

@@ -57,4 +57,15 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
   return query_values;
 }
 
+geometry_msgs::msg::Quaternion lerpOrientation(
+  const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
+  const double ratio)
+{
+  tf2::Quaternion q_from, q_to;
+  tf2::fromMsg(o_from, q_from);
+  tf2::fromMsg(o_to, q_to);
+
+  const auto q_interpolated = q_from.slerp(q_to, ratio);
+  return tf2::toMsg(q_interpolated);
+}
 }  // namespace interpolation

common/motion_utils/include/motion_utils/trajectory/conversion.hpp

@@ -15,6 +15,8 @@
 #ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
 #define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
 
+#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp"
+#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
 #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp"
 #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp"
 #include "std_msgs/msg/header.hpp"
@@ -23,6 +25,8 @@
 
 namespace motion_utils
 {
+using TrajectoryPoints = std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>;
+
 /**
  * @brief Convert std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> to
  * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to
@@ -45,6 +49,73 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
 std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> convertToTrajectoryPointArray(
   const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
 
+template <class T>
+autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input)
+{
+  static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used.");
+  throw std::logic_error("Only specializations of convertToPath can be used.");
+}
+
+template <>
+inline autoware_auto_planning_msgs::msg::Path convertToPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input)
+{
+  autoware_auto_planning_msgs::msg::Path output{};
+  output.header = input.header;
+  output.left_bound = input.left_bound;
+  output.right_bound = input.right_bound;
+  output.points.resize(input.points.size());
+  for (size_t i = 0; i < input.points.size(); ++i) {
+    output.points.at(i) = input.points.at(i).point;
+  }
+  return output;
+}
+
+template <class T>
+TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input)
+{
+  static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used.");
+  throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used.");
+}
+
+template <>
+inline TrajectoryPoints convertToTrajectoryPoints(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input)
+{
+  TrajectoryPoints output{};
+  for (const auto & p : input.points) {
+    autoware_auto_planning_msgs::msg::TrajectoryPoint tp;
+    tp.pose = p.point.pose;
+    tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
+    // since path point doesn't have acc for now
+    tp.acceleration_mps2 = 0;
+    output.emplace_back(tp);
+  }
+  return output;
+}
+
+template <class T>
+autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
+  [[maybe_unused]] const T & input)
+{
+  static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used.");
+  throw std::logic_error("Only specializations of convertToPathWithLaneId can be used.");
+}
+
+template <>
+inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
+  const TrajectoryPoints & input)
+{
+  autoware_auto_planning_msgs::msg::PathWithLaneId output{};
+  for (const auto & p : input) {
+    autoware_auto_planning_msgs::msg::PathPointWithLaneId pp;
+    pp.point.pose = p.pose;
+    pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
+    output.points.emplace_back(pp);
+  }
+  return output;
+}
+
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

@@ -16,6 +16,8 @@
 #define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_
 
 #include "interpolation/linear_interpolation.hpp"
+#include "interpolation/spherical_linear_interpolation.hpp"
+#include "motion_utils/trajectory/conversion.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "tf2/utils.h"
 
@@ -74,14 +76,6 @@ Pose calcPoseAfterTimeDelay(
   const Pose & current_pose, const double delay_time, const double current_vel,
   const double current_acc);
 
-/**
- * @brief apply linear interpolation to orientation
- * @param [in] o_from first orientation
- * @param [in] o_to second orientation
- * @param [in] ratio ratio between o_from and o_to for interpolation
- */
-Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);
-
 /**
  * @brief apply linear interpolation to trajectory point that is nearest to a certain point
  * @param [in] points trajectory points
@@ -107,7 +101,7 @@ TrajectoryPoint lerpTrajectoryPoint(
       points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
     interpolated_point.pose.position.y = interpolation::lerp(
       points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
-    interpolated_point.pose.orientation = lerpOrientation(
+    interpolated_point.pose.orientation = interpolation::lerpOrientation(
       points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
     interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
       points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps,

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

@@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio)
   return v_from + (v_to - v_from) * ratio;
 }
 
-Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
-{
-  tf2::Quaternion q_from, q_to;
-  tf2::fromMsg(o_from, q_from);
-  tf2::fromMsg(o_to, q_to);
-
-  const auto q_interpolated = q_from.slerp(q_to, ratio);
-  return tf2::toMsg(q_interpolated);
-}
-
 double applyDiffLimitFilter(
   const double input_val, const double prev_val, const double dt, const double max_val,
   const double min_val)

control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp

@@ -13,6 +13,8 @@
 // limitations under the License.
 
 #include "gtest/gtest.h"
+#include "interpolation/spherical_linear_interpolation.hpp"
+#include "motion_utils/trajectory/conversion.hpp"
 #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp"
 #include "tf2/LinearMath/Quaternion.h"
 
@@ -303,15 +305,15 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
   o_to.setRPY(M_PI_4, M_PI_4, M_PI_4);
 
   ratio = 0.0;
-  result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
+  result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
   tf2::convert(result, o_result);
   tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
   EXPECT_DOUBLE_EQ(roll, 0.0);
   EXPECT_DOUBLE_EQ(pitch, 0.0);
   EXPECT_DOUBLE_EQ(yaw, 0.0);
 
   ratio = 1.0;
-  result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
+  result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
   tf2::convert(result, o_result);
   tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
   EXPECT_DOUBLE_EQ(roll, M_PI_4);
@@ -320,23 +322,23 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
 
   ratio = 0.5;
   o_to.setRPY(M_PI_4, 0.0, 0.0);
-  result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
+  result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
   tf2::convert(result, o_result);
   tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
   EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2);
   EXPECT_DOUBLE_EQ(pitch, 0.0);
   EXPECT_DOUBLE_EQ(yaw, 0.0);
 
   o_to.setRPY(0.0, M_PI_4, 0.0);
-  result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
+  result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
   tf2::convert(result, o_result);
   tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
   EXPECT_DOUBLE_EQ(roll, 0.0);
   EXPECT_DOUBLE_EQ(pitch, M_PI_4 / 2);
   EXPECT_DOUBLE_EQ(yaw, 0.0);
 
   o_to.setRPY(0.0, 0.0, M_PI_4);
-  result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
+  result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
   tf2::convert(result, o_result);
   tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
   EXPECT_DOUBLE_EQ(roll, 0.0);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -17,6 +17,7 @@
 #include "behavior_path_planner_common/marker_utils/utils.hpp"
 #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner_common/utils/path_utils.hpp"
+#include "motion_utils/trajectory/conversion.hpp"
 
 #include <tier4_autoware_utils/ros/update_param.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
@@ -700,7 +701,8 @@ Path BehaviorPathPlannerNode::convertToPath(
     return output;
   }
 
-  output = utils::toPath(*path_candidate_ptr);
+  output = motion_utils::convertToPath<autoware_auto_planning_msgs::msg::PathWithLaneId>(
+    *path_candidate_ptr);
   // header is replaced by the input one, so it is substituted again
   output.header = planner_data->route_handler->getRouteHeader();
   output.header.stamp = this->now();

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp

@@ -58,8 +58,6 @@ PathWithLaneId resamplePathWithSpline(
   const PathWithLaneId & path, const double interval, const bool keep_input_points = false,
   const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});
 
-Path toPath(const PathWithLaneId & input);
-
 size_t getIdxByArclength(
   const PathWithLaneId & path, const size_t target_idx, const double signed_arc);
 

planning/behavior_path_planner_common/src/utils/path_utils.cpp

@@ -155,19 +155,6 @@ PathWithLaneId resamplePathWithSpline(
   return motion_utils::resamplePath(path, s_out);
 }
 
-Path toPath(const PathWithLaneId & input)
-{
-  Path output{};
-  output.header = input.header;
-  output.left_bound = input.left_bound;
-  output.right_bound = input.right_bound;
-  output.points.resize(input.points.size());
-  for (size_t i = 0; i < input.points.size(); ++i) {
-    output.points.at(i) = input.points.at(i).point;
-  }
-  return output;
-}
-
 size_t getIdxByArclength(
   const PathWithLaneId & path, const size_t target_idx, const double signed_arc)
 {

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp

@@ -35,11 +35,6 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using geometry_msgs::msg::Quaternion;
 using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;
 
-TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path);
-PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory);
-
-Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);
-
 //! smooth path point with lane id starts from ego position on path to the path end
 bool smoothPath(
   const PathWithLaneId & in_path, PathWithLaneId & out_path,

planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp

@@ -13,6 +13,8 @@
 // limitations under the License.
 
 // #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+#include "motion_utils/trajectory/conversion.hpp"
+
 #include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <motion_velocity_smoother/trajectory_utils.hpp>
@@ -41,41 +43,6 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
 using geometry_msgs::msg::Quaternion;
 using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;
 
-TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path)
-{
-  TrajectoryPoints tps;
-  for (const auto & p : path.points) {
-    TrajectoryPoint tp;
-    tp.pose = p.point.pose;
-    tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
-    // since path point doesn't have acc for now
-    tp.acceleration_mps2 = 0;
-    tps.emplace_back(tp);
-  }
-  return tps;
-}
-PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory)
-{
-  PathWithLaneId path;
-  for (const auto & p : trajectory) {
-    PathPointWithLaneId pp;
-    pp.point.pose = p.pose;
-    pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
-    path.points.emplace_back(pp);
-  }
-  return path;
-}
-
-Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
-{
-  tf2::Quaternion q_from, q_to;
-  tf2::fromMsg(o_from, q_from);
-  tf2::fromMsg(o_to, q_to);
-
-  const auto q_interpolated = q_from.slerp(q_to, ratio);
-  return tf2::toMsg(q_interpolated);
-}
-
 //! smooth path point with lane id starts from ego position on path to the path end
 bool smoothPath(
   const PathWithLaneId & in_path, PathWithLaneId & out_path,
@@ -87,7 +54,9 @@ bool smoothPath(
   const auto & external_v_limit = planner_data->external_velocity_limit;
   const auto & smoother = planner_data->velocity_smoother_;
 
-  auto trajectory = convertPathToTrajectoryPoints(in_path);
+  auto trajectory =
+    motion_utils::convertToTrajectoryPoints<autoware_auto_planning_msgs::msg::PathWithLaneId>(
+      in_path);
   const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
 
   const auto traj_steering_rate_limited =
@@ -117,7 +86,7 @@ bool smoothPath(
     motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
       traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed);
   }
-  out_path = convertTrajectoryPointsToPath(traj_smoothed);
+  out_path = motion_utils::convertToPathWithLaneId<TrajectoryPoints>(traj_smoothed);
   return true;
 }
 

planning/planning_test_utils/include/planning_test_utils/planning_interface_test_manager_utils.hpp

@@ -492,19 +492,6 @@ void updateNodeOptions(
   node_options.arguments(std::vector<std::string>{arguments.begin(), arguments.end()});
 }
 
-Path toPath(const PathWithLaneId & input)
-{
-  Path output{};
-  output.header = input.header;
-  output.left_bound = input.left_bound;
-  output.right_bound = input.right_bound;
-  output.points.resize(input.points.size());
-  for (size_t i = 0; i < input.points.size(); ++i) {
-    output.points.at(i) = input.points.at(i).point;
-  }
-  return output;
-}
-
 PathWithLaneId loadPathWithLaneIdInYaml()
 {
   const auto planning_test_utils_dir =

planning/planning_test_utils/package.xml

@@ -23,6 +23,7 @@
   <depend>component_interface_utils</depend>
   <depend>lanelet2_extension</depend>
   <depend>lanelet2_io</depend>
+  <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>route_handler</depend>