0
$\begingroup$

Gazebo Answers logo

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?


Originally posted by Elte Hupkes on Gazebo Answers with karma: 133 on 2015-03-25

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Gazebo Answers logo

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

Originally posted by nkoenig with karma: 7676 on 2015-03-25

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Elte Hupkes on 2015-03-25:
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!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.