orca-robotics
INTRODUCTION
Overview
Download and Install
Documentation
REPOSITORY
Interfaces
Drivers
Libraries
Utilities
Software Map
DEVELOPER
Dashboard
PEOPLE
Contributors
Users
Project
Download
Mailing lists
pose.h
00001 #ifndef ORCANAVUTIL_POSE_H 00002 #define ORCANAVUTIL_POSE_H 00003 00004 #include <cmath> 00005 #include <iostream> 00006 #include <string> 00007 #include <assert.h> 00008 #include <hydronavutil/offset.h> 00009 00010 namespace hydronavutil { 00011 00015 class Pose 00016 { 00017 00018 public: 00019 00020 Pose() {}; 00021 Pose( double x, double y, double theta ) 00022 :x_(x),y_(y),theta_(theta) {} 00023 00024 // non-const versions 00025 double &x() { return x_; } 00026 double &y() { return y_; } 00027 double &theta() { return theta_; } 00028 00029 // const versions 00030 double x() const { return x_; } 00031 double y() const { return y_; } 00032 double theta() const { return theta_; } 00033 00034 // Access like a vector 00035 double v( int i ) const 00036 { 00037 if ( i==0 ) return x_; 00038 else if ( i==1 ) return y_; 00039 else if ( i==2 ) return theta_; 00040 else { assert( false&&"bad index" ); return 0; } 00041 } 00042 00043 std::string toString() const; 00044 00045 private: 00046 00047 double x_, y_, theta_; 00048 00049 }; 00050 std::ostream &operator<<( std::ostream &s, const Pose &p ); 00051 00053 // 00054 // Non-member convenience functions for pose manipulation (see offset.h) 00055 // 00056 00058 inline void normaliseHeading( Pose &pose ) 00059 { NORMALISE_ANGLE( pose.theta() ); } 00060 00062 inline void addPoseOffset( Pose &pose, 00063 const Pose &offset, 00064 bool normaliseHeading ) 00065 { 00066 addPoseOffset( pose.x(), pose.y(), pose.theta(), 00067 offset.x(), offset.y(), offset.theta(), 00068 normaliseHeading ); 00069 } 00070 00072 inline void addPoseOffset( const Pose &start, 00073 const Pose &offset, 00074 Pose &result, 00075 bool normaliseHeading ) 00076 { 00077 addPoseOffset( start.x(), start.y(), start.theta(), 00078 offset.x(), offset.y(), offset.theta(), 00079 result.x(), result.y(), result.theta(), 00080 normaliseHeading ); 00081 } 00082 00084 inline void subtractFinalPoseOffset( Pose &pose, 00085 const Pose &offset, 00086 bool normaliseHeading ) 00087 { 00088 subtractFinalPoseOffset( pose.x(), pose.y(), pose.theta(), 00089 offset.x(), offset.y(), offset.theta(), 00090 normaliseHeading ); 00091 } 00092 00094 inline void subtractFinalPoseOffset( const Pose &start, 00095 const Pose &offset, 00096 Pose &result, 00097 bool normaliseHeading ) 00098 { 00099 subtractFinalPoseOffset( start.x(), start.y(), start.theta(), 00100 offset.x(), offset.y(), offset.theta(), 00101 result.x(), result.y(), result.theta(), 00102 normaliseHeading ); 00103 } 00104 00106 inline void subtractInitialOffset( Pose &pose, 00107 const Pose &initialOffset ) 00108 { 00109 subtractInitialOffset( pose.x(), pose.y(), pose.theta(), 00110 initialOffset.x(), initialOffset.y(), initialOffset.theta() ); 00111 } 00112 00114 inline void subtractInitialOffset( const Pose &pose, 00115 const Pose &initialOffset, 00116 Pose &result ) 00117 { 00118 subtractInitialOffset( pose.x(), pose.y(), pose.theta(), 00119 initialOffset.x(), initialOffset.y(), initialOffset.theta(), 00120 result.x(), result.y(), result.theta() ); 00121 } 00122 00123 00124 00125 } // end namespace 00126 00127 #endif
