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

hiep sua ten file

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:56:49 +07:00
parent 2987c1a481
commit e4db1da907
5 changed files with 32 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp
View File

@@ -39,14 +39,14 @@ static const double EPS = 1e-3;
TEST(Tf2Sensor, PointCloud2)
{
sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud);
robot_sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = 1;
*iter_y = 2;
@@ -62,12 +62,12 @@ TEST(Tf2Sensor, PointCloud2)
data_convert::convertTime(robot::Time(2.0))
);
sensor_msgs::PointCloud2 cloud_simple; // hoặc v1
robot_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");
// robot_sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
robot_sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
EXPECT_NEAR(*iter_x_after, -9, EPS);
EXPECT_NEAR(*iter_y_after, 18, EPS);
EXPECT_NEAR(*iter_z_after, 27, EPS);
@@ -79,13 +79,13 @@ TEST(Tf2Sensor, PointCloud2)
data_convert::convertTime(robot::Time(2.0))
);
sensor_msgs::PointCloud2 cloud_advanced; // hoặc v1
robot_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),
// robot_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");
robot_sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS);