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

Compare commits

base: DuongTD:9208c8bcdc6cfa8b3b31db8c41785939b49f2d6d
Branches Tags
DuongTD:main
..
compare: DuongTD:main
Branches Tags
DuongTD:main

3 Commits
9208c8bcdc ... main

Author SHA1 Message Date
hoangson02
2fcd211ccf update 2026-03-03 07:26:47 +00:00
HiepLM
3d621de809 fix bugs 2026-02-26 14:54:23 +07:00
HiepLM
1f9e9f1398 update 2026-02-26 14:52:33 +07:00
1 changed files with 48 additions and 50 deletions
Expand all files Collapse all files

96
src/costmap_2d_robot.cpp
View File

@@ -82,7 +82,6 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
name_ = name;
std::string config_file_name = "costmap_params.yaml";
getParams(config_file_name, name_, nh);
nh.printParams();
// create a thread to handle updating the map
stop_updates_ = false;
@@ -130,6 +129,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::st
{
if (last_error + robot::Duration(5.0) < robot::Time::now())
{
std::string all_frames_string = tf_.allFramesAsString();
robot::log_info("[%s:%d]\n INFO: tf allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
// std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
robot::log_warning("[%s:%d] %0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
__FILE__, __LINE__, robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
@@ -333,21 +334,19 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_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());
robot::NodeHandle costmap_nh(nh, costmap_name);
robot::NodeHandle costmap_plugin_nh(costmap_nh, plugin_name);
robot::NodeHandle plugin_nh(nh, plugin_name);
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);
move_parameter(plugin_nh, costmap_plugin_nh, "map_topic", map_topic);
move_parameter(plugin_nh, costmap_plugin_nh, "unknown_cost_value", unknown_cost_value);
move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
}
else if(plugin_type == "VoxelLayer")
{
@@ -357,24 +356,23 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
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"))
move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
if(plugin_nh.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());
move_parameter(plugin_nh, costmap_plugin_nh, "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());
robot::NodeHandle plugin_nh_element(plugin_nh, source);
robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
std::string topic;
std::string data_type;
bool clearing;
@@ -382,14 +380,14 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
bool inf_is_valid;
double min_obstacle_height;
double max_obstacle_height;
move_parameter(priv_nh_3, target_layer_2, "topic", topic);
// robot::log_error("topic: %s", topic.c_str());
move_parameter(priv_nh_3, target_layer_2, "data_type", data_type);
move_parameter(priv_nh_3, target_layer_2, "clearing", clearing);
move_parameter(priv_nh_3, target_layer_2, "marking", marking);
move_parameter(priv_nh_3, target_layer_2, "inf_is_valid", inf_is_valid);
move_parameter(priv_nh_3, target_layer_2, "min_obstacle_height", min_obstacle_height);
move_parameter(priv_nh_3, target_layer_2, "max_obstacle_height", max_obstacle_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
}
}
}
@@ -399,21 +397,21 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
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"))
move_parameter(plugin_nh, costmap_plugin_nh, "max_obstacle_height", max_obstacle_height);
move_parameter(plugin_nh, costmap_plugin_nh, "raytrace_range", raytrace_range);
move_parameter(plugin_nh, costmap_plugin_nh, "obstacle_range", obstacle_range);
move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
if(plugin_nh.hasParam("observation_sources"))
{
std::string topics_string;
move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string);
move_parameter(plugin_nh, costmap_plugin_nh, "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::NodeHandle plugin_nh_element(plugin_nh, source);
robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
std::string topic;
std::string data_type;
bool clearing;
@@ -421,14 +419,14 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
bool inf_is_valid;
double min_obstacle_height;
double max_obstacle_height;
move_parameter(priv_nh_3, target_layer_2, "topic", topic);
// robot::log_error("topic: %s", topic.c_str());
move_parameter(priv_nh_3, target_layer_2, "data_type", data_type);
move_parameter(priv_nh_3, target_layer_2, "clearing", clearing);
move_parameter(priv_nh_3, target_layer_2, "marking", marking);
move_parameter(priv_nh_3, target_layer_2, "inf_is_valid", inf_is_valid);
move_parameter(priv_nh_3, target_layer_2, "min_obstacle_height", min_obstacle_height);
move_parameter(priv_nh_3, target_layer_2, "max_obstacle_height", max_obstacle_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
}
}
}
@@ -436,8 +434,8 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name,
{
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);
move_parameter(plugin_nh, costmap_plugin_nh, "cost_scaling_factor", cost_scaling_factor);
move_parameter(plugin_nh, costmap_plugin_nh, "inflation_radius", inflation_radius);
}
}
@@ -696,7 +694,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
}
catch (tf3::ExtrapolationException& ex)
{
robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
// robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
return false;
}
// ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);