287 lines
11 KiB
C++
Executable File
287 lines
11 KiB
C++
Executable File
/*
|
|
* 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 <tf2_ros/transform_listener.h>
|
|
// #include <ros/ros.h>
|
|
#include <gtest/gtest.h>
|
|
#include <tf3/buffer_core.h>
|
|
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
|
#include <data_convert/data_convert.h>
|
|
|
|
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
|
|
// data_convert::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
|
|
data_convert::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
|
|
data_convert::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
|
|
data_convert::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 = data_convert::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
|
|
data_convert::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<int> 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 = data_convert::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;
|
|
}
|
|
|
|
|
|
|
|
|
|
|