BagFile.cpp

Go to the documentation of this file.

/*
 * BagFile.cpp
 *
 *  Created on: Apr 30, 2012
 *      Author: mriedel
 */

#include "BagFile.hpp"
#include <rosbag/bag.h>
#include <rosbag/view.h>

//Messages
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/CompressedImage.h>
#include <tk_haptics_msgs/TKHapticOutput.h>
#include <telekyb_msgs/TKState.h>
#include <telekyb_msgs/TKTrajectory.h>

namespace TELEKYB_NAMESPACE
{

BagFile::BagFile() {
        // TODO Auto-generated constructor stub

}

BagFile::~BagFile() {
        // TODO Auto-generated destructor stub
}


void BagFile::process()
{
        rosbag::Bag bag;
        bag.open(options.tInputFilename->getValue(), rosbag::bagmode::Read);
        std::cout << options.tTopic->getValue() << std::endl;

        // View
        rosbag::View view(bag, rosbag::TopicQuery(options.tTopic->getValue()));
        BOOST_FOREACH(rosbag::MessageInstance const m, view)
        {
                // cast to specific msg
                std::string dataType = m.getDataType();
                if (dataType.compare("geometry_msgs/Vector3Stamped") == 0) {
                        geometry_msgs::Vector3Stamped::ConstPtr msg = m.instantiate<geometry_msgs::Vector3Stamped>();
                        std::cout << m.getTime() << ",";
                        std::cout << msg->header.seq  << ",";
                        std::cout << msg->header.stamp  << ",";
                        std::cout << msg->vector.x << ",";
                        std::cout << msg->vector.y << ",";
                        std::cout << msg->vector.z << std::endl;
                } else if (dataType.compare("sensor_msgs/CompressedImage") == 0) {
                        sensor_msgs::CompressedImage::ConstPtr msg = m.instantiate<sensor_msgs::CompressedImage>();
                        std::cout << m.getTime() << ",";
                        std::cout << msg->header.seq  << ",";
                        std::cout << msg->header.stamp << std::endl;
                } else if (dataType.compare("tk_haptics_msgs/TKHapticOutput") == 0) {
                        tk_haptics_msgs::TKHapticOutput::ConstPtr msg = m.instantiate<tk_haptics_msgs::TKHapticOutput>();
                        std::cout << m.getTime() << ",";
                        std::cout << msg->header.seq  << ",";
                        std::cout << msg->header.stamp  << ",";
                        std::cout << msg->force.x << ",";
                        std::cout << msg->force.y << ",";
                        std::cout << msg->force.z << ",";
                        std::cout << msg->frequency << ",";
                        std::cout << msg->pose.position.x << ",";
                        std::cout << msg->pose.position.y << ",";
                        std::cout << msg->pose.position.z << ",";
                        std::cout << msg->pose.orientation.w << ",";
                        std::cout << msg->pose.orientation.x << ",";
                        std::cout << msg->pose.orientation.y << ",";
                        std::cout << msg->pose.orientation.z << ",";
                        std::cout << msg->linear.x << ",";
                        std::cout << msg->linear.y << ",";
                        std::cout << msg->linear.z << ",";
                        std::cout << msg->primaryButton << std::endl;
                } else if (dataType.compare("telekyb_msgs/TKTrajectory") == 0) {
                        telekyb_msgs::TKTrajectory::ConstPtr msg = m.instantiate<telekyb_msgs::TKTrajectory>();
                        std::cout << m.getTime() << ",";
                        std::cout << msg->header.seq  << ",";
                        std::cout << msg->header.stamp  << ",";
                        std::cout << msg->position.x << ",";
                        std::cout << msg->position.y << ",";
                        std::cout << msg->position.z << ",";
                        std::cout << msg->velocity.x << ",";
                        std::cout << msg->velocity.y << ",";
                        std::cout << msg->velocity.z << ",";
                        std::cout << msg->acceleration.x << ",";
                        std::cout << msg->acceleration.y << ",";
                        std::cout << msg->acceleration.z << ",";
                        std::cout << (int)msg->xAxisCtrlType << ",";
                        std::cout << (int)msg->yAxisCtrlType << ",";
                        std::cout << (int)msg->zAxisCtrlType << ",";
                        std::cout << msg->yawAngle << ",";
                        std::cout << msg->yawRate << ",";
                        std::cout << msg->yawAcceleration << ",";
                        std::cout << (int)msg->yawCtrlType << std::endl;
                } else if (dataType.compare("telekyb_msgs/TKState") == 0) {
                        telekyb_msgs::TKState::ConstPtr msg = m.instantiate<telekyb_msgs::TKState>();
                        std::cout << m.getTime() << ",";
                        std::cout << msg->header.seq  << ",";
                        std::cout << msg->header.stamp  << ",";
                        std::cout << msg->pose.position.x << ",";
                        std::cout << msg->pose.position.y << ",";
                        std::cout << msg->pose.position.z << ",";
                        std::cout << msg->pose.orientation.w  << ",";
                        std::cout << msg->pose.orientation.x << ",";
                        std::cout << msg->pose.orientation.y << ",";
                        std::cout << msg->pose.orientation.z << ",";
                        std::cout << msg->twist.linear.x << ",";
                        std::cout << msg->twist.linear.y << ",";
                        std::cout << msg->twist.linear.z << ",";
                        std::cout << msg->twist.angular.x << ",";
                        std::cout << msg->twist.angular.y << ",";
                        std::cout << msg->twist.angular.z << std::endl;
                }


                //std::cout << m.getDataType() << std::endl;


        }


        bag.close();
}

}