Creating continous tf broadcaster with Boost::Thread
This is a bit of a continuation from a previous question here:here.
Basically, I want to create a class that allows me to continuously publish frames to tf. I have no experience at all in multithreaded programming, but I found a decent introduction to Boost that I have been referencing. I have gotten my code to compile, but when I try to use the plotf()
method it spits out the following error:
Adding a frame to be plotted
Creating broadcasting thread
Broadcasting 1 transforms
[FATAL] [1406225580.899040462]: ASSERTION FAILED
file = /opt/ros/hydro/include/ros/publisher.h
line = 108
cond = false
message =
[FATAL] [1406225580.899232266]: Call to publish() on an invalid Publisher (topic [/tf])
[FATAL] [1406225580.899378655]:
Here is my header file:
#ifndef UTILITIES_H
#define UTILITIES_H
#include <Eigen/Core>
#include <tf/transform_broadcaster.h>
#include <boost/thread.hpp>
class RvizPlotter{
class RvizFrame{
public:
std::string parentName;
std::string childName;
tf::Transform frame;
RvizFrame(tf::Transform frame, std::string parentName, std::string childName);
};
std::vector<RvizFrame> frames;
tf::TransformBroadcaster br;
void broadcast();
public:
RvizPlotter(int argc, char **argv);
void plotf(Eigen::Matrix4f f, std::string parentName, std::string childName);
void plotf(Eigen::Matrix4f f, std::string frameName);
};
#endif
And here is the source file:
#include <ros/ros.h>
#include <Eigen/Core>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <ur5/utilities.h>
RvizPlotter::RvizFrame::RvizFrame(tf::Transform frame, std::string parentName, std::string childName){
this->frame = frame; this->parentName = parentName; this->childName = childName;
}
void RvizPlotter::plotf(Eigen::Matrix4f h, std::string parentName, std::string childName){
printf("Adding a frame to be plotted\n");
tf::Transform frame;
frame.setOrigin(tf::Vector3(h(0,3), h(1,3), h(2,3)));
frame.setBasis(tf::Matrix3x3(h(0,0), h(0,1), h(0,2),
h(1,0), h(1,1), h(1,2),
h(2,0), h(2,1), h(2,2)));
frames.push_back(RvizPlotter::RvizFrame(frame, parentName, childName));
}
void RvizPlotter::plotf(Eigen::Matrix4f h, std::string frameName){
plotf(h,"map",frameName);
}
void RvizPlotter::broadcast(){
printf("Broadcasting %d transforms\n",frames.size());
while(frames.size() > 0)
{
for(int i = 0; i < frames.size(); i++){
br.sendTransform(tf::StampedTransform(frames[i].frame, ros::Time::now(), frames[i].parentName, frames[i].childName));
}
}
boost::this_thread::sleep(boost::posix_time::millisec(100));
}
RvizPlotter::RvizPlotter(int argc, char **argv){
ros::init(argc, argv, "plotter");
ros::NodeHandle node;
//Testing that frames do actually get pushed into vector
Eigen::Matrix4f h = Eigen::MatrixXf::Identity(4,4);
plotf(h,"Frame");
printf("Creating broadcasting thread\n");
boost::thread* broadcastThread = new boost::thread(boost::bind(&RvizPlotter::broadcast, this));
}
Any ideas on what I'm doing wrong?