main.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "move_base_msgs/MoveBaseActionGoal.h"
#include <fstream>
#include <sstream>
#include <string>
#include <ctime>


std::time_t t = std::time(0);
std::string path = std::to_string(t)+"posen.xml";

void chatterCallback(const move_base_msgs::MoveBaseActionGoal::ConstPtr& msg)
{
    std::string xmlString;

    float x =  msg->goal.target_pose.pose.position.x;
    float y = msg->goal.target_pose.pose.position.y;
    float z = msg->goal.target_pose.pose.position.z;

    float xq = msg->goal.target_pose.pose.orientation.x;
    float yq = msg->goal.target_pose.pose.orientation.y;
    float zq = msg->goal.target_pose.pose.orientation.z;
    float wq = msg->goal.target_pose.pose.orientation.w;

    ROS_INFO("I heard x : [%f]",x);
    ROS_INFO("I heard y : [%f]",y);
    ROS_INFO("I heard z : [%f]",z);

    ROS_INFO("I heard x quat : [%f]",xq);
    ROS_INFO("I heard y quat : [%f]",yq);
    ROS_INFO("I heard z quat : [%f]",zq);
    ROS_INFO("I heard w quat : [%f]",wq);

    xmlString += "<Pose X=\""+std::to_string(x)+"\" ";
    xmlString += "Y=\""+std::to_string(y)+"\" ";
    xmlString += "Z=\""+std::to_string(z)+"\" ";
    xmlString += "XQ=\""+std::to_string(xq)+"\" ";
    xmlString += "YQ=\""+std::to_string(yq)+"\" ";
    xmlString += "ZQ=\""+std::to_string(zq)+"\" ";
    xmlString += "WQ=\""+std::to_string(wq)+"\"></Pose>";

    std::ofstream pose_xml_file(path,std::ofstream::out|std::ofstream::app);
    pose_xml_file << xmlString << std::endl;
    pose_xml_file.close();

}

int main(int argc, char **argv)
{

    std::string xmlString = "<?xml version=\"1.0\" encoding=\"utf-8\"?>";
    std::ofstream pose_xml_file(path,std::ofstream::out|std::ofstream::app);
    pose_xml_file << xmlString << std::endl << "<Posen>" << std::endl;

    ros::init(argc, argv, "asr_rviz_pose_manager");

    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("move_base/goal", 1000, chatterCallback);

    ros::spin();

    pose_xml_file << "</Posen>" << std::endl;
    pose_xml_file.close();


    return 0;
}