15 #ifndef GEOMETRY__SPLINE__HERMITE_CURVE_HPP_
16 #define GEOMETRY__SPLINE__HERMITE_CURVE_HPP_
18 #include <gtest/gtest.h>
21 #include <geometry_msgs/msg/point.hpp>
22 #include <geometry_msgs/msg/pose.hpp>
23 #include <geometry_msgs/msg/vector3.hpp>
35 double ax_, bx_, cx_, dx_;
36 double ay_, by_, cy_, dy_;
37 double az_, bz_, cz_, dz_;
42 geometry_msgs::msg::Pose start_pose, geometry_msgs::msg::Pose goal_pose,
43 geometry_msgs::msg::Vector3 start_vec, geometry_msgs::msg::Vector3 goal_vec);
45 double ax,
double bx,
double cx,
double dx,
double ay,
double by,
double cy,
double dy,
46 double az,
double bz,
double cz,
double dz);
47 std::vector<geometry_msgs::msg::Point>
getTrajectory(
size_t num_points = 30)
const;
49 double start_s,
double end_s,
double resolution,
bool denormalize_s =
false)
const;
50 const geometry_msgs::msg::Pose
getPose(
51 double s,
bool denormalize_s =
false,
bool fill_pitch =
true)
const;
52 const geometry_msgs::msg::Point
getPoint(
double s,
bool denormalize_s =
false)
const;
53 const geometry_msgs::msg::Vector3
getTangentVector(
double s,
bool denormalize_s =
false)
const;
54 const geometry_msgs::msg::Vector3
getNormalVector(
double s,
bool denormalize_s =
false)
const;
57 double getLength(
size_t num_points)
const;
60 const geometry_msgs::msg::Pose & pose,
double threshold_distance = 3.0,
61 bool denormalize_s =
false)
const;
63 const geometry_msgs::msg::Point & point,
double s,
bool denormalize_s =
false)
const;
65 const geometry_msgs::msg::Point & point,
double s,
bool denormalize_s =
false)
const;
67 const geometry_msgs::msg::Point & point0,
const geometry_msgs::msg::Point & point1,
68 bool search_backward =
false,
bool denormalize_s =
false)
const;
70 const geometry_msgs::msg::Point & point0,
const geometry_msgs::msg::Point & point1,
71 bool search_backward =
false,
bool denormalize_s =
false)
const;
73 const std::vector<geometry_msgs::msg::Point> & polygon,
bool search_backward =
false,
74 bool close_start_end =
true,
bool denormalize_s =
false)
const;
76 const std::vector<geometry_msgs::msg::Point> & polygon,
bool search_backward =
false,
77 bool close_start_end =
true,
bool denormalize_s =
false)
const;
80 std::pair<double, double> get2DMinMaxCurvatureValue()
const;
Definition: hermite_curve.hpp:32
const geometry_msgs::msg::Point getPoint(double s, bool denormalize_s=false) const
Definition: hermite_curve.cpp:362
const geometry_msgs::msg::Pose getPose(double s, bool denormalize_s=false, bool fill_pitch=true) const
Definition: hermite_curve.cpp:284
const geometry_msgs::msg::Vector3 getNormalVector(double s, bool denormalize_s=false) const
Definition: hermite_curve.cpp:258
friend class HermiteCurveTest
Definition: hermite_curve.hpp:34
geometry_msgs::msg::Vector3 getSquaredDistanceVector(const geometry_msgs::msg::Point &point, double s, bool denormalize_s=false) const
Definition: hermite_curve.cpp:78
double get2DCurvature(double s, bool denormalize_s=false) const
Definition: hermite_curve.cpp:301
HermiteCurve(geometry_msgs::msg::Pose start_pose, geometry_msgs::msg::Pose goal_pose, geometry_msgs::msg::Vector3 start_vec, geometry_msgs::msg::Vector3 goal_vec)
Definition: hermite_curve.cpp:50
std::vector< geometry_msgs::msg::Point > getTrajectory(size_t num_points=30) const
Definition: hermite_curve.cpp:244
double getLength() const
Definition: hermite_curve.hpp:58
std::optional< double > getSValue(const geometry_msgs::msg::Pose &pose, double threshold_distance=3.0, bool denormalize_s=false) const
Definition: hermite_curve.cpp:204
double getMaximum2DCurvature() const
Definition: hermite_curve.cpp:330
const geometry_msgs::msg::Vector3 getTangentVector(double s, bool denormalize_s=false) const
Definition: hermite_curve.cpp:271
double getSquaredDistanceIn2D(const geometry_msgs::msg::Point &point, double s, bool denormalize_s=false) const
Definition: hermite_curve.cpp:71
std::set< double > getCollisionPointsIn2D(const geometry_msgs::msg::Point &point0, const geometry_msgs::msg::Point &point1, bool search_backward=false, bool denormalize_s=false) const
Definition: hermite_curve.cpp:125
std::optional< double > getCollisionPointIn2D(const geometry_msgs::msg::Point &point0, const geometry_msgs::msg::Point &point1, bool search_backward=false, bool denormalize_s=false) const
Definition: hermite_curve.cpp:190
Definition: polynomial_solver.hpp:26
Definition: bounding_box.hpp:32