2077 lines
56 KiB
C++
2077 lines
56 KiB
C++
#include <robot/node_handle.h>
|
|
#include <robot/console.h>
|
|
#include <sstream>
|
|
#include <algorithm>
|
|
#include <cctype>
|
|
#include <fstream>
|
|
#include <filesystem>
|
|
#include <functional>
|
|
#include <iostream>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <unistd.h>
|
|
#include <limits.h>
|
|
#ifdef __linux__
|
|
#include <linux/limits.h>
|
|
#endif
|
|
#include <robot_xmlrpcpp/XmlRpcValue.h>
|
|
|
|
namespace robot
|
|
{
|
|
|
|
NodeHandle::~NodeHandle() {}
|
|
|
|
// Constructor with namespace and optional remappings
|
|
NodeHandle::NodeHandle(const std::string &ns, const M_string &remappings)
|
|
{
|
|
// Apply remappings if provided
|
|
(void)remappings; // Suppress unused parameter warning for now
|
|
|
|
// Step 1: If root_ is not defined, load config files into root_
|
|
if (!root_.IsDefined() || !root_.IsMap() || root_.size() == 0)
|
|
{
|
|
// Find and set config directory first
|
|
if (config_directory_.empty())
|
|
{
|
|
std::string config_dir = findConfigDirectory();
|
|
if (!config_dir.empty())
|
|
{
|
|
config_directory_ = config_dir;
|
|
}
|
|
}
|
|
|
|
// Initialize root_ as empty map
|
|
root_ = YAML::Node(YAML::NodeType::Map);
|
|
|
|
// Auto-load config files and merge into root_
|
|
if (!config_directory_.empty() && std::filesystem::exists(config_directory_) && std::filesystem::is_directory(config_directory_))
|
|
{
|
|
int loaded_count = loadYamlFilesFromDirectory(config_directory_);
|
|
if (loaded_count > 0)
|
|
{
|
|
std::cout << "[NodeHandle] Auto-loaded " << loaded_count
|
|
<< " YAML file(s) from: " << config_directory_ << std::endl;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Step 2: Update node_handle_ from root_ based on namespace_
|
|
// Normalize namespace: strip leading '/' if present
|
|
std::string ns_normalized = ns;
|
|
if (!ns_normalized.empty() && ns_normalized.front() == '/')
|
|
{
|
|
ns_normalized = ns_normalized.substr(1);
|
|
}
|
|
if (ns_normalized == "~" || ns_normalized.empty())
|
|
{
|
|
// If namespace is root, node_handle_ should be the full root_ (direct reference)
|
|
// node_handle_ = root_;
|
|
namespace_ = "/";
|
|
}
|
|
else
|
|
{
|
|
// Try to scope node_handle_ to namespace_ from root_
|
|
std::stringstream ns_ss(ns_normalized);
|
|
std::string ns_segment;
|
|
std::vector<std::string> segs;
|
|
|
|
while (std::getline(ns_ss, ns_segment, '/'))
|
|
{
|
|
if (!ns_segment.empty())
|
|
{
|
|
segs.push_back(ns_segment);
|
|
}
|
|
}
|
|
if (!segs.empty())
|
|
{
|
|
std::vector<YAML::Node> curs;
|
|
curs.push_back(root_);
|
|
bool ns_found = true;
|
|
for (int i = 0; i < segs.size() - 1; i++)
|
|
{
|
|
const auto &k = segs[i];
|
|
if (k.empty())
|
|
continue;
|
|
if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
|
|
{
|
|
ns_found = false;
|
|
break;
|
|
}
|
|
curs.push_back(curs.back()[k]);
|
|
}
|
|
if (ns_found && curs.back().IsDefined())
|
|
{
|
|
// Scope node_handle_ to the found namespace (direct reference, syncs with root_)
|
|
// node_handle_ = curs.back();
|
|
namespace_ = ns_normalized;
|
|
}
|
|
else
|
|
{
|
|
// Namespace not found: use root_ as fallback
|
|
// node_handle_ = root_;
|
|
namespace_ = "/"; // indicate root when ns not found
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Namespace not found: use root_ as fallback
|
|
// node_handle_ = root_;
|
|
namespace_ = "/"; // indicate root when ns not found
|
|
}
|
|
}
|
|
}
|
|
|
|
// Constructor from parent NodeHandle with new namespace (2 parameters - delegates to 3 parameter version)
|
|
// This is needed to ensure the symbol is exported correctly in the shared library
|
|
NodeHandle::NodeHandle(const NodeHandle &parent, const std::string &ns)
|
|
: NodeHandle(parent, ns, M_string())
|
|
{
|
|
}
|
|
|
|
// Constructor from parent NodeHandle with new namespace and remappings
|
|
NodeHandle::NodeHandle(const NodeHandle &parent, const std::string &ns, const M_string &remappings)
|
|
{
|
|
// Apply remappings if provided
|
|
(void)remappings; // Suppress unused parameter warning for now
|
|
|
|
// Step 1: If root_ is not defined, load config files into root_
|
|
if (!root_.IsDefined() || !root_.IsMap() || root_.size() == 0)
|
|
{
|
|
// Find and set config directory first
|
|
if (config_directory_.empty())
|
|
{
|
|
std::string config_dir = findConfigDirectory();
|
|
if (!config_dir.empty())
|
|
{
|
|
config_directory_ = config_dir;
|
|
}
|
|
}
|
|
|
|
// Initialize root_ as empty map
|
|
root_ = YAML::Node(YAML::NodeType::Map);
|
|
log_warning("Root is not defined, initializing root_ as empty map");
|
|
|
|
// Auto-load config files and merge into root_
|
|
if (!config_directory_.empty() && std::filesystem::exists(config_directory_) && std::filesystem::is_directory(config_directory_))
|
|
{
|
|
int loaded_count = loadYamlFilesFromDirectory(config_directory_);
|
|
if (loaded_count > 0)
|
|
{
|
|
std::cout << "[NodeHandle] Auto-loaded " << loaded_count
|
|
<< " YAML file(s) from: " << config_directory_ << std::endl;
|
|
}
|
|
}
|
|
}
|
|
// Normalize child namespace (remove leading '/')
|
|
std::string child_ns = ns;
|
|
while (!child_ns.empty() && child_ns.front() == '/')
|
|
{
|
|
child_ns.erase(child_ns.begin());
|
|
}
|
|
std::string parent_ns = parent.getNamespace();
|
|
while (!parent_ns.empty() && parent_ns.front() == '/')
|
|
{
|
|
parent_ns.erase(parent_ns.begin());
|
|
}
|
|
std::string ns_normalized = parent_ns + "/" + child_ns;
|
|
if (ns_normalized != "~" && !ns_normalized.empty())
|
|
{
|
|
// Try to scope node_handle_ to namespace_ from root_
|
|
std::stringstream ns_ss(ns_normalized);
|
|
std::string ns_segment;
|
|
std::vector<std::string> segs;
|
|
while (std::getline(ns_ss, ns_segment, '/'))
|
|
{
|
|
if (!ns_segment.empty())
|
|
{
|
|
segs.push_back(ns_segment);
|
|
}
|
|
}
|
|
if (!segs.empty())
|
|
{
|
|
std::vector<YAML::Node> curs = {root_};
|
|
bool ns_found = true;
|
|
for (int i = 0; i < segs.size() - 1; i++)
|
|
{
|
|
const auto k = segs[i];
|
|
if (k.empty())
|
|
continue;
|
|
if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
|
|
{
|
|
robot::log_warning_at(__FILE__, __LINE__, "%s %x %x", k.c_str(), curs.back()[k].IsDefined(), curs.back()[k].IsMap());
|
|
ns_found = false;
|
|
break;
|
|
}
|
|
curs.push_back(curs.back()[k]);
|
|
}
|
|
|
|
if (ns_found && curs.back().IsDefined() && curs.size() >= 1)
|
|
{
|
|
// Scope node_handle_ to the found namespace (direct reference, syncs with root_)
|
|
// node_handle_ = curs.back()[segs.back()];
|
|
namespace_ = ns_normalized;
|
|
}
|
|
else
|
|
{
|
|
// Namespace not found: use root_ as fallback
|
|
// node_handle_ = parent.getNodeHandle();
|
|
namespace_ = parent.getNamespace(); // indicate root when ns not found
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Copy constructor
|
|
NodeHandle::NodeHandle(const NodeHandle &parent)
|
|
{
|
|
this->namespace_ = parent.namespace_;
|
|
if(!namespace_.empty() && namespace_ != "/")
|
|
{
|
|
// Try to scope node_handle_ to namespace_ from root_
|
|
std::stringstream ns_ss(namespace_);
|
|
std::string ns_segment;
|
|
std::vector<std::string> segs;
|
|
while (std::getline(ns_ss, ns_segment, '/'))
|
|
{
|
|
if (!ns_segment.empty())
|
|
{
|
|
segs.push_back(ns_segment);
|
|
}
|
|
}
|
|
if (!segs.empty())
|
|
{
|
|
std::vector<YAML::Node> curs;
|
|
curs.push_back(parent.root_);
|
|
bool ns_found = true;
|
|
for (int i = 0; i < segs.size() - 1; i++)
|
|
{
|
|
|
|
const auto &k = segs[i];
|
|
if (k.empty())
|
|
continue;
|
|
if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
|
|
{
|
|
ns_found = false;
|
|
break;
|
|
}
|
|
curs.push_back(curs.back()[k]);
|
|
}
|
|
if (ns_found && curs.back().IsDefined() && curs.size() >= 1)
|
|
{
|
|
// Scope node_handle_ to the found namespace (direct reference, syncs with root_)
|
|
// node_handle_[segs.back()] = curs.back()[segs.back()];
|
|
}
|
|
else
|
|
{
|
|
// Namespace not found: use root_ as fallback
|
|
// node_handle_ = parent.getNodeHandle();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Assignment operator
|
|
NodeHandle &NodeHandle::operator=(const NodeHandle &parent)
|
|
{
|
|
if (this != &parent)
|
|
{
|
|
namespace_ = parent.getNamespace();
|
|
// node_handle_ = parent.getNodeHandle();
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
bool NodeHandle::hasParam(const std::string &key) const
|
|
{
|
|
// Safety check: validate key parameter
|
|
if (key.empty())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
std::string full_key = namespace_ + "/" + key;
|
|
if (!root_.IsDefined() || !root_.IsMap())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Split key by '/' to handle nested keys
|
|
std::stringstream ss(full_key);
|
|
std::string segment;
|
|
std::vector<std::string> segments;
|
|
|
|
while (std::getline(ss, segment, '/'))
|
|
{
|
|
if (!segment.empty())
|
|
{
|
|
segments.push_back(segment);
|
|
}
|
|
}
|
|
|
|
if (segments.empty())
|
|
{
|
|
// Empty key refers to the whole tree; treat as existing only if map is non-empty
|
|
return false;
|
|
}
|
|
|
|
const YAML::Node *current = &root_;
|
|
|
|
for (size_t i = 0; i < segments.size(); ++i)
|
|
{
|
|
// Safety check: ensure current is valid
|
|
if (!current || !current->IsDefined())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (current->IsMap())
|
|
{
|
|
// Search for the segment without inserting default nodes
|
|
const YAML::Node *found_child = nullptr;
|
|
|
|
try
|
|
{
|
|
for (YAML::const_iterator it = current->begin(); it != current->end(); ++it)
|
|
{
|
|
try
|
|
{
|
|
// Safety check: validate iterator nodes
|
|
if (!it->first.IsDefined())
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Safely convert key to string
|
|
std::string key_str;
|
|
try
|
|
{
|
|
key_str = it->first.as<std::string>();
|
|
}
|
|
catch (...)
|
|
{
|
|
// Skip nodes that can't be converted to string
|
|
continue;
|
|
}
|
|
|
|
if (key_str == segments[i])
|
|
{
|
|
// Safety check: validate value node
|
|
if (it->second.IsDefined())
|
|
{
|
|
found_child = &(it->second);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
// Skip invalid nodes and continue iteration
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
// If iteration fails, return false
|
|
return false;
|
|
}
|
|
|
|
if (!found_child || !found_child->IsDefined())
|
|
{
|
|
return false;
|
|
}
|
|
current = found_child;
|
|
continue;
|
|
}
|
|
|
|
if (current->IsSequence())
|
|
{
|
|
// For sequences, interpret segment as index if possible
|
|
try
|
|
{
|
|
size_t idx = static_cast<size_t>(std::stoul(segments[i]));
|
|
if (idx >= current->size())
|
|
{
|
|
return false;
|
|
}
|
|
const YAML::Node &child = (*current)[idx];
|
|
if (!child.IsDefined())
|
|
{
|
|
return false;
|
|
}
|
|
current = &child;
|
|
continue;
|
|
}
|
|
catch (...)
|
|
{
|
|
// Segment is not an index
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Neither map nor sequence: cannot traverse further
|
|
return false;
|
|
}
|
|
|
|
// Safety check: ensure current is valid before returning
|
|
return current && current->IsDefined();
|
|
}
|
|
|
|
YAML::Node NodeHandle::getNestedValue(const std::string &key) const
|
|
{
|
|
// Safety check: validate key parameter
|
|
if (key.empty())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Safety check: validate root node
|
|
if (!root_.IsDefined() || !root_.IsMap())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Split key by '/' to handle nested keys
|
|
std::stringstream ss(namespace_ + "/" + key);
|
|
std::string segment;
|
|
std::vector<std::string> segments;
|
|
|
|
while (std::getline(ss, segment, '/'))
|
|
{
|
|
if (!segment.empty())
|
|
{
|
|
segments.push_back(segment);
|
|
}
|
|
}
|
|
|
|
if (segments.empty())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Traverse the tree without modifying node_handle_
|
|
// Use const reference to the original node_handle_ to avoid any modifications
|
|
const YAML::Node *current = &root_;
|
|
|
|
for (size_t i = 0; i < segments.size(); ++i)
|
|
{
|
|
// Safety check: ensure current is valid
|
|
if (!current || !current->IsDefined() || !current->IsMap())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Access child node without mutating the map (avoid operator[] default insertion)
|
|
const YAML::Node *found_child = nullptr;
|
|
|
|
try
|
|
{
|
|
for (YAML::const_iterator it = current->begin(); it != current->end(); ++it)
|
|
{
|
|
try
|
|
{
|
|
// Safety check: validate iterator nodes
|
|
if (!it->first.IsDefined())
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Safely convert key to string
|
|
std::string key_str;
|
|
try
|
|
{
|
|
key_str = it->first.as<std::string>();
|
|
}
|
|
catch (...)
|
|
{
|
|
// Skip nodes that can't be converted to string
|
|
continue;
|
|
}
|
|
|
|
if (key_str == segments[i])
|
|
{
|
|
// Safety check: validate value node
|
|
if (it->second.IsDefined())
|
|
{
|
|
found_child = &(it->second);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
// Skip invalid nodes and continue iteration
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
// If iteration fails, return empty node
|
|
return YAML::Node();
|
|
}
|
|
|
|
if (!found_child || !found_child->IsDefined())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Move to next level
|
|
current = found_child;
|
|
}
|
|
|
|
// Safety check: ensure current is valid before cloning
|
|
if (!current || !current->IsDefined())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Return a clone of the final node to ensure it's independent
|
|
try
|
|
{
|
|
return YAML::Clone(*current);
|
|
}
|
|
catch (...)
|
|
{
|
|
// If cloning fails, return empty node
|
|
return YAML::Node();
|
|
}
|
|
}
|
|
|
|
YAML::Node NodeHandle::getNestedValueDebug(const std::string &key) const
|
|
{
|
|
std::string full_key = namespace_ + "/" + key;
|
|
if (!root_.IsDefined() || !root_.IsMap())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Split key by '/' to handle nested keys
|
|
std::stringstream ss(key);
|
|
std::string segment;
|
|
std::vector<std::string> segments;
|
|
|
|
while (std::getline(ss, segment, '/'))
|
|
{
|
|
if (!segment.empty())
|
|
{
|
|
segments.push_back(segment);
|
|
}
|
|
}
|
|
|
|
if (segments.empty())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
const YAML::Node *current = &root_;
|
|
|
|
for (size_t i = 0; i < segments.size(); ++i)
|
|
{
|
|
if (!current->IsMap())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
// Access child node without mutating the map (avoid operator[] default insertion)
|
|
YAML::Node segment_node;
|
|
bool found_child = false;
|
|
for (auto it = current->begin(); it != current->end(); ++it)
|
|
{
|
|
if (it->first.as<std::string>() == segments[i])
|
|
{
|
|
segment_node = it->second;
|
|
found_child = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found_child)
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
if (!segment_node.IsDefined())
|
|
{
|
|
return YAML::Node();
|
|
}
|
|
|
|
current = &segment_node;
|
|
}
|
|
return YAML::Clone(*current);
|
|
}
|
|
|
|
YAML::Node NodeHandle::getParamValue(const std::string &key) const
|
|
{
|
|
return getNestedValue(key);
|
|
}
|
|
|
|
std::string NodeHandle::getNamespace() const
|
|
{
|
|
// namespace_ is already set to config directory path if name was "~" or ""
|
|
return namespace_;
|
|
}
|
|
|
|
void NodeHandle::printParams() const
|
|
{
|
|
log_info("\n[NodeHandle] ========== Root Parameters ==========");
|
|
log_info("Namespace: %s", this->getNamespace().c_str());
|
|
printParams(root_);
|
|
}
|
|
|
|
void NodeHandle::printParams(YAML::Node node) const
|
|
{
|
|
if (!node.IsDefined())
|
|
{
|
|
std::cout << "[NodeHandle] Node is not defined!" << std::endl;
|
|
return;
|
|
}
|
|
|
|
if (!node.IsMap())
|
|
{
|
|
std::cout << "[NodeHandle] Node is not a map!" << std::endl;
|
|
return;
|
|
}
|
|
|
|
std::cout << "[NodeHandle] Total top-level keys: " << node.size() << std::endl;
|
|
std::cout << std::endl;
|
|
|
|
// Helper lambda to print nested structure with indentation
|
|
std::function<void(const YAML::Node &, const std::string &, int)> printNode =
|
|
[&printNode](const YAML::Node &node, const std::string &prefix, int depth)
|
|
{
|
|
if (depth > 10)
|
|
return; // Limit depth to avoid infinite recursion
|
|
|
|
std::string indent(depth * 2, ' '); // 2 spaces per depth level
|
|
|
|
if (node.IsMap())
|
|
{
|
|
for (auto it = node.begin(); it != node.end(); ++it)
|
|
{
|
|
std::string key = it->first.as<std::string>();
|
|
const YAML::Node &value = it->second;
|
|
std::string full_key = prefix.empty() ? key : prefix + "/" + key;
|
|
if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos)
|
|
continue;
|
|
if (value.IsMap())
|
|
{
|
|
std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl;
|
|
printNode(value, full_key, depth + 1);
|
|
}
|
|
else if (value.IsSequence())
|
|
{
|
|
std::cout << "[NodeHandle] " << indent << full_key << " (sequence, " << value.size() << " items):" << std::endl;
|
|
// Print sequence items
|
|
for (size_t i = 0; i < value.size(); ++i)
|
|
{
|
|
const YAML::Node &item = value[i];
|
|
if (item.IsMap())
|
|
{
|
|
std::cout << "[NodeHandle] " << indent << " [" << i << "]:" << std::endl;
|
|
printNode(item, full_key + "[" + std::to_string(i) + "]", depth + 2);
|
|
}
|
|
else if (item.IsScalar())
|
|
{
|
|
std::cout << "[NodeHandle] " << indent << " [" << i << "] = " << item.as<std::string>() << std::endl;
|
|
}
|
|
}
|
|
}
|
|
else if (value.IsScalar())
|
|
{
|
|
try
|
|
{
|
|
std::cout << "[NodeHandle] " << indent << full_key << " = " << value.as<std::string>() << std::endl;
|
|
}
|
|
catch (...)
|
|
{
|
|
std::cout << "[NodeHandle] " << indent << full_key << " = (unable to convert to string)" << std::endl;
|
|
}
|
|
}
|
|
else if (value.IsNull())
|
|
{
|
|
std::cout << "[NodeHandle] " << indent << full_key << " = null" << std::endl;
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
printNode(node, "", 0);
|
|
std::cout << "[NodeHandle] ================================================\n"
|
|
<< std::endl;
|
|
}
|
|
|
|
void NodeHandle::mergeYamlNode(const YAML::Node &source, YAML::Node &output)
|
|
{
|
|
if (!source.IsDefined())
|
|
{
|
|
return;
|
|
}
|
|
|
|
// If source is not a map, just replace node_handle_
|
|
if (!source.IsMap())
|
|
{
|
|
output = source;
|
|
return;
|
|
}
|
|
|
|
// Initialize node_handle_ as a map if it's not already
|
|
if (!output.IsDefined() || !output.IsMap())
|
|
{
|
|
output = YAML::Node(YAML::NodeType::Map);
|
|
}
|
|
|
|
// Helper lambda to recursively merge two YAML nodes
|
|
// Note: yaml-cpp's operator[] returns YAML::Node (not reference), so we need to work with copies
|
|
std::function<void(YAML::Node &, const YAML::Node &)> mergeRecursive =
|
|
[&mergeRecursive](YAML::Node &dest, const YAML::Node &src)
|
|
{
|
|
if (!src.IsMap())
|
|
{
|
|
dest = src;
|
|
return;
|
|
}
|
|
|
|
// Ensure dest is a map
|
|
if (!dest.IsMap())
|
|
{
|
|
dest = YAML::Node(YAML::NodeType::Map);
|
|
}
|
|
|
|
for (auto it = src.begin(); it != src.end(); ++it)
|
|
{
|
|
std::string key = it->first.as<std::string>();
|
|
const YAML::Node &src_value = it->second;
|
|
|
|
// Get current value in dest (if exists)
|
|
YAML::Node dest_current = dest[key];
|
|
|
|
// Check if key exists in dest and if both are maps
|
|
if (dest_current.IsDefined() && dest_current.IsMap() && src_value.IsMap())
|
|
{
|
|
// Both are maps, recursively merge
|
|
// Create a copy of dest[key] to work with, then assign back
|
|
YAML::Node dest_value = dest_current;
|
|
mergeRecursive(dest_value, src_value);
|
|
dest[key] = dest_value;
|
|
}
|
|
else
|
|
{
|
|
// Overwrite with source value (or add if doesn't exist)
|
|
dest[key] = src_value;
|
|
}
|
|
}
|
|
};
|
|
|
|
// Merge source into output
|
|
mergeRecursive(output, source);
|
|
}
|
|
|
|
int NodeHandle::loadYamlFilesFromDirectory(const std::string &directory_path)
|
|
{
|
|
int loaded_count = 0;
|
|
|
|
try
|
|
{
|
|
// Check if directory exists
|
|
if (!std::filesystem::exists(directory_path) || !std::filesystem::is_directory(directory_path))
|
|
{
|
|
std::cerr << "Directory does not exist or is not a directory: " << directory_path << std::endl;
|
|
return 0;
|
|
}
|
|
|
|
// Iterate through all files in the directory
|
|
for (const auto &entry : std::filesystem::directory_iterator(directory_path))
|
|
{
|
|
if (entry.is_regular_file())
|
|
{
|
|
std::string file_path = entry.path().string();
|
|
std::string extension = entry.path().extension().string();
|
|
|
|
// Only process .yaml and .yml files
|
|
if (extension == ".yaml" || extension == ".yml")
|
|
{
|
|
try
|
|
{
|
|
YAML::Node file_node = YAML::LoadFile(file_path);
|
|
if (file_node.IsDefined())
|
|
{
|
|
mergeYamlNode(file_node, root_);
|
|
loaded_count++;
|
|
}
|
|
}
|
|
catch (const YAML::Exception &e)
|
|
{
|
|
std::cerr << "Error loading YAML file " << file_path << ": " << e.what() << std::endl;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
std::cerr << "Error reading directory " << directory_path << ": " << e.what() << std::endl;
|
|
}
|
|
|
|
return loaded_count;
|
|
}
|
|
|
|
std::string NodeHandle::findConfigDirectory()
|
|
{
|
|
// If config directory is already set, use it
|
|
if (!config_directory_.empty() && std::filesystem::exists(config_directory_))
|
|
{
|
|
return config_directory_;
|
|
}
|
|
|
|
// Try environment variable first
|
|
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
|
|
if (env_config && std::filesystem::exists(env_config))
|
|
{
|
|
config_directory_ = std::string(env_config);
|
|
return config_directory_;
|
|
}
|
|
|
|
// Try workspace path + /config
|
|
const char *workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
|
|
if (workspace_path)
|
|
{
|
|
std::string config_path = std::string(workspace_path) + "/config";
|
|
if (std::filesystem::exists(config_path))
|
|
{
|
|
config_directory_ = config_path;
|
|
return config_directory_;
|
|
}
|
|
}
|
|
|
|
// Try to find config relative to executable path
|
|
try
|
|
{
|
|
char exe_path[PATH_MAX];
|
|
ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX);
|
|
if (count != -1)
|
|
{
|
|
exe_path[count] = '\0';
|
|
std::filesystem::path exe_dir = std::filesystem::path(exe_path).parent_path();
|
|
|
|
// Try paths relative to executable: ../../config, ../config, etc.
|
|
std::vector<std::filesystem::path> relative_paths = {
|
|
exe_dir / "../../config",
|
|
exe_dir / "../config",
|
|
exe_dir / "config",
|
|
exe_dir.parent_path() / "config",
|
|
exe_dir.parent_path().parent_path() / "config"};
|
|
|
|
for (const auto &rel_path : relative_paths)
|
|
{
|
|
if (std::filesystem::exists(rel_path) && std::filesystem::is_directory(rel_path))
|
|
{
|
|
config_directory_ = std::filesystem::canonical(rel_path).string();
|
|
return config_directory_;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (const std::exception &)
|
|
{
|
|
// Ignore errors when trying to find executable path
|
|
}
|
|
|
|
// Try hardcoded fallback paths
|
|
std::vector<std::string> possible_paths = {
|
|
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/config",
|
|
"../config",
|
|
"../../config",
|
|
"./config"};
|
|
|
|
for (const auto &path : possible_paths)
|
|
{
|
|
if (std::filesystem::exists(path) && std::filesystem::is_directory(path))
|
|
{
|
|
try
|
|
{
|
|
config_directory_ = std::filesystem::canonical(path).string();
|
|
return config_directory_;
|
|
}
|
|
catch (const std::exception &)
|
|
{
|
|
// If canonical fails, try the path as-is
|
|
config_directory_ = path;
|
|
return config_directory_;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Return empty string if not found
|
|
return "";
|
|
}
|
|
|
|
void NodeHandle::autoLoadConfigFiles()
|
|
{
|
|
// Auto-load if config_directory_ is set (regardless of namespace)
|
|
// This ensures root_ always contains the full merged YAML tree
|
|
if (config_directory_.empty())
|
|
{
|
|
// Config directory not set, can't auto-load
|
|
return;
|
|
}
|
|
|
|
if (!std::filesystem::exists(config_directory_) || !std::filesystem::is_directory(config_directory_))
|
|
{
|
|
// Config directory doesn't exist, can't auto-load
|
|
return;
|
|
}
|
|
|
|
std::string config_dir_to_load = config_directory_;
|
|
|
|
// Load all YAML files from config directory
|
|
int loaded_count = loadYamlFilesFromDirectory(config_dir_to_load);
|
|
if (loaded_count <= 0)
|
|
{
|
|
std::cout << "[NodeHandle] No YAML files found or loaded from: " << config_dir_to_load << std::endl;
|
|
}
|
|
}
|
|
|
|
void NodeHandle::setParamInternal(const std::string &key, const YAML::Node &value, YAML::NodeType::value expected_type)
|
|
{
|
|
YAML::Node existing = getNestedValue(key);
|
|
|
|
// If key doesn't exist, create it
|
|
if (!existing.IsDefined())
|
|
{
|
|
setParam(key, value);
|
|
return;
|
|
}
|
|
// If key exists, check type compatibility
|
|
// For scalar types, we allow conversion between compatible types
|
|
if (expected_type == YAML::NodeType::Scalar && existing.IsScalar())
|
|
{
|
|
// Scalar types are compatible, just update the value
|
|
setParam(key, value);
|
|
}
|
|
else if (existing.Type() == expected_type)
|
|
{
|
|
// Same type, update value
|
|
setParam(key, value);
|
|
}
|
|
else
|
|
{
|
|
// Type mismatch - overwrite with new type (as per requirement)
|
|
setParam(key, value);
|
|
}
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const YAML::Node &value)
|
|
{
|
|
// Split key by '/' to handle nested keys
|
|
std::stringstream ss(namespace_ + "/" + key);
|
|
std::string segment;
|
|
std::vector<std::string> segments;
|
|
|
|
while (std::getline(ss, segment, '/'))
|
|
{
|
|
if (!segment.empty())
|
|
{
|
|
segments.push_back(segment);
|
|
}
|
|
}
|
|
|
|
if (segments.empty())
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Step 1: Calculate full key path in root_ (namespace_ + "/" + key)
|
|
std::string full_key = key;
|
|
if (!namespace_.empty() && namespace_ != "/")
|
|
{
|
|
if (full_key.empty())
|
|
{
|
|
full_key = namespace_;
|
|
}
|
|
else
|
|
{
|
|
full_key = namespace_ + "/" + full_key;
|
|
}
|
|
}
|
|
|
|
std::stringstream s(full_key);
|
|
std::string seg;
|
|
std::vector<std::string> segs;
|
|
while (std::getline(s, seg, '/'))
|
|
{
|
|
if (!seg.empty())
|
|
segs.push_back(seg);
|
|
}
|
|
|
|
// Step 2: Update root_ with the full_key (preserve existing data)
|
|
std::vector<YAML::Node> curs = {root_};
|
|
if (!segs.empty())
|
|
{
|
|
for (size_t i = 0; i < segs.size() - 1; ++i)
|
|
{
|
|
const auto &k = segs[i];
|
|
if (!curs.back()[k].IsDefined() || !curs.back()[k].IsMap())
|
|
{
|
|
curs.back()[k] = YAML::Node(YAML::NodeType::Map);
|
|
}
|
|
curs.push_back(curs.back()[k]);
|
|
}
|
|
curs.back()[segs.back()] = value;
|
|
}
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, bool b) const
|
|
{
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(b), YAML::NodeType::Scalar);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const char *s) const
|
|
{
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(std::string(s)), YAML::NodeType::Scalar);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::string &s) const
|
|
{
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(s), YAML::NodeType::Scalar);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, int i) const
|
|
{
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(i), YAML::NodeType::Scalar);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, double d) const
|
|
{
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(d), YAML::NodeType::Scalar);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::vector<bool> &vec) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Sequence);
|
|
for (bool b : vec)
|
|
{
|
|
node.push_back(YAML::Node(b));
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::vector<int> &vec) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Sequence);
|
|
for (int i : vec)
|
|
{
|
|
node.push_back(YAML::Node(i));
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::vector<float> &vec) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Sequence);
|
|
for (float f : vec)
|
|
{
|
|
node.push_back(YAML::Node(static_cast<double>(f)));
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::vector<double> &vec) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Sequence);
|
|
for (double d : vec)
|
|
{
|
|
node.push_back(YAML::Node(d));
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::vector<std::string> &vec) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Sequence);
|
|
for (const std::string &s : vec)
|
|
{
|
|
node.push_back(YAML::Node(s));
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Sequence);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::map<std::string, bool> &map) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Map);
|
|
for (const auto &pair : map)
|
|
{
|
|
node[pair.first] = YAML::Node(pair.second);
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::map<std::string, int> &map) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Map);
|
|
for (const auto &pair : map)
|
|
{
|
|
node[pair.first] = YAML::Node(pair.second);
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::map<std::string, float> &map) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Map);
|
|
for (const auto &pair : map)
|
|
{
|
|
node[pair.first] = YAML::Node(static_cast<double>(pair.second));
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::map<std::string, double> &map) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Map);
|
|
for (const auto &pair : map)
|
|
{
|
|
node[pair.first] = YAML::Node(pair.second);
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
|
|
}
|
|
|
|
void NodeHandle::setParam(const std::string &key, const std::map<std::string, std::string> &map) const
|
|
{
|
|
YAML::Node node(YAML::NodeType::Map);
|
|
for (const auto &pair : map)
|
|
{
|
|
node[pair.first] = YAML::Node(pair.second);
|
|
}
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Map);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
// On error, create empty node
|
|
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, bool &b, bool default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined())
|
|
{
|
|
b = default_value;
|
|
return false;
|
|
}
|
|
|
|
if (value.IsScalar())
|
|
{
|
|
try
|
|
{
|
|
if (value.Type() == YAML::NodeType::Scalar)
|
|
{
|
|
std::string str = value.as<std::string>();
|
|
std::transform(str.begin(), str.end(), str.begin(), ::tolower);
|
|
if (str == "true" || str == "1" || str == "yes" || str == "on")
|
|
{
|
|
b = true;
|
|
return true;
|
|
}
|
|
else if (str == "false" || str == "0" || str == "no" || str == "off")
|
|
{
|
|
b = false;
|
|
return true;
|
|
}
|
|
}
|
|
b = value.as<bool>();
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
b = default_value;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
b = default_value;
|
|
return false;
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, double &d, double default_value) const
|
|
{
|
|
// Safety check: validate key parameter
|
|
if (key.empty())
|
|
{
|
|
d = default_value;
|
|
return false;
|
|
}
|
|
YAML::Node value = getNestedValue(key);
|
|
if (!value.IsDefined() || !value.IsScalar())
|
|
{
|
|
d = default_value;
|
|
return false;
|
|
}
|
|
if(key == "local_costmap/inscribed_radius")
|
|
{
|
|
robot::log_warning_at(__FILE__, __LINE__, "3");
|
|
}
|
|
if(!value.IsDefined() || !value.IsScalar())
|
|
if(key == "local_costmap/inscribed_radius")
|
|
{
|
|
robot::log_warning_at(__FILE__, __LINE__, "Inscribed radius: %f", default_value);
|
|
}
|
|
if(key == "local_costmap/inscribed_radius")
|
|
{
|
|
robot::log_warning_at(__FILE__, __LINE__, "4");
|
|
}
|
|
try
|
|
{
|
|
d = value.as<double>();
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
d = default_value;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, float &f, float default_value) const
|
|
{
|
|
double d;
|
|
if (getParam(key, d, static_cast<double>(default_value)))
|
|
{
|
|
f = static_cast<float>(d);
|
|
return true;
|
|
}
|
|
f = default_value;
|
|
return false;
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, int &i, int default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsScalar())
|
|
{
|
|
i = default_value;
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
std::string str = value.as<std::string>();
|
|
// Handle hex format
|
|
if (str.length() > 2 && str.substr(0, 2) == "0x")
|
|
{
|
|
i = std::stoi(str, nullptr, 16);
|
|
}
|
|
else
|
|
{
|
|
i = value.as<int>();
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
i = default_value;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::string &s, std::string default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
// If key doesn't exist or is not a scalar, return default value
|
|
if (!value.IsDefined() || !value.IsScalar())
|
|
{
|
|
s = default_value;
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
s = value.as<std::string>();
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
s = default_value;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::vector<bool> &vec, std::vector<bool> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsSequence())
|
|
{
|
|
vec = default_value;
|
|
return false;
|
|
}
|
|
|
|
vec.clear();
|
|
try
|
|
{
|
|
for (size_t i = 0; i < value.size(); ++i)
|
|
{
|
|
if (value[i].IsScalar())
|
|
{
|
|
try
|
|
{
|
|
bool b = value[i].as<bool>();
|
|
vec.push_back(b);
|
|
}
|
|
catch (...)
|
|
{
|
|
// Try as string and convert
|
|
std::string str = value[i].as<std::string>();
|
|
std::transform(str.begin(), str.end(), str.begin(), ::tolower);
|
|
if (str == "true" || str == "1" || str == "yes" || str == "on")
|
|
{
|
|
vec.push_back(true);
|
|
}
|
|
else if (str == "false" || str == "0" || str == "no" || str == "off")
|
|
{
|
|
vec.push_back(false);
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::vector<double> &vec, std::vector<double> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsSequence())
|
|
{
|
|
vec = default_value;
|
|
return false;
|
|
}
|
|
|
|
vec.clear();
|
|
try
|
|
{
|
|
for (size_t i = 0; i < value.size(); ++i)
|
|
{
|
|
vec.push_back(value[i].as<double>());
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::vector<float> &vec, std::vector<float> default_value) const
|
|
{
|
|
std::vector<double> dvec;
|
|
std::vector<double> ddefault;
|
|
ddefault.reserve(default_value.size());
|
|
for (float f : default_value)
|
|
{
|
|
ddefault.push_back(static_cast<double>(f));
|
|
}
|
|
|
|
if (getParam(key, dvec, ddefault))
|
|
{
|
|
vec.clear();
|
|
vec.reserve(dvec.size());
|
|
for (double d : dvec)
|
|
{
|
|
vec.push_back(static_cast<float>(d));
|
|
}
|
|
return true;
|
|
}
|
|
vec = default_value;
|
|
return false;
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::vector<int> &vec, std::vector<int> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsSequence())
|
|
{
|
|
vec = default_value;
|
|
return false;
|
|
}
|
|
|
|
vec.clear();
|
|
try
|
|
{
|
|
for (size_t i = 0; i < value.size(); ++i)
|
|
{
|
|
vec.push_back(value[i].as<int>());
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::vector<std::string> &vec, std::vector<std::string> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsSequence())
|
|
{
|
|
vec = default_value;
|
|
return false;
|
|
}
|
|
|
|
vec.clear();
|
|
try
|
|
{
|
|
for (size_t i = 0; i < value.size(); ++i)
|
|
{
|
|
vec.push_back(value[i].as<std::string>());
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::map<std::string, bool> &map, std::map<std::string, bool> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsMap())
|
|
{
|
|
map = default_value;
|
|
return false;
|
|
}
|
|
|
|
map.clear();
|
|
try
|
|
{
|
|
for (auto it = value.begin(); it != value.end(); ++it)
|
|
{
|
|
std::string key_str = it->first.as<std::string>();
|
|
if (it->second.IsScalar())
|
|
{
|
|
try
|
|
{
|
|
bool b = it->second.as<bool>();
|
|
map[key_str] = b;
|
|
}
|
|
catch (...)
|
|
{
|
|
// Try as string and convert
|
|
std::string str = it->second.as<std::string>();
|
|
std::transform(str.begin(), str.end(), str.begin(), ::tolower);
|
|
if (str == "true" || str == "1" || str == "yes" || str == "on")
|
|
{
|
|
map[key_str] = true;
|
|
}
|
|
else if (str == "false" || str == "0" || str == "no" || str == "off")
|
|
{
|
|
map[key_str] = false;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::map<std::string, double> &map, std::map<std::string, double> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsMap())
|
|
{
|
|
map = default_value;
|
|
return false;
|
|
}
|
|
|
|
map.clear();
|
|
try
|
|
{
|
|
for (auto it = value.begin(); it != value.end(); ++it)
|
|
{
|
|
std::string key_str = it->first.as<std::string>();
|
|
if (it->second.IsScalar())
|
|
{
|
|
map[key_str] = it->second.as<double>();
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::map<std::string, float> &map, std::map<std::string, float> default_value) const
|
|
{
|
|
std::map<std::string, double> dmap;
|
|
std::map<std::string, double> ddefault;
|
|
for (const auto &pair : default_value)
|
|
{
|
|
ddefault[pair.first] = static_cast<double>(pair.second);
|
|
}
|
|
|
|
if (getParam(key, dmap, ddefault))
|
|
{
|
|
map.clear();
|
|
for (const auto &pair : dmap)
|
|
{
|
|
map[pair.first] = static_cast<float>(pair.second);
|
|
}
|
|
return true;
|
|
}
|
|
map = default_value;
|
|
return false;
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::map<std::string, int> &map, std::map<std::string, int> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsMap())
|
|
{
|
|
map = default_value;
|
|
return false;
|
|
}
|
|
|
|
map.clear();
|
|
try
|
|
{
|
|
for (auto it = value.begin(); it != value.end(); ++it)
|
|
{
|
|
std::string key_str = it->first.as<std::string>();
|
|
if (it->second.IsScalar())
|
|
{
|
|
map[key_str] = it->second.as<int>();
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, std::map<std::string, std::string> &map, std::map<std::string, std::string> default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined() || !value.IsMap())
|
|
{
|
|
map = default_value;
|
|
return false;
|
|
}
|
|
|
|
map.clear();
|
|
try
|
|
{
|
|
for (auto it = value.begin(); it != value.end(); ++it)
|
|
{
|
|
std::string key_str = it->first.as<std::string>();
|
|
if (it->second.IsScalar())
|
|
{
|
|
map[key_str] = it->second.as<std::string>();
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, YAML::Node &v, YAML::Node default_value) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
|
|
if (!value.IsDefined())
|
|
{
|
|
v = default_value;
|
|
return false;
|
|
}
|
|
|
|
v = value;
|
|
return true;
|
|
}
|
|
|
|
namespace
|
|
{
|
|
bool yamlToXmlRpc(const YAML::Node &y, robot_xmlrpcpp::XmlRpcValue &out)
|
|
{
|
|
if (!y.IsDefined())
|
|
return false;
|
|
if (y.IsScalar())
|
|
{
|
|
try
|
|
{
|
|
std::string s = y.as<std::string>();
|
|
std::string lower = s;
|
|
std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
|
|
if (lower == "true" || lower == "1" || lower == "yes" || lower == "on")
|
|
{
|
|
out = robot_xmlrpcpp::XmlRpcValue(true);
|
|
return true;
|
|
}
|
|
if (lower == "false" || lower == "0" || lower == "no" || lower == "off")
|
|
{
|
|
out = robot_xmlrpcpp::XmlRpcValue(false);
|
|
return true;
|
|
}
|
|
try
|
|
{
|
|
int i = y.as<int>();
|
|
out = robot_xmlrpcpp::XmlRpcValue(i);
|
|
return true;
|
|
}
|
|
catch (...) {}
|
|
try
|
|
{
|
|
double d = y.as<double>();
|
|
out = robot_xmlrpcpp::XmlRpcValue(d);
|
|
return true;
|
|
}
|
|
catch (...) {}
|
|
out = robot_xmlrpcpp::XmlRpcValue(s);
|
|
return true;
|
|
}
|
|
catch (...)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
if (y.IsSequence())
|
|
{
|
|
out = robot_xmlrpcpp::XmlRpcValue();
|
|
out.setSize(static_cast<int>(y.size()));
|
|
for (size_t i = 0; i < y.size(); ++i)
|
|
{
|
|
robot_xmlrpcpp::XmlRpcValue item;
|
|
if (yamlToXmlRpc(y[i], item))
|
|
out[static_cast<int>(i)] = item;
|
|
}
|
|
return true;
|
|
}
|
|
if (y.IsMap())
|
|
{
|
|
out = robot_xmlrpcpp::XmlRpcValue();
|
|
for (YAML::const_iterator it = y.begin(); it != y.end(); ++it)
|
|
{
|
|
std::string k = it->first.as<std::string>();
|
|
robot_xmlrpcpp::XmlRpcValue item;
|
|
if (yamlToXmlRpc(it->second, item))
|
|
out[k] = item;
|
|
}
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool NodeHandle::getParam(const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const
|
|
{
|
|
YAML::Node value = getNestedValue(key);
|
|
if (!value.IsDefined())
|
|
return false;
|
|
return yamlToXmlRpc(value, v);
|
|
}
|
|
|
|
template <typename T>
|
|
T NodeHandle::param(const std::string ¶m_name) const
|
|
{
|
|
T param_val;
|
|
T default_val = T{}; // Default-constructed value
|
|
if (param(param_name, param_val, default_val))
|
|
{
|
|
return param_val;
|
|
}
|
|
return default_val;
|
|
}
|
|
|
|
template <typename T>
|
|
T NodeHandle::param(const std::string ¶m_name, const T &default_val) const
|
|
{
|
|
T param_val;
|
|
if (param(param_name, param_val, default_val))
|
|
{
|
|
return param_val;
|
|
}
|
|
return default_val;
|
|
}
|
|
|
|
template <typename T>
|
|
bool NodeHandle::param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
|
|
{
|
|
return getParam(param_name, param_val, default_val);
|
|
}
|
|
|
|
bool NodeHandle::searchParam(const std::string &key, std::string &result) const
|
|
{
|
|
result.clear();
|
|
if (key.empty())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
auto splitPath = [](const std::string &path) -> std::vector<std::string>
|
|
{
|
|
std::vector<std::string> parts;
|
|
std::stringstream ss(path);
|
|
std::string seg;
|
|
while (std::getline(ss, seg, '/'))
|
|
{
|
|
if (!seg.empty())
|
|
{
|
|
parts.push_back(seg);
|
|
}
|
|
}
|
|
return parts;
|
|
};
|
|
|
|
auto joinPath = [](const std::vector<std::string> &parts) -> std::string
|
|
{
|
|
std::string out;
|
|
for (size_t i = 0; i < parts.size(); ++i)
|
|
{
|
|
if (i)
|
|
out += "/";
|
|
out += parts[i];
|
|
}
|
|
return out;
|
|
};
|
|
|
|
auto existsInRoot = [&](const std::string &full_key_no_leading_slash) -> bool
|
|
{
|
|
// Prefer searching in the global tree if available; otherwise fall back to
|
|
// this NodeHandle's scoped view.
|
|
const YAML::Node *search_root = nullptr;
|
|
if (root_.IsDefined() && root_.IsMap())
|
|
{
|
|
search_root = &root_;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
|
|
const std::vector<std::string> segments = splitPath(full_key_no_leading_slash);
|
|
if (segments.empty())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
const YAML::Node *current = search_root;
|
|
for (const auto &seg : segments)
|
|
{
|
|
// Safety check: ensure current is valid
|
|
if (!current || !current->IsDefined() || !current->IsMap())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
const YAML::Node *found_child = nullptr;
|
|
|
|
try
|
|
{
|
|
for (YAML::const_iterator it = current->begin(); it != current->end(); ++it)
|
|
{
|
|
try
|
|
{
|
|
// Safety check: validate iterator nodes
|
|
if (!it->first.IsDefined())
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Safely convert key to string
|
|
std::string key_str;
|
|
try
|
|
{
|
|
key_str = it->first.as<std::string>();
|
|
}
|
|
catch (...)
|
|
{
|
|
// Skip nodes that can't be converted to string
|
|
continue;
|
|
}
|
|
|
|
if (key_str == seg)
|
|
{
|
|
// Safety check: validate value node
|
|
if (it->second.IsDefined())
|
|
{
|
|
found_child = &(it->second);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
// Skip invalid nodes and continue iteration
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
// If iteration fails, return false
|
|
return false;
|
|
}
|
|
|
|
if (!found_child || !found_child->IsDefined())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
current = found_child;
|
|
}
|
|
|
|
// Safety check: ensure current is valid before returning
|
|
return current && current->IsDefined();
|
|
};
|
|
|
|
auto stripLeadingSlash = [](const std::string &s) -> std::string
|
|
{
|
|
std::string out = s;
|
|
while (!out.empty() && out.front() == '/')
|
|
{
|
|
out.erase(out.begin());
|
|
}
|
|
return out;
|
|
};
|
|
|
|
const std::string key_no_slash = stripLeadingSlash(key);
|
|
if (key_no_slash.empty())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// If caller passed an absolute key (started with '/'), we only check that exact path.
|
|
// Use the key as-is (without adding this NodeHandle's namespace).
|
|
if (!key.empty() && key.front() == '/')
|
|
{
|
|
if (existsInRoot(key_no_slash))
|
|
{
|
|
result = "/" + key_no_slash;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// Search up the namespace tree. Example:
|
|
// existing: a/b
|
|
// namespace: a/c/d
|
|
// search "b" -> check a/c/d/b, a/c/b, a/b
|
|
std::vector<std::string> ns_parts;
|
|
if (!namespace_.empty() && namespace_ != "/")
|
|
{
|
|
ns_parts = splitPath(stripLeadingSlash(namespace_));
|
|
}
|
|
const std::vector<std::string> key_parts = splitPath(key_no_slash);
|
|
|
|
for (int depth = static_cast<int>(ns_parts.size()); depth >= 0; --depth)
|
|
{
|
|
std::vector<std::string> candidate_parts;
|
|
candidate_parts.reserve(static_cast<size_t>(depth) + key_parts.size());
|
|
|
|
// Safety check: ensure depth doesn't exceed ns_parts size
|
|
size_t safe_depth = static_cast<size_t>(depth);
|
|
if (safe_depth > ns_parts.size())
|
|
{
|
|
safe_depth = ns_parts.size();
|
|
}
|
|
|
|
for (size_t i = 0; i < safe_depth; ++i)
|
|
{
|
|
candidate_parts.push_back(ns_parts[i]);
|
|
}
|
|
for (const auto &kp : key_parts)
|
|
{
|
|
candidate_parts.push_back(kp);
|
|
}
|
|
|
|
const std::string candidate = joinPath(candidate_parts);
|
|
if (existsInRoot(candidate))
|
|
{
|
|
result = "/" + candidate;
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
const YAML::Node &NodeHandle::getNodeHandle() const
|
|
{
|
|
return root_;
|
|
}
|
|
|
|
// Explicit template instantiations for param function
|
|
// String instantiations
|
|
template std::string NodeHandle::param<std::string>(const std::string ¶m_name) const;
|
|
template std::string NodeHandle::param<std::string>(const std::string ¶m_name, const std::string &default_val) const;
|
|
template bool NodeHandle::param<std::string>(const std::string ¶m_name, std::string ¶m_val, const std::string &default_val) const;
|
|
|
|
// Bool instantiations
|
|
template bool NodeHandle::param<bool>(const std::string ¶m_name) const;
|
|
template bool NodeHandle::param<bool>(const std::string ¶m_name, const bool &default_val) const;
|
|
template bool NodeHandle::param<bool>(const std::string ¶m_name, bool ¶m_val, const bool &default_val) const;
|
|
|
|
// Int instantiations
|
|
template int NodeHandle::param<int>(const std::string ¶m_name) const;
|
|
template int NodeHandle::param<int>(const std::string ¶m_name, const int &default_val) const;
|
|
template bool NodeHandle::param<int>(const std::string ¶m_name, int ¶m_val, const int &default_val) const;
|
|
|
|
// Double instantiations
|
|
template double NodeHandle::param<double>(const std::string ¶m_name) const;
|
|
template double NodeHandle::param<double>(const std::string ¶m_name, const double &default_val) const;
|
|
template bool NodeHandle::param<double>(const std::string ¶m_name, double ¶m_val, const double &default_val) const;
|
|
|
|
// Float instantiations
|
|
template float NodeHandle::param<float>(const std::string ¶m_name) const;
|
|
template float NodeHandle::param<float>(const std::string ¶m_name, const float &default_val) const;
|
|
template bool NodeHandle::param<float>(const std::string ¶m_name, float ¶m_val, const float &default_val) const;
|
|
|
|
// Vector<std::string> instantiations
|
|
template std::vector<std::string> NodeHandle::param<std::vector<std::string>>(const std::string ¶m_name) const;
|
|
template std::vector<std::string> NodeHandle::param<std::vector<std::string>>(const std::string ¶m_name, const std::vector<std::string> &default_val) const;
|
|
template bool NodeHandle::param<std::vector<std::string>>(const std::string ¶m_name, std::vector<std::string> ¶m_val, const std::vector<std::string> &default_val) const;
|
|
|
|
// Static member initialization
|
|
std::string NodeHandle::config_directory_;
|
|
YAML::Node NodeHandle::root_;
|
|
|
|
} // namespace robot
|