feat(autoware_trajectory): add trajectory point (#233)

* add TrajectoryPoint class to templates Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add tests Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add method to_point for TrajectoryPoint type Signed-off-by: Daniel <danielsanchezaran@gmail.com> * change name of test to avoid name collision Signed-off-by: Daniel <danielsanchezaran@gmail.com> * add missing items Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * rename example name for clarity Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Signed-off-by: Daniel <danielsanchezaran@gmail.com> Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> Co-authored-by: Y.Hisaki <yhisaki31@gmail.com>
autowarefoundation · Mar 7, 2025 · 2e5c9e0 · 2e5c9e0
1 parent d56554a
commit 2e5c9e0
Show file tree

Hide file tree

Showing 8 changed files with 567 additions and 0 deletions.
diff --git a/...xamples/example_trajectory_path_point.cpp → ...rajectory/examples/example_path_point.cpp b/...xamples/example_trajectory_path_point.cpp → ...rajectory/examples/example_path_point.cpp
diff --git a/...ory/examples/example_trajectory_point.cpp → ...are_trajectory/examples/example_point.cpp b/...ory/examples/example_trajectory_point.cpp → ...are_trajectory/examples/example_point.cpp
diff --git a/...tory/examples/example_trajectory_pose.cpp → ...ware_trajectory/examples/example_pose.cpp b/...tory/examples/example_trajectory_pose.cpp → ...ware_trajectory/examples/example_pose.cpp
diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp
@@ -20,7 +20,9 @@
 #include <Eigen/Core>
 
 #include <autoware_internal_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp>
 #include <autoware_planning_msgs/msg/path_point.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
@@ -63,12 +65,14 @@ struct ImmutablePoint3d : ImmutablePoint2d
 MutablePoint3d to_point(geometry_msgs::msg::Point & p);
 MutablePoint3d to_point(geometry_msgs::msg::Pose & p);
 MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p);
+MutablePoint3d to_point(autoware_planning_msgs::msg::TrajectoryPoint & p);
 MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p);
 MutablePoint2d to_point(lanelet::BasicPoint2d & p);
 
 ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p);
 ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p);
 ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p);
+ImmutablePoint3d to_point(const autoware_planning_msgs::msg::TrajectoryPoint & p);
 ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p);
 ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p);
 

diff --git a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp
@@ -0,0 +1,221 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_
+#define AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_
+
+#include "autoware/trajectory/detail/interpolated_array.hpp"
+#include "autoware/trajectory/pose.hpp"
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace autoware::trajectory
+{
+template <>
+class Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>
+: public Trajectory<geometry_msgs::msg::Pose>
+{
+  using BaseClass = Trajectory<geometry_msgs::msg::Pose>;
+  using PointType = autoware_planning_msgs::msg::TrajectoryPoint;
+
+  std::shared_ptr<detail::InterpolatedArray<double>>
+    longitudinal_velocity_mps_;  //!< Longitudinal velocity in m/s
+  std::shared_ptr<detail::InterpolatedArray<double>>
+    lateral_velocity_mps_;  //!< Lateral velocity in m/s
+  std::shared_ptr<detail::InterpolatedArray<double>>
+    heading_rate_rps_;  //!< Heading rate in rad/s};
+  std::shared_ptr<detail::InterpolatedArray<double>>
+    acceleration_mps2_;  //!< Longitudinal acceleration in m/s^2} Warning, this is not used
+  std::shared_ptr<detail::InterpolatedArray<double>>
+    front_wheel_angle_rad_;  //!< Front wheel angle in rad} Warning, this is not used
+  std::shared_ptr<detail::InterpolatedArray<double>>
+    rear_wheel_angle_rad_;  //!< Rear wheel angle in rad} Warning, this is not used
+
+public:
+  Trajectory();
+  ~Trajectory() override = default;
+  Trajectory(const Trajectory & rhs);
+  Trajectory(Trajectory && rhs) = default;
+  Trajectory & operator=(const Trajectory & rhs);
+  Trajectory & operator=(Trajectory && rhs) = default;
+
+  [[nodiscard]] std::vector<double> get_internal_bases() const override;
+
+  detail::InterpolatedArray<double> & longitudinal_velocity_mps()
+  {
+    return *longitudinal_velocity_mps_;
+  }
+
+  detail::InterpolatedArray<double> & lateral_velocity_mps() { return *lateral_velocity_mps_; }
+
+  detail::InterpolatedArray<double> & heading_rate_rps() { return *heading_rate_rps_; }
+
+  detail::InterpolatedArray<double> & acceleration_mps2() { return *acceleration_mps2_; }
+
+  detail::InterpolatedArray<double> & front_wheel_angle_rad() { return *front_wheel_angle_rad_; }
+
+  detail::InterpolatedArray<double> & rear_wheel_angle_rad() { return *rear_wheel_angle_rad_; }
+
+  [[nodiscard]] const detail::InterpolatedArray<double> & longitudinal_velocity_mps() const
+  {
+    return *longitudinal_velocity_mps_;
+  }
+
+  [[nodiscard]] const detail::InterpolatedArray<double> & lateral_velocity_mps() const
+  {
+    return *lateral_velocity_mps_;
+  }
+
+  [[nodiscard]] const detail::InterpolatedArray<double> & heading_rate_rps() const
+  {
+    return *heading_rate_rps_;
+  }
+
+  [[nodiscard]] const detail::InterpolatedArray<double> & acceleration_mps2() const
+  {
+    return *acceleration_mps2_;
+  }
+
+  [[nodiscard]] const detail::InterpolatedArray<double> & front_wheel_angle_rad() const
+  {
+    return *front_wheel_angle_rad_;
+  }
+
+  [[nodiscard]] const detail::InterpolatedArray<double> & rear_wheel_angle_rad() const
+  {
+    return *rear_wheel_angle_rad_;
+  }
+
+  /**
+   * @brief Build the trajectory from the points
+   * @param points Vector of points
+   * @return True if the build is successful
+   */
+  bool build(const std::vector<PointType> & points);
+
+  /**
+   * @brief Compute the point on the trajectory at a given s value
+   * @param s Arc length
+   * @return Point on the trajectory
+   */
+  [[nodiscard]] PointType compute(double s) const;
+
+  /**
+   * @brief Restore the trajectory points
+   * @param min_points Minimum number of points
+   * @return Vector of points
+   */
+  [[nodiscard]] std::vector<PointType> restore(const size_t & min_points = 4) const;
+
+  class Builder
+  {
+  private:
+    std::unique_ptr<Trajectory> trajectory_;
+
+  public:
+    Builder() : trajectory_(std::make_unique<Trajectory>()) {}
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_xy_interpolator(Args &&... args)
+    {
+      trajectory_->x_interpolator_ =
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...);
+      trajectory_->y_interpolator_ =
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...);
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_z_interpolator(Args &&... args)
+    {
+      trajectory_->z_interpolator_ =
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...);
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_orientation_interpolator(Args &&... args)
+    {
+      trajectory_->orientation_interpolator_ =
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...);
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_longitudinal_velocity_interpolator(Args &&... args)
+    {
+      trajectory_->longitudinal_velocity_mps_ = std::make_shared<detail::InterpolatedArray<double>>(
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...));
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_lateral_velocity_interpolator(Args &&... args)
+    {
+      trajectory_->lateral_velocity_mps_ = std::make_shared<detail::InterpolatedArray<double>>(
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...));
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_heading_rate_interpolator(Args &&... args)
+    {
+      trajectory_->heading_rate_rps_ = std::make_shared<detail::InterpolatedArray<double>>(
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...));
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_acceleration_interpolator(Args &&... args)
+    {
+      trajectory_->acceleration_mps2_ = std::make_shared<detail::InterpolatedArray<double>>(
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...));
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_front_wheel_angle_interpolator(Args &&... args)
+    {
+      trajectory_->front_wheel_angle_rad_ = std::make_shared<detail::InterpolatedArray<double>>(
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...));
+      return *this;
+    }
+
+    template <class InterpolatorType, class... Args>
+    Builder & set_rear_wheel_angle_interpolator(Args &&... args)
+    {
+      trajectory_->rear_wheel_angle_rad_ = std::make_shared<detail::InterpolatedArray<double>>(
+        std::make_shared<InterpolatorType>(std::forward<Args>(args)...));
+      return *this;
+    }
+
+    std::optional<Trajectory> build(const std::vector<PointType> & points)
+    {
+      if (trajectory_->build(points)) {
+        auto result = std::make_optional<Trajectory>(std::move(*trajectory_));
+        trajectory_.reset();
+        return result;
+      }
+      return std::nullopt;
+    }
+  };
+};
+
+}  // namespace autoware::trajectory
+
+#endif  // AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_
diff --git a/common/autoware_trajectory/src/detail/types.cpp b/common/autoware_trajectory/src/detail/types.cpp
@@ -14,6 +14,8 @@
 
 #include "autoware/trajectory/detail/types.hpp"
 
+#include <autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp>
+
 namespace autoware::trajectory::detail
 {
 MutablePoint3d to_point(geometry_msgs::msg::Point & p)
@@ -31,6 +33,11 @@ MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p)
   return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
 }
 
+MutablePoint3d to_point(autoware_planning_msgs::msg::TrajectoryPoint & p)
+{
+  return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
+}
+
 MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z};
@@ -56,6 +63,11 @@ ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p)
   return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
 }
 
+ImmutablePoint3d to_point(const autoware_planning_msgs::msg::TrajectoryPoint & p)
+{
+  return {p.pose.position.x, p.pose.position.y, p.pose.position.z};
+}
+
 ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p)
 {
   return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z};