feat(motion_utils, etc): add header argument in convertToTrajectory (#6021)

* feat(motion_utils, etc): add header argument in convertToTrajectory

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

* fix include

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

---------

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

Author: takayuki5168

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

@@ -17,6 +17,7 @@
 
 #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"
 
 #include <vector>
 
@@ -34,7 +35,8 @@ namespace motion_utils
  * points larger than the capacity. (Tier IV)
  */
 autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory);
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std_msgs::msg::Header & header = std_msgs::msg::Header{});
 
 /**
  * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to

common/motion_utils/src/trajectory/conversion.cpp

@@ -30,9 +30,11 @@ namespace motion_utils
  * points larger than the capacity. (Tier IV)
  */
 autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory)
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std_msgs::msg::Header & header)
 {
   autoware_auto_planning_msgs::msg::Trajectory output{};
+  output.header = header;
   for (const auto & pt : trajectory) output.points.push_back(pt);
   return output;
 }

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp

@@ -154,9 +154,6 @@ size_t findEgoSegmentIndex(
     points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold);
 }
 
-Trajectory createTrajectory(
-  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points);
-
 std::vector<TrajectoryPoint> resampleTrajectoryPoints(
   const std::vector<TrajectoryPoint> traj_points, const double interval);
 

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -15,6 +15,7 @@
 #include "obstacle_avoidance_planner/mpt_optimizer.hpp"
 
 #include "interpolation/spline_interpolation_points_2d.hpp"
+#include "motion_utils/trajectory/conversion.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "obstacle_avoidance_planner/utils/geometry_utils.hpp"
 #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp"
@@ -1708,17 +1709,17 @@ void MPTOptimizer::publishDebugTrajectories(
   time_keeper_ptr_->tic(__func__);
 
   // reference points
-  const auto ref_traj = trajectory_utils::createTrajectory(
-    header, trajectory_utils::convertToTrajectoryPoints(ref_points));
+  const auto ref_traj = motion_utils::convertToTrajectory(
+    trajectory_utils::convertToTrajectoryPoints(ref_points), header);
   debug_ref_traj_pub_->publish(ref_traj);
 
   // fixed reference points
   const auto fixed_traj_points = extractFixedPoints(ref_points);
-  const auto fixed_traj = trajectory_utils::createTrajectory(header, fixed_traj_points);
+  const auto fixed_traj = motion_utils::convertToTrajectory(fixed_traj_points, header);
   debug_fixed_traj_pub_->publish(fixed_traj);
 
   // mpt points
-  const auto mpt_traj = trajectory_utils::createTrajectory(header, mpt_traj_points);
+  const auto mpt_traj = motion_utils::convertToTrajectory(mpt_traj_points, header);
   debug_mpt_traj_pub_->publish(mpt_traj);
 
   time_keeper_ptr_->toc(__func__, "        ");

planning/obstacle_avoidance_planner/src/node.cpp

@@ -16,6 +16,7 @@
 
 #include "interpolation/spline_interpolation_points_2d.hpp"
 #include "motion_utils/marker/marker_helper.hpp"
+#include "motion_utils/trajectory/conversion.hpp"
 #include "obstacle_avoidance_planner/debug_marker.hpp"
 #include "obstacle_avoidance_planner/utils/geometry_utils.hpp"
 #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp"
@@ -236,7 +237,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
       "Backward path is NOT supported. Just converting path to trajectory");
 
     const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points);
-    const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points);
+    const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header);
     traj_pub_->publish(output_traj_msg);
     return;
   }
@@ -268,7 +269,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
     createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime()));
 
   const auto output_traj_msg =
-    trajectory_utils::createTrajectory(path_ptr->header, full_traj_points);
+    motion_utils::convertToTrajectory(full_traj_points, path_ptr->header);
   traj_pub_->publish(output_traj_msg);
 }
 
@@ -656,7 +657,7 @@ void ObstacleAvoidancePlanner::publishDebugData(const Header & header) const
 
   // publish trajectories
   const auto debug_extended_traj =
-    trajectory_utils::createTrajectory(header, debug_data_ptr_->extended_traj_points);
+    motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header);
   debug_extended_traj_pub_->publish(debug_extended_traj);
 
   time_keeper_ptr_->toc(__func__, "  ");

planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp

@@ -117,15 +117,6 @@ geometry_msgs::msg::Point getNearestPosition(
   return points.back().pose.position;
 }
 
-Trajectory createTrajectory(
-  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points)
-{
-  auto traj = motion_utils::convertToTrajectory(traj_points);
-  traj.header = header;
-
-  return traj;
-}
-
 std::vector<TrajectoryPoint> resampleTrajectoryPoints(
   const std::vector<TrajectoryPoint> traj_points, const double interval)
 {

planning/obstacle_cruise_planner/src/node.cpp

@@ -171,15 +171,6 @@ std::vector<TrajectoryPoint> extendTrajectoryPoints(
   return output_points;
 }
 
-Trajectory createTrajectory(
-  const std::vector<TrajectoryPoint> & traj_points, const std_msgs::msg::Header & header)
-{
-  auto traj = motion_utils::convertToTrajectory(traj_points);
-  traj.header = header;
-
-  return traj;
-}
-
 std::vector<int> getTargetObjectType(rclcpp::Node & node, const std::string & param_prefix)
 {
   std::unordered_map<std::string, int> types_map{
@@ -525,7 +516,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
   publishVelocityLimit(slow_down_vel_limit, "slow_down");
 
   // 7. Publish trajectory
-  const auto output_traj = createTrajectory(slow_down_traj_points, msg->header);
+  const auto output_traj = motion_utils::convertToTrajectory(slow_down_traj_points, msg->header);
   trajectory_pub_->publish(output_traj);
 
   // 8. Publish debug data

planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp

@@ -113,9 +113,6 @@ size_t findEgoSegmentIndex(
     points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold);
 }
 
-Trajectory createTrajectory(
-  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points);
-
 Path create_path(Path path_msg, const std::vector<TrajectoryPoint> & traj_points);
 
 std::vector<TrajectoryPoint> resampleTrajectoryPoints(

planning/path_smoother/src/elastic_band.cpp

@@ -14,6 +14,7 @@
 
 #include "path_smoother/elastic_band.hpp"
 
+#include "motion_utils/trajectory/conversion.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "path_smoother/type_alias.hpp"
 #include "path_smoother/utils/geometry_utils.hpp"
@@ -262,7 +263,7 @@ std::vector<TrajectoryPoint> EBPathSmoother::smoothTrajectory(
 
   // 8. publish eb trajectory
   const auto eb_traj =
-    trajectory_utils::createTrajectory(createHeader(clock_.now()), *eb_traj_points);
+    motion_utils::convertToTrajectory(*eb_traj_points, createHeader(clock_.now()));
   debug_eb_traj_pub_->publish(eb_traj);
 
   time_keeper_ptr_->toc(__func__, "      ");
@@ -389,7 +390,7 @@ void EBPathSmoother::updateConstraint(
 
   // publish fixed trajectory
   const auto eb_fixed_traj =
-    trajectory_utils::createTrajectory(createHeader(clock_.now()), debug_fixed_traj_points);
+    motion_utils::convertToTrajectory(debug_fixed_traj_points, createHeader(clock_.now()));
   debug_eb_fixed_traj_pub_->publish(eb_fixed_traj);
 
   time_keeper_ptr_->toc(__func__, "        ");

planning/path_smoother/src/elastic_band_smoother.cpp

@@ -15,6 +15,7 @@
 #include "path_smoother/elastic_band_smoother.hpp"
 
 #include "interpolation/spline_interpolation_points_2d.hpp"
+#include "motion_utils/trajectory/conversion.hpp"
 #include "path_smoother/utils/geometry_utils.hpp"
 #include "path_smoother/utils/trajectory_utils.hpp"
 #include "rclcpp/time.hpp"
@@ -167,7 +168,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
       "Backward path is NOT supported. Just converting path to trajectory");
 
     const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points);
-    const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, traj_points);
+    const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header);
     traj_pub_->publish(output_traj_msg);
     path_pub_->publish(*path_ptr);
     return;
@@ -220,7 +221,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
     createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime()));
 
   const auto output_traj_msg =
-    trajectory_utils::createTrajectory(path_ptr->header, full_traj_points);
+    motion_utils::convertToTrajectory(full_traj_points, path_ptr->header);
   traj_pub_->publish(output_traj_msg);
   const auto output_path_msg = trajectory_utils::create_path(*path_ptr, full_traj_points);
   path_pub_->publish(output_path_msg);

planning/path_smoother/src/utils/trajectory_utils.cpp

@@ -34,15 +34,6 @@ namespace path_smoother
 {
 namespace trajectory_utils
 {
-Trajectory createTrajectory(
-  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points)
-{
-  auto traj = motion_utils::convertToTrajectory(traj_points);
-  traj.header = header;
-
-  return traj;
-}
-
 Path create_path(Path path_msg, const std::vector<TrajectoryPoint> & traj_points)
 {
   path_msg.points.clear();

planning/planning_topic_converter/src/path_to_trajectory.cpp

@@ -40,15 +40,6 @@ std::vector<TrajectoryPoint> convertToTrajectoryPoints(const std::vector<PathPoi
   }
   return traj_points;
 }
-
-Trajectory createTrajectory(
-  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & trajectory_points)
-{
-  auto trajectory = motion_utils::convertToTrajectory(trajectory_points);
-  trajectory.header = header;
-
-  return trajectory;
-}
 }  // namespace
 
 PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options)
@@ -59,7 +50,7 @@ PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options)
 void PathToTrajectory::process(const Path::ConstSharedPtr msg)
 {
   const auto trajectory_points = convertToTrajectoryPoints(msg->points);
-  const auto output = createTrajectory(msg->header, trajectory_points);
+  const auto output = motion_utils::convertToTrajectory(trajectory_points, msg->header);
   pub_->publish(output);
 }
 

planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp

@@ -138,9 +138,6 @@ size_t findEgoSegmentIndex(
     points, ego_pose, ego_nearest_param.dist_threshold, ego_nearest_param.yaw_threshold);
 }
 
-Trajectory createTrajectory(
-  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points);
-
 std::vector<TrajectoryPoint> resampleTrajectoryPoints(
   const std::vector<TrajectoryPoint> traj_points, const double interval);
 

planning/sampling_based_planner/path_sampler/src/node.cpp

@@ -16,6 +16,7 @@
 
 #include "interpolation/spline_interpolation_points_2d.hpp"
 #include "motion_utils/marker/marker_helper.hpp"
+#include "motion_utils/trajectory/conversion.hpp"
 #include "path_sampler/path_generation.hpp"
 #include "path_sampler/prepare_inputs.hpp"
 #include "path_sampler/utils/geometry_utils.hpp"
@@ -244,13 +245,12 @@ void PathSampler::onPath(const Path::SharedPtr path_ptr)
   if (!generated_traj_points.empty()) {
     auto full_traj_points = extendTrajectory(planner_data.traj_points, generated_traj_points);
     const auto output_traj_msg =
-      trajectory_utils::createTrajectory(path_ptr->header, full_traj_points);
+      motion_utils::convertToTrajectory(full_traj_points, path_ptr->header);
     traj_pub_->publish(output_traj_msg);
   } else {
     auto stopping_traj = trajectory_utils::convertToTrajectoryPoints(planner_data.traj_points);
     for (auto & p : stopping_traj) p.longitudinal_velocity_mps = 0.0;
-    const auto output_traj_msg =
-      trajectory_utils::createTrajectory(path_ptr->header, stopping_traj);
+    const auto output_traj_msg = motion_utils::convertToTrajectory(stopping_traj, path_ptr->header);
     traj_pub_->publish(output_traj_msg);
   }
 

planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp

@@ -58,15 +58,6 @@ void compensateLastPose(
   }
 }
 
-Trajectory createTrajectory(
-  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & traj_points)
-{
-  auto traj = motion_utils::convertToTrajectory(traj_points);
-  traj.header = header;
-
-  return traj;
-}
-
 std::vector<TrajectoryPoint> resampleTrajectoryPoints(
   const std::vector<TrajectoryPoint> traj_points, const double interval)
 {