From 81e78742744c0a5b8a8ef298fa254035c41dc1da Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 12 Jan 2026 15:48:43 +0700 Subject: [PATCH] Duong update --- include/robot_costmap_2d/costmap_2d_robot.h | 15 +- include/robot_costmap_2d/footprint.h | 1 + include/robot_costmap_2d/layer.h | 3 +- include/robot_costmap_2d/observation_buffer.h | 2 + plugins/directional_layer.cpp | 1 - plugins/inflation_layer.cpp | 9 +- plugins/obstacle_layer.cpp | 14 +- plugins/static_layer.cpp | 9 +- plugins/voxel_layer.cpp | 11 +- src/costmap_2d_robot.cpp | 205 +++++++++++++++--- src/footprint.cpp | 1 - src/layered_costmap.cpp | 1 - src/observation_buffer.cpp | 1 - test/static_layer_test.cpp | 5 +- 14 files changed, 219 insertions(+), 59 deletions(-) diff --git a/include/robot_costmap_2d/costmap_2d_robot.h b/include/robot_costmap_2d/costmap_2d_robot.h index 164d4df..af81ee9 100755 --- a/include/robot_costmap_2d/costmap_2d_robot.h +++ b/include/robot_costmap_2d/costmap_2d_robot.h @@ -53,8 +53,7 @@ #include -#include -#include + 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& 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 unpadded_footprint_; std::vector 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 diff --git a/include/robot_costmap_2d/footprint.h b/include/robot_costmap_2d/footprint.h index de44a3f..51d8e03 100755 --- a/include/robot_costmap_2d/footprint.h +++ b/include/robot_costmap_2d/footprint.h @@ -44,6 +44,7 @@ #include #include +#include #include namespace robot_costmap_2d diff --git a/include/robot_costmap_2d/layer.h b/include/robot_costmap_2d/layer.h index 0a9dbc2..1a6270f 100755 --- a/include/robot_costmap_2d/layer.h +++ b/include/robot_costmap_2d/layer.h @@ -42,7 +42,8 @@ #include #include #include -#include +#include +#include namespace robot_costmap_2d { diff --git a/include/robot_costmap_2d/observation_buffer.h b/include/robot_costmap_2d/observation_buffer.h index 95c13d2..2d6e7a0 100755 --- a/include/robot_costmap_2d/observation_buffer.h +++ b/include/robot_costmap_2d/observation_buffer.h @@ -48,6 +48,8 @@ // Thread support #include +#include +#include namespace robot_costmap_2d { diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index 591d6a0..260af1f 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -1,5 +1,4 @@ #include -#include #include #include #include diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index f864242..05c797b 100755 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include @@ -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); diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 77a5183..e557c4c 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -39,7 +39,6 @@ #include #include -#include #include #include @@ -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( diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index b0d6f51..2add362 100755 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -41,7 +41,6 @@ #include #include -#include #include #include @@ -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); diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index 064461e..9f94e7a 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -37,7 +37,6 @@ *********************************************************************/ #include #include -#include #include #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(); } diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index dbad0d1..1d6f04b 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -55,6 +55,16 @@ using namespace std; namespace robot_costmap_2d { +template +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 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( 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:"<> 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"); diff --git a/src/footprint.cpp b/src/footprint.cpp index 24ee5aa..62fec78 100755 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -34,7 +34,6 @@ #include #include #include -#include namespace robot_costmap_2d { diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp index a882ae4..92b0297 100755 --- a/src/layered_costmap.cpp +++ b/src/layered_costmap.cpp @@ -37,7 +37,6 @@ *********************************************************************/ #include #include -#include #include #include #include diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 9f59484..fdc1869 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -39,7 +39,6 @@ #include #include #include -#include using namespace std; using namespace tf3; diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index 95effd1..20d91a2 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -3,7 +3,6 @@ #include #include #include -#include 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));