How to run a thread/parallel process in gazebo plugin

asked 2017-10-27 10:40:14 -0600

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

Hi all, I am working on RL algorithm to learn some task with a robot. I am using gazebo as a simulator. I don't want to use ROS. I am able to create plugin for publishing robot joint angles on a topic and subscribe to that topic with another code (main cpp file for RL).

But I wanted to run my RL algorithm within plugin (as it is a cpp code) so that I don't have overhead of publishing and subscribing. So in my new plugin code I am updating my global joint_angles whenever gazebo updates by binding a routine with update of gazebo. Following is my new world plugin file

#include <gazebo/gazebo.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <boost/bind.hpp>
#include <gazebo/common/common.hh>
#include <stdio.h>
#include <string>

namespace gazebo
{
    class WorldPluginTutorial : public WorldPlugin
    {
        public: WorldPluginTutorial() : WorldPlugin()
        {
            this->joint_angles[0] = 0;
            this->joint_angles[1] = 0;
            this->joint_angles[2] = 0;
            this->joint_angles[3] = 0;
            this->joint_angles[4] = 0;
            this->joint_angles[5] = 0;
        }

        public: void Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)
        {
            this->model = _world->GetModel("robot_model").get();
            this->updateConnection =event::Events::ConnectWorldUpdateBegin(
                                            boost::bind(&WorldPluginTutorial::OnUpdate, this, _1));  

            while(true)
            {
                std::cout<<"program running"<<std::endl;
                gazebo::common::Time::MSleep(500);
            }

        }

        public: void OnUpdate(const common::UpdateInfo & /*_info*/)
        {
            this->joint_angles[0] = this->model->GetJoint("joint1")->GetAngle(0).Radian();
            this->joint_angles[1] = this->model->GetJoint("joint2")->GetAngle(0).Radian();
            this->joint_angles[2] = this->model->GetJoint("joint3")->GetAngle(0).Radian();
            this->joint_angles[3] = this->model->GetJoint("joint3")->GetAngle(0).Radian();
            this->joint_angles[4] = this->model->GetJoint("joint4")->GetAngle(0).Radian();
            this->joint_angles[5] = this->model->GetJoint("joint5")->GetAngle(0).Radian();

            std::cout<<"Angles Updated"<<std::endl;
        }

        private: gazebo::physics::Model *model;
        private: event::ConnectionPtr updateConnection;
        private: double joint_angles[6];
        private: transport::SubscriberPtr sub;
        private: transport::NodePtr node;
    };
  GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
}

Main problem is the while loop in Load() function keeps on running while OnUpdate() function is not called ("Angles updated" does not print when I start simulation). If I keep while loop for particular number of iterations then those iterations are completed first and then OnUpdate() function starts. So basically Load function should end before starting OnUpdate() callbacks. I also tried using a thread and binded a routine with that but same results. OnUpdate() starts only when that thread is over. Is there a way to run a process in parallel so that my main RL algorithm is in that and OnUpdate function updates joint angles. Thanks in advance any help is really appreciated.

edit retag flag offensive close merge delete