Gazebo | Ignition | Community
Ask Your Question
0

Subscribing to sensor data from model plugin

asked 2015-03-25 09:50:04 -0500

Elte Hupkes gravatar image

I'm attempting to subscribe to a sensor's data from a model plugin. Simplified, my model plugin code looks like:

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <iostream>

namespace gazebo
{
  class SensorRead : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      // Store the pointer to the model
      this->model = _parent;

      std::cout << "Plugin loaded" << std::endl;
      std::string topicName = "~/my_model/my_link/my_sensor/imu";

        transport::NodePtr node(new transport::Node());
        node->Init("default");
      node->Subscribe(topicName, &SensorRead::OnUpdate, this);
    }

    // Called by the world update start event
    public: void OnUpdate(ConstIMUPtr & /*msg*/)
    {
      std::cout << "An update!" << std::endl;
    }

    // Pointer to the model
    private: physics::ModelPtr model;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(SensorRead)
}

The world code I've tested it with is equally simple, just a box, with a link, with an IMU sensor with an update rate of 5.

When I open a command prompt and do

gz topic -e "~/my_model/my_link/my_sensor/imu"

it starts spitting out IMU data 5 times per second as is to be expected. However, upon loading the model with the plugin, it outputs "Plugin loaded", and then... nothing. I've tried a wide array of namespaces, and have even verified in code that the topic name I use exists through TopicManager::FindPublication(). It seems like the topic is there, the events are there, but my model plugin is not allowed to have them.

I've no idea what I'm doing wrong, is what I'm trying to do not meant to be possible?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-03-25 13:32:13 -0500

nkoenig gravatar image

It's probably because your Subscriber gets deleted at the end of the Load function. The Subscribe function returns a smart pointer, which will get deleted once it goes out of scope. The solution is to added a class member variable to hold the subscriber pointer:

this->mySubscriber = node->Subscribe(topicName, &SensorRead::OnUpdate, this);
...
private: transport::SubscriberPtr mySubscriber
edit flag offensive delete link more

Comments

Oh god, I feel like an idiot. I was so involved in finding a deeper problem that I forgot about C++ memory management. Yups, solves it, many thanks!

Elte Hupkes gravatar imageElte Hupkes ( 2015-03-25 17:02:29 -0500 )edit

Question Tools

Stats

Asked: 2015-03-25 09:50:04 -0500

Seen: 2,165 times

Last updated: Mar 25 '15