From ee9d7923a38b6c58490d4c0324eaba213d878164 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 3 Mar 2025 16:30:23 +0900 Subject: [PATCH] add TrajectoryPoint class to templates Signed-off-by: Daniel Sanchez --- .../autoware/trajectory/trajectory_point.hpp | 221 ++++++++++++++++++ .../src/trajectory_point.cpp | 153 ++++++++++++ 2 files changed, 374 insertions(+) create mode 100644 common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp create mode 100644 common/autoware_trajectory/src/trajectory_point.cpp diff --git a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp new file mode 100644 index 0000000000..e507c933b8 --- /dev/null +++ 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 + +#include +#include +#include + +namespace autoware::trajectory +{ +template <> +class Trajectory +: public Trajectory +{ + using BaseClass = Trajectory; + using PointType = autoware_planning_msgs::msg::TrajectoryPoint; + + std::shared_ptr> + longitudinal_velocity_mps_; //!< Longitudinal velocity in m/s + std::shared_ptr> + lateral_velocity_mps_; //!< Lateral velocity in m/s + std::shared_ptr> + heading_rate_rps_; //!< Heading rate in rad/s}; + std::shared_ptr> + acceleration_mps2_; //!< Longitudinal acceleration in m/s^2} Warning, this is not used + std::shared_ptr> + front_wheel_angle_rad_; //!< Front wheel angle in rad} Warning, this is not used + std::shared_ptr> + 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 get_internal_bases() const override; + + detail::InterpolatedArray & longitudinal_velocity_mps() + { + return *longitudinal_velocity_mps_; + } + + detail::InterpolatedArray & lateral_velocity_mps() { return *lateral_velocity_mps_; } + + detail::InterpolatedArray & heading_rate_rps() { return *heading_rate_rps_; } + + detail::InterpolatedArray & acceleration_mps2() { return *acceleration_mps2_; } + + detail::InterpolatedArray & front_wheel_angle_rad() { return *front_wheel_angle_rad_; } + + detail::InterpolatedArray & rear_wheel_angle_rad() { return *rear_wheel_angle_rad_; } + + [[nodiscard]] const detail::InterpolatedArray & longitudinal_velocity_mps() const + { + return *longitudinal_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & lateral_velocity_mps() const + { + return *lateral_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & heading_rate_rps() const + { + return *heading_rate_rps_; + } + + [[nodiscard]] const detail::InterpolatedArray & acceleration_mps2() const + { + return *acceleration_mps2_; + } + + [[nodiscard]] const detail::InterpolatedArray & front_wheel_angle_rad() const + { + return *front_wheel_angle_rad_; + } + + [[nodiscard]] const detail::InterpolatedArray & 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 & 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 restore(const size_t & min_points = 4) const; + + class Builder + { + private: + std::unique_ptr trajectory_; + + public: + Builder() : trajectory_(std::make_unique()) {} + + template + Builder & set_xy_interpolator(Args &&... args) + { + trajectory_->x_interpolator_ = + std::make_shared(std::forward(args)...); + trajectory_->y_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_z_interpolator(Args &&... args) + { + trajectory_->z_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_orientation_interpolator(Args &&... args) + { + trajectory_->orientation_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_longitudinal_velocity_interpolator(Args &&... args) + { + trajectory_->longitudinal_velocity_mps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_lateral_velocity_interpolator(Args &&... args) + { + trajectory_->lateral_velocity_mps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_heading_rate_interpolator(Args &&... args) + { + trajectory_->heading_rate_rps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_acceleration_interpolator(Args &&... args) + { + trajectory_->acceleration_mps2_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_front_wheel_angle_interpolator(Args &&... args) + { + trajectory_->front_wheel_angle_rad_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_rear_wheel_angle_interpolator(Args &&... args) + { + trajectory_->rear_wheel_angle_rad_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + std::optional build(const std::vector & points) + { + if (trajectory_->build(points)) { + auto result = std::make_optional(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/trajectory_point.cpp b/common/autoware_trajectory/src/trajectory_point.cpp new file mode 100644 index 0000000000..2828a1ecd2 --- /dev/null +++ b/common/autoware_trajectory/src/trajectory_point.cpp @@ -0,0 +1,153 @@ +// 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. + +#include "autoware/trajectory/trajectory_point.hpp" + +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/forward.hpp" +#include "autoware/trajectory/interpolator/stairstep.hpp" +#include "autoware/trajectory/pose.hpp" + +#include + +#include +#include + +namespace autoware::trajectory +{ + +using PointType = autoware_planning_msgs::msg::TrajectoryPoint; + +Trajectory::Trajectory() +: longitudinal_velocity_mps_(std::make_shared>( + std::make_shared>())), + lateral_velocity_mps_(std::make_shared>( + std::make_shared>())), + heading_rate_rps_(std::make_shared>( + std::make_shared>())), + acceleration_mps2_(std::make_shared>( + std::make_shared>())), + front_wheel_angle_rad_(std::make_shared>( + std::make_shared>())), + rear_wheel_angle_rad_(std::make_shared>( + std::make_shared>())) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: BaseClass(rhs), + longitudinal_velocity_mps_( + std::make_shared>(*rhs.longitudinal_velocity_mps_)), + lateral_velocity_mps_( + std::make_shared>(*rhs.lateral_velocity_mps_)), + heading_rate_rps_(std::make_shared>(*rhs.heading_rate_rps_)), + acceleration_mps2_(std::make_shared>(*rhs.acceleration_mps2_)), + front_wheel_angle_rad_( + std::make_shared>(*rhs.front_wheel_angle_rad_)), + rear_wheel_angle_rad_( + std::make_shared>(*rhs.rear_wheel_angle_rad_)) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + *longitudinal_velocity_mps_ = *rhs.longitudinal_velocity_mps_; + *lateral_velocity_mps_ = *rhs.lateral_velocity_mps_; + *heading_rate_rps_ = *rhs.heading_rate_rps_; + *acceleration_mps2_ = *rhs.acceleration_mps2_; + *front_wheel_angle_rad_ = *rhs.front_wheel_angle_rad_; + *rear_wheel_angle_rad_ = *rhs.rear_wheel_angle_rad_; + } + return *this; +} + +bool Trajectory::build(const std::vector & points) +{ + std::vector poses; + std::vector longitudinal_velocity_mps_values; + std::vector lateral_velocity_mps_values; + std::vector heading_rate_rps_values; + std::vector acceleration_mps2_values; + std::vector front_wheel_angle_rad_values; + std::vector rear_wheel_angle_rad_values; + + for (const auto & point : points) { + poses.emplace_back(point.pose); + longitudinal_velocity_mps_values.emplace_back(point.longitudinal_velocity_mps); + lateral_velocity_mps_values.emplace_back(point.lateral_velocity_mps); + heading_rate_rps_values.emplace_back(point.heading_rate_rps); + acceleration_mps2_values.emplace_back(point.acceleration_mps2); + front_wheel_angle_rad_values.emplace_back(point.front_wheel_angle_rad); + rear_wheel_angle_rad_values.emplace_back(point.rear_wheel_angle_rad); + } + + bool is_valid = true; + + is_valid &= Trajectory::build(poses); + is_valid &= this->longitudinal_velocity_mps().build(bases_, longitudinal_velocity_mps_values); + is_valid &= this->lateral_velocity_mps().build(bases_, lateral_velocity_mps_values); + is_valid &= this->heading_rate_rps().build(bases_, heading_rate_rps_values); + is_valid &= this->acceleration_mps2().build(bases_, acceleration_mps2_values); + is_valid &= this->front_wheel_angle_rad().build(bases_, front_wheel_angle_rad_values); + is_valid &= this->rear_wheel_angle_rad().build(bases_, rear_wheel_angle_rad_values); + + return is_valid; +} + +std::vector Trajectory::get_internal_bases() const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()), + get_bases(this->heading_rate_rps())); + + bases = detail::crop_bases(bases, start_, end_); + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + +PointType Trajectory::compute(double s) const +{ + PointType result; + result.pose = Trajectory::compute(s); + s = clamp(s); + result.longitudinal_velocity_mps = + static_cast(this->longitudinal_velocity_mps().compute(s)); + result.lateral_velocity_mps = static_cast(this->lateral_velocity_mps().compute(s)); + result.heading_rate_rps = static_cast(this->heading_rate_rps().compute(s)); + return result; +} + +std::vector Trajectory::restore(const size_t & min_points) const +{ + std::vector bases = get_internal_bases(); + bases = detail::fill_bases(bases, min_points); + + std::vector points; + points.reserve(bases.size()); + for (const auto & s : bases) { + points.emplace_back(compute(s)); + } + return points; +} + +} // namespace autoware::trajectory