Skip to content

Commit dc12384

Browse files
jhughes50jhughes50
authored
Merge pull request #15 from KumarRobotics/test/ros-conversions
unit-tests-ros-conversions
2 parents 0478f66 + 8e23e5b commit dc12384

File tree

4 files changed

+214
-3
lines changed

4 files changed

+214
-3
lines changed

‎.github/workflows/humble-ci.yml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,6 @@ jobs:
1919
run: |
2020
docker run --rm -v $(pwd):/workspace -w /workspace \
2121
glider-ros:humble \
22-
bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \
22+
bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \
2323
&& cd build && cmake --build . && ctest --output-on-failure"
2424

‎.github/workflows/jazzy-ci.yml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,5 +19,5 @@ jobs:
1919
run: |
2020
docker run --rm -v $(pwd):/workspace -w /workspace \
2121
glider-ros:jazzy \
22-
bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \
22+
bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \
2323
&& cd build && cmake --build . && ctest --output-on-failure"

‎glider/CMakeLists.txt‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -199,7 +199,14 @@ if (BUILD_TESTS)
199199
GTest::Main
200200
${PROJECT_NAME}
201201
)
202-
202+
203+
add_executable(ros_conversion_test test/test_ros_conversions.cpp)
204+
target_link_libraries(ros_conversion_test
205+
GTest::GTest
206+
GTest::Main
207+
${PROJECT_NAME}
208+
${PROJECT_NAME}_ros
209+
)
203210

204211
include(GoogleTest)
205212
gtest_discover_tests(utm_test)
@@ -209,4 +216,5 @@ if (BUILD_TESTS)
209216
gtest_discover_tests(glider_test)
210217
gtest_discover_tests(odometry_test)
211218
gtest_discover_tests(odometry_with_cov_test)
219+
gtest_discover_tests(ros_conversion_test)
212220
endif()

‎glider/test/test_ros_conversions.cpp‎

Lines changed: 203 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,203 @@
1+
/*!
2+
* Jason Hughes
3+
* October 2025
4+
*
5+
* Unit tests for ros to eigen and
6+
* eigen to ros convertions
7+
*/
8+
9+
#include <gtest/gtest.h>
10+
11+
#include "ros/conversions.hpp"
12+
13+
TEST(RosToEigenTestSuite, Vector3Test)
14+
{
15+
geometry_msgs::msg::Vector3 msg;
16+
msg.x = 1.0;
17+
msg.y = 1.0;
18+
msg.z = 1.0;
19+
20+
Eigen::Vector3d gt(msg.x, msg.y, msg.z);
21+
22+
ASSERT_EQ(GliderROS::Conversions::rosToEigen<Eigen::Vector3d>(msg), gt);
23+
}
24+
25+
TEST(RosToEigenTestSuite, OrientTest)
26+
{
27+
geometry_msgs::msg::Quaternion msg;
28+
msg.w = 1.0;
29+
msg.x = 0.0;
30+
msg.y = 0.0;
31+
msg.z = 0.0;
32+
33+
Eigen::Vector4d gt(msg.w, msg.x, msg.y, msg.z);
34+
35+
ASSERT_EQ(GliderROS::Conversions::rosToEigen<Eigen::Vector4d>(msg), gt);
36+
}
37+
38+
TEST(RosToEigenTestSuite, NavSatFixTest)
39+
{
40+
sensor_msgs::msg::NavSatFix msg;
41+
msg.latitude = 32.925;
42+
msg.longitude = -75.119;
43+
msg.altitude = 10.0;
44+
45+
Eigen::Vector3d gt(msg.latitude, msg.longitude, msg.altitude);
46+
47+
ASSERT_EQ(GliderROS::Conversions::rosToEigen<Eigen::Vector3d>(msg), gt);
48+
}
49+
50+
TEST(RosToEigenTestSuite, PoseTest)
51+
{
52+
geometry_msgs::msg::PoseStamped msg;
53+
msg.pose.position.x = 1.0;
54+
msg.pose.position.y = 1.0;
55+
msg.pose.position.z = 1.0;
56+
msg.pose.orientation.w = 1.0;
57+
msg.pose.orientation.x = 0.0;
58+
msg.pose.orientation.y = 0.0;
59+
msg.pose.orientation.z = 0.0;
60+
61+
Eigen::Quaterniond quat(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z);
62+
Eigen::Vector3d trans(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z);
63+
64+
Eigen::Isometry3d gt = Eigen::Isometry3d::Identity();
65+
gt.linear() = quat.toRotationMatrix();
66+
gt.translation() = trans;
67+
68+
ASSERT_EQ(GliderROS::Conversions::rosToEigen<Eigen::Isometry3d>(msg).linear(), gt.linear());
69+
ASSERT_EQ(GliderROS::Conversions::rosToEigen<Eigen::Isometry3d>(msg).translation(), gt.translation());
70+
}
71+
72+
TEST(RosToEigenTestSuite, OdometryTest)
73+
{
74+
nav_msgs::msg::Odometry msg;
75+
msg.pose.pose.position.x = 1.0;
76+
msg.pose.pose.position.y = 1.0;
77+
msg.pose.pose.position.z = 1.0;
78+
msg.pose.pose.orientation.w = 1.0;
79+
msg.pose.pose.orientation.x = 0.0;
80+
msg.pose.pose.orientation.y = 0.0;
81+
msg.pose.pose.orientation.z = 0.0;
82+
msg.twist.twist.linear.x = 1.0;
83+
msg.twist.twist.linear.y = 1.0;
84+
msg.twist.twist.linear.z = 1.0;
85+
86+
Eigen::Isometry3d odom = GliderROS::Conversions::rosToEigen<Eigen::Isometry3d>(msg);
87+
88+
Eigen::Quaterniond quat(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z);
89+
Eigen::Vector3d trans(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z);
90+
91+
Eigen::Isometry3d gt = Eigen::Isometry3d::Identity();
92+
gt.linear() = quat.toRotationMatrix();
93+
gt.translation() = trans;
94+
95+
ASSERT_EQ(odom.linear(), gt.linear());
96+
ASSERT_EQ(odom.translation(), gt.translation());
97+
}
98+
99+
TEST(EigenToRosTestSuite, Vector3Test)
100+
{
101+
Eigen::Vector3d in(1.0, 1.0, 1.0);
102+
103+
geometry_msgs::msg::Vector3 msg = GliderROS::Conversions::eigenToRos<geometry_msgs::msg::Vector3>(in);
104+
105+
ASSERT_EQ(msg.x, in(0));
106+
ASSERT_EQ(msg.y, in(1));
107+
ASSERT_EQ(msg.z, in(2));
108+
}
109+
110+
TEST(EigenToRosTestSuite, NavSatFixTest)
111+
{
112+
Eigen::Vector3d in(32.925, -75.119, 10.0);
113+
114+
sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::eigenToRos<sensor_msgs::msg::NavSatFix>(in);
115+
116+
ASSERT_EQ(msg.latitude, in(0));
117+
ASSERT_EQ(msg.longitude, in(1));
118+
ASSERT_EQ(msg.altitude, in(2));
119+
}
120+
121+
TEST(EigenToRosTestSuite, OrientTest)
122+
{
123+
Eigen::Vector4d in(1.0, 0.0, 0.0, 0.0);
124+
125+
geometry_msgs::msg::Quaternion msg = GliderROS::Conversions::eigenToRos<geometry_msgs::msg::Quaternion>(in);
126+
127+
ASSERT_EQ(msg.w, in(0));
128+
ASSERT_EQ(msg.x, in(1));
129+
ASSERT_EQ(msg.y, in(2));
130+
ASSERT_EQ(msg.z, in(3));
131+
}
132+
133+
TEST(EigenToRosTestSuite, PoseTest)
134+
{
135+
Eigen::Isometry3d in = Eigen::Isometry3d::Identity();
136+
Eigen::Quaterniond quat(1.0, 0.0, 0.0, 0.0);
137+
Eigen::Vector3d t(1.0, 1.0, 1.0);
138+
139+
in.linear() = quat.toRotationMatrix();
140+
in.translation() = t;
141+
142+
geometry_msgs::msg::PoseStamped msg = GliderROS::Conversions::eigenToRos<geometry_msgs::msg::PoseStamped>(in);
143+
144+
ASSERT_EQ(msg.pose.position.x, t(0));
145+
ASSERT_EQ(msg.pose.position.y, t(1));
146+
ASSERT_EQ(msg.pose.position.z, t(2));
147+
148+
ASSERT_EQ(msg.pose.orientation.w, quat.w());
149+
ASSERT_EQ(msg.pose.orientation.x, quat.x());
150+
ASSERT_EQ(msg.pose.orientation.y, quat.y());
151+
ASSERT_EQ(msg.pose.orientation.z, quat.z());
152+
}
153+
154+
TEST(OdometryToRosTestSuite, OdometryTest)
155+
{
156+
int64_t timestamp = 1;
157+
gtsam::Rot3 rot = gtsam::Rot3::Quaternion(1.0, 0.0, 0.0, 0.0);
158+
gtsam::Point3 t(482981.6098624719, 4421256.568684888, 10.0);
159+
gtsam::Point3 v(0.0, 0.0, 0.0);
160+
161+
gtsam::NavState ns(rot, t, v);
162+
163+
Glider::Odometry odom(ns, timestamp, true);
164+
165+
nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos<nav_msgs::msg::Odometry>(odom);
166+
167+
ASSERT_EQ(msg.pose.pose.position.x, odom.getPosition<Eigen::Vector3d>()(0));
168+
ASSERT_EQ(msg.pose.pose.position.y, odom.getPosition<Eigen::Vector3d>()(1));
169+
ASSERT_EQ(msg.pose.pose.position.z, odom.getPosition<Eigen::Vector3d>()(2));
170+
171+
ASSERT_EQ(msg.pose.pose.orientation.w, odom.getOrientation<Eigen::Vector4d>()(0));
172+
ASSERT_EQ(msg.pose.pose.orientation.x, odom.getOrientation<Eigen::Vector4d>()(1));
173+
ASSERT_EQ(msg.pose.pose.orientation.y, odom.getOrientation<Eigen::Vector4d>()(2));
174+
ASSERT_EQ(msg.pose.pose.orientation.z, odom.getOrientation<Eigen::Vector4d>()(3));
175+
176+
ASSERT_EQ(msg.twist.twist.linear.x, odom.getVelocity<Eigen::Vector3d>()(0));
177+
ASSERT_EQ(msg.twist.twist.linear.y, odom.getVelocity<Eigen::Vector3d>()(1));
178+
ASSERT_EQ(msg.twist.twist.linear.z, odom.getVelocity<Eigen::Vector3d>()(2));
179+
180+
ASSERT_STREQ(msg.header.frame_id.c_str(), "enu");
181+
ASSERT_GT(msg.header.stamp.nanosec, 0);
182+
}
183+
184+
TEST(OdometryToRosTestSuite, NavSatFixTest)
185+
{
186+
int64_t timestamp = 1;
187+
gtsam::Rot3 rot = gtsam::Rot3::Quaternion(1.0, 0.0, 0.0, 0.0);
188+
gtsam::Point3 t(482981.6098624719, 4421256.568684888, 10.0);
189+
gtsam::Point3 v(0.0, 0.0, 0.0);
190+
191+
gtsam::NavState ns(rot, t, v);
192+
193+
Glider::Odometry odom(ns, timestamp, true);
194+
195+
sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::odomToRos<sensor_msgs::msg::NavSatFix>(odom, "18S");
196+
197+
ASSERT_NEAR(msg.latitude, 39.941259, 0.001);
198+
ASSERT_NEAR(msg.longitude, -75.199202, 0.001);
199+
ASSERT_EQ(msg.altitude, 10.0);
200+
ASSERT_STREQ(msg.header.frame_id.c_str(), "enu");
201+
ASSERT_GT(msg.header.stamp.nanosec, 0);
202+
}
203+

0 commit comments

Comments
 (0)