boost::lock_error when joining a broadcasting thread

asked 2014-10-09 15:51:45 -0600

rkeatin3 gravatar image

I have been using a class, RvizPlotter, that I wrote to broadcast tf frames and visualization markers to rviz. Here's the relevant part of the header:

class RvizPlotter{


    class RvizFrame{
        public:
            std::string parentName;
            std::string childName;
            tf::Transform transform;
            RvizFrame(tf::Transform transform, std::string parentName, std::string childName);
    };
std::vector<RvizFrame> frames;
std::vector<visualization_msgs::Marker> vectors;
tf::TransformBroadcaster br;
ros::Publisher pb;
boost::thread *broadcastThread;

void broadcast();

public:
    RvizPlotter();
    RvizPlotter(ros::NodeHandle &n);
    RvizPlotter(const RvizPlotter& other);
    ~RvizPlotter();
    RvizPlotter& operator=(const RvizPlotter& other);
    void plotf(Eigen::Matrix4f f, std::string parentName, std::string childName);
    void plotf(Eigen::Matrix4f f, std::string frameName);
    void plotv(std::string frameName, Eigen::Vector3f point1, Eigen::Vector3f point2);

};

And here's the corresponding source code:

RvizPlotter::RvizFrame::RvizFrame(tf::Transform transform, std::string parentName, std::string childName){
  this->transform = transform; 
  this->parentName = parentName; 
  this->childName = childName;
}

void RvizPlotter::plotf(Eigen::Matrix4f h, std::string parentName, std::string childName){
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(h(0,3), h(1,3), h(2,3)));
  transform.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)));
  for(int i=0; i < frames.size(); i++)
  {
    if(childName.compare(frames[i].childName.c_str()) == 0)
    {
      frames[i].parentName = parentName;
      frames[i].transform = transform;
      usleep(50000);
      return;
    }  
  }
  frames.push_back(RvizPlotter::RvizFrame(transform, parentName, childName));
  usleep(50000);
}

void RvizPlotter::plotf(Eigen::Matrix4f h, std::string frameName){
  plotf(h,"map",frameName);
}



void RvizPlotter::plotv(std::string frameName, Eigen::Vector3f start, Eigen::Vector3f end)
{
  frameName.insert(0,"/");
  visualization_msgs::Marker marker;
  marker.header.frame_id = frameName;
  marker.header.stamp = ros::Time();
  marker.ns = "vectors";
  marker.id = vectors.size();
  marker.type = visualization_msgs::Marker::ARROW;
  marker.action = visualization_msgs::Marker::ADD;
  marker.color.a = 1.0;
  marker.color.r = 1.0;
  marker.color.g = 1.0;
  marker.color.b = 0.4;
  geometry_msgs::Point point1;
  geometry_msgs::Point point2;
  point1.x = start(0);
  point1.y = start(1);
  point1.z = start(2);
  point2.x = end(0);
  point2.y = end(1);
  point2.z = end(2);
  marker.scale.x = 0.03;
  marker.scale.y = 0.07;
  marker.scale.z = 0.1;
  marker.points.push_back(point1);
  marker.points.push_back(point2);
  marker.lifetime = ros::Duration();
  vectors.push_back(marker);
}

void RvizPlotter::broadcast(){
  while(ros::ok()){
    boost::this_thread::interruption_point();
    for(int i = 0; i < frames.size(); i++){
      br.sendTransform(tf::StampedTransform(frames[i].transform, ros::Time::now(), frames[i].parentName, frames[i].childName));
    }
    for(int i = 0; i < vectors.size(); i++){
      pb.publish(vectors[i]);
    }
    boost::this_thread::sleep(boost::posix_time::millisec(10));
  }
}

/**
 * Default constructor included to allow RvizPlotters to be 
 * declared before calling proper constructor with NodeHandle. 
 */
RvizPlotter::RvizPlotter(){}

RvizPlotter::RvizPlotter(ros::NodeHandle &n){
  tf::TransformBroadcaster br;
  pb = n.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
  broadcastThread = new boost::thread(boost::bind(&RvizPlotter::broadcast, this));
}

RvizPlotter::~RvizPlotter()
{
  //Keep plotting around for long enough
  usleep(500000);
  try{
    broadcastThread->interrupt();
  } catch(boost::exception const&  ex)
  {
    ROS_ERROR("Interrupting thread causes exception.");
  }
  broadcastThread->join();
  delete broadcastThread;
}

RvizPlotter::RvizPlotter(const ...
(more)
edit retag flag offensive close merge delete

Comments

Why are you initializing rvizPlotter twice in the constructor of UR5?

kmhallen gravatar image kmhallen  ( 2014-10-09 20:40:35 -0600 )edit

Oops, I changed around the constructor when debugging and forgot to change it back. That shouldn't be contributing to the issue.

rkeatin3 gravatar image rkeatin3  ( 2014-10-10 09:02:20 -0600 )edit