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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
include/robot_costmap_2d/costmap_2d_robot.h
View File

@ -53,8 +53,7 @@
#include <robot_xmlrpcpp/XmlRpcValue.h>
#include <robot/node_handle.h>
#include <robot/plugin_loader_helper.h>
class RobotSuperValue : public robot_xmlrpcpp::XmlRpcValue
{
@ -199,13 +198,19 @@ public:
return padded_footprint_;
}
inline const robot_geometry_msgs::PolygonStamped& getRobotFootprintPolygonStamped() const noexcept
{
return footprint_;
}
/** @brief Return the current unpadded footprint of the robot as a vector of points.
*
* This is the raw version of the footprint without padding.
*
* The footprint initially comes from the rosparam "footprint" but
* can be overwritten by dynamic reconfigure or by messages received
* on the "footprint" topic. */
* on the "footprint" topic.
*/
inline const std::vector<robot_geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
{
return unpadded_footprint_;
@ -250,6 +255,7 @@ protected:
double transform_tolerance_; ///< timeout before transform errors
private:
void copyParentParameters(const std::string& costmap_name, const std::string& plugin_name, const std::string& plugin_type, robot::NodeHandle& nh);
/** @brief Set the footprint from the new_config object.
*
* If the values of footprint and robot_radius are the same in
@ -270,10 +276,11 @@ private:
std::vector<robot_geometry_msgs::Point> unpadded_footprint_;
std::vector<robot_geometry_msgs::Point> padded_footprint_;
robot_geometry_msgs::PolygonStamped footprint_;
float footprint_padding_;
private:
void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
void getParams(const std::string& config_file_name,const std::string& name, robot::NodeHandle& nh);
};
// class Costmap2DROBOT
} // namespace robot_costmap_2d

1
include/robot_costmap_2d/footprint.h
View File

@ -44,6 +44,7 @@
#include <robot_geometry_msgs/Point32.h>
#include <robot/node_handle.h>
#include <robot/console.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
namespace robot_costmap_2d

3
include/robot_costmap_2d/layer.h
View File

@ -42,7 +42,8 @@
#include <robot_costmap_2d/utils.h>
#include <string>
#include <tf3/buffer_core.h>
#include <robot/node_handle.h>
#include <robot/console.h>
#include <robot/plugin_loader_helper.h>
namespace robot_costmap_2d
{

2
include/robot_costmap_2d/observation_buffer.h
View File

@ -48,6 +48,8 @@
// Thread support
#include <boost/thread.hpp>
#include <robot/console.h>
#include <robot/plugin_loader_helper.h>
namespace robot_costmap_2d
{

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

205
src/costmap_2d_robot.cpp
View File

@ -55,6 +55,16 @@ using namespace std;
namespace robot_costmap_2d
{
template <class T>
void move_parameter(robot::NodeHandle& old_h, robot::NodeHandle& new_h, std::string name,T& value)
{
if (!old_h.hasParam(name))
return;
old_h.getParam(name, value);
new_h.setParam(name, value);
}
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
layered_costmap_(NULL),
name_(name),
@ -71,7 +81,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
robot::NodeHandle priv_nh(nh, name);
name_ = name;
std::string config_file_name = "costmap_params.yaml";
getParams(config_file_name, priv_nh);
getParams(config_file_name, name_, nh);
// create a thread to handle updating the map
stop_updates_ = false;
@ -79,30 +89,37 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
stopped_ = false;
}
void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeHandle& nh)
void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::string& 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);
YAML::Node layer = config["robot_costmap_2d"];
robot::NodeHandle priv_nh(priv_nh, name);
std::string global_frame =
loadParam(layer, "global_frame", std::string("map"));
std::string robot_base_frame =
loadParam(layer, "robot_base_frame", std::string("base_link"));
if (nh.hasParam("global_frame"))
nh.getParam("global_frame", global_frame);
if (priv_nh.hasParam("global_frame"))
priv_nh.getParam("global_frame", global_frame);
if (nh.hasParam("robot_base_frame"))
nh.getParam("robot_base_frame", robot_base_frame);
global_frame_ = global_frame;
robot_base_frame_ = robot_base_frame;
robot::log_error("robot_base_frame: %s", robot_base_frame.c_str());
robot::Time last_error = robot::Time::now();
std::string tf_error;
@ -130,15 +147,14 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
robot::PluginLoaderHelper loader;
if (nh.hasParam("rolling_window"))
nh.getParam("rolling_window", rolling_window);
if (priv_nh.hasParam("rolling_window"))
priv_nh.getParam("rolling_window", rolling_window);
if (nh.hasParam("track_unknown_space"))
nh.getParam("track_unknown_space", track_unknown_space);
if (priv_nh.hasParam("track_unknown_space"))
priv_nh.getParam("track_unknown_space", track_unknown_space);
if (nh.hasParam("library_path"))
if (priv_nh.hasParam("library_path"))
path_plugins = loader.findLibraryPath(name_);
robot::log_error("name: %s | rolling_window: %x", name_.c_str(), rolling_window);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
// find size parameters
@ -148,16 +164,16 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
double origin_x = loadParam(layer, "origin_x", 0.0);
double origin_y = loadParam(layer, "origin_y", 0.0);
if (nh.hasParam("width"))
nh.getParam("width", map_width_meters);
if (nh.hasParam("height"))
nh.getParam("height", map_height_meters);
if (nh.hasParam("resolution"))
nh.getParam("resolution", resolution);
if (nh.hasParam("origin_x"))
nh.getParam("origin_x", origin_x);
if (nh.hasParam("origin_y"))
nh.getParam("origin_y", origin_y);
if (priv_nh.hasParam("width"))
priv_nh.getParam("width", map_width_meters);
if (priv_nh.hasParam("height"))
priv_nh.getParam("height", map_height_meters);
if (priv_nh.hasParam("resolution"))
priv_nh.getParam("resolution", resolution);
if (priv_nh.hasParam("origin_x"))
priv_nh.getParam("origin_x", origin_x);
if (priv_nh.hasParam("origin_y"))
priv_nh.getParam("origin_y", origin_y);
if (!layered_costmap_->isSizeLocked())
{
@ -173,10 +189,10 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
};
std::vector<PluginConfig> my_list;
if(nh.hasParam("plugins"))
if(priv_nh.hasParam("plugins"))
{
my_list.clear();
YAML::Node my_plugins = nh.getParamValue("plugins");
YAML::Node my_plugins = priv_nh.getParamValue("plugins");
if (my_plugins && my_plugins.IsSequence())
{
for (size_t i = 0; i < my_plugins.size(); ++i)
@ -230,12 +246,12 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
}
}
}
robot::NodeHandle private_nh("~");
for (auto& info : my_list)
{
try
{
// copyParentParameters(pname, type, private_nh);
copyParentParameters(name_, info.name, info.type, private_nh);
creators_.push_back(
boost::dll::import_alias<PluginLayerPtr()>(
path_plugins, info.type, boost::dll::load_mode::append_decorations)
@ -257,14 +273,15 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
new_footprint = loadFootprint(layer["footprint"], new_footprint);
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
if (nh.hasParam("footprint"))
if (priv_nh.hasParam("footprint"))
{
std::cout <<"FOOTPRINT ROBOT:"<<std::endl;
new_footprint = makeFootprintFromParams(nh);
new_footprint = makeFootprintFromParams(priv_nh);
}
if (nh.hasParam("transform_tolerance"))
nh.getParam("transform_tolerance", transform_tolerance_);
// robot::log_info("transform_tolerance: %d", transform_tolerance_);
setUnpaddedRobotFootprint(new_footprint);
@ -280,8 +297,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
map_update_thread_shutdown_ = false;
double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
if (nh.hasParam("update_frequency"))
nh.getParam("update_frequency", map_update_frequency);
if (priv_nh.hasParam("update_frequency"))
priv_nh.getParam("update_frequency", map_update_frequency);
// If the padding has changed, call setUnpaddedRobotFootprint() to
// re-apply the padding.
@ -295,8 +312,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
}
double robot_radius = loadParam(layer, "robot_radius", 0.0);
if (nh.hasParam("robot_radius"))
nh.getParam("robot_radius", robot_radius);
if (priv_nh.hasParam("robot_radius"))
priv_nh.getParam("robot_radius", robot_radius);
readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
// only construct the thread if the frequency is positive
@ -310,6 +327,121 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
}
}
void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
const std::string& plugin_name,
const std::string& plugin_type,
robot::NodeHandle& nh)
{
robot::NodeHandle priv_nh_1(nh, costmap_name);
robot::NodeHandle target_layer(priv_nh_1, plugin_name);
robot::NodeHandle priv_nh_2(nh, plugin_name);
// robot::log_error("TEST: %s",priv_nh_2.getNamespace().c_str());
if(plugin_type == "StaticLayer")
{
std::string map_topic;
int unknown_cost_value;
int lethal_cost_threshold;
bool track_unknown_space;
move_parameter(priv_nh_2, target_layer, "map_topic", map_topic);
move_parameter(priv_nh_2, target_layer, "unknown_cost_value", unknown_cost_value);
move_parameter(priv_nh_2, target_layer, "lethal_cost_threshold", lethal_cost_threshold);
move_parameter(priv_nh_2, target_layer, "track_unknown_space", track_unknown_space);
}
else if(plugin_type == "VoxelLayer")
{
double origin_z;
double z_resolution;
int z_voxels;
int mark_threshold;
int unknown_threshold;
bool publish_voxel_map;
move_parameter(priv_nh_2, target_layer, "origin_z", origin_z);
move_parameter(priv_nh_2, target_layer, "z_resolution", z_resolution);
move_parameter(priv_nh_2, target_layer, "z_voxels", z_voxels);
move_parameter(priv_nh_2, target_layer, "mark_threshold", mark_threshold);
move_parameter(priv_nh_2, target_layer, "unknown_threshold", unknown_threshold);
move_parameter(priv_nh_2, target_layer, "publish_voxel_map", publish_voxel_map);
if(priv_nh_2.hasParam("observation_sources"))
{
std::string topics_string;
move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string);
robot::log_error("topics_string: %s", topics_string.c_str());
std::stringstream ss(topics_string);
std::string source;
while (ss >> source)
{
robot::NodeHandle priv_nh_3(priv_nh_2, source);
robot::NodeHandle target_layer_2(target_layer, source);
robot::log_warning("priv_nh_3: %s",priv_nh_3.getNamespace().c_str());
std::string topic;
std::string data_type;
bool clearing;
bool marking;
bool inf_is_valid;
double min_obstacle_height;
double max_obstacle_height;
move_parameter(priv_nh_2, target_layer_2, "topic", topic);
robot::log_error("topic: %s", topic.c_str());
move_parameter(priv_nh_2, target_layer_2, "data_type", data_type);
move_parameter(priv_nh_2, target_layer_2, "clearing", clearing);
move_parameter(priv_nh_2, target_layer_2, "marking", marking);
move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid);
move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height);
move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height);
target_layer_2.printParams();
}
}
}
else if(plugin_type == "ObstacleLayer")
{
double max_obstacle_height;
double raytrace_range;
double obstacle_range;
bool track_unknown_space;
move_parameter(priv_nh_2, target_layer, "max_obstacle_height", max_obstacle_height);
move_parameter(priv_nh_2, target_layer, "raytrace_range", raytrace_range);
move_parameter(priv_nh_2, target_layer, "obstacle_range", obstacle_range);
move_parameter(priv_nh_2, target_layer, "track_unknown_space", track_unknown_space);
if(priv_nh_2.hasParam("observation_sources"))
{
std::string topics_string;
move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string);
robot::log_error("topics_string: %s", topics_string.c_str());
std::stringstream ss(topics_string);
std::string source;
while (ss >> source)
{
robot::NodeHandle priv_nh_3(priv_nh_2, source);
robot::NodeHandle target_layer_2(target_layer, source);
std::string topic;
std::string data_type;
bool clearing;
bool marking;
bool inf_is_valid;
double min_obstacle_height;
double max_obstacle_height;
move_parameter(priv_nh_2, target_layer_2, "topic", topic);
move_parameter(priv_nh_2, target_layer_2, "data_type", data_type);
move_parameter(priv_nh_2, target_layer_2, "clearing", clearing);
move_parameter(priv_nh_2, target_layer_2, "marking", marking);
move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid);
move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height);
move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height);
}
}
}
else if(plugin_type == "InflationLayer")
{
double cost_scaling_factor;
double inflation_radius;
move_parameter(priv_nh_2, target_layer, "cost_scaling_factor", cost_scaling_factor);
move_parameter(priv_nh_2, target_layer, "inflation_radius", inflation_radius);
}
}
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint)
{
setUnpaddedRobotFootprint(toPointVector(footprint));
@ -402,16 +534,17 @@ void Costmap2DROBOT::updateMap()
yaw = data_convert::getYaw(pose.pose.orientation);
// robot::log_error("ROBOT POSE: %f | %f | %f",x,y,yaw);
layered_costmap_->updateMap(x, y, yaw);
robot_geometry_msgs::PolygonStamped footprint;
footprint.header.frame_id = global_frame_;
footprint.header.stamp = robot::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint);
footprint_.header.frame_id = global_frame_;
footprint_.header.stamp = robot::Time::now();
transformFootprint(x, y, yaw, padded_footprint_, footprint_);
initialized_ = true;
}
}
}
void Costmap2DROBOT::start()
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::start");

1
src/footprint.cpp
View File

@ -34,7 +34,6 @@
#include <robot_costmap_2d/footprint.h>
#include <robot_costmap_2d/array_parser.h>
#include <robot_geometry_msgs/Point32.h>
#include <robot/console.h>
namespace robot_costmap_2d
{

1
src/layered_costmap.cpp
View File

@ -37,7 +37,6 @@
*********************************************************************/
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/footprint.h>
#include <robot/console.h>
#include <cstdio>
#include <string>
#include <algorithm>

1
src/observation_buffer.cpp
View File

@ -39,7 +39,6 @@
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <robot/console.h>
using namespace std;
using namespace tf3;

5
test/static_layer_test.cpp
View File

@ -3,7 +3,6 @@
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>
#include <robot_geometry_msgs/PoseWithCovariance.h>
#include <robot/console.h>
namespace robot_costmap_2d {
@ -68,9 +67,9 @@ void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned
double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
if(neighbor_cost < expected_lowest_cost){
robot::log_error("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
printf("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
robot::log_error("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
costmap.saveMap("failing_costmap.pgm");
}
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));