update plan when docking

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -16,6 +16,70 @@
 #endif
 #include <robot_xmlrpcpp/XmlRpcValue.h>
 
+namespace
+{
+
+  static YAML::Node xmlRpcToYaml(robot_xmlrpcpp::XmlRpcValue v)
+  {
+    using robot_xmlrpcpp::XmlRpcValue;
+    switch (v.getType())
+    {
+    case XmlRpcValue::TypeBoolean:
+    {
+      bool b = v;
+      return YAML::Node(b);
+    }
+    case XmlRpcValue::TypeInt:
+    {
+      int i = v;
+      return YAML::Node(i);
+    }
+    case XmlRpcValue::TypeDouble:
+    {
+      double d = v;
+      return YAML::Node(d);
+    }
+    case XmlRpcValue::TypeString:
+    {
+      std::string s = v;
+      return YAML::Node(s);
+    }
+    case XmlRpcValue::TypeArray:
+    {
+      YAML::Node seq(YAML::NodeType::Sequence);
+      for (int i = 0; i < v.size(); ++i)
+        seq.push_back(xmlRpcToYaml(v[i]));
+      return seq;
+    }
+    case XmlRpcValue::TypeStruct:
+    {
+      YAML::Node map(YAML::NodeType::Map);
+      const XmlRpcValue::ValueStruct *members = v.getStructMembers();
+      if (members)
+      {
+        for (const auto &kv : *members)
+          map[kv.first] = xmlRpcToYaml(kv.second);
+      }
+      return map;
+    }
+    default:
+      return YAML::Node();
+    }
+  }
+
+  static YAML::NodeType::value yamlNodeCategory(const YAML::Node &n)
+  {
+    if (n.IsMap())
+      return YAML::NodeType::Map;
+    if (n.IsSequence())
+      return YAML::NodeType::Sequence;
+    if (n.IsScalar())
+      return YAML::NodeType::Scalar;
+    return YAML::NodeType::Null;
+  }
+
+} // namespace
+
 namespace robot
 {
 
@@ -1147,92 +1211,14 @@ namespace robot
 
   void NodeHandle::setParam(const std::string &key, const robot_xmlrpcpp::XmlRpcValue &v) const
   {
-    // Convert XmlRpcValue to YAML::Node
-    // Create non-const copy to use conversion operators
-    robot_xmlrpcpp::XmlRpcValue v_copy = v;
-    YAML::Node node;
     try
     {
-      switch (v.getType())
-      {
-      case robot_xmlrpcpp::XmlRpcValue::TypeBoolean:
-      {
-        bool b = v_copy;
-        node = YAML::Node(b);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeInt:
-      {
-        int i = v_copy;
-        node = YAML::Node(i);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeDouble:
-      {
-        double d = v_copy;
-        node = YAML::Node(d);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeString:
-      {
-        std::string s = v_copy;
-        node = YAML::Node(s);
-        const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeArray:
-        {
-        YAML::Node seq(YAML::NodeType::Sequence);
-        for (int i = 0; i < v_copy.size(); ++i)
-        {
-          YAML::Node item;
-          robot_xmlrpcpp::XmlRpcValue item_v = v_copy[i];
-          if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeBoolean)
-          {
-            bool b = item_v;
-            item = YAML::Node(b);
-          }
-          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt)
-          {
-            int i_val = item_v;
-            item = YAML::Node(i_val);
-          }
-          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeDouble)
-          {
-            double d = item_v;
-            item = YAML::Node(d);
-          }
-          else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeString)
-          {
-            std::string s = item_v;
-            item = YAML::Node(s);
-          }
-          seq.push_back(item);
-        }
-        const_cast<NodeHandle *>(this)->setParamInternal(key, seq, YAML::NodeType::Sequence);
-      }
-      break;
-      case robot_xmlrpcpp::XmlRpcValue::TypeStruct:
-      {
-        YAML::Node map_node(YAML::NodeType::Map);
-        // XmlRpcValue::TypeStruct doesn't have begin/end, need to use different approach
-        // We'll need to iterate through the struct differently
-        // For now, create empty map
-        const_cast<NodeHandle *>(this)->setParamInternal(key, map_node, YAML::NodeType::Map);
-      }
-      break;
-      default:
-        // Unknown type, create empty node
-        const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
-        break;
-      }
+      robot_xmlrpcpp::XmlRpcValue v_copy = v;
+      YAML::Node node = xmlRpcToYaml(v_copy);
+      const_cast<NodeHandle *>(this)->setParamInternal(key, node, yamlNodeCategory(node));
     }
     catch (...)
     {
-      // On error, create empty node
       const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
     }
   }