C++ subscriber inside class that updates class variable
I want to control my robot with an object of a class - that class will start subscribers during initialization and automatically update it's class variables. Then I can use getters to get current state of robot. But I have currently problem with receiving the data - the subscriber doesn't work (I tested subscribing to topics with gazebo tutorial and my publishers work).
If I am right then the callback doesn't work because when I create the class, the C++ code stops and so subscriber stops. Have anybody done something similar? I don't know C++ -> that's why I am binding everything with python that I can use it. Only idea I got is to start a new process while initializing the class and that process should take care of running the subscriber - but I am not sure if it would work and how to do it.
Here is some of my code:
#include <string>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <gazebo/gazebo_config.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <stdlib.h>
#include <gazebo/gazebo_client.hh>
namespace py = pybind11;
using namespace std;
class Interface {
public:
vector<string> _jointVel = {};
Interface(string robot) {
gazebo::client::setup();
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();
cout << "~/" + robot + "/joints_vel" +" setting up \n";
// Subscriber
gazebo::transport::SubscriberPtr subJointVel = node->Subscribe("~/" + robot + "/joints_vel", &Interface::cb, this);
}
void cb(ConstGzString_VPtr &_msg)
{
cout << " omg \n";
_jointVel.push_back("bla");
for (int i = 0; i < _msg->data_size(); i++)
_jointVel.push_back(_msg->data(i));
for (vector<string>::const_iterator i = _jointVel.begin(); i != _jointVel.end(); ++i)
cout << *i << ' ';
cout << " were received values \n";
}
vector<string> getJointsVel()
{
cout << " getJointsVelFunction \n";
return _jointVel;
}
void finish(){
// Make sure to shut everything down.
gazebo::client::shutdown();
}
};
PYBIND11_MODULE(publisher, m) {
py::class_<Interface>(m, "Interface")
.def(py::init<const string &>())
.def("getJointsVel", &Interface::getJointsVel);
}