Hiep update

robot_tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp

@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <gtest/gtest.h>
+#include <tf3/buffer_core.h>
+
+tf3::BufferCore* tf_buffer;
+static const double EPS = 1e-3;
+
+
+TEST(Tf2Sensor, PointCloud2)
+{
+  robot_sensor_msgs::PointCloud2 cloud;
+  robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
+  modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
+  modifier.resize(1);
+
+  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;
+  *iter_z = 3;
+
+  cloud.header.stamp = robot::Time(2);
+  cloud.header.frame_id = "A";
+
+  // simple api
+  tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
+                        "B",                      // frame đích
+                        cloud.header.frame_id,                      // frame nguồn
+                        data_convert::convertTime(robot::Time(2.0))
+                    );
+
+  robot_sensor_msgs::PointCloud2 cloud_simple;   // hoặc v1
+  tf3::doTransform(cloud, cloud_simple, tfm1);
+  // 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);
+
+  // advanced api
+  tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
+                        "B",                      // frame đích
+                        "A",                      // frame nguồn
+                        data_convert::convertTime(robot::Time(2.0))
+                    );
+
+  robot_sensor_msgs::PointCloud2 cloud_advanced;   // hoặc v1
+  tf3::doTransform(cloud, cloud_advanced, tfm2);
+  // robot_sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
+  //                                                                "A", ros::Duration(3.0));
+  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);
+}
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+
+  tf_buffer = new tf3::BufferCore();
+
+  // populate buffer
+  robot_geometry_msgs::TransformStamped t;
+  t.transform.translation.x = 10;
+  t.transform.translation.y = 20;
+  t.transform.translation.z = 30;
+  t.transform.rotation.x = 1;
+  t.transform.rotation.y = 0;
+  t.transform.rotation.z = 0;
+  t.transform.rotation.w = 0;
+  t.header.stamp = robot::Time(2.0);
+  t.header.frame_id = "A";
+  t.child_frame_id = "B";
+  tf3::TransformStampedMsg t_msg = data_convert::convertToTransformStampedMsg(t);
+  tf_buffer->setTransform(t_msg, "test");
+
+  int ret = RUN_ALL_TESTS();
+  delete tf_buffer;
+  return ret;
+}