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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-31 15:43:07 +07:00
parent ae469e3271
commit b6733ae04c
14 changed files with 24 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/observation_buffer.cpp
View File

@@ -66,7 +66,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
std::string tf_error;
robot_geometry_msgs::TransformStamped transformStamped;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time(), &tf_error))
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
@@ -90,7 +90,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
origin.header.frame_id, // frame nguồn
tf3::Time()
transform_time
);
tf3::doTransform(origin, origin, tfm_1);
obs.origin_ = origin.point;
@@ -100,7 +100,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
new_global_frame, // frame đích
obs.cloud_->header.frame_id, // frame nguồn
tf3::Time()
transform_time
);
tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
}
@@ -140,7 +140,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
global_frame_, // frame đích
local_origin.header.frame_id, // frame nguồn
tf3::Time()
data_convert::convertTime(local_origin.header.stamp)
);
tf3::doTransform(local_origin, global_origin, tfm_1);
@@ -161,7 +161,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
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;