DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #27 from bfjelds/ros2-devel

Browse Source 
Make laser_geometry build for ros2 (on windows 10)
This commit is contained in:
Jonathan Binney 2018-02-08 17:42:58 -08:00 committed by GitHub
commit 4a570ae622
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 165 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
CMakeLists.txt
View File

@ -1,47 +1,45 @@
cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 3.5)
project(laser_geometry) project(laser_geometry)
find_package(catkin REQUIRED find_package(ament_cmake REQUIRED)
COMPONENTS
angles
cmake_modules
roscpp
sensor_msgs
tf
tf2
)
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() # TODO(dhood): enable python support once ported to ROS 2
# catkin_python_setup()
catkin_package(
INCLUDE_DIRS include
LIBRARIES laser_geometry
DEPENDS Boost Eigen
)
include_directories(include include_directories(include
${catkin_INCLUDE_DIRS} ${angles_INCLUDE_DIRS}
${rclcpp_INCLUDE_DIRS}
${sensor_msgs_INCLUDE_DIRS}
${tf2_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}
) )
add_library(laser_geometry src/laser_geometry.cpp) 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) ament_export_include_directories(include)
catkin_add_gtest(projection_test test/projection_test.cpp) ament_export_libraries(laser_geometry)
target_link_libraries(projection_test laser_geometry) ament_package(CONFIG_EXTRAS laser_geometry-extras.cmake)
catkin_add_nosetests(test/projection_test.py)
endif()
install(TARGETS laser_geometry install(TARGETS laser_geometry
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION lib
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) LIBRARY DESTINATION lib)
install(DIRECTORY include/laser_geometry/ install(DIRECTORY include/laser_geometry/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} DESTINATION include/${PROJECT_NAME}/
FILES_MATCHING PATTERN "*.h") FILES_MATCHING PATTERN "*.h")

111
include/laser_geometry/laser_geometry.h
View File

@ -37,15 +37,19 @@
#include "boost/numeric/ublas/matrix.hpp" #include "boost/numeric/ublas/matrix.hpp"
#include "boost/thread/mutex.hpp" #include "boost/thread/mutex.hpp"
#include <tf/tf.h>
#include <tf2/buffer_core.h> #include <tf2/buffer_core.h>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "sensor_msgs/LaserScan.h" #ifndef ROS_DEBUG
#include "sensor_msgs/PointCloud.h" #define ROS_DEBUG(...)
#include "sensor_msgs/PointCloud.h" #endif // !ROS_DEBUG
#ifndef ROS_ASSERT
#define ROS_ASSERT(...)
#endif // !ROS_ASSERT
#include <Eigen/Core> #include <Eigen/Core>
#include <sensor_msgs/PointCloud2.h>
namespace laser_geometry namespace laser_geometry
{ {
@ -106,7 +110,7 @@ namespace laser_geometry
//! Destructor to deallocate stored unit vectors //! Destructor to deallocate stored unit vectors
~LaserProjection(); ~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 * Project a single laser scan from a linear array into a 3D
* point cloud. The generated cloud will be in the same frame * 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::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp. * channel_option::Distance, channel_option::Timestamp.
*/ */
void projectLaser (const sensor_msgs::LaserScan& scan_in, void projectLaser (const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out, sensor_msgs::msg::PointCloud& cloud_out,
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
{ {
return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options); 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 * Project a single laser scan from a linear array into a 3D
* point cloud. The generated cloud will be in the same frame * 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::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp. * channel_option::Distance, channel_option::Timestamp.
*/ */
void projectLaser (const sensor_msgs::LaserScan& scan_in, void projectLaser (const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::msg::PointCloud2 &cloud_out,
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
{ {
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
} }
//! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud in target frame
//! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame
/*! /*!
* Transform a single laser scan from a linear array into a 3D * Transform a single laser scan from a linear array into a 3D
* point cloud, accounting for movement of the laser over the * point cloud, accounting for movement of the laser over the
@ -177,16 +180,16 @@ namespace laser_geometry
* channel_option::Distance, channel_option::Timestamp. * channel_option::Distance, channel_option::Timestamp.
*/ */
void transformLaserScanToPointCloud (const std::string& target_frame, void transformLaserScanToPointCloud (const std::string& target_frame,
const sensor_msgs::LaserScan& scan_in, const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out, sensor_msgs::msg::PointCloud& cloud_out,
tf::Transformer& tf, tf2::BufferCore &tf,
double range_cutoff, double range_cutoff,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
{ {
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options); 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 * Transform a single laser scan from a linear array into a 3D
* point cloud, accounting for movement of the laser over the * point cloud, accounting for movement of the laser over the
@ -206,47 +209,15 @@ namespace laser_geometry
* channel_option::Distance, channel_option::Timestamp. * channel_option::Distance, channel_option::Timestamp.
*/ */
void transformLaserScanToPointCloud (const std::string& target_frame, void transformLaserScanToPointCloud (const std::string& target_frame,
const sensor_msgs::LaserScan& scan_in, const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out, sensor_msgs::msg::PointCloud& cloud_out,
tf::Transformer& tf, tf2::BufferCore &tf,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
{ {
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options); 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 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
* 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 single laser scan from a linear array into a 3D * Transform a single laser scan from a linear array into a 3D
* point cloud, accounting for movement of the laser over the * point cloud, accounting for movement of the laser over the
@ -269,8 +240,8 @@ namespace laser_geometry
* channel_option::Distance, channel_option::Timestamp. * channel_option::Distance, channel_option::Timestamp.
*/ */
void transformLaserScanToPointCloud(const std::string &target_frame, void transformLaserScanToPointCloud(const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::BufferCore &tf, tf2::BufferCore &tf,
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
@ -294,46 +265,38 @@ namespace laser_geometry
private: private:
//! Internal hidden representation of projectLaser //! Internal hidden representation of projectLaser
void projectLaser_ (const sensor_msgs::LaserScan& scan_in, void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out, sensor_msgs::msg::PointCloud& cloud_out,
double range_cutoff, double range_cutoff,
bool preservative, bool preservative,
int channel_options); int channel_options);
//! Internal hidden representation of projectLaser //! Internal hidden representation of projectLaser
void projectLaser_ (const sensor_msgs::LaserScan& scan_in, void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::msg::PointCloud2 &cloud_out,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
//! Internal hidden representation of transformLaserScanToPointCloud //! Internal hidden representation of transformLaserScanToPointCloud
void transformLaserScanToPointCloud_ (const std::string& target_frame, void transformLaserScanToPointCloud_ (const std::string& target_frame,
sensor_msgs::PointCloud& cloud_out, sensor_msgs::msg::PointCloud& cloud_out,
const sensor_msgs::LaserScan& scan_in, const sensor_msgs::msg::LaserScan& scan_in,
tf::Transformer & tf, tf2::BufferCore &tf,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
//! Internal hidden representation of transformLaserScanToPointCloud2 //! Internal hidden representation of transformLaserScanToPointCloud2
void transformLaserScanToPointCloud_ (const std::string &target_frame, void transformLaserScanToPointCloud_ (const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::msg::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,
tf2::BufferCore &tf, tf2::BufferCore &tf,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
//! Function used by the several forms of transformLaserScanToPointCloud_ //! Function used by the several forms of transformLaserScanToPointCloud_
void transformLaserScanToPointCloud_ (const std::string &target_frame, void transformLaserScanToPointCloud_ (const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::Quaternion quat_start, tf2::Quaternion quat_start,
tf2::Vector3 origin_start, tf2::Vector3 origin_start,
tf2::Quaternion quat_end, tf2::Quaternion quat_end,

31
laser_geometry-extras.cmake Normal file
View File

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

32
package.xml
View File

@ -1,4 +1,6 @@
<package> <?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>laser_geometry</name> <name>laser_geometry</name>
<version>1.6.4</version> <version>1.6.4</version>
<description> <description>
@ -16,23 +18,23 @@
<url>http://ros.org/wiki/laser_geometry</url> <url>http://ros.org/wiki/laser_geometry</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>angles</build_depend> <build_depend>angles</build_depend>
<build_depend>boost</build_depend> <build_depend>Eigen3</build_depend>
<build_depend>cmake_modules</build_depend> <build_depend>rclcpp</build_depend>
<build_depend>eigen</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend> <build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend> <build_depend>tf2</build_depend>
<run_depend>angles</run_depend> <exec_depend>angles</exec_depend>
<run_depend>boost</run_depend> <exec_depend>Eigen3</exec_depend>
<run_depend>eigen</run_depend> <exec_depend>rclcpp</exec_depend>
<run_depend>python-numpy</run_depend> <exec_depend>sensor_msgs</exec_depend>
<run_depend>roscpp</run_depend> <exec_depend>tf2</exec_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>tf</run_depend>
<test_depend>rosunit</test_depend> <exec_depend>ament_cmake</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package> </package>

186
src/laser_geometry.cpp
View File

@ -29,14 +29,21 @@
#include "laser_geometry/laser_geometry.h" #include "laser_geometry/laser_geometry.h"
#include <algorithm> #include <algorithm>
#include <ros/assert.h> #include "rclcpp/time.hpp"
#define TIME rclcpp::Time
#define POINT_FIELD sensor_msgs::msg::PointField
// TODO: fix definitions
typedef double tfScalar;
#include <tf2/LinearMath/Transform.h> #include <tf2/LinearMath/Transform.h>
namespace laser_geometry namespace laser_geometry
{ {
void 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) bool preservative, int mask)
{ {
boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size()); boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
@ -185,96 +192,8 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
} }
}; };
void void LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in,
LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in, sensor_msgs::msg::PointCloud2 &cloud_out,
tf::Transformer& tf, double range_cutoff, int mask)
{
cloud_out.header = scan_in.header;
tf::Stamped<tf::Point> pointIn;
tf::Stamped<tf::Point> 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,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -313,15 +232,15 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.fields.resize (3); cloud_out.fields.resize (3);
cloud_out.fields[0].name = "x"; cloud_out.fields[0].name = "x";
cloud_out.fields[0].offset = 0; 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[0].count = 1;
cloud_out.fields[1].name = "y"; cloud_out.fields[1].name = "y";
cloud_out.fields[1].offset = 4; 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[1].count = 1;
cloud_out.fields[2].name = "z"; cloud_out.fields[2].name = "z";
cloud_out.fields[2].offset = 8; 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; cloud_out.fields[2].count = 1;
// Define 4 indices in the channel array for each possible value type // Define 4 indices in the channel array for each possible value type
@ -334,7 +253,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
int field_size = cloud_out.fields.size(); int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "intensity"; 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].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -346,7 +265,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
int field_size = cloud_out.fields.size(); int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "index"; 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].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -358,7 +277,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
int field_size = cloud_out.fields.size(); int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "distances"; 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].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -370,7 +289,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
int field_size = cloud_out.fields.size(); int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "stamps"; 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].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -383,19 +302,19 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.fields.resize(field_size + 3); cloud_out.fields.resize(field_size + 3);
cloud_out.fields[field_size].name = "vp_x"; 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].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
cloud_out.fields[field_size + 1].name = "vp_y"; 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].offset = offset;
cloud_out.fields[field_size + 1].count = 1; cloud_out.fields[field_size + 1].count = 1;
offset += 4; offset += 4;
cloud_out.fields[field_size + 2].name = "vp_z"; 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].offset = offset;
cloud_out.fields[field_size + 2].count = 1; cloud_out.fields[field_size + 2].count = 1;
offset += 4; offset += 4;
@ -494,8 +413,8 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
} }
void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame, void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::Quaternion quat_start, tf2::Quaternion quat_start,
tf2::Vector3 origin_start, tf2::Vector3 origin_start,
tf2::Quaternion quat_end, tf2::Quaternion quat_end,
@ -596,7 +515,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
//if the user didn't request the index field, then we need to copy the PointCloud and drop it //if the user didn't request the index field, then we need to copy the PointCloud and drop it
if(!requested_index) if(!requested_index)
{ {
sensor_msgs::PointCloud2 cloud_without_index; sensor_msgs::msg::PointCloud2 cloud_without_index;
//copy basic meta data //copy basic meta data
cloud_without_index.header = cloud_out.header; cloud_without_index.header = cloud_out.header;
@ -650,53 +569,26 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
} }
void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::msg::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,
tf2::BufferCore &tf, tf2::BufferCore &tf,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
ros::Time start_time = scan_in.header.stamp; TIME start_time = scan_in.header.stamp;
ros::Time end_time = scan_in.header.stamp; 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); // 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); std::chrono::milliseconds start(start_time.nanoseconds());
geometry_msgs::TransformStamped end_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time); std::chrono::time_point<std::chrono::system_clock> 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<std::chrono::system_clock> 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, tf2::Quaternion quat_start(start_transform.transform.rotation.x,
start_transform.transform.rotation.y, start_transform.transform.rotation.y,

24
test/projection_test.cpp
View File

@ -45,14 +45,14 @@
class BuildScanException { }; 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, double ang_min, double ang_max, double ang_increment,
ros::Duration scan_time) ros::Duration scan_time)
{ {
if (((ang_max - ang_min) / ang_increment) < 0) if (((ang_max - ang_min) / ang_increment) < 0)
throw (BuildScanException()); throw (BuildScanException());
sensor_msgs::LaserScan scan; sensor_msgs::msg::LaserScan scan;
scan.header.stamp = ros::Time::now(); scan.header.stamp = ros::Time::now();
scan.header.frame_id = "laser_frame"; scan.header.frame_id = "laser_frame";
scan.angle_min = ang_min; scan.angle_min = ang_min;
@ -239,9 +239,9 @@ TEST(laser_geometry, projectLaser)
try try
{ {
// printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); // 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); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
@ -354,9 +354,9 @@ TEST(laser_geometry, projectLaser2)
try try
{ {
// printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); // 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); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); 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 index_offset = 0;
uint32_t distance_offset = 0; uint32_t distance_offset = 0;
uint32_t stamps_offset = 0; uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) for (std::vector<sensor_msgs::msg::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
{ {
if (f->name == "x") x_offset = f->offset; if (f->name == "x") x_offset = f->offset;
if (f->name == "y") y_offset = f->offset; if (f->name == "y") y_offset = f->offset;
@ -491,10 +491,10 @@ TEST(laser_geometry, transformLaserScanToPointCloud)
try try
{ {
//printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); //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"; 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); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
@ -608,10 +608,10 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
try try
{ {
//printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); //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"; 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); 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); 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); 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 index_offset = 0;
uint32_t distance_offset = 0; uint32_t distance_offset = 0;
uint32_t stamps_offset = 0; uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) for (std::vector<sensor_msgs::msg::PointField>::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++)
{ {
if (f->name == "x") x_offset = f->offset; if (f->name == "x") x_offset = f->offset;
if (f->name == "y") y_offset = f->offset; if (f->name == "y") y_offset = f->offset;