00001
00002
00003
00004
00005
00006
00007
00008 #ifndef GEOMETRY_HPP_
00009 #define GEOMETRY_HPP_
00010
00011
00012 #include <tf/tf.h>
00013
00014 #include <geometry_msgs/Pose.h>
00015 #include <geometry_msgs/PoseStamped.h>
00016
00017
00018 namespace mtk
00019 {
00020
00026 double wrapAngle(double a);
00027
00033 double roll(const tf::Transform& tf);
00034 double roll(geometry_msgs::Pose pose);
00035 double roll(geometry_msgs::PoseStamped pose);
00036
00042 double pitch(const tf::Transform& tf);
00043 double pitch(geometry_msgs::Pose pose);
00044 double pitch(geometry_msgs::PoseStamped pose);
00045
00052 double distance2D(double ax, double ay, double bx, double by);
00053 double distance2D(geometry_msgs::Point a, geometry_msgs::Point b = geometry_msgs::Point());
00054 double distance2D(geometry_msgs::Pose a, geometry_msgs::Pose b = geometry_msgs::Pose());
00055 double distance2D(const tf::Vector3& a, const tf::Vector3& b = tf::Vector3());
00056 double distance2D(const tf::Transform& a, const tf::Transform& b = tf::Transform());
00057
00064 double distance3D(double ax, double ay, double az, double bx, double by, double bz);
00065 double distance3D(geometry_msgs::Point a, geometry_msgs::Point b = geometry_msgs::Point());
00066 double distance3D(geometry_msgs::Pose a, geometry_msgs::Pose b = geometry_msgs::Pose());
00067 double distance3D(const tf::Vector3& a, const tf::Vector3& b);
00068 double distance3D(const tf::Transform& a, const tf::Transform& b);
00069
00076 double heading(geometry_msgs::Point a, geometry_msgs::Point b = geometry_msgs::Point());
00077 double heading(geometry_msgs::Pose a, geometry_msgs::Pose b = geometry_msgs::Pose());
00078 double heading(const tf::Vector3& a, const tf::Vector3& b);
00079 double heading(const tf::Transform& a, const tf::Transform& b);
00080
00087 double minAngle(geometry_msgs::Quaternion a, geometry_msgs::Quaternion b);
00088 double minAngle(geometry_msgs::Pose a, geometry_msgs::Pose b);
00089 double minAngle(const tf::Quaternion& a, const tf::Quaternion& b);
00090 double minAngle(const tf::Transform& a, const tf::Transform& b);
00091
00098 bool sameFrame(const std::string& frame_a, const std::string& frame_b);
00099 bool sameFrame(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b);
00100
00111 double pointSegmentDistance(double px, double py, double s1x, double s1y, double s2x, double s2y);
00112
00128 bool raySegmentIntersection(double r1x, double r1y, double r2x, double r2y,
00129 double s1x, double s1y, double s2x, double s2y,
00130 double& ix, double& iy, double& distance);
00131
00144 bool rayCircleIntersection(double rx, double ry, double cx, double cy, double radius,
00145 double& ix, double& iy, double& distance);
00146
00147 }
00148
00149 #endif