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

update package tf3_sensor_msgs

Browse Source
This commit is contained in:
duongtd
2025-11-24 11:17:47 +07:00
parent 0dca929962
commit 88613a94f0
311 changed files with 22225 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
tf3_sensor_msgs/test/test.launch
View File

@@ -1,3 +0,0 @@
<launch>
<test test-name="tf2_sensor_msgs" pkg="tf2_sensor_msgs" type="test_tf2_sensor_msgs_cpp" time-limit="120" />
</launch>

41
tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp
View File

@@ -28,14 +28,12 @@
*/
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_listener.h>
#include <ros/ros.h>
#include <gtest/gtest.h>
#include <tf2_ros/buffer.h>
#include <tf3/buffer_core.h>
tf2_ros::Buffer* tf_buffer;
tf3::BufferCore* tf_buffer;
static const double EPS = 1e-3;
@@ -54,11 +52,19 @@ TEST(Tf2Sensor, PointCloud2)
*iter_y = 2;
*iter_z = 3;
cloud.header.stamp = ros::Time(2);
cloud.header.stamp = robot::Time(2);
cloud.header.frame_id = "A";
// simple api
sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
"B", // frame đích
cloud.header.frame_id, // frame nguồn
tf3::convertTime(robot::Time(2.0))
);
sensor_msgs::PointCloud2 cloud_simple; // hoặc v1
tf3::doTransform(cloud, cloud_simple, tfm1);
// sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
@@ -67,8 +73,16 @@ TEST(Tf2Sensor, PointCloud2)
EXPECT_NEAR(*iter_z_after, 27, EPS);
// advanced api
sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
"B", // frame đích
"A", // frame nguồn
tf3::convertTime(robot::Time(2.0))
);
sensor_msgs::PointCloud2 cloud_advanced; // hoặc v1
tf3::doTransform(cloud, cloud_advanced, tfm2);
// sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
// "A", ros::Duration(3.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
@@ -79,10 +93,8 @@ TEST(Tf2Sensor, PointCloud2)
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test");
ros::NodeHandle n;
tf_buffer = new tf2_ros::Buffer();
tf_buffer = new tf3::BufferCore();
// populate buffer
geometry_msgs::TransformStamped t;
@@ -93,10 +105,11 @@ int main(int argc, char **argv){
t.transform.rotation.y = 0;
t.transform.rotation.z = 0;
t.transform.rotation.w = 0;
t.header.stamp = ros::Time(2.0);
t.header.stamp = robot::Time(2.0);
t.header.frame_id = "A";
t.child_frame_id = "B";
tf_buffer->setTransform(t, "test");
tf3::TransformStampedMsg t_msg = tf3::convertToTransformStampedMsg(t);
tf_buffer->setTransform(t_msg, "test");
int ret = RUN_ALL_TESTS();
delete tf_buffer;