Added get configuration

CMakeLists.txt

@@ -79,6 +79,7 @@ pkg_check_modules(JSONCPP jsoncpp)
 ## Generate services in the 'srv' folder
  add_service_files(
    FILES
+   GetConfig.srv
    SetConfig.srv
  )
 

src/config_plugin.cpp

@@ -6,6 +6,7 @@
 #include "jsoncpp/json/json.h"
 #include "brass_gazebo_battery/SetLoad.h"
 #include "brass_gazebo_config_manager/SetConfig.h"
+#include "brass_gazebo_config_manager/GetConfig.h"
 
 namespace gazebo{
 class GAZEBO_VISIBLE ConfigurationPlugin : public ModelPlugin
@@ -17,6 +18,7 @@ protected: physics::ModelPtr model;
 protected: std::unique_ptr<ros::NodeHandle> rosNode;
 protected: ros::ServiceClient power_load_client;
 protected: ros::ServiceServer set_bot_configuration;
+protected: ros::ServiceServer get_bot_configuration;
 protected: boost::mutex lock;
 protected: Json::Value config_list;
 private: const char * home = getenv("HOME");
@@ -59,6 +61,7 @@ private: const char * home = getenv("HOME");
 
         this->power_load_client = this->rosNode->serviceClient<brass_gazebo_battery::SetLoad>(this->model->GetName() + "/set_power_load");
         this->set_bot_configuration = this->rosNode->advertiseService(this->model->GetName() + "/set_robot_configuration", &ConfigurationPlugin::SetConfiguration, this);
+        this->get_bot_configuration = this->rosNode->advertiseService(this->model->GetName() + "/get_robot_configuration", &ConfigurationPlugin::GetConfiguration, this);
 
         Json::Reader json_reader;
         std::string config_path = _sdf->Get<std::string>("config_list_path");
@@ -110,6 +113,19 @@ public: bool SetConfiguration(brass_gazebo_config_manager::SetConfig::Request &r
         return true;
     }
 
+public: bool GetConfiguration(brass_gazebo_config_manager::GetConfig::Request &req,
+                                brass_gazebo_config_manager::GetConfig::Response &res)
+    {
+        lock.lock();
+        if (req.give_current_config)
+        {
+            ROS_GREEN_STREAM("The current configuration was requested: " << this->current_config);
+            res.result = this->current_config;
+        }
+        lock.unlock();
+        return true;
+    }
+
 };
     // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
     GZ_REGISTER_MODEL_PLUGIN(ConfigurationPlugin)

srv/GetConfig.srv

@@ -0,0 +1,3 @@
+bool give_current_config
+---
+int64 result