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;
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,
54  const bool search_backward = false) const -> std::set<double>;
56  const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
57  const bool search_backward = false) const -> std::optional<double>;
59  const std::vector<geometry_msgs::msg::Point> & polygon,
60  const bool search_backward = false) const -> std::optional<double> override;
61  auto getPolygon(const double width, const size_t num_points = 30, const double z_offset = 0)
62  -> std::vector<geometry_msgs::msg::Point>;
63  const std::vector<geometry_msgs::msg::Point> control_points;
64  virtual ~CatmullRomSpline() = default;
65 
66 private:
67  auto getRightBounds(const double width, const size_t num_points = 30, const double z_offset = 0)
68  const -> std::vector<geometry_msgs::msg::Point>;
69  auto getLeftBounds(const double width, const size_t num_points = 30, const double z_offset = 0)
70  const -> std::vector<geometry_msgs::msg::Point>;
71  auto getSInSplineCurve(const size_t curve_index, const double s) const -> double;
72  auto getCurveIndexAndS(const double s) const -> std::pair<size_t, double>;
73  auto checkConnection() const -> bool;
74  auto equals(const geometry_msgs::msg::Point & p0, const geometry_msgs::msg::Point & p1) const
75  -> bool;
76  std::vector<LineSegment> line_segments_;
77  std::vector<HermiteCurve> curves_;
78  std::vector<double> length_list_;
79  std::vector<double> maximum_2d_curvatures_;
80  double total_length_;
81 };
82 } // namespace geometry
83 } // namespace math
84 
85 #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:374
auto getPoint(const double s) const -> geometry_msgs::msg::Point
Definition: catmull_rom_spline.cpp:516
auto getSValue(const geometry_msgs::msg::Pose &pose, double threshold_distance=3.0) const -> std::optional< double >
Definition: catmull_rom_spline.cpp:399
const std::vector< geometry_msgs::msg::Point > control_points
Definition: catmull_rom_spline.hpp:63
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:478
auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point &point, const double s) const -> double
Definition: catmull_rom_spline.cpp:442
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:650
auto getCollisionPointsIn2D(const std::vector< geometry_msgs::msg::Point > &polygon, const bool search_backward=false) const -> std::set< double >
Definition: catmull_rom_spline.cpp:281
auto getMaximum2DCurvature() const -> double
Definition: catmull_rom_spline.cpp:563
auto getTangentVector(const double s) const -> geometry_msgs::msg::Vector3
Definition: catmull_rom_spline.cpp:605
auto getNormalVector(const double s) const -> geometry_msgs::msg::Vector3
Definition: catmull_rom_spline.cpp:576
Definition: hermite_curve.hpp:32
Definition: line_segment.hpp:30
Definition: bounding_box.hpp:32
Definition: cache.hpp:27