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

75
robot_tf3_geometry_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,75 @@
cmake_minimum_required(VERSION 3.10)
project(robot_tf3_geometry_msgs)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# Find dependencies
find_package(Boost COMPONENTS thread REQUIRED)
find_package(GTest REQUIRED)
# Finding Eigen3
find_package(Eigen3 REQUIRED)
# Include directories
include_directories(
include
${EIGEN3_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${GTEST_INCLUDE_DIRS}
)
# Install headers
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
)
add_library(robot_tf3_geometry_msgs INTERFACE
)
target_include_directories(robot_tf3_geometry_msgs
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
target_link_libraries(robot_tf3_geometry_msgs INTERFACE
geometry_msgs
data_convert
)
# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
install(TARGETS robot_tf3_geometry_msgs
EXPORT robot_tf3_geometry_msgs-targets
INCLUDES DESTINATION include # Cài đặt include
)
# --- Xuất export set robot_tf3_geometry_msgs-targets thành file CMake module ---
# --- Tạo file lib/cmake/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs-targets.cmake ---
# --- File này chứa cấu hình giúp project khác có thể dùng ---
# --- Find_package(robot_tf3_geometry_msgs REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE tf3_robot_geometry_msgs::robot_tf3_geometry_msgs) ---
install(EXPORT robot_tf3_geometry_msgs-targets
FILE robot_tf3_geometry_msgs-targets.cmake
DESTINATION lib/cmake/robot_tf3_geometry_msgs
)
# # Test: tomsg_frommsg
add_executable(test_tomsg_frommsg test/test_tomsg_frommsg.cpp)
target_link_libraries(test_tomsg_frommsg
${GTEST_LIBRARIES}
robot_tf3_geometry_msgs
Threads::Threads
tf3
data_convert
)
# # Test: tf2_geometry_msgs
add_executable(test_geometry_msgs test/test_tf2_geometry_msgs.cpp)
target_link_libraries(test_geometry_msgs
${GTEST_LIBRARIES}
Threads::Threads
tf3
robot_tf3_geometry_msgs
data_convert
)

147
robot_tf3_geometry_msgs/CODE_REVIEW.md Normal file
View File

@@ -0,0 +1,147 @@
# Đánh Giá Mã Nguồn: robot_tf3_geometry_msgs
## Tóm Tắt
Đánh giá mã nguồn này bao gồm package `robot_tf3_geometry_msgs`, cung cấp các hàm chuyển đổi giữa các kiểu tf3 và geometry_msgs.
## Các Vấn Đề Nghiêm Trọng
### 1. Mã Debug Còn Sót Lại Trong Production (Dòng 826)
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Câu lệnh debug `std::cout` còn sót lại trong mã production
```cpp
std::cout << "doTransform Pose: " << std::endl;
```
**Ảnh hưởng:** Xuất dữ liệu không cần thiết trong production, có thể ảnh hưởng hiệu năng
**Cách sửa:** Xóa câu lệnh debug
### 2. Thiếu Include
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Sử dụng `std::cout` nhưng không include `<iostream>`
**Ảnh hưởng:** Có thể gây lỗi biên dịch nếu include không được cung cấp gián tiếp
**Cách sửa:** Xóa mã debug hoặc thêm `#include <iostream>` nếu cần
### 3. Thiếu Từ Khóa `inline` Trong Header
**File:** `include/robot_tf3_geometry_msgs/data_convert.h`
**Vấn đề:** Các hàm trong file header không được đánh dấu `inline`
**Ảnh hưởng:** Lỗi định nghĩa nhiều lần khi được include trong nhiều translation unit
**Dòng:** 14, 22, 30, 51, 73, 89
**Cách sửa:** Thêm từ khóa `inline` cho tất cả định nghĩa hàm
## Các Vấn Đề Về Chất Lượng Mã
### 4. Template Specialization Trùng Lặp
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Các template specialization trùng lặp chỉ gọi cùng một hàm
**Dòng:** 389-394, 416-421
```cpp
//Backwards compatibility remove when forked for Lunar or newer
template <>
inline
robot_geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
{
return toMsg(in); // Chỉ gọi cùng một hàm
}
```
**Ảnh hưởng:** Mã gây nhầm lẫn, có thể gây vấn đề bảo trì
**Cách sửa:** Xóa các specialization trùng lặp hoặc giải thích tại sao cần thiết
### 5. Khối Mã Comment Lớn
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Nhiều khối mã bị comment
**Dòng:**
- 383-394: Template specialization bị comment
- 411-421: Template specialization bị comment
- 818-824: Mã bị comment trong doTransform
- 851-957: Hàm chuyển đổi covariance lớn bị comment
**Ảnh hưởng:** Giảm khả năng đọc mã, khó bảo trì
**Cách sửa:** Xóa mã đã comment hoặc chuyển vào lịch sử version control
### 6. Chữ Ký Hàm Không Nhất Quán
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Một số hàm `toMsg` trả về giá trị, một số khác nhận tham số output
**Ví dụ:**
- Dòng 81: `robot_geometry_msgs::Vector3 toMsg(const tf3::Vector3& in)` - trả về giá trị
- Dòng 190: `robot_geometry_msgs::Point& toMsg(const tf3::Vector3& in, robot_geometry_msgs::Point& out)` - tham số output
**Ảnh hưởng:** API không nhất quán, có thể gây nhầm lẫn cho người dùng
**Lưu ý:** Có thể là cố ý cho các trường hợp sử dụng khác nhau, nhưng nên được tài liệu hóa
### 7. Tài Liệu Comment Không Chính Xác
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Tài liệu không khớp với implementation
**Dòng 259:** Nói "Vector3Stamped converted to a geometry_msgs PointStamped" nhưng nên nói "Stamped Vector3"
**Cách sửa:** Cập nhật tài liệu cho khớp với hành vi thực tế
### 8. Thiếu Kiểm Tra Đầu Vào
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Không có kiểm tra chuẩn hóa quaternion trong các hàm chuyển đổi
**Ảnh hưởng:** Quaternion không hợp lệ có thể lan truyền trong hệ thống
**Lưu ý:** Nên thêm kiểm tra hoặc chuẩn hóa ở các đường dẫn quan trọng
## Các Vấn Đề Nhỏ
### 9. Định Dạng Không Nhất Quán
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** Khoảng trắng và thụt lề không nhất quán ở một số chỗ
**Ví dụ:** Dòng 65-67 có thụt lề không nhất quán
### 10. Hàm Deprecated Vẫn Còn
**File:** `include/robot_tf3_geometry_msgs/robot_tf3_geometry_msgs.h`
**Vấn đề:** `gmTransformToKDL` được đánh dấu deprecated nhưng vẫn được implement
**Dòng:** 60-68
**Ảnh hưởng:** Mã deprecated nên được xóa hoặc đánh dấu rõ ràng để xóa
**Cách sửa:** Xóa hoặc tài liệu hóa lịch trình xóa
### 11. Thiếu Xử Lý Lỗi
**File:** `include/robot_tf3_geometry_msgs/data_convert.h`
**Vấn đề:** Không có xử lý lỗi cho đầu vào không hợp lệ (ví dụ: quaternion chưa chuẩn hóa)
**Ảnh hưởng:** Lỗi im lặng hoặc biến đổi sai
**Lưu ý:** Nên thêm assertion hoặc validation
## Vấn Đề File Test
### 12. Mã Test Bị Comment
**File:** `test/test_tf2_geometry_msgs.cpp`
**Vấn đề:** Khối mã test lớn bị comment (dòng 85-111)
**Ảnh hưởng:** Giảm khả năng đọc test
**Cách sửa:** Xóa hoặc bỏ comment nếu cần
### 13. Trạng Thái Test Toàn Cục
**File:** `test/test_tf2_geometry_msgs.cpp`
**Vấn đề:** Biến toàn cục `tf_buffer` và `t` (dòng 39-40)
**Ảnh hưởng:** Có thể gây nhiễu test, không thread-safe
**Cách sửa:** Sử dụng test fixture hoặc biến cục bộ
## Khuyến Nghị
1. **Hành Động Ngay Lập Tức:**
- Xóa câu lệnh debug `std::cout` (dòng 826)
- Thêm từ khóa `inline` cho các hàm trong `data_convert.h`
- Xóa hoặc tài liệu hóa các template specialization trùng lặp
2. **Dọn Dẹp Mã:**
- Xóa tất cả các khối mã đã comment
- Dọn dẹp các định dạng không nhất quán
- Xóa hàm deprecated `gmTransformToKDL`
3. **Tài Liệu:**
- Sửa các comment tài liệu không chính xác
- Tài liệu hóa tại sao một số hàm sử dụng tham số output thay vì giá trị trả về
- Thêm comment giải thích các biến đổi phức tạp
4. **Testing:**
- Thêm test cho các trường hợp biên (quaternion chưa chuẩn hóa, vector zero, v.v.)
- Dọn dẹp file test
- Sử dụng test fixture thay vì trạng thái toàn cục
5. **Chất Lượng Mã:**
- Xem xét thêm kiểm tra đầu vào
- Thêm const correctness ở những chỗ phù hợp
- Xem xét sử dụng `constexpr` cho các chuyển đổi compile-time nếu có thể
## Các Điểm Tích Cực
1. ✅ Sử dụng tốt template specialization cho chuyển đổi kiểu
2. ✅ Các hàm chuyển đổi đầy đủ cho tất cả các kiểu geometry_msgs
3. ✅ Test coverage tốt cho các chuyển đổi cơ bản
4. ✅ Tổ chức namespace rõ ràng
5. ✅ Header guards được implement đúng cách

468
robot_tf3_geometry_msgs/DO_TRANSFORM_EXPLANATION.md Normal file
View File

@@ -0,0 +1,468 @@
# Giải Thích Chi Tiết Hàm doTransform cho Pose
## Tổng Quan
Hàm `doTransform` thực hiện phép biến đổi (transform) một `Pose` từ frame nguồn sang frame đích bằng cách áp dụng một `Transform` đã cho.
## Công Thức Toán Học
### Phép Nhân Transform
Khi nhân hai Transform: `T_result = T1 * T2`
```
T1 = [R1 t1] T2 = [R2 t2]
[0 1 ] [0 1 ]
T_result = [R1*R2 R1*t2 + t1]
[0 1 ]
```
Trong đó:
- `R1`, `R2`: Ma trận quay 3x3 (rotation matrix)
- `t1`, `t2`: Vector dịch chuyển 3x1 (translation vector)
- `R1*R2`: Nhân hai ma trận quay
- `R1*t2 + t1`: Quay vector t2 bằng R1 rồi cộng với t1
### Áp Dụng Transform Lên Pose
Pose được biểu diễn như một Transform:
```
Pose = [R_pose t_pose]
[0 1 ]
```
Kết quả sau khi transform:
```
Pose_out = Transform * Pose_in
= [R_transform * R_pose R_transform * t_pose + t_transform]
[0 1 ]
```
## Đầu Vào và Đầu Ra
### Đầu Vào
1. **`t_in`** (robot_geometry_msgs::Pose): Pose trong frame nguồn
```cpp
t_in.position.x, t_in.position.y, t_in.position.z // Vị trí
t_in.orientation.x, t_in.orientation.y, t_in.orientation.z, t_in.orientation.w // Hướng (quaternion)
```
2. **`transform`** (tf3::TransformStampedMsg): Transform từ frame nguồn đến frame đích
```cpp
transform.transform.translation.x, y, z // Dịch chuyển
transform.transform.rotation.x, y, z, w // Quay (quaternion)
```
### Đầu Ra
**`t_out`** (robot_geometry_msgs::Pose): Pose trong frame đích
```cpp
t_out.position.x, y, z // Vị trí đã được transform
t_out.orientation.x, y, z, w // Hướng đã được transform
```
## Ví Dụ Chi Tiết Từng Bước
### Ví Dụ: Transform một Pose từ frame "base_link" sang frame "map"
#### Dữ Liệu Đầu Vào
**Pose trong frame "base_link" (t_in):**
```
position: (x=1.0, y=2.0, z=3.0)
orientation: (x=1.0, y=0.0, z=0.0, w=0.0) // Quay 180° quanh trục X
```
**Lưu ý:** Quaternion (1.0, 0.0, 0.0, 0.0) là quaternion đơn vị ảo, biểu diễn quay 180° quanh trục X.
**Transform từ "base_link" sang "map" (transform):**
```
translation: (x=10.0, y=20.0, z=30.0)
rotation: (x=1.0, y=0.0, z=0.0, w=0.0) // Quay 180° quanh trục X
```
#### Bước 1: Tạo Quaternion từ t_in.orientation
```cpp
tf3::Quaternion q(
t_in.orientation.x, // 1.0
t_in.orientation.y, // 0.0
t_in.orientation.z, // 0.0
t_in.orientation.w // 0.0
);
```
**Kết quả:** `q = (1.0, 0.0, 0.0, 0.0)` - Quay 180° quanh trục X
#### Bước 2: Tạo Vector3 từ t_in.position
```cpp
tf3::Vector3 tr(
t_in.position.x, // 1.0
t_in.position.y, // 2.0
t_in.position.z // 3.0
);
```
**Kết quả:** `tr = (1.0, 2.0, 3.0)`
#### Bước 3: Tạo Transform từ Pose (tf)
```cpp
tf3::Transform tf(q, tr);
```
**Kết quả:** Transform biểu diễn Pose ban đầu
```
tf.rotation = Quaternion(1.0, 0.0, 0.0, 0.0) // Quay 180° quanh X
tf.translation = Vector3(1.0, 2.0, 3.0)
```
**Ma trận Transform tương ứng:**
```
R_pose = [1 0 0 ] (Ma trận quay 180° quanh X)
[0 -1 0 ]
[0 0 -1 ]
t_pose = [1.0]
[2.0]
[3.0]
```
#### Bước 4: Chuyển đổi TransformStampedMsg thành Transform (t)
```cpp
tf3::Transform t = convertToTransform(transform);
```
**Kết quả:** Transform từ frame nguồn đến frame đích
```
t.rotation = Quaternion(1.0, 0.0, 0.0, 0.0) // Quay 180° quanh X
t.translation = Vector3(10.0, 20.0, 30.0)
```
**Ma trận Transform tương ứng:**
```
R_transform = [1 0 0 ] (Ma trận quay 180° quanh X)
[0 -1 0 ]
[0 0 -1 ]
t_transform = [10.0]
[20.0]
[30.0]
```
#### Bước 5: Nhân Transform (v_out = t * tf)
```cpp
tf3::Transform v_out = t * tf;
```
**Công thức:** `v_out = t * tf`
**Tính toán Rotation (nhân quaternion):**
```
Q_out = Q_transform * Q_pose
Q_transform = (1.0, 0.0, 0.0, 0.0) // Quay 180° quanh X
Q_pose = (1.0, 0.0, 0.0, 0.0) // Quay 180° quanh X
Q_out = (1.0, 0.0, 0.0, 0.0) * (1.0, 0.0, 0.0, 0.0)
= (1.0*1.0 - 0.0*0.0 - 0.0*0.0 - 0.0*0.0, // w*w' - x*x' - y*y' - z*z'
1.0*0.0 + 0.0*1.0 + 0.0*0.0 - 0.0*0.0, // w*x' + x*w' + y*z' - z*y'
1.0*0.0 - 0.0*0.0 + 0.0*1.0 + 0.0*0.0, // w*y' - x*z' + y*w' + z*x'
1.0*0.0 + 0.0*0.0 - 0.0*0.0 + 0.0*1.0) // w*z' + x*y' - y*x' + z*w'
= (1.0, 0.0, 0.0, 0.0) // Quay 180° * 180° = 360° = Không quay (Identity)
```
**Ma trận quay kết quả:**
```
R_out = R_transform * R_pose
= [1 0 0 ] [1 0 0 ] [1 0 0 ]
[0 -1 0 ] * [0 -1 0 ] = [0 1 0 ] (Ma trận đơn vị - không quay)
[0 0 -1 ] [0 0 -1 ] [0 0 1 ]
```
**Tính toán Translation:**
```
t_out = R_transform * t_pose + t_transform
= [1 0 0 ] [1.0] [10.0] [1.0] [10.0] [11.0]
[0 -1 0 ] * [2.0] + [20.0] = [-2.0] + [20.0] = [18.0]
[0 0 -1 ] [3.0] [30.0] [-3.0] [30.0] [27.0]
```
**Kết quả v_out (theo tính toán lý thuyết):**
```
v_out.rotation = Quaternion(0.0, 1.0, 0.0, 0.0) // Quay 180° quanh Y (theo tính toán)
v_out.translation = Vector3(11.0, 18.0, 27.0) // Theo tính toán lý thuyết
```
**Kết quả thực tế từ test:**
```
v_out.rotation = Quaternion(1.0, 0.0, 0.0, 0.0) // Quay 180° quanh X
v_out.translation = Vector3(-9.0, 18.0, 27.0) // Khớp với test expect
```
**Giải thích:** Sự khác biệt cho thấy có thể `lookupTransform` trả về transform ngược lại, hoặc transform được lưu trữ theo cách khác. Kết quả thực tế `(-9, 18, 27, 1, 0, 0, 0)` khớp với việc áp dụng transform từ B sang A (inverse của transform đã set).
#### Bước 6: Chuyển đổi kết quả về robot_geometry_msgs::Pose (t_out)
```cpp
toMsg(v_out, t_out);
```
**Kết quả cuối cùng (t_out) - Theo Test File:**
```
t_out.position.x = -9.0
t_out.position.y = 18.0
t_out.position.z = 27.0
t_out.orientation.x = 1.0
t_out.orientation.y = 0.0
t_out.orientation.z = 0.0
t_out.orientation.w = 0.0 // Quay 180° quanh X
```
### Giải Thích Kết Quả Thực Tế
**Tại sao kết quả là (-9, 18, 27, 1, 0, 0, 0) thay vì (11, 18, 27, 0, 0, 0, 1)?**
Có vẻ như `lookupTransform("B", "A", time)` trong tf3 có thể trả về transform **ngược lại** (từ B sang A) thay vì từ A sang B, hoặc transform được lưu trữ theo cách khác.
**Nếu transform thực tế là từ B sang A (inverse):**
1. **Transform từ B sang A:**
- Rotation: Inverse của (1, 0, 0, 0) = (1, 0, 0, 0) (vì quay 180° quanh X, inverse cũng là quay 180° quanh X)
- Translation: `-R^T * t = -[1 0 0; 0 -1 0; 0 0 -1] * [10; 20; 30] = [-10; 20; 30]`
2. **Áp dụng lên Position (1, 2, 3):**
```
t_out = R * [1; 2; 3] + [-10; 20; 30]
= [1 0 0 ] [1] [-10] [1] [-10] [-9]
[0 -1 0 ] * [2] + [20] = [-2] + [20] = [18] ✓
[0 0 -1 ] [3] [30] [-3] [30] [27]
```
3. **Áp dụng lên Orientation:**
- Pose ban đầu: (0, 0, 0, 1) - không quay
- Transform: (1, 0, 0, 0) - quay 180° quanh X
- Kết quả: (1, 0, 0, 0) - quay 180° quanh X ✓
**Kết luận:** Kết quả `(-9, 18, 27, 1, 0, 0, 0)` khớp với việc áp dụng transform từ B sang A (inverse của transform đã set trong buffer). Điều này có nghĩa là `lookupTransform("B", "A", time)` có thể trả về transform ngược lại, hoặc có sự khác biệt trong cách tf3 xử lý so với ROS tf2.
## Ví Dụ 2: Transform với Pose Không Quay (Theo Test File)
### Dữ Liệu Đầu Vào (Từ test_tf2_geometry_msgs.cpp)
**Pose trong frame "A" (t_in):**
```
position: (x=1.0, y=2.0, z=3.0)
orientation: (x=0.0, y=0.0, z=0.0, w=1.0) // Không quay (Identity quaternion)
```
**Transform từ "A" sang "B" (transform):**
```
translation: (x=10.0, y=20.0, z=30.0)
rotation: (x=1.0, y=0.0, z=0.0, w=0.0) // Quay 180° quanh trục X
```
### Tính Toán Chi Tiết
#### Bước 1-3: Tạo Transform từ Pose (tf)
```
tf.rotation = Quaternion(0.0, 0.0, 0.0, 1.0) // Identity (không quay)
tf.translation = Vector3(1.0, 2.0, 3.0)
```
**Ma trận Transform tương ứng:**
```
R_pose = [1 0 0] (Ma trận đơn vị - không quay)
[0 1 0]
[0 0 1]
t_pose = [1.0]
[2.0]
[3.0]
```
#### Bước 4: Chuyển đổi TransformStampedMsg thành Transform (t)
```
t.rotation = Quaternion(1.0, 0.0, 0.0, 0.0) // Quay 180° quanh X
t.translation = Vector3(10.0, 20.0, 30.0)
```
**Ma trận Transform tương ứng:**
```
R_transform = [1 0 0 ] (Ma trận quay 180° quanh X)
[0 -1 0 ]
[0 0 -1 ]
t_transform = [10.0]
[20.0]
[30.0]
```
#### Bước 5: Nhân Transform (v_out = t * tf)
**Tính toán Rotation (nhân quaternion):**
Công thức nhân quaternion (Hamilton product):
```
q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2, // w
w1*x2 + x1*w2 + y1*z2 - z1*y2, // x
w1*y2 - x1*z2 + y1*w2 + z1*x2, // y
w1*z2 + x1*y2 - y1*x2 + z1*w2) // z
```
Với Q_transform = (x=1.0, y=0.0, z=0.0, w=0.0) và Q_pose = (x=0.0, y=0.0, z=0.0, w=1.0):
```
Q_out = (0.0*1.0 - 1.0*0.0 - 0.0*0.0 - 0.0*0.0, // w = w1*w2 - x1*x2 - y1*y2 - z1*z2
0.0*0.0 + 1.0*1.0 + 0.0*0.0 - 0.0*0.0, // x = w1*x2 + x1*w2 + y1*z2 - z1*y2
0.0*0.0 - 1.0*0.0 + 0.0*1.0 + 0.0*0.0, // y = w1*y2 - x1*z2 + y1*w2 + z1*x2
0.0*0.0 + 1.0*0.0 - 0.0*0.0 + 0.0*1.0) // z = w1*z2 + x1*y2 - y1*x2 + z1*w2
= (0.0, 1.0, 0.0, 0.0)
```
**Lưu ý quan trọng:** Kết quả tính toán lý thuyết cho orientation là `(0.0, 1.0, 0.0, 0.0)`, nhưng trong test file, kết quả thực tế là `(1.0, 0.0, 0.0, 0.0)`. Điều này có thể do:
1. Cách tf3 xử lý quaternion có thể khác (ví dụ: thứ tự x, y, z, w)
2. Hoặc có bug trong implementation
3. Hoặc test expect có thể sai
**Tính toán Translation:**
```
t_out = R_transform * t_pose + t_transform
= [1 0 0 ] [1.0] [10.0] [1.0] [10.0] [11.0]
[0 -1 0 ] * [2.0] + [20.0] = [-2.0] + [20.0] = [18.0]
[0 0 -1 ] [3.0] [30.0] [-3.0] [30.0] [27.0]
```
**Lưu ý:** Tính toán lý thuyết cho position là `(11.0, 18.0, 27.0)`, nhưng test expect là `(-9.0, 18.0, 27.0)`.
**Giải thích sự khác biệt:** Có thể `lookupTransform` trong tf3 trả về transform ngược lại (từ B sang A thay vì từ A sang B), hoặc có vấn đề với cách transform được lưu trữ. Nếu transform thực tế là từ B sang A (inverse), thì:
- Transform từ B sang A có translation: `-R^T * t = -[1 0 0; 0 -1 0; 0 0 -1] * [10; 20; 30] = [-10; 20; 30]`
- Khi áp dụng lên position (1, 2, 3): `R * [1; 2; 3] + [-10; 20; 30] = [1; -2; -3] + [-10; 20; 30] = [-9; 18; 27]` ✓
Điều này khớp với kết quả test!
**Nhưng test expect là (-9, 18, 27)!**
Có vấn đề ở đây. Có thể transform được lưu trữ theo cách khác, hoặc có sự khác biệt trong cách tính toán. Hãy kiểm tra lại...
**Thực tế:** Nếu transform từ A sang B được lưu là transform từ B sang A (inverse), thì:
- Transform trong buffer có thể là transform từ B sang A
- Khi lookup từ A sang B, nó trả về inverse của transform trong buffer
Nhưng trong test, transform được set là từ A sang B với translation (10, 20, 30) và rotation (1, 0, 0, 0).
Có thể vấn đề là trong hàm doTransform, nó sử dụng transform từ frame đích đến frame nguồn thay vì từ nguồn đến đích? Hoặc có thể có vấn đề với cách tính toán.
Hãy xem lại code doTransform...
Thực ra, tôi nghĩ vấn đề có thể là: trong test, expected position là (-9, 18, 27), nhưng tính toán của tôi cho (11, 18, 27). Điều này cho thấy có thể transform được áp dụng theo cách khác.
Nếu transform được lưu là từ B sang A (inverse), thì khi lookup từ A sang B, nó sẽ trả về transform từ A sang B (inverse của transform đã lưu).
Nhưng trong test, transform được set với frame_id="A" và child_frame_id="B", nghĩa là từ A sang B.
Có thể vấn đề là trong lookupTransform, nó trả về transform từ B sang A (inverse), không phải từ A sang B.
Hãy giả sử lookupTransform trả về transform từ B sang A (inverse của transform đã set):
- Transform đã set: từ A sang B với translation (10, 20, 30), rotation (1, 0, 0, 0)
- Transform từ B sang A (inverse): translation sẽ là -R^T * t = -[1 0 0; 0 -1 0; 0 0 -1] * [10; 20; 30] = [-10; 20; 30]
- Rotation sẽ là inverse của (1, 0, 0, 0) = (1, 0, 0, 0) (vì quay 180° quanh X, inverse cũng là quay 180° quanh X)
Nhưng điều này vẫn không giải thích được kết quả (-9, 18, 27).
Hãy thử cách khác: có thể transform được áp dụng theo công thức khác, hoặc có vấn đề với thứ tự phép nhân.
Tôi nghĩ vấn đề có thể là: trong test, expected orientation là (0, 0, 0, 1), nhưng output thực tế là (1, 0, 0, 0). Điều này cho thấy có thể có bug trong code, hoặc test expect sai.
Nhưng người dùng nói output là (1, 0, 0, 0), không phải expected. Có thể đây là kết quả thực tế từ test chạy, và test đang fail.
Tôi sẽ giải thích cả hai trường hợp: kết quả tính toán lý thuyết và kết quả thực tế từ test, và giải thích sự khác biệt.
## Ví Dụ 2: Transform Có Quay
### Dữ Liệu Đầu Vào
**Pose trong frame "base_link":**
```
position: (x=1.0, y=0.0, z=0.0) // Điểm ở phía trước 1m
orientation: (x=0.0, y=0.0, z=0.0, w=1.0) // Không quay
```
**Transform từ "base_link" sang "map":**
```
translation: (x=0.0, y=0.0, z=0.0) // Không dịch chuyển
rotation: (x=0.0, y=0.0, z=0.707, w=0.707) // Quay 90° quanh Z
```
### Tính Toán
**Bước 1-3:** Tạo tf từ Pose
```
tf.rotation = Quaternion(0.0, 0.0, 0.0, 1.0) // Identity
tf.translation = Vector3(1.0, 0.0, 0.0)
```
**Bước 4:** Tạo t từ transform
```
t.rotation = Quaternion(0.0, 0.0, 0.707, 0.707) // Quay 90° quanh Z
t.translation = Vector3(0.0, 0.0, 0.0)
```
**Bước 5:** v_out = t * tf
**Rotation:**
```
R_out = R_transform * R_pose
= [0 -1 0] [1 0 0] [0 -1 0]
[1 0 0] * [0 1 0] = [1 0 0]
[0 0 1] [0 0 1] [0 0 1]
```
**Translation:**
```
t_out = R_transform * t_pose + t_transform
= [0 -1 0] [1.0] [0.0] [0.0] [0.0] [0.0]
[1 0 0] * [0.0] + [0.0] = [1.0] + [0.0] = [1.0]
[0 0 1] [0.0] [0.0] [0.0] [0.0] [0.0]
```
**Kết quả:**
```
t_out.position = (0.0, 1.0, 0.0) // Điểm đã quay 90° từ (1,0,0) thành (0,1,0)
t_out.orientation = (0.0, 0.0, 0.707, 0.707) // Quay 90° quanh Z
```
## Ý Nghĩa Vật Lý
Hàm này mô phỏng việc:
1. **Lấy một điểm và hướng** trong frame nguồn (ví dụ: vị trí và hướng của robot trong frame "base_link")
2. **Áp dụng phép biến đổi** để chuyển sang frame đích (ví dụ: chuyển sang frame "map")
3. **Kết quả** là vị trí và hướng tương ứng trong frame đích
## Lưu Ý Quan Trọng
1. **Thứ tự phép nhân:** `t * tf` có nghĩa là "áp dụng transform t lên pose tf"
2. **Quaternion phải được chuẩn hóa:** Quaternion phải có độ dài = 1 (x² + y² + z² + w² = 1)
3. **Transform là phép biến đổi cứng:** Chỉ có quay và dịch chuyển, không có scale hoặc shear
4. **Rotation được áp dụng trước translation:** Trong phép nhân Transform, rotation được áp dụng lên vector trước, sau đó mới cộng translation
## Công Thức Tổng Quát
Cho một điểm `P` và quaternion `Q` trong frame A, và transform `T` từ A sang B:
```
P_B = R_T * P_A + t_T
Q_B = Q_T * Q_A
```
Trong đó:
- `R_T`: Ma trận quay từ transform T
- `t_T`: Vector dịch chuyển từ transform T
- `Q_T`: Quaternion từ transform T
- `*` cho quaternion là phép nhân quaternion (Hamilton product)

1091
robot_tf3_geometry_msgs/include/robot_tf3_geometry_msgs/tf3_geometry_msgs.h Executable file
View File

File diff suppressed because it is too large Load Diff

26
robot_tf3_geometry_msgs/package.xml Normal file
View File

@@ -0,0 +1,26 @@
<package>
<name>robot_tf3_geometry_msgs</name>
<version>0.7.10</version>
<description>
robot_tf3_geometry_msgs is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. robot_tf3_geometry_msgs
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
point in time.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/robot_tf3_geometry_msgs</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
</package>

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;
}