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

20
plugins/static_layer.cpp
View File

@@ -3,8 +3,8 @@
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/data_convert.h>
#include <costmap_2d/utils.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
#include <boost/dll/alias.hpp>
@@ -283,25 +283,25 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
double wx, wy;
// Might even be in a different frame
// geometry_msgs::TransformStamped transform;
tf2::TransformStampedMsg transformMsg;
tf3::TransformStampedMsg transformMsg;
try
{
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
bool status =tf_->canTransform(map_frame_, global_frame_, tf2::Time());
if(!status) throw tf2::TransformException("[static_layer] Cannot transform");
bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time());
if(!status) throw tf3::TransformException("[static_layer] Cannot transform");
transformMsg = tf_->lookupTransform(map_frame_,
global_frame_,
tf2::Time());
tf3::Time());
}
catch (tf2::TransformException ex)
catch (tf3::TransformException ex)
{
printf("%s", ex.what());
return;
}
// Copy map data given proper transformations
tf2::Transform tf2_transform;
tf3::Transform tf2_transform;
tf2_transform = convertToTf2Transform(transformMsg.transform);
// tf2::convert(transformMsg.transform, tf2_transform);
// tf3::convert(transformMsg.transform, tf2_transform);
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
@@ -309,7 +309,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf2::Vector3 p(wx, wy, 0);
tf3::Vector3 p(wx, wy, 0);
p = tf2_transform*p;
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))