Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
00032 #include <geometry_msgs/PoseStamped.h>
00033 #include <tf2_ros/transform_listener.h>
00034 #include <ros/ros.h>
00035 #include <gtest/gtest.h>
00036 #include <tf2_ros/buffer.h>
00037
00038 tf2_ros::Buffer* tf_buffer;
00039 static const double EPS = 1e-3;
00040
00041
00042 TEST(Tf2Sensor, PointCloud2)
00043 {
00044 sensor_msgs::PointCloud2 cloud;
00045 sensor_msgs::PointCloud2Modifier modifier(cloud);
00046 modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
00047 modifier.resize(1);
00048
00049 sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
00050 sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
00051 sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
00052
00053 *iter_x = 1;
00054 *iter_y = 2;
00055 *iter_z = 3;
00056
00057 cloud.header.stamp = ros::Time(2);
00058 cloud.header.frame_id = "A";
00059
00060
00061 sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
00062 sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
00063 sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
00064 sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
00065 EXPECT_NEAR(*iter_x_after, -9, EPS);
00066 EXPECT_NEAR(*iter_y_after, 18, EPS);
00067 EXPECT_NEAR(*iter_z_after, 27, EPS);
00068
00069
00070 sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
00071 "A", ros::Duration(3.0));
00072 sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
00073 sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
00074 sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
00075 EXPECT_NEAR(*iter_x_advanced, -9, EPS);
00076 EXPECT_NEAR(*iter_y_advanced, 18, EPS);
00077 EXPECT_NEAR(*iter_z_advanced, 27, EPS);
00078 }
00079
00080 int main(int argc, char **argv){
00081 testing::InitGoogleTest(&argc, argv);
00082 ros::init(argc, argv, "test");
00083 ros::NodeHandle n;
00084
00085 tf_buffer = new tf2_ros::Buffer();
00086
00087
00088 geometry_msgs::TransformStamped t;
00089 t.transform.translation.x = 10;
00090 t.transform.translation.y = 20;
00091 t.transform.translation.z = 30;
00092 t.transform.rotation.x = 1;
00093 t.transform.rotation.y = 0;
00094 t.transform.rotation.z = 0;
00095 t.transform.rotation.w = 0;
00096 t.header.stamp = ros::Time(2.0);
00097 t.header.frame_id = "A";
00098 t.child_frame_id = "B";
00099 tf_buffer->setTransform(t, "test");
00100
00101 int ret = RUN_ALL_TESTS();
00102 delete tf_buffer;
00103 return ret;
00104 }