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

update function dataCallBack and file test static layer

Browse Source
This commit is contained in:
duongtd
2025-11-11 14:28:20 +07:00
parent 498b606e15
commit 19683269c3
7 changed files with 213 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/observation_buffer.cpp
View File

@@ -41,12 +41,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
std::string tf_error;
// if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
// {
// printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
// global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
// return false;
// }
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
return false;
}
list<Observation>::iterator obs_it;
for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it)
@@ -61,17 +61,17 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
origin.point = obs.origin_;
// we need to transform the origin of the observation to the new global frame
// tf2::doTransform(origin, origin,
// tf2_buffer_.lookupTransform(new_global_frame,
// tf2::getFrameId(origin),
// tf2::getTimestamp(origin)));
// obs.origin_ = origin.point;
tf2::doTransform(origin, origin,
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(origin),
tf2::getTimestamp(origin)));
obs.origin_ = origin.point;
// // we also need to transform the cloud of the observation to the new global frame
// tf2::doTransform(*(obs.cloud_), *(obs.cloud_),
// tf2_buffer_.lookupTransform(new_global_frame,
// tf2::getFrameId(*(obs.cloud_)),
// tf2::getTimestamp(*(obs.cloud_))));
// we also need to transform the cloud of the observation to the new global frame
tf2::doTransform(*(obs.cloud_), *(obs.cloud_),
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(*(obs.cloud_)),
tf2::getTimestamp(*(obs.cloud_))));
}
catch (TransformException& ex)
{
@@ -105,10 +105,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
// tf2::doTransform(local_origin, global_origin,
// tf2_buffer_.lookupTransform(global_frame_,
// tf2::getFrameId(local_origin),
// tf2::getTimestamp(local_origin)));
tf2::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(local_origin),
tf2::getTimestamp(local_origin)));
tf2::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
@@ -117,12 +117,12 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// // transform the point cloud
// tf2::doTransform(cloud, global_frame_cloud,
// tf2_buffer_.lookupTransform(global_frame_,
// tf2::getFrameId(cloud),
// tf2::getTimestamp(cloud)));
// global_frame_cloud.header.stamp = cloud.header.stamp;
// transform the point cloud
tf2::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(cloud),
tf2::getTimestamp(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_);