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

update file costmap_2d_robot and file config params

Browse Source
This commit is contained in:
duongtd
2025-12-03 11:36:06 +07:00
parent 64db092d46
commit 4fb2554291
16 changed files with 169 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
src/costmap_2d_robot.cpp
View File

@@ -66,7 +66,8 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
map_update_thread_(NULL),
footprint_padding_(0.0)
{
getParams("config.yaml");
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
// create a thread to handle updating the map
stop_updates_ = false;
@@ -80,14 +81,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
{
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
if(path_source != " ")
{
std::cout << "Path source: " << path_source << std::endl;
}
else
{
std::cout<< config_file_name << " file not found!" << std::endl;
}
YAML::Node config = YAML::LoadFile(path_source);
@@ -99,27 +92,28 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
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())
{
printf("%f: 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())
// {
// printf("%f: 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, always_send_full_costmap;
bool rolling_window, track_unknown_space;
rolling_window = loadParam(layer, "rolling_window", false);
track_unknown_space = loadParam(layer, "track_unknown_space", false);
always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
struct PluginConfig {
std::string name;
std::string type;
@@ -136,8 +130,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
}
}
std::string path = loadParam(layer, "path", std::string(" "));
std::cout << "Plugin to load: " << path << std::endl;
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)
{
@@ -146,22 +140,24 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
// copyParentParameters(pname, type, private_nh);
creators_.push_back(
boost::dll::import_alias<PluginLayerPtr()>(
path, info.type, boost::dll::load_mode::append_decorations)
path_plugins, info.type, boost::dll::load_mode::append_decorations)
);
PluginLayerPtr plugin = creators_.back()();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_);
plugin->initialize(layered_costmap_, layer_config_file_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", ex.what());
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());
}
}
// setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
new_footprint = loadFootprint(layer["foot_print"], new_footprint);
setUnpaddedRobotFootprint(new_footprint);
transform_tolerance_ = loadParam(layer, "transform_tolerance", false);
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
if (map_update_thread_ != NULL)
{
map_update_thread_shutdown_ = true;
@@ -170,14 +166,14 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
map_update_thread_ = NULL;
}
map_update_thread_shutdown_ = false;
double map_update_frequency = loadParam(layer, "update_frequency", false);
double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
// find size parameters
double map_width_meters = loadParam(layer, "width", false);
double map_height_meters = loadParam(layer, "height", false);
double resolution = loadParam(layer, "resolution", false);
double origin_x = loadParam(layer, "origin_x", false);
double origin_y = loadParam(layer, "origin_y", false);
double map_width_meters = loadParam(layer, "width", 0.0);
double map_height_meters = loadParam(layer, "height", 0.0);
double resolution = loadParam(layer, "resolution", 0.0);
double origin_x = loadParam(layer, "origin_x", 0.0);
double origin_y = loadParam(layer, "origin_y", 0.0);
if (!layered_costmap_->isSizeLocked())
{
@@ -195,7 +191,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
}
double robot_radius = loadParam(layer, "robot_radius", 0.0);
std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
// only construct the thread if the frequency is positive
@@ -279,7 +274,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency)
while (!map_update_thread_shutdown_)
{
updateMap();
std::cout << "Costmap2DROBOT::mapUpdateLoop updateMap\n";
r.sleep();
// make sure to sleep for the remainder of our cycle time
if (r.cycleTime() > robot::Duration(1 / frequency))

6
src/footprint.cpp
View File

@@ -27,13 +27,13 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include<costmap_2d/costmap_math.h>
#include <costmap_2d/costmap_math.h>
#include <boost/tokenizer.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <costmap_2d/footprint.h>
#include <costmap_2d/array_parser.h>
#include<geometry_msgs/Point32.h>
#include <geometry_msgs/Point32.h>
namespace costmap_2d
{
@@ -207,8 +207,6 @@ 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;