boost::lock_error when joining a broadcasting thread
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 ...
Why are you initializing rvizPlotter twice in the constructor of UR5?
Oops, I changed around the constructor when debugging and forgot to change it back. That shouldn't be contributing to the issue.