diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4abe0d7..d172173 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -83,6 +83,8 @@ pkg_check_modules(JSONCPP jsoncpp)
    SetConfig.srv
  )
 
+
+
 ## Generate actions in the 'action' folder
 # add_action_files(
 #   FILES
diff --git a/package.xml b/package.xml
index 30b42ca..e9f765b 100644
--- a/package.xml
+++ b/package.xml
@@ -37,13 +37,13 @@
   <!--   <build_depend>roscpp</build_depend> -->
   <!--   <exec_depend>roscpp</exec_depend> -->
   <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
+  <build_depend>message_generation</build_depend>
   <!-- Use build_export_depend for packages you need in order to build against this package: -->
   <!--   <build_export_depend>message_generation</build_export_depend> -->
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <exec_depend>message_runtime</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->
diff --git a/src/config_list.json b/src/config_list.json
deleted file mode 100644
index 8825916..0000000
--- a/src/config_list.json
+++ /dev/null
@@ -1,57 +0,0 @@
-{
-  "0":{
-    "config_id": 100,
-    "power_load": 50.7,
-    "speed": 0.5
-  },
-  "1":{
-  "config_id": 12,
-  "power_load": 60,
-  "speed": 0.4
-  },
-  "2":{
-  "config_id": 32,
-  "power_load": 55,
-  "speed": 0.5
-  },
-  "3":{
-  "config_id": 125,
-  "power_load": 76,
-  "speed": 0.7
-  },
-  "4":{
-  "config_id": 345,
-  "power_load": 76,
-  "speed": 0.7
-  },
-  "5":{
-  "config_id": 432,
-  "power_load": 76,
-  "speed": 0.7
-  },
-  "6":{
-  "config_id": 555,
-  "power_load": 76,
-  "speed": 0.7
-  },
-  "7":{
-  "config_id": 666,
-  "power_load": 76,
-  "speed": 0.7
-  },
-  "8":{
-  "config_id": 777,
-  "power_load": 76,
-  "speed": 0.7
-  },
-  "9":{
-  "config_id": 999,
-  "power_load": 76,
-  "speed": 0.7
-  },
-  "10":{
-  "config_id": 888,
-  "power_load": 76,
-  "speed": 0.7
-  }
-}
\ No newline at end of file
diff --git a/src/config_plugin.cpp b/src/config_plugin.cpp
index ae493f3..7666f0f 100644
--- a/src/config_plugin.cpp
+++ b/src/config_plugin.cpp
@@ -11,7 +11,7 @@
 namespace gazebo{
 class GAZEBO_VISIBLE ConfigurationPlugin : public ModelPlugin
 {
-protected: double power_load;
+protected: float power_load;
 protected: int default_config;
 protected: int current_config;
 protected: physics::ModelPtr model;
@@ -72,17 +72,36 @@ private: const char * home = getenv("HOME");
         std::ifstream config_file(home + config_path, std::ifstream::binary);
         json_reader.parse(config_file, this->config_list, false);
 
+        ROS_GREEN_STREAM("Configuration Plugin is fully loaded.");
+    }
+
+    private: float get_power_load_by_id(int config_id)
+    {
+        for (const Json::Value& config: this->config_list["configurations"])
+        {
+            if (config["config_id"] == config_id)
+            {
+                return config["power_load_w"].asFloat();
+            }
+        }
+
     }
 
     public: virtual void Init()
     {
-        std::string load = config_list[std::to_string(this->default_config)]["power_load"].asString();
-        this->power_load = std::stod(load.c_str());
+        ROS_GREEN_STREAM("Init Config Manager");
+        this->power_load = get_power_load_by_id(this->default_config);
+        ROS_GREEN_STREAM(this->power_load);
         SetPowerLoad(this->power_load);
         ROS_GREEN_STREAM("Default configuration has been set");
     }
 
-public: bool SetPowerLoad(double power_load)
+    public: virtual void Reset()
+    {
+        this->Init();
+    }
+
+    public: bool SetPowerLoad(float power_load)
     {
         brass_gazebo_battery::SetLoad srv;
         srv.request.power_load = power_load;
@@ -93,27 +112,26 @@ public: bool SetPowerLoad(double power_load)
             return true;
         }
         else{
-            ROS_RED_STREAM("An error happened calling power load service set");
+            ROS_RED_STREAM("An error happened calling power load service");
             return false;
         }
 
     }
 
-public: bool SetConfiguration(brass_gazebo_config_manager::SetConfig::Request &req,
+    public: bool SetConfiguration(brass_gazebo_config_manager::SetConfig::Request &req,
                                 brass_gazebo_config_manager::SetConfig::Response &res)
     {
         lock.lock();
         this->current_config = req.current_config;
         ROS_GREEN_STREAM("New configuration of the robot: " << this->current_config);
-        std::string load = config_list[std::to_string(this->current_config)]["power_load"].asString();
-        this->power_load = std::stod(load.c_str());
+        this->power_load = get_power_load_by_id(this->current_config);
         SetPowerLoad(this->power_load);
         lock.unlock();
         res.result = true;
         return true;
     }
 
-public: bool GetConfiguration(brass_gazebo_config_manager::GetConfig::Request &req,
+    public: bool GetConfiguration(brass_gazebo_config_manager::GetConfig::Request &req,
                                 brass_gazebo_config_manager::GetConfig::Response &res)
     {
         lock.lock();