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
#include
#include
#include
#include
#include
#include
#include
namespace py = pybind11;
using namespace std;
class Interface {
public:
vector _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::const_iterator i = _jointVel.begin(); i != _jointVel.end(); ++i)
cout << *i << ' ';
cout << " were received values \n";
}
vector getJointsVel()
{
cout << " getJointsVelFunction \n";
return _jointVel;
}
void finish(){
// Make sure to shut everything down.
gazebo::client::shutdown();
}
};
PYBIND11_MODULE(publisher, m) {
py::class_(m, "Interface")
.def(py::init())
.def("getJointsVel", &Interface::getJointsVel);
}
↧