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

12
src/observation_buffer.cpp
View File

@@ -38,7 +38,7 @@
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
using namespace std;
using namespace tf3;
@@ -117,7 +117,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
return true;
}
void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
{
robot_geometry_msgs::PointStamped global_origin;
@@ -154,7 +154,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
observation_list_.front().raytrace_range_ = raytrace_range_;
observation_list_.front().obstacle_range_ = obstacle_range_;
sensor_msgs::PointCloud2 global_frame_cloud;
robot_sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
// tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
@@ -167,7 +167,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
global_frame_cloud.header.stamp = cloud.header.stamp;
// now we need to remove observations from the cloud that are below or above our height thresholds
sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
observation_cloud.height = global_frame_cloud.height;
observation_cloud.width = global_frame_cloud.width;
observation_cloud.fields = global_frame_cloud.fields;
@@ -177,12 +177,12 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
observation_cloud.is_dense = global_frame_cloud.is_dense;
unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
modifier.resize(cloud_size);
unsigned int point_count = 0;
// copy over the points that are within our height bounds
sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)