Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <geometry_msgs/Pose.h>
00032 #include <cstring>
00033 #include <mongo_ros/message_with_metadata.h>
00034
00035 const double TOL=1e-3;
00036 using std::ostream;
00037
00038 namespace geometry_msgs
00039 {
00040
00041 inline
00042 bool operator== (const Pose& p1, const Pose& p2)
00043 {
00044 const Point& pos1 = p1.position;
00045 const Point& pos2 = p2.position;
00046 const Quaternion& q1 = p1.orientation;
00047 const Quaternion& q2 = p2.orientation;
00048 return (pos1.x == pos2.x) && (pos1.y == pos2.y) && (pos1.z == pos2.z) &&
00049 (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w);
00050 }
00051
00052 }
00053
00054 template <class T>
00055 ostream& operator<< (ostream& str, const mongo_ros::MessageWithMetadata<T>& s)
00056 {
00057 const T& msg = s;
00058 str << "Message: " << msg;
00059 str << "\nMetadata: " << s.metadata.toString();
00060 return str;
00061 }
00062
00063
00064 geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
00065 {
00066 geometry_msgs::Quaternion q;
00067 q.w = cos(yaw/2);
00068 q.z = sin(yaw/2);
00069 q.x = 0;
00070 q.y = 0;
00071 return q;
00072 }
00073
00074
00075 inline
00076 geometry_msgs::Pose makePose (const double x, const double y, const double theta)
00077 {
00078 geometry_msgs::Pose p;
00079 p.position.x = x;
00080 p.position.y = y;
00081 p.orientation = createQuaternionMsgFromYaw(theta);
00082 return p;
00083 }
00084
00085 using std::string;
00086
00087 bool contains (const string& s1, const string& s2)
00088 {
00089 return strstr(s1.c_str(), s2.c_str())!=NULL;
00090 }
00091