scenario_simulator_v2 C++ API
catmull_rom_spline.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV.inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 #ifndef GEOMETRY__SPLINE__CATMULL_ROM_SPLINE_HPP_
15 #define GEOMETRY__SPLINE__CATMULL_ROM_SPLINE_HPP_
16 
17 #include <exception>
21 #include <geometry_msgs/msg/point.hpp>
22 #include <optional>
23 #include <string>
24 #include <utility>
25 #include <vector>
26 
27 namespace math
28 {
29 namespace geometry
30 {
32 {
33 public:
34  CatmullRomSpline() = default;
35  explicit CatmullRomSpline(const std::vector<geometry_msgs::msg::Point> & control_points);
36  auto getLength() const -> double override { return total_length_; }
37  auto getMaximum2DCurvature() const -> double;
38  auto getPoint(const double s) const -> geometry_msgs::msg::Point;
39  auto getPoint(const double s, const double offset) const -> geometry_msgs::msg::Point;
40  auto getTangentVector(const double s) const -> geometry_msgs::msg::Vector3;
41  auto getNormalVector(const double s) const -> geometry_msgs::msg::Vector3;
42  auto getPose(const double s, const bool fill_pitch = true) const -> geometry_msgs::msg::Pose;
43  auto getTrajectory(
44  const double start_s, const double end_s, const double resolution,
45  const double offset = 0.0) const -> std::vector<geometry_msgs::msg::Point>;
46  auto getSValue(const geometry_msgs::msg::Pose & pose, double threshold_distance = 3.0) const
47  -> std::optional<double>;
48  auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const
49  -> double override;
50  auto getSquaredDistanceVector(const geometry_msgs::msg::Point & point, const double s) const
51  -> geometry_msgs::msg::Vector3;
53  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward = false,
54  const std::optional<std::pair<double, double>> & s_range = std::nullopt) const
55  -> std::set<double>;
57  const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
58  const bool search_backward = false) const -> std::optional<double>;
60  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward = false,
61  const std::optional<std::pair<double, double>> & s_range = std::nullopt) const
62  -> std::optional<double> override;
63  auto getPolygon(const double width, const size_t num_points = 30, const double z_offset = 0)
64  -> std::vector<geometry_msgs::msg::Point>;
65  const std::vector<geometry_msgs::msg::Point> control_points;
66  virtual ~CatmullRomSpline() = default;
67 
68 private:
69  auto getRightBounds(const double width, const size_t num_points = 30, const double z_offset = 0)
70  const -> std::vector<geometry_msgs::msg::Point>;
71  auto getLeftBounds(const double width, const size_t num_points = 30, const double z_offset = 0)
72  const -> std::vector<geometry_msgs::msg::Point>;
73  auto getSInSplineCurve(const size_t curve_index, const double s) const -> double;
74  auto getCurveIndexAndS(const double s) const -> std::pair<size_t, double>;
75  auto checkConnection() const -> bool;
76  auto equals(const geometry_msgs::msg::Point & p0, const geometry_msgs::msg::Point & p1) const
77  -> bool;
78  std::vector<LineSegment> line_segments_;
79  std::vector<HermiteCurve> curves_;
80  std::vector<double> length_list_;
81  std::vector<double> maximum_2d_curvatures_;
82  double total_length_;
83 };
84 } // namespace geometry
85 } // namespace math
86 
87 #endif // GEOMETRY__SPLINE__CATMULL_ROM_SPLINE_HPP_
Definition: catmull_rom_spline_interface.hpp:30
Definition: catmull_rom_spline.hpp:32
auto getCollisionPointIn2D(const geometry_msgs::msg::Point &point0, const geometry_msgs::msg::Point &point1, const bool search_backward=false) const -> std::optional< double >
Definition: catmull_rom_spline.cpp:397
auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point &point, const double s) const -> double override
Definition: catmull_rom_spline.cpp:465
auto getPoint(const double s) const -> geometry_msgs::msg::Point
Definition: catmull_rom_spline.cpp:539
auto getSValue(const geometry_msgs::msg::Pose &pose, double threshold_distance=3.0) const -> std::optional< double >
Definition: catmull_rom_spline.cpp:422
const std::vector< geometry_msgs::msg::Point > control_points
Definition: catmull_rom_spline.hpp:65
auto getPolygon(const double width, const size_t num_points=30, const double z_offset=0) -> std::vector< geometry_msgs::msg::Point >
Definition: catmull_rom_spline.cpp:29
auto getSquaredDistanceVector(const geometry_msgs::msg::Point &point, const double s) const -> geometry_msgs::msg::Vector3
Definition: catmull_rom_spline.cpp:501
auto getCollisionPointsIn2D(const std::vector< geometry_msgs::msg::Point > &polygon, const bool search_backward=false, const std::optional< std::pair< double, double >> &s_range=std::nullopt) const -> std::set< double >
Definition: catmull_rom_spline.cpp:282
auto getLength() const -> double override
Definition: catmull_rom_spline.hpp:36
auto getTrajectory(const double start_s, const double end_s, const double resolution, const double offset=0.0) const -> std::vector< geometry_msgs::msg::Point >
Definition: catmull_rom_spline.cpp:100
auto getPose(const double s, const bool fill_pitch=true) const -> geometry_msgs::msg::Pose
Definition: catmull_rom_spline.cpp:673
auto getMaximum2DCurvature() const -> double
Definition: catmull_rom_spline.cpp:586
auto getTangentVector(const double s) const -> geometry_msgs::msg::Vector3
Definition: catmull_rom_spline.cpp:628
auto getNormalVector(const double s) const -> geometry_msgs::msg::Vector3
Definition: catmull_rom_spline.cpp:599
Definition: hermite_curve.hpp:32
Definition: line_segment.hpp:30
Definition: bounding_box.hpp:32
Definition: lanelet_wrapper.hpp:40
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
geometry_msgs::msg::Point Point
Definition: lanelet_wrapper.hpp:65
geometry_msgs::msg::Vector3 Vector3
Definition: lanelet_wrapper.hpp:69