HiepLM update
This commit is contained in:
parent
3c51b228ec
commit
2987c1a481
|
|
@ -49,7 +49,7 @@ install(TARGETS tf3_geometry_msgs
|
|||
# --- 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 ---
|
||||
# --- 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
|
||||
FILE tf3_geometry_msgs-targets.cmake
|
||||
DESTINATION lib/cmake/tf3_geometry_msgs
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ std::cout << "doTransform Pose: " << std::endl;
|
|||
//Backwards compatibility remove when forked for Lunar or newer
|
||||
template <>
|
||||
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
|
||||
}
|
||||
|
|
@ -60,8 +60,8 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
|
|||
**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í dụ:**
|
||||
- Dòng 81: `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 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
|
||||
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ Pose_out = Transform * Pose_in
|
|||
|
||||
### Đầ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
|
||||
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)
|
||||
|
|
@ -57,7 +57,7 @@ Pose_out = Transform * Pose_in
|
|||
|
||||
### Đầu Ra
|
||||
|
||||
**`t_out`** (geometry_msgs::Pose): Pose trong frame đích
|
||||
**`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
|
||||
|
|
@ -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).
|
||||
|
||||
#### 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
|
||||
toMsg(v_out, t_out);
|
||||
|
|
|
|||
|
|
@ -36,15 +36,15 @@
|
|||
#include <tf3/LinearMath/Quaternion.h>
|
||||
#include <tf3/LinearMath/Transform.h>
|
||||
#include <data_convert/data_convert.h>
|
||||
#include <geometry_msgs/PointStamped.h>
|
||||
#include <geometry_msgs/QuaternionStamped.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <geometry_msgs/Vector3Stamped.h>
|
||||
#include <geometry_msgs/Pose.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h>
|
||||
#include <geometry_msgs/Wrench.h>
|
||||
#include <geometry_msgs/WrenchStamped.h>
|
||||
#include <robot_geometry_msgs/PointStamped.h>
|
||||
#include <robot_geometry_msgs/QuaternionStamped.h>
|
||||
#include <robot_geometry_msgs/TransformStamped.h>
|
||||
#include <robot_geometry_msgs/Vector3Stamped.h>
|
||||
#include <robot_geometry_msgs/Pose.h>
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/PoseWithCovarianceStamped.h>
|
||||
#include <robot_geometry_msgs/Wrench.h>
|
||||
#include <robot_geometry_msgs/WrenchStamped.h>
|
||||
#include <kdl/frames.hpp>
|
||||
|
||||
#include <array>
|
||||
|
|
@ -58,9 +58,9 @@ namespace tf3
|
|||
* \deprecated
|
||||
*/
|
||||
inline
|
||||
ROS_DEPRECATED KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t);
|
||||
ROS_DEPRECATED KDL::Frame gmTransformToKDL(const robot_geometry_msgs::TransformStamped& t);
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
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.y = in.getY();
|
||||
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.
|
||||
*/
|
||||
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);
|
||||
}
|
||||
|
|
@ -111,7 +111,7 @@ void fromMsg(const geometry_msgs::Vector3& in, tf3::Vector3& out)
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* This function is a specialization of the getFrameId template defined in tf3/convert.h.
|
||||
|
|
@ -121,7 +121,7 @@ inline
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
|
|
@ -130,7 +130,7 @@ inline
|
|||
* \return The input argument.
|
||||
*/
|
||||
inline
|
||||
geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
|
||||
robot_geometry_msgs::Vector3Stamped toMsg(const robot_geometry_msgs::Vector3Stamped& in)
|
||||
{
|
||||
return in;
|
||||
}
|
||||
|
|
@ -141,7 +141,7 @@ geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
|
|||
* \param out The input argument.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
|
@ -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.
|
||||
*/
|
||||
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.frame_id = in.frame_id_;
|
||||
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.
|
||||
*/
|
||||
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.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.
|
||||
*/
|
||||
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.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.
|
||||
*/
|
||||
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);
|
||||
}
|
||||
|
|
@ -219,7 +219,7 @@ void fromMsg(const geometry_msgs::Point& in, tf3::Vector3& out)
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* This function is a specialization of the getFrameId template defined in tf3/convert.h.
|
||||
|
|
@ -229,7 +229,7 @@ inline
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* This function is a specialization of the toMsg template defined in tf3/convert.h.
|
||||
|
|
@ -237,7 +237,7 @@ inline
|
|||
* \return The input argument.
|
||||
*/
|
||||
inline
|
||||
geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
|
||||
robot_geometry_msgs::PointStamped toMsg(const robot_geometry_msgs::PointStamped& in)
|
||||
{
|
||||
return in;
|
||||
}
|
||||
|
|
@ -248,7 +248,7 @@ geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
|
|||
* \param out The input argument.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
|
@ -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.
|
||||
*/
|
||||
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.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.
|
||||
*/
|
||||
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.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.
|
||||
*/
|
||||
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.x = in.getX();
|
||||
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.
|
||||
*/
|
||||
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
|
||||
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 <>
|
||||
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.
|
||||
* 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 <>
|
||||
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.
|
||||
* 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.
|
||||
*/
|
||||
inline
|
||||
geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in)
|
||||
robot_geometry_msgs::QuaternionStamped toMsg(const robot_geometry_msgs::QuaternionStamped& in)
|
||||
{
|
||||
return in;
|
||||
}
|
||||
|
|
@ -357,7 +357,7 @@ geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& i
|
|||
* \param out The input argument.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
|
@ -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.
|
||||
*/
|
||||
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.frame_id = in.frame_id_;
|
||||
out.quaternion.w = in.getW();
|
||||
|
|
@ -382,13 +382,13 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
|
|||
|
||||
// template <>
|
||||
// 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
|
||||
template <>
|
||||
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);
|
||||
}
|
||||
|
|
@ -399,7 +399,7 @@ geometry_msgs::QuaternionStamped toMsg(const tf3::Stamped<tf3::Quaternion>& in)
|
|||
* \param out The QuaternionStamped converted to the equivalent tf3 type.
|
||||
*/
|
||||
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.frame_id_ = in.header.frame_id;
|
||||
|
|
@ -410,12 +410,12 @@ void fromMsg(const geometry_msgs::QuaternionStamped& in, tf3::Stamped<tf3::Quate
|
|||
|
||||
// template<>
|
||||
// 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
|
||||
template<>
|
||||
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);
|
||||
}
|
||||
|
|
@ -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.
|
||||
*/
|
||||
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);
|
||||
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.
|
||||
*/
|
||||
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));
|
||||
// w at the end in the constructor
|
||||
|
|
@ -461,7 +461,7 @@ void fromMsg(const geometry_msgs::Pose& in, tf3::Transform& out)
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* This function is a specialization of the getFrameId template defined in tf3/convert.h.
|
||||
|
|
@ -470,7 +470,7 @@ inline
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* This function is a specialization of the toMsg template defined in tf3/convert.h.
|
||||
|
|
@ -478,7 +478,7 @@ inline
|
|||
* \return The input argument.
|
||||
*/
|
||||
inline
|
||||
geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
|
||||
robot_geometry_msgs::PoseStamped toMsg(const robot_geometry_msgs::PoseStamped& in)
|
||||
{
|
||||
return in;
|
||||
}
|
||||
|
|
@ -489,7 +489,7 @@ geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
|
|||
* \param out The input argument.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
|
@ -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.
|
||||
*/
|
||||
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.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.
|
||||
*/
|
||||
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.frame_id_ = msg.header.frame_id;
|
||||
|
|
@ -535,7 +535,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Transform>
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* This function is a specialization of the getFrameId template defined in tf3/convert.h.
|
||||
|
|
@ -544,7 +544,7 @@ inline
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* This function is a specialization of the toMsg template defined in tf3/convert.h.
|
||||
|
|
@ -552,7 +552,7 @@ inline
|
|||
* \return The input argument.
|
||||
*/
|
||||
inline
|
||||
geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in)
|
||||
robot_geometry_msgs::PoseWithCovarianceStamped toMsg(const robot_geometry_msgs::PoseWithCovarianceStamped& in)
|
||||
{
|
||||
return in;
|
||||
}
|
||||
|
|
@ -563,7 +563,7 @@ geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCova
|
|||
* \param out The input argument.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
|
@ -574,7 +574,7 @@ void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs:
|
|||
* \return The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type.
|
||||
*/
|
||||
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.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.
|
||||
*/
|
||||
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.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.
|
||||
*/
|
||||
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.rotation = toMsg(in.getRotation());
|
||||
return out;
|
||||
|
|
@ -622,7 +622,7 @@ geometry_msgs::Transform toMsg(const tf3::Transform& in)
|
|||
* \param out The Transform converted to a tf3 type.
|
||||
*/
|
||||
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;
|
||||
fromMsg(in.translation, v);
|
||||
|
|
@ -659,7 +659,7 @@ void fromMsg(const tf3::TransformMsg& in, tf3::Transform& out)
|
|||
*/
|
||||
template <>
|
||||
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.
|
||||
* 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 <>
|
||||
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.
|
||||
* 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.
|
||||
*/
|
||||
inline
|
||||
geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in)
|
||||
robot_geometry_msgs::TransformStamped toMsg(const robot_geometry_msgs::TransformStamped& in)
|
||||
{
|
||||
return in;
|
||||
}
|
||||
|
|
@ -686,7 +686,7 @@ geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in)
|
|||
* \param out The input argument.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
|
@ -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.
|
||||
*/
|
||||
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.frame_id = in.frame_id_;
|
||||
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.
|
||||
*/
|
||||
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.frame_id_ = msg.header.frame_id;
|
||||
|
|
@ -731,7 +731,7 @@ void fromMsg(const geometry_msgs::TransformStamped& msg, tf3::Stamped<tf3::Trans
|
|||
*/
|
||||
template <>
|
||||
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);
|
||||
|
|
@ -749,7 +749,7 @@ inline
|
|||
*/
|
||||
template <>
|
||||
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);
|
||||
t_out.header.stamp = data_convert::convertTime(transform.header.stamp);
|
||||
|
|
@ -764,7 +764,7 @@ inline
|
|||
*/
|
||||
template <>
|
||||
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;
|
||||
fromMsg(transform.transform.rotation, t);
|
||||
|
|
@ -782,7 +782,7 @@ void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternio
|
|||
*/
|
||||
template <>
|
||||
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);
|
||||
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 <>
|
||||
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 <>
|
||||
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);
|
||||
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 <>
|
||||
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;
|
||||
fromMsg(t_in.pose.pose.position, v);
|
||||
|
|
@ -963,7 +963,7 @@ void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_
|
|||
*/
|
||||
template <>
|
||||
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);
|
||||
|
||||
|
|
@ -984,7 +984,7 @@ void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::Tra
|
|||
*/
|
||||
template <>
|
||||
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::Vector3 v_out = t.getBasis() * tf3::Vector3(t_in.x, t_in.y, t_in.z);
|
||||
|
|
@ -1001,7 +1001,7 @@ inline
|
|||
*/
|
||||
template <>
|
||||
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);
|
||||
t_out.header.stamp = data_convert::convertTime(transform.header.stamp);
|
||||
|
|
@ -1014,29 +1014,29 @@ inline
|
|||
/**********************/
|
||||
template <>
|
||||
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 <>
|
||||
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
|
||||
geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in)
|
||||
robot_geometry_msgs::WrenchStamped toMsg(const robot_geometry_msgs::WrenchStamped& in)
|
||||
{
|
||||
return in;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
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.frame_id = in.frame_id_;
|
||||
|
|
@ -1047,7 +1047,7 @@ geometry_msgs::WrenchStamped toMsg(const tf3::Stamped<std::array<tf3::Vector3, 2
|
|||
|
||||
|
||||
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.frame_id_ = msg.header.frame_id;
|
||||
|
|
@ -1063,7 +1063,7 @@ void fromMsg(const geometry_msgs::WrenchStamped& msg, tf3::Stamped<std::array<tf
|
|||
|
||||
template<>
|
||||
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.torque, t_out.torque, transform);
|
||||
|
|
@ -1079,7 +1079,7 @@ void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out
|
|||
|
||||
template<>
|
||||
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);
|
||||
t_out.header.stamp = data_convert::convertTime(transform.header.stamp);
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ static const double EPS = 1e-3;
|
|||
|
||||
TEST(TfGeometry, Frame)
|
||||
{
|
||||
geometry_msgs::PoseStamped v1;
|
||||
robot_geometry_msgs::PoseStamped v1;
|
||||
v1.pose.position.x = 1;
|
||||
v1.pose.position.y = 2;
|
||||
v1.pose.position.z = 3;
|
||||
|
|
@ -55,9 +55,9 @@ TEST(TfGeometry, Frame)
|
|||
v1.header.frame_id = "A";
|
||||
|
||||
// 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;
|
||||
geometry_msgs::PoseStamped v_simple = v1;
|
||||
robot_geometry_msgs::PoseStamped v_simple = v1;
|
||||
// std::cout << "lookupTransform call:" << std::endl;
|
||||
|
||||
tf3::TransformStampedMsg tfm = tf_buffer->lookupTransform(
|
||||
|
|
@ -86,7 +86,7 @@ TEST(TfGeometry, Frame)
|
|||
EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
|
||||
|
||||
// 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));
|
||||
|
||||
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
|
||||
|
|
@ -95,7 +95,7 @@ TEST(TfGeometry, Frame)
|
|||
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);
|
||||
// std::cout << v_advanced.pose.position.x << " , "
|
||||
// << v_advanced.pose.position.y << " , "
|
||||
|
|
@ -116,7 +116,7 @@ TEST(TfGeometry, Frame)
|
|||
|
||||
TEST(TfGeometry, PoseWithCovarianceStamped)
|
||||
{
|
||||
geometry_msgs::PoseWithCovarianceStamped v1;
|
||||
robot_geometry_msgs::PoseWithCovarianceStamped v1;
|
||||
v1.pose.pose.position.x = 1;
|
||||
v1.pose.pose.position.y = 2;
|
||||
v1.pose.pose.position.z = 3;
|
||||
|
|
@ -138,7 +138,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
|
|||
|
||||
|
||||
// simple api
|
||||
geometry_msgs::PoseWithCovarianceStamped v_simple;
|
||||
robot_geometry_msgs::PoseWithCovarianceStamped v_simple;
|
||||
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
|
||||
"B", // frame đích
|
||||
"A", // frame nguồn
|
||||
|
|
@ -170,7 +170,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
|
|||
EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS);
|
||||
|
||||
// advanced api
|
||||
geometry_msgs::PoseWithCovarianceStamped v_advanced;
|
||||
robot_geometry_msgs::PoseWithCovarianceStamped v_advanced;
|
||||
tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
|
||||
"B", // frame đích
|
||||
"A", // frame nguồn
|
||||
|
|
@ -178,7 +178,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
|
|||
);
|
||||
|
||||
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));
|
||||
std::cout << v_advanced.pose.pose.position.x << " , "
|
||||
<< v_advanced.pose.pose.position.y << " , "
|
||||
|
|
@ -206,7 +206,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
|
|||
/** now add rotation to transform to test the effect on covariance **/
|
||||
|
||||
// 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);
|
||||
t_rot.transform.rotation.x = q_rot.x();
|
||||
t_rot.transform.rotation.y = q_rot.y();
|
||||
|
|
@ -224,7 +224,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped)
|
|||
v1.pose.covariance[12] = 1;
|
||||
|
||||
// perform rotation
|
||||
geometry_msgs::PoseWithCovarianceStamped v_rotated;
|
||||
robot_geometry_msgs::PoseWithCovarianceStamped v_rotated;
|
||||
tf3::TransformStampedMsg tfm3 = tf_buffer->lookupTransform(
|
||||
"rotated", // frame đích
|
||||
"A", // frame nguồn
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ tf3::Vector3 get_tf3_vector()
|
|||
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.y = 2;
|
||||
|
|
@ -55,14 +55,14 @@ std_msgs::Header& value_initialize(std_msgs::Header & 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.vector);
|
||||
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.y = 2;
|
||||
|
|
@ -70,14 +70,14 @@ geometry_msgs::Point& value_initialize(geometry_msgs::Point &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.point);
|
||||
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.y = 0;
|
||||
|
|
@ -86,35 +86,35 @@ geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &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.quaternion);
|
||||
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.orientation);
|
||||
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.pose);
|
||||
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.rotation);
|
||||
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.transform);
|
||||
|
|
@ -130,14 +130,14 @@ void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2)
|
|||
/*
|
||||
* 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.y, v2.y(), 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.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);
|
||||
}
|
||||
|
||||
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.vector, p2.vector);
|
||||
|
|
@ -160,21 +160,21 @@ void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::
|
|||
/*
|
||||
* 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.y, v2.y(), 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.y, v2.y, 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.point, p2.point);
|
||||
|
|
@ -184,21 +184,21 @@ void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::Po
|
|||
/*
|
||||
* 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.y, v2.y(), 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.y, v2.y, 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.quaternion, p2.quaternion);
|
||||
|
|
@ -207,19 +207,19 @@ void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msg
|
|||
/*
|
||||
* 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.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.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.pose, p2.pose);
|
||||
|
|
@ -228,19 +228,19 @@ void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::Pos
|
|||
/*
|
||||
* 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.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.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.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)
|
||||
{
|
||||
geometry_msgs::Vector3 m1;
|
||||
robot_geometry_msgs::Vector3 m1;
|
||||
value_initialize(m1);
|
||||
tf3::Vector3 v1;
|
||||
fromMsg(m1, v1);
|
||||
SCOPED_TRACE("m1 v1");
|
||||
expect_near(m1, v1);
|
||||
geometry_msgs::Vector3 m2 = toMsg(v1);
|
||||
robot_geometry_msgs::Vector3 m2 = toMsg(v1);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
}
|
||||
|
||||
TEST(tf3_geometry_msgs, Point)
|
||||
{
|
||||
geometry_msgs::Point m1;
|
||||
robot_geometry_msgs::Point m1;
|
||||
value_initialize(m1);
|
||||
tf3::Vector3 v1;
|
||||
SCOPED_TRACE("m1 v1");
|
||||
fromMsg(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");
|
||||
expect_near(m1, m2);
|
||||
}
|
||||
|
||||
TEST(tf3_geometry_msgs, Quaternion)
|
||||
{
|
||||
geometry_msgs::Quaternion m1;
|
||||
robot_geometry_msgs::Quaternion m1;
|
||||
value_initialize(m1);
|
||||
tf3::Quaternion q1;
|
||||
SCOPED_TRACE("m1 q1");
|
||||
fromMsg(m1, q1);
|
||||
expect_near(m1, q1);
|
||||
geometry_msgs::Quaternion m2 = toMsg(q1);
|
||||
robot_geometry_msgs::Quaternion m2 = toMsg(q1);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
}
|
||||
|
||||
TEST(tf3_geometry_msgs, Pose)
|
||||
{
|
||||
geometry_msgs::Pose m1;
|
||||
robot_geometry_msgs::Pose m1;
|
||||
value_initialize(m1);
|
||||
tf3::Transform t1;
|
||||
fromMsg(m1, t1);
|
||||
SCOPED_TRACE("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");
|
||||
expect_near(m1, m2);
|
||||
}
|
||||
|
||||
TEST(tf3_geometry_msgs, Transform)
|
||||
{
|
||||
geometry_msgs::Transform m1;
|
||||
robot_geometry_msgs::Transform m1;
|
||||
value_initialize(m1);
|
||||
tf3::Transform t1;
|
||||
fromMsg(m1, t1);
|
||||
SCOPED_TRACE("m1 t1");
|
||||
expect_near(m1, t1);
|
||||
geometry_msgs::Transform m2 = toMsg(t1);
|
||||
robot_geometry_msgs::Transform m2 = toMsg(t1);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
}
|
||||
|
||||
TEST(tf3_geometry_msgs, Vector3Stamped)
|
||||
{
|
||||
geometry_msgs::Vector3Stamped m1;
|
||||
robot_geometry_msgs::Vector3Stamped m1;
|
||||
value_initialize(m1);
|
||||
tf3::Stamped<tf3::Vector3> v1;
|
||||
fromMsg(m1, v1);
|
||||
SCOPED_TRACE("m1 v1");
|
||||
// expect_near(m1, v1);
|
||||
geometry_msgs::Vector3Stamped m2;
|
||||
robot_geometry_msgs::Vector3Stamped m2;
|
||||
m2 = toMsg(v1);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
|
|
@ -341,13 +341,13 @@ TEST(tf3_geometry_msgs, Vector3Stamped)
|
|||
|
||||
TEST(tf3_geometry_msgs, PointStamped)
|
||||
{
|
||||
geometry_msgs::PointStamped m1;
|
||||
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
|
||||
geometry_msgs::PointStamped m2;
|
||||
robot_geometry_msgs::PointStamped m2;
|
||||
m2 = toMsg(v1, m2);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
|
|
@ -355,13 +355,13 @@ TEST(tf3_geometry_msgs, PointStamped)
|
|||
|
||||
TEST(tf3_geometry_msgs, QuaternionStamped)
|
||||
{
|
||||
geometry_msgs::QuaternionStamped m1;
|
||||
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
|
||||
geometry_msgs::QuaternionStamped m2;
|
||||
robot_geometry_msgs::QuaternionStamped m2;
|
||||
m2 = tf3::toMsg(v1);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
|
|
@ -369,13 +369,13 @@ TEST(tf3_geometry_msgs, QuaternionStamped)
|
|||
|
||||
TEST(tf3_geometry_msgs, PoseStamped)
|
||||
{
|
||||
geometry_msgs::PoseStamped m1;
|
||||
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
|
||||
geometry_msgs::PoseStamped m2;
|
||||
robot_geometry_msgs::PoseStamped m2;
|
||||
m2 = tf3::toMsg(v1, m2);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
|
|
@ -383,13 +383,13 @@ TEST(tf3_geometry_msgs, PoseStamped)
|
|||
|
||||
TEST(tf3_geometry_msgs, TransformStamped)
|
||||
{
|
||||
geometry_msgs::TransformStamped m1;
|
||||
robot_geometry_msgs::TransformStamped m1;
|
||||
value_initialize(m1);
|
||||
tf3::Stamped<tf3::Transform> v1;
|
||||
fromMsg(m1, v1);
|
||||
SCOPED_TRACE("m1 v1");
|
||||
// expect_near(m1, v1);
|
||||
geometry_msgs::TransformStamped m2;
|
||||
robot_geometry_msgs::TransformStamped m2;
|
||||
m2 = tf3::toMsg(v1);
|
||||
SCOPED_TRACE("m1 m2");
|
||||
expect_near(m1, m2);
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
#include <tf3/time.h>
|
||||
#include <tf3/compat.h>
|
||||
#include <tf3/LinearMath/Transform.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <robot_geometry_msgs/TransformStamped.h>
|
||||
#include <robot/time.h>
|
||||
#include <cmath>
|
||||
|
||||
|
|
@ -27,15 +27,15 @@ namespace tf3
|
|||
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);
|
||||
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)
|
||||
|
|
@ -59,7 +59,7 @@ namespace tf3
|
|||
return out;
|
||||
}
|
||||
|
||||
inline tf3::Transform convertToTransform(const geometry_msgs::TransformStamped& msg)
|
||||
inline tf3::Transform convertToTransform(const robot_geometry_msgs::TransformStamped& msg)
|
||||
{
|
||||
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;
|
||||
out.header.seq = msg.header.seq;
|
||||
|
|
@ -98,9 +98,9 @@ namespace tf3
|
|||
|
||||
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.stamp = convertTime(msg.header.stamp);
|
||||
out.header.frame_id = msg.header.frame_id;
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@
|
|||
|
||||
|
||||
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
|
||||
|
|
@ -97,7 +97,7 @@ int main(int argc, char **argv){
|
|||
tf_buffer = new tf3::BufferCore();
|
||||
|
||||
// populate buffer
|
||||
geometry_msgs::TransformStamped t;
|
||||
robot_geometry_msgs::TransformStamped t;
|
||||
t.transform.translation.x = 10;
|
||||
t.transform.translation.y = 20;
|
||||
t.transform.translation.z = 30;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user