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
#include
#include
#include
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 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();
}
}
↧