diff --git a/CMakeLists.txt b/CMakeLists.txt index 28ab530..fd5332d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,47 +1,45 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) + project(laser_geometry) -find_package(catkin REQUIRED - COMPONENTS - angles - cmake_modules - roscpp - sensor_msgs - tf - tf2 -) +find_package(ament_cmake REQUIRED) -find_package(Boost REQUIRED COMPONENTS system thread) +find_package(angles REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(Eigen3 REQUIRED) -find_package(Eigen REQUIRED) +set(Boost_USE_STATIC_LIBS ON) +find_package(Boost REQUIRED) -catkin_python_setup() - -catkin_package( - INCLUDE_DIRS include - LIBRARIES laser_geometry - DEPENDS Boost Eigen -) +# TODO(dhood): enable python support once ported to ROS 2 +# catkin_python_setup() include_directories(include - ${catkin_INCLUDE_DIRS} + ${angles_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} ) add_library(laser_geometry src/laser_geometry.cpp) -target_link_libraries(laser_geometry ${Boost_LIBRARIES} ${tf_LIBRARIES}) +target_link_libraries(laser_geometry + ${angles_LIBRARIES} + ${rclcpp_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${tf2_LIBRARIES} +) -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(projection_test test/projection_test.cpp) - target_link_libraries(projection_test laser_geometry) - - catkin_add_nosetests(test/projection_test.py) -endif() +ament_export_include_directories(include) +ament_export_libraries(laser_geometry) +ament_package(CONFIG_EXTRAS laser_geometry-extras.cmake) install(TARGETS laser_geometry - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) install(DIRECTORY include/laser_geometry/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME}/ FILES_MATCHING PATTERN "*.h") diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index 9731898..fee95ce 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -37,15 +37,19 @@ #include "boost/numeric/ublas/matrix.hpp" #include "boost/thread/mutex.hpp" -#include #include +#include +#include +#include -#include "sensor_msgs/LaserScan.h" -#include "sensor_msgs/PointCloud.h" -#include "sensor_msgs/PointCloud.h" +#ifndef ROS_DEBUG +#define ROS_DEBUG(...) +#endif // !ROS_DEBUG +#ifndef ROS_ASSERT +#define ROS_ASSERT(...) +#endif // !ROS_ASSERT #include -#include namespace laser_geometry { @@ -106,7 +110,7 @@ namespace laser_geometry //! Destructor to deallocate stored unit vectors ~LaserProjection(); - //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud + //! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud /*! * Project a single laser scan from a linear array into a 3D * point cloud. The generated cloud will be in the same frame @@ -122,15 +126,15 @@ namespace laser_geometry * channel_option::Intensity, channel_option::Index, * channel_option::Distance, channel_option::Timestamp. */ - void projectLaser (const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud& cloud_out, + void projectLaser (const sensor_msgs::msg::LaserScan& scan_in, + sensor_msgs::msg::PointCloud& cloud_out, double range_cutoff = -1.0, int channel_options = channel_option::Default) { return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options); } - //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 + //! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 /*! * Project a single laser scan from a linear array into a 3D * point cloud. The generated cloud will be in the same frame @@ -146,16 +150,15 @@ namespace laser_geometry * channel_option::Intensity, channel_option::Index, * channel_option::Distance, channel_option::Timestamp. */ - void projectLaser (const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud2 &cloud_out, + void projectLaser (const sensor_msgs::msg::LaserScan& scan_in, + sensor_msgs::msg::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::LaserScan into a sensor_msgs::PointCloud in target frame + //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud 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 @@ -177,16 +180,16 @@ namespace laser_geometry * channel_option::Distance, channel_option::Timestamp. */ void transformLaserScanToPointCloud (const std::string& target_frame, - const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud& cloud_out, - tf::Transformer& tf, + const sensor_msgs::msg::LaserScan& scan_in, + sensor_msgs::msg::PointCloud& cloud_out, + tf2::BufferCore &tf, double range_cutoff, int channel_options = channel_option::Default) { return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options); } - //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame + //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud 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 @@ -206,47 +209,15 @@ namespace laser_geometry * channel_option::Distance, channel_option::Timestamp. */ void transformLaserScanToPointCloud (const std::string& target_frame, - const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud& cloud_out, - tf::Transformer& tf, + const sensor_msgs::msg::LaserScan& scan_in, + sensor_msgs::msg::PointCloud& cloud_out, + tf2::BufferCore &tf, int channel_options = channel_option::Default) { return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options); } - //! Transform a sensor_msgs::LaserScan into a 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 - * course of the scan. In order for this transform to be - * meaningful at a single point in time, the target_frame must - * be a fixed reference frame. See the tf documentation for - * more information on fixed frames. - * - * \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 tf::Transformer object to use to perform the - * transform - * \param range_cutoff An additional range cutoff which can be - * applied to discard everything above it. - * Defaults to -1.0, which means the laser scan max range. - * \param channel_option An OR'd set of channels to include. - * Options include: channel_option::Default, - * channel_option::Intensity, channel_option::Index, - * channel_option::Distance, channel_option::Timestamp. - */ - void transformLaserScanToPointCloud(const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, - tf::Transformer &tf, - double range_cutoff = -1.0, - int channel_options = channel_option::Default) - { - transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); - } - - //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame + //! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::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 @@ -269,8 +240,8 @@ namespace laser_geometry * channel_option::Distance, channel_option::Timestamp. */ void transformLaserScanToPointCloud(const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, + const sensor_msgs::msg::LaserScan &scan_in, + sensor_msgs::msg::PointCloud2 &cloud_out, tf2::BufferCore &tf, double range_cutoff = -1.0, int channel_options = channel_option::Default) @@ -294,46 +265,38 @@ namespace laser_geometry private: //! Internal hidden representation of projectLaser - void projectLaser_ (const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud& cloud_out, + void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, + sensor_msgs::msg::PointCloud& cloud_out, double range_cutoff, bool preservative, int channel_options); //! Internal hidden representation of projectLaser - void projectLaser_ (const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud2 &cloud_out, + void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, + sensor_msgs::msg::PointCloud2 &cloud_out, double range_cutoff, int channel_options); //! Internal hidden representation of transformLaserScanToPointCloud void transformLaserScanToPointCloud_ (const std::string& target_frame, - sensor_msgs::PointCloud& cloud_out, - const sensor_msgs::LaserScan& scan_in, - tf::Transformer & tf, + sensor_msgs::msg::PointCloud& cloud_out, + const sensor_msgs::msg::LaserScan& scan_in, + tf2::BufferCore &tf, double range_cutoff, int channel_options); //! Internal hidden representation of transformLaserScanToPointCloud2 void transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, - tf::Transformer &tf, - double range_cutoff, - int channel_options); - - //! Internal hidden representation of transformLaserScanToPointCloud2 - void transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, + const sensor_msgs::msg::LaserScan &scan_in, + sensor_msgs::msg::PointCloud2 &cloud_out, tf2::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::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, + const sensor_msgs::msg::LaserScan &scan_in, + sensor_msgs::msg::PointCloud2 &cloud_out, tf2::Quaternion quat_start, tf2::Vector3 origin_start, tf2::Quaternion quat_end, diff --git a/laser_geometry-extras.cmake b/laser_geometry-extras.cmake new file mode 100644 index 0000000..aa495ef --- /dev/null +++ b/laser_geometry-extras.cmake @@ -0,0 +1,31 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# copied from rclcpp/rclcpp-extras.cmake + +# register ament_package() hook for node plugins once + +find_package(Eigen3 REQUIRED) + +# Eigen3 uses non-standard variable for +# include dirs (case and name): EIGEN3_INCLUDE_DIR. +if(NOT Eigen3_INCLUDE_DIRS) + if (EIGEN3_INCLUDE_DIR) + message(STATUS "append ${EIGEN3_INCLUDE_DIR} to (${laser_geometry_INCLUDE_DIRS})") + list(APPEND laser_geometry_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) + message(STATUS "laser_geometry_INCLUDE_DIRS=${laser_geometry_INCLUDE_DIRS}") + else() + message(FATAL_ERROR "Eigen3_INCLUDE_DIRS not found") + endif() +endif() diff --git a/package.xml b/package.xml index 31573b9..fe0ecf5 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,6 @@ - + + + laser_geometry 1.6.4 @@ -16,23 +18,23 @@ http://ros.org/wiki/laser_geometry - catkin + ament_cmake angles - boost - cmake_modules - eigen - roscpp + Eigen3 + rclcpp sensor_msgs - tf + tf2 - angles - boost - eigen - python-numpy - roscpp - sensor_msgs - tf + angles + Eigen3 + rclcpp + sensor_msgs + tf2 - rosunit + ament_cmake + + + ament_cmake + diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 1e2e955..e6d4f9a 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -29,14 +29,21 @@ #include "laser_geometry/laser_geometry.h" #include -#include +#include "rclcpp/time.hpp" +#define TIME rclcpp::Time + +#define POINT_FIELD sensor_msgs::msg::PointField + +// TODO: fix definitions +typedef double tfScalar; + #include namespace laser_geometry { void - LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, + LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::PointCloud & cloud_out, double range_cutoff, bool preservative, int mask) { boost::numeric::ublas::matrix ranges(2, scan_in.ranges.size()); @@ -185,96 +192,8 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do } }; - void - LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in, - tf::Transformer& tf, double range_cutoff, int mask) - { - cloud_out.header = scan_in.header; - - tf::Stamped pointIn; - tf::Stamped pointOut; - - //check if the user has requested the index field - bool requested_index = false; - if ((mask & channel_option::Index)) - requested_index = true; - - //we need to make sure that we include the index in our mask - //in order to guarantee that we get our timestamps right - mask |= channel_option::Index; - - pointIn.frame_id_ = scan_in.header.frame_id; - - projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask); - - cloud_out.header.frame_id = target_frame; - - // Extract transforms for the beginning and end of the laser scan - ros::Time start_time = scan_in.header.stamp ; - ros::Time end_time = scan_in.header.stamp ; - if(!scan_in.ranges.empty()) end_time += ros::Duration().fromSec( (scan_in.ranges.size()-1) * scan_in.time_increment); - - tf::StampedTransform start_transform ; - tf::StampedTransform end_transform ; - tf::StampedTransform cur_transform ; - - tf.lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ; - tf.lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ; - - //we need to find the index of the index channel - int index_channel_idx = -1; - for(unsigned int i = 0; i < cloud_out.channels.size(); ++i) - { - if(cloud_out.channels[i].name == "index") - { - index_channel_idx = i; - break; - } - } - - //check just in case - ROS_ASSERT(index_channel_idx >= 0); - - for(unsigned int i = 0; i < cloud_out.points.size(); ++i) - { - //get the index for this point - uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i]; - - // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms - tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ; - - //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector) - - //Interpolate translation - tf::Vector3 v (0, 0, 0); - v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ; - cur_transform.setOrigin(v) ; - - //Interpolate rotation - tf::Quaternion q1, q2 ; - start_transform.getBasis().getRotation(q1) ; - end_transform.getBasis().getRotation(q2) ; - - // Compute the slerp-ed rotation - cur_transform.setRotation( slerp( q1, q2 , ratio) ) ; - - // Apply the transform to the current point - tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ; - tf::Vector3 pointOut = cur_transform * pointIn ; - - // Copy transformed point into cloud - cloud_out.points[i].x = pointOut.x(); - cloud_out.points[i].y = pointOut.y(); - cloud_out.points[i].z = pointOut.z(); - } - - //if the user didn't request the index, we want to remove it from the channels - if(!requested_index) - cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx); - } - - void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud2 &cloud_out, + void LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, + sensor_msgs::msg::PointCloud2 &cloud_out, double range_cutoff, int channel_options) { @@ -313,15 +232,15 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do cloud_out.fields.resize (3); cloud_out.fields[0].name = "x"; cloud_out.fields[0].offset = 0; - cloud_out.fields[0].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[0].datatype = POINT_FIELD::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::PointField::FLOAT32; + cloud_out.fields[1].datatype = POINT_FIELD::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::PointField::FLOAT32; + cloud_out.fields[2].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[2].count = 1; // Define 4 indices in the channel array for each possible value type @@ -334,7 +253,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do int 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::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -346,7 +265,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do int 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::PointField::INT32; + cloud_out.fields[field_size].datatype = POINT_FIELD::INT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -358,7 +277,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do int 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::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -370,7 +289,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do int 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::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].count = 1; offset += 4; @@ -383,19 +302,19 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do cloud_out.fields.resize(field_size + 3); cloud_out.fields[field_size].name = "vp_x"; - cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; + cloud_out.fields[field_size].datatype = POINT_FIELD::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::PointField::FLOAT32; + cloud_out.fields[field_size + 1].datatype = POINT_FIELD::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::PointField::FLOAT32; + cloud_out.fields[field_size + 2].datatype = POINT_FIELD::FLOAT32; cloud_out.fields[field_size + 2].offset = offset; cloud_out.fields[field_size + 2].count = 1; offset += 4; @@ -494,8 +413,8 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do } void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, + const sensor_msgs::msg::LaserScan &scan_in, + sensor_msgs::msg::PointCloud2 &cloud_out, tf2::Quaternion quat_start, tf2::Vector3 origin_start, tf2::Quaternion quat_end, @@ -596,7 +515,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do //if the user didn't request the index field, then we need to copy the PointCloud and drop it if(!requested_index) { - sensor_msgs::PointCloud2 cloud_without_index; + sensor_msgs::msg::PointCloud2 cloud_without_index; //copy basic meta data cloud_without_index.header = cloud_out.header; @@ -650,53 +569,26 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do } void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, - tf::Transformer &tf, - double range_cutoff, - int channel_options) - { - ros::Time start_time = scan_in.header.stamp; - ros::Time end_time = scan_in.header.stamp; - if(!scan_in.ranges.empty()) end_time += ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment); - - tf::StampedTransform start_transform, end_transform ; - - tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform); - tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform); - - tf::Quaternion q; - start_transform.getBasis().getRotation(q); - tf2::Quaternion quat_start(q.getX(), q.getY(), q.getZ(), q.getW()); - end_transform.getBasis().getRotation(q); - tf2::Quaternion quat_end(q.getX(), q.getY(), q.getZ(), q.getW()); - - tf2::Vector3 origin_start(start_transform.getOrigin().getX(), - start_transform.getOrigin().getY(), - start_transform.getOrigin().getZ()); - tf2::Vector3 origin_end(end_transform.getOrigin().getX(), - end_transform.getOrigin().getY(), - end_transform.getOrigin().getZ()); - transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, - quat_start, origin_start, - quat_end, origin_end, - range_cutoff, - channel_options); - } - - void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, + const sensor_msgs::msg::LaserScan &scan_in, + sensor_msgs::msg::PointCloud2 &cloud_out, tf2::BufferCore &tf, double range_cutoff, int channel_options) { - ros::Time start_time = scan_in.header.stamp; - ros::Time end_time = scan_in.header.stamp; - if(!scan_in.ranges.empty()) end_time += ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment); + TIME start_time = scan_in.header.stamp; + TIME end_time = scan_in.header.stamp; + // TODO: reconcile all the different time constructs + if (!scan_in.ranges.empty()) + { + end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0); + } - geometry_msgs::TransformStamped start_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time); - geometry_msgs::TransformStamped end_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time); + std::chrono::milliseconds start(start_time.nanoseconds()); + std::chrono::time_point st(start); + geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform(target_frame, scan_in.header.frame_id, st); + std::chrono::milliseconds end(end_time.nanoseconds()); + std::chrono::time_point e(end); + 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, start_transform.transform.rotation.y, diff --git a/test/projection_test.cpp b/test/projection_test.cpp index c0132a0..d664789 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -45,14 +45,14 @@ class BuildScanException { }; -sensor_msgs::LaserScan build_constant_scan(double range, double intensity, +sensor_msgs::msg::LaserScan build_constant_scan(double range, double intensity, double ang_min, double ang_max, double ang_increment, ros::Duration scan_time) { if (((ang_max - ang_min) / ang_increment) < 0) throw (BuildScanException()); - sensor_msgs::LaserScan scan; + sensor_msgs::msg::LaserScan scan; scan.header.stamp = ros::Time::now(); scan.header.frame_id = "laser_frame"; scan.angle_min = ang_min; @@ -239,9 +239,9 @@ TEST(laser_geometry, projectLaser) try { // printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); + sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); - sensor_msgs::PointCloud cloud_out; + sensor_msgs::msg::PointCloud cloud_out; projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); @@ -354,9 +354,9 @@ 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::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); + sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); - sensor_msgs::PointCloud2 cloud_out; + sensor_msgs::msg::PointCloud2 cloud_out; projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); @@ -387,7 +387,7 @@ TEST(laser_geometry, projectLaser2) uint32_t index_offset = 0; uint32_t distance_offset = 0; uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) + for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) { if (f->name == "x") x_offset = f->offset; if (f->name == "y") y_offset = f->offset; @@ -491,10 +491,10 @@ TEST(laser_geometry, transformLaserScanToPointCloud) try { //printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); - sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); + sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); scan.header.frame_id = "laser_frame"; - sensor_msgs::PointCloud cloud_out; + sensor_msgs::msg::PointCloud cloud_out; projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity); @@ -608,10 +608,10 @@ 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::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); + sensor_msgs::msg::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); scan.header.frame_id = "laser_frame"; - sensor_msgs::PointCloud2 cloud_out; + sensor_msgs::msg::PointCloud2 cloud_out; projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -1.0, laser_geometry::channel_option::None); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None); @@ -659,7 +659,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) uint32_t index_offset = 0; uint32_t distance_offset = 0; uint32_t stamps_offset = 0; - for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) + for (std::vector::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) { if (f->name == "x") x_offset = f->offset; if (f->name == "y") y_offset = f->offset;