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
00033 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00034 #include <tf2_ros/transform_listener.h>
00035 #include <ros/ros.h>
00036 #include <gtest/gtest.h>
00037 #include <tf2_ros/buffer.h>
00038 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00039
00040 tf2_ros::Buffer* tf_buffer;
00041 static const double EPS = 1e-3;
00042
00043
00044 TEST(TfGeometry, Frame)
00045 {
00046 geometry_msgs::PoseStamped v1;
00047 v1.pose.position.x = 1;
00048 v1.pose.position.y = 2;
00049 v1.pose.position.z = 3;
00050 v1.pose.orientation.x = 1;
00051 v1.header.stamp = ros::Time(2);
00052 v1.header.frame_id = "A";
00053
00054
00055 geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00056 EXPECT_NEAR(v_simple.pose.position.x, -9, EPS);
00057 EXPECT_NEAR(v_simple.pose.position.y, 18, EPS);
00058 EXPECT_NEAR(v_simple.pose.position.z, 27, EPS);
00059 EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS);
00060 EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS);
00061 EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS);
00062 EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
00063
00064
00065
00066 geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00067 "A", ros::Duration(3.0));
00068 EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS);
00069 EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS);
00070 EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS);
00071 EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS);
00072 EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS);
00073 EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS);
00074 EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS);
00075 }
00076
00077 TEST(TfGeometry, Transform)
00078 {
00079 geometry_msgs::TransformStamped v1;
00080 v1.transform.translation.x = 1;
00081 v1.transform.translation.y = 2;
00082 v1.transform.translation.z = 3;
00083 v1.transform.rotation.x = 1;
00084 v1.header.stamp = ros::Time(2);
00085 v1.header.frame_id = "A";
00086
00087
00088 geometry_msgs::TransformStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00089 EXPECT_NEAR(v_simple.transform.translation.x, -9, EPS);
00090 EXPECT_NEAR(v_simple.transform.translation.y, 18, EPS);
00091 EXPECT_NEAR(v_simple.transform.translation.z, 27, EPS);
00092 EXPECT_NEAR(v_simple.transform.rotation.x, 0.0, EPS);
00093 EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS);
00094 EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS);
00095 EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS);
00096
00097
00098
00099 geometry_msgs::TransformStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00100 "A", ros::Duration(3.0));
00101 EXPECT_NEAR(v_advanced.transform.translation.x, -9, EPS);
00102 EXPECT_NEAR(v_advanced.transform.translation.y, 18, EPS);
00103 EXPECT_NEAR(v_advanced.transform.translation.z, 27, EPS);
00104 EXPECT_NEAR(v_advanced.transform.rotation.x, 0.0, EPS);
00105 EXPECT_NEAR(v_advanced.transform.rotation.y, 0.0, EPS);
00106 EXPECT_NEAR(v_advanced.transform.rotation.z, 0.0, EPS);
00107 EXPECT_NEAR(v_advanced.transform.rotation.w, 1.0, EPS);
00108 }
00109
00110 TEST(TfGeometry, Vector)
00111 {
00112 geometry_msgs::Vector3Stamped v1, res;
00113 v1.vector.x = 1;
00114 v1.vector.y = 2;
00115 v1.vector.z = 3;
00116 v1.header.stamp = ros::Time(2.0);
00117 v1.header.frame_id = "A";
00118
00119
00120 geometry_msgs::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00121 EXPECT_NEAR(v_simple.vector.x, 1, EPS);
00122 EXPECT_NEAR(v_simple.vector.y, -2, EPS);
00123 EXPECT_NEAR(v_simple.vector.z, -3, EPS);
00124
00125
00126 geometry_msgs::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00127 "A", ros::Duration(3.0));
00128 EXPECT_NEAR(v_advanced.vector.x, 1, EPS);
00129 EXPECT_NEAR(v_advanced.vector.y, -2, EPS);
00130 EXPECT_NEAR(v_advanced.vector.z, -3, EPS);
00131 }
00132
00133
00134 TEST(TfGeometry, Point)
00135 {
00136 geometry_msgs::PointStamped v1, res;
00137 v1.point.x = 1;
00138 v1.point.y = 2;
00139 v1.point.z = 3;
00140 v1.header.stamp = ros::Time(2.0);
00141 v1.header.frame_id = "A";
00142
00143
00144 geometry_msgs::PointStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00145 EXPECT_NEAR(v_simple.point.x, -9, EPS);
00146 EXPECT_NEAR(v_simple.point.y, 18, EPS);
00147 EXPECT_NEAR(v_simple.point.z, 27, EPS);
00148
00149
00150 geometry_msgs::PointStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00151 "A", ros::Duration(3.0));
00152 EXPECT_NEAR(v_advanced.point.x, -9, EPS);
00153 EXPECT_NEAR(v_advanced.point.y, 18, EPS);
00154 EXPECT_NEAR(v_advanced.point.z, 27, EPS);
00155 }
00156
00157
00158 int main(int argc, char **argv){
00159 testing::InitGoogleTest(&argc, argv);
00160 ros::init(argc, argv, "test");
00161 ros::NodeHandle n;
00162
00163 tf_buffer = new tf2_ros::Buffer();
00164
00165
00166 geometry_msgs::TransformStamped t;
00167 t.transform.translation.x = 10;
00168 t.transform.translation.y = 20;
00169 t.transform.translation.z = 30;
00170 t.transform.rotation.x = 1;
00171 t.header.stamp = ros::Time(2.0);
00172 t.header.frame_id = "A";
00173 t.child_frame_id = "B";
00174 tf_buffer->setTransform(t, "test");
00175
00176 int ret = RUN_ALL_TESTS();
00177 delete tf_buffer;
00178 return ret;
00179 }
00180
00181
00182
00183
00184