Hey guys,
I am publishing a Vector3 through a world plugin.
This is the code that publishes
namespace gazebo
{
class global_wind : public WorldPlugin
{
ignition::math::Vector3d wind;
transport::PublisherPtr pub;
private: event::ConnectionPtr updateConnection;
public: void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
{
gazebo::transport::NodePtr node(new gazebo::transport::Node);
node->Init(_parent->GetName());
transport::run();
pub = node->Advertise("~/global_wind");
this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&global_wind::OnUpdate, this));
// gazebo::transport::fini();
}
public: void OnUpdate()
{
// pub->WaitForConnection();
wind.Set(2,2,0);
gazebo::msgs::Vector3d msg;
gazebo::msgs::Set(&msg, wind);
pub->Publish(msg);
}
};
GZ_REGISTER_WORLD_PLUGIN(global_wind)
}
The lines that are commented are causing some problems. The first commented line causes the following error
[Err] [ConnectionManager.cc:611] ConnectionManager is not initialized
[Err] [TransportIface.cc:416] ConnectionManager has not been initialized!
[Err] [Node.cc:125] No namespaces found
The 2nd commented line just makes gazebo hang when launching. Nonetheless, the plugin 'works' when these 2 are commented.
This is the code that sunscribes.
class wind_push : public ModelPlugin
{
// Pointer to the model
private: physics::ModelPtr model;
// Pointer to the update event connection
private: event::ConnectionPtr updateConnection;
math::Vector3 force;
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Store the pointer to the model
this->model = _parent;
// Create our node for communication
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();
sub = node->Subscribe("/gazebo/default/global_wind", &wind_push::publ, this);
gazebo::physics::LinkPtr link= model->GetChildLink("link_1");
link->AddForce(gazebo::math::Vector3(20, 20, 0));
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&wind_push::OnUpdate, this));
}
// Called by the world update start event
public: void OnUpdate()
{
gazebo::physics::LinkPtr link= model->GetChildLink("link_1");
link->AddForce(force);
}
public: void publ(ConstVector3dPtr &_msg)
{
std::cout << this->model->GetName();
force.Set(_msg->x(),_msg->y(),_msg->z());
}
};
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(wind_push)
}
So what's happening here is that I have many models with the above model plugin. The global_wind plugin publishes a global topic, from which wind_push is supposed to get the wind force vector and apply a suitable force. The problem is that only one model is responding to this force. After further debugging, I found out that only the model plugin on THAT model is calling the callback in the subscriber function, while I actually want all of them to be called. Debugging also revealed that all the model plugins are running, executing other parts of the code. I don't understand why this is happening. Thanks!
↧