DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Duong update

Browse Source
This commit is contained in:
HiepLM
2026-01-12 15:48:43 +07:00
parent 9d3d31a4f9
commit 81e7874274
14 changed files with 219 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
plugins/directional_layer.cpp
View File

@@ -1,5 +1,4 @@
#include <robot_costmap_2d/directional_layer.h>
#include <robot/console.h>
#include <data_convert/data_convert.h>
#include <tf3/convert.h>
#include <tf3/utils.h>

9
plugins/inflation_layer.cpp
View File

@@ -39,7 +39,6 @@
#include <robot_costmap_2d/inflation_layer.h>
#include <robot_costmap_2d/costmap_math.h>
#include <robot_costmap_2d/footprint.h>
#include <robot/console.h>
#include <boost/thread.hpp>
#include <boost/dll/alias.hpp>
@@ -92,7 +91,13 @@ void InflationLayer::onInitialize()
bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = ROBOT_COSTMAP_2D_DIR;
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
std::string folder;
if (env_config && std::filesystem::exists(env_config))
{
folder = std::string(env_config);
// robot::log_error("config_directory: %s", folder.c_str());
}
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);

14
plugins/obstacle_layer.cpp
View File

@@ -39,7 +39,6 @@
#include <robot_costmap_2d/costmap_math.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <robot/console.h>
#include <tf3/exceptions.h>
#include <boost/dll/alias.hpp>
@@ -76,7 +75,14 @@ ObstacleLayer::~ObstacleLayer()
bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = ROBOT_COSTMAP_2D_DIR;
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
std::string folder;
if (env_config && std::filesystem::exists(env_config))
{
folder = std::string(env_config);
// robot::log_error("config_directory: %s", folder.c_str());
}
// robot::log_error("folder: %s", folder.c_str());
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
@@ -112,7 +118,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
if (nh.hasParam("observation_sources"))
nh.getParam("observation_sources", topics_string);
robot::log_error("Subscribed to Topics: %s\n", topics_string.c_str());
robot::log_info("Subscribed to Topics: %s\n", topics_string.c_str());
// now we need to split the topics based on whitespace which we can use a stringstream for
std::stringstream ss(topics_string);
@@ -189,7 +195,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
// enabled_ = enabled;
robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
sensor_frame.c_str());
priv_nh.getNamespace().c_str());
// create an observation buffer
observation_buffers_.push_back(

9
plugins/static_layer.cpp
View File

@@ -41,7 +41,6 @@
#include <tf3/convert.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot/console.h>
#include <boost/dll/alias.hpp>
#include <fstream>
@@ -77,7 +76,13 @@ void StaticLayer::onInitialize()
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = ROBOT_COSTMAP_2D_DIR;
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
std::string folder;
if (env_config && std::filesystem::exists(env_config))
{
folder = std::string(env_config);
// robot::log_error("config_directory: %s", folder.c_str());
}
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);

11
plugins/voxel_layer.cpp
View File

@@ -37,7 +37,6 @@
*********************************************************************/
#include <robot_costmap_2d/voxel_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <robot/console.h>
#include <boost/dll/alias.hpp>
#define VOXEL_BITS 16
@@ -69,7 +68,14 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
{
try
{
std::string folder = ROBOT_COSTMAP_2D_DIR;
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
std::string folder;
if (env_config && std::filesystem::exists(env_config))
{
folder = std::string(env_config);
// robot::log_error("config_directory: %s", folder.c_str());
}
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
@@ -112,7 +118,6 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
}
if (nh.hasParam("combination_method"))
nh.getParam("combination_method", combination_method_);
this->matchSize();
}