HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
navigations/geometry2/tf2_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

56
navigations/geometry2/tf2_sensor_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,56 @@
cmake_minimum_required(VERSION 3.0.2)
project(tf2_sensor_msgs)
find_package(catkin REQUIRED COMPONENTS cmake_modules sensor_msgs tf2_ros tf2)
find_package(Boost COMPONENTS thread REQUIRED)
# Finding Eigen is somewhat complicated because of our need to support Ubuntu
# all the way back to saucy. First we look for the Eigen3 cmake module
# provided by the libeigen3-dev on newer Ubuntu. If that fails, then we
# fall-back to the version provided by cmake_modules, which is a stand-in.
find_package(Eigen3 QUIET)
if(NOT EIGEN3_FOUND)
find_package(cmake_modules REQUIRED)
find_package(Eigen REQUIRED)
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
endif()
# Note that eigen 3.2 (on Ubuntu Wily) 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()
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS sensor_msgs tf2_ros tf2
DEPENDS EIGEN3
)
include_directories(include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
catkin_python_setup()
if(CATKIN_ENABLE_TESTING)
catkin_add_nosetests(test/test_tf2_sensor_msgs.py)
find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs rostest tf2_ros tf2)
add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp)
target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
if(TARGET tests)
add_dependencies(tests test_tf2_sensor_msgs_cpp)
endif()
add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch)
endif()

107
navigations/geometry2/tf2_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
navigations/geometry2/tf2_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>BSD</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>

13
navigations/geometry2/tf2_sensor_msgs/setup.py Executable file
View File

@@ -0,0 +1,13 @@
#!/usr/bin/env python
from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['tf2_sensor_msgs'],
package_dir={'': 'src'},
requires=['rospy','sensor_msgs','tf2_ros','orocos_kdl']
)
setup(**d)

1
navigations/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py Executable file
View File

@@ -0,0 +1 @@
from .tf2_sensor_msgs import *

60
navigations/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py Executable file
View File

@@ -0,0 +1,60 @@
# 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.
from sensor_msgs.msg import PointCloud2
from sensor_msgs.point_cloud2 import read_points, create_cloud
import PyKDL
import rospy
import tf2_ros
def to_msg_msg(msg):
return msg
tf2_ros.ConvertRegistration().add_to_msg(PointCloud2, to_msg_msg)
def from_msg_msg(msg):
return msg
tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg)
def transform_to_kdl(t):
return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
t.transform.rotation.z, t.transform.rotation.w),
PyKDL.Vector(t.transform.translation.x,
t.transform.translation.y,
t.transform.translation.z))
# PointStamped
def do_transform_cloud(cloud, transform):
t_kdl = transform_to_kdl(transform)
points_out = []
for p_in in read_points(cloud):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append((p_out[0], p_out[1], p_out[2]) + p_in[3:])
res = create_cloud(transform.header, cloud.fields, points_out)
return res
tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud)

3
navigations/geometry2/tf2_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
navigations/geometry2/tf2_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;
}

103
navigations/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py Executable file
View File

@@ -0,0 +1,103 @@
#!/usr/bin/env python
from __future__ import print_function
import unittest
import struct
import tf2_sensor_msgs
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointField
from tf2_ros import TransformStamped
import copy
## A sample python unit test
class PointCloudConversions(unittest.TestCase):
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
self.point_cloud_in.point_step = 4 * 3
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
points = [1, 2, 0, 10, 20, 30]
self.point_cloud_in.data = struct.pack('%sf' % len(points), *points)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = 300
self.transform_translate_xyz_300.transform.translation.y = 300
self.transform_translate_xyz_300.transform.translation.z = 300
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)])
def test_simple_transform(self):
old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
k = 300
expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)]
new_points = list(point_cloud2.read_points(point_cloud_transformed))
print("new_points are %s" % new_points)
assert(expected_coordinates == new_points)
assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud
## A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version)
class PointCloudConversionsMultichannel(unittest.TestCase):
TRANSFORM_OFFSET_DISTANCE = 300
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('index', 12, PointField.INT32, 1)]
self.point_cloud_in.point_step = 4 * 4
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
for point in self.points:
self.point_cloud_in.data += struct.pack('3fi', *point)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points)
def test_simple_transform_multichannel(self):
old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
expected_coordinates = []
for point in self.points:
expected_coordinates += [(
point[0] + self.TRANSFORM_OFFSET_DISTANCE,
point[1] + self.TRANSFORM_OFFSET_DISTANCE,
point[2] + self.TRANSFORM_OFFSET_DISTANCE,
point[3] # index channel must be kept same
)]
new_points = list(point_cloud2.read_points(point_cloud_transformed))
print("new_points are %s" % new_points)
assert(expected_coordinates == new_points)
assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud
if __name__ == '__main__':
import rosunit
rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions)
rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel)