How to run a thread/parallel process in gazebo plugin. [closed]

edit

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.