Skip to content

Commit 32201be

Browse files
jorgenfjjorgenfj
authored
Merge pull request #19 from vortexntnu/feat/reference-to-pose
to pose_msg functions
2 parents c409297 + 986c798 commit 32201be

File tree

5 files changed

+381
-1
lines changed

5 files changed

+381
-1
lines changed

‎CMakeLists.txt‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ find_package(ament_cmake REQUIRED)
88
find_package(rclcpp)
99
find_package(ament_cmake_python REQUIRED)
1010
find_package(Eigen3 REQUIRED)
11+
find_package(geometry_msgs REQUIRED)
1112

1213
include_directories(include)
1314

@@ -18,7 +19,11 @@ target_include_directories(${PROJECT_NAME} PUBLIC
1819
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
1920
$<INSTALL_INTERFACE:include>)
2021

21-
ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp Eigen3)
22+
ament_target_dependencies(${PROJECT_NAME} PUBLIC
23+
rclcpp
24+
Eigen3
25+
geometry_msgs
26+
)
2227

2328
ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
2429

‎cpp_test/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ add_executable(
88
${TEST_BINARY_NAME}
99
test_math.cpp
1010
test_types.cpp
11+
test_ros_conversions.cpp
1112
)
1213

1314
target_link_libraries(

‎cpp_test/test_ros_conversions.cpp‎

Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
1+
#include <gtest/gtest.h>
2+
#include <eigen3/Eigen/Dense>
3+
#include <string>
4+
5+
#include "vortex/utils/ros_conversions.hpp"
6+
#include "vortex/utils/types.hpp"
7+
8+
// ================================================================
9+
// Compile-Time Concept Tests
10+
// ================================================================
11+
12+
struct ValidEulerPose {
13+
double x = 0, y = 0, z = 0;
14+
double roll = 0, pitch = 0, yaw = 0;
15+
};
16+
17+
struct ValidEulerPoseExtra {
18+
double x = 0, y = 0, z = 0;
19+
double roll = 0, pitch = 0, yaw = 0;
20+
double something_extra = 42.0;
21+
};
22+
23+
struct ValidQuatPose {
24+
double x = 1, y = 2, z = 3;
25+
double qw = 1, qx = 0, qy = 0, qz = 0;
26+
};
27+
28+
struct MissingYaw {
29+
double x = 0, y = 0, z = 0;
30+
double roll = 0, pitch = 0;
31+
};
32+
33+
struct WrongType {
34+
double x = 0, y = 0, z = 0;
35+
std::string roll; // not convertible to double
36+
double pitch = 0, yaw = 0;
37+
};
38+
39+
struct MissingQuaternionField {
40+
double x = 0, y = 0, z = 0;
41+
double qx = 0, qy = 0, qz = 0; // missing qw
42+
};
43+
44+
// ================================================================
45+
// Concept: EulerPoseLike
46+
// ================================================================
47+
static_assert(vortex::utils::ros_conversions::EulerPoseLike<ValidEulerPose>,
48+
"ValidEulerPose should satisfy EulerPoseLike");
49+
50+
static_assert(
51+
vortex::utils::ros_conversions::EulerPoseLike<ValidEulerPoseExtra>,
52+
"ValidEulerPoseExtra should satisfy EulerPoseLike");
53+
54+
static_assert(!vortex::utils::ros_conversions::EulerPoseLike<MissingYaw>,
55+
"MissingYaw should NOT satisfy EulerPoseLike");
56+
57+
static_assert(!vortex::utils::ros_conversions::EulerPoseLike<WrongType>,
58+
"WrongType should NOT satisfy EulerPoseLike");
59+
60+
static_assert(
61+
!vortex::utils::ros_conversions::EulerPoseLike<ValidQuatPose>,
62+
"ValidQuatPose uses quaternion fields and must NOT satisfy EulerPoseLike");
63+
64+
// ================================================================
65+
// Concept: QuatPoseLike
66+
// ================================================================
67+
static_assert(vortex::utils::ros_conversions::QuatPoseLike<ValidQuatPose>,
68+
"ValidQuatPose should satisfy QuatPoseLike");
69+
70+
static_assert(
71+
!vortex::utils::ros_conversions::QuatPoseLike<MissingQuaternionField>,
72+
"MissingQuaternionField should NOT satisfy QuatPoseLike");
73+
74+
static_assert(!vortex::utils::ros_conversions::QuatPoseLike<ValidEulerPose>,
75+
"Euler pose must NOT satisfy QuatPoseLike");
76+
77+
// ================================================================
78+
// Concept: Eigen6dEuler
79+
// ================================================================
80+
static_assert(
81+
vortex::utils::ros_conversions::Eigen6dEuler<Eigen::Matrix<double, 6, 1>>,
82+
"Eigen::Vector6d should satisfy Eigen6dEuler");
83+
84+
static_assert(!vortex::utils::ros_conversions::Eigen6dEuler<Eigen::Vector3d>,
85+
"Eigen::Vector3d must NOT satisfy Eigen6dEuler");
86+
87+
// ================================================================
88+
// Concept: PoseLike (master)
89+
// ================================================================
90+
static_assert(vortex::utils::ros_conversions::PoseLike<ValidEulerPose>,
91+
"Euler pose should satisfy PoseLike");
92+
93+
static_assert(vortex::utils::ros_conversions::PoseLike<ValidQuatPose>,
94+
"Quat pose should satisfy PoseLike");
95+
96+
static_assert(
97+
vortex::utils::ros_conversions::PoseLike<Eigen::Matrix<double, 6, 1>>,
98+
"Eigen::Vector6d pose should satisfy PoseLike");
99+
100+
static_assert(!vortex::utils::ros_conversions::PoseLike<WrongType>,
101+
"WrongType must NOT satisfy PoseLike");
102+
103+
// ================================================================
104+
// Function Acceptance Using Concepts (Overload Resolution)
105+
// ================================================================
106+
template <typename T>
107+
concept AcceptsPose =
108+
requires(T t) { vortex::utils::ros_conversions::pose_like_to_pose_msg(t); };
109+
110+
static_assert(AcceptsPose<ValidEulerPose>, "Euler pose should be accepted");
111+
112+
static_assert(AcceptsPose<ValidQuatPose>, "Quaternion pose should be accepted");
113+
114+
static_assert(AcceptsPose<Eigen::Matrix<double, 6, 1>>,
115+
"Eigen6d should be accepted");
116+
117+
static_assert(!AcceptsPose<MissingYaw>, "MissingYaw should NOT be accepted");
118+
119+
static_assert(!AcceptsPose<WrongType>, "WrongType should NOT be accepted");
120+
121+
// ================================================================
122+
// Runtime Tests: Euler
123+
// ================================================================
124+
TEST(pose_like_to_pose_msg, euler_zero_eta) {
125+
vortex::utils::types::Eta eta;
126+
127+
auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(eta);
128+
129+
EXPECT_NEAR(pose.position.x, 0.0, 1e-6);
130+
EXPECT_NEAR(pose.position.y, 0.0, 1e-6);
131+
EXPECT_NEAR(pose.position.z, 0.0, 1e-6);
132+
133+
EXPECT_NEAR(pose.orientation.x, 0.0, 1e-6);
134+
EXPECT_NEAR(pose.orientation.y, 0.0, 1e-6);
135+
EXPECT_NEAR(pose.orientation.z, 0.0, 1e-6);
136+
EXPECT_NEAR(pose.orientation.w, 1.0, 1e-6);
137+
}
138+
139+
TEST(pose_like_to_pose_msg, euler_nonzero_angles) {
140+
struct EP {
141+
double x = 1, y = 2, z = 3;
142+
double roll = 1, pitch = 1, yaw = 1;
143+
};
144+
EP p;
145+
146+
auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(p);
147+
148+
EXPECT_NEAR(pose.position.x, 1, 1e-6);
149+
EXPECT_NEAR(pose.position.y, 2, 1e-6);
150+
EXPECT_NEAR(pose.position.z, 3, 1e-6);
151+
152+
Eigen::Quaterniond expected = vortex::utils::math::euler_to_quat(1, 1, 1);
153+
154+
EXPECT_NEAR(pose.orientation.x, expected.x(), 1e-6);
155+
EXPECT_NEAR(pose.orientation.y, expected.y(), 1e-6);
156+
EXPECT_NEAR(pose.orientation.z, expected.z(), 1e-6);
157+
EXPECT_NEAR(pose.orientation.w, expected.w(), 1e-6);
158+
}
159+
160+
// ================================================================
161+
// Runtime Tests: Quaternion Pose
162+
// ================================================================
163+
TEST(pose_like_to_pose_msg, quat_pose_conversion) {
164+
ValidQuatPose qp;
165+
166+
auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(qp);
167+
168+
EXPECT_NEAR(pose.position.x, 1.0, 1e-6);
169+
EXPECT_NEAR(pose.position.y, 2.0, 1e-6);
170+
EXPECT_NEAR(pose.position.z, 3.0, 1e-6);
171+
172+
EXPECT_NEAR(pose.orientation.w, 1.0, 1e-6);
173+
EXPECT_NEAR(pose.orientation.x, 0.0, 1e-6);
174+
EXPECT_NEAR(pose.orientation.y, 0.0, 1e-6);
175+
EXPECT_NEAR(pose.orientation.z, 0.0, 1e-6);
176+
}
177+
178+
// ================================================================
179+
// Runtime Tests: Eigen::Vector6d
180+
// ================================================================
181+
TEST(pose_like_to_pose_msg, eigen6d_conversion) {
182+
Eigen::Matrix<double, 6, 1> v;
183+
v << 1, 2, 3, // xyz
184+
0.1, 0.2, 0.3; // roll pitch yaw
185+
186+
auto pose = vortex::utils::ros_conversions::pose_like_to_pose_msg(v);
187+
188+
EXPECT_NEAR(pose.position.x, 1.0, 1e-6);
189+
EXPECT_NEAR(pose.position.y, 2.0, 1e-6);
190+
EXPECT_NEAR(pose.position.z, 3.0, 1e-6);
191+
192+
Eigen::Quaterniond expected =
193+
vortex::utils::math::euler_to_quat(0.1, 0.2, 0.3);
194+
195+
EXPECT_NEAR(pose.orientation.w, expected.w(), 1e-6);
196+
EXPECT_NEAR(pose.orientation.x, expected.x(), 1e-6);
197+
EXPECT_NEAR(pose.orientation.y, expected.y(), 1e-6);
198+
EXPECT_NEAR(pose.orientation.z, expected.z(), 1e-6);
199+
}

0 commit comments

Comments
 (0)