Publishing parameters to a running gazebo simulation

asked 2017-11-28 07:31:12 -0500

elcymon gravatar image

HI I am trying to update the parameters of my plugins before starting the simulation.

So I want to create a standalone publisher that will publish to a topic listened to by the world and model plugins in the simulation. However, this does not work. Gazebo just hangs and stop responding. When I try to restart the standalone publisher, Gazebo crashes with this error:

terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::thread_resource_error> >'
  what():  boost thread: trying joining itself: Resource deadlock avoided

I am using Gazebo 7.4.0 The minimized version of the independent publisher is:

#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo_client.hh>
#include "gazebo/common/common.hh"
#include <gazebo/gazebo.hh>

#include <gazebo/physics/physics.hh>

#include <iostream>
#include <fstream>
#include <set>
#include <mutex>
std::mutex mutex1;

using namespace std;
bool world_loaded;

void world_loaded_func(ConstAnyPtr &any)
{std::lock_guard<std::mutex> lock(mutex1);
    world_loaded = any->bool_value();
}

int main(int _argc, char **_argv)
{
    world_loaded = false;
    //Load gazebo
    gazebo::client::setup(_argc, _argv);

    //create our node for communication
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init();

    //listen to when world is loaded
    gazebo::transport::SubscriberPtr sub_world_loaded;
    sub_world_loaded = node->Subscribe("/world_loaded",world_loaded_func);


    //for advertising simulation parameters
    gazebo::transport::PublisherPtr pub_params = node->Advertise<gazebo::msgs::Any>("/params_topic");
    pub_params->WaitForConnection();

    gazebo::msgs::Any any;
    any.set_type(gazebo::msgs::Any::STRING);
    std::string pars = "nei_sensing:5.0";
    any.set_string_value(pars);

    while(true){//busy wait
        gazebo::common::Time::MSleep(100);
        if(world_loaded)
        {
            cout<<"world_loaded"<<endl;
                    pub_params->Publish(any);

        }
        //cout<<"running"<<endl;
    }

    //Make sure to shut everything down
    gazebo::client::shutdown();
}

Sample code of world plugin (one of the plugins I need to set parameters for).

#include <ignition/math/Pose3.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"
#include <boost/bind.hpp>
#include <iostream>
#include <string>
#include <fstream>
#include <mutex>
#include <map>
#include <vector>
#include <set>

using namespace std;
namespace gazebo
{
    class WP_Swarm1 : public WorldPlugin
    {
        //Pointer to the update event connection
        private:
            event::ConnectionPtr updateConnection;
            physics::WorldPtr world;
            std::mutex mutex;
            transport::NodePtr node;
            transport::PublisherPtr pub_world_loaded;
            std::string world_info_string;//should state when simulation starts and ends per world
            bool world_info_bool;//used to indicate start of new simulation
            double nei_sensing;
            bool param_set;
            gazebo::transport::SubscriberPtr sub_params;

        public:
            void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
            {
                this->world = _parent;
                this->node = transport::NodePtr(new transport::Node());
                this->node->Init();
                this->pub_world_loaded = this->node->Advertise<msgs::Any>("/world_loaded");
                            this->world_info_bool = true;
                this->param_set = false;
                this->sub_params = this->node->Subscribe("/params_topic",&WP_Swarm1::Params_cb,this);

                this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                            boost::bind(&WP_Swarm1::OnUpdate,this,_1));
            }
            void Params_cb(ConstAnyPtr &a)
            {//set all parameters and set param_set = true when done.
                std::lock_guard<std::mutex> lock(this->mutex);
                if(!this->param_set)
                {
                    std::string all_params = a->string_value();
                    //all_params = all_params + " ";
                    while(all_params.find(" ") != std::string::npos)
                    {//loop through all parameters and assign to appropriate variable in plugin.
                        int ploc = all_params.find(" ");
                        std::string temp = all_params.substr(0,ploc);

                        int aloc = temp.find(":");
                        std ...
(more)
edit retag flag offensive close merge delete