update load param using node handle

src/footprint.cpp

@@ -207,45 +207,26 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
   return true;
 }
 
-// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
-// {
-//   std::string full_param_name;
-//   std::string full_radius_param_name;
-//   std::vector<geometry_msgs::Point> points;
+std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
+{
+  YAML::Node ft = nh.getParamValue("footprint");
+  std::vector<geometry_msgs::Point> footprint;
 
-//   if (nh.searchParam("footprint", full_param_name))
-//   {
-//     XmlRpc::XmlRpcValue footprint_xmlrpc;
-//     nh.getParam(full_param_name, footprint_xmlrpc);
-//     if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
-//         footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
-//     {
-//       if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
-//       {
-//         writeFootprintToParam(nh, points);
-//         return points;
-//       }
-//     }
-//     else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
-//     {
-//       points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
-//       writeFootprintToParam(nh, points);
-//       return points;
-//     }
-//   }
-
-//   if (nh.searchParam("robot_radius", full_radius_param_name))
-//   {
-//     double robot_radius;
-//     nh.param(full_radius_param_name, robot_radius, 1.234);
-//     points = makeFootprintFromRadius(robot_radius);
-//     nh.setParam("robot_radius", robot_radius);
-//   }
-//   // Else neither param was found anywhere this knows about, so
-//   // defaults will come from dynamic_reconfigure stuff, set in
-//   // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
-//   return points;
-// }
+  if (ft && ft.IsSequence())
+  {
+    for (size_t i = 0; i < ft.size(); ++i)
+    {
+        auto pt = ft[i];  
+        geometry_msgs::Point p;
+        p.x = pt[0].as<double>();
+        p.y = pt[1].as<double>();
+        p.z = 0.0;
+        footprint.push_back(p);
+        std::cout << "Pose[" << i << "]" << p.x << ", " << p.y << std::endl;
+    }
+  }
+  return footprint;
+}
 
 // void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
 // {