update

src/Libraries/nav_2d_utils/CMakeLists.txt

@@ -32,6 +32,8 @@ target_link_libraries(conversions
         nav_msgs
         nav_grid
         nav_core2
+        robot::node_handle
+        robot::console
     PRIVATE
         console_bridge::console_bridge
         Boost::system
@@ -48,6 +50,8 @@ target_link_libraries(path_ops
     PUBLIC
         nav_2d_msgs
         geometry_msgs
+        robot::node_handle
+        robot::console
 )
 
 add_library(polygons src/polygons.cpp src/footprint.cpp)
@@ -60,6 +64,8 @@ target_link_libraries(polygons
     PUBLIC
         nav_2d_msgs
         geometry_msgs
+        robot::node_handle
+        robot::console
     PRIVATE
         ${xmlrpcpp_LIBRARIES}
 )
@@ -81,6 +87,8 @@ target_link_libraries(bounds
     PUBLIC
         nav_grid
         nav_core2
+        robot::node_handle
+        robot::console
 )
 
 add_library(tf_help src/tf_help.cpp)
@@ -95,6 +103,8 @@ target_link_libraries(tf_help
         geometry_msgs
         nav_core2
         tf3
+        robot::node_handle
+        robot::console
 )
 
 # Create an INTERFACE library that represents all nav_2d_utils libraries
@@ -111,6 +121,8 @@ target_link_libraries(nav_2d_utils
         polygons
         bounds
         tf_help
+        robot::node_handle
+        robot::console
 )
 
 # Install header files

src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h

@@ -35,7 +35,7 @@
 #ifndef NAV_2D_UTILS_FOOTPRINT_H
 #define NAV_2D_UTILS_FOOTPRINT_H
 
-#include <yaml-cpp/yaml.h>
+#include <robot/node_handle.h>
 #include <nav_2d_msgs/Polygon2D.h>
 
 namespace nav_2d_utils
@@ -47,7 +47,7 @@ namespace nav_2d_utils
  * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
  * is present.
  */
-nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true);
+nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle& nh, bool write = true);
 
 }  // namespace nav_2d_utils
 

src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h

@@ -41,7 +41,7 @@
 #include <boost/thread/mutex.hpp>
 #include <string>
 #include <iostream>
-#include <yaml-cpp/yaml.h>
+#include <robot/node_handle.h>
 
 namespace nav_2d_utils
 {
@@ -59,7 +59,7 @@ public:
    * @param nh NodeHandle for creating subscriber
    * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
    */
-  explicit OdomSubscriber(YAML::Node& nh, std::string default_topic = "odom")
+  explicit OdomSubscriber(robot::NodeHandle& nh, std::string default_topic = "odom")
   {
     std::string odom_topic;
     // nh.param("odom_topic", odom_topic, default_topic);

src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h

@@ -52,7 +52,7 @@ namespace nav_2d_utils
  * @return Value of parameter if found, otherwise the default_value
  */
 template<class param_t>
-param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, const param_t& default_value)
+param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value)
 {
   std::string resolved_name;
   // if (nh.searchParam(param_name, resolved_name))
@@ -73,19 +73,19 @@ param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, c
  * @return The value of the parameter or the default value
  */
 template<class param_t>
-param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string current_name,
+param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::string current_name,
                                      const std::string old_name, const param_t& default_value)
 {
   param_t value;
-  if (nh[current_name] && nh[current_name].IsDefined())
+  if (nh.hasParam(current_name))
   {
-    value = nh[current_name].as<param_t>();
+    nh.getParam(current_name, value, default_value);
     return value;
   }
-  if (nh[old_name] && nh[old_name].IsDefined())
+  if (nh.hasParam(old_name))
   {
     std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
-    value = nh[old_name].as<param_t>();
+    nh.getParam(old_name, value, default_value);
     return value;
   }
   return default_value;
@@ -98,14 +98,14 @@ param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string cur
  * @param old_name Deprecated parameter name
  */
 template<class param_t>
-void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_name, const std::string old_name)
+void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string current_name, const std::string old_name)
 {
-  if (!nh[old_name] || !nh[old_name].IsDefined()) return;
+  if (!nh.hasParam(old_name)) return;
 
   param_t value;
   std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
-  value = nh[old_name].as<param_t>();
-  nh[current_name] = value;
+  value = nh.param<param_t>(old_name);
+  nh.setParam(current_name, value);
 }
 
 /**
@@ -122,7 +122,7 @@ void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_nam
  * @param should_delete If true, whether to delete the parameter from the old name
  */
 template<class param_t>
-void moveParameter(const YAML::Node& nh, std::string old_name,
+void moveParameter(const robot::NodeHandle& nh, std::string old_name,
                    std::string current_name, param_t default_value, bool should_delete = true)
 {
   // if (nh.hasParam(current_name))

src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h

@@ -207,7 +207,7 @@ protected:
   // ROS Interface
   ros::ServiceServer switch_plugin_srv_;
   ros::Publisher current_plugin_pub_;
-  YAML::Node private_nh_;
+  robot::NodeHandle private_nh_;
   std::string ros_name_;
 
   // Switch Callback

src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h

@@ -89,7 +89,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
 //  * @param search Whether to search up the namespace for the parameter name
 //  * @return Loaded polygon
 //  */
-// nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name,
+// nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name,
 //                                          bool search = true);
 
 /**
@@ -116,7 +116,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool
 //  * @param parameter_name Name of the parameter
 //  * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
 //  */
-// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
+// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const robot::NodeHandle& nh, const std::string parameter_name,
 //                      bool array_of_arrays = true);
 
 /**