/* * 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. */ /** \author Wim Meeussen */ // #include // #include #include #include #include #include tf3::BufferCore* tf_buffer; tf3::TransformStampedMsg t; static const double EPS = 1e-3; TEST(TfGeometry, Frame) { geometry_msgs::PoseStamped v1; v1.pose.position.x = 1; v1.pose.position.y = 2; v1.pose.position.z = 3; v1.pose.orientation.x = 1; v1.pose.orientation.y = 0; v1.pose.orientation.z = 0; v1.pose.orientation.w = 0; v1.header.stamp = robot::Time(2.0); v1.header.frame_id = "A"; // simple api // geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); // std::cout << "STEP 2" << std::endl; geometry_msgs::PoseStamped v_simple = v1; // std::cout << "lookupTransform call:" << std::endl; tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform( "B", // frame đích v1.header.frame_id, // frame nguồn // tf3::convertTime(v1.header.stamp) tf3::Time() ); // std::cout << "STEP 3" << std::endl; tf3::doTransform(v1, v_simple, tfm); // std::cout << "STEP 4" << std::endl; // std::cout << v_simple.pose.position.x << " , " // << v_simple.pose.position.y << " , " // << v_simple.pose.position.z << " , " // << v_simple.pose.orientation.x << " , " // << v_simple.pose.orientation.y << " , " // << v_simple.pose.orientation.z << " , " // << v_simple.pose.orientation.w << std::endl; EXPECT_NEAR(v_simple.pose.position.x, -9, EPS); EXPECT_NEAR(v_simple.pose.position.y, 18, EPS); EXPECT_NEAR(v_simple.pose.position.z, 27, EPS); EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS); // advanced api // geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), // "A", ros::Duration(3.0)); tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( "B", // frame đích "A", // frame nguồn tf3::convertTime(robot::Time(2.0)) ); geometry_msgs::PoseStamped v_advanced = v1; // hoặc v1 tf3::doTransform(v1, v_advanced, tfm1); // std::cout << v_advanced.pose.position.x << " , " // << v_advanced.pose.position.y << " , " // << v_advanced.pose.position.z << " , " // << v_advanced.pose.orientation.x << " , " // << v_advanced.pose.orientation.y << " , " // << v_advanced.pose.orientation.z << " , " // << v_advanced.pose.orientation.w << std::endl; EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS); EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS); EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS); EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS); } TEST(TfGeometry, PoseWithCovarianceStamped) { geometry_msgs::PoseWithCovarianceStamped v1; v1.pose.pose.position.x = 1; v1.pose.pose.position.y = 2; v1.pose.pose.position.z = 3; v1.pose.pose.orientation.x = 1; v1.pose.pose.orientation.y = 0; v1.pose.pose.orientation.z = 0; v1.pose.pose.orientation.w = 0; v1.header.stamp = robot::Time(2); v1.header.frame_id = "A"; for(int i = 0; i < 36; i++) { v1.pose.covariance[i] = 0.0; } v1.pose.covariance[0] = 1; v1.pose.covariance[7] = 1; v1.pose.covariance[14] = 1; v1.pose.covariance[21] = 1; v1.pose.covariance[28] = 1; v1.pose.covariance[35] = 1; // simple api geometry_msgs::PoseWithCovarianceStamped v_simple; tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( "B", // frame đích "A", // frame nguồn tf3::convertTime(robot::Time(2.0)) ); tf3::doTransform(v1, v_simple, tfm1); std::cout << v_simple.pose.pose.position.x << " , " << v_simple.pose.pose.position.y << " , " << v_simple.pose.pose.position.z << " , " << v_simple.pose.pose.orientation.x << " , " << v_simple.pose.pose.orientation.y << " , " << v_simple.pose.pose.orientation.z << " , " << v_simple.pose.pose.orientation.w << std::endl; EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS); EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS); EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS); // no rotation in this transformation, so no change to covariance EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS); // advanced api geometry_msgs::PoseWithCovarianceStamped v_advanced; tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform( "B", // frame đích "A", // frame nguồn tf3::convertTime(robot::Time(2.0)) ); tf3::doTransform(v1, v_advanced, tfm2); // const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), // "A", ros::Duration(3.0)); std::cout << v_advanced.pose.pose.position.x << " , " << v_advanced.pose.pose.position.y << " , " << v_advanced.pose.pose.position.z << " , " << v_advanced.pose.pose.orientation.x << " , " << v_advanced.pose.pose.orientation.y << " , " << v_advanced.pose.pose.orientation.z << " , " << v_advanced.pose.pose.orientation.w << std::endl; EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS); EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS); EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS); // no rotation in this transformation, so no change to covariance EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS); /** now add rotation to transform to test the effect on covariance **/ // rotate pi/2 radians about x-axis geometry_msgs::TransformStamped t_rot; tf3::Quaternion q_rot = tf3::Quaternion(tf3::Vector3(1,0,0), M_PI/2); t_rot.transform.rotation.x = q_rot.x(); t_rot.transform.rotation.y = q_rot.y(); t_rot.transform.rotation.z = q_rot.z(); t_rot.transform.rotation.w = q_rot.w(); t_rot.header.stamp = robot::Time(2.0); t_rot.header.frame_id = "A"; t_rot.child_frame_id = "rotated"; tf3::TransformStampedMsg t_rot_msg = tf3::convertToTransformStampedMsg(t_rot); tf_buffer->setTransform(t_rot_msg, "rotation_test"); // need to put some covariance in the matrix v1.pose.covariance[1] = 1; v1.pose.covariance[6] = 1; v1.pose.covariance[12] = 1; // perform rotation geometry_msgs::PoseWithCovarianceStamped v_rotated; tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform( "rotated", // frame đích "A", // frame nguồn tf3::convertTime(robot::Time(2.0)) ); tf3::doTransform(v1, v_rotated, tfm3); // the covariance matrix should now be transformed EXPECT_NEAR(v_rotated.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[1], 0.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[2],-1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[6], 1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[7], 1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[8], 0.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS); // set buffer back to original transform tf_buffer->setTransform(t, "test"); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); // std::vector data = {2,2,7,4,5}; // for(int i = 0; i < data.size(); i++) { // std::cout << data.size() << std::endl; // std::cout<< i << " " << data[i] << std::endl; // } tf_buffer = new tf3::BufferCore(); tf_buffer->setUsingDedicatedThread(true); // std::cout << "STEP 1" << std::endl; // populate buffer 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 = tf3::convertTime(robot::Time(2.0)); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); int ret = RUN_ALL_TESTS(); delete tf_buffer; return ret; }