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

hiep sua ten file

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:56:35 +07:00
parent 2c3d7d586d
commit 4246453ae6
14 changed files with 97 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
plugins/obstacle_layer.cpp
View File

@@ -38,7 +38,7 @@
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <tf3/exceptions.h>
#include <boost/dll/alias.hpp>
@@ -199,14 +199,14 @@ void ObstacleLayer::handleImpl(const void* data,
{
if(observation_buffers_.empty()) return;
boost::shared_ptr<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);
if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser") {
laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser_valid") {
laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(robot_sensor_msgs::PointCloud) && topic == "pcl_cb") {
pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
} else if (type == typeid(robot_sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
} else {
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
}
@@ -218,11 +218,11 @@ void ObstacleLayer::handleImpl(const void* data,
}
}
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// project the laser into a point cloud
sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2 cloud;
cloud.header = message.header;
// project the scan into a point cloud
@@ -248,12 +248,12 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
buffer->unlock();
}
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
void ObstacleLayer::laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& 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;
robot_sensor_msgs::LaserScan message = raw_message;
for (size_t i = 0; i < message.ranges.size(); i++)
{
float range = message.ranges[ i ];
@@ -264,7 +264,7 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
}
// project the laser into a point cloud
sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2 cloud;
cloud.header = message.header;
// project the scan into a point cloud
@@ -290,12 +290,12 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
buffer->unlock();
}
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
void ObstacleLayer::pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
sensor_msgs::PointCloud2 cloud2;
robot_sensor_msgs::PointCloud2 cloud2;
if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
if (!robot_sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
{
printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
return;
@@ -307,7 +307,7 @@ void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
buffer->unlock();
}
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& message,
void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
// buffer the point cloud
@@ -346,13 +346,13 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
{
const Observation& obs = *it;
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
@@ -479,7 +479,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
{
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
const robot_sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
// get the map coordinates of the origin of the sensor
unsigned int x0, y0;
@@ -500,8 +500,8 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
touch(ox, oy, min_x, min_y, max_x, max_y);
// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
{