Publishing parameters to a running gazebo simulation
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::string param_name = temp.substr(0,aloc);
if(param_name.compare("nei_sensing") == 0)
{
std::string param_value_str = temp.substr(aloc+1);
double value = std::stod(param_value_str);//double value of parameter
this->nei_sensing = value;
this->param_set = true;
}
}
}
}
void OnUpdate(const common::UpdateInfo &_info)
{
std::lock_guard<std::mutex> lock(this->mutex);
gazebo::common::Time st = _info.simTime;
if(this->param_set)
{
//do stuff
if(st.sec >= 30)
{//stop simulation after 30 seconds
msgs::Any any;
any.set_type(msgs::Any::BOOLEAN);
any.set_bool_value(this->world_info_bool);
this->pub_world_loaded->Publish(any);
this->world->Reset();
this->world->Stop();//or this->world->Fini());
}
}
if(this->world_info_bool){
msgs::Any any;
any.set_type(msgs::Any::BOOLEAN);
any.set_bool_value(this->world_info_bool);
this->pub_world_loaded->Publish(any);
this->world_info_bool = false;
}
}
};
//Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(WP_Swarm1)
}
Asked by elcymon on 2017-11-28 08:31:12 UTC
Comments