Gazebo node cannot see Gazebo Topic, need help understanding why
I used this tutorial to get me going http://gazebosim.org/tutorials?tut=gu....
It gave me enough information to create a gazebo model plugin, and a standalone gazebo node that would use a gazebo transport node to publish commands to a Gazebo topic.
My problem is that my gazebo model plugin is unable to see when a message is sent to the gazebo topic ("/topic/a") even though I have a gazebo subscriber callback function for "/topic/a". To confirm that I was publishing and subscribing to the gazebo topic I opened a terminal and used the command
"gz topic -i /topic/a"
The response was
Type: gazebo.msgs.Vector3d
Publishers:
Same IP:43457
Subscribers:
Same IP:45557
They had the same IP but different ports. Is that normal? How do I get both gazebo nodes to look at the same Gazebo topic?
My code for my standalone is below
#include <gazebo/gazebo_client.hh>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo_config.h>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
void callback_from_gazebo(ConstVector3dPtr &_msg) { std::cout << "Callback received () \n"; }
int main(int _argc, char **_argv) { std::cout << "Gazebo Initialization \n";
gazebo::transport::PublisherPtr pub1,pub2;
gazebo::transport::SubscriberPtr sub1;
gazebo::client::setup(_argc, _argv);
gazebo::transport::init();
gazebo::transport::run();
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init("default");
std::cout << "End Gazebo Initialization \n";
std::cout << "Define Gazebo Publishers and Subscribers \n";
pub1 = node->Advertise<gazebo::msgs::Vector3d>("/topic/a");
pub2 = node->Advertise<gazebo::msgs::WorldControl>("/gazebo/torqueprofile/world_control");
sub1 = node->Subscribe("/topic/b", callback_from_gazebo);
std::cout << "Waiting for connection\n";
pub1->WaitForConnection();
pub2->WaitForConnection();
std::cout << "Connections confirmed\n";
while(1)
{
gazebo::msgs::Vector3d cmd;
cmd.set_x(1);
cmd.set_y(2);
cmd.set_z(3);
pub1->WaitForConnection();
pub1->Publish(cmd);
gazebo::msgs::WorldControl world_msg;
world_msg.set_step(true);
pub2->Publish(world_msg);
gazebo::common::Time::MSleep(1000);
}
gazebo::transport::fini();
gazebo::client::shutdown();
return 0; }
My code for my plugin is below
#include <boost/bind.hpp>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/Angle.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/gazebo_config.h>
#include <stdio.h>
#include <fstream>
namespace gazebo
{
/// \brief A plugin to control two cubes strictly in Gazebo
class gazebo_simple_plugin : public ModelPlugin
{
/// \brief Constructor
public: gazebo_simple_plugin()
{
}
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
model = _model;
if (model->GetJointCount() == 0)
{
std::cerr << "\n no joints \n";
return;
}
else
std::cerr << "\n joints are there \n";
//set joint handle to joints in the _model
joint1 = model->GetJoints()[0];
// Create the Gazebo node
node = transport::NodePtr(new transport::Node());
node->Init(this->model->GetWorld()->GetName());
pub1 = node->Advertise<gazebo::msgs::Vector3d>("/topic/b");
sub1 = node->Subscribe("/topic/a", &gazebo_simple_plugin::callback_from_standalone, this);
//Listen to the update event. This event is broadcast every
updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&gazebo_simple_plugin::OnUpdate, this, _1));
}
private: void callback_from_standalone(ConstVector3dPtr &_msg)
{
cmd=(_msg->x());
printf("cmd received ...
Make sure there is no typo in the topic name for your subscribe call. Can you provide code and what Gazebo version you are using?
I made sure there was no typo in the topic name. Also i am able to see both the publisher and the subscriber to the topic so that confirms that the gazebo nodes are looking at the topic. I did find this page to be helpful (http://gazebosim.org/tutorials?tut=custom_messages#CodeExplained13). It made it clear that in my gazebo node executable (that uses int main()) I needed to make a call to these two transport functions: gazebo::transport::init(); gazebo::transport::run();
I also added the call pub1->WaitForConnection(); before each message that I publish over the gazebo transport layer, before I just called it after defining the gazebo publisher handle. I am using Gazebo 7, and cannot show code due to employment restrictions.
It is normal that the publisher and subscriber have different ports. The output of gz topic indicates that there is one publisher on /topic/a and one subscriber. If you are running a model plugin, then you shouldn't call gazebo::transport::init nor gazebo::transport::run. The documentation you referenced is for a standalone program. Can you post your code?
I can show small portions though. Here is my subscriber for topic a in one gazebo node sub1 = node->Subscribe("/gazebo/torqueprofile/feedback", callback_from_gazebo); Here is my code for the publisher for "topic a" in the gazebo control plugin pub1 = node->Advertise<gazebo::msgs::vector3d>("/gazebo/torqueprofile/feedback"); They are identical name definition
Can you create a simple plugin that demonstrates your problem, but doesn't violate your employment restrictions. Creating a simple plugin will also help you self-debug the problem.
I added the code to the question now. So basically what I see now is the stand alone can publish to topic a, but the gazebo plugin cannot publish to topic b, any ideas?