tf from rosbag within node

asked 2018-12-07 12:58:20 -0500

-LD- gravatar image

Hello,

i recorded 40 rosbags with my mobile base moving autonomously. I just recorded the tf-topic. If i play the bag from command line i can see the model of my base in rviz. So the bags are just fine. But i want to visualize the driven path. therefor i need to get the coordinates. i do this with a tf-listener. Just fine so far. But i have to start the bags one by one from the terminal. i would prefer to run them within my node. open one bag (index 01), play it, extract coords, save coords to .txt, play next bag (index 02), ....

But it seems like i don't get it. i tried the code at the end. But my problem is, that i don't subscribe to the TF-topic. I just need it for the Listener. So i don't know what to do with the normal code (see comments).

So, is there a way? Or do i have to deal with all the 40 files by hand?

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf/tf.h"
#include "tf/transform_listener.h"
#include "tf/transform_datatypes.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <iostream>
#include <fstream>
using namespace std;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "logger");
  ros::NodeHandle n;
  ros::Rate loop_rate(10);

  tf::TransformListener listener;
  rosbag::Bag bag;
  bag.open("/home/......../door_2/door_01.bag", rosbag::bagmode::Read);
  std::vector<std::string> topics;
  topics.push_back("/tf");
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  while (ros::ok())
  {
    // normally something like this is used, but i don't subscribe to any topic
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
       if (m.getTopic() == depth_cam_info) 
       { }
    }
    // get coords (works if bag is run from terminal)
    geometry_msgs::PoseStamped base_pose, map_pose;
    base_pose.header.frame_id = "base_link";
    base_pose.pose.orientation.w = 1.0;
    listener.transformPose("map", base_pose, map_pose);
    // append to file (works if bag is played from terminal)
    ofstream myfile;
    myfile.open ("/home/lukas/Desktop/coordinates.txt", std::ios_base::app);
    myfile << map_pose.pose.position.x << ", " << map_pose.pose.position.y << "\n";
    myfile.close();
    // just needed
    ros::spinOnce();
    loop_rate.sleep();
  }
}
edit retag flag offensive close merge delete