-
Notifications
You must be signed in to change notification settings - Fork 683
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'main' into fix/blockage_diag_should_throw_error_message
- Loading branch information
Showing
249 changed files
with
4,795 additions
and
2,412 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
115 changes: 115 additions & 0 deletions
115
common/autoware_motion_utils/examples/example_interpolator.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,115 @@ | ||
| // 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/motion_utils/trajectory_container/interpolator/linear.hpp" | ||
|
|
||
| #include <autoware/motion_utils/trajectory_container/interpolator.hpp> | ||
|
|
||
| #include <matplotlibcpp17/pyplot.h> | ||
|
|
||
| #include <random> | ||
| #include <vector> | ||
|
|
||
| int main() | ||
| { | ||
| pybind11::scoped_interpreter guard{}; | ||
| auto plt = matplotlibcpp17::pyplot::import(); | ||
|
|
||
| // create random values | ||
| std::vector<double> axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; | ||
| std::vector<double> values; | ||
| std::random_device seed_gen; | ||
| std::mt19937 engine(seed_gen()); | ||
| std::uniform_real_distribution<> dist(-1.0, 1.0); | ||
| for (size_t i = 0; i < axis.size(); ++i) { | ||
| values.push_back(dist(engine)); | ||
| } | ||
| // Scatter Data | ||
| plt.scatter(Args(axis, values)); | ||
|
|
||
| using autoware::motion_utils::trajectory_container::interpolator::Interpolator; | ||
| using autoware::motion_utils::trajectory_container::interpolator::InterpolatorCreator; | ||
| // Linear Interpolator | ||
| { | ||
| using autoware::motion_utils::trajectory_container::interpolator::Linear; | ||
| auto interpolator = *InterpolatorCreator<Linear>().set_axis(axis).set_values(values).create(); | ||
| std::vector<double> x; | ||
| std::vector<double> y; | ||
| for (double i = axis.front(); i < axis.back(); i += 0.01) { | ||
| x.push_back(i); | ||
| y.push_back(interpolator.compute(i)); | ||
| } | ||
| plt.plot(Args(x, y), Kwargs("label"_a = "Linear")); | ||
| } | ||
|
|
||
| // AkimaSpline Interpolator | ||
| { | ||
| using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline; | ||
| auto interpolator = | ||
| *InterpolatorCreator<AkimaSpline>().set_axis(axis).set_values(values).create(); | ||
| std::vector<double> x; | ||
| std::vector<double> y; | ||
| for (double i = axis.front(); i < axis.back(); i += 0.01) { | ||
| x.push_back(i); | ||
| y.push_back(interpolator.compute(i)); | ||
| } | ||
| plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline")); | ||
| } | ||
|
|
||
| // CubicSpline Interpolator | ||
| { | ||
| using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; | ||
| auto interpolator = | ||
| *InterpolatorCreator<CubicSpline>().set_axis(axis).set_values(values).create(); | ||
| std::vector<double> x; | ||
| std::vector<double> y; | ||
| for (double i = axis.front(); i < axis.back(); i += 0.01) { | ||
| x.push_back(i); | ||
| y.push_back(interpolator.compute(i)); | ||
| } | ||
| plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline")); | ||
| } | ||
|
|
||
| // NearestNeighbor Interpolator | ||
| { | ||
| using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor; | ||
| auto interpolator = | ||
| *InterpolatorCreator<NearestNeighbor<double>>().set_axis(axis).set_values(values).create(); | ||
| std::vector<double> x; | ||
| std::vector<double> y; | ||
| for (double i = axis.front(); i < axis.back(); i += 0.01) { | ||
| x.push_back(i); | ||
| y.push_back(interpolator.compute(i)); | ||
| } | ||
| plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor")); | ||
| } | ||
|
|
||
| // Stairstep Interpolator | ||
| { | ||
| using autoware::motion_utils::trajectory_container::interpolator::Stairstep; | ||
| auto interpolator = | ||
| *InterpolatorCreator<Stairstep<double>>().set_axis(axis).set_values(values).create(); | ||
| std::vector<double> x; | ||
| std::vector<double> y; | ||
| for (double i = axis.front(); i < axis.back(); i += 0.01) { | ||
| x.push_back(i); | ||
| y.push_back(interpolator.compute(i)); | ||
| } | ||
| plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep")); | ||
| } | ||
|
|
||
| plt.legend(); | ||
| plt.show(); | ||
| return 0; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
23 changes: 23 additions & 0 deletions
23
...autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,23 @@ | ||
| // 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__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ | ||
| #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ | ||
| #include <autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp> | ||
| #include <autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp> | ||
| #include <autoware/motion_utils/trajectory_container/interpolator/interpolator_creator.hpp> | ||
| #include <autoware/motion_utils/trajectory_container/interpolator/linear.hpp> | ||
| #include <autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp> | ||
| #include <autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp> | ||
| #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ |
98 changes: 98 additions & 0 deletions
98
...on_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,98 @@ | ||
| // 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__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ | ||
| #define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ | ||
|
|
||
| #include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" | ||
|
|
||
| #include <Eigen/Dense> | ||
|
|
||
| #include <vector> | ||
|
|
||
| namespace autoware::motion_utils::trajectory_container::interpolator | ||
| { | ||
|
|
||
| /** | ||
| * @brief Class for Akima spline interpolation. | ||
| * | ||
| * This class provides methods to perform Akima spline interpolation on a set of data points. | ||
| */ | ||
| class AkimaSpline : public Interpolator<double> | ||
| { | ||
| template <typename InterpolatorType> | ||
| friend class InterpolatorCreator; | ||
|
|
||
| private: | ||
| Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline. | ||
|
|
||
| AkimaSpline() = default; | ||
|
|
||
| /** | ||
| * @brief Compute the spline parameters. | ||
| * | ||
| * This method computes the coefficients for the Akima spline. | ||
| * | ||
| * @param axis The axis values. | ||
| * @param values The values to interpolate. | ||
| */ | ||
| void compute_parameters( | ||
| const Eigen::Ref<const Eigen::VectorXd> & axis, | ||
| const Eigen::Ref<const Eigen::VectorXd> & values); | ||
|
|
||
| /** | ||
| * @brief Build the interpolator with the given values. | ||
| * | ||
| * @param axis The axis values. | ||
| * @param values The values to interpolate. | ||
| */ | ||
| void build_impl( | ||
| const Eigen::Ref<const Eigen::VectorXd> & axis, const std::vector<double> & values) override; | ||
|
|
||
| /** | ||
| * @brief Compute the interpolated value at the given point. | ||
| * | ||
| * @param s The point at which to compute the interpolated value. | ||
| * @return The interpolated value. | ||
| */ | ||
| [[nodiscard]] double compute_impl(const double & s) const override; | ||
|
|
||
| /** | ||
| * @brief Compute the first derivative at the given point. | ||
| * | ||
| * @param s The point at which to compute the first derivative. | ||
| * @return The first derivative. | ||
| */ | ||
| [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; | ||
|
|
||
| /** | ||
| * @brief Compute the second derivative at the given point. | ||
| * | ||
| * @param s The point at which to compute the second derivative. | ||
| * @return The second derivative. | ||
| */ | ||
| [[nodiscard]] double compute_second_derivative_impl(const double & s) const override; | ||
|
|
||
| public: | ||
| /** | ||
| * @brief Get the minimum number of required points for the interpolator. | ||
| * | ||
| * @return The minimum number of required points. | ||
| */ | ||
| [[nodiscard]] size_t minimum_required_points() const override { return 5; } | ||
| }; | ||
|
|
||
| } // namespace autoware::motion_utils::trajectory_container::interpolator | ||
|
|
||
| #endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ |
Oops, something went wrong.