ROS C++ Publishing from Class using threads

edit

Hi,

I've tried to set up a class where I am able to publish and process ROS messages using callback queues on separate threads. The following is done to set up the Class:

Declare the following in my class header:

/// \brief Node used for ROS transport.
private: std::unique_ptr<ros::NodeHandle> rosNode;

/// \brief ROS Generalized Coordinates Subscriber.
private: ros::Subscriber genCoordSub;

/// \brief ROS Generalized Coordinates Subscriber.
private: ros::Subscriber genVelSub;

/// \brief ROS Joint State Publisher
private: ros::Publisher jointStatePub;

/// \brief ROS callbackque that helps process messages.
private: ros::CallbackQueue rosProcessQueue;

/// \brief ROS callbackque that helps publish messages.
private: ros::CallbackQueue rosPublishQueue;

/// \brief Thread running the rosProcessQueue.
private: std::thread rosProcessQueueThread;

/// \brief Thread running the rosPublishQueue.
private: std::thread rosPublishQueueThread;

Setup the following in my class cpp file:

if (!ros::isInitialized())
{
    int argc = 0;
    char **argv = NULL;
    ros::init(
        argc,
        argv,
        "gait_control_node",
        ros::init_options::NoSigintHandler
    );
}

this->rosNode.reset(new ros::NodeHandle("gait_control_node"));

ros::SubscribeOptions gen_coord_so = 
    ros::SubscribeOptions::create<std_msgs::Float64MultiArray>(
        "/my_robot/gen_coord",
        1,
        boost::bind(&GaitControl::OnGenCoordMsg, this, _1),
        ros::VoidPtr(),
        &this->rosProcessQueue
        );

ros::SubscribeOptions gen_vel_so = 
    ros::SubscribeOptions::create<std_msgs::Float64MultiArray>(
        "/my_robot/gen_vel",
        1,
        boost::bind(&GaitControl::OnGenVelMsg, this, _1),
        ros::VoidPtr(),
        &this->rosProcessQueue
        );

ros::AdvertiseOptions joint_state_ao =
    ros::AdvertiseOptions::create<sensor_msgs::JointState>(
        "/my_robot/joint_state_cmd",
        1,
        ros::SubscriberStatusCallback(),
        ros::SubscriberStatusCallback(),
        ros::VoidPtr(),
        &this->rosPublishQueue
    );

this->genCoordSub = this->rosNode->subscribe(gen_coord_so);

this->genVelSub = this->rosNode->subscribe(gen_vel_so);

this->jointStatePub = this->rosNode->advertise(joint_state_ao);

this->rosPublishQueueThread = std::thread(
    std::bind(&GaitControl::PublishQueueThread, this)
);

this->rosProcessQueueThread = std::thread(
    std::bind(&GaitControl::ProcessQueueThread, this)
);

with the thread functions defined as:

 // Setup thread to process messages
 void GaitControl::ProcessQueueThread()
{
     static const double timeout = 0.01;
     while (this->rosNode->ok())
     {
           this->rosProcessQueue.callAvailable(ros::WallDuration(timeout));
     }
 }

 // Setup thread to process messages 
 void GaitControl::PublishQueueThread()
 {
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
         this->rosPublishQueue.callAvailable(ros::WallDuration(timeout));
      }
 }

The problem i have is whenever I try to create another class functions and for instance publish a message within the function as for example:

 void GaitControl::PublishSomeRandomMessage()
 { 
       sensor_msgs::JointState joint_state_msg;

       this->jointStatePub.publish(joint_state_msg);
 }

I seem to not be able to publish onto the topic. I find this odd as if i publish messages within for instance one of the subscribers callbacks, it works perfectly fine. Also, if i publish from within the PublishQueueThread() function it also works fine, e.g.,

 void GaitControl::PublishQueueThread()
 {
      ros::Rate loop_rate(100);
      while (this->rosNode->ok())
      {
           sensor_msgs::JointState joint_state_msg;

           this->jointStatePub.publish(joint_state_msg);

           loop_rate.sleep();
      }
 }

Is there any specific reasons why the publishing would suddenly stop working as soon as I do it in another class function which is not an callback or the thread function?

The full code is found in the two following files:

https://github.com/Pathorse/Tetrapod-...

https://github.com/Pathorse/Tetrapod-...

Any input is appreciated, thanks!