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

update obstacle_layer

Browse Source
This commit is contained in:
duongtd
2026-01-06 17:33:13 +07:00
parent ae469e3271
commit 800e5c1735
6 changed files with 637 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

283
plugins/obstacle_layer.cpp
View File

@@ -77,118 +77,145 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
try {
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
YAML::Node layer = config["obstacle_layer"];
YAML::Node layer = config["obstacle_layer"];
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
double max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
int combination_method = loadParam(layer, "combination_method", 1);
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
// // get the topics that we'll subscribe to from the parameter server
// std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
// printf("Subscribed to Topics: %s\n", topics_string.c_str());
if (nh.hasParam("track_unknown_space"))
nh.getParam("track_unknown_space", track_unknown_space);
if (nh.hasParam("transform_tolerance"))
nh.getParam("transform_tolerance", transform_tolerance);
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("combination_method"))
nh.getParam("combination_method", combination_method);
// get the parameters for the specific topic
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
bool inf_is_valid = false, clearing=false, marking=true;
topic = loadParam(layer,"topic", topic);
sensor_frame = loadParam(layer,"sensor_frame", std::string(""));
observation_keep_time = loadParam(layer,"observation_persistence", 0.0);
expected_update_rate = loadParam(layer,"expected_update_rate", 0.0);
data_type = loadParam(layer,"data_type", std::string("PointCloud"));
min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0);
max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
inf_is_valid = loadParam(layer,"inf_is_valid", false);
clearing = loadParam(layer,"clearing", false);
marking = loadParam(layer,"marking", true);
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
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);
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;
combination_method_ = combination_method;
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
printf("Only topics that use point clouds or laser scans are currently supported\n");
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
// get the topics that we'll subscribe to from the parameter server
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());
// now we need to split the topics based on whitespace which we can use a stringstream for
std::stringstream ss(topics_string);
std::string source;
callback_infos_.clear();
while (ss >> source)
{
// get the parameters for the specific topic
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
bool inf_is_valid = false, clearing=false, marking=true;
robot::NodeHandle priv_nh(nh, source);
topic = loadParam(layer[source],"topic", topic);
sensor_frame = loadParam(layer[source],"sensor_frame", std::string(""));
observation_keep_time = loadParam(layer[source],"observation_persistence", 0.0);
expected_update_rate = loadParam(layer[source],"expected_update_rate", 0.0);
data_type = loadParam(layer[source],"data_type", std::string("PointCloud"));
min_obstacle_height = loadParam(layer[source],"min_obstacle_height", 0.0);
max_obstacle_height = loadParam(layer[source],"max_obstacle_height", 2.0);
inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
clearing = loadParam(layer[source],"clearing", false);
marking = loadParam(layer[source],"marking", true);
if (priv_nh.hasParam("topic"))
priv_nh.getParam("topic", topic);
if (priv_nh.hasParam("sensor_frame"))
priv_nh.getParam("sensor_frame", sensor_frame);
if (priv_nh.hasParam("observation_persistence"))
priv_nh.getParam("observation_persistence", observation_keep_time);
if (priv_nh.hasParam("expected_update_rate"))
priv_nh.getParam("expected_update_rate", expected_update_rate);
if (priv_nh.hasParam("data_type"))
priv_nh.getParam("data_type", data_type);
if (priv_nh.hasParam("min_obstacle_height"))
priv_nh.getParam("min_obstacle_height", min_obstacle_height);
if (priv_nh.hasParam("max_obstacle_height"))
priv_nh.getParam("max_obstacle_height", max_obstacle_height);
if (priv_nh.hasParam("inf_is_valid"))
priv_nh.getParam("inf_is_valid", inf_is_valid);
if (priv_nh.hasParam("clearing"))
priv_nh.getParam("clearing", clearing);
if (priv_nh.hasParam("marking"))
priv_nh.getParam("marking", marking);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
robot::log_error("Only topics that use point clouds or laser scans are currently supported\n");
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
}
CallBackInfo info_tmp;
info_tmp.data_type = data_type;
info_tmp.topic = topic;
info_tmp.inf_is_valid = inf_is_valid;
callback_infos_.push_back(info_tmp);
std::string raytrace_range_param_name, obstacle_range_param_name;
double obstacle_range = 2.5;
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
double raytrace_range = 3.0;
raytrace_range = loadParam(layer[source],"raytrace_range", raytrace_range);
if (priv_nh.hasParam("obstacle_range"))
priv_nh.getParam("obstacle_range", obstacle_range);
if (priv_nh.hasParam("raytrace_range"))
priv_nh.getParam("raytrace_range", raytrace_range);
// enabled_ = enabled;
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
sensor_frame.c_str());
// create an observation buffer
observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
sensor_frame, transform_tolerance)));
if (marking)
marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
printf(
"Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
}
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
std::string raytrace_range_param_name, obstacle_range_param_name;
double obstacle_range = 2.5;
obstacle_range = loadParam(layer,"obstacle_range", obstacle_range);
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;
combination_method_ = combination_method;
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
sensor_frame.c_str());
// create an observation buffer
observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
sensor_frame, transform_tolerance)));
if (marking)
marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
printf(
"Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
return true;
}
void ObstacleLayer::handleImpl(const void* data,
@@ -197,18 +224,52 @@ void ObstacleLayer::handleImpl(const void* data,
{
if(!stop_receiving_data_)
{
if(observation_buffers_.empty()) return;
boost::shared_ptr<ObservationBuffer> buffer = observation_buffers_.back();
if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser") {
if(observation_buffers_.empty() || callback_infos_.empty()) return;
int size_callback = static_cast<int>(callback_infos_.size());
for(int i = 0; i < size_callback; i++)
{
boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];
if (type == typeid(robot_sensor_msgs::LaserScan) &&
callback_infos_[i].data_type == "LaserScan" &&
topic == callback_infos_[i].topic &&
!callback_infos_[i].inf_is_valid)
{
laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser_valid") {
}
else if (type == typeid(robot_sensor_msgs::LaserScan) &&
callback_infos_[i].data_type == "LaserScan" &&
topic == callback_infos_[i].topic &&
callback_infos_[i].inf_is_valid)
{
laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(robot_sensor_msgs::PointCloud) && topic == "pcl_cb") {
}
else if (type == typeid(robot_sensor_msgs::PointCloud) &&
callback_infos_[i].data_type == "PointCloud" &&
topic == callback_infos_[i].topic)
{
if (callback_infos_[i].inf_is_valid)
{
printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}
pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
} else if (type == typeid(robot_sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
}
else if (type == typeid(robot_sensor_msgs::PointCloud2) &&
callback_infos_[i].data_type == "PointCloud2" &&
topic == callback_infos_[i].topic)
{
if (callback_infos_[i].inf_is_valid)
{
printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}
pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
} else {
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
}
else
{
std::cout << "obstacle_layer: Unknown type: " << type.name() << std::endl;
}
}
}
else

43
plugins/static_layer.cpp
View File

@@ -43,6 +43,7 @@
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/dll/alias.hpp>
#include <fstream>
using robot_costmap_2d::NO_INFORMATION;
@@ -82,7 +83,7 @@ bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHand
YAML::Node layer = config["static_layer"];
// ===== 1. Load default từ layer (yaml / plugin config) =====
bool enabled = loadParam(layer, "enabled", true);
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);
bool track_unknown_space = loadParam(layer, "track_unknown_space", true);
@@ -90,11 +91,14 @@ bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHand
int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100);
int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1);
bool trinary_costmap = loadParam(layer, "trinary_costmap", true);
std::string map_topic = loadParam(layer, "map_topic", std::string("map"));
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("map_topic"))
nh.getParam("map_topic", map_topic);
if (nh.hasParam("first_map_only"))
nh.getParam("first_map_only", first_map_only);
if (nh.hasParam("subscribe_to_updates"))
@@ -112,6 +116,7 @@ bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHand
if (nh.hasParam("unknown_cost_value"))
nh.getParam("unknown_cost_value", unknown_cost_value);
map_topic_ = map_topic;
first_map_only_ = first_map_only;
subscribe_to_updates_ = subscribe_to_updates;
track_unknown_space_ = track_unknown_space;
@@ -152,15 +157,25 @@ void StaticLayer::matchSize()
unsigned char StaticLayer::interpretValue(unsigned char value)
{
// check if the static value is above the unknown or lethal thresholds
if (track_unknown_space_ && value == unknown_cost_value_)
return NO_INFORMATION;
{
return NO_INFORMATION;
}
else if (!track_unknown_space_ && value == unknown_cost_value_)
{
return FREE_SPACE;
}
else if (value >= lethal_threshold_)
return LETHAL_OBSTACLE;
{
return LETHAL_OBSTACLE;
}
else if (trinary_costmap_)
return FREE_SPACE;
{
return FREE_SPACE;
}
double scale = (double) value / lethal_threshold_;
return scale * LETHAL_OBSTACLE;
@@ -170,9 +185,9 @@ void StaticLayer::handleImpl(const void* data,
const std::type_info& type,
const std::string& topic)
{
if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == "map") {
if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == map_topic_) {
incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == "map_update") {
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == map_topic_ + "_updates") {
incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
} else {
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
@@ -215,6 +230,15 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
}
unsigned int index = 0;
// // 1. Tạo file (nếu chưa có) + mở để ghi
// std::ofstream file("/home/duongtd/ros_test_ws/src/third_party/costmap_2d/data.txt", std::ios::app);
// // 2. Kiểm tra mở file thành công
// if (!file)
// {
// std::cerr << "Cannot create file\n";
// return ;
// }
// initialize the costmap with static data
for (unsigned int i = 0; i < size_y; ++i)
@@ -223,9 +247,16 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
{
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
// printf("%d , ",costmap_[index]);
// 3. Ghi giá trị biến
// file << static_cast<int>(costmap_[index]) << " , ";
++index;
// printf("%d , ",costmap_[index]);
}
// file << std::endl;
}
// // 4. Đóng file
// file.close();
map_frame_ = new_map.header.frame_id;
// we have a new map, update full size of map