HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update plan when docking

Browse Source
This commit is contained in:
HiepLM
2026-03-22 17:19:37 +07:00
parent d0ad2d0e21
commit f7fa96ff8b
10 changed files with 134 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
src/Libraries/robot_cpp/src/node_handle.cpp
View File

@@ -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);
}
}