ROS, class with callback methods
Hi,
I wanted to create a class which initializes a node and has some callback that would be called when I do ros::spin
.
The one implemented bellow doesn't compile (why?). It tells me :
invalid use of non-static member function ‘void EKF_node::chatterCallback(const ConstPtr&)’
ros::Subscriber sub_camera = n.subscribe("chatter", 1000, chatterCallback);
Also, it seems that if I do ros::spin
in the main, it won't call the callback methods. How should I fix this stuff?
#include "ros/ros.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <std_msgs/String.h>
using namespace message_filters;
using namespace std_msgs;
using namespace std;
class EKF_node
{
public:
// constructor
EKF_node(int argc,char** argv)
{
ros::init(argc, argv, "listenerMultipleTopics");
ros::NodeHandle n;
ros::Subscriber sub_camera = n.subscribe("chatter", 1000, chatterCallback);
ros::Subscriber sub_imu = n.subscribe("chatter2", 1000, chatterCallback2);
}
// callback1
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
// callback2
void chatterCallback2(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard from the second chatter: [%s]", msg->data.c_str());
}
private:
// vector containing the outputs
vector<string> y;
};
int main(int argc, char **argv)
{
ros::spin();
return 0;
}
edit :
So I followed the nice suggestions from Thomas and Airuno and I try to do a small code that puts the received string in a string vector and than in the main loop I wan't to display this string components.
The first problem is when I execute this listener node, I cannot use ctrl+c
to stop the node.
The second thing is that between two displays of the vectors there are a lot of white lines that are displayed.
Note that the talker I use are simply the talkers from the tutorial https://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
#include "ros/ros.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <std_msgs/String.h>
using namespace message_filters;
using namespace std_msgs;
using namespace std;
class EKF_node
{
public:
// constructor
EKF_node()
: y(2)
{
ros::NodeHandle n;
sub_camera = n.subscribe("chatter", 1000, &EKF_node::chatterCallback, this);
sub_imu = n.subscribe("chatter2", 1000, &EKF_node::chatterCallback2, this);
}
// callback1
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
//ROS_INFO("I heard: [%s]", msg->data.c_str());
y[0] = msg->data.c_str();
}
// callback2
void chatterCallback2(const std_msgs::String::ConstPtr& msg)
{
//ROS_INFO("I heard from the second chatter: [%s]", msg->data.c_str());
y[1] = msg->data.c_str();
}
//private:
// vector containing the outputs
vector<string> y;
ros::Subscriber sub_camera;
ros::Subscriber sub_imu;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "listenerMultipleTopics");
EKF_node myNode;
ros::Rate r(10);
while(ros::ok)
{
ros::spinOnce;
cout << myNode.y[0] << endl << myNode.y[1] << endl;
r.sleep();
}
return 0;
}
Your update seems like a new question. But you might be able to change to
ros::spinOnce();
.Yes, but the ctrl+c still doesn't work. Actually I am suprised it even compiled without the parenthesis for spinOnce.
ctrl-C doesn't work because you're checking that the ros::ok function exists instead of calling it. Try
while(ros::ok())
damned! Thanks a lot ahendrix, this is absolutely correct!