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

4
CMakeLists.txt
View File

@ -71,6 +71,8 @@ target_link_libraries(costmap_2d
xmlrpcpp # XMLRPC
yaml-cpp
dl
robot::node_handle
robot::console
)
# --- Include directories cho target ---
@ -125,6 +127,8 @@ target_link_libraries(plugins
${Boost_LIBRARIES}
yaml-cpp
robot_time
robot::node_handle
robot::console
)

4
config/costmap_params.yaml
View File

@ -18,14 +18,12 @@ costmap_2d:
type: create_voxel_layer
path_plugins: ./src/costmap_2d/libplugins.so
foot_print:
footprint:
- [0.3, 0.3]
- [0.3, -0.3]
- [-0.3, -0.3]
- [-0.3, 0.3]
layer_config_file_name: layer_params
transform_tolerance: 0.0
update_frequency: 1.0
width: 0.0

5
config/inflation_layer_params.yaml Normal file
View File

@ -0,0 +1,5 @@
inflation_layer:
enabled: true
inflate_unknown: false
cost_scaling_factor: 15.0
inflation_radius: 0.55

46
config/layer_params.yaml
View File

@ -1,46 +0,0 @@
static_layer:
enabled: true
first_map_only: false
subscribe_to_updates: false
track_unknown_space: true
use_maximum: false
lethal_cost_threshold: 100
unknown_cost_value: -1
trinary_costmap: true
base_frame_id: "map"
inflation_layer:
enabled: true
inflate_unknown: false
cost_scaling_factor: 15.0
inflation_radius: 0.55
obstacle_layer:
track_unknown_space: true
transform_tolerance: 0.2
topic: "map"
sensor_frame: laser_frame
observation_persistence: 0.0
expected_update_rate: 0.0
data_type: PointCloud
min_obstacle_height: 0.0
max_obstacle_height: 2.0
inf_is_valid: false
clearing: false
marking: true
obstacle_range: 2.5
raytrace_range: 3.0
footprint_clearing_enabled: true
combination_method: 1
voxel_layer:
enabled: true
footprint_clearing_enabled: true
max_obstacle_height: 3.0
origin_z: 0.0
z_resolution: 0.2
z_voxels: 16
unknown_threshold: 15.0
mark_threshold: 0
combination_method: 3

18
config/obstacle_layer_params.yaml Normal file
View File

@ -0,0 +1,18 @@
obstacle_layer:
track_unknown_space: true
transform_tolerance: 0.2
topic: "map"
sensor_frame: laser_frame
observation_persistence: 0.0
expected_update_rate: 0.0
data_type: PointCloud
min_obstacle_height: 0.0
max_obstacle_height: 2.0
inf_is_valid: false
clearing: false
marking: true
obstacle_range: 2.5
raytrace_range: 3.0
footprint_clearing_enabled: true
combination_method: 1

10
config/static_layer_params.yaml Normal file
View File

@ -0,0 +1,10 @@
static_layer:
enabled: true
first_map_only: false
subscribe_to_updates: false
track_unknown_space: true
use_maximum: false
lethal_cost_threshold: 100
unknown_cost_value: -1
trinary_costmap: true
base_frame_id: "map"

11
config/voxel_layer_params.yaml Normal file
View File

@ -0,0 +1,11 @@
voxel_layer:
enabled: true
footprint_clearing_enabled: true
max_obstacle_height: 3.0
origin_z: 0.0
z_resolution: 0.2
z_voxels: 16
unknown_threshold: 15.0
mark_threshold: 0
combination_method: 3

4
include/costmap_2d/costmap_2d_robot.h
View File

@ -52,6 +52,8 @@
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include <robot/node_handle.h>
class SuperValue : public XmlRpc::XmlRpcValue
{
public:
@ -269,7 +271,7 @@ private:
float footprint_padding_;
private:
void getParams(const std::string& config_file_name);
void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
};
// class Costmap2DROBOT
} // namespace costmap_2d

11
include/costmap_2d/footprint.h
View File

@ -43,6 +43,7 @@
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <robot/node_handle.h>
#include <xmlrpcpp/XmlRpcValue.h>
namespace costmap_2d
@ -118,11 +119,11 @@ std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius);
*/
bool makeFootprintFromString(const std::string& footprint_string, std::vector<geometry_msgs::Point>& footprint);
// /**
// * @brief Read the ros-params "footprint" and/or "robot_radius" from
// * the given NodeHandle using searchParam() to go up the tree.
// */
// std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh);
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandle& nh);
/**
* @brief Create the footprint from the given XmlRpcValue.

2
include/costmap_2d/inflation_layer.h
View File

@ -187,7 +187,7 @@ private:
double** cached_distances_;
double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
bool getParams(const std::string& config_file_name);
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around.
};

3
include/costmap_2d/layer.h
View File

@ -42,8 +42,7 @@
#include <costmap_2d/utils.h>
#include <string>
#include <tf3/buffer_core.h>
#include <robot/node_handle.h>
namespace costmap_2d
{
class LayeredCostmap;

2
include/costmap_2d/obstacle_layer.h
View File

@ -164,7 +164,7 @@ protected:
int combination_method_;
private:
bool getParams(const std::string& config_file_name);
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
};
} // namespace costmap_2d

3
include/costmap_2d/static_layer.h
View File

@ -88,8 +88,7 @@ protected:
unsigned char lethal_threshold_, unknown_cost_value_;
private:
bool getParams(const std::string& config_file_name);
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
};
} // namespace costmap_2d

2
include/costmap_2d/voxel_layer.h
View File

@ -83,7 +83,7 @@ protected:
virtual void resetMaps();
private:
bool getParams(const std::string& config_file_name);
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);

24
plugins/inflation_layer.cpp
View File

@ -71,7 +71,9 @@ InflationLayer::InflationLayer()
void InflationLayer::onInitialize()
{
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
current_ = true;
if (seen_)
@ -79,15 +81,14 @@ void InflationLayer::onInitialize()
seen_ = NULL;
seen_size_ = 0;
need_reinflation_ = false;
std::string config_file_name = name_ + ".yaml";
std::cout << "InflationLayer: " << config_file_name << std::endl;
getParams(config_file_name);
}
std::string config_file_name = "inflation_layer_params.yaml";
// std::cout << "InflationLayer: " << config_file_name << std::endl;
getParams(config_file_name, priv_nh);
matchSize();
}
bool InflationLayer::getParams(const std::string& config_file_name)
bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
@ -97,11 +98,22 @@ bool InflationLayer::getParams(const std::string& config_file_name)
YAML::Node layer = config["inflation_layer"];
double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
double inflation_radius = loadParam(layer, "inflation_radius", 0.55);
if (nh.hasParam("cost_scaling_factor"))
nh.getParam("cost_scaling_factor", cost_scaling_factor);
if (nh.hasParam("inflation_radius"))
nh.getParam("inflation_radius", inflation_radius);
setInflationParameters(inflation_radius, cost_scaling_factor);
bool enabled = loadParam(layer, "enabled", true);
bool inflate_unknown = loadParam(layer, "inflate_unknown", false);
if (nh.hasParam("enabled"))
nh.getParam("enabled", enabled);
if (nh.hasParam("inflate_unknown"))
nh.getParam("inflate_unknown", inflate_unknown);
if (enabled_ != enabled || inflate_unknown_ != inflate_unknown) {
enabled_ = enabled;
inflate_unknown_ = inflate_unknown;

43
plugins/obstacle_layer.cpp
View File

@ -56,20 +56,23 @@ namespace costmap_2d
void ObstacleLayer::onInitialize()
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
rolling_window_ = layered_costmap_->isRolling();
ObstacleLayer::matchSize();
current_ = true;
stop_receiving_data_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
std::string config_file_name = "obstacle_layer_params.yaml";
getParams(config_file_name, priv_nh);
}
ObstacleLayer::~ObstacleLayer()
{}
bool ObstacleLayer::getParams(const std::string& config_file_name)
bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
@ -104,6 +107,30 @@ bool ObstacleLayer::getParams(const std::string& config_file_name)
inf_is_valid = loadParam(layer,"inf_is_valid", false);
clearing = loadParam(layer,"clearing", false);
marking = loadParam(layer,"marking", true);
if (nh.hasParam("transform_tolerance"))
nh.getParam("transform_tolerance", transform_tolerance);
if (nh.hasParam("topic"))
nh.getParam("topic", topic);
if (nh.hasParam("sensor_frame"))
nh.getParam("sensor_frame", sensor_frame);
if (nh.hasParam("observation_persistence"))
nh.getParam("observation_persistence", observation_keep_time);
if (nh.hasParam("expected_update_rate"))
nh.getParam("expected_update_rate", expected_update_rate);
if (nh.hasParam("data_type"))
nh.getParam("data_type", data_type);
if (nh.hasParam("min_obstacle_height"))
nh.getParam("min_obstacle_height", min_obstacle_height);
if (nh.hasParam("max_obstacle_height"))
nh.getParam("max_obstacle_height", max_obstacle_height);
if (nh.hasParam("inf_is_valid"))
nh.getParam("inf_is_valid", inf_is_valid);
if (nh.hasParam("clearing"))
nh.getParam("clearing", clearing);
if (nh.hasParam("marking"))
nh.getParam("marking", marking);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
printf("Only topics that use point clouds or laser scans are currently supported\n");
@ -117,10 +144,18 @@ bool ObstacleLayer::getParams(const std::string& config_file_name)
double raytrace_range = 3.0;
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
int combination_method = loadParam(layer, "combination_method", 1);
if (nh.hasParam("obstacle_range"))
nh.getParam("obstacle_range", obstacle_range);
if (nh.hasParam("raytrace_range"))
nh.getParam("raytrace_range", raytrace_range);
if (nh.hasParam("footprint_clearing_enabled"))
nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled);
if (nh.hasParam("combination_method"))
nh.getParam("combination_method", combination_method);
// enabled_ = enabled;
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;

33
plugins/static_layer.cpp
View File

@ -62,25 +62,26 @@ StaticLayer::~StaticLayer()
void StaticLayer::onInitialize()
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
std::string config_file_name = "static_layer_params.yaml";
getParams(config_file_name, priv_nh);
}
bool StaticLayer::getParams(const std::string& config_file_name)
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["static_layer"];
// ===== 1. Load default từ layer (yaml / plugin config) =====
bool enabled = loadParam(layer, "enabled", true);
bool first_map_only = loadParam(layer, "first_map_only", false);
bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false);
@ -91,6 +92,26 @@ bool StaticLayer::getParams(const std::string& config_file_name)
bool trinary_costmap = loadParam(layer, "trinary_costmap", true);
std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map"));
// ===== 2. Override từ ROS param server (chỉ khi param tồn tại) =====
if (nh.hasParam("enabled"))
nh.getParam("enabled", enabled, enabled);
if (nh.hasParam("first_map_only"))
nh.getParam("first_map_only", first_map_only);
if (nh.hasParam("subscribe_to_updates"))
nh.getParam("subscribe_to_updates", subscribe_to_updates);
if (nh.hasParam("track_unknown_space"))
nh.getParam("track_unknown_space", track_unknown_space);
if (nh.hasParam("use_maximum"))
nh.getParam("use_maximum", use_maximum);
if (nh.hasParam("trinary_costmap"))
nh.getParam("trinary_costmap", trinary_costmap);
if (nh.hasParam("base_frame_id"))
nh.getParam("base_frame_id", base_frame_id);
if (nh.hasParam("lethal_cost_threshold"))
nh.getParam("lethal_cost_threshold", lethal_cost_threshold);
if (nh.hasParam("unknown_cost_value"))
nh.getParam("unknown_cost_value", unknown_cost_value);
first_map_only_ = first_map_only;
subscribe_to_updates_ = subscribe_to_updates;
track_unknown_space_ = track_unknown_space;

46
plugins/voxel_layer.cpp
View File

@ -53,17 +53,21 @@ namespace costmap_2d
void VoxelLayer::onInitialize()
{
robot::NodeHandle nh("~");
robot::NodeHandle priv_nh(nh, name_);
ObstacleLayer::onInitialize();
std::string config_file_name = name_ + ".yaml";
getParams(config_file_name);
std::string config_file_name = "voxel_layer_params.yaml";
getParams(config_file_name, priv_nh);
}
VoxelLayer::~VoxelLayer()
{}
bool VoxelLayer::getParams(const std::string& config_file_name)
bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
@ -74,12 +78,44 @@ bool VoxelLayer::getParams(const std::string& config_file_name)
enabled_ = loadParam(layer, "enabled", true);
footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true);
max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0);
size_z_ = loadParam(layer, "z_voxels", 10.0);
size_z_ = loadParam(layer, "z_voxels", 10);
origin_z_ = loadParam(layer, "origin_z", 0.0);
z_resolution_ = loadParam(layer, "z_resolution", 0.2);
unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
combination_method_ = loadParam(layer, "combination_method", 0.0);
int size_z, unknown_threshold, mark_threshold;
if (nh.hasParam("enabled"))
nh.getParam("enabled", enabled_);
if (nh.hasParam("footprint_clearing_enabled"))
nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled_);
if (nh.hasParam("max_obstacle_height"))
nh.getParam("max_obstacle_height", max_obstacle_height_);
if (nh.hasParam("z_voxels"))
{
nh.getParam("z_voxels", size_z);
size_z_ = size_z;
}
if (nh.hasParam("origin_z"))
nh.getParam("origin_z", origin_z_);
if (nh.hasParam("unknown_threshold"))
{
nh.getParam("unknown_threshold", unknown_threshold);
unknown_threshold_ = unknown_threshold;
}
if (nh.hasParam("mark_threshold"))
{
nh.getParam("mark_threshold", mark_threshold);
mark_threshold_ = mark_threshold;
}
if (nh.hasParam("combination_method"))
nh.getParam("combination_method", combination_method_);
this->matchSize();
}
catch (const YAML::BadFile& e) {

158
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,40 +88,108 @@ 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 {
struct PluginConfig
{
std::string name;
std::string type;
};
std::vector<PluginConfig> my_list;
if(nh.hasParam("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.type = plugin_i["type"].as<std::string>();
p.name = plugin_i["name"].as<std::string>();
my_list.push_back(p);
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"]) {
@ -126,12 +197,9 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
p.name = loadParam(plugin_node, "name", std::string(" "));
p.type = loadParam(plugin_node, "type", std::string(" "));
my_list.push_back(p);
std::cout << "Plugin to load: " << p.name << ", Type: " << p.type << 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)
{
@ -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);
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);
transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
if (map_update_thread_ != NULL)
{
map_update_thread_shutdown_ = true;
@ -175,6 +256,19 @@ 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 (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 (!layered_costmap_->isSizeLocked())
{
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
@ -184,6 +278,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
// 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;
@ -191,6 +287,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
}
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

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)
// {