64 lines
2.0 KiB
C++
64 lines
2.0 KiB
C++
#ifndef COSTMAP_2D_UTILS_H
|
|
#define COSTMAP_2D_UTILS_H
|
|
#include <yaml-cpp/yaml.h>
|
|
#include <filesystem>
|
|
#include <string>
|
|
#include <iostream>
|
|
|
|
namespace costmap_2d
|
|
{
|
|
template<typename T>
|
|
T loadParam(const YAML::Node& node, const std::string& key, const T& default_value)
|
|
{
|
|
if (node[key] && node[key].IsDefined())
|
|
return node[key].as<T>();
|
|
return default_value;
|
|
}
|
|
|
|
inline std::vector<geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<geometry_msgs::Point>& default_value)
|
|
{
|
|
if( !node || !node.IsDefined())
|
|
return default_value;
|
|
|
|
std::vector<geometry_msgs::Point> fp;
|
|
|
|
for (const auto& p : node) {
|
|
if (p.size() != 2)
|
|
throw std::runtime_error("Footprint point must be [x, y]");
|
|
|
|
fp.push_back(geometry_msgs::Point{p[0].as<double>(), p[1].as<double>()});
|
|
}
|
|
std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl;
|
|
|
|
return fp;
|
|
}
|
|
|
|
|
|
inline std::string getSourceFile(const std::string& root, const std::string& config_file_name)
|
|
{
|
|
std::string path_source = " ";
|
|
std::string sub_folder = "config";
|
|
namespace fs = std::filesystem;
|
|
|
|
fs::path cfg_dir = fs::path(root) / sub_folder;
|
|
|
|
if (fs::exists(cfg_dir) && fs::is_directory(cfg_dir))
|
|
{
|
|
for (const auto& entry : fs::recursive_directory_iterator(cfg_dir))
|
|
{
|
|
if (entry.is_regular_file() && entry.path().filename() == config_file_name)
|
|
{
|
|
path_source = entry.path().string();
|
|
std::cout << "Path source: " << path_source << std::endl;
|
|
break; // tìm thấy thì dừng
|
|
}
|
|
}
|
|
}
|
|
if(path_source == " ")
|
|
std::cout<< config_file_name << " file not found at path: "<< cfg_dir << std::endl;
|
|
return path_source;
|
|
}
|
|
|
|
}
|
|
|
|
#endif // COSTMAP_2D_UTILS_H
|