diff --git a/tf2_sensor_msgs/CMakeLists.txt b/tf2_sensor_msgs/CMakeLists.txt index 45243b083..4c02c8126 100644 --- a/tf2_sensor_msgs/CMakeLists.txt +++ b/tf2_sensor_msgs/CMakeLists.txt @@ -24,24 +24,34 @@ 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) +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) + #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/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h index d95ec4924..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 @@ -1,37 +1,36 @@ -/* - * 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 +#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 eeef071de..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 @@ -1,34 +1,33 @@ -/* - * 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 +#ifndef TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_ +#define TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_HPP_ #include #include @@ -38,6 +37,8 @@ #include #include +#include + namespace tf2 { @@ -46,26 +47,42 @@ 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; - 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")); @@ -76,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(); @@ -84,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_ diff --git a/tf2_sensor_msgs/package.xml b/tf2_sensor_msgs/package.xml index cac2b780f..f8cd14517 100644 --- a/tf2_sensor_msgs/package.xml +++ b/tf2_sensor_msgs/package.xml @@ -23,12 +23,13 @@ sensor_msgs tf2 tf2_ros - - tf2_ros_py ament_cmake_gtest + ament_lint_auto + ament_lint_common + rclcpp ament_cmake diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py index 93b67779a..6b4eefa5f 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py @@ -1 +1,29 @@ -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. + +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 807f33c21..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 @@ -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 @@ -25,29 +26,36 @@ # 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 +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.point_cloud2 import create_cloud, read_points +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) @@ -57,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.cpp b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp index 506376f93..e9adb6203 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -1,39 +1,43 @@ -/* - * 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 -#include -#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 +46,45 @@ 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 = 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)); - 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,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 = builtin_interfaces::msg::Time(2.0); + t.header.stamp = rclcpp::Time(2, 0); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py index 09fc0936c..665f08e04 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py @@ -1,51 +1,89 @@ #!/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 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 +# 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) + 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)