Shutting down listener reliably abort traps

edit

I'm writing a simple robot plugin with a custom message and a standalone executable that listens for world_stats topics to get the time, calculates some joint angles, and sends them back to the plugin over a custom topic. This is mostly a mashup of the velodyne plugin tutorial plus the custom message tutorial.

The twist is that I'm trying to break out the "initialize gazebo transport", "calculate and send joint angles" and "shut everything down" functions out into separate function calls in a shared library.

This works very well until it comes time to shut down. At that point, after everything has executed and the thread exits, I get the following:

libc++abi.dylib: terminating with uncaught exception of type boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >: boost: mutex lock failed in pthread_mutex_lock: Invalid argument Abort trap: 6

I can make this entirely go away if I don't ever subscribe with a callback function to listen to the world_stats topic.

I've attached my code below. Any ideas what might be causing this?

#include "robot_lib.h"
extern "C" {

gazebo::transport::NodePtr node;
allegrodog_msgs::msgs::RobotSetAnglesRequest theta;
gazebo::msgs::WorldControl step;
gazebo::transport::PublisherPtr anglepub;
gazebo::transport::PublisherPtr step_cmd;
gazebo::transport::SubscriberPtr sub;
double __sim_gt;

void _cb(ConstWorldStatisticsPtr &_msg)
{
     gazebo::common::Time t;

   t = gazebo::msgs::Convert(_msg->sim_time());    
   __sim_gt = t.Double();
}

void initGazebo() {

 gazebo::client::setup();
 node = gazebo::transport::NodePtr(new gazebo::transport::Node());
 node->Init("default");

 // This topic will let us issue gazebo single-step commands
 step_cmd = node->Advertise<gazebo::msgs::WorldControl>("/gazebo/default/world_control");

 // This topic will let us issue joint commands to the robot
 anglepub = node->Advertise<allegrodog_msgs::msgs::RobotSetAnglesRequest>("/gazebo/default/my_robot/set_angles");

 // Listen to Gazebo world_stats topic
 sub = node->Subscribe("/gazebo/default/world_stats", _cb);

 anglepub->WaitForConnection();
}


void sendAnglesGazebo(double* jointpos) {
    theta.set_theta1(jointpos[0]);
    theta.set_theta2(jointpos[1]);
    theta.set_theta3(jointpos[2]);

    anglepub->Publish(theta);
    step.set_step(true);
    step_cmd->Publish(step);
}

void shutdownGazebo() {
    sub->Unsubscribe(); 
    //node->Fini();
    gazebo::transport::fini();
    gazebo::client::shutdown();
    //gazebo::shutdown();
    std::cout << "Shut down\n";
    }
}