diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp
index 1d6f04b..626c9cd 100644
--- a/src/costmap_2d_robot.cpp
+++ b/src/costmap_2d_robot.cpp
@@ -82,7 +82,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
   name_ = name;
   std::string config_file_name = "costmap_params.yaml";
   getParams(config_file_name, name_, nh);
-
+ 
   // create a thread to handle updating the map
   stop_updates_ = false;
   initialized_ = true;
@@ -332,21 +332,19 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
                                           const std::string& plugin_type, 
                                           robot::NodeHandle& nh)
 {
-  robot::NodeHandle priv_nh_1(nh, costmap_name);
-  robot::NodeHandle target_layer(priv_nh_1, plugin_name);
-  robot::NodeHandle priv_nh_2(nh, plugin_name);
-  // robot::log_error("TEST: %s",priv_nh_2.getNamespace().c_str());
-  
+  robot::NodeHandle costmap_nh(nh, costmap_name);
+  robot::NodeHandle costmap_plugin_nh(costmap_nh, plugin_name);
+  robot::NodeHandle plugin_nh(nh, plugin_name);
   if(plugin_type == "StaticLayer")
   {
     std::string map_topic;
     int unknown_cost_value;
     int lethal_cost_threshold;
     bool track_unknown_space;
-    move_parameter(priv_nh_2, target_layer, "map_topic", map_topic);
-    move_parameter(priv_nh_2, target_layer, "unknown_cost_value", unknown_cost_value);
-    move_parameter(priv_nh_2, target_layer, "lethal_cost_threshold", lethal_cost_threshold);
-    move_parameter(priv_nh_2, target_layer, "track_unknown_space", track_unknown_space);
+    move_parameter(plugin_nh, costmap_plugin_nh, "map_topic", map_topic);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_cost_value", unknown_cost_value);
+    move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
   }
   else if(plugin_type == "VoxelLayer")
   {
@@ -356,24 +354,23 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
     int mark_threshold;
     int unknown_threshold;
     bool publish_voxel_map;
-    move_parameter(priv_nh_2, target_layer, "origin_z", origin_z);
-    move_parameter(priv_nh_2, target_layer, "z_resolution", z_resolution);
-    move_parameter(priv_nh_2, target_layer, "z_voxels", z_voxels);
-    move_parameter(priv_nh_2, target_layer, "mark_threshold", mark_threshold);
-    move_parameter(priv_nh_2, target_layer, "unknown_threshold", unknown_threshold);
-    move_parameter(priv_nh_2, target_layer, "publish_voxel_map", publish_voxel_map);
-    if(priv_nh_2.hasParam("observation_sources"))
+    move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
+    move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
+    if(plugin_nh.hasParam("observation_sources"))
     {
       std::string topics_string;
-      move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string);
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
       robot::log_error("topics_string: %s", topics_string.c_str());
       std::stringstream ss(topics_string);
       std::string source;
       while (ss >> source)
       {
-        robot::NodeHandle priv_nh_3(priv_nh_2, source);
-        robot::NodeHandle target_layer_2(target_layer, source);
-        robot::log_warning("priv_nh_3: %s",priv_nh_3.getNamespace().c_str());
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
         std::string topic;
         std::string data_type;
         bool clearing;
@@ -381,15 +378,14 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
         bool inf_is_valid;
         double min_obstacle_height;
         double max_obstacle_height;
-        move_parameter(priv_nh_2, target_layer_2, "topic", topic);
-        robot::log_error("topic: %s", topic.c_str());
-        move_parameter(priv_nh_2, target_layer_2, "data_type", data_type);
-        move_parameter(priv_nh_2, target_layer_2, "clearing", clearing);
-        move_parameter(priv_nh_2, target_layer_2, "marking", marking);
-        move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid);
-        move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height);
-        move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height);
-        target_layer_2.printParams();
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
       }
     }
   }
@@ -399,21 +395,21 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
     double raytrace_range;
     double obstacle_range;
     bool track_unknown_space;
-    move_parameter(priv_nh_2, target_layer, "max_obstacle_height", max_obstacle_height);
-    move_parameter(priv_nh_2, target_layer, "raytrace_range", raytrace_range);
-    move_parameter(priv_nh_2, target_layer, "obstacle_range", obstacle_range);
-    move_parameter(priv_nh_2, target_layer, "track_unknown_space", track_unknown_space);
-    if(priv_nh_2.hasParam("observation_sources"))
+    move_parameter(plugin_nh, costmap_plugin_nh, "max_obstacle_height", max_obstacle_height);
+    move_parameter(plugin_nh, costmap_plugin_nh, "raytrace_range", raytrace_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "obstacle_range", obstacle_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
+    if(plugin_nh.hasParam("observation_sources"))
     {
       std::string topics_string;
-      move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string);
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
       robot::log_error("topics_string: %s", topics_string.c_str());
       std::stringstream ss(topics_string);
       std::string source;
       while (ss >> source)
       {
-        robot::NodeHandle priv_nh_3(priv_nh_2, source);
-        robot::NodeHandle target_layer_2(target_layer, source);
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
         std::string topic;
         std::string data_type;
         bool clearing;
@@ -421,13 +417,14 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
         bool inf_is_valid;
         double min_obstacle_height;
         double max_obstacle_height;
-        move_parameter(priv_nh_2, target_layer_2, "topic", topic);
-        move_parameter(priv_nh_2, target_layer_2, "data_type", data_type);
-        move_parameter(priv_nh_2, target_layer_2, "clearing", clearing);
-        move_parameter(priv_nh_2, target_layer_2, "marking", marking);
-        move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid);
-        move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height);
-        move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
       }
     }
 
@@ -436,8 +433,8 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
   {
     double cost_scaling_factor;
     double inflation_radius;
-    move_parameter(priv_nh_2, target_layer, "cost_scaling_factor", cost_scaling_factor);
-    move_parameter(priv_nh_2, target_layer, "inflation_radius", inflation_radius);
+    move_parameter(plugin_nh, costmap_plugin_nh, "cost_scaling_factor", cost_scaling_factor);
+    move_parameter(plugin_nh, costmap_plugin_nh, "inflation_radius", inflation_radius);
   }
 
 }