DataTracker.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <asr_msgs/AsrObject.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>
#include <posedetection_msgs/Object6DPose.h>
#include <string>
#include <iostream>
#include <fstream>
#include <boost/thread.hpp>


class DataTracker {
public:
        ros::NodeHandle nh;

        // Subscriptions
        ros::Subscriber ptu_sub;
        ros::Subscriber fob_sub;
        ros::Subscriber cbd_sub;

        std::string ptu_topic, fob_topic, cbd_topic, output_file_path;

        double pan, tilt;
        geometry_msgs::Pose fob_pose;
        geometry_msgs::Pose cbd_pose;

        int data_counter;

        void ptuCB(const sensor_msgs::JointState& ptu_msg) {
            ROS_DEBUG("ptu Message recieved");
            pan = ptu_msg.position[0];
            tilt = ptu_msg.position[1];
        }

        void fobCB(const asr_msgs::AsrObject& fob_msg) {
            if (!fob_msg.identifier.empty() && strcmp(fob_msg.identifier.c_str(), "tracker_left_raw") == 0) {
                ROS_DEBUG("fob message recieved");
                geometry_msgs::PoseWithCovariance current_pose_with_c = fob_msg.sampledPoses.back();
                fob_pose = current_pose_with_c.pose;
            }
        }
        void cbdCB(const geometry_msgs::PoseStamped& cbd_msg) {
            ROS_DEBUG("cbd message recieved");
            ROS_INFO("cbd message recieved");
            cbd_pose = cbd_msg.pose;
        }

        DataTracker(ros::NodeHandle nh){
            this->nh = nh;
            nh.getParam("ptu_topic", ptu_topic);
            nh.getParam("fob_topic", fob_topic);
            nh.getParam("cbd_topic", cbd_topic);
            nh.getParam("output_file_path", output_file_path);

            ptu_sub = nh.subscribe(ptu_topic, 1, &DataTracker::ptuCB, this);
            fob_sub = nh.subscribe(fob_topic, 1, &DataTracker::fobCB, this);
            cbd_sub = nh.subscribe(cbd_topic, 1, &DataTracker::cbdCB, this);
            data_counter = 0;
            boost::thread trackerThread(spin, this);
            ros::spin();
        }
        static void spin(DataTracker* obj){
            ROS_INFO("Capturing started.");
            ROS_INFO("Enter 'e' to stop capturing.");
            while (std::cin.get() != 'e'){
               obj->write();
            }
            ROS_INFO("Capturing stopped.");
        }


        void write(){
            std::stringstream ss;
            ss << pan << "; " << tilt << "; ";
            ss << fob_pose.position.x << "; " << fob_pose.position.y << "; " << fob_pose.position.z << "; ";
            ss << fob_pose.orientation.w << "; " << fob_pose.orientation.x << "; " << fob_pose.orientation.y << "; " << fob_pose.orientation.z << "; ";
            ss << cbd_pose.position.x << "; " << cbd_pose.position.y << "; " << cbd_pose.position.z << std::endl;
            ROS_INFO("%s", ss.str().c_str());
            std::ofstream dataFile;
            dataFile.open (output_file_path.c_str(),std::ios::app);
            dataFile << ss.str();
            dataFile.close();
            ROS_INFO("data captured %d", data_counter);
            data_counter++;
        }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "calibration_tracker");

  ros::NodeHandle nh("~");
  new ::DataTracker(nh);
  //ros::spin();

  return 0;
}