Thread Subscriber and Publisher in the same class ?

asked 2014-05-05 06:13:25 -0500

mrshifo gravatar image

updated 2014-05-05 22:27:06 -0500

I would like to have a class that manages threads. Each Thread subscribe and public at different frequencies.

Error:

Description Resource    Path    Location    Type
no match for call to ‘(boost::_mfi::mf1<void, b, const boost::shared_ptr<const std_msgs::String_<std::allocator<void> > >&>) (const boost::shared_ptr<const std_msgs::String_<std::allocator<void> > >&)’   test        line 225, external location: /usr/include/boost/function/function_template.hpp  C/C++ Problem

Code:

#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "std_msgs/String.h"

#include "ros/network.h"

#include <boost/thread.hpp>


class b
{
public:
    b();
    ros::CallbackQueue g_queue;
    ros::NodeHandle n;
    ros::SubscribeOptions ops;
    ros::Subscriber sub;
    void chatterCallbackMainQueue(const std_msgs::String::ConstPtr& msg);
    void chatterCallbackCustomQueue(const std_msgs::String::ConstPtr& msg);
    void callbackThread();

};

void b::chatterCallbackMainQueue(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" << boost::this_thread::get_id() << "] (Main thread)");
  static double t1 = ros::Time::now().toSec();
  double t2 = ros::Time::now().toSec();
  ROS_INFO("Freq to call main: %f",1/(t2-t1));
}

void b::chatterCallbackCustomQueue(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO_STREAM("I heard: [ " << msg->data << "] in thread [" << boost::this_thread::get_id() << "]");
  static double t1 = ros::Time::now().toSec();
  double t2 = ros::Time::now().toSec();
  ROS_INFO("Freq to call custom: %f",1/(t2-t1));
  t1 = t2;

}

void b::callbackThread()
{
  ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());

  double tim1;
  double tim2;

  ros::NodeHandle n2;
  while (n2.ok())
  { tim1 = ros::Time::now().toSec();
    g_queue.callAvailable(ros::WallDuration(0.01));
    tim2 = ros::Time::now().toSec();
    ROS_INFO("thread freq: %f",1/(tim2-tim1));

  }


}

b::b()
{
    ops = ros::SubscribeOptions::create<std_msgs::String>("chatter", 1000,&b::chatterCallbackCustomQueue,ros::VoidPtr(), &g_queue);
    sub = n.subscribe(ops);

    //ros::Subscriber sub2 = n.subscribe("chatter", 1000, chatterCallbackMainQueue);

    boost::thread chatter_thread(boost::bind(&b::callbackThread, this));

    //ROS_INFO_STREAM("Main thread id=" << boost::this_thread::get_id());


    double t1;
    double t2;

    ros::Rate r(1);
    while (n.ok())
    {
      t1 = ros::Time::now().toSec();
      ros::spinOnce();
      r.sleep();
      t2 = ros::Time::now().toSec();
      ROS_INFO("main loop Hz: %f",1/(t2-t1));
    }


  chatter_thread.join();
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener_with_custom_callback_processing");

  b obj;

  return 0;
}
edit retag flag offensive close merge delete