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

Hiep update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 10:24:40 +07:00
parent e4db1da907
commit 5558086d10
281 changed files with 122 additions and 18222 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
robot_tf3_sensor_msgs/CMakeLists.txt Executable file
View File

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

119
robot_tf3_sensor_msgs/include/robot_tf3_sensor_msgs/data_convert.h Normal file
View File

@@ -0,0 +1,119 @@
#ifndef DATA_CONVERT_ROBOT_TF3_SENSOR_MSGS_H
#define DATA_CONVERT_ROBOT_TF3_SENSOR_MSGS_H
#include <tf3/time.h>
#include <tf3/compat.h>
#include <tf3/LinearMath/Transform.h>
#include <robot_geometry_msgs/TransformStamped.h>
#include <robot/time.h>
#include <cmath>
namespace tf3
{
inline robot::Time convertTime(const tf3::Time& time)
{
robot::Time time_tmp;
time_tmp.sec = time.sec;
time_tmp.nsec = time.nsec;
return time_tmp;
}
inline tf3::Time convertTime(const robot::Time& time)
{
tf3::Time time_tmp;
time_tmp.sec = time.sec;
time_tmp.nsec = time.nsec;
return time_tmp;
}
inline tf3::Quaternion convertQuaternion(const robot_geometry_msgs::Quaternion& q)
{
tf3::Quaternion out(q.x,q.y,q.z,q.w);
return out;
}
inline robot_geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
{
return robot_geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
}
inline tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg)
{
tf3::Transform out;
out.setOrigin(tf3::Vector3(
msg.transform.translation.x,
msg.transform.translation.y,
msg.transform.translation.z
));
tf3::Quaternion q(
msg.transform.rotation.x,
msg.transform.rotation.y,
msg.transform.rotation.z,
msg.transform.rotation.w
);
out.setBasis(tf3::Matrix3x3(q));
return out;
}
inline tf3::Transform convertToTransform(const robot_geometry_msgs::TransformStamped& msg)
{
tf3::Transform out;
out.setOrigin(tf3::Vector3(
msg.transform.translation.x,
msg.transform.translation.y,
msg.transform.translation.z
));
tf3::Quaternion q(
msg.transform.rotation.x,
msg.transform.rotation.y,
msg.transform.rotation.z,
msg.transform.rotation.w
);
out.setBasis(tf3::Matrix3x3(q));
return out;
}
inline tf3::TransformStampedMsg convertToTransformStampedMsg(const robot_geometry_msgs::TransformStamped& msg)
{
tf3::TransformStampedMsg out;
out.header.seq = msg.header.seq;
out.header.stamp = convertTime(msg.header.stamp);
out.header.frame_id = msg.header.frame_id;
out.child_frame_id = msg.child_frame_id;
out.transform.translation.x = msg.transform.translation.x;
out.transform.translation.y = msg.transform.translation.y;
out.transform.translation.z = msg.transform.translation.z;
out.transform.rotation.x = msg.transform.rotation.x;
out.transform.rotation.y = msg.transform.rotation.y;
out.transform.rotation.z = msg.transform.rotation.z;
out.transform.rotation.w = msg.transform.rotation.w;
return out;
}
inline robot_geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{
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;
out.child_frame_id = msg.child_frame_id;
out.transform.translation.x = msg.transform.translation.x;
out.transform.translation.y = msg.transform.translation.y;
out.transform.translation.z = msg.transform.translation.z;
out.transform.rotation.x = msg.transform.rotation.x;
out.transform.rotation.y = msg.transform.rotation.y;
out.transform.rotation.z = msg.transform.rotation.z;
out.transform.rotation.w = msg.transform.rotation.w;
return out;
}
}
#endif // DATA_CONVERT_ROBOT_TF3_SENSOR_MSGS_H

111
robot_tf3_sensor_msgs/include/robot_tf3_sensor_msgs/tf3_sensor_msgs.h Executable file
View File

@@ -0,0 +1,111 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROBOT_TF3_SENSOR_MSGS_H
#define ROBOT_TF3_SENSOR_MSGS_H
#include <tf3/convert.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <data_convert/data_convert.h>
namespace tf3
{
/********************/
/** PointCloud2 **/
/********************/
/** \brief Extract a timestamp from the header of a PointCloud2 message.
* This function is a specialization of the getTimestamp template defined in tf3/convert.h.
* \param t PointCloud2 message to extract the timestamp from.
* \return The timestamp of the message. The lifetime of the returned reference
* is bound to the lifetime of the argument.
*/
template <>
inline
const tf3::Time& getTimestamp(const robot_sensor_msgs::PointCloud2& p) {return data_convert::convertTime(p.header.stamp);}
/** \brief Extract a frame ID from the header of a PointCloud2 message.
* This function is a specialization of the getFrameId template defined in tf3/convert.h.
* \param t PointCloud2 message to extract the frame ID from.
* \return A string containing the frame ID of the message. The lifetime of the
* returned reference is bound to the lifetime of the argument.
*/
template <>
inline
const std::string& getFrameId(const robot_sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
// this method needs to be implemented by client library developers
template <>
inline
void doTransform(const robot_sensor_msgs::PointCloud2 &p_in, robot_sensor_msgs::PointCloud2 &p_out, const tf3::TransformStampedMsg& t_in)
{
p_out = p_in;
p_out.header.seq = p_in.header.seq;
p_out.header.stamp = data_convert::convertTime(t_in.header.stamp);
p_out.header.frame_id = t_in.header.frame_id;
Eigen::Transform<float,3,Eigen::Isometry> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
t_in.transform.translation.z) * Eigen::Quaternion<float>(
t_in.transform.rotation.w, t_in.transform.rotation.x,
t_in.transform.rotation.y, t_in.transform.rotation.z);
robot_sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
robot_sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
robot_sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
robot_sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
robot_sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
Eigen::Vector3f point;
for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
*x_out = point.x();
*y_out = point.y();
*z_out = point.z();
}
}
inline
robot_sensor_msgs::PointCloud2 toMsg(const robot_sensor_msgs::PointCloud2 &in)
{
return in;
}
inline
void fromMsg(const robot_sensor_msgs::PointCloud2 &msg, robot_sensor_msgs::PointCloud2 &out)
{
out = msg;
}
} // namespace
#endif // ROBOT_TF3_SENSOR_MSGS_H

26
robot_tf3_sensor_msgs/package.xml Normal file
View File

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

117
robot_tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp Executable file
View File

@@ -0,0 +1,117 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>
tf3::BufferCore* tf_buffer;
static const double EPS = 1e-3;
TEST(Tf2Sensor, PointCloud2)
{
robot_sensor_msgs::PointCloud2 cloud;
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1);
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = 1;
*iter_y = 2;
*iter_z = 3;
cloud.header.stamp = robot::Time(2);
cloud.header.frame_id = "A";
// simple api
tf3::TransformStampedMsg tfm1 = tf_buffer->lookupTransform(
"B", // frame đích
cloud.header.frame_id, // frame nguồn
data_convert::convertTime(robot::Time(2.0))
);
robot_sensor_msgs::PointCloud2 cloud_simple; // hoặc v1
tf3::doTransform(cloud, cloud_simple, tfm1);
// robot_sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
robot_sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
EXPECT_NEAR(*iter_x_after, -9, EPS);
EXPECT_NEAR(*iter_y_after, 18, EPS);
EXPECT_NEAR(*iter_z_after, 27, EPS);
// advanced api
tf3::TransformStampedMsg tfm2 = tf_buffer->lookupTransform(
"B", // frame đích
"A", // frame nguồn
data_convert::convertTime(robot::Time(2.0))
);
robot_sensor_msgs::PointCloud2 cloud_advanced; // hoặc v1
tf3::doTransform(cloud, cloud_advanced, tfm2);
// robot_sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
// "A", ros::Duration(3.0));
robot_sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
robot_sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
robot_sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
tf_buffer = new tf3::BufferCore();
// populate buffer
robot_geometry_msgs::TransformStamped t;
t.transform.translation.x = 10;
t.transform.translation.y = 20;
t.transform.translation.z = 30;
t.transform.rotation.x = 1;
t.transform.rotation.y = 0;
t.transform.rotation.z = 0;
t.transform.rotation.w = 0;
t.header.stamp = robot::Time(2.0);
t.header.frame_id = "A";
t.child_frame_id = "B";
tf3::TransformStampedMsg t_msg = data_convert::convertToTransformStampedMsg(t);
tf_buffer->setTransform(t_msg, "test");
int ret = RUN_ALL_TESTS();
delete tf_buffer;
return ret;
}