Subscribing to sensor data from model plugin
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?