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

otimal deep coppy obj

Browse Source
This commit is contained in:
duongtd
2026-07-14 11:08:23 +07:00
parent 6a9834d3a8
commit bdbb03aa51
17 changed files with 702 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

154
src/observation_buffer.cpp
View File

@@ -40,6 +40,8 @@
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <cstring>
using namespace std;
using namespace tf3;
@@ -97,6 +99,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
Observation& obs = *obs_it;
if (!obs.cloud_handle_.unique())
{
obs.cloud_handle_ = boost::make_shared<robot_sensor_msgs::PointCloud2>(*obs.cloud_);
obs.cloud_ = obs.cloud_handle_.get();
}
robot_geometry_msgs::PointStamped origin;
origin.header.frame_id = global_frame_;
origin.header.stamp = data_convert::convertTime(transform_time);
@@ -137,9 +145,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
{
robot_geometry_msgs::PointStamped global_origin;
// create a new observation on the list to be populated
observation_list_.push_front(Observation());
Observation observation;
// check whether the origin frame has been set explicitly or whether we should get it from the cloud
string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
@@ -154,81 +160,68 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
local_origin.point.y = 0;
local_origin.point.z = 0;
// tf3_buffer_.transform(local_origin, global_origin, global_frame_);
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
local_origin.header.frame_id, // frame nguồn
tf3::Time()
// data_convert::convertTime(cloud.header.stamp)
);
tf3::doTransform(local_origin, global_origin, tfm_1);
const tf3::TransformStampedMsg cloud_transform = tf3_buffer_.lookupTransform(
global_frame_, cloud.header.frame_id, tf3::Time());
if (origin_frame == cloud.header.frame_id)
tf3::doTransform(local_origin, global_origin, cloud_transform);
else
tf3::doTransform(
local_origin, global_origin,
tf3_buffer_.lookupTransform(global_frame_, origin_frame, tf3::Time()));
/////////////////////////////////////////////////
///////////chú ý hàm này/////////////////////////
tf3::convert(global_origin.point, observation_list_.front().origin_);
/////////////////////////////////////////////////
/////////////////////////////////////////////////
tf3::convert(global_origin.point, observation.origin_);
observation.raytrace_range_ = raytrace_range_;
observation.obstacle_range_ = obstacle_range_;
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
observation_list_.front().raytrace_range_ = raytrace_range_;
observation_list_.front().obstacle_range_ = obstacle_range_;
robot_sensor_msgs::PointCloud2& observation_cloud = *observation.cloud_;
tf3::doTransform(cloud, observation_cloud, cloud_transform);
observation_cloud.header.stamp = cloud.header.stamp;
robot_sensor_msgs::PointCloud2 global_frame_cloud;
const std::size_t cloud_size =
static_cast<std::size_t>(observation_cloud.height) * observation_cloud.width;
const std::size_t point_step = observation_cloud.point_step;
std::size_t point_count = 0;
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(observation_cloud, "z");
// transform the point cloud
// tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
cloud.header.frame_id, // frame nguồn
tf3::Time()
// data_convert::convertTime(cloud.header.stamp)
);
tf3::doTransform(cloud, global_frame_cloud, tfm_2);
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
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;
observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
observation_cloud.point_step = global_frame_cloud.point_step;
observation_cloud.row_step = global_frame_cloud.row_step;
observation_cloud.is_dense = global_frame_cloud.is_dense;
unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
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
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)
// Compact accepted points in-place. This avoids allocating and copying a
// second full-size filtered cloud after the TF transform.
for (std::size_t read_index = 0; read_index < cloud_size; ++read_index, ++iter_z)
{
if ((*iter_z) <= max_obstacle_height_
&& (*iter_z) >= min_obstacle_height_)
if ((*iter_z) > max_obstacle_height_ || (*iter_z) < min_obstacle_height_)
continue;
if (point_count != read_index)
{
std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
iter_obs += global_frame_cloud.point_step;
++point_count;
std::memmove(observation_cloud.data.data() + point_count * point_step,
observation_cloud.data.data() + read_index * point_step,
point_step);
}
++point_count;
}
// resize the cloud for the number of legal points
modifier.resize(point_count);
observation_cloud.header.stamp = cloud.header.stamp;
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
if (point_count != cloud_size)
{
robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
modifier.resize(point_count);
}
}
catch (TransformException& ex)
{
// if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
if (observation_keep_time_ == robot::Duration(0.0) && !observation_list_.empty())
{
observation_list_.front() = std::move(observation);
observation_list_.erase(++observation_list_.begin(), observation_list_.end());
}
else
{
observation_list_.push_front(std::move(observation));
}
// if the update was successful, we want to update the last updated time
last_updated_ = robot::Time::now();
@@ -238,21 +231,28 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
void ObservationBuffer::bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth_camera_data)
{
depth_observation_list_.push_front(DepthCameraObservation());
if (depth_observation_list_.front().data_ == nullptr)
bufferDepthCamera(boost::make_shared<robot_sensor_msgs::DepthCameraData>(depth_camera_data));
}
void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth_camera_data)
{
if (!depth_camera_data)
return;
DepthCameraObservation observation(
std::move(depth_camera_data), topic_name_, robot::Time::now(),
frustum_pixel_step_, frustum_min_range_, frustum_max_range_);
if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty())
{
depth_observation_list_.front().data_ =
new robot_sensor_msgs::DepthCameraData(depth_camera_data);
depth_observation_list_.front() = std::move(observation);
depth_observation_list_.erase(++depth_observation_list_.begin(), depth_observation_list_.end());
}
else
{
*depth_observation_list_.front().data_ = depth_camera_data;
depth_observation_list_.push_front(std::move(observation));
}
depth_observation_list_.front().pixel_step_ = frustum_pixel_step_;
depth_observation_list_.front().min_range_ = frustum_min_range_;
depth_observation_list_.front().max_range_ = frustum_max_range_;
// if the update was successful, we want to update the last updated time
last_updated_ = robot::Time::now();
@@ -280,11 +280,18 @@ void ObservationBuffer::getDepthObservations(vector<DepthCameraObservation>& obs
purgeStaleDepthObservations();
// now we'll just copy the observations for the caller
list<DepthCameraObservation>::iterator obs_it;
for (obs_it = depth_observation_list_.begin(); obs_it != depth_observation_list_.end(); ++obs_it)
if (observation_keep_time_ == robot::Duration(0.0))
{
observations.push_back(*obs_it);
if (!depth_observation_list_.empty())
{
observations.push_back(std::move(depth_observation_list_.front()));
depth_observation_list_.clear();
}
return;
}
observations.insert(
observations.end(), depth_observation_list_.begin(), depth_observation_list_.end());
}
void ObservationBuffer::purgeStaleObservations()
@@ -356,4 +363,3 @@ void ObservationBuffer::resetLastUpdated()
last_updated_ = robot::Time::now();
}
} // namespace robot_costmap_2d