ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Creating continous tf broadcaster with Boost::Thread

asked 2014-07-24 14:19:29 -0500

rkeatin3 gravatar image

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?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2014-07-24 15:13:11 -0500

Wolf gravatar image

updated 2014-07-24 15:13:49 -0500

I am not sure what you code is supposed to do, maybe, there is a more elegant to create your desired behaviour....

However, your problem is, that your tf::TransformBroadcaster br; is created before ros::init. As class member it is implizitly instantiated:

    RvizPlotter::RvizPlotter(int argc, char **argv)
// br is created implizitly at this point in the code
{
      ros::init(argc, argv, "plotter");
      ros::NodeHandle node;

Before ros::init and the first nodeHandle is created, it can not set up its internal publishers, i. e. is invalid.

edit flag offensive delete link more
1

answered 2014-07-24 16:18:28 -0500

ahendrix gravatar image

To expand on the previous answer a bit:

ros::init should only be called once during a program, so you probably shouldn't call it inside your RvizPlotter, particularly if users are expected to instantiate more than one instance of it.

Then you can simply require your users to call ros::init before creating any instances of your RvizPlotter, and your ordering problems are much easier to sort out.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2014-07-24 14:19:29 -0500

Seen: 498 times

Last updated: Jul 24 '14