diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp index af47c404f7..cc69ffc2ae 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp @@ -36,6 +36,9 @@ namespace autoware::trajectory template <> class Trajectory { + template + friend class Trajectory; + using PointType = geometry_msgs::msg::Point; protected: diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp index 911db11bf8..a7030e352b 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp @@ -49,6 +49,9 @@ class Trajectory : public Trajectory & point_trajectory); + bool build(const std::vector & points); /** diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index 0dc3287769..37dca2f24a 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -15,6 +15,7 @@ #include "autoware/trajectory/pose.hpp" #include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/forward.hpp" #include "autoware/trajectory/interpolator/spherical_linear.hpp" #include @@ -45,6 +46,32 @@ Trajectory & Trajectory::operator=(const Trajectory & rhs) return *this; } +Trajectory::Trajectory(const Trajectory & point_trajectory) +: Trajectory() +{ + x_interpolator_ = point_trajectory.x_interpolator_->clone(); + y_interpolator_ = point_trajectory.y_interpolator_->clone(); + z_interpolator_ = point_trajectory.z_interpolator_->clone(); + bases_ = point_trajectory.get_internal_bases(); + start_ = point_trajectory.start_; + end_ = point_trajectory.end_; + + // build mock orientations + std::vector orientations(bases_.size()); + for (size_t i = 0; i < bases_.size(); ++i) { + orientations[i].w = 1.0; + } + bool success = orientation_interpolator_->build(bases_, orientations); + + if (!success) { + throw std::runtime_error( + "Failed to build orientation interpolator."); // This Exception should not be thrown. + } + + // align orientation with trajectory direction + align_orientation_with_trajectory_direction(); +} + bool Trajectory::build(const std::vector & points) { std::vector path_points;