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

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