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

update function dataCallBack and file test static layer

Browse Source
This commit is contained in:
duongtd
2025-11-11 14:28:20 +07:00
parent 498b606e15
commit 19683269c3
7 changed files with 213 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
plugins/obstacle_layer.cpp
View File

@@ -1,5 +1,6 @@
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/utils.h>
// #include <tf2_ros/message_filter.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2/utils.h>
@@ -17,8 +18,10 @@ namespace costmap_2d
void ObstacleLayer::onInitialize()
{
getParams();
// ros::NodeHandle nh("~/" + name_), g_nh;
rolling_window_ = layered_costmap_->isRolling();
bool track_unknown_space;
// nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
@@ -115,7 +118,7 @@ void ObstacleLayer::onInitialize()
> sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_,50 , g_nh));
if (inf_is_valid)
{
@@ -190,13 +193,29 @@ void ObstacleLayer::onInitialize()
ObstacleLayer::~ObstacleLayer()
{}
// void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
// {
// enabled_ = config.enabled;
// footprint_clearing_enabled_ = config.footprint_clearing_enabled;
// max_obstacle_height_ = config.max_obstacle_height;
// combination_method_ = config.combination_method;
// }
bool StaticLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
YAML::Node layer = config["obstacle_layer"];
bool enabled = loadParam(layer, "enabled", true);
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
double max_obstacle_height = loadParam(layer, "max_obstacle_height", 2);
int combination_method = loadParam(layer, "combination_method", 1);
enabled_ = enabled;
footprint_clearing_enabled_ = footprint_clearing_enabled;
max_obstacle_height_ = max_obstacle_height;
combination_method_ = combination_method;
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
// void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
// const boost::shared_ptr<ObservationBuffer>& buffer)