From 33c0a39f6c7d56b5adc8a545a25bdba6d4c633c6 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Mon, 25 Jan 2021 22:46:22 +0000 Subject: [PATCH 01/10] Reenable sensor_msgs cpp test --- tf2_sensor_msgs/CMakeLists.txt | 40 ++++++++------- tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp | 49 ++++++++++--------- 2 files changed, 48 insertions(+), 41 deletions(-) diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index 45243b083..713e82630 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -24,24 +24,26 @@ find_package(Eigen3 REQUIRED) ament_export_dependencies(eigen3_cmake_module) ament_export_dependencies(Eigen3) - -# TODO enable tests -#if(BUILD_TESTING) -# catkin_add_nosetests(test/test_tf2_sensor_msgs.py) - -#find_package(catkin REQUIRED COMPONENTS -# sensor_msgs -# rostest -# tf2_ros -# tf2 -#) -#include_directories(${EIGEN_INCLUDE_DIRS}) -#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() +ament_export_dependencies(tf2) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + # TODO(ros2/orocos_kinematics_dynamics): reenable when PyKDL is ready to use + #ament_add_pytest_test(test_tf2_sensor_msgs_py test/test_tf2_sensor_msgs.py) + + ament_add_gtest(test_tf2_sensor_msgs_cpp test/test_tf2_sensor_msgs.cpp) + if(TARGET test_tf2_sensor_msgs_cpp) + target_include_directories(test_tf2_sensor_msgs_cpp PUBLIC include) + ament_target_dependencies(test_tf2_sensor_msgs_cpp + "Eigen3" + "rclcpp" + "sensor_msgs" + "tf2" + "tf2_ros" + ) + endif() +endif() ament_auto_package() diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index 506376f93..e9171a41e 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -28,12 +28,13 @@ */ -#include -#include -#include -#include +#include +#include #include +#include #include +#include +#include std::unique_ptr tf_buffer = nullptr; static const double EPS = 1e-3; @@ -42,45 +43,48 @@ static const double EPS = 1e-3; TEST(Tf2Sensor, PointCloud2) { sensor_msgs::msg::PointCloud2 cloud; - sensor_msgs::msg::PointCloud2Modifier modifier(cloud); + sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); modifier.resize(1); - sensor_msgs::msg::PointCloud2Iterator iter_x(cloud, "x"); - sensor_msgs::msg::PointCloud2Iterator iter_y(cloud, "y"); - sensor_msgs::msg::PointCloud2Iterator iter_z(cloud, "z"); + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); *iter_x = 1; *iter_y = 2; *iter_z = 3; - cloud.header.stamp = builtin_interfaces::msg::Time(2); + cloud.header.stamp.sec = 2; + cloud.header.stamp.nanosec = 0; cloud.header.frame_id = "A"; // simple api - sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", tf2::durationFromSec(2.0)); - sensor_msgs::msg::PointCloud2Iterator iter_x_after(cloud_simple, "x"); - sensor_msgs::msg::PointCloud2Iterator iter_y_after(cloud_simple, "y"); - sensor_msgs::msg::PointCloud2Iterator iter_z_after(cloud_simple, "z"); + sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform( + cloud, "B", tf2::durationFromSec( + 2.0)); + sensor_msgs::PointCloud2Iterator iter_x_after(cloud_simple, "x"); + sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); + sensor_msgs::PointCloud2Iterator 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::msg::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", builtin_interfaces::msg::Time(2.0), - "A", tf2::durationFromSec(3.0)); - sensor_msgs::msg::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); - sensor_msgs::msg::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); - sensor_msgs::msg::PointCloud2Iterator iter_z_advanced(cloud_advanced, "z"); + sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform( + cloud, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); + sensor_msgs::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); + sensor_msgs::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); + sensor_msgs::PointCloud2Iterator 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){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test"); - ros::NodeHandle n; rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf_buffer = std::make_unique(clock); @@ -94,7 +98,8 @@ int main(int argc, char **argv){ t.transform.rotation.y = 0; t.transform.rotation.z = 0; t.transform.rotation.w = 0; - t.header.stamp = builtin_interfaces::msg::Time(2.0); + t.header.stamp.sec = 2; + t.header.stamp.nanosec = 0; t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); From 7eb81ba70867802d2378aec1d072e5340b0f8b41 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 26 May 2021 14:15:52 +0000 Subject: [PATCH 02/10] Small cleanups. Signed-off-by: Chris Lalancette --- tf2_sensor_msgs/CMakeLists.txt | 3 ++- tf2_sensor_msgs/package.xml | 4 ++-- tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp | 2 ++ 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index 713e82630..37c2cddb8 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -25,12 +25,13 @@ find_package(Eigen3 REQUIRED) ament_export_dependencies(eigen3_cmake_module) ament_export_dependencies(Eigen3) ament_export_dependencies(tf2) +ament_export_dependencies(tf2_ros) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_pytest REQUIRED) # TODO(ros2/orocos_kinematics_dynamics): reenable when PyKDL is ready to use + #find_package(ament_cmake_pytest REQUIRED) #ament_add_pytest_test(test_tf2_sensor_msgs_py test/test_tf2_sensor_msgs.py) ament_add_gtest(test_tf2_sensor_msgs_cpp test/test_tf2_sensor_msgs.cpp) diff --git a/tf2_sensor_msgs/package.xml b/tf2_sensor_msgs/package.xml index cac2b780f..a9466632d 100644 --- a/tf2_sensor_msgs/package.xml +++ b/tf2_sensor_msgs/package.xml @@ -25,10 +25,10 @@ tf2_ros - - tf2_ros_py + ament_cmake_gtest + rclcpp ament_cmake diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index e9171a41e..ecccc7806 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -27,10 +27,12 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include +#include #include #include #include From 283cc4bc4538b1804f2c0cbef66dad2652145df7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 26 May 2021 15:59:09 +0000 Subject: [PATCH 03/10] Add static_casts for Windows compile warning. Signed-off-by: Chris Lalancette --- .../tf2_sensor_msgs/tf2_sensor_msgs.hpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp index eeef071de..e204e92de 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp @@ -62,10 +62,21 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po { p_out = p_in; p_out.header = t_in.header; - Eigen::Transform t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y, - t_in.transform.translation.z) * Eigen::Quaternion( - t_in.transform.rotation.w, t_in.transform.rotation.x, - t_in.transform.rotation.y, t_in.transform.rotation.z); + // FIXME(clalancette): The static casts to float aren't ideal; the incoming + // transform uses double, and hence may have values that are too large to represent + // in a float. But since this method implicitly returns a float PointCloud2, we don't + // have much choice here without subtly changing the API. + auto translation = Eigen::Translation3f( + static_cast(t_in.transform.translation.x), + static_cast(t_in.transform.translation.y), + static_cast(t_in.transform.translation.z)); + auto quaternion = Eigen::Quaternion( + static_cast(t_in.transform.rotation.w), + static_cast(t_in.transform.rotation.x), + static_cast(t_in.transform.rotation.y), + static_cast(t_in.transform.rotation.z)); + + Eigen::Transform t = translation * quaternion; sensor_msgs::PointCloud2ConstIterator x_in(p_in, std::string("x")); sensor_msgs::PointCloud2ConstIterator y_in(p_in, std::string("y")); From 86c30b5a6d8fe3a028637551539e64d6a1d24b81 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 26 May 2021 20:28:03 +0000 Subject: [PATCH 04/10] Nitpicks (including cpplint) --- tf2_sensor_msgs/package.xml | 3 --- tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp | 22 +++++++++---------- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/tf2_sensor_msgs/package.xml b/tf2_sensor_msgs/package.xml index a9466632d..7de27ec43 100644 --- a/tf2_sensor_msgs/package.xml +++ b/tf2_sensor_msgs/package.xml @@ -23,9 +23,6 @@ sensor_msgs tf2 tf2_ros - - - ament_cmake_gtest rclcpp diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index ecccc7806..97d07443a 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -27,17 +27,19 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include + +#include +#include #include #include -#include #include #include -#include -#include #include +#include + std::unique_ptr tf_buffer = nullptr; static const double EPS = 1e-3; @@ -57,14 +59,12 @@ TEST(Tf2Sensor, PointCloud2) *iter_y = 2; *iter_z = 3; - cloud.header.stamp.sec = 2; - cloud.header.stamp.nanosec = 0; + cloud.header.stamp = rclcpp::Time(2, 0); cloud.header.frame_id = "A"; // simple api sensor_msgs::msg::PointCloud2 cloud_simple = tf_buffer->transform( - cloud, "B", tf2::durationFromSec( - 2.0)); + cloud, "B", tf2::durationFromSec(2.0)); sensor_msgs::PointCloud2Iterator iter_x_after(cloud_simple, "x"); sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); sensor_msgs::PointCloud2Iterator iter_z_after(cloud_simple, "z"); @@ -74,8 +74,7 @@ TEST(Tf2Sensor, PointCloud2) // advanced api sensor_msgs::msg::PointCloud2 cloud_advanced = tf_buffer->transform( - cloud, "B", tf2::timeFromSec(2.0), - "A", tf2::durationFromSec(3.0)); + cloud, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); sensor_msgs::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); sensor_msgs::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); sensor_msgs::PointCloud2Iterator iter_z_advanced(cloud_advanced, "z"); @@ -100,8 +99,7 @@ int main(int argc, char ** argv) t.transform.rotation.y = 0; t.transform.rotation.z = 0; t.transform.rotation.w = 0; - t.header.stamp.sec = 2; - t.header.stamp.nanosec = 0; + t.header.stamp = rclcpp::Time(2, 0); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); From 1d9c7518882c6653b361a687757f43fca6058f0d Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 26 May 2021 20:47:10 +0000 Subject: [PATCH 05/10] Enable linting --- tf2_sensor_msgs/CMakeLists.txt | 7 +++++++ tf2_sensor_msgs/package.xml | 2 ++ 2 files changed, 9 insertions(+) diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index 37c2cddb8..4c02c8126 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -29,6 +29,13 @@ ament_export_dependencies(tf2_ros) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + # TODO(ros2/geometry2#259) Remove once headers + # are renamed to .hpp + set(ament_cmake_uncrustify_ADDITIONAL_ARGS --language CPP) + set(ament_cmake_cppcheck_LANGUAGE c++) + ament_lint_auto_find_test_dependencies() # TODO(ros2/orocos_kinematics_dynamics): reenable when PyKDL is ready to use #find_package(ament_cmake_pytest REQUIRED) diff --git a/tf2_sensor_msgs/package.xml b/tf2_sensor_msgs/package.xml index 7de27ec43..2f5553538 100644 --- a/tf2_sensor_msgs/package.xml +++ b/tf2_sensor_msgs/package.xml @@ -25,6 +25,8 @@ tf2_ros ament_cmake_gtest + ament_lint_auto + ament_lint_common rclcpp From b3922698f7c593daf3bedf74ec1ad5ad79493601 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 26 May 2021 20:48:20 +0000 Subject: [PATCH 06/10] Fix copyright --- .../include/tf2_sensor_msgs/tf2_sensor_msgs.h | 55 +++++++++---------- .../tf2_sensor_msgs/tf2_sensor_msgs.hpp | 55 +++++++++---------- .../src/tf2_sensor_msgs/__init__.py | 28 +++++++++- .../src/tf2_sensor_msgs/tf2_sensor_msgs.py | 23 ++++---- tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp | 55 +++++++++---------- tf2_sensor_msgs/test/test_tf2_sensor_msgs.py | 28 ++++++++++ 6 files changed, 148 insertions(+), 96 deletions(-) diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h index d95ec4924..2c2afef25 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h @@ -1,31 +1,30 @@ -/* - * 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. - */ +// Copyright 2008 Willow Garage, Inc. +// +// 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 HOLDER 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 diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp index e204e92de..6dcd400d6 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp @@ -1,31 +1,30 @@ -/* - * 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. - */ +// Copyright 2008 Willow Garage, Inc. +// +// 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 HOLDER 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_HPP #define TF2_SENSOR_MSGS_HPP diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py index 93b67779a..0a75ea21d 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py @@ -1 +1,27 @@ -from tf2_sensor_msgs import * +# Copyright 2008 Willow Garage, Inc. +# +# 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 HOLDER 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. diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py index 807f33c21..e1f72829b 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py @@ -1,22 +1,23 @@ -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# Copyright 2008 Willow Garage, Inc. # # 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. +# * 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 +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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 diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index 97d07443a..e9adb6203 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -1,31 +1,30 @@ -/* - * 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. - */ +// Copyright 2008 Willow Garage, Inc. +// +// 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 HOLDER 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 diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py index 09fc0936c..119d4b7d4 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py @@ -1,4 +1,32 @@ #!/usr/bin/env python +# +# Copyright 2008 Willow Garage, Inc. +# +# 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 HOLDER 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. import unittest import struct From 9b9c170cc451e5e9b68b7a7927df306af89da7b4 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 26 May 2021 20:49:24 +0000 Subject: [PATCH 07/10] Fix flake8 python scripts don't seem to be ported to ROS2 tho --- .../src/tf2_sensor_msgs/__init__.py | 2 ++ .../src/tf2_sensor_msgs/tf2_sensor_msgs.py | 25 +++++++++----- tf2_sensor_msgs/test/test_tf2_sensor_msgs.py | 34 ++++++++++++------- 3 files changed, 40 insertions(+), 21 deletions(-) diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py index 0a75ea21d..6b4eefa5f 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py @@ -25,3 +25,5 @@ # 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 tf2_sensor_msgs import * # noqa(E401) diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py index e1f72829b..aee6d0f6b 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py @@ -1,8 +1,8 @@ # Copyright 2008 Willow Garage, Inc. -# +# # 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. # @@ -13,7 +13,7 @@ # * 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 @@ -29,26 +29,33 @@ from sensor_msgs.msg import PointCloud2 from sensor_msgs.point_cloud2 import read_points, create_cloud import PyKDL -import rospy +import rospy # noqa(E401) 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, + 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) @@ -58,4 +65,6 @@ def do_transform_cloud(cloud, transform): points_out.append(p_out) res = create_cloud(transform.header, cloud.fields, points_out) return res + + tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud) diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py index 119d4b7d4..9ce80b0f3 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py @@ -36,44 +36,52 @@ from tf2_ros import TransformStamped import copy -## A sample python unit test + +# 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.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 + 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 + # no rotation so we only set w + self.transform_translate_xyz_300.transform.rotation.w = 1 - assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)]) + 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) + # deepcopy is not required here because we have a str for now + old_data = copy.deepcopy(self.point_cloud_in.data) + 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 + # checking no modification in input cloud + assert(old_data == self.point_cloud_in.data) + 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", + PointCloudConversions) From 778574f72d26a4deb2df6142de94242b840a2f3d Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 26 May 2021 20:49:50 +0000 Subject: [PATCH 08/10] fix cpp linting --- .../include/tf2_sensor_msgs/tf2_sensor_msgs.h | 6 ++-- .../tf2_sensor_msgs/tf2_sensor_msgs.hpp | 35 +++++++++++-------- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h index 2c2afef25..99535af0a 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h @@ -26,11 +26,11 @@ // 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 +#ifndef TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_H_ +#define TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_H_ #warning This header is obsolete, please include tf2_sensor_msgs/tf2_sensor_msgs.hpp instead #include -#endif // TF2_SENSOR_MSGS_H +#endif // TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_H_ diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp index 6dcd400d6..de64c4008 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp @@ -26,8 +26,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef TF2_SENSOR_MSGS_HPP -#define TF2_SENSOR_MSGS_HPP +#ifndef TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_ +#define TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_ #include #include @@ -37,6 +37,8 @@ #include #include +#include + namespace tf2 { @@ -45,19 +47,24 @@ namespace tf2 /********************/ // method to extract timestamp from object -template <> +template<> inline -tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2& p) {return tf2_ros::fromMsg(p.header.stamp);} +tf2::TimePoint getTimestamp(const sensor_msgs::msg::PointCloud2 & p) +{ + return tf2_ros::fromMsg(p.header.stamp); +} // method to extract frame id from object -template <> +template<> inline -std::string getFrameId(const sensor_msgs::msg::PointCloud2 &p) {return p.header.frame_id;} +std::string getFrameId(const sensor_msgs::msg::PointCloud2 & p) {return p.header.frame_id;} // this method needs to be implemented by client library developers -template <> +template<> inline -void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::PointCloud2 &p_out, const geometry_msgs::msg::TransformStamped& t_in) +void doTransform( + const sensor_msgs::msg::PointCloud2 & p_in, sensor_msgs::msg::PointCloud2 & p_out, + const geometry_msgs::msg::TransformStamped & t_in) { p_out = p_in; p_out.header = t_in.header; @@ -75,7 +82,7 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po static_cast(t_in.transform.rotation.y), static_cast(t_in.transform.rotation.z)); - Eigen::Transform t = translation * quaternion; + Eigen::Transform t = translation * quaternion; sensor_msgs::PointCloud2ConstIterator x_in(p_in, std::string("x")); sensor_msgs::PointCloud2ConstIterator y_in(p_in, std::string("y")); @@ -86,7 +93,7 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po sensor_msgs::PointCloud2Iterator z_out(p_out, std::string("z")); Eigen::Vector3f point; - for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) { + 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(); @@ -94,16 +101,16 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po } } inline -sensor_msgs::msg::PointCloud2 toMsg(const sensor_msgs::msg::PointCloud2 &in) +sensor_msgs::msg::PointCloud2 toMsg(const sensor_msgs::msg::PointCloud2 & in) { return in; } inline -void fromMsg(const sensor_msgs::msg::PointCloud2 &msg, sensor_msgs::msg::PointCloud2 &out) +void fromMsg(const sensor_msgs::msg::PointCloud2 & msg, sensor_msgs::msg::PointCloud2 & out) { out = msg; } -} // namespace +} // namespace tf2 -#endif // TF2_SENSOR_MSGS_HPP +#endif // TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_ From 1f8065ae9afa11b6d45ac2f143714b36a7ff0cb1 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 26 May 2021 21:45:31 +0000 Subject: [PATCH 09/10] re-add tf2_ros_py dependency --- tf2_sensor_msgs/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tf2_sensor_msgs/package.xml b/tf2_sensor_msgs/package.xml index 2f5553538..f8cd14517 100644 --- a/tf2_sensor_msgs/package.xml +++ b/tf2_sensor_msgs/package.xml @@ -24,6 +24,8 @@ tf2 tf2_ros + tf2_ros_py + ament_cmake_gtest ament_lint_auto ament_lint_common From 192d585e0c6e55ee300f2d1500ac685397980e1c Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Thu, 27 May 2021 08:08:33 +0000 Subject: [PATCH 10/10] fix further flake8 complains --- .../src/tf2_sensor_msgs/tf2_sensor_msgs.py | 4 ++-- tf2_sensor_msgs/test/test_tf2_sensor_msgs.py | 12 +++++++----- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py index aee6d0f6b..a3014ce36 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py @@ -26,9 +26,9 @@ # 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 +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.point_cloud2 import create_cloud, read_points import rospy # noqa(E401) import tf2_ros diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py index 9ce80b0f3..665f08e04 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py @@ -28,17 +28,19 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import unittest +import copy import struct -import tf2_sensor_msgs +import unittest + from sensor_msgs import point_cloud2 from sensor_msgs.msg import PointField from tf2_ros import TransformStamped -import copy +import tf2_sensor_msgs # A sample python unit test class PointCloudConversions(unittest.TestCase): + def setUp(self): self.point_cloud_in = point_cloud2.PointCloud2() self.point_cloud_in.fields = [ @@ -75,7 +77,7 @@ def test_simple_transform(self): 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) + print('new_points are %s' % new_points) assert(expected_coordinates == new_points) # checking no modification in input cloud assert(old_data == self.point_cloud_in.data) @@ -83,5 +85,5 @@ def test_simple_transform(self): if __name__ == '__main__': import rosunit - rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", + rosunit.unitrun('test_tf2_sensor_msgs', 'test_point_cloud_conversion', PointCloudConversions)