Skip to content

Commit e9da371

Browse files
gleichdickgleichdick
authored
Conversion tests for toMsg() (#423)
* [tf2_geometry_msgs] Point and Vector convert * Add test for Vector/Point toMsg() * Add Eigen dependency to test_tf2 * reenable test_tf2_bullet * reenable more tf2_bullet tests Co-authored-by: Bjar Ne <[email protected]>
1 parent c75f9b3 commit e9da371

File tree

4 files changed

+211
-54
lines changed

4 files changed

+211
-54
lines changed

‎test_tf2/CMakeLists.txt‎

Lines changed: 49 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -20,23 +20,20 @@ endif()
2020

2121
find_package(ament_cmake_gtest REQUIRED)
2222
find_package(builtin_interfaces REQUIRED)
23+
find_package(eigen3_cmake_module REQUIRED)
24+
find_package(Eigen3 REQUIRED)
2325
find_package(geometry_msgs REQUIRED)
2426
find_package(launch_testing_ament_cmake REQUIRED)
2527
find_package(rclcpp REQUIRED)
2628
find_package(tf2 REQUIRED)
27-
# TODO (ahcorde): activate when tf2_bullet is merged
28-
# find_package(tf2_bullet REQUIRED)
29+
find_package(tf2_bullet REQUIRED)
30+
find_package(tf2_eigen REQUIRED)
2931
find_package(tf2_geometry_msgs REQUIRED)
3032
find_package(tf2_kdl REQUIRED)
3133
find_package(tf2_ros REQUIRED)
3234

3335
ament_find_gtest()
3436

35-
# TODO (ahcorde): activate when tf2_bullet is merged
36-
# find_package(PkgConfig REQUIRED)
37-
# pkg_check_modules(bullet REQUIRED bullet)
38-
# include_directories(include ${bullet_INCLUDE_DIRS})
39-
4037
ament_add_gtest(buffer_core_test test/buffer_core_test.cpp)
4138
if(TARGET buffer_core_test)
4239
ament_target_dependencies(buffer_core_test
@@ -60,14 +57,18 @@ if(TARGET test_message_filter)
6057
)
6158
endif()
6259

63-
# TODO (ahcorde): activate when tf2_bullet is merged
64-
# ament_add_gtest(test_convert test/test_convert.cpp)
65-
# if(TARGET test_convert)
66-
# ament_target_dependencies(test_convert
67-
# tf2
68-
# tf2_bullet
69-
# )
70-
# endif()
60+
ament_add_gtest(test_convert test/test_convert.cpp)
61+
if(TARGET test_convert)
62+
ament_target_dependencies(test_convert
63+
Eigen3
64+
eigen3_cmake_module
65+
tf2
66+
tf2_bullet
67+
tf2_eigen
68+
tf2_geometry_msgs
69+
tf2_kdl
70+
)
71+
endif()
7172

7273
ament_add_gtest(test_utils test/test_utils.cpp)
7374
if(TARGET test_utils)
@@ -79,28 +80,27 @@ if(TARGET test_utils)
7980
)
8081
endif()
8182

82-
# TODO (ahcorde): activate when tf2_bullet is merged
83-
# add_executable(test_buffer_server test/test_buffer_server.cpp)
84-
# if(TARGET test_buffer_server)
85-
# ament_target_dependencies(test_buffer_server
86-
# rclcpp
87-
# tf2_bullet
88-
# tf2_ros
89-
# )
90-
# endif()
91-
92-
# TODO (ahcorde): activate when tf2_bullet is merged
93-
# add_executable(test_buffer_client test/test_buffer_client.cpp)
94-
# if(TARGET test_buffer_client)
95-
# ament_target_dependencies(test_buffer_client
96-
# rclcpp
97-
# tf2_bullet
98-
# tf2_kdl
99-
# tf2_ros
100-
# )
101-
# target_link_libraries(test_buffer_client ${GTEST_LIBRARIES})
102-
# add_launch_test(test/buffer_client_tester.launch.py)
103-
# endif()
83+
add_executable(test_buffer_server test/test_buffer_server.cpp)
84+
if(TARGET test_buffer_server)
85+
ament_target_dependencies(test_buffer_server
86+
rclcpp
87+
tf2_bullet
88+
tf2_ros
89+
)
90+
endif()
91+
92+
add_executable(test_buffer_client test/test_buffer_client.cpp)
93+
if(TARGET test_buffer_client)
94+
ament_target_dependencies(test_buffer_client
95+
rclcpp
96+
tf2_bullet
97+
tf2_geometry_msgs
98+
tf2_kdl
99+
tf2_ros
100+
)
101+
target_link_libraries(test_buffer_client ${GTEST_LIBRARIES})
102+
add_launch_test(test/buffer_client_tester.launch.py)
103+
endif()
104104

105105
add_executable(test_static_publisher test/test_static_publisher.cpp)
106106
if(TARGET test_static_publisher)
@@ -115,23 +115,22 @@ if(TARGET test_static_publisher)
115115
add_launch_test(test/static_publisher.launch.py)
116116
endif()
117117

118-
# TODO (ahcorde): activate when tf2_bullet is merged
119-
# ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp)
120-
# if(TARGET test_tf2_bullet)
121-
# ament_target_dependencies(test_tf2_bullet
122-
# rclcpp
123-
# tf2_bullet
124-
# tf2_ros
125-
# )
126-
# endif()
127-
#
118+
ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp)
119+
if(TARGET test_tf2_bullet)
120+
ament_target_dependencies(test_tf2_bullet
121+
rclcpp
122+
tf2_bullet
123+
tf2_ros
124+
)
125+
endif()
126+
127+
# TODO(ahcorde): enable once python part of tf2_geometry_msgs is working
128128
# add_launch_test(test/test_buffer_client.launch.py)
129-
# add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch)
130129

131130
# install executables
132131
install(TARGETS
133-
# test_buffer_client
134-
# test_buffer_server
132+
test_buffer_client
133+
test_buffer_server
135134
test_static_publisher
136135
DESTINATION lib/${PROJECT_NAME}
137136
)

‎test_tf2/package.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,11 +12,16 @@
1212
<author>Eitan Marder-Eppstein</author>
1313

1414
<buildtool_depend>ament_cmake</buildtool_depend>
15+
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
16+
17+
<build_depend>eigen</build_depend>
1518

1619
<depend>builtin_interfaces</depend>
1720
<depend>geometry_msgs</depend>
1821
<depend>rclcpp</depend>
1922
<depend>tf2</depend>
23+
<depend>tf2_bullet</depend>
24+
<depend>tf2_eigen</depend>
2025
<depend>tf2_geometry_msgs</depend>
2126
<depend>tf2_kdl</depend>
2227
<depend>tf2_ros</depend>

‎test_tf2/test/test_convert.cpp‎

Lines changed: 96 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -35,16 +35,23 @@
3535
* Author: Eitan Marder-Eppstein
3636
*********************************************************************/
3737
#include <gtest/gtest.h>
38+
#include <geometry_msgs/msg/point_stamped.hpp>
39+
#include <geometry_msgs/msg/point.hpp>
40+
#include <geometry_msgs/msg/vector3.hpp>
3841
#include <tf2/convert.h>
42+
#include <tf2/LinearMath/Quaternion.h>
3943
#include <tf2_kdl/tf2_kdl.hpp>
4044
#include <tf2_bullet/tf2_bullet.hpp>
45+
#include <tf2_eigen/tf2_eigen.hpp>
4146
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
4247

48+
#include <Eigen/Geometry>
49+
4350
TEST(tf2Convert, kdlToBullet)
4451
{
4552
double epsilon = 1e-9;
4653

47-
tf2::Stamped<btVector3> b(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame");
54+
tf2::Stamped<btVector3> b(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame");
4855

4956
tf2::Stamped<btVector3> b1 = b;
5057
tf2::Stamped<KDL::Vector> k1;
@@ -71,11 +78,12 @@ TEST(tf2Convert, kdlBulletROSConversions)
7178
{
7279
double epsilon = 1e-9;
7380

74-
tf2::Stamped<btVector3> b1(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4;
81+
tf2::Stamped<btVector3> b1(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4;
7582
geometry_msgs::msg::PointStamped r1, r2, r3;
7683
tf2::Stamped<KDL::Vector> k1, k2, k3;
7784

78-
// Do bullet -> self -> bullet -> KDL -> self -> KDL -> ROS -> self -> ROS -> KDL -> bullet -> ROS -> bullet
85+
// Do bullet -> self -> bullet -> KDL -> self -> KDL -> ROS -> self
86+
// -> ROS -> KDL -> bullet -> ROS -> bullet
7987
tf2::convert(b1, b1);
8088
tf2::convert(b1, b2);
8189
tf2::convert(b2, k1);
@@ -96,7 +104,91 @@ TEST(tf2Convert, kdlBulletROSConversions)
96104
EXPECT_NEAR(b1.z(), b4.z(), epsilon);
97105
}
98106

99-
int main(int argc, char** argv)
107+
TEST(tf2Convert, ConvertTf2Quaternion)
108+
{
109+
const tf2::Quaternion tq(1, 2, 3, 4);
110+
Eigen::Quaterniond eq;
111+
// TODO(gleichdick): switch to tf2::convert() when it's working
112+
tf2::fromMsg(tf2::toMsg(tq), eq);
113+
114+
EXPECT_EQ(tq.w(), eq.w());
115+
EXPECT_EQ(tq.x(), eq.x());
116+
EXPECT_EQ(tq.y(), eq.y());
117+
EXPECT_EQ(tq.z(), eq.z());
118+
}
119+
120+
TEST(tf2Convert, PointVectorDefaultMessagetype)
121+
{
122+
// Verify the return type of `toMsg()`
123+
// as it can return a Vector3 or a Point for certain datatypes
124+
{
125+
// Bullet
126+
const tf2::Stamped<btVector3> b1{btVector3{1.0, 3.0, 4.0}, tf2::TimePoint(), "my_frame"};
127+
const geometry_msgs::msg::PointStamped msg = tf2::toMsg(b1);
128+
129+
EXPECT_EQ(msg.point.x, 1.0);
130+
EXPECT_EQ(msg.point.y, 3.0);
131+
EXPECT_EQ(msg.point.z, 4.0);
132+
EXPECT_EQ(msg.header.frame_id, b1.frame_id_);
133+
}
134+
{
135+
// Eigen
136+
const Eigen::Vector3d e1{2.0, 4.0, 5.0};
137+
const geometry_msgs::msg::Point msg = tf2::toMsg(e1);
138+
139+
EXPECT_EQ(msg.x, 2.0);
140+
EXPECT_EQ(msg.y, 4.0);
141+
EXPECT_EQ(msg.z, 5.0);
142+
}
143+
{
144+
// tf2
145+
const tf2::Vector3 t1{2.0, 4.0, 5.0};
146+
const geometry_msgs::msg::Vector3 msg = tf2::toMsg(t1);
147+
148+
EXPECT_EQ(msg.x, 2.0);
149+
EXPECT_EQ(msg.y, 4.0);
150+
EXPECT_EQ(msg.z, 5.0);
151+
}
152+
{
153+
// KDL
154+
const tf2::Stamped<KDL::Vector> k1{KDL::Vector{1.0, 3.0, 4.0}, tf2::TimePoint(), "my_frame"};
155+
const geometry_msgs::msg::PointStamped msg = tf2::toMsg(k1);
156+
157+
EXPECT_EQ(msg.point.x, 1.0);
158+
EXPECT_EQ(msg.point.y, 3.0);
159+
EXPECT_EQ(msg.point.z, 4.0);
160+
EXPECT_EQ(msg.header.frame_id, k1.frame_id_);
161+
}
162+
}
163+
164+
TEST(tf2Convert, PointVectorOtherMessagetype)
165+
{
166+
{
167+
const tf2::Vector3 t1{2.0, 4.0, 5.0};
168+
geometry_msgs::msg::Point msg;
169+
const geometry_msgs::msg::Point & msg2 = tf2::toMsg(t1, msg);
170+
171+
// returned reference is second argument
172+
EXPECT_EQ(&msg2, &msg);
173+
EXPECT_EQ(msg.x, 2.0);
174+
EXPECT_EQ(msg.y, 4.0);
175+
EXPECT_EQ(msg.z, 5.0);
176+
}
177+
{
178+
// Eigen
179+
const Eigen::Vector3d e1{2.0, 4.0, 5.0};
180+
geometry_msgs::msg::Vector3 msg;
181+
const geometry_msgs::msg::Vector3 & msg2 = tf2::toMsg(e1, msg);
182+
183+
// returned reference is second argument
184+
EXPECT_EQ(&msg2, &msg);
185+
EXPECT_EQ(msg.x, 2.0);
186+
EXPECT_EQ(msg.y, 4.0);
187+
EXPECT_EQ(msg.z, 5.0);
188+
}
189+
}
190+
191+
int main(int argc, char ** argv)
100192
{
101193
testing::InitGoogleTest(&argc, argv);
102194
return RUN_ALL_TESTS();

‎tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,12 +35,15 @@
3535
#include <tf2/convert.h>
3636
#include <tf2/LinearMath/Quaternion.h>
3737
#include <tf2/LinearMath/Transform.h>
38+
#include <tf2/LinearMath/Vector3.h>
3839
#include <tf2_ros/buffer_interface.h>
3940
#include <geometry_msgs/msg/point_stamped.hpp>
4041
#include <geometry_msgs/msg/quaternion_stamped.hpp>
4142
#include <geometry_msgs/msg/transform_stamped.hpp>
43+
#include <geometry_msgs/msg/vector3.hpp>
4244
#include <geometry_msgs/msg/vector3_stamped.hpp>
4345
#include <geometry_msgs/msg/pose.hpp>
46+
#include <geometry_msgs/msg/point.hpp>
4447
#include <geometry_msgs/msg/pose_stamped.hpp>
4548
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
4649
#include <kdl/frames.hpp>
@@ -60,6 +63,35 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t)
6063
KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
6164
}
6265

66+
/*************/
67+
/** Vector3 **/
68+
/*************/
69+
70+
/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation.
71+
* This function is a specialization of the toMsg template defined in tf2/convert.h.
72+
* \param in A tf2 Vector3 object.
73+
* \return The Vector3 converted to a geometry_msgs message type.
74+
*/
75+
inline
76+
geometry_msgs::msg::Vector3 toMsg(const tf2::Vector3 & in)
77+
{
78+
geometry_msgs::msg::Vector3 out;
79+
out.x = in.getX();
80+
out.y = in.getY();
81+
out.z = in.getZ();
82+
return out;
83+
}
84+
85+
/** \brief Convert a Vector3 message to its equivalent tf2 representation.
86+
* This function is a specialization of the fromMsg template defined in tf2/convert.h.
87+
* \param in A Vector3 message type.
88+
* \param out The Vector3 converted to a tf2 type.
89+
*/
90+
inline
91+
void fromMsg(const geometry_msgs::msg::Vector3 & in, tf2::Vector3 & out)
92+
{
93+
out = tf2::Vector3(in.x, in.y, in.z);
94+
}
6395

6496
/********************/
6597
/** Vector3Stamped **/
@@ -124,6 +156,35 @@ void fromMsg(const geometry_msgs::msg::Vector3Stamped& msg, geometry_msgs::msg::
124156
}
125157

126158

159+
/***********/
160+
/** Point **/
161+
/***********/
162+
163+
/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation.
164+
* This function is a specialization of the toMsg template defined in tf2/convert.h.
165+
* \param in A tf2 Vector3 object.
166+
* \return The Vector3 converted to a geometry_msgs message type.
167+
*/
168+
inline
169+
geometry_msgs::msg::Point & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point & out)
170+
{
171+
out.x = in.getX();
172+
out.y = in.getY();
173+
out.z = in.getZ();
174+
return out;
175+
}
176+
177+
/** \brief Convert a Vector3 message to its equivalent tf2 representation.
178+
* This function is a specialization of the fromMsg template defined in tf2/convert.h.
179+
* \param in A Vector3 message type.
180+
* \param out The Vector3 converted to a tf2 type.
181+
*/
182+
inline
183+
void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
184+
{
185+
out = tf2::Vector3(in.x, in.y, in.z);
186+
}
187+
127188

128189
/******************/
129190
/** PointStamped **/

0 commit comments

Comments
 (0)