Gazebo | Ignition | Community
Ask Your Question
0

C++ subscriber inside class that updates class variable

asked 2018-01-21 12:16:51 -0600

Kanter666 gravatar image

updated 2018-01-21 12:20:39 -0600

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);
}
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2018-01-22 05:54:09 -0600

Carlos Agüero gravatar image

You need to declare your subscriber as a member variable. Otherwise, your subJointVel variable runs out of scope in the constructor and your subscription is terminated.

edit flag offensive delete link more

Comments

When I declare subscriber as member variable and then run simple loop where I try to get the data, I get an error - void boost::recursive_mutex::lock(): Assertion `!pthread_mutex_lock(&m)' failed. However, I did one work around when I set up just node as member variable, and then set up subscriber in the function and wait till the variable loads - I got more than 200 runs in 1 second so it works fine.

Kanter666 gravatar imageKanter666 ( 2018-01-26 12:32:33 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2018-01-21 12:16:51 -0600

Seen: 1,618 times

Last updated: Jan 22 '18