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

update file obstacle_layer and file test plugin

Browse Source
This commit is contained in:
duongtd
2025-11-12 17:38:38 +07:00
parent 19683269c3
commit c94de60a7b
12 changed files with 393 additions and 517 deletions
Download Patch File Download Diff File Expand all files Collapse all files

504
plugins/obstacle_layer.cpp
View File

@@ -1,8 +1,8 @@
#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/convert.h>
#include <tf2/utils.h>
#include <boost/dll/alias.hpp>
@@ -18,196 +18,94 @@ 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());
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
ObstacleLayer::matchSize();
current_ = true;
stop_receiving_data_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID();
double transform_tolerance;
// nh.param("transform_tolerance", transform_tolerance, 0.2);
std::string topics_string;
// get the topics that we'll subscribe to from the parameter server
// nh.param("observation_sources", topics_string, std::string(""));
// ROS_INFO(" Subscribed to Topics: %s", 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;
while (ss >> source)
{
ros::NodeHandle source_node(nh, source);
// get the parameters for the specific topic
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
std::string topic, sensor_frame, data_type;
bool inf_is_valid, clearing, marking;
source_node.param("topic", topic, source);
source_node.param("sensor_frame", sensor_frame, std::string(""));
source_node.param("observation_persistence", observation_keep_time, 0.0);
source_node.param("expected_update_rate", expected_update_rate, 0.0);
source_node.param("data_type", data_type, std::string("PointCloud"));
source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
source_node.param("inf_is_valid", inf_is_valid, false);
source_node.param("clearing", clearing, false);
source_node.param("marking", marking, true);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
}
std::string raytrace_range_param_name, obstacle_range_param_name;
// get the obstacle range for the sensor
double obstacle_range = 2.5;
if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
{
source_node.getParam(obstacle_range_param_name, obstacle_range);
}
// get the raytrace range for the sensor
double raytrace_range = 3.0;
if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
{
source_node.getParam(raytrace_range_param_name, raytrace_range);
}
printf("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), 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)));
// check if we'll add this buffer to our marking observation buffers
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 source %s, topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f",
source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
// create a callback for the topic
if (data_type == "LaserScan")
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
> 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));
if (inf_is_valid)
{
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
}
else
{
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
}
observation_subscribers_.push_back(sub);
observation_notifiers_.push_back(filter);
observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
}
else if (data_type == "PointCloud")
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
if (inf_is_valid)
{
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
observation_subscribers_.push_back(sub);
observation_notifiers_.push_back(filter);
}
else
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
if (inf_is_valid)
{
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
observation_subscribers_.push_back(sub);
observation_notifiers_.push_back(filter);
}
if (sensor_frame != "")
{
std::vector < std::string > target_frames;
target_frames.push_back(global_frame_);
target_frames.push_back(sensor_frame);
observation_notifiers_.back()->setTargetFrames(target_frames);
}
}
*/
// dsrv_ = NULL;
// setupDynamicReconfigure(nh);
getParams();
}
// void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
// {
// dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
// dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
// [this](auto& config, auto level){ reconfigureCB(config, level); };
// dsrv_->setCallback(cb);
// }
ObstacleLayer::~ObstacleLayer()
{}
bool StaticLayer::getParams()
bool ObstacleLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
YAML::Node layer = config["obstacle_layer"];
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);
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;
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());
// 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 (!(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");
}
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);
// 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",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
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;
@@ -217,103 +115,133 @@ bool StaticLayer::getParams()
return true;
}
// void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
// const boost::shared_ptr<ObservationBuffer>& buffer)
// {
// // project the laser into a point cloud
// sensor_msgs::PointCloud2 cloud;
// cloud.header = message->header;
void ObstacleLayer::handleImpl(const void* data,
const std::type_info& type,
const std::string& topic)
{
if(!stop_receiving_data_)
{
if(observation_buffers_.empty()) return;
boost::shared_ptr<costmap_2d::ObservationBuffer> buffer = observation_buffers_.back();
if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") {
laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") {
laserScanValidInfCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") {
pointCloudCallback(*static_cast<const sensor_msgs::PointCloud*>(data), buffer);
} else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
pointCloud2Callback(*static_cast<const sensor_msgs::PointCloud2*>(data), buffer);
} else {
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
}
}
else
{
std::cout << "Stop receiving data!" << std::endl;
return;
}
}
// // project the scan into a point cloud
// try
// {
// projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
// }
// catch (tf2::TransformException &ex)
// {
// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
// ex.what());
// projector_.projectLaser(*message, cloud);
// }
// catch (std::runtime_error &ex)
// {
// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
// return; //ignore this message
// }
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// project the laser into a point cloud
sensor_msgs::PointCloud2 cloud;
cloud.header = message.header;
// printf("TEST PLUGIN OBSTACLE!!!\n");
// // buffer the point cloud
// buffer->lock();
// buffer->bufferCloud(cloud);
// buffer->unlock();
// }
// project the scan into a point cloud
try
{
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
}
catch (tf2::TransformException &ex)
{
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
ex.what());
projector_.projectLaser(message, cloud);
}
catch (std::runtime_error &ex)
{
printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
return; //ignore this message
}
// void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
// const boost::shared_ptr<ObservationBuffer>& buffer)
// {
// // Filter positive infinities ("Inf"s) to max_range.
// float epsilon = 0.0001; // a tenth of a millimeter
// sensor_msgs::LaserScan message = *raw_message;
// for (size_t i = 0; i < message.ranges.size(); i++)
// {
// float range = message.ranges[ i ];
// if (!std::isfinite(range) && range > 0)
// {
// message.ranges[ i ] = message.range_max - epsilon;
// }
// }
// buffer the point cloud
buffer->lock();
buffer->bufferCloud(cloud);
buffer->unlock();
}
// // project the laser into a point cloud
// sensor_msgs::PointCloud2 cloud;
// cloud.header = message.header;
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// printf("TEST PLUGIN OBSTACLE 2!!!\n");
// Filter positive infinities ("Inf"s) to max_range.
float epsilon = 0.0001; // a tenth of a millimeter
sensor_msgs::LaserScan message = raw_message;
for (size_t i = 0; i < message.ranges.size(); i++)
{
float range = message.ranges[ i ];
if (!std::isfinite(range) && range > 0)
{
message.ranges[ i ] = message.range_max - epsilon;
}
}
// // project the scan into a point cloud
// try
// {
// projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
// }
// catch (tf2::TransformException &ex)
// {
// ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
// global_frame_.c_str(), ex.what());
// projector_.projectLaser(message, cloud);
// }
// catch (std::runtime_error &ex)
// {
// ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
// return; //ignore this message
// }
// project the laser into a point cloud
sensor_msgs::PointCloud2 cloud;
cloud.header = message.header;
// // buffer the point cloud
// buffer->lock();
// buffer->bufferCloud(cloud);
// buffer->unlock();
// }
// project the scan into a point cloud
try
{
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
}
catch (tf2::TransformException &ex)
{
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
global_frame_.c_str(), ex.what());
projector_.projectLaser(message, cloud);
}
catch (std::runtime_error &ex)
{
printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
return; //ignore this message
}
// void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
// const boost::shared_ptr<ObservationBuffer>& buffer)
// {
// sensor_msgs::PointCloud2 cloud2;
// buffer the point cloud
buffer->lock();
buffer->bufferCloud(cloud);
buffer->unlock();
}
// if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
// {
// ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
// return;
// }
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// printf("TEST PLUGIN OBSTACLE 3!!!\n");
sensor_msgs::PointCloud2 cloud2;
// // buffer the point cloud
// buffer->lock();
// buffer->bufferCloud(cloud2);
// buffer->unlock();
// }
if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
{
printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
return;
}
// void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
// const boost::shared_ptr<ObservationBuffer>& buffer)
// {
// // buffer the point cloud
// buffer->lock();
// buffer->bufferCloud(*message);
// buffer->unlock();
// }
// buffer the point cloud
buffer->lock();
buffer->bufferCloud(cloud2);
buffer->unlock();
}
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// buffer the point cloud
buffer->lock();
buffer->bufferCloud(message);
buffer->unlock();
}
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
@@ -360,7 +288,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
// if the obstacle is too high or too far away from the robot we won't add it
if (pz > max_obstacle_height_)
{
printf("The point is too high");
printf("The point is too high\n");
continue;
}
@@ -371,7 +299,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
// if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
{
printf("The point is too far away");
printf("The point is too far away\n");
continue;
}
@@ -379,7 +307,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
unsigned int mx, my;
if (!worldToMap(px, py, mx, my))
{
printf("Computing map coords failed");
printf("Computing map coords failed\n");
continue;
}
@@ -485,7 +413,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
if (!worldToMap(ox, oy, x0, y0))
{
printf(
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
ox, oy);
return;
}
@@ -556,29 +484,19 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
}
}
// void ObstacleLayer::activate()
// {
// // if we're stopped we need to re-subscribe to topics
// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
// {
// if (observation_subscribers_[i] != NULL)
// observation_subscribers_[i]->subscribe();
// }
// for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
// {
// if (observation_buffers_[i])
// observation_buffers_[i]->resetLastUpdated();
// }
// }
// void ObstacleLayer::deactivate()
// {
// for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
// {
// if (observation_subscribers_[i] != NULL)
// observation_subscribers_[i]->unsubscribe();
// }
// }
void ObstacleLayer::activate()
{
stop_receiving_data_ = false;
for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
{
if (observation_buffers_[i])
observation_buffers_[i]->resetLastUpdated();
}
}
void ObstacleLayer::deactivate()
{
stop_receiving_data_ = true;
}
void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
double* min_x, double* min_y, double* max_x, double* max_y)
@@ -590,16 +508,16 @@ void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double
touch(ex, ey, min_x, min_y, max_x, max_y);
}
// void ObstacleLayer::reset()
// {
void ObstacleLayer::reset()
{
// deactivate();
// resetMaps();
// current_ = true;
// activate();
// }
}
// Export factory function
static PluginPtr create_obstacle_plugin() {
static PluginCostmapLayerPtr create_obstacle_plugin() {
return std::make_shared<ObstacleLayer>();
}