DuongTD/geometry2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Hiep update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 10:24:40 +07:00
parent e4db1da907
commit 5558086d10
281 changed files with 122 additions and 18222 deletions
Download Patch File Download Diff File Expand all files Collapse all files

286
robot_tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp Executable file
View File

@@ -0,0 +1,286 @@
/*
* 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 <robot_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)
{
robot_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
// robot_geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
// std::cout << "STEP 2" << std::endl;
robot_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
// robot_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))
);
robot_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)
{
robot_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
robot_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
robot_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 robot_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
robot_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
robot_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;
}

405
robot_tf3_geometry_msgs/test/test_tomsg_frommsg.cpp Executable file
View File

@@ -0,0 +1,405 @@
/*
* 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 <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <gtest/gtest.h>
static const double EPS = 1e-6;
tf3::Vector3 get_tf3_vector()
{
return tf3::Vector3(1.0, 2.0, 3.0);
}
robot_geometry_msgs::Vector3& value_initialize(robot_geometry_msgs::Vector3 &m1)
{
m1.x = 1;
m1.y = 2;
m1.z = 3;
return m1;
}
robot_std_msgs::Header& value_initialize(robot_std_msgs::Header & h)
{
h.stamp = robot::Time(10);
h.frame_id = "foobar";
return h;
}
robot_geometry_msgs::Vector3Stamped& value_initialize(robot_geometry_msgs::Vector3Stamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.vector);
return m1;
}
robot_geometry_msgs::Point& value_initialize(robot_geometry_msgs::Point &m1)
{
m1.x = 1;
m1.y = 2;
m1.z = 3;
return m1;
}
robot_geometry_msgs::PointStamped& value_initialize(robot_geometry_msgs::PointStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.point);
return m1;
}
robot_geometry_msgs::Quaternion & value_initialize(robot_geometry_msgs::Quaternion &m1)
{
m1.x = 0;
m1.y = 0;
m1.z = 0.7071067811;
m1.w = 0.7071067811;
return m1;
}
robot_geometry_msgs::QuaternionStamped& value_initialize(robot_geometry_msgs::QuaternionStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.quaternion);
return m1;
}
robot_geometry_msgs::Pose & value_initialize(robot_geometry_msgs::Pose & m1)
{
value_initialize(m1.position);
value_initialize(m1.orientation);
return m1;
}
robot_geometry_msgs::PoseStamped& value_initialize(robot_geometry_msgs::PoseStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.pose);
return m1;
}
robot_geometry_msgs::Transform & value_initialize(robot_geometry_msgs::Transform & m1)
{
value_initialize(m1.translation);
value_initialize(m1.rotation);
return m1;
}
robot_geometry_msgs::TransformStamped& value_initialize(robot_geometry_msgs::TransformStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.transform);
return m1;
}
void expect_near(const robot_std_msgs::Header & h1, const robot_std_msgs::Header & h2)
{
EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS);
EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str());
}
/*
* Vector3
*/
void expect_near(const robot_geometry_msgs::Vector3 & v1, const tf3::Vector3 & v2)
{
EXPECT_NEAR(v1.x, v2.x(), EPS);
EXPECT_NEAR(v1.y, v2.y(), EPS);
EXPECT_NEAR(v1.z, v2.z(), EPS);
}
void expect_near(const robot_geometry_msgs::Vector3 & v1, const robot_geometry_msgs::Vector3 & v2)
{
EXPECT_NEAR(v1.x, v2.x, EPS);
EXPECT_NEAR(v1.y, v2.y, EPS);
EXPECT_NEAR(v1.z, v2.z, EPS);
}
void expect_near(const tf3::Vector3 & v1, const tf3::Vector3 & v2)
{
EXPECT_NEAR(v1.x(), v2.x(), EPS);
EXPECT_NEAR(v1.y(), v2.y(), EPS);
EXPECT_NEAR(v1.z(), v2.z(), EPS);
}
void expect_near(const robot_geometry_msgs::Vector3Stamped & p1, const robot_geometry_msgs::Vector3Stamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.vector, p2.vector);
}
/*
* Point
*/
void expect_near(const robot_geometry_msgs::Point & p1, const tf3::Vector3 & v2)
{
EXPECT_NEAR(p1.x, v2.x(), EPS);
EXPECT_NEAR(p1.y, v2.y(), EPS);
EXPECT_NEAR(p1.z, v2.z(), EPS);
}
void expect_near(const robot_geometry_msgs::Point & p1, const robot_geometry_msgs::Point & v2)
{
EXPECT_NEAR(p1.x, v2.x, EPS);
EXPECT_NEAR(p1.y, v2.y, EPS);
EXPECT_NEAR(p1.z, v2.z, EPS);
}
void expect_near(const robot_geometry_msgs::PointStamped & p1, const robot_geometry_msgs::PointStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.point, p2.point);
}
/*
* Quaternion
*/
void expect_near(const robot_geometry_msgs::Quaternion & q1, const tf3::Quaternion & v2)
{
EXPECT_NEAR(q1.x, v2.x(), EPS);
EXPECT_NEAR(q1.y, v2.y(), EPS);
EXPECT_NEAR(q1.z, v2.z(), EPS);
}
void expect_near(const robot_geometry_msgs::Quaternion & q1, const robot_geometry_msgs::Quaternion & v2)
{
EXPECT_NEAR(q1.x, v2.x, EPS);
EXPECT_NEAR(q1.y, v2.y, EPS);
EXPECT_NEAR(q1.z, v2.z, EPS);
}
void expect_near(const robot_geometry_msgs::QuaternionStamped & p1, const robot_geometry_msgs::QuaternionStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.quaternion, p2.quaternion);
}
/*
* Pose
*/
void expect_near(const robot_geometry_msgs::Pose & p, const tf3::Transform & t)
{
expect_near(p.position, t.getOrigin());
expect_near(p.orientation, t.getRotation());
}
void expect_near(const robot_geometry_msgs::Pose & p1, const robot_geometry_msgs::Pose & p2)
{
expect_near(p1.position, p2.position);
expect_near(p1.orientation, p2.orientation);
}
void expect_near(const robot_geometry_msgs::PoseStamped & p1, const robot_geometry_msgs::PoseStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.pose, p2.pose);
}
/*
* Transform
*/
void expect_near(const robot_geometry_msgs::Transform & p, const tf3::Transform & t)
{
expect_near(p.translation, t.getOrigin());
expect_near(p.rotation, t.getRotation());
}
void expect_near(const robot_geometry_msgs::Transform & p1, const robot_geometry_msgs::Transform & p2)
{
expect_near(p1.translation, p2.translation);
expect_near(p1.rotation, p2.rotation);
}
void expect_near(const robot_geometry_msgs::TransformStamped & p1, const robot_geometry_msgs::TransformStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.transform, p2.transform);
}
/*
* Stamped templated expect_near
*/
template <typename T>
void expect_near(const tf3::Stamped<T>& s1, const tf3::Stamped<T>& s2)
{
expect_near((T)s1, (T)s2);
}
/*********************
* Tests
*********************/
TEST(robot_tf3_geometry_msgs, Vector3)
{
robot_geometry_msgs::Vector3 m1;
value_initialize(m1);
tf3::Vector3 v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
expect_near(m1, v1);
robot_geometry_msgs::Vector3 m2 = toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, Point)
{
robot_geometry_msgs::Point m1;
value_initialize(m1);
tf3::Vector3 v1;
SCOPED_TRACE("m1 v1");
fromMsg(m1, v1);
expect_near(m1, v1);
robot_geometry_msgs::Point m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, Quaternion)
{
robot_geometry_msgs::Quaternion m1;
value_initialize(m1);
tf3::Quaternion q1;
SCOPED_TRACE("m1 q1");
fromMsg(m1, q1);
expect_near(m1, q1);
robot_geometry_msgs::Quaternion m2 = toMsg(q1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, Pose)
{
robot_geometry_msgs::Pose m1;
value_initialize(m1);
tf3::Transform t1;
fromMsg(m1, t1);
SCOPED_TRACE("m1 t1");
expect_near(m1, t1);
robot_geometry_msgs::Pose m2 = toMsg(t1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, Transform)
{
robot_geometry_msgs::Transform m1;
value_initialize(m1);
tf3::Transform t1;
fromMsg(m1, t1);
SCOPED_TRACE("m1 t1");
expect_near(m1, t1);
robot_geometry_msgs::Transform m2 = toMsg(t1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, Vector3Stamped)
{
robot_geometry_msgs::Vector3Stamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Vector3> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1);
robot_geometry_msgs::Vector3Stamped m2;
m2 = toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, PointStamped)
{
robot_geometry_msgs::PointStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Vector3> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly
robot_geometry_msgs::PointStamped m2;
m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, QuaternionStamped)
{
robot_geometry_msgs::QuaternionStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Quaternion> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly
robot_geometry_msgs::QuaternionStamped m2;
m2 = tf3::toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, PoseStamped)
{
robot_geometry_msgs::PoseStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Transform> v1;
SCOPED_TRACE("m1 v1");
fromMsg(m1, v1);
// expect_near(m1, v1); //TODO implement cross verification explicityly
robot_geometry_msgs::PoseStamped m2;
m2 = tf3::toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(robot_tf3_geometry_msgs, TransformStamped)
{
robot_geometry_msgs::TransformStamped m1;
value_initialize(m1);
tf3::Stamped<tf3::Transform> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1);
robot_geometry_msgs::TransformStamped m2;
m2 = tf3::toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}