I used this tutorial to get me going http://gazebosim.org/tutorials?tut=guided_i5#TestthemessagepassingAPI.
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 %f \n",cmd); } // Called by the world update start event public: void OnUpdate(const common::UpdateInfo & /*_info*/) { //double force_1 = joint1_sy->GetForceTorque(0).body2Force.z; joint1->SetForce(2, cmd); gazebo::msgs::Vector3d feedback_msg; feedback_msg.set_x(joint1->GetAngle(2).Degree()); pub1->WaitForConnection(); pub1->Publish(feedback_msg); printf("ONUPDATE: Update occurred"); } // Pointer to the model private: physics::ModelPtr model; /// \brief Pointer to the joint. private: physics::JointPtr joint1; //shoulder yaw /// \brief A node used for transport public: transport::NodePtr node; public: transport::PublisherPtr pub1; public: transport::SubscriberPtr sub1; // Pointer to the update event connection private: event::ConnectionPtr updateConnection; private: double cmd; }; // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin. GZ_REGISTER_MODEL_PLUGIN(gazebo_simple_plugin) } Originally posted by Francis123 on Gazebo Answers with karma: 33 on 2017-01-25
Post score: 0
Original comments
Comment by hugomatic on 2017-01-26:
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?
Comment by Francis123 on 2017-01-27:
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();
Comment by Francis123 on 2017-01-27:
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.
Comment by nkoenig on 2017-01-27:
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?
Comment by Francis123 on 2017-01-27:
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->Advertisegazebo::msgs::Vector3d("/gazebo/torqueprofile/feedback");
They are identical name definition
Comment by nkoenig on 2017-01-27:
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.
Comment by Francis123 on 2017-01-27:
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?