Reenable sensor_msgs test (#422)

* Reenable sensor_msgs cpp test

Co-authored-by: Bjar Ne <gleichdick@users.noreply.github.com>
Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>

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()

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 <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 
-#endif // TF2_SENSOR_MSGS_H
+#endif  // TF2_SENSOR_MSGS__TF2_SENSOR_MSGS_H_

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 <tf2/convert.h>
 #include <tf2/time.h>
@@ -38,6 +37,8 @@
 #include <Eigen/Geometry>
 #include <tf2_ros/buffer_interface.h>
 
+#include <string>
+
 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<float,3,Eigen::Affine> 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);
+  // 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<float>(t_in.transform.translation.x),
+    static_cast<float>(t_in.transform.translation.y),
+    static_cast<float>(t_in.transform.translation.z));
+  auto quaternion = Eigen::Quaternion<float>(
+    static_cast<float>(t_in.transform.rotation.w),
+    static_cast<float>(t_in.transform.rotation.x),
+    static_cast<float>(t_in.transform.rotation.y),
+    static_cast<float>(t_in.transform.rotation.z));
+
+  Eigen::Transform<float, 3, Eigen::Affine> t = translation * quaternion;
 
   sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, std::string("x"));
   sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, std::string("y"));
@@ -76,24 +93,24 @@ void doTransform(const sensor_msgs::msg::PointCloud2 &p_in, sensor_msgs::msg::Po
   sensor_msgs::PointCloud2Iterator<float> 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();
     *z_out = point.z();
   }
 }
 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_

tf2_sensor_msgs/package.xml

@@ -23,12 +23,13 @@
   <depend>sensor_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
-  <!-- <depend>python_orocos_kdl</depend> -->
-  <!-- <test_depend>rostest</test_depend> -->
 
   <exec_depend>tf2_ros_py</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>rclcpp</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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)