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

10
plugins/directional_layer.cpp
View File

@@ -23,14 +23,14 @@ namespace costmap_2d
{
if (directional_areas_.empty())
throw "Has no map data";
nav_msgs::Path path;
robot_nav_msgs::Path path;
path.header.stamp = robot::Time::now();
path.header.frame_id = map_frame_;
path.poses = plan;
return this->laneFilter(directional_areas_, path);
}
void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
void DirectionalLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
{
if(!map_shutdown_)
{
@@ -110,7 +110,7 @@ namespace costmap_2d
}
}
bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path)
{
boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
if (new_map.empty())
@@ -159,7 +159,7 @@ namespace costmap_2d
if (!Yaw_list.empty())
{
laneFilter(X_List, Y_List, Yaw_list);
nav_msgs::OccupancyGrid lanes;
robot_nav_msgs::OccupancyGrid lanes;
convertToMap(costmap_, lanes, 0.65, 0.196);
//////////////////////////////////
@@ -177,7 +177,7 @@ namespace costmap_2d
}
void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
void DirectionalLayer::convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
{
dir_map.header = new_map_.header;
dir_map.header.stamp = robot::Time::now();

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)
{

12
plugins/static_layer.cpp
View File

@@ -170,16 +170,16 @@ void StaticLayer::handleImpl(const void* data,
const std::type_info& type,
const std::string& topic)
{
if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") {
incomingMap(*static_cast<const nav_msgs::OccupancyGrid*>(data));
} else if (type == typeid(map_msgs::OccupancyGridUpdate) && topic == "map_update") {
incomingUpdate(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == "map") {
incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == "map_update") {
incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
} else {
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
}
}
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
{
if(!map_shutdown_)
{
@@ -248,7 +248,7 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
}
}
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
void StaticLayer::incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& update)
{
if(!map_update_shutdown_)
{

16
plugins/voxel_layer.cpp
View File

@@ -36,7 +36,7 @@
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/voxel_layer.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <boost/dll/alias.hpp>
#define VOXEL_BITS 16
@@ -180,13 +180,13 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
{
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 (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
@@ -333,9 +333,9 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{