Go to the documentation of this file.00001
00018 #include "ros/ros.h"
00019 #include "move_base_msgs/MoveBaseActionGoal.h"
00020 #include <fstream>
00021 #include <sstream>
00022 #include <string>
00023 #include <ctime>
00024
00025
00026 std::time_t t = std::time(0);
00027 std::string path = std::to_string(t)+"posen.xml";
00028
00029 void chatterCallback(const move_base_msgs::MoveBaseActionGoal::ConstPtr& msg)
00030 {
00031 std::string xmlString;
00032
00033 float x = msg->goal.target_pose.pose.position.x;
00034 float y = msg->goal.target_pose.pose.position.y;
00035 float z = msg->goal.target_pose.pose.position.z;
00036
00037 float xq = msg->goal.target_pose.pose.orientation.x;
00038 float yq = msg->goal.target_pose.pose.orientation.y;
00039 float zq = msg->goal.target_pose.pose.orientation.z;
00040 float wq = msg->goal.target_pose.pose.orientation.w;
00041
00042 ROS_INFO("I heard x : [%f]",x);
00043 ROS_INFO("I heard y : [%f]",y);
00044 ROS_INFO("I heard z : [%f]",z);
00045
00046 ROS_INFO("I heard x quat : [%f]",xq);
00047 ROS_INFO("I heard y quat : [%f]",yq);
00048 ROS_INFO("I heard z quat : [%f]",zq);
00049 ROS_INFO("I heard w quat : [%f]",wq);
00050
00051 xmlString += "<Pose X=\""+std::to_string(x)+"\" ";
00052 xmlString += "Y=\""+std::to_string(y)+"\" ";
00053 xmlString += "Z=\""+std::to_string(z)+"\" ";
00054 xmlString += "XQ=\""+std::to_string(xq)+"\" ";
00055 xmlString += "YQ=\""+std::to_string(yq)+"\" ";
00056 xmlString += "ZQ=\""+std::to_string(zq)+"\" ";
00057 xmlString += "WQ=\""+std::to_string(wq)+"\"></Pose>";
00058
00059 std::ofstream pose_xml_file(path,std::ofstream::out|std::ofstream::app);
00060 pose_xml_file << xmlString << std::endl;
00061 pose_xml_file.close();
00062
00063 }
00064
00065 int main(int argc, char **argv)
00066 {
00067
00068 std::string xmlString = "<?xml version=\"1.0\" encoding=\"utf-8\"?>";
00069 std::ofstream pose_xml_file(path,std::ofstream::out|std::ofstream::app);
00070 pose_xml_file << xmlString << std::endl << "<Posen>" << std::endl;
00071
00072 ros::init(argc, argv, "asr_rviz_pose_manager");
00073
00074 ros::NodeHandle n;
00075
00076 ros::Subscriber sub = n.subscribe("move_base/goal", 1000, chatterCallback);
00077
00078 ros::spin();
00079
00080 pose_xml_file << "</Posen>" << std::endl;
00081 pose_xml_file.close();
00082
00083
00084 return 0;
00085 }