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
00039 #include <graph_mapping_utils/to_string.h>
00040 #include <graph_mapping_utils/geometry.h>
00041
00042 namespace graph_mapping_utils
00043 {
00044
00045 string toString (const gm::Pose2D& pose)
00046 {
00047 return (format("[%.2f, %.2f, %.2f]") % pose.x % pose.y % pose.theta).str();
00048 }
00049
00050 string toString (const gm::Pose& pose)
00051 {
00052 const gm::Point& p = pose.position;
00053 const gm::Quaternion q = pose.orientation;
00054 return (format("[(%.4f, %.4f, %.4f), (%.4f, %.4f, %.4f, %.4f)]") % p.x % p.y % p.z % q.x % q.y % q.z % q.w).str();
00055 }
00056
00057 string toString (const gm::Point& p)
00058 {
00059 return (format("[%.2f, %.2f]") % p.x % p.y).str();
00060 }
00061
00062
00063 string toString2D (const gm::Pose& pose)
00064 {
00065 gm::Pose2D p = projectToPose2D(pose);
00066
00067 return (format("(%2f, %2f, %2f)") % p.x % p.y %
00068 p.theta).str();
00069 }
00070
00071 string toString2D (const tf::Pose& pose)
00072 {
00073 return toString2D(toPose(pose));
00074 }
00075
00076 string toString (const btVector3& v)
00077 {
00078 return (format("(%.3f, %.3f, %.3f)") % v.x() % v.y() % v.z()).str();
00079 }
00080
00081 string toString (const btQuaternion& q)
00082 {
00083 return (format("(%.3f, %.3f, %.3f, %.3f)") % q.x() % q.y() % q.z() % q.w()).str();
00084 }
00085
00086
00087
00088 string toString (const tf::Pose& pose)
00089 {
00090 return (format ("[%1%, %2%]") % toString(pose.getOrigin()) % toString(pose.getRotation())).str();
00091 }
00092
00093
00094
00095 }