fix bug nan in function transformLaserScanToPointCloud_

Signed-off-by: Alejandro Hernandez Cordero <ahcorde@gmail.com>

.gitignore

@@ -1,2 +0,0 @@
-*~
-*.pyc

CHANGELOG.rst

@@ -1,169 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package laser_geometry
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-2.10.0 (2024-12-20)
--------------------
-* Deprecating tf2 C Headers (`#98 <https://github.com/ros-perception/laser_geometry/issues/98>`_)
-* Remove CODEOWNERS and mirror-rolling-to-main workflow (`#100 <https://github.com/ros-perception/laser_geometry/issues/100>`_)
-* Contributors: Alejandro Hernández Cordero, Lucas Wendland
-
-2.9.0 (2024-11-20)
-------------------
-* Stop using python_cmake_module. (`#93 <https://github.com/ros-perception/laser_geometry/issues/93>`_)
-* Contributors: Chris Lalancette
-
-2.8.1 (2024-10-03)
-------------------
-* Added common linters (`#96 <https://github.com/ros-perception/laser_geometry/issues/96>`_)
-* Contributors: Alejandro Hernández Cordero
-
-2.8.0 (2024-04-26)
-------------------
-
-2.7.0 (2023-12-26)
-------------------
-* Switch to target_link_libraries. (`#92 <https://github.com/ros-perception/laser_geometry/issues/92>`_)
-* Contributors: Chris Lalancette
-
-2.6.0 (2023-04-28)
-------------------
-
-2.5.0 (2023-02-14)
-------------------
-* Update laser_geometry to C++17. (`#90 <https://github.com/ros-perception/laser_geometry/issues/90>`_)
-* Update Maintainers (`#88 <https://github.com/ros-perception/laser_geometry/issues/88>`_)
-* Mirror rolling to ros2
-* Contributors: Audrow Nash, Chris Lalancette
-
-2.4.0 (2022-03-01)
-------------------
-* Install headers to include/${PROJECT_NAME} (`#86 <https://github.com/ros-perception/laser_geometry/issues/86>`_)
-* Explicit cast to double to prevent loss of precision
-* Fix Duration casting issue leading to no undistortion
-* Contributors: Jonathan Binney, Marco Lampacrescia, Shane Loretz
-
-2.3.0 (2022-01-14)
-------------------
-* Fix building on running on Windows Debug (`#82 <https://github.com/ros-perception/laser_geometry/issues/82>`_)
-* Update python code and tests for ros2 (`#80 <https://github.com/ros-perception/laser_geometry/issues/80>`_)
-* Contributors: Chris Lalancette, Jonathan Binney
-
-2.2.2 (2021-05-11)
-------------------
-* Export sensor_msgs, tf2, and rclcpp as dependencies
-* Contributors: Mabel Zhang, Michel Hidalgo
-
-2.2.1 (2020-12-08)
-------------------
-* Use rclcpp::Duration::from_seconds (`#72 <https://github.com/ros-perception/laser_geometry/issues/72>`_)
-* update maintainers
-* increase test timeout
-* Contributors: Dirk Thomas, Ivan Santiago Paunovic, Jonathan Binney, Mabel Zhang
-
-2.2.0 (2020-04-30)
-------------------
-* use ament_export_targets()
-* code style only: wrap after open parenthesis if not in one line (`#52 <https://github.com/ros-perception/laser_geometry/issues/52>`_)
-* use target_include_directories
-* Drop CMake extras redundant with eigen3_cmake_module. (`#50 <https://github.com/ros-perception/laser_geometry/issues/50>`_)
-* Contributors: Dirk Thomas, Jonathan Binney, Karsten Knese, Michel Hidalgo
-
-2.1.0 (2019-09-27)
-------------------
-* Merge pull request `#46 <https://github.com/ros-perception/laser_geometry/issues/46>`_ from sloretz/eigen3_cmake_module
-* Contributors: Jonathan Binney, Shane Loretz
-
-2.0.0 (2018-06-27)
-------------------
-* Removed the ``angle`` dependency as no longer necessary.
-* Updated to build statically but use position independent code.
-* Updated to compile, and to remove PointCloud support, and remove boost.
-* Added visibility headers modified from ``rclcpp``.
-* Updated ``laser_geometry`` to build for ros2 (and on Windows 10).
-* Improved use of numpy. (`#14 <https://github.com/ros-perception/laser_geometry/issues/14>`_)
-* Contributors: Alessandro Bottero, Andreas Greimel, Brian Fjeldstad, Eric Wieser, Jon Binney, Jonathan Binney, Martin Idel, Mikael Arguedas, Vincent Rabaud, William Woodall
-
-1.6.4 (2015-05-18)
-------------------
-* Fix segfault when laserscan ranges[] is empty
-* Contributors: Timm Linder, Vincent Rabaud
-
-1.6.3 (2015-03-07)
-------------------
-* provide support for tf2
-* Contributors: Vincent Rabaud
-
-1.6.2 (2014-06-08)
-------------------
-* adds python port (only simple projection)
-* allows to have range_cutoff > range_max
-  NOTE this is required if we need to keep the range_max readings
-  in the point cloud.
-  An example application is an obstacle_layer in a costmap.
-* Contributors: Vincent Rabaud, enriquefernandez
-
-1.6.1 (2014-02-23)
-------------------
-* Added dependency on cmake_modules
-* Contributors: William Woodall
-
-1.6.0 (2014-02-21)
-------------------
-* Adding William Woodall as a co-maintainer
-* Contributors: Vincent Rabaud, William Woodall
-
-1.5.15 (2013-12-02)
--------------------
-* Fix mistake in end_time calculation for scan transformation in #6
-
-1.5.14 (2013-11-04)
--------------------
-* Treat max_range as invalid measurement
-* Properly propagate range_cutoff
-* check for CATKIN_ENABLE_TESTING
-
-1.5.13 (2013-10-06)
--------------------
-* fixes `#3 <https://github.com/ros-perception/laser_geometry/issues/3>`_
-
-1.5.12 (2013-09-14)
--------------------
-* fix case of Eigen find_package name
-
-1.5.11 (2013-07-01)
--------------------
-* added missing run deps
-
-1.5.10 (2013-06-28 15:09)
--------------------------
-* [bugfix] export boost and eigen via DEPENDS
-
-1.5.9 (2013-06-28 11:38)
-------------------------
-* [bugfix] export boost and eigen include dirs
-
-1.5.8 (2012-12-14 13:54)
-------------------------
-* Added buildtool_depend on catkin
-
-1.5.7 (2012-12-14 13:48)
-------------------------
-* CMake clean up
-
-1.5.6 (2012-12-10)
-------------------
-* Removed vestigial manifest.xml
-
-1.5.5 (2012-11-15)
-------------------
-* Added .count field (of 1) to every PointCloud2 field description.
-  This fixes the bug referred to here: http://dev.pointclouds.org/issues/821 which is useful because that fix in PCL
-  seems not to be released yet.
-  Also this way is more correct, as far as I can tell.
-* Tidied up CMakeLists.txt based on Dirk's recommendations.
-
-1.5.4 (2012-10-10)
-------------------
-* added install rules to CMakeLists.txt needed for catkinization.
-* catkinized

CMakeLists.txt

@@ -1,92 +1,187 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_laser_geometry VERSION 1.0.0 LANGUAGES CXX)
 
-project(laser_geometry)
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_laser_geometry with Catkin")
 
-if(NOT CMAKE_CXX_STANDARD)
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_laser_geometry with Standalone CMake")
+endif()
+
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
-endif()
+set(CMAKE_CXX_EXTENSIONS OFF)
 
-find_package(ament_cmake REQUIRED)
+# Find dependencies
-find_package(ament_cmake_python REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(GTest REQUIRED)
 
-find_package(rclcpp REQUIRED)
+if (NOT BUILDING_WITH_CATKIN)
-find_package(sensor_msgs REQUIRED)
+
-find_package(tf2 REQUIRED)
+  # Enable Position Independent Code
-find_package(eigen3_cmake_module REQUIRED)
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+  set(PACKAGES_DIR  
+    robot_sensor_msgs
+    geometry_msgs
+    robot_time
+    tf3
+    data_convert
+  )
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_sensor_msgs
+    geometry_msgs
+    robot_time
+    data_convert
+  )
+
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
   find_package(Eigen3 REQUIRED)
 
-ament_python_install_package(laser_geometry PACKAGE_DIR src/laser_geometry)
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS robot_sensor_msgs geometry_msgs robot_time data_convert
+    DEPENDS Eigen3 
+  )
 
-add_library(laser_geometry SHARED src/laser_geometry.cpp)
+  include_directories(
-target_include_directories(laser_geometry
+    include
-  PUBLIC
+    ${catkin_INCLUDE_DIRS}
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    ${EIGEN3_INCLUDE_DIRS}
-    $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+    ${TF3_INCLUDE_DIR}
-  ${Eigen3_INCLUDE_DIRS}
   )
-target_link_libraries(laser_geometry PUBLIC
-  ${sensor_msgs_TARGETS}
-  tf2::tf2
-)
-if(TARGET Eigen3::Eigen)
-  target_link_libraries(laser_geometry PUBLIC Eigen3::Eigen)
-else()
-  target_include_directories(laser_geometry PUBLIC ${Eigen3_INCLUDE_DIRS})
 endif()
 
-target_link_libraries(laser_geometry PRIVATE
+# Libraries
-  rclcpp::rclcpp
+add_library(${PROJECT_NAME} SHARED
+  src/laser_geometry.cpp
 )
 
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+    ${TF3_LIBRARY}
+  )
+
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      ${PACKAGES_DIR}
+      ${TF3_LIBRARY}
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
+
+# Link Eigen3
+if(TARGET Eigen3::Eigen)
+  target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen)
+else()
+  target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS})
+endif()
+
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
-target_compile_definitions(laser_geometry PRIVATE "LASER_GEOMETRY_BUILDING_LIBRARY")
+target_compile_definitions(${PROJECT_NAME} PRIVATE "ROBOT_LASER_GEOMETRY_BUILDING_LIBRARY")
 
-# Export old-style CMake variables
+if(BUILDING_WITH_CATKIN)
-ament_export_include_directories("include/${PROJECT_NAME}")
+  ## Mark libraries for installation
-ament_export_libraries(laser_geometry)
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-
+  install(TARGETS ${PROJECT_NAME}
-# Export modern CMake targets
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-ament_export_targets(laser_geometry)
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-ament_export_dependencies(
-  Eigen3
-  sensor_msgs
-  tf2
   )
 
-install(
+  ## Mark cpp header files for installation
-  TARGETS laser_geometry
+  install(DIRECTORY include/${PROJECT_NAME}/
-  EXPORT laser_geometry
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib
     RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
   )
 
-install(
+  # Export targets
-  DIRECTORY include/
+  install(EXPORT ${PROJECT_NAME}-targets
-  DESTINATION include/${PROJECT_NAME}
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
   )
 
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Dependencies: robot_sensor_msgs, geometry_msgs, robot_time, tf3, data_convert, Eigen3")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Test executables
+# ========================================================
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
+  enable_testing()
-  ament_lint_auto_find_test_dependencies()
-  find_package(ament_cmake_gtest REQUIRED)
   
-  ament_add_gtest(projection_test
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/projection_test.cpp)
-    test/projection_test.cpp
+    add_executable(${PROJECT_NAME}_test test/projection_test.cpp)
-    TIMEOUT 240)
+    target_link_libraries(${PROJECT_NAME}_test PRIVATE 
-  if(TARGET projection_test)
+      ${PROJECT_NAME}
-    target_link_libraries(projection_test laser_geometry rclcpp::rclcpp)
+      GTest::GTest
-  endif()
+      GTest::Main
-
+      Boost::system Boost::thread
-  find_package(ament_cmake_pytest REQUIRED)
+      ${TF3_LIBRARY}
-
-  ament_add_pytest_test(projection test/projection_test.py
-    TIMEOUT 120
     )
+    add_test(NAME ${PROJECT_NAME}_test COMMAND ${PROJECT_NAME}_test)
+  endif()
 endif()
-
-ament_package()

CONTRIBUTING.md

@@ -1,3 +0,0 @@
-Any contribution that you make to this repository will
-be under the 3-Clause BSD License, as dictated by that
-[license](https://opensource.org/licenses/BSD-3-Clause).

LICENSE

@@ -1,29 +0,0 @@
-BSD 3-Clause License
-
-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 copyright holder 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/robot_laser_geometry/laser_geometry.hpp

@@ -29,8 +29,8 @@
 // POSSIBILITY OF SUCH DAMAGE.
 
 
-#ifndef LASER_GEOMETRY__LASER_GEOMETRY_HPP_
+#ifndef ROBOT_LASER_GEOMETRY_ROBOT_LASER_GEOMETRY_HPP_
-#define LASER_GEOMETRY__LASER_GEOMETRY_HPP_
+#define ROBOT_LASER_GEOMETRY_ROBOT_LASER_GEOMETRY_HPP_
 
 #include <map>
 #include <iostream>
@@ -39,12 +39,13 @@
 
 #include <Eigen/Core>  // NOLINT (cpplint cannot handle include order here)
 
-#include "tf2/buffer_core.hpp"
+#include "tf3/buffer_core.h"
-#include "sensor_msgs/msg/laser_scan.hpp"
+#include <robot_sensor_msgs/LaserScan.h>
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <robot_sensor_msgs/PointCloud2.h>
-#include "laser_geometry/visibility_control.hpp"
+#include <robot_geometry_msgs/TransformStamped.h>
+#include "robot_laser_geometry/visibility_control.hpp"
 
-namespace laser_geometry
+namespace robot_laser_geometry
 {
 // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
 const float LASER_SCAN_INVALID = -1.0;
@@ -95,7 +96,7 @@ enum ChannelOption
  */
 
 // TODO(Martin-Idel-SI): the support for PointCloud1 has been removed for now.
-// Refer to the GitHub issue #29: https://github.com/ros-perception/laser_geometry/issues/29
+// Refer to the GitHub issue #29: https://github.com/ros-perception/robot_laser_geometry/issues/29
 
 class LaserProjection
 {
@@ -103,7 +104,7 @@ public:
   LaserProjection()
   : angle_min_(0), angle_max_(0) {}
 
-  // Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2
+  // Project a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::PointCloud2
   /*!
    * Project a single laser scan from a linear array into a 3D
    * point cloud.  The generated cloud will be in the same frame
@@ -119,17 +120,17 @@ public:
    *   channel_option::Intensity, channel_option::Index,
    *   channel_option::Distance, channel_option::Timestamp.
    */
-  LASER_GEOMETRY_PUBLIC
+  ROBOT_LASER_GEOMETRY_PUBLIC
   void projectLaser(
-    const sensor_msgs::msg::LaserScan & scan_in,
+    const robot_sensor_msgs::LaserScan & scan_in,
-    sensor_msgs::msg::PointCloud2 & cloud_out,
+    robot_sensor_msgs::PointCloud2 & cloud_out,
     double range_cutoff = -1.0,
     int channel_options = channel_option::Default)
   {
     projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
   }
 
-  // Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame
+  // Transform a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::PointCloud2 in target frame
   /*!
    * Transform a single laser scan from a linear array into a 3D
    * point cloud, accounting for movement of the laser over the
@@ -141,7 +142,7 @@ public:
    * \param target_frame The frame of the resulting point cloud
    * \param scan_in The input laser scan
    * \param cloud_out The output point cloud
-   * \param tf a tf2::BufferCore object to use to perform the
+   * \param tf a tf3::BufferCore object to use to perform the
    *   transform
    * \param range_cutoff An additional range cutoff which can be
    *   applied to discard everything above it.
@@ -151,12 +152,12 @@ public:
    *   channel_option::Intensity, channel_option::Index,
    *   channel_option::Distance, channel_option::Timestamp.
    */
-  LASER_GEOMETRY_PUBLIC
+  ROBOT_LASER_GEOMETRY_PUBLIC
   void transformLaserScanToPointCloud(
     const std::string & target_frame,
-    const sensor_msgs::msg::LaserScan & scan_in,
+    const robot_sensor_msgs::LaserScan & scan_in,
-    sensor_msgs::msg::PointCloud2 & cloud_out,
+    robot_sensor_msgs::PointCloud2 & cloud_out,
-    tf2::BufferCore & tf,
+    tf3::BufferCore & tf,
     double range_cutoff = -1.0,
     int channel_options = channel_option::Default)
   {
@@ -167,29 +168,29 @@ public:
 private:
   // Internal hidden representation of projectLaser
   void projectLaser_(
-    const sensor_msgs::msg::LaserScan & scan_in,
+    const robot_sensor_msgs::LaserScan & scan_in,
-    sensor_msgs::msg::PointCloud2 & cloud_out,
+    robot_sensor_msgs::PointCloud2 & cloud_out,
     double range_cutoff,
     int channel_options);
 
   // Internal hidden representation of transformLaserScanToPointCloud2
   void transformLaserScanToPointCloud_(
     const std::string & target_frame,
-    const sensor_msgs::msg::LaserScan & scan_in,
+    const robot_sensor_msgs::LaserScan & scan_in,
-    sensor_msgs::msg::PointCloud2 & cloud_out,
+    robot_sensor_msgs::PointCloud2 & cloud_out,
-    tf2::BufferCore & tf,
+    tf3::BufferCore & tf,
     double range_cutoff,
     int channel_options);
 
   // Function used by the several forms of transformLaserScanToPointCloud_
   void transformLaserScanToPointCloud_(
     const std::string & target_frame,
-    const sensor_msgs::msg::LaserScan & scan_in,
+    const robot_sensor_msgs::LaserScan & scan_in,
-    sensor_msgs::msg::PointCloud2 & cloud_out,
+    robot_sensor_msgs::PointCloud2 & cloud_out,
-    tf2::Quaternion quat_start,
+    tf3::Quaternion quat_start,
-    tf2::Vector3 origin_start,
+    tf3::Vector3 origin_start,
-    tf2::Quaternion quat_end,
+    tf3::Quaternion quat_end,
-    tf2::Vector3 origin_end,
+    tf3::Vector3 origin_end,
     double range_cutoff,
     int channel_options);
 
@@ -199,6 +200,6 @@ private:
   Eigen::ArrayXXd co_sine_map_;
 };
 
-}  // namespace laser_geometry
+}  // namespace robot_laser_geometry
 
-#endif  // LASER_GEOMETRY__LASER_GEOMETRY_HPP_
+#endif  // ROBOT_LASER_GEOMETRY__ROBOT_LASER_GEOMETRY_HPP_

include/robot_laser_geometry/visibility_control.hpp

@@ -27,38 +27,38 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#ifndef LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_
+#ifndef ROBOT_LASER_GEOMETRY_VISIBILITY_CONTROL_HPP_
-#define LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_
+#define ROBOT_LASER_GEOMETRY_VISIBILITY_CONTROL_HPP_
 
 // This logic was borrowed (then namespaced) from the examples on the gcc wiki:
 //     https://gcc.gnu.org/wiki/Visibility
 
 #if defined _WIN32 || defined __CYGWIN__
   #ifdef __GNUC__
-    #define LASER_GEOMETRY_EXPORT __attribute__ ((dllexport))
+    #define ROBOT_LASER_GEOMETRY_EXPORT __attribute__ ((dllexport))
-    #define LASER_GEOMETRY_IMPORT __attribute__ ((dllimport))
+    #define ROBOT_LASER_GEOMETRY_IMPORT __attribute__ ((dllimport))
   #else
-    #define LASER_GEOMETRY_EXPORT __declspec(dllexport)
+    #define ROBOT_LASER_GEOMETRY_EXPORT __declspec(dllexport)
-    #define LASER_GEOMETRY_IMPORT __declspec(dllimport)
+    #define ROBOT_LASER_GEOMETRY_IMPORT __declspec(dllimport)
   #endif
-  #ifdef LASER_GEOMETRY_BUILDING_LIBRARY
+  #ifdef ROBOT_LASER_GEOMETRY_BUILDING_LIBRARY
-    #define LASER_GEOMETRY_PUBLIC LASER_GEOMETRY_EXPORT
+    #define ROBOT_LASER_GEOMETRY_PUBLIC ROBOT_LASER_GEOMETRY_EXPORT
   #else
-    #define LASER_GEOMETRY_PUBLIC LASER_GEOMETRY_IMPORT
+    #define ROBOT_LASER_GEOMETRY_PUBLIC ROBOT_LASER_GEOMETRY_IMPORT
   #endif
-  #define LASER_GEOMETRY_PUBLIC_TYPE LASER_GEOMETRY_PUBLIC
+  #define ROBOT_LASER_GEOMETRY_PUBLIC_TYPE ROBOT_LASER_GEOMETRY_PUBLIC
-  #define LASER_GEOMETRY_LOCAL
+  #define ROBOT_LASER_GEOMETRY_LOCAL
 #else
-  #define LASER_GEOMETRY_EXPORT __attribute__ ((visibility("default")))
+  #define ROBOT_LASER_GEOMETRY_EXPORT __attribute__ ((visibility("default")))
-  #define LASER_GEOMETRY_IMPORT
+  #define ROBOT_LASER_GEOMETRY_IMPORT
   #if __GNUC__ >= 4
-    #define LASER_GEOMETRY_PUBLIC __attribute__ ((visibility("default")))
+    #define ROBOT_LASER_GEOMETRY_PUBLIC __attribute__ ((visibility("default")))
-    #define LASER_GEOMETRY_LOCAL  __attribute__ ((visibility("hidden")))
+    #define ROBOT_LASER_GEOMETRY_LOCAL  __attribute__ ((visibility("hidden")))
   #else
-    #define LASER_GEOMETRY_PUBLIC
+    #define ROBOT_LASER_GEOMETRY_PUBLIC
-    #define LASER_GEOMETRY_LOCAL
+    #define ROBOT_LASER_GEOMETRY_LOCAL
   #endif
-  #define LASER_GEOMETRY_PUBLIC_TYPE
+  #define ROBOT_LASER_GEOMETRY_PUBLIC_TYPE
 #endif
 
-#endif  // LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_
+#endif  // ROBOT_LASER_GEOMETRY__VISIBILITY_CONTROL_HPP_

package.xml

@@ -1,55 +1,34 @@
-<?xml version="1.0"?>
+<package>
-<?xml-model
+  <name>robot_laser_geometry</name>
-  href="http://download.ros.org/schema/package_format2.xsd"
+  <version>0.7.10</version>
-  schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="2">
-  <name>laser_geometry</name>
-  <version>2.10.0</version>
   <description>
-    This package contains a class for converting from a 2D laser scan as defined by
+    robot_laser_geometry is the second generation of the transform library, which lets
-    sensor_msgs/LaserScan into a point cloud as defined by sensor_msgs/PointCloud
+    the user keep track of multiple coordinate frames over time. robot_laser_geometry
-    or sensor_msgs/PointCloud2. In particular, it contains functionality to account
+    maintains the relationship between coordinate frames in a tree
-    for the skew resulting from moving robots or tilting laser scanners.
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
   </description>
-
+  <author>Tully Foote</author>
-  <maintainer email="dharini@openrobotics.org">Dharini Dutia</maintainer>
+  <author>Eitan Marder-Eppstein</author>
-
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
   <license>BSD</license>
 
-  <url type="website">http://github.com/ros-perception/laser_geometry</url>
+  <url type="website">http://www.ros.org/wiki/robot_laser_geometry</url>
     
-  <author email="dave.hershberger@sri.com">Dave Hershberger</author>
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
-  <author email="mabel@openrobotics.org">Mabel Zhang</author>
-  <author>Radu Bogdan Rusu</author>
-  <author>Tully Foote</author>
-  <author email="william@osrfoundation.org">William Woodall</author>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>robot_sensor_msgs</build_depend>
-  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <run_depend>robot_sensor_msgs</run_depend>
-  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
-  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <run_depend>geometry_msgs</run_depend>
 
-  <build_depend>eigen</build_depend>
+  <build_depend>robot_time</build_depend>
-  <build_depend>rclcpp</build_depend>
+  <run_depend>robot_time</run_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>tf2</build_depend>
 
-  <build_export_depend>eigen</build_export_depend>
+  <build_depend>data_convert</build_depend>
+  <run_depend>data_convert</run_depend>
 
-  <exec_depend>python3-numpy</exec_depend>
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>rclpy</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>sensor_msgs_py</exec_depend>
-  <exec_depend>tf2</exec_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_cmake_pytest</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
 </package>

pytest.ini

@@ -1,3 +0,0 @@
-; Needed to suppress warnings from pytest about the default junit_family
-[pytest]
-junit_family=xunit2

src/laser_geometry.cpp

@@ -28,24 +28,28 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#include "laser_geometry/laser_geometry.hpp"
+#include "robot_laser_geometry/laser_geometry.hpp"
+#include "data_convert/data_convert.h"
 
 #include <Eigen/Core>
 
 #include <algorithm>
 #include <string>
 
-#include "rclcpp/time.hpp"
+#include <robot/time.h>
-#include "sensor_msgs/msg/laser_scan.hpp"
+#include <robot/duration.h>
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <robot_sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/PointCloud2.h>
 
-#include "tf2/LinearMath/Transform.hpp"
+#include <tf3/buffer_core.h>
+#include<tf3/convert.h>
+#include "tf3/LinearMath/Transform.h"
 
-namespace laser_geometry
+namespace robot_laser_geometry
 {
 void LaserProjection::projectLaser_(
-  const sensor_msgs::msg::LaserScan & scan_in,
+  const robot_sensor_msgs::LaserScan & scan_in,
-  sensor_msgs::msg::PointCloud2 & cloud_out,
+  robot_sensor_msgs::PointCloud2 & cloud_out,
   double range_cutoff,
   int channel_options)
 {
@@ -85,15 +89,15 @@ void LaserProjection::projectLaser_(
   cloud_out.fields.resize(3);
   cloud_out.fields[0].name = "x";
   cloud_out.fields[0].offset = 0;
-  cloud_out.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
+  cloud_out.fields[0].datatype = robot_sensor_msgs::PointField::FLOAT32;
   cloud_out.fields[0].count = 1;
   cloud_out.fields[1].name = "y";
   cloud_out.fields[1].offset = 4;
-  cloud_out.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
+  cloud_out.fields[1].datatype = robot_sensor_msgs::PointField::FLOAT32;
   cloud_out.fields[1].count = 1;
   cloud_out.fields[2].name = "z";
   cloud_out.fields[2].offset = 8;
-  cloud_out.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
+  cloud_out.fields[2].datatype = robot_sensor_msgs::PointField::FLOAT32;
   cloud_out.fields[2].count = 1;
 
   // Define 4 indices in the channel array for each possible value type
@@ -106,7 +110,7 @@ void LaserProjection::projectLaser_(
     size_t field_size = cloud_out.fields.size();
     cloud_out.fields.resize(field_size + 1);
     cloud_out.fields[field_size].name = "intensity";
-    cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
     cloud_out.fields[field_size].offset = offset;
     cloud_out.fields[field_size].count = 1;
     offset += 4;
@@ -117,7 +121,7 @@ void LaserProjection::projectLaser_(
     size_t field_size = cloud_out.fields.size();
     cloud_out.fields.resize(field_size + 1);
     cloud_out.fields[field_size].name = "index";
-    cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::INT32;
+    cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::INT32;
     cloud_out.fields[field_size].offset = offset;
     cloud_out.fields[field_size].count = 1;
     offset += 4;
@@ -128,7 +132,7 @@ void LaserProjection::projectLaser_(
     size_t field_size = cloud_out.fields.size();
     cloud_out.fields.resize(field_size + 1);
     cloud_out.fields[field_size].name = "distances";
-    cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
     cloud_out.fields[field_size].offset = offset;
     cloud_out.fields[field_size].count = 1;
     offset += 4;
@@ -139,7 +143,7 @@ void LaserProjection::projectLaser_(
     size_t field_size = cloud_out.fields.size();
     cloud_out.fields.resize(field_size + 1);
     cloud_out.fields[field_size].name = "stamps";
-    cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
     cloud_out.fields[field_size].offset = offset;
     cloud_out.fields[field_size].count = 1;
     offset += 4;
@@ -151,19 +155,19 @@ void LaserProjection::projectLaser_(
     cloud_out.fields.resize(field_size + 3);
 
     cloud_out.fields[field_size].name = "vp_x";
-    cloud_out.fields[field_size].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
     cloud_out.fields[field_size].offset = offset;
     cloud_out.fields[field_size].count = 1;
     offset += 4;
 
     cloud_out.fields[field_size + 1].name = "vp_y";
-    cloud_out.fields[field_size + 1].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    cloud_out.fields[field_size + 1].datatype = robot_sensor_msgs::PointField::FLOAT32;
     cloud_out.fields[field_size + 1].offset = offset;
     cloud_out.fields[field_size + 1].count = 1;
     offset += 4;
 
     cloud_out.fields[field_size + 2].name = "vp_z";
-    cloud_out.fields[field_size + 2].datatype = sensor_msgs::msg::PointField::FLOAT32;
+    cloud_out.fields[field_size + 2].datatype = robot_sensor_msgs::PointField::FLOAT32;
     cloud_out.fields[field_size + 2].offset = offset;
     cloud_out.fields[field_size + 2].count = 1;
     offset += 4;
@@ -265,12 +269,12 @@ void LaserProjection::projectLaser_(
 
 void LaserProjection::transformLaserScanToPointCloud_(
   const std::string & target_frame,
-  const sensor_msgs::msg::LaserScan & scan_in,
+  const robot_sensor_msgs::LaserScan & scan_in,
-  sensor_msgs::msg::PointCloud2 & cloud_out,
+  robot_sensor_msgs::PointCloud2 & cloud_out,
-  tf2::Quaternion quat_start,
+  tf3::Quaternion quat_start,
-  tf2::Vector3 origin_start,
+  tf3::Vector3 origin_start,
-  tf2::Quaternion quat_end,
+  tf3::Quaternion quat_end,
-  tf2::Vector3 origin_end,
+  tf3::Vector3 origin_end,
   double range_cutoff,
   int channel_options)
 {
@@ -314,7 +318,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
 
   cloud_out.header.frame_id = target_frame;
 
-  tf2::Transform cur_transform;
+  tf3::Transform cur_transform;
 
   double ranges_norm = 1 / (static_cast<double>(scan_in.ranges.size()) - 1.0);
 
@@ -333,15 +337,15 @@ void LaserProjection::transformLaserScanToPointCloud_(
     // TODO(anon): Make a function that performs both the slerp and linear interpolation needed to
     // interpolate a Full Transform (Quaternion + Vector)
     // Interpolate translation
-    tf2::Vector3 v(0, 0, 0);
+    tf3::Vector3 v(0, 0, 0);
     v.setInterpolate3(origin_start, origin_end, ratio);
     cur_transform.setOrigin(v);
 
     // Compute the slerp-ed rotation
     cur_transform.setRotation(slerp(quat_start, quat_end, ratio));
 
-    tf2::Vector3 point_in(pstep[0], pstep[1], pstep[2]);
+    tf3::Vector3 point_in(pstep[0], pstep[1], pstep[2]);
-    tf2::Vector3 point_out = cur_transform * point_in;
+    tf3::Vector3 point_out = cur_transform * point_in;
 
     // Copy transformed point into cloud
     pstep[0] = static_cast<float>(point_out.x());
@@ -352,7 +356,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
     if (has_viewpoint) {
       auto vpstep =
         reinterpret_cast<float *>(&cloud_out.data[i * cloud_out.point_step + vp_x_offset]);
-      point_in = tf2::Vector3(vpstep[0], vpstep[1], vpstep[2]);
+      point_in = tf3::Vector3(vpstep[0], vpstep[1], vpstep[2]);
       point_out = cur_transform * point_in;
 
       // Copy transformed point into cloud
@@ -364,7 +368,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
 
   // if the user didn't request the index field, then we need to copy the PointCloud and drop it
   if (!requested_index) {
-    sensor_msgs::msg::PointCloud2 cloud_without_index;
+    robot_sensor_msgs::PointCloud2 cloud_without_index;
 
     // copy basic meta data
     cloud_without_index.header = cloud_out.header;
@@ -415,44 +419,59 @@ void LaserProjection::transformLaserScanToPointCloud_(
 
 void LaserProjection::transformLaserScanToPointCloud_(
   const std::string & target_frame,
-  const sensor_msgs::msg::LaserScan & scan_in,
+  const robot_sensor_msgs::LaserScan & scan_in,
-  sensor_msgs::msg::PointCloud2 & cloud_out,
+  robot_sensor_msgs::PointCloud2 & cloud_out,
-  tf2::BufferCore & tf,
+  tf3::BufferCore & tf,
   double range_cutoff,
   int channel_options)
 {
-  rclcpp::Time start_time = scan_in.header.stamp;
+  robot::Time start_time;
-  rclcpp::Time end_time = scan_in.header.stamp;
+  start_time.sec = scan_in.header.stamp.sec;
+  start_time.nsec = scan_in.header.stamp.nsec;
+  robot::Time end_time = start_time;
+  
   // TODO(anonymous): reconcile all the different time constructs
   if (!scan_in.ranges.empty()) {
-    end_time = start_time + rclcpp::Duration::from_seconds(
+    robot::Duration duration(static_cast<double>(scan_in.ranges.size() - 1) * static_cast<double>(scan_in.time_increment));
-      static_cast<double>(scan_in.ranges.size() - 1) * static_cast<double>(scan_in.time_increment));
+    end_time = start_time + duration;
   }
 
-  std::chrono::nanoseconds start(start_time.nanoseconds());
+  tf3::Time st = data_convert::convertTime(start_time);
-  std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> st(start);
+  tf3::Time e = data_convert::convertTime(end_time);
-  geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform(
+
+  tf3::TransformStampedMsg start_transform_msg = tf.lookupTransform(
     target_frame, scan_in.header.frame_id, st);
-  std::chrono::nanoseconds end(end_time.nanoseconds());
+  
-  std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> e(end);
+  tf3::TransformStampedMsg end_transform_msg = tf.lookupTransform(
-  geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform(
     target_frame, scan_in.header.frame_id, e);
 
-  tf2::Quaternion quat_start(start_transform.transform.rotation.x,
+  robot_geometry_msgs::TransformStamped start_transform = data_convert::convertToTransformStamped(start_transform_msg);
+  robot_geometry_msgs::TransformStamped end_transform = data_convert::convertToTransformStamped(end_transform_msg);
+
+  tf3::Quaternion quat_start(start_transform.transform.rotation.x,
     start_transform.transform.rotation.y,
     start_transform.transform.rotation.z,
     start_transform.transform.rotation.w);
-  tf2::Quaternion quat_end(end_transform.transform.rotation.x,
+  tf3::Quaternion quat_end(end_transform.transform.rotation.x,
     end_transform.transform.rotation.y,
     end_transform.transform.rotation.z,
     end_transform.transform.rotation.w);
 
-  tf2::Vector3 origin_start(start_transform.transform.translation.x,
+  tf3::Vector3 origin_start(start_transform.transform.translation.x,
     start_transform.transform.translation.y,
     start_transform.transform.translation.z);
-  tf2::Vector3 origin_end(end_transform.transform.translation.x,
+  tf3::Vector3 origin_end(end_transform.transform.translation.x,
     end_transform.transform.translation.y,
     end_transform.transform.translation.z);
+
+  if(target_frame == scan_in.header.frame_id)
+  {
+    quat_start = tf3::Quaternion(0, 0, 0, 1);
+    quat_end = tf3::Quaternion(0, 0, 0, 1);
+    origin_start = tf3::Vector3(0, 0, 0);
+    origin_end = tf3::Vector3( 0, 0, 0);
+  }
+  
   transformLaserScanToPointCloud_(
     target_frame, scan_in, cloud_out,
     quat_start, origin_start,
@@ -461,4 +480,4 @@ void LaserProjection::transformLaserScanToPointCloud_(
     channel_options);
 }
 
-}  // namespace laser_geometry
+}  // namespace robot_laser_geometry

src/laser_geometry/__init__.py

@@ -27,4 +27,4 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-from .laser_geometry import LaserProjection  # noqa: F401
+from .robot_laser_geometry import LaserProjection  # noqa: F401

src/laser_geometry/laser_geometry.py

@@ -30,8 +30,8 @@
 import numpy as np
 import rclpy
 import rclpy.logging
-from sensor_msgs.msg import PointCloud2
+from robot_sensor_msgs.msg import PointCloud2
-import sensor_msgs_py.point_cloud2 as pc2
+import robot_sensor_msgs_py.point_cloud2 as pc2
 
 
 class LaserProjection:
@@ -86,7 +86,7 @@ class LaserProjection:
             self, scan_in,
             range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT):
         """
-        Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2.
+        Project a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::PointCloud2.
 
         Project a single laser scan from a linear array into a 3D
         point cloud. The generated cloud will be in the same frame

test/projection_test.cpp

@@ -33,10 +33,12 @@
 #include <cmath>
 #include <vector>
 
-#include "rclcpp/rclcpp.hpp"
+#include <robot/time.h>
+#include <robot/duration.h>
 
-#include "laser_geometry/laser_geometry.hpp"
+#include "robot_laser_geometry/laser_geometry.hpp"
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/LaserScan.h>
 
 #define PROJECTION_TEST_RANGE_MIN (0.23f)
 #define PROJECTION_TEST_RANGE_MAX (40.0f)
@@ -54,12 +56,12 @@ struct ScanOptions
   float ang_min_;
   float ang_max_;
   float ang_increment_;
-  rclcpp::Duration scan_time_;
+  robot::Duration scan_time_;
 
   ScanOptions(
     float range, float intensity,
     float ang_min, float ang_max, float ang_increment,
-    rclcpp::Duration scan_time)
+    robot::Duration scan_time)
   : range_(range),
     intensity_(intensity),
     ang_min_(ang_min),
@@ -68,19 +70,21 @@ struct ScanOptions
     scan_time_(scan_time) {}
 };
 
-sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
+robot_sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options)
 {
   if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) {
     throw (BuildScanException());
   }
 
-  sensor_msgs::msg::LaserScan scan;
+  robot_sensor_msgs::LaserScan scan;
-  scan.header.stamp = rclcpp::Clock().now();
+  robot::Time now = robot::Time::now();
+  scan.header.stamp.sec = now.sec;
+  scan.header.stamp.nsec = now.nsec;
   scan.header.frame_id = "laser_frame";
   scan.angle_min = options.ang_min_;
   scan.angle_max = options.ang_max_;
   scan.angle_increment = options.ang_increment_;
-  scan.scan_time = static_cast<float>(options.scan_time_.nanoseconds());
+  scan.scan_time = static_cast<float>(options.scan_time_.toSec());
   scan.range_min = PROJECTION_TEST_RANGE_MIN;
   scan.range_max = PROJECTION_TEST_RANGE_MAX;
   uint32_t i = 0;
@@ -90,23 +94,23 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
   }
 
   scan.time_increment =
-    static_cast<float>(options.scan_time_.nanoseconds() / static_cast<double>(i));
+    static_cast<float>(options.scan_time_.toSec() / static_cast<double>(i));
 
   return scan;
 }
 
 template<typename T>
-T cloudData(sensor_msgs::msg::PointCloud2 cloud_out, uint32_t index)
+T cloudData(robot_sensor_msgs::PointCloud2 cloud_out, uint32_t index)
 {
   return *reinterpret_cast<T *>(&cloud_out.data[index]);
 }
 
-TEST(laser_geometry, projectLaser2) {
+TEST(robot_laser_geometry, projectLaser2) {
   double tolerance = 1e-12;
-  laser_geometry::LaserProjection projector;
+  robot_laser_geometry::LaserProjection projector;
 
   std::vector<float> ranges, intensities, min_angles, max_angles, angle_increments;
-  std::vector<rclcpp::Duration> increment_times, scan_times;
+  std::vector<robot::Duration> increment_times, scan_times;
 
   ranges.push_back(-1.0f);
   ranges.push_back(1.0f);
@@ -129,8 +133,8 @@ TEST(laser_geometry, projectLaser2) {
   angle_increments.push_back(PI / 180);  // one degree
   angle_increments.push_back(PI / 720);  // quarter degree
 
-  scan_times.push_back(rclcpp::Duration::from_seconds(1. / 40));
+  scan_times.push_back(robot::Duration(1. / 40));
-  scan_times.push_back(rclcpp::Duration::from_seconds(1. / 20));
+  scan_times.push_back(robot::Duration(1. / 20));
 
   std::vector<ScanOptions> options;
   for (auto range : ranges) {
@@ -153,34 +157,34 @@ TEST(laser_geometry, projectLaser2) {
     try {
       // printf("%f %f %f %f %f %f\n",
       //   range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
-      sensor_msgs::msg::LaserScan scan = build_constant_scan(option);
+      robot_sensor_msgs::LaserScan scan = build_constant_scan(option);
 
-      sensor_msgs::msg::PointCloud2 cloud_out;
+      robot_sensor_msgs::PointCloud2 cloud_out;
-      projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
+      projector.projectLaser(scan, cloud_out, -1.0, robot_laser_geometry::channel_option::Index);
       EXPECT_EQ(cloud_out.fields.size(), 4u);
-      projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
+      projector.projectLaser(scan, cloud_out, -1.0, robot_laser_geometry::channel_option::Intensity);
       EXPECT_EQ(cloud_out.fields.size(), 4u);
 
       projector.projectLaser(scan, cloud_out, -1.0);
       EXPECT_EQ(cloud_out.fields.size(), 5u);
       projector.projectLaser(
         scan, cloud_out, -1.0,
-        laser_geometry::channel_option::Intensity |
+        robot_laser_geometry::channel_option::Intensity |
-        laser_geometry::channel_option::Index);
+        robot_laser_geometry::channel_option::Index);
       EXPECT_EQ(cloud_out.fields.size(), 5u);
 
       projector.projectLaser(
         scan, cloud_out, -1.0,
-        laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
+        robot_laser_geometry::channel_option::Intensity | robot_laser_geometry::channel_option::Index |
-        laser_geometry::channel_option::Distance);
+        robot_laser_geometry::channel_option::Distance);
       EXPECT_EQ(cloud_out.fields.size(), 6u);
 
       projector.projectLaser(
         scan, cloud_out, -1.0,
-        laser_geometry::channel_option::Intensity |
+        robot_laser_geometry::channel_option::Intensity |
-        laser_geometry::channel_option::Index |
+        robot_laser_geometry::channel_option::Index |
-        laser_geometry::channel_option::Distance |
+        robot_laser_geometry::channel_option::Distance |
-        laser_geometry::channel_option::Timestamp);
+        robot_laser_geometry::channel_option::Timestamp);
       EXPECT_EQ(cloud_out.fields.size(), 7u);
 
       unsigned int valid_points = 0;
@@ -201,7 +205,7 @@ TEST(laser_geometry, projectLaser2) {
       uint32_t index_offset = 0;
       uint32_t distance_offset = 0;
       uint32_t stamps_offset = 0;
-      for (std::vector<sensor_msgs::msg::PointField>::iterator f = cloud_out.fields.begin();
+      for (std::vector<robot_sensor_msgs::PointField>::iterator f = cloud_out.fields.begin();
         f != cloud_out.fields.end(); f++)
       {
         if (f->name == "x") {x_offset = f->offset;}
@@ -251,14 +255,14 @@ TEST(laser_geometry, projectLaser2) {
 // TODO(Martin-Idel-SI): Reenable test if possible. Test fails due to lookupTransform failing
 // Needs to publish a transform to "laser_frame" in order to work.
 #if 0
-TEST(laser_geometry, transformLaserScanToPointCloud2) {
+TEST(robot_laser_geometry, transformLaserScanToPointCloud2) {
-  tf2::BufferCore tf2;
+  tf3::BufferCore tf3;
 
   double tolerance = 1e-12;
-  laser_geometry::LaserProjection projector;
+  robot_laser_geometry::LaserProjection projector;
 
   std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
-  std::vector<rclcpp::Duration> increment_times, scan_times;
+  std::vector<robot::Duration> increment_times, scan_times;
 
   ranges.push_back(-1.0);
   ranges.push_back(1.0);
@@ -291,8 +295,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
   angle_increments.push_back(M_PI / 360);  // half degree
   angle_increments.push_back(M_PI / 720);  // quarter degree
 
-  scan_times.push_back(rclcpp::Duration::from_seconds(1. / 40));
+  scan_times.push_back(robot::Duration(1. / 40));
-  scan_times.push_back(rclcpp::Duration::from_seconds(1. / 20));
+  scan_times.push_back(robot::Duration(1. / 20));
 
   std::vector<ScanOptions> options;
   for (auto range : ranges) {
@@ -315,43 +319,43 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
     try {
       // printf("%f %f %f %f %f %f\n",
       //   range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
-      sensor_msgs::msg::LaserScan scan = build_constant_scan(option);
+      robot_sensor_msgs::LaserScan scan = build_constant_scan(option);
 
       scan.header.frame_id = "laser_frame";
 
-      sensor_msgs::msg::PointCloud2 cloud_out;
+      robot_sensor_msgs::PointCloud2 cloud_out;
       projector.transformLaserScanToPointCloud(
-        scan.header.frame_id, scan, cloud_out, tf2, -1.0,
+        scan.header.frame_id, scan, cloud_out, tf3, -1.0,
-        laser_geometry::channel_option::None);
+        robot_laser_geometry::channel_option::None);
       EXPECT_EQ(cloud_out.fields.size(), 3u);
       projector.transformLaserScanToPointCloud(
-        scan.header.frame_id, scan, cloud_out, tf2, -1.0,
+        scan.header.frame_id, scan, cloud_out, tf3, -1.0,
-        laser_geometry::channel_option::Index);
+        robot_laser_geometry::channel_option::Index);
       EXPECT_EQ(cloud_out.fields.size(), 4u);
       projector.transformLaserScanToPointCloud(
-        scan.header.frame_id, scan, cloud_out, tf2, -1.0,
+        scan.header.frame_id, scan, cloud_out, tf3, -1.0,
-        laser_geometry::channel_option::Intensity);
+        robot_laser_geometry::channel_option::Intensity);
       EXPECT_EQ(cloud_out.fields.size(), 4u);
 
-      projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
+      projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf3);
       EXPECT_EQ(cloud_out.fields.size(), 5u);
       projector.transformLaserScanToPointCloud(
-        scan.header.frame_id, scan, cloud_out, tf2, -1.0,
+        scan.header.frame_id, scan, cloud_out, tf3, -1.0,
-        laser_geometry::channel_option::Intensity |
+        robot_laser_geometry::channel_option::Intensity |
-        laser_geometry::channel_option::Index);
+        robot_laser_geometry::channel_option::Index);
       EXPECT_EQ(cloud_out.fields.size(), 5u);
 
       projector.transformLaserScanToPointCloud(
-        scan.header.frame_id, scan, cloud_out, tf2, -1.0,
+        scan.header.frame_id, scan, cloud_out, tf3, -1.0,
-        laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
+        robot_laser_geometry::channel_option::Intensity | robot_laser_geometry::channel_option::Index |
-        laser_geometry::channel_option::Distance);
+        robot_laser_geometry::channel_option::Distance);
       EXPECT_EQ(cloud_out.fields.size(), 6u);
 
       projector.transformLaserScanToPointCloud(
-        scan.header.frame_id, scan, cloud_out, tf2, -1.0,
+        scan.header.frame_id, scan, cloud_out, tf3, -1.0,
-        laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
+        robot_laser_geometry::channel_option::Intensity | robot_laser_geometry::channel_option::Index |
-        laser_geometry::channel_option::Distance |
+        robot_laser_geometry::channel_option::Distance |
-        laser_geometry::channel_option::Timestamp);
+        robot_laser_geometry::channel_option::Timestamp);
       EXPECT_EQ(cloud_out.fields.size(), 7u);
 
       EXPECT_EQ(cloud_out.is_dense, false);
@@ -373,7 +377,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
       uint32_t index_offset = 0;
       uint32_t distance_offset = 0;
       uint32_t stamps_offset = 0;
-      for (std::vector<sensor_msgs::msg::PointField>::iterator f = cloud_out.fields.begin();
+      for (std::vector<robot_sensor_msgs::PointField>::iterator f = cloud_out.fields.begin();
         f != cloud_out.fields.end(); f++)
       {
         if (f->name == "x") {x_offset = f->offset;}

test/projection_test.py

@@ -1,156 +0,0 @@
-# Copyright (c) 2014, Enrique Fernandez
-# 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 copyright holder 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 itertools import product
-
-from laser_geometry import LaserProjection
-import numpy as np
-import pytest
-import rclpy
-import rclpy.duration
-import rclpy.time
-from sensor_msgs.msg import LaserScan
-import sensor_msgs_py.point_cloud2 as pc2
-
-PROJECTION_TEST_RANGE_MIN = 0.23
-PROJECTION_TEST_RANGE_MAX = 40.00
-
-
-class BuildScanException(Exception):
-    pass
-
-
-def build_constant_scan(
-        range_val, intensity_val,
-        angle_min, angle_max, angle_increment, scan_time):
-    count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
-    if count < 0:
-        raise BuildScanException
-
-    scan = LaserScan()
-    scan.header.stamp = rclpy.time.Time(seconds=10.10).to_msg()
-    scan.header.frame_id = 'laser_frame'
-    scan.angle_min = angle_min
-    scan.angle_max = angle_max
-    scan.angle_increment = angle_increment
-    scan.scan_time = scan_time.nanoseconds / 1e9
-    scan.range_min = PROJECTION_TEST_RANGE_MIN
-    scan.range_max = PROJECTION_TEST_RANGE_MAX
-    scan.ranges = [range_val for _ in range(count)]
-    scan.intensities = [intensity_val for _ in range(count)]
-    scan.time_increment = scan_time.nanoseconds / 1e9 / count
-
-    return scan
-
-
-def test_project_laser():
-    tolerance = 6  # decimal places
-    projector = LaserProjection()
-
-    ranges = [-1.0, 1.0, 5.0, 100.0]
-    intensities = np.arange(1.0, 4.0).tolist()
-
-    min_angles = -np.pi / np.array([1.0, 1.5, 8.0])
-    max_angles = -min_angles
-
-    angle_increments = np.pi / np.array([180., 360., 720.])
-
-    scan_times = [rclpy.duration.Duration(seconds=1./i) for i in [40, 20]]
-
-    for range_val, intensity_val, \
-        angle_min, angle_max, angle_increment, scan_time in \
-        product(
-            ranges, intensities,
-            min_angles, max_angles,
-            angle_increments, scan_times):
-        try:
-            scan = build_constant_scan(
-                range_val, intensity_val,
-                angle_min, angle_max, angle_increment, scan_time)
-        except BuildScanException:
-            assert (angle_max - angle_min)/angle_increment <= 0
-
-        cloud_out = projector.projectLaser(
-            scan, -1.0,
-            LaserProjection.ChannelOption.INTENSITY |
-            LaserProjection.ChannelOption.INDEX |
-            LaserProjection.ChannelOption.DISTANCE |
-            LaserProjection.ChannelOption.TIMESTAMP)
-        assert len(cloud_out.fields) == 7, 'PointCloud2 with channel INDEX: fields size != 7'
-
-        valid_points = 0
-        for i in range(len(scan.ranges)):
-            ri = scan.ranges[i]
-            if (PROJECTION_TEST_RANGE_MIN <= ri and ri <= PROJECTION_TEST_RANGE_MAX):
-                valid_points += 1
-
-        assert valid_points == cloud_out.width, 'Valid points != PointCloud2 width'
-
-        idx_x = idx_y = idx_z = 0
-        idx_intensity = idx_index = 0
-        idx_distance = idx_stamps = 0
-
-        i = 0
-        for f in cloud_out.fields:
-            if f.name == 'x':
-                idx_x = i
-            elif f.name == 'y':
-                idx_y = i
-            elif f.name == 'z':
-                idx_z = i
-            elif f.name == 'intensity':
-                idx_intensity = i
-            elif f.name == 'index':
-                idx_index = i
-            elif f.name == 'distances':
-                idx_distance = i
-            elif f.name == 'stamps':
-                idx_stamps = i
-            i += 1
-
-        i = 0
-        for point in pc2.read_points(cloud_out):
-            ri = scan.ranges[i]
-            ai = scan.angle_min + i * scan.angle_increment
-
-            assert point[idx_x] == pytest.approx(ri * np.cos(ai), abs=tolerance), 'x not equal'
-            assert point[idx_y] == pytest.approx(ri * np.sin(ai), tolerance), 'y not equal'
-            assert point[idx_z] == pytest.approx(0, tolerance), 'z not equal'
-            assert point[idx_intensity] == pytest.approx(
-                scan.intensities[i],
-                tolerance), 'Intensity not equal'
-            assert point[idx_index] == pytest.approx(i, tolerance), 'Index not equal'
-            assert point[idx_distance] == pytest.approx(ri, tolerance), 'Distance not equal'
-            assert point[idx_stamps] == pytest.approx(
-                i * scan.time_increment, tolerance), 'Timestamp not equal'
-            i += 1
-
-
-if __name__ == '__main__':
-    pytest.main()