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

first commit

Browse Source
This commit is contained in:
duongtd
2025-11-24 10:18:31 +07:00
commit 0dca929962
146 changed files with 11133 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
tf3_sensor_msgs/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,110 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package tf2_sensor_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.7.6 (2022-10-11)
------------------
0.7.5 (2020-09-01)
------------------
0.7.4 (2020-09-01)
------------------
0.7.3 (2020-08-25)
------------------
* Use list instead of set to make build reproducible (`#473 <https://github.com/ros/geometry2/issues/473>`_)
* Contributors: Jochen Sprickerhof
0.7.2 (2020-06-08)
------------------
0.7.1 (2020-05-13)
------------------
* import setup from setuptools instead of distutils-core (`#449 <https://github.com/ros/geometry2/issues/449>`_)
* Contributors: Alejandro Hernández Cordero
0.7.0 (2020-03-09)
------------------
* Replace kdl packages with rosdep keys (`#447 <https://github.com/ros/geometry2/issues/447>`_)
* Bump CMake version to avoid CMP0048 warning (`#445 <https://github.com/ros/geometry2/issues/445>`_)
* Merge pull request `#378 <https://github.com/ros/geometry2/issues/378>`_ from peci1/tf2_sensor_msgs_isometry
Affine->Isometry
* Python 3 compatibility: relative imports and print statement
* Contributors: Martin Pecka, Shane Loretz, Timon Engelke, Tully Foote
0.6.5 (2018-11-16)
------------------
0.6.4 (2018-11-06)
------------------
0.6.3 (2018-07-09)
------------------
0.6.2 (2018-05-02)
------------------
0.6.1 (2018-03-21)
------------------
0.6.0 (2018-03-21)
------------------
0.5.17 (2018-01-01)
-------------------
* Merge pull request `#257 <https://github.com/ros/geometry2/issues/257>`_ from delftrobotics-forks/python3
Make tf2_py python3 compatible again
* Use python3 print function.
* Contributors: Maarten de Vries, Tully Foote
0.5.16 (2017-07-14)
-------------------
* Fix do_transform_cloud for multi-channelled pointcloud2. (`#241 <https://github.com/ros/geometry2/issues/241>`_)
* store gtest return value as int (`#229 <https://github.com/ros/geometry2/issues/229>`_)
* Document the lifetime of the returned reference for getFrameId and getTimestamp
* Find eigen in a much nicer way.
* Switch tf2_sensor_msgs over to package format 2.
* Contributors: Atsushi Watanabe, Chris Lalancette, dhood
0.5.15 (2017-01-24)
-------------------
0.5.14 (2017-01-16)
-------------------
0.5.13 (2016-03-04)
-------------------
* add missing Python runtime dependency
* fix wrong comment
* Adding tests to package
* Fixing do_transform_cloud for python
The previous code was not used at all (it was a mistake in the __init_\_.py so
the do_transform_cloud was not available to the python users).
The python code need some little correction (e.g there is no method named
read_cloud but it's read_points for instance, and as we are in python we can't
use the same trick as in c++ when we got an immutable)
* Contributors: Laurent GEORGE, Vincent Rabaud
0.5.12 (2015-08-05)
-------------------
0.5.11 (2015-04-22)
-------------------
0.5.10 (2015-04-21)
-------------------
0.5.9 (2015-03-25)
------------------
0.5.8 (2015-03-17)
------------------
* ODR violation fixes and more conversions
* Fix keeping original pointcloud header in transformed pointcloud
* Contributors: Paul Bovbel, Tully Foote, Vincent Rabaud
0.5.7 (2014-12-23)
------------------
* add support for transforming sensor_msgs::PointCloud2
* Contributors: Vincent Rabaud

71
tf3_sensor_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,71 @@
cmake_minimum_required(VERSION 3.10)
project(tf2_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 QUIET)
# Finding Eigen3
find_package(Eigen3 QUIET)
if(NOT EIGEN3_FOUND)
# Fallback: try to find Eigen via cmake_modules or pkg-config
find_package(PkgConfig QUIET)
if(PkgConfig_FOUND)
pkg_check_modules(EIGEN3 QUIET eigen3)
endif()
# If still not found, try common include paths
if(NOT EIGEN3_FOUND)
find_path(EIGEN3_INCLUDE_DIRS
NAMES Eigen/Core
PATHS /usr/include/eigen3 /usr/local/include/eigen3
)
if(EIGEN3_INCLUDE_DIRS)
set(EIGEN3_FOUND TRUE)
endif()
endif()
endif()
# Note: eigen 3.2 (on older Ubuntu) only provides EIGEN3_INCLUDE_DIR,
# not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former.
if(NOT EIGEN3_INCLUDE_DIRS)
set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()
if(NOT EIGEN3_FOUND)
message(FATAL_ERROR "Eigen3 not found. Please install libeigen3-dev")
endif()
# Include directories
include_directories(
include
${EIGEN3_INCLUDE_DIRS}
)
# Install headers
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
)
# Testing
if(GTest_FOUND OR GTEST_FOUND)
enable_testing()
find_package(Threads REQUIRED)
add_executable(test_tf2_sensor_msgs test/test_tf2_sensor_msgs.cpp)
target_include_directories(test_tf2_sensor_msgs PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIRS}
${GTEST_INCLUDE_DIRS}
)
target_link_libraries(test_tf2_sensor_msgs
${GTEST_LIBRARIES}
Threads::Threads
)
add_test(NAME test_tf2_sensor_msgs COMMAND test_tf2_sensor_msgs)
else()
message(STATUS "Google Test not found. Tests will not be built.")
endif()

107
tf3_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h Executable file
View File

@@ -0,0 +1,107 @@
/*
* 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 TF2_SENSOR_MSGS_H
#define TF2_SENSOR_MSGS_H
#include <tf2/convert.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
namespace tf2
{
/********************/
/** PointCloud2 **/
/********************/
/** \brief Extract a timestamp from the header of a PointCloud2 message.
* This function is a specialization of the getTimestamp template defined in tf2/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 ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return 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 tf2/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 sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
// this method needs to be implemented by client library developers
template <>
inline
void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
{
p_out = p_in;
p_out.header = t_in.header;
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);
sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
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
sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
{
return in;
}
inline
void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out)
{
out = msg;
}
} // namespace
#endif // TF2_SENSOR_MSGS_H

31
tf3_sensor_msgs/package.xml Executable file
View File

@@ -0,0 +1,31 @@
<package format="2">
<name>tf2_sensor_msgs</name>
<version>0.7.6</version>
<description>
Small lib to transform sensor_msgs with tf. Most notably, PointCloud2
</description>
<author>Vincent Rabaud</author>
<maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
<license>TODO</license>
<url type="website">http://www.ros.org/wiki/tf2_ros</url>
<buildtool_depend version_gte="0.5.6">catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>eigen</build_depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<exec_depend>python3-pykdl</exec_depend>
<exec_depend>rospy</exec_depend>
<build_export_depend>eigen</build_export_depend>
<test_depend>rostest</test_depend>
<test_depend>geometry_msgs</test_depend>
</package>

3
tf3_sensor_msgs/test/test.launch Executable file
View File

@@ -0,0 +1,3 @@
<launch>
<test test-name="tf2_sensor_msgs" pkg="tf2_sensor_msgs" type="test_tf2_sensor_msgs_cpp" time-limit="120" />
</launch>

104
tf3_sensor_msgs/test/test_tf2_sensor_msgs.cpp Executable file
View File

@@ -0,0 +1,104 @@
/*
* 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 <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_listener.h>
#include <ros/ros.h>
#include <gtest/gtest.h>
#include <tf2_ros/buffer.h>
tf2_ros::Buffer* tf_buffer;
static const double EPS = 1e-3;
TEST(Tf2Sensor, PointCloud2)
{
sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = 1;
*iter_y = 2;
*iter_z = 3;
cloud.header.stamp = ros::Time(2);
cloud.header.frame_id = "A";
// simple api
sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
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
sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
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);
ros::init(argc, argv, "test");
ros::NodeHandle n;
tf_buffer = new tf2_ros::Buffer();
// populate buffer
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 = ros::Time(2.0);
t.header.frame_id = "A";
t.child_frame_id = "B";
tf_buffer->setTransform(t, "test");
int ret = RUN_ALL_TESTS();
delete tf_buffer;
return ret;
}