From 801375e7db8a92deb4581747e2c28c7a02af5e0e Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Wed, 12 Sep 2018 13:49:58 -0500 Subject: [PATCH 1/3] Wait until simulation begins before advertising AddGroupFromNameSrv in HebirosGazeboPlugin --- .../include/hebiros_gazebo_plugin.h | 1 + .../plugin/hebiros_gazebo_plugin.cpp | 13 +++++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h index 6ec767e..b79ef51 100644 --- a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h +++ b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h @@ -36,6 +36,7 @@ class HebirosGazeboPlugin: public ModelPlugin { private: physics::ModelPtr model; + bool _first_sim_iteration; event::ConnectionPtr update_connection; std::map> hebiros_groups; std::map> hebiros_joints; diff --git a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp index 27213c1..f792dd8 100644 --- a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp +++ b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp @@ -16,10 +16,7 @@ void HebirosGazeboPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { ros::init(argc, argv, "hebiros_gazebo_plugin_node"); this->n.reset(new ros::NodeHandle); - this->add_group_srv = - this->n->advertiseService( - "/hebiros_gazebo_plugin/add_group", boost::bind( - &HebirosGazeboPlugin::SrvAddGroup, this, _1, _2)); + this->_first_sim_iteration = true; this->update_connection = event::Events::ConnectWorldUpdateBegin ( boost::bind(&HebirosGazeboPlugin::OnUpdate, this, _1)); @@ -30,6 +27,14 @@ void HebirosGazeboPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { //Update the joints at every simulation iteration void HebirosGazeboPlugin::OnUpdate(const common::UpdateInfo & _info) { + if (this->_first_sim_iteration) { + this->_first_sim_iteration = false; + this->add_group_srv = + this->n->advertiseService( + "/hebiros_gazebo_plugin/add_group", boost::bind( + &HebirosGazeboPlugin::SrvAddGroup, this, _1, _2)); + } + ros::Time current_time = ros::Time::now(); for (auto group_pair : hebiros_groups) { From 83bdbf9b00d70f9158909f7729773342100e82b8 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Wed, 12 Sep 2018 14:02:37 -0500 Subject: [PATCH 2/3] Remove leading underscore from variable name --- hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h | 2 +- hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h index b79ef51..15a7002 100644 --- a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h +++ b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h @@ -36,7 +36,7 @@ class HebirosGazeboPlugin: public ModelPlugin { private: physics::ModelPtr model; - bool _first_sim_iteration; + bool first_sim_iteration; event::ConnectionPtr update_connection; std::map> hebiros_groups; std::map> hebiros_joints; diff --git a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp index f792dd8..457d76a 100644 --- a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp +++ b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp @@ -16,7 +16,7 @@ void HebirosGazeboPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { ros::init(argc, argv, "hebiros_gazebo_plugin_node"); this->n.reset(new ros::NodeHandle); - this->_first_sim_iteration = true; + this->first_sim_iteration = true; this->update_connection = event::Events::ConnectWorldUpdateBegin ( boost::bind(&HebirosGazeboPlugin::OnUpdate, this, _1)); @@ -27,8 +27,8 @@ void HebirosGazeboPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { //Update the joints at every simulation iteration void HebirosGazeboPlugin::OnUpdate(const common::UpdateInfo & _info) { - if (this->_first_sim_iteration) { - this->_first_sim_iteration = false; + if (this->first_sim_iteration) { + this->first_sim_iteration = false; this->add_group_srv = this->n->advertiseService( "/hebiros_gazebo_plugin/add_group", boost::bind( From af1537f89d4ae68f9ecabe0744713a4e0c770b56 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Tue, 20 Nov 2018 11:05:34 -0600 Subject: [PATCH 3/3] Use bracket initialization for first_sim_iteration flag --- hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h | 2 +- hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h index 15a7002..f067fa6 100644 --- a/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h +++ b/hebiros_gazebo_plugin/include/hebiros_gazebo_plugin.h @@ -36,7 +36,7 @@ class HebirosGazeboPlugin: public ModelPlugin { private: physics::ModelPtr model; - bool first_sim_iteration; + bool first_sim_iteration{true}; event::ConnectionPtr update_connection; std::map> hebiros_groups; std::map> hebiros_joints; diff --git a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp index 457d76a..2ba5ff0 100644 --- a/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp +++ b/hebiros_gazebo_plugin/plugin/hebiros_gazebo_plugin.cpp @@ -16,8 +16,6 @@ void HebirosGazeboPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { ros::init(argc, argv, "hebiros_gazebo_plugin_node"); this->n.reset(new ros::NodeHandle); - this->first_sim_iteration = true; - this->update_connection = event::Events::ConnectWorldUpdateBegin ( boost::bind(&HebirosGazeboPlugin::OnUpdate, this, _1));