planner_modules_pr2: module_param

Go to the documentation of this file.
00001 #include "planner_modules_pr2/module_param_cache.h"
00002 
00003 std::string createPoseParamString(const geometry_msgs::Pose & pose, double precPose, double precQuat)
00004 {
00005     std::stringstream ss;
00006     ss.precision(0);
00007     ss << std::fixed;
00008     // the actual values don't matter as long as they are unique for this pose 
00009     // cannot use doubles here, as param keys are not allowed to contain '.' or '-'
00010     ss << "x";
00011     if(pose.position.x < 0)
00012         ss << "N";
00013     ss << abs(lrint(1.0/precPose * pose.position.x));
00014     ss << "y";
00015     if(pose.position.y < 0)
00016         ss << "N";
00017     ss << abs(lrint(1.0/precPose * pose.position.y));
00018     ss << "z";
00019     if(pose.position.z < 0)
00020         ss << "N";
00021     ss << abs(lrint(1.0/precPose * pose.position.z));
00022 
00023     ss << "qx";
00024     if(pose.orientation.x < 0)
00025         ss << "N";
00026     ss << abs(lrint(1.0/precQuat * pose.orientation.x));
00027     ss << "qy";
00028     if(pose.orientation.y < 0)
00029         ss << "N";
00030     ss << abs(lrint(1.0/precQuat * pose.orientation.y));
00031     ss << "qz";
00032     if(pose.orientation.z < 0)
00033         ss << "N";
00034     ss << abs(lrint(1.0/precQuat * pose.orientation.z));
00035     ss << "qw";
00036     if(pose.orientation.w < 0)
00037         ss << "N";
00038     ss << abs(lrint(1.0/precQuat * pose.orientation.w));
00039 
00040     return ss.str();
00041 }
00042