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

HiepLM update

Browse Source
This commit is contained in:
HiepLM 2025-12-30 09:06:48 +07:00
parent 3c51b228ec
commit 2987c1a481
8 changed files with 158 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
tf3_geometry_msgs/CMakeLists.txt
View File

@ -49,7 +49,7 @@ install(TARGETS tf3_geometry_msgs
# --- Tạo file lib/cmake/tf3_geometry_msgs/tf3_geometry_msgs-targets.cmake --- # --- Tạo file lib/cmake/tf3_geometry_msgs/tf3_geometry_msgs-targets.cmake ---
# --- File này chứa cấu hình giúp project khác có thể dùng --- # --- File này chứa cấu hình giúp project khác có thể dùng ---
# --- Find_package(tf3_geometry_msgs REQUIRED) --- # --- Find_package(tf3_geometry_msgs REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE tf3_geometry_msgs::tf3_geometry_msgs) --- # --- Target_link_libraries(my_app PRIVATE tf3_robot_geometry_msgs::tf3_geometry_msgs) ---
install(EXPORT tf3_geometry_msgs-targets install(EXPORT tf3_geometry_msgs-targets
FILE tf3_geometry_msgs-targets.cmake FILE tf3_geometry_msgs-targets.cmake
DESTINATION lib/cmake/tf3_geometry_msgs DESTINATION lib/cmake/tf3_geometry_msgs

6
tf3_geometry_msgs/CODE_REVIEW.md
View File

@ -37,7 +37,7 @@ std::cout << "doTransform Pose: " << std::endl;
//Backwards compatibility remove when forked for Lunar or newer //Backwards compatibility remove when forked for Lunar or newer
template <> template <>
inline inline
geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in) robot_geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
{ {
return toMsg(in); // Chỉ gọi cùng một hàm return toMsg(in); // Chỉ gọi cùng một hàm
} }
@ -60,8 +60,8 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
**File:** `include/tf3_geometry_msgs/tf3_geometry_msgs.h` **File:** `include/tf3_geometry_msgs/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ấn đề:** Một số hàm `toMsg` trả về giá trị, một số khác nhận tham số output
**Ví dụ:** **Ví dụ:**
- Dòng 81: `geometry_msgs::Vector3 toMsg(const tf3::Vector3& in)` - trả về giá trị - Dòng 81: `robot_geometry_msgs::Vector3 toMsg(const tf3::Vector3& in)` - trả về giá trị
- Dòng 190: `geometry_msgs::Point& toMsg(const tf3::Vector3& in, geometry_msgs::Point& out)` - tham số output - 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 **Ả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 **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

6
tf3_geometry_msgs/DO_TRANSFORM_EXPLANATION.md
View File

@ -43,7 +43,7 @@ Pose_out = Transform * Pose_in
### Đầu Vào ### Đầu Vào
1. **`t_in`** (geometry_msgs::Pose): Pose trong frame nguồn 1. **`t_in`** (robot_geometry_msgs::Pose): Pose trong frame nguồn
```cpp ```cpp
t_in.position.x, t_in.position.y, t_in.position.z // Vị trí 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) t_in.orientation.x, t_in.orientation.y, t_in.orientation.z, t_in.orientation.w // Hướng (quaternion)
@ -57,7 +57,7 @@ Pose_out = Transform * Pose_in
### Đầu Ra ### Đầu Ra
**`t_out`** (geometry_msgs::Pose): Pose trong frame đích **`t_out`** (robot_geometry_msgs::Pose): Pose trong frame đích
```cpp ```cpp
t_out.position.x, y, z // Vị trí đã được transform t_out.position.x, y, z // Vị trí đã được transform
t_out.orientation.x, y, z, w // Hướng đã được transform t_out.orientation.x, y, z, w // Hướng đã được transform
@ -207,7 +207,7 @@ 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). **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ề geometry_msgs::Pose (t_out) #### Bước 6: Chuyển đổi kết quả về robot_geometry_msgs::Pose (t_out)
```cpp ```cpp
toMsg(v_out, t_out); toMsg(v_out, t_out);

170
tf3_geometry_msgs/include/tf3_geometry_msgs/tf3_geometry_msgs.h
View File

@ -36,15 +36,15 @@
#include <tf3/LinearMath/Quaternion.h> #include <tf3/LinearMath/Quaternion.h>
#include <tf3/LinearMath/Transform.h> #include <tf3/LinearMath/Transform.h>
#include <data_convert/data_convert.h> #include <data_convert/data_convert.h>
#include <geometry_msgs/PointStamped.h> #include <robot_geometry_msgs/PointStamped.h>
#include <geometry_msgs/QuaternionStamped.h> #include <robot_geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/TransformStamped.h> #include <robot_geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Vector3Stamped.h> #include <robot_geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Pose.h> #include <robot_geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h> #include <robot_geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Wrench.h> #include <robot_geometry_msgs/Wrench.h>
#include <geometry_msgs/WrenchStamped.h> #include <robot_geometry_msgs/WrenchStamped.h>
#include <kdl/frames.hpp> #include <kdl/frames.hpp>
#include <array> #include <array>
@ -58,9 +58,9 @@ namespace tf3
* \deprecated * \deprecated
*/ */
inline inline
ROS_DEPRECATED KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t); ROS_DEPRECATED KDL::Frame gmTransformToKDL(const robot_geometry_msgs::TransformStamped& t);
inline inline
KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) KDL::Frame gmTransformToKDL(const robot_geometry_msgs::TransformStamped& t)
{ {
return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
t.transform.rotation.z, t.transform.rotation.w), t.transform.rotation.z, t.transform.rotation.w),
@ -78,9 +78,9 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t)
* \return The Vector3 converted to a geometry_msgs message type. * \return The Vector3 converted to a geometry_msgs message type.
*/ */
inline inline
geometry_msgs::Vector3 toMsg(const tf3::Vector3& in) robot_geometry_msgs::Vector3 toMsg(const tf3::Vector3& in)
{ {
geometry_msgs::Vector3 out; robot_geometry_msgs::Vector3 out;
out.x = in.getX(); out.x = in.getX();
out.y = in.getY(); out.y = in.getY();
out.z = in.getZ(); out.z = in.getZ();
@ -93,7 +93,7 @@ geometry_msgs::Vector3 toMsg(const tf3::Vector3& in)
* \param out The Vector3 converted to a tf3 type. * \param out The Vector3 converted to a tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::Vector3& in, tf3::Vector3& out) void fromMsg(const robot_geometry_msgs::Vector3& in, tf3::Vector3& out)
{ {
out = tf3::Vector3(in.x, in.y, in.z); out = tf3::Vector3(in.x, in.y, in.z);
} }
@ -111,7 +111,7 @@ void fromMsg(const geometry_msgs::Vector3& in, tf3::Vector3& out)
*/ */
template <> template <>
inline inline
const tf3::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return data_convert::convertTime(t.header.stamp);} const tf3::Time& getTimestamp(const robot_geometry_msgs::Vector3Stamped& t) {return data_convert::convertTime(t.header.stamp);}
/** \brief Extract a frame ID from the header of a Vector message. /** \brief Extract a frame ID from the header of a Vector message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h. * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@ -121,7 +121,7 @@ inline
*/ */
template <> template <>
inline inline
const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;} const std::string& getFrameId(const robot_geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;}
/** \brief Trivial "conversion" function for Vector3 message type. /** \brief Trivial "conversion" function for Vector3 message type.
@ -130,7 +130,7 @@ inline
* \return The input argument. * \return The input argument.
*/ */
inline inline
geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in) robot_geometry_msgs::Vector3Stamped toMsg(const robot_geometry_msgs::Vector3Stamped& in)
{ {
return in; return in;
} }
@ -141,7 +141,7 @@ geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
* \param out The input argument. * \param out The input argument.
*/ */
inline inline
void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out) void fromMsg(const robot_geometry_msgs::Vector3Stamped& msg, robot_geometry_msgs::Vector3Stamped& out)
{ {
out = msg; out = msg;
} }
@ -152,9 +152,9 @@ void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Sta
* \return The Vector3Stamped converted to a geometry_msgs Vector3Stamped message type. * \return The Vector3Stamped converted to a geometry_msgs Vector3Stamped message type.
*/ */
inline inline
geometry_msgs::Vector3Stamped toMsg(const tf3::Stamped<tf3::Vector3>& in) robot_geometry_msgs::Vector3Stamped toMsg(const tf3::Stamped<tf3::Vector3>& in)
{ {
geometry_msgs::Vector3Stamped out; robot_geometry_msgs::Vector3Stamped out;
out.header.stamp = data_convert::convertTime(in.stamp_); out.header.stamp = data_convert::convertTime(in.stamp_);
out.header.frame_id = in.frame_id_; out.header.frame_id = in.frame_id_;
out.vector.x = in.getX(); out.vector.x = in.getX();
@ -169,7 +169,7 @@ geometry_msgs::Vector3Stamped toMsg(const tf3::Stamped<tf3::Vector3>& in)
* \param out The Vector3Stamped converted to the equivalent tf3 type. * \param out The Vector3Stamped converted to the equivalent tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf3::Stamped<tf3::Vector3>& out) void fromMsg(const robot_geometry_msgs::Vector3Stamped& msg, tf3::Stamped<tf3::Vector3>& out)
{ {
out.stamp_ = data_convert::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
@ -187,7 +187,7 @@ void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf3::Stamped<tf3::Vector3
* \return The Vector3 converted to a geometry_msgs message type. * \return The Vector3 converted to a geometry_msgs message type.
*/ */
inline inline
geometry_msgs::Point& toMsg(const tf3::Vector3& in, geometry_msgs::Point& out) robot_geometry_msgs::Point& toMsg(const tf3::Vector3& in, robot_geometry_msgs::Point& out)
{ {
out.x = in.getX(); out.x = in.getX();
out.y = in.getY(); out.y = in.getY();
@ -201,7 +201,7 @@ geometry_msgs::Point& toMsg(const tf3::Vector3& in, geometry_msgs::Point& out)
* \param out The Vector3 converted to a tf3 type. * \param out The Vector3 converted to a tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::Point& in, tf3::Vector3& out) void fromMsg(const robot_geometry_msgs::Point& in, tf3::Vector3& out)
{ {
out = tf3::Vector3(in.x, in.y, in.z); out = tf3::Vector3(in.x, in.y, in.z);
} }
@ -219,7 +219,7 @@ void fromMsg(const geometry_msgs::Point& in, tf3::Vector3& out)
*/ */
template <> template <>
inline inline
const tf3::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return data_convert::convertTime(t.header.stamp);} const tf3::Time& getTimestamp(const robot_geometry_msgs::PointStamped& t) {return data_convert::convertTime(t.header.stamp);}
/** \brief Extract a frame ID from the header of a Point message. /** \brief Extract a frame ID from the header of a Point message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h. * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@ -229,7 +229,7 @@ inline
*/ */
template <> template <>
inline inline
const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;} const std::string& getFrameId(const robot_geometry_msgs::PointStamped& t) {return t.header.frame_id;}
/** \brief Trivial "conversion" function for Point message type. /** \brief Trivial "conversion" function for Point message type.
* This function is a specialization of the toMsg template defined in tf3/convert.h. * This function is a specialization of the toMsg template defined in tf3/convert.h.
@ -237,7 +237,7 @@ inline
* \return The input argument. * \return The input argument.
*/ */
inline inline
geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in) robot_geometry_msgs::PointStamped toMsg(const robot_geometry_msgs::PointStamped& in)
{ {
return in; return in;
} }
@ -248,7 +248,7 @@ geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
* \param out The input argument. * \param out The input argument.
*/ */
inline inline
void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out) void fromMsg(const robot_geometry_msgs::PointStamped& msg, robot_geometry_msgs::PointStamped& out)
{ {
out = msg; out = msg;
} }
@ -259,7 +259,7 @@ void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped
* \return The Vector3Stamped converted to a geometry_msgs PointStamped message type. * \return The Vector3Stamped converted to a geometry_msgs PointStamped message type.
*/ */
inline inline
geometry_msgs::PointStamped toMsg(const tf3::Stamped<tf3::Vector3>& in, geometry_msgs::PointStamped & out) robot_geometry_msgs::PointStamped toMsg(const tf3::Stamped<tf3::Vector3>& in, robot_geometry_msgs::PointStamped & out)
{ {
out.header.stamp = data_convert::convertTime(in.stamp_); out.header.stamp = data_convert::convertTime(in.stamp_);
out.header.frame_id = in.frame_id_; out.header.frame_id = in.frame_id_;
@ -275,7 +275,7 @@ geometry_msgs::PointStamped toMsg(const tf3::Stamped<tf3::Vector3>& in, geometry
* \param out The PointStamped converted to the equivalent tf3 type. * \param out The PointStamped converted to the equivalent tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::PointStamped& msg, tf3::Stamped<tf3::Vector3>& out) void fromMsg(const robot_geometry_msgs::PointStamped& msg, tf3::Stamped<tf3::Vector3>& out)
{ {
out.stamp_ = data_convert::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
@ -293,9 +293,9 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf3::Stamped<tf3::Vector3>&
* \return The Quaternion converted to a geometry_msgs message type. * \return The Quaternion converted to a geometry_msgs message type.
*/ */
inline inline
geometry_msgs::Quaternion toMsg(const tf3::Quaternion& in) robot_geometry_msgs::Quaternion toMsg(const tf3::Quaternion& in)
{ {
geometry_msgs::Quaternion out; robot_geometry_msgs::Quaternion out;
out.w = in.getW(); out.w = in.getW();
out.x = in.getX(); out.x = in.getX();
out.y = in.getY(); out.y = in.getY();
@ -309,7 +309,7 @@ geometry_msgs::Quaternion toMsg(const tf3::Quaternion& in)
* \param out The Quaternion converted to a tf3 type. * \param out The Quaternion converted to a tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::Quaternion& in, tf3::Quaternion& out) void fromMsg(const robot_geometry_msgs::Quaternion& in, tf3::Quaternion& out)
{ {
// w at the end in the constructor // w at the end in the constructor
out = tf3::Quaternion(in.x, in.y, in.z, in.w); out = tf3::Quaternion(in.x, in.y, in.z, in.w);
@ -328,7 +328,7 @@ void fromMsg(const geometry_msgs::Quaternion& in, tf3::Quaternion& out)
*/ */
template <> template <>
inline inline
const tf3::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return data_convert::convertTime(t.header.stamp);} const tf3::Time& getTimestamp(const robot_geometry_msgs::QuaternionStamped& t) {return data_convert::convertTime(t.header.stamp);}
/** \brief Extract a frame ID from the header of a Quaternion message. /** \brief Extract a frame ID from the header of a Quaternion message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h. * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@ -338,7 +338,7 @@ const tf3::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {retur
*/ */
template <> template <>
inline inline
const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;} const std::string& getFrameId(const robot_geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;}
/** \brief Trivial "conversion" function for Quaternion message type. /** \brief Trivial "conversion" function for Quaternion message type.
* This function is a specialization of the toMsg template defined in tf3/convert.h. * This function is a specialization of the toMsg template defined in tf3/convert.h.
@ -346,7 +346,7 @@ const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {retur
* \return The input argument. * \return The input argument.
*/ */
inline inline
geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in) robot_geometry_msgs::QuaternionStamped toMsg(const robot_geometry_msgs::QuaternionStamped& in)
{ {
return in; return in;
} }
@ -357,7 +357,7 @@ geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& i
* \param out The input argument. * \param out The input argument.
*/ */
inline inline
void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out) void fromMsg(const robot_geometry_msgs::QuaternionStamped& msg, robot_geometry_msgs::QuaternionStamped& out)
{ {
out = msg; out = msg;
} }
@ -368,9 +368,9 @@ void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::Quatern
* \return The QuaternionStamped converted to a geometry_msgs QuaternionStamped message type. * \return The QuaternionStamped converted to a geometry_msgs QuaternionStamped message type.
*/ */
inline inline
geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in) robot_geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
{ {
geometry_msgs::QuaternionStamped out; robot_geometry_msgs::QuaternionStamped out;
out.header.stamp = data_convert::convertTime(in.stamp_); out.header.stamp = data_convert::convertTime(in.stamp_);
out.header.frame_id = in.frame_id_; out.header.frame_id = in.frame_id_;
out.quaternion.w = in.getW(); out.quaternion.w = in.getW();
@ -382,13 +382,13 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
// template <> // template <>
// inline // inline
// ROS_DEPRECATED geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in); // ROS_DEPRECATED robot_geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in);
//Backwards compatibility remove when forked for Lunar or newer //Backwards compatibility remove when forked for Lunar or newer
template <> template <>
inline inline
geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in) robot_geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
{ {
return toMsg(in); return toMsg(in);
} }
@ -399,7 +399,7 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
* \param out The QuaternionStamped converted to the equivalent tf3 type. * \param out The QuaternionStamped converted to the equivalent tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out) void fromMsg(const robot_geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out)
{ {
out.stamp_ = data_convert::convertTime(in.header.stamp); out.stamp_ = data_convert::convertTime(in.header.stamp);
out.frame_id_ = in.header.frame_id; out.frame_id_ = in.header.frame_id;
@ -410,12 +410,12 @@ void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quate
// template<> // template<>
// inline // inline
// ROS_DEPRECATED void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out); // ROS_DEPRECATED void fromMsg(const robot_geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out);
//Backwards compatibility remove when forked for Lunar or newer //Backwards compatibility remove when forked for Lunar or newer
template<> template<>
inline inline
void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out) void fromMsg(const robot_geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quaternion>& out)
{ {
fromMsg(in, out); fromMsg(in, out);
} }
@ -429,7 +429,7 @@ void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quate
* \param out The Transform converted to a geometry_msgs Pose message type. * \param out The Transform converted to a geometry_msgs Pose message type.
*/ */
inline inline
geometry_msgs::Pose& toMsg(const tf3::Transform& in, geometry_msgs::Pose& out) robot_geometry_msgs::Pose& toMsg(const tf3::Transform& in, robot_geometry_msgs::Pose& out)
{ {
toMsg(in.getOrigin(), out.position); toMsg(in.getOrigin(), out.position);
out.orientation = toMsg(in.getRotation()); out.orientation = toMsg(in.getRotation());
@ -441,7 +441,7 @@ geometry_msgs::Pose& toMsg(const tf3::Transform& in, geometry_msgs::Pose& out)
* \param out The Pose converted to a tf3 Transform type. * \param out The Pose converted to a tf3 Transform type.
*/ */
inline inline
void fromMsg(const geometry_msgs::Pose& in, tf3::Transform& out) void fromMsg(const robot_geometry_msgs::Pose& in, tf3::Transform& out)
{ {
out.setOrigin(tf3::Vector3(in.position.x, in.position.y, in.position.z)); out.setOrigin(tf3::Vector3(in.position.x, in.position.y, in.position.z));
// w at the end in the constructor // w at the end in the constructor
@ -461,7 +461,7 @@ void fromMsg(const geometry_msgs::Pose& in, tf3::Transform& out)
*/ */
template <> template <>
inline inline
const tf3::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return data_convert::convertTime(t.header.stamp);} const tf3::Time& getTimestamp(const robot_geometry_msgs::PoseStamped& t) {return data_convert::convertTime(t.header.stamp);}
/** \brief Extract a frame ID from the header of a Pose message. /** \brief Extract a frame ID from the header of a Pose message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h. * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@ -470,7 +470,7 @@ inline
*/ */
template <> template <>
inline inline
const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;} const std::string& getFrameId(const robot_geometry_msgs::PoseStamped& t) {return t.header.frame_id;}
/** \brief Trivial "conversion" function for Pose message type. /** \brief Trivial "conversion" function for Pose message type.
* This function is a specialization of the toMsg template defined in tf3/convert.h. * This function is a specialization of the toMsg template defined in tf3/convert.h.
@ -478,7 +478,7 @@ inline
* \return The input argument. * \return The input argument.
*/ */
inline inline
geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in) robot_geometry_msgs::PoseStamped toMsg(const robot_geometry_msgs::PoseStamped& in)
{ {
return in; return in;
} }
@ -489,7 +489,7 @@ geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
* \param out The input argument. * \param out The input argument.
*/ */
inline inline
void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out) void fromMsg(const robot_geometry_msgs::PoseStamped& msg, robot_geometry_msgs::PoseStamped& out)
{ {
out = msg; out = msg;
} }
@ -500,7 +500,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped&
* \return The PoseStamped converted to a geometry_msgs PoseStamped message type. * \return The PoseStamped converted to a geometry_msgs PoseStamped message type.
*/ */
inline inline
geometry_msgs::PoseStamped toMsg(const tf3::Stamped<tf3::Transform>& in, geometry_msgs::PoseStamped & out) robot_geometry_msgs::PoseStamped toMsg(const tf3::Stamped<tf3::Transform>& in, robot_geometry_msgs::PoseStamped & out)
{ {
out.header.stamp = data_convert::convertTime(in.stamp_); out.header.stamp = data_convert::convertTime(in.stamp_);
out.header.frame_id = in.frame_id_; out.header.frame_id = in.frame_id_;
@ -515,7 +515,7 @@ geometry_msgs::PoseStamped toMsg(const tf3::Stamped<tf3::Transform>& in, geometr
* \param out The PoseStamped converted to the equivalent tf3 type. * \param out The PoseStamped converted to the equivalent tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>& out) void fromMsg(const robot_geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>& out)
{ {
out.stamp_ = data_convert::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
@ -535,7 +535,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>
*/ */
template <> template <>
inline inline
const tf3::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return data_convert::convertTime(t.header.stamp);} const tf3::Time& getTimestamp(const robot_geometry_msgs::PoseWithCovarianceStamped& t) {return data_convert::convertTime(t.header.stamp);}
/** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message. /** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h. * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@ -544,7 +544,7 @@ inline
*/ */
template <> template <>
inline inline
const std::string& getFrameId(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;} const std::string& getFrameId(const robot_geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;}
/** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. /** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type.
* This function is a specialization of the toMsg template defined in tf3/convert.h. * This function is a specialization of the toMsg template defined in tf3/convert.h.
@ -552,7 +552,7 @@ inline
* \return The input argument. * \return The input argument.
*/ */
inline inline
geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in) robot_geometry_msgs::PoseWithCovarianceStamped toMsg(const robot_geometry_msgs::PoseWithCovarianceStamped& in)
{ {
return in; return in;
} }
@ -563,7 +563,7 @@ geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCova
* \param out The input argument. * \param out The input argument.
*/ */
inline inline
void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out) void fromMsg(const robot_geometry_msgs::PoseWithCovarianceStamped& msg, robot_geometry_msgs::PoseWithCovarianceStamped& out)
{ {
out = msg; out = msg;
} }
@ -574,7 +574,7 @@ void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs:
* \return The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type. * \return The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type.
*/ */
inline inline
geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped<tf3::Transform>& in, geometry_msgs::PoseWithCovarianceStamped & out) robot_geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped<tf3::Transform>& in, robot_geometry_msgs::PoseWithCovarianceStamped & out)
{ {
out.header.stamp = data_convert::convertTime(in.stamp_); out.header.stamp = data_convert::convertTime(in.stamp_);
out.header.frame_id = in.frame_id_; out.header.frame_id = in.frame_id_;
@ -589,7 +589,7 @@ geometry_msgs::PoseWithCovarianceStamped toMsg(const tf3::Stamped<tf3::Transform
* \param out The PoseWithCovarianceStamped converted to the equivalent tf3 type. * \param out The PoseWithCovarianceStamped converted to the equivalent tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf3::Stamped<tf3::Transform>& out) void fromMsg(const robot_geometry_msgs::PoseWithCovarianceStamped& msg, tf3::Stamped<tf3::Transform>& out)
{ {
out.stamp_ = data_convert::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
@ -608,9 +608,9 @@ void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf3::Stamped<t
* \return The Transform converted to a geometry_msgs message type. * \return The Transform converted to a geometry_msgs message type.
*/ */
inline inline
geometry_msgs::Transform toMsg(const tf3::Transform& in) robot_geometry_msgs::Transform toMsg(const tf3::Transform& in)
{ {
geometry_msgs::Transform out; robot_geometry_msgs::Transform out;
out.translation = toMsg(in.getOrigin()); out.translation = toMsg(in.getOrigin());
out.rotation = toMsg(in.getRotation()); out.rotation = toMsg(in.getRotation());
return out; return out;
@ -622,7 +622,7 @@ geometry_msgs::Transform toMsg(const tf3::Transform& in)
* \param out The Transform converted to a tf3 type. * \param out The Transform converted to a tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::Transform& in, tf3::Transform& out) void fromMsg(const robot_geometry_msgs::Transform& in, tf3::Transform& out)
{ {
tf3::Vector3 v; tf3::Vector3 v;
fromMsg(in.translation, v); fromMsg(in.translation, v);
@ -659,7 +659,7 @@ void fromMsg(const tf3::TransformMsg& in, tf3::Transform& out)
*/ */
template <> template <>
inline inline
const tf3::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return data_convert::convertTime(t.header.stamp);} const tf3::Time& getTimestamp(const robot_geometry_msgs::TransformStamped& t) {return data_convert::convertTime(t.header.stamp);}
/** \brief Extract a frame ID from the header of a Transform message. /** \brief Extract a frame ID from the header of a Transform message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h. * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@ -668,7 +668,7 @@ const tf3::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return
*/ */
template <> template <>
inline inline
const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;} const std::string& getFrameId(const robot_geometry_msgs::TransformStamped& t) {return t.header.frame_id;}
/** \brief Trivial "conversion" function for Transform message type. /** \brief Trivial "conversion" function for Transform message type.
* This function is a specialization of the toMsg template defined in tf3/convert.h. * This function is a specialization of the toMsg template defined in tf3/convert.h.
@ -676,7 +676,7 @@ const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return
* \return The input argument. * \return The input argument.
*/ */
inline inline
geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in) robot_geometry_msgs::TransformStamped toMsg(const robot_geometry_msgs::TransformStamped& in)
{ {
return in; return in;
} }
@ -686,7 +686,7 @@ geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in)
* \param out The input argument. * \param out The input argument.
*/ */
inline inline
void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out) void fromMsg(const robot_geometry_msgs::TransformStamped& msg, robot_geometry_msgs::TransformStamped& out)
{ {
out = msg; out = msg;
} }
@ -697,9 +697,9 @@ void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::Transfor
* \return The tf3::Stamped<tf3::Transform> converted to a geometry_msgs TransformStamped message type. * \return The tf3::Stamped<tf3::Transform> converted to a geometry_msgs TransformStamped message type.
*/ */
inline inline
geometry_msgs::TransformStamped toMsg(const tf3::Stamped<tf3::Transform>& in) robot_geometry_msgs::TransformStamped toMsg(const tf3::Stamped<tf3::Transform>& in)
{ {
geometry_msgs::TransformStamped out; robot_geometry_msgs::TransformStamped out;
out.header.stamp = data_convert::convertTime(in.stamp_); out.header.stamp = data_convert::convertTime(in.stamp_);
out.header.frame_id = in.frame_id_; out.header.frame_id = in.frame_id_;
out.transform.translation = toMsg(in.getOrigin()); out.transform.translation = toMsg(in.getOrigin());
@ -714,7 +714,7 @@ geometry_msgs::TransformStamped toMsg(const tf3::Stamped<tf3::Transform>& in)
* \param out The TransformStamped converted to the equivalent tf3 type. * \param out The TransformStamped converted to the equivalent tf3 type.
*/ */
inline inline
void fromMsg(const geometry_msgs::TransformStamped& msg, tf3::Stamped<tf3::Transform>& out) void fromMsg(const robot_geometry_msgs::TransformStamped& msg, tf3::Stamped<tf3::Transform>& out)
{ {
out.stamp_ = data_convert::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
@ -731,7 +731,7 @@ void fromMsg(const geometry_msgs::TransformStamped& msg, tf3::Stamped<tf3::Trans
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::Point& t_in, robot_geometry_msgs::Point& t_out, const tf3::TransformStampedMsg& transform)
{ {
tf3::Transform t = data_convert::convertTotf3Transform(transform); tf3::Transform t = data_convert::convertTotf3Transform(transform);
@ -749,7 +749,7 @@ inline
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::PointStamped& t_in, robot_geometry_msgs::PointStamped& t_out, const tf3::TransformStampedMsg& transform)
{ {
doTransform(t_in.point, t_out.point, transform); doTransform(t_in.point, t_out.point, transform);
t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.stamp = data_convert::convertTime(transform.header.stamp);
@ -764,7 +764,7 @@ inline
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::Quaternion& t_in, robot_geometry_msgs::Quaternion& t_out, const tf3::TransformStampedMsg& transform)
{ {
tf3::Quaternion t, q_in; tf3::Quaternion t, q_in;
fromMsg(transform.transform.rotation, t); fromMsg(transform.transform.rotation, t);
@ -782,7 +782,7 @@ void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternio
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::QuaternionStamped& t_in, robot_geometry_msgs::QuaternionStamped& t_out, const tf3::TransformStampedMsg& transform)
{ {
doTransform(t_in.quaternion, t_out.quaternion, transform); doTransform(t_in.quaternion, t_out.quaternion, transform);
t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.stamp = data_convert::convertTime(transform.header.stamp);
@ -798,7 +798,7 @@ void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::Qu
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::Pose& t_in, robot_geometry_msgs::Pose& t_out, const tf3::TransformStampedMsg& transform)
{ {
@ -834,7 +834,7 @@ void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, co
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::PoseStamped& t_in, robot_geometry_msgs::PoseStamped& t_out, const tf3::TransformStampedMsg& transform)
{ {
doTransform(t_in.pose, t_out.pose, transform); doTransform(t_in.pose, t_out.pose, transform);
t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.stamp = data_convert::convertTime(transform.header.stamp);
@ -938,7 +938,7 @@ transformCovariance(const boost::array<double, 36>& cov_in, const Transform& tra
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::PoseWithCovarianceStamped& t_in, robot_geometry_msgs::PoseWithCovarianceStamped& t_out, const tf3::TransformStampedMsg& transform)
{ {
tf3::Vector3 v; tf3::Vector3 v;
fromMsg(t_in.pose.pose.position, v); fromMsg(t_in.pose.pose.position, v);
@ -963,7 +963,7 @@ void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::TransformStamped& t_in, robot_geometry_msgs::TransformStamped& t_out, const tf3::TransformStampedMsg& transform)
{ {
tf3::Transform input = data_convert::convertTotf3Transform(t_in); tf3::Transform input = data_convert::convertTotf3Transform(t_in);
@ -984,7 +984,7 @@ void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::Tra
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::Vector3& t_in, robot_geometry_msgs::Vector3& t_out, const tf3::TransformStampedMsg& transform)
{ {
tf3::Transform t = data_convert::convertTotf3Transform(transform); tf3::Transform t = data_convert::convertTotf3Transform(transform);
tf3::Vector3 v_out = t.getBasis() * tf3::Vector3(t_in.x, t_in.y, t_in.z); tf3::Vector3 v_out = t.getBasis() * tf3::Vector3(t_in.x, t_in.y, t_in.z);
@ -1001,7 +1001,7 @@ inline
*/ */
template <> template <>
inline inline
void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::Vector3Stamped& t_in, robot_geometry_msgs::Vector3Stamped& t_out, const tf3::TransformStampedMsg& transform)
{ {
doTransform(t_in.vector, t_out.vector, transform); doTransform(t_in.vector, t_out.vector, transform);
t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.stamp = data_convert::convertTime(transform.header.stamp);
@ -1014,29 +1014,29 @@ inline
/**********************/ /**********************/
template <> template <>
inline inline
const tf3::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return data_convert::convertTime(t.header.stamp);} const tf3::Time& getTimestamp(const robot_geometry_msgs::WrenchStamped& t) {return data_convert::convertTime(t.header.stamp);}
template <> template <>
inline inline
const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;} const std::string& getFrameId(const robot_geometry_msgs::WrenchStamped& t) {return t.header.frame_id;}
inline inline
geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in) robot_geometry_msgs::WrenchStamped toMsg(const robot_geometry_msgs::WrenchStamped& in)
{ {
return in; return in;
} }
inline inline
void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out) void fromMsg(const robot_geometry_msgs::WrenchStamped& msg, robot_geometry_msgs::WrenchStamped& out)
{ {
out = msg; out = msg;
} }
inline inline
geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2>>& in, geometry_msgs::WrenchStamped & out) robot_geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2>>& in, robot_geometry_msgs::WrenchStamped & out)
{ {
out.header.stamp = data_convert::convertTime(in.stamp_); out.header.stamp = data_convert::convertTime(in.stamp_);
out.header.frame_id = in.frame_id_; out.header.frame_id = in.frame_id_;
@ -1047,7 +1047,7 @@ geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2
inline inline
void fromMsg(const geometry_msgs::WrenchStamped& msg, tf3::Stamped<std::array<tf3::Vector3, 2>>& out) void fromMsg(const robot_geometry_msgs::WrenchStamped& msg, tf3::Stamped<std::array<tf3::Vector3, 2>>& out)
{ {
out.stamp_ = data_convert::convertTime(msg.header.stamp); out.stamp_ = data_convert::convertTime(msg.header.stamp);
out.frame_id_ = msg.header.frame_id; out.frame_id_ = msg.header.frame_id;
@ -1063,7 +1063,7 @@ void fromMsg(const geometry_msgs::WrenchStamped& msg, tf3::Stamped<std::array<tf
template<> template<>
inline inline
void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::Wrench& t_in, robot_geometry_msgs::Wrench& t_out, const tf3::TransformStampedMsg& transform)
{ {
doTransform(t_in.force, t_out.force, transform); doTransform(t_in.force, t_out.force, transform);
doTransform(t_in.torque, t_out.torque, transform); doTransform(t_in.torque, t_out.torque, transform);
@ -1079,7 +1079,7 @@ void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out
template<> template<>
inline inline
void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const tf3::TransformStampedMsg& transform) void doTransform(const robot_geometry_msgs::WrenchStamped& t_in, robot_geometry_msgs::WrenchStamped& t_out, const tf3::TransformStampedMsg& transform)
{ {
doTransform(t_in.wrench, t_out.wrench, transform); doTransform(t_in.wrench, t_out.wrench, transform);
t_out.header.stamp = data_convert::convertTime(transform.header.stamp); t_out.header.stamp = data_convert::convertTime(transform.header.stamp);

22
tf3_geometry_msgs/test/test_tf2_geometry_msgs.cpp
View File

@ -43,7 +43,7 @@ static const double EPS = 1e-3;
TEST(TfGeometry, Frame) TEST(TfGeometry, Frame)
{ {
geometry_msgs::PoseStamped v1; robot_geometry_msgs::PoseStamped v1;
v1.pose.position.x = 1; v1.pose.position.x = 1;
v1.pose.position.y = 2; v1.pose.position.y = 2;
v1.pose.position.z = 3; v1.pose.position.z = 3;
@ -55,9 +55,9 @@ TEST(TfGeometry, Frame)
v1.header.frame_id = "A"; v1.header.frame_id = "A";
// simple api // simple api
// geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); // robot_geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
// std::cout << "STEP 2" << std::endl; // std::cout << "STEP 2" << std::endl;
geometry_msgs::PoseStamped v_simple = v1; robot_geometry_msgs::PoseStamped v_simple = v1;
// std::cout << "lookupTransform call:" << std::endl; // std::cout << "lookupTransform call:" << std::endl;
tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform(
@ -86,7 +86,7 @@ TEST(TfGeometry, Frame)
EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
// advanced api // advanced api
// geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), // robot_geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
// "A", ros::Duration(3.0)); // "A", ros::Duration(3.0));
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
@ -95,7 +95,7 @@ TEST(TfGeometry, Frame)
data_convert::convertTime(robot::Time(2.0)) data_convert::convertTime(robot::Time(2.0))
); );
geometry_msgs::PoseStamped v_advanced = v1; // hoặc v1 robot_geometry_msgs::PoseStamped v_advanced = v1; // hoặc v1
tf3::doTransform(v1, v_advanced, tfm1); tf3::doTransform(v1, v_advanced, tfm1);
// std::cout << v_advanced.pose.position.x << " , " // std::cout << v_advanced.pose.position.x << " , "
// << v_advanced.pose.position.y << " , " // << v_advanced.pose.position.y << " , "
@ -116,7 +116,7 @@ TEST(TfGeometry, Frame)
TEST(TfGeometry, PoseWithCovarianceStamped) TEST(TfGeometry, PoseWithCovarianceStamped)
{ {
geometry_msgs::PoseWithCovarianceStamped v1; robot_geometry_msgs::PoseWithCovarianceStamped v1;
v1.pose.pose.position.x = 1; v1.pose.pose.position.x = 1;
v1.pose.pose.position.y = 2; v1.pose.pose.position.y = 2;
v1.pose.pose.position.z = 3; v1.pose.pose.position.z = 3;
@ -138,7 +138,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
// simple api // simple api
geometry_msgs::PoseWithCovarianceStamped v_simple; robot_geometry_msgs::PoseWithCovarianceStamped v_simple;
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
"A", // frame nguồn "A", // frame nguồn
@ -170,7 +170,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS);
// advanced api // advanced api
geometry_msgs::PoseWithCovarianceStamped v_advanced; robot_geometry_msgs::PoseWithCovarianceStamped v_advanced;
tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
"B", // frame đích "B", // frame đích
"A", // frame nguồn "A", // frame nguồn
@ -178,7 +178,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
); );
tf3::doTransform(v1, v_advanced, tfm2); tf3::doTransform(v1, v_advanced, tfm2);
// const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), // const robot_geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
// "A", ros::Duration(3.0)); // "A", ros::Duration(3.0));
std::cout << v_advanced.pose.pose.position.x << " , " std::cout << v_advanced.pose.pose.position.x << " , "
<< v_advanced.pose.pose.position.y << " , " << v_advanced.pose.pose.position.y << " , "
@ -206,7 +206,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
/** now add rotation to transform to test the effect on covariance **/ /** now add rotation to transform to test the effect on covariance **/
// rotate pi/2 radians about x-axis // rotate pi/2 radians about x-axis
geometry_msgs::TransformStamped t_rot; robot_geometry_msgs::TransformStamped t_rot;
tf3::Quaternion q_rot = tf3::Quaternion(tf3::Vector3(1,0,0), M_PI/2); 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.x = q_rot.x();
t_rot.transform.rotation.y = q_rot.y(); t_rot.transform.rotation.y = q_rot.y();
@ -224,7 +224,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
v1.pose.covariance[12] = 1; v1.pose.covariance[12] = 1;
// perform rotation // perform rotation
geometry_msgs::PoseWithCovarianceStamped v_rotated; robot_geometry_msgs::PoseWithCovarianceStamped v_rotated;
tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform( tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform(
"rotated", // frame đích "rotated", // frame đích
"A", // frame nguồn "A", // frame nguồn

90
tf3_geometry_msgs/test/test_tomsg_frommsg.cpp
View File

@ -40,7 +40,7 @@ tf3::Vector3 get_tf3_vector()
return tf3::Vector3(1.0, 2.0, 3.0); return tf3::Vector3(1.0, 2.0, 3.0);
} }
geometry_msgs::Vector3& value_initialize(geometry_msgs::Vector3 &m1) robot_geometry_msgs::Vector3& value_initialize(robot_geometry_msgs::Vector3 &m1)
{ {
m1.x = 1; m1.x = 1;
m1.y = 2; m1.y = 2;
@ -55,14 +55,14 @@ std_msgs::Header& value_initialize(std_msgs::Header & h)
return h; return h;
} }
geometry_msgs::Vector3Stamped& value_initialize(geometry_msgs::Vector3Stamped &m1) robot_geometry_msgs::Vector3Stamped& value_initialize(robot_geometry_msgs::Vector3Stamped &m1)
{ {
value_initialize(m1.header); value_initialize(m1.header);
value_initialize(m1.vector); value_initialize(m1.vector);
return m1; return m1;
} }
geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1) robot_geometry_msgs::Point& value_initialize(robot_geometry_msgs::Point &m1)
{ {
m1.x = 1; m1.x = 1;
m1.y = 2; m1.y = 2;
@ -70,14 +70,14 @@ geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1)
return m1; return m1;
} }
geometry_msgs::PointStamped& value_initialize(geometry_msgs::PointStamped &m1) robot_geometry_msgs::PointStamped& value_initialize(robot_geometry_msgs::PointStamped &m1)
{ {
value_initialize(m1.header); value_initialize(m1.header);
value_initialize(m1.point); value_initialize(m1.point);
return m1; return m1;
} }
geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1) robot_geometry_msgs::Quaternion & value_initialize(robot_geometry_msgs::Quaternion &m1)
{ {
m1.x = 0; m1.x = 0;
m1.y = 0; m1.y = 0;
@ -86,35 +86,35 @@ geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1)
return m1; return m1;
} }
geometry_msgs::QuaternionStamped& value_initialize(geometry_msgs::QuaternionStamped &m1) robot_geometry_msgs::QuaternionStamped& value_initialize(robot_geometry_msgs::QuaternionStamped &m1)
{ {
value_initialize(m1.header); value_initialize(m1.header);
value_initialize(m1.quaternion); value_initialize(m1.quaternion);
return m1; return m1;
} }
geometry_msgs::Pose & value_initialize(geometry_msgs::Pose & m1) robot_geometry_msgs::Pose & value_initialize(robot_geometry_msgs::Pose & m1)
{ {
value_initialize(m1.position); value_initialize(m1.position);
value_initialize(m1.orientation); value_initialize(m1.orientation);
return m1; return m1;
} }
geometry_msgs::PoseStamped& value_initialize(geometry_msgs::PoseStamped &m1) robot_geometry_msgs::PoseStamped& value_initialize(robot_geometry_msgs::PoseStamped &m1)
{ {
value_initialize(m1.header); value_initialize(m1.header);
value_initialize(m1.pose); value_initialize(m1.pose);
return m1; return m1;
} }
geometry_msgs::Transform & value_initialize(geometry_msgs::Transform & m1) robot_geometry_msgs::Transform & value_initialize(robot_geometry_msgs::Transform & m1)
{ {
value_initialize(m1.translation); value_initialize(m1.translation);
value_initialize(m1.rotation); value_initialize(m1.rotation);
return m1; return m1;
} }
geometry_msgs::TransformStamped& value_initialize(geometry_msgs::TransformStamped &m1) robot_geometry_msgs::TransformStamped& value_initialize(robot_geometry_msgs::TransformStamped &m1)
{ {
value_initialize(m1.header); value_initialize(m1.header);
value_initialize(m1.transform); value_initialize(m1.transform);
@ -130,14 +130,14 @@ void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2)
/* /*
* Vector3 * Vector3
*/ */
void expect_near(const geometry_msgs::Vector3 & v1, const tf3::Vector3 & v2) void expect_near(const robot_geometry_msgs::Vector3 & v1, const tf3::Vector3 & v2)
{ {
EXPECT_NEAR(v1.x, v2.x(), EPS); EXPECT_NEAR(v1.x, v2.x(), EPS);
EXPECT_NEAR(v1.y, v2.y(), EPS); EXPECT_NEAR(v1.y, v2.y(), EPS);
EXPECT_NEAR(v1.z, v2.z(), EPS); EXPECT_NEAR(v1.z, v2.z(), EPS);
} }
void expect_near(const geometry_msgs::Vector3 & v1, const geometry_msgs::Vector3 & v2) 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.x, v2.x, EPS);
EXPECT_NEAR(v1.y, v2.y, EPS); EXPECT_NEAR(v1.y, v2.y, EPS);
@ -151,7 +151,7 @@ void expect_near(const tf3::Vector3 & v1, const tf3::Vector3 & v2)
EXPECT_NEAR(v1.z(), v2.z(), EPS); EXPECT_NEAR(v1.z(), v2.z(), EPS);
} }
void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::Vector3Stamped & p2) void expect_near(const robot_geometry_msgs::Vector3Stamped & p1, const robot_geometry_msgs::Vector3Stamped & p2)
{ {
expect_near(p1.header, p2.header); expect_near(p1.header, p2.header);
expect_near(p1.vector, p2.vector); expect_near(p1.vector, p2.vector);
@ -160,21 +160,21 @@ void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::
/* /*
* Point * Point
*/ */
void expect_near(const geometry_msgs::Point & p1, const tf3::Vector3 & v2) void expect_near(const robot_geometry_msgs::Point & p1, const tf3::Vector3 & v2)
{ {
EXPECT_NEAR(p1.x, v2.x(), EPS); EXPECT_NEAR(p1.x, v2.x(), EPS);
EXPECT_NEAR(p1.y, v2.y(), EPS); EXPECT_NEAR(p1.y, v2.y(), EPS);
EXPECT_NEAR(p1.z, v2.z(), EPS); EXPECT_NEAR(p1.z, v2.z(), EPS);
} }
void expect_near(const geometry_msgs::Point & p1, const geometry_msgs::Point & v2) 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.x, v2.x, EPS);
EXPECT_NEAR(p1.y, v2.y, EPS); EXPECT_NEAR(p1.y, v2.y, EPS);
EXPECT_NEAR(p1.z, v2.z, EPS); EXPECT_NEAR(p1.z, v2.z, EPS);
} }
void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::PointStamped & p2) void expect_near(const robot_geometry_msgs::PointStamped & p1, const robot_geometry_msgs::PointStamped & p2)
{ {
expect_near(p1.header, p2.header); expect_near(p1.header, p2.header);
expect_near(p1.point, p2.point); expect_near(p1.point, p2.point);
@ -184,21 +184,21 @@ void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::Po
/* /*
* Quaternion * Quaternion
*/ */
void expect_near(const geometry_msgs::Quaternion & q1, const tf3::Quaternion & v2) void expect_near(const robot_geometry_msgs::Quaternion & q1, const tf3::Quaternion & v2)
{ {
EXPECT_NEAR(q1.x, v2.x(), EPS); EXPECT_NEAR(q1.x, v2.x(), EPS);
EXPECT_NEAR(q1.y, v2.y(), EPS); EXPECT_NEAR(q1.y, v2.y(), EPS);
EXPECT_NEAR(q1.z, v2.z(), EPS); EXPECT_NEAR(q1.z, v2.z(), EPS);
} }
void expect_near(const geometry_msgs::Quaternion & q1, const geometry_msgs::Quaternion & v2) 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.x, v2.x, EPS);
EXPECT_NEAR(q1.y, v2.y, EPS); EXPECT_NEAR(q1.y, v2.y, EPS);
EXPECT_NEAR(q1.z, v2.z, EPS); EXPECT_NEAR(q1.z, v2.z, EPS);
} }
void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msgs::QuaternionStamped & p2) void expect_near(const robot_geometry_msgs::QuaternionStamped & p1, const robot_geometry_msgs::QuaternionStamped & p2)
{ {
expect_near(p1.header, p2.header); expect_near(p1.header, p2.header);
expect_near(p1.quaternion, p2.quaternion); expect_near(p1.quaternion, p2.quaternion);
@ -207,19 +207,19 @@ void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msg
/* /*
* Pose * Pose
*/ */
void expect_near(const geometry_msgs::Pose & p, const tf3::Transform & t) void expect_near(const robot_geometry_msgs::Pose & p, const tf3::Transform & t)
{ {
expect_near(p.position, t.getOrigin()); expect_near(p.position, t.getOrigin());
expect_near(p.orientation, t.getRotation()); expect_near(p.orientation, t.getRotation());
} }
void expect_near(const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2) void expect_near(const robot_geometry_msgs::Pose & p1, const robot_geometry_msgs::Pose & p2)
{ {
expect_near(p1.position, p2.position); expect_near(p1.position, p2.position);
expect_near(p1.orientation, p2.orientation); expect_near(p1.orientation, p2.orientation);
} }
void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::PoseStamped & p2) void expect_near(const robot_geometry_msgs::PoseStamped & p1, const robot_geometry_msgs::PoseStamped & p2)
{ {
expect_near(p1.header, p2.header); expect_near(p1.header, p2.header);
expect_near(p1.pose, p2.pose); expect_near(p1.pose, p2.pose);
@ -228,19 +228,19 @@ void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::Pos
/* /*
* Transform * Transform
*/ */
void expect_near(const geometry_msgs::Transform & p, const tf3::Transform & t) void expect_near(const robot_geometry_msgs::Transform & p, const tf3::Transform & t)
{ {
expect_near(p.translation, t.getOrigin()); expect_near(p.translation, t.getOrigin());
expect_near(p.rotation, t.getRotation()); expect_near(p.rotation, t.getRotation());
} }
void expect_near(const geometry_msgs::Transform & p1, const geometry_msgs::Transform & p2) void expect_near(const robot_geometry_msgs::Transform & p1, const robot_geometry_msgs::Transform & p2)
{ {
expect_near(p1.translation, p2.translation); expect_near(p1.translation, p2.translation);
expect_near(p1.rotation, p2.rotation); expect_near(p1.rotation, p2.rotation);
} }
void expect_near(const geometry_msgs::TransformStamped & p1, const geometry_msgs::TransformStamped & p2) void expect_near(const robot_geometry_msgs::TransformStamped & p1, const robot_geometry_msgs::TransformStamped & p2)
{ {
expect_near(p1.header, p2.header); expect_near(p1.header, p2.header);
expect_near(p1.transform, p2.transform); expect_near(p1.transform, p2.transform);
@ -262,78 +262,78 @@ void expect_near(const tf3::Stamped<T>& s1, const tf3::Stamped<T>& s2)
TEST(tf3_geometry_msgs, Vector3) TEST(tf3_geometry_msgs, Vector3)
{ {
geometry_msgs::Vector3 m1; robot_geometry_msgs::Vector3 m1;
value_initialize(m1); value_initialize(m1);
tf3::Vector3 v1; tf3::Vector3 v1;
fromMsg(m1, v1); fromMsg(m1, v1);
SCOPED_TRACE("m1 v1"); SCOPED_TRACE("m1 v1");
expect_near(m1, v1); expect_near(m1, v1);
geometry_msgs::Vector3 m2 = toMsg(v1); robot_geometry_msgs::Vector3 m2 = toMsg(v1);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
} }
TEST(tf3_geometry_msgs, Point) TEST(tf3_geometry_msgs, Point)
{ {
geometry_msgs::Point m1; robot_geometry_msgs::Point m1;
value_initialize(m1); value_initialize(m1);
tf3::Vector3 v1; tf3::Vector3 v1;
SCOPED_TRACE("m1 v1"); SCOPED_TRACE("m1 v1");
fromMsg(m1, v1); fromMsg(m1, v1);
expect_near(m1, v1); expect_near(m1, v1);
geometry_msgs::Point m2 = toMsg(v1, m2); robot_geometry_msgs::Point m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
} }
TEST(tf3_geometry_msgs, Quaternion) TEST(tf3_geometry_msgs, Quaternion)
{ {
geometry_msgs::Quaternion m1; robot_geometry_msgs::Quaternion m1;
value_initialize(m1); value_initialize(m1);
tf3::Quaternion q1; tf3::Quaternion q1;
SCOPED_TRACE("m1 q1"); SCOPED_TRACE("m1 q1");
fromMsg(m1, q1); fromMsg(m1, q1);
expect_near(m1, q1); expect_near(m1, q1);
geometry_msgs::Quaternion m2 = toMsg(q1); robot_geometry_msgs::Quaternion m2 = toMsg(q1);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
} }
TEST(tf3_geometry_msgs, Pose) TEST(tf3_geometry_msgs, Pose)
{ {
geometry_msgs::Pose m1; robot_geometry_msgs::Pose m1;
value_initialize(m1); value_initialize(m1);
tf3::Transform t1; tf3::Transform t1;
fromMsg(m1, t1); fromMsg(m1, t1);
SCOPED_TRACE("m1 t1"); SCOPED_TRACE("m1 t1");
expect_near(m1, t1); expect_near(m1, t1);
geometry_msgs::Pose m2 = toMsg(t1, m2); robot_geometry_msgs::Pose m2 = toMsg(t1, m2);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
} }
TEST(tf3_geometry_msgs, Transform) TEST(tf3_geometry_msgs, Transform)
{ {
geometry_msgs::Transform m1; robot_geometry_msgs::Transform m1;
value_initialize(m1); value_initialize(m1);
tf3::Transform t1; tf3::Transform t1;
fromMsg(m1, t1); fromMsg(m1, t1);
SCOPED_TRACE("m1 t1"); SCOPED_TRACE("m1 t1");
expect_near(m1, t1); expect_near(m1, t1);
geometry_msgs::Transform m2 = toMsg(t1); robot_geometry_msgs::Transform m2 = toMsg(t1);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
} }
TEST(tf3_geometry_msgs, Vector3Stamped) TEST(tf3_geometry_msgs, Vector3Stamped)
{ {
geometry_msgs::Vector3Stamped m1; robot_geometry_msgs::Vector3Stamped m1;
value_initialize(m1); value_initialize(m1);
tf3::Stamped<tf3::Vector3> v1; tf3::Stamped<tf3::Vector3> v1;
fromMsg(m1, v1); fromMsg(m1, v1);
SCOPED_TRACE("m1 v1"); SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); // expect_near(m1, v1);
geometry_msgs::Vector3Stamped m2; robot_geometry_msgs::Vector3Stamped m2;
m2 = toMsg(v1); m2 = toMsg(v1);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
@ -341,13 +341,13 @@ TEST(tf3_geometry_msgs, Vector3Stamped)
TEST(tf3_geometry_msgs, PointStamped) TEST(tf3_geometry_msgs, PointStamped)
{ {
geometry_msgs::PointStamped m1; robot_geometry_msgs::PointStamped m1;
value_initialize(m1); value_initialize(m1);
tf3::Stamped<tf3::Vector3> v1; tf3::Stamped<tf3::Vector3> v1;
fromMsg(m1, v1); fromMsg(m1, v1);
SCOPED_TRACE("m1 v1"); SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly // expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::PointStamped m2; robot_geometry_msgs::PointStamped m2;
m2 = toMsg(v1, m2); m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
@ -355,13 +355,13 @@ TEST(tf3_geometry_msgs, PointStamped)
TEST(tf3_geometry_msgs, QuaternionStamped) TEST(tf3_geometry_msgs, QuaternionStamped)
{ {
geometry_msgs::QuaternionStamped m1; robot_geometry_msgs::QuaternionStamped m1;
value_initialize(m1); value_initialize(m1);
tf3::Stamped<tf3::Quaternion> v1; tf3::Stamped<tf3::Quaternion> v1;
fromMsg(m1, v1); fromMsg(m1, v1);
SCOPED_TRACE("m1 v1"); SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly // expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::QuaternionStamped m2; robot_geometry_msgs::QuaternionStamped m2;
m2 = tf3::toMsg(v1); m2 = tf3::toMsg(v1);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
@ -369,13 +369,13 @@ TEST(tf3_geometry_msgs, QuaternionStamped)
TEST(tf3_geometry_msgs, PoseStamped) TEST(tf3_geometry_msgs, PoseStamped)
{ {
geometry_msgs::PoseStamped m1; robot_geometry_msgs::PoseStamped m1;
value_initialize(m1); value_initialize(m1);
tf3::Stamped<tf3::Transform> v1; tf3::Stamped<tf3::Transform> v1;
SCOPED_TRACE("m1 v1"); SCOPED_TRACE("m1 v1");
fromMsg(m1, v1); fromMsg(m1, v1);
// expect_near(m1, v1); //TODO implement cross verification explicityly // expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::PoseStamped m2; robot_geometry_msgs::PoseStamped m2;
m2 = tf3::toMsg(v1, m2); m2 = tf3::toMsg(v1, m2);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);
@ -383,13 +383,13 @@ TEST(tf3_geometry_msgs, PoseStamped)
TEST(tf3_geometry_msgs, TransformStamped) TEST(tf3_geometry_msgs, TransformStamped)
{ {
geometry_msgs::TransformStamped m1; robot_geometry_msgs::TransformStamped m1;
value_initialize(m1); value_initialize(m1);
tf3::Stamped<tf3::Transform> v1; tf3::Stamped<tf3::Transform> v1;
fromMsg(m1, v1); fromMsg(m1, v1);
SCOPED_TRACE("m1 v1"); SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); // expect_near(m1, v1);
geometry_msgs::TransformStamped m2; robot_geometry_msgs::TransformStamped m2;
m2 = tf3::toMsg(v1); m2 = tf3::toMsg(v1);
SCOPED_TRACE("m1 m2"); SCOPED_TRACE("m1 m2");
expect_near(m1, m2); expect_near(m1, m2);

16
tf3_sensor_msgs/include/tf3_sensor_msgs/data_convert.h
View File

@ -4,7 +4,7 @@
#include <tf3/time.h> #include <tf3/time.h>
#include <tf3/compat.h> #include <tf3/compat.h>
#include <tf3/LinearMath/Transform.h> #include <tf3/LinearMath/Transform.h>
#include <geometry_msgs/TransformStamped.h> #include <robot_geometry_msgs/TransformStamped.h>
#include <robot/time.h> #include <robot/time.h>
#include <cmath> #include <cmath>
@ -27,15 +27,15 @@ namespace tf3
return time_tmp; return time_tmp;
} }
inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) inline tf3::Quaternion convertQuaternion(const robot_geometry_msgs::Quaternion& q)
{ {
tf3::Quaternion out(q.x,q.y,q.z,q.w); tf3::Quaternion out(q.x,q.y,q.z,q.w);
return out; return out;
} }
inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) inline robot_geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
{ {
return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w()); return robot_geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
} }
inline tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg) inline tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg)
@ -59,7 +59,7 @@ namespace tf3
return out; return out;
} }
inline tf3::Transform convertToTransform(const geometry_msgs::TransformStamped& msg) inline tf3::Transform convertToTransform(const robot_geometry_msgs::TransformStamped& msg)
{ {
tf3::Transform out; tf3::Transform out;
@ -81,7 +81,7 @@ namespace tf3
} }
inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) inline tf3::TransformStampedMsg convertToTransformStampedMsg(const robot_geometry_msgs::TransformStamped& msg)
{ {
tf3::TransformStampedMsg out; tf3::TransformStampedMsg out;
out.header.seq = msg.header.seq; out.header.seq = msg.header.seq;
@ -98,9 +98,9 @@ namespace tf3
return out; return out;
} }
inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) inline robot_geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{ {
geometry_msgs::TransformStamped out; robot_geometry_msgs::TransformStamped out;
out.header.seq = msg.header.seq; out.header.seq = msg.header.seq;
out.header.stamp = convertTime(msg.header.stamp); out.header.stamp = convertTime(msg.header.stamp);
out.header.frame_id = msg.header.frame_id; out.header.frame_id = msg.header.frame_id;

4
tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp
View File

@ -29,7 +29,7 @@
#include <tf3_sensor_msgs/tf3_sensor_msgs.h> #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
@ -97,7 +97,7 @@ int main(int argc, char **argv){
tf_buffer = new tf3::BufferCore(); tf_buffer = new tf3::BufferCore();
// populate buffer // populate buffer
geometry_msgs::TransformStamped t; robot_geometry_msgs::TransformStamped t;
t.transform.translation.x = 10; t.transform.translation.x = 10;
t.transform.translation.y = 20; t.transform.translation.y = 20;
t.transform.translation.z = 30; t.transform.translation.z = 30;