Home | Tutorials | Wiki | Issues
Ask Your Question
0

Only one model plugin receiving subscription data

asked 2018-05-20 02:50:36 -0500

varunvp gravatar image

updated 2018-05-21 01:10:27 -0500

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<gazebo::msgs::Vector3d>("~/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!

Edit:I printed the &wind_push::publ variable in the load function, and all model plugins referred to the same address! Shouldn't gazebo be running a seperate instance of the plugin for each model? Another thing I noticed is that the model which responds to the wind is the last model described with the subscriber plugin in the world file. I think that only the last model plugin's callback is being called. Or there ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-05-21 10:39:58 -0500

Some ideas:

  1. Use the fully qualified topic name on both the publisher and the subscriber (i.e. /gazebo/default/global_wind)
  2. Did you try printing this instead of &wind_push::publ to see if that's also the same?
  3. Why do you want to call gazebo::transport::fini() on your load function? That will finish the whole transport system, so no more pub/sub for anyone.
  4. While running the simulation, run gz topic -i /gazebo/default/global_wind to see how many subscribers and publishers are there
  5. Store your subscriber in a member variable, otherwise it will only live in the scope of the load function.
edit flag offensive delete link more

Comments

Apologies for the delay. The 1st solution worked for me! Thank you! Edit:Nope, it was actaully the last solution that worked for me. I reverted to the earlier topic name. Also, I did store the subscriber in a member variable, but it was initialised outside the class. Don't know why I did that. It worked when I added it inside.

varunvp gravatar imagevarunvp ( 2018-05-24 07:32:34 -0500 )edit

Glad to hear it worked out

chapulina gravatar imagechapulina ( 2018-05-24 12:37:01 -0500 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2018-05-20 02:50:36 -0500

Seen: 138 times

Last updated: May 21 '18