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

22
plugins/directional_layer.cpp
View File

@@ -1,7 +1,7 @@
#include <costmap_2d/directional_layer.h>
#include <costmap_2d/data_convert.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
#include <boost/dll/alias.hpp>
@@ -134,8 +134,8 @@ namespace costmap_2d
return false;
}
// Convert to yaw
tf2::Quaternion quaternion = convertQuaternion(p.pose.orientation);
double theta = tf2::getYaw(quaternion);
tf3::Quaternion quaternion = convertQuaternion(p.pose.orientation);
double theta = tf3::getYaw(quaternion);
unsigned char value = laneFilter(mx, my, theta);
if (get_success)
{
@@ -399,27 +399,27 @@ namespace costmap_2d
bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw)
{
tf2::TransformStampedMsg transformMsg;
tf3::TransformStampedMsg transformMsg;
try
{
transformMsg = tf_->lookupTransform(map_frame_, base_frame_id_, tf2::Time());
transformMsg = tf_->lookupTransform(map_frame_, base_frame_id_, tf3::Time());
// listener_.lookupTransform(map_frame_, base_frame_id_, tf2::Time(), transform);
// listener_.lookupTransform(map_frame_, base_frame_id_, tf3::Time(), transform);
}
catch (tf2::TransformException &ex)
catch (tf3::TransformException &ex)
{
printf("%s\n", ex.what());
return false;
}
// Copy map data given proper transformations
tf2::Transform tf2_transform;
tf3::Transform tf2_transform;
tf2_transform = convertToTf2Transform(transformMsg.transform);
x = tf2_transform.getOrigin().x();
y = tf2_transform.getOrigin().y();
// Extract the rotation as a quaternion from the transform
tf2::Quaternion rotation = tf2_transform.getRotation();
tf3::Quaternion rotation = tf2_transform.getRotation();
// Convert the quaternion to a yaw angle (in radians)
yaw = tf2::getYaw(rotation);
yaw = tf3::getYaw(rotation);
return true;
}