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

update to tf3

Browse Source
This commit is contained in:
duongtd
2025-11-17 15:04:11 +07:00
parent 42840e3bbc
commit 0c61984d3e
12 changed files with 749 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/observation_buffer.cpp
View File

@@ -2,7 +2,7 @@
#include <costmap_2d/data_convert.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include<tf2/convert.h>
#include<tf3/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint>
@@ -10,13 +10,13 @@
using namespace std;
using namespace tf2;
using namespace tf3;
namespace costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf2::BufferCore& tf2_buffer, string global_frame,
double raytrace_range, tf3::BufferCore& tf2_buffer, string global_frame,
string sensor_frame, double tf_tolerance) :
tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(robot::Time::now()),
@@ -36,7 +36,7 @@ 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))
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &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());
@@ -56,14 +56,14 @@ 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,
tf3::doTransform(origin, origin,
tf2_buffer_.lookupTransform(new_global_frame,
origin.header.frame_id,
convertTime(origin.header.stamp)));
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_),
tf3::doTransform(*(obs.cloud_), *(obs.cloud_),
tf2_buffer_.lookupTransform(new_global_frame,
obs.cloud_->header.frame_id,
convertTime(obs.cloud_->header.stamp)));
@@ -100,11 +100,11 @@ 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,
tf3::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_,
local_origin.header.frame_id,
convertTime(local_origin.header.stamp)));
tf2::convert(global_origin.point, observation_list_.front().origin_);
tf3::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
observation_list_.front().raytrace_range_ = raytrace_range_;
@@ -113,7 +113,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
tf2::doTransform(cloud, global_frame_cloud,
tf3::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_,
(cloud.header.frame_id),
convertTime(cloud.header.stamp)));