#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
void model_states_callback(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
ROS_INFO("I heard the model state!");
}
namespace gazebo
{
class kiwi_pos : public ModelPlugin
{
public: kiwi_pos() : ModelPlugin()
{
}
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
if(!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
ROS_INFO("KIWI POSITION PLUGIN LOADED");
// Store the pointer to the model
this->model = _parent;
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&kiwi_pos::OnUpdate, this, _1));
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("gazebo/model_states",1000,model_states_callback);
//ros::spin();
}
// Called by the world update start event
public: void OnUpdate(const common::UpdateInfo & /*_info*/)
{
}
// Pointer to the model
private: physics::ModelPtr model;
// Pointer to the update event connection
private: event::ConnectionPtr updateConnection;
};
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(kiwi_pos)
}
Here is my code, I am simply trying to listen in on the model states topic and publish a confirmation when I hear it. When I run the simulation I get the PLUGIN LOADED ROS_INFO but nothing else is outputted.
↧