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

update load param using node handle

Browse Source
This commit is contained in:
duongtd
2025-12-11 17:16:46 +07:00
parent 22cfd07519
commit c53db2280e
20 changed files with 397 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files

212
src/costmap_2d_robot.cpp
View File

@@ -66,8 +66,11 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
map_update_thread_(NULL),
footprint_padding_(0.0)
{
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name);
name_ = name;
std::string config_file_name = "costmap_params.yaml";
getParams(config_file_name, priv_nh);
// create a thread to handle updating the map
stop_updates_ = false;
@@ -75,7 +78,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
stopped_ = false;
}
void Costmap2DROBOT::getParams(const std::string& config_file_name)
void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeHandle& nh)
{
try
{
@@ -85,53 +88,118 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["costmap_2d"];
global_frame_ = loadParam(layer, "global_frame", std::string("map"));
robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link"));
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 (nh.hasParam("robot_base_frame"))
nh.getParam("robot_base_frame", robot_base_frame);
global_frame_ = global_frame;
robot_base_frame_ = robot_base_frame;
robot::Time last_error = robot::Time::now();
std::string tf_error;
// we need to make sure that the transform between the robot base frame and the global frame is available
while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
{
if (last_error + robot::Duration(5.0) < robot::Time::now())
{
// std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
printf("%0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = robot::Time::now();
}
// The error string will accumulate and errors will typically be the same, so the last
// will do for the warning above. Reset the string here to avoid accumulation.
tf_error.clear();
}
// // we need to make sure that the transform between the robot base frame and the global frame is available
// while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
// {
// if (last_error + robot::Duration(5.0) < robot::Time::now())
// {
// // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
// printf("%0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
// robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
// last_error = robot::Time::now();
// }
// // The error string will accumulate and errors will typically be the same, so the last
// // will do for the warning above. Reset the string here to avoid accumulation.
// tf_error.clear();
// }
// check if we want a rolling window version of the costmap
bool rolling_window, track_unknown_space;
rolling_window = loadParam(layer, "rolling_window", false);
track_unknown_space = loadParam(layer, "track_unknown_space", false);
bool rolling_window = loadParam(layer, "rolling_window", false);
bool track_unknown_space = loadParam(layer, "track_unknown_space", false);
std::string path_plugins = loadParam(layer, "path_plugins", std::string(" "));
if (nh.hasParam("rolling_window"))
nh.getParam("rolling_window", rolling_window);
if (nh.hasParam("track_unknown_space"))
nh.getParam("track_unknown_space", track_unknown_space);
if (nh.hasParam("path_plugins"))
nh.getParam("path_plugins", path_plugins);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
struct PluginConfig {
std::string name;
std::string type;
struct PluginConfig
{
std::string name;
std::string type;
};
std::vector<PluginConfig> my_list;
if (layer["plugins"] && layer["plugins"].IsSequence())
if(nh.hasParam("plugins"))
{
for (const auto& plugin_node : layer["plugins"]) {
my_list.clear();
YAML::Node my_plugins = nh.getParamValue("plugins");
if (my_plugins && my_plugins.IsSequence())
{
for (size_t i = 0; i < my_plugins.size(); ++i)
{
YAML::Node plugin_i = my_plugins[i].as<YAML::Node>();
PluginConfig p;
p.name = loadParam(plugin_node, "name", std::string(" "));
p.type = loadParam(plugin_node, "type", std::string(" "));
p.type = plugin_i["type"].as<std::string>();
p.name = plugin_i["name"].as<std::string>();
my_list.push_back(p);
std::cout << "Plugin to load: " << p.name << ", Type: " << p.type << std::endl;
if (!plugin_i.IsMap())
{
std::cerr << "Plugins must be specified as maps. We'll use the default recovery behaviors instead." << std::endl;
return;
}
if (!plugin_i["name"] || !plugin_i["type"])
{
std::cerr << "Plugins must have a name and a type. Using the default recovery behaviors instead." << std::endl;
return;
}
// check for recovery behaviors with the same name
std::string name_i = plugin_i["name"].as<std::string>();
for (size_t j = i + 1; j < my_plugins.size(); ++j)
{
YAML::Node plugin_j = my_plugins[j].as<YAML::Node>();
if (plugin_j.IsMap() && plugin_j["name"])
{
std::string name_j = plugin_j["name"].as<std::string>();
if (name_i == name_j)
{
std::cerr << "A plugin with the name " << name_i << " already exists, this is not allowed. Using the default recovery behaviors instead." << std::endl;
return;
}
}
}
}
}
}
else
{
my_list.clear();
if (layer["plugins"] && layer["plugins"].IsSequence())
{
for (const auto& plugin_node : layer["plugins"]) {
PluginConfig p;
p.name = loadParam(plugin_node, "name", std::string(" "));
p.type = loadParam(plugin_node, "type", std::string(" "));
my_list.push_back(p);
}
}
}
std::string path_plugins = loadParam(layer, "path_plugins", std::string(" "));
std::string layer_config_file_name = loadParam(layer, "layer_config_file_name", std::string("layer_params"));
for (auto& info : my_list)
{
@@ -144,20 +212,33 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
);
PluginLayerPtr plugin = creators_.back()();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, layer_config_file_name, &tf_);
plugin->initialize(layered_costmap_,name_ + "/" + info.name, &tf_);
layered_costmap_->addPlugin(plugin);
}
catch (std::exception &ex)
{
printf("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", info.name.c_str(), ex.what());
return;
}
}
std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
new_footprint = loadFootprint(layer["foot_print"], new_footprint);
setUnpaddedRobotFootprint(new_footprint);
new_footprint = loadFootprint(layer["footprint"], new_footprint);
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
if (nh.hasParam("footprint"))
{
std::cout <<"FOOT PRINT ROBOT:"<<std::endl;
new_footprint = makeFootprintFromParams(nh);
}
if (nh.hasParam("transform_tolerance"))
nh.getParam("transform_tolerance", transform_tolerance_);
setUnpaddedRobotFootprint(new_footprint);
if (map_update_thread_ != NULL)
{
map_update_thread_shutdown_ = true;
@@ -175,27 +256,44 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
double origin_x = loadParam(layer, "origin_x", 0.0);
double origin_y = loadParam(layer, "origin_y", 0.0);
if (!layered_costmap_->isSizeLocked())
{
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
if (nh.hasParam("update_frequency"))
nh.getParam("update_frequency", map_update_frequency);
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 the padding has changed, call setUnpaddedRobotFootprint() to
// re-apply the padding.
float footprint_padding = loadParam(layer, "footprint_padding", 0.0);
if (footprint_padding_ != footprint_padding)
{
footprint_padding_ = footprint_padding;
setUnpaddedRobotFootprint(unpadded_footprint_);
}
if (!layered_costmap_->isSizeLocked())
{
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
double robot_radius = loadParam(layer, "robot_radius", 0.0);
readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
// If the padding has changed, call setUnpaddedRobotFootprint() to
// re-apply the padding.
float footprint_padding = loadParam(layer, "footprint_padding", 0.0);
if (nh.hasParam("footprint_padding"))
nh.getParam("footprint_padding", footprint_padding);
if (footprint_padding_ != footprint_padding)
{
footprint_padding_ = footprint_padding;
setUnpaddedRobotFootprint(unpadded_footprint_);
}
// only construct the thread if the frequency is positive
if(map_update_frequency > 0.0)
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency));
double robot_radius = loadParam(layer, "robot_radius", 0.0);
if (nh.hasParam("robot_radius"))
nh.getParam("robot_radius", robot_radius);
readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
// only construct the thread if the frequency is positive
if(map_update_frequency > 0.0)
map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency));
}
catch (const YAML::BadFile& e)
{

57
src/footprint.cpp
View File

@@ -207,45 +207,26 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
return true;
}
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
// {
// std::string full_param_name;
// std::string full_radius_param_name;
// std::vector<geometry_msgs::Point> points;
std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh)
{
YAML::Node ft = nh.getParamValue("footprint");
std::vector<geometry_msgs::Point> footprint;
// if (nh.searchParam("footprint", full_param_name))
// {
// XmlRpc::XmlRpcValue footprint_xmlrpc;
// nh.getParam(full_param_name, footprint_xmlrpc);
// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
// footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
// {
// if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
// {
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
// {
// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
// writeFootprintToParam(nh, points);
// return points;
// }
// }
// if (nh.searchParam("robot_radius", full_radius_param_name))
// {
// double robot_radius;
// nh.param(full_radius_param_name, robot_radius, 1.234);
// points = makeFootprintFromRadius(robot_radius);
// nh.setParam("robot_radius", robot_radius);
// }
// // Else neither param was found anywhere this knows about, so
// // defaults will come from dynamic_reconfigure stuff, set in
// // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
// return points;
// }
if (ft && ft.IsSequence())
{
for (size_t i = 0; i < ft.size(); ++i)
{
auto pt = ft[i];
geometry_msgs::Point p;
p.x = pt[0].as<double>();
p.y = pt[1].as<double>();
p.z = 0.0;
footprint.push_back(p);
std::cout << "Pose[" << i << "]" << p.x << ", " << p.y << std::endl;
}
}
return footprint;
}
// void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
// {