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

Fix cpplint

Browse Source
This commit is contained in:
Martin Idel 2018-03-14 13:46:00 +01:00
parent 1dd3314e3b
commit 2783d803f8
3 changed files with 123 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
include/laser_geometry/laser_geometry.hpp
View File

@ -27,26 +27,20 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
#ifndef LASER_SCAN_UTILS_LASERSCAN_H #ifndef LASER_GEOMETRY__LASER_GEOMETRY_HPP_
#define LASER_SCAN_UTILS_LASERSCAN_H #define LASER_GEOMETRY__LASER_GEOMETRY_HPP_
#include <map> #include <map>
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <string>
#include <Eigen/Core> // NOLINT (cpplint cannot handle include order here)
#include "tf2/buffer_core.h" #include "tf2/buffer_core.h"
#include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/point_cloud2.hpp"
#ifndef ROS_DEBUG
#define ROS_DEBUG(...)
#endif // !ROS_DEBUG
#ifndef ROS_ASSERT
#define ROS_ASSERT(...)
#endif // !ROS_ASSERT
#include <Eigen/Core>
namespace laser_geometry namespace laser_geometry
{ {
// NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle) // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
@ -56,7 +50,7 @@ const float LASER_SCAN_MAX_RANGE = -3.0;
namespace channel_option namespace channel_option
{ {
//! Enumerated output channels options. // Enumerated output channels options.
/*! /*!
* An OR'd set of these options is passed as the final argument of * An OR'd set of these options is passed as the final argument of
* the projectLaser and transformLaserScanToPointCloud calls to * the projectLaser and transformLaserScanToPointCloud calls to
@ -72,7 +66,7 @@ enum ChannelOption
Viewpoint = 0x10, //!< Enable "viewpoint" channel Viewpoint = 0x10, //!< Enable "viewpoint" channel
Default = (Intensity | Index) //!< Enable "intensities" and "index" channels Default = (Intensity | Index) //!< Enable "intensities" and "index" channels
}; };
} } // namespace channel_option
//! \brief A Class to Project Laser Scan //! \brief A Class to Project Laser Scan
/*! /*!
@ -98,12 +92,11 @@ enum ChannelOption
*/ */
class LaserProjection class LaserProjection
{ {
public: public:
LaserProjection() LaserProjection()
: angle_min_(0), angle_max_(0) {} : angle_min_(0), angle_max_(0) {}
//! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::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
@ -128,7 +121,7 @@ public:
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::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 * 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
@ -163,14 +156,14 @@ public:
} }
private: private:
//! Internal hidden representation of projectLaser // Internal hidden representation of projectLaser
void projectLaser_( void projectLaser_(
const sensor_msgs::msg::LaserScan & scan_in, const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::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 transformLaserScanToPointCloud2 // Internal hidden representation of transformLaserScanToPointCloud2
void transformLaserScanToPointCloud_( void transformLaserScanToPointCloud_(
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in, const sensor_msgs::msg::LaserScan & scan_in,
@ -179,7 +172,7 @@ private:
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_( void transformLaserScanToPointCloud_(
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in, const sensor_msgs::msg::LaserScan & scan_in,
@ -191,12 +184,12 @@ private:
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
//! Internal map of pointers to stored values // Internal map of pointers to stored values
float angle_min_; float angle_min_;
float angle_max_; float angle_max_;
Eigen::ArrayXXd co_sine_map_; Eigen::ArrayXXd co_sine_map_;
}; };
} } // namespace laser_geometry
#endif //LASER_SCAN_UTILS_LASERSCAN_H #endif // LASER_GEOMETRY__LASER_GEOMETRY_HPP_

46
src/laser_geometry.cpp
View File

@ -30,15 +30,17 @@
#include "laser_geometry/laser_geometry.hpp" #include "laser_geometry/laser_geometry.hpp"
#include <algorithm> #include <algorithm>
#include <string>
#include "rclcpp/time.hpp" #include "rclcpp/time.hpp"
#define TIME rclcpp::Time #define TIME rclcpp::Time
#define POINT_FIELD sensor_msgs::msg::PointField #define POINT_FIELD sensor_msgs::msg::PointField
// TODO: fix definitions
typedef double tfScalar; typedef double tfScalar;
#include <tf2/LinearMath/Transform.h> #include "tf2/LinearMath/Transform.h"
namespace laser_geometry namespace laser_geometry
{ {
@ -54,22 +56,24 @@ void LaserProjection::projectLaser_(
// Get the ranges into Eigen format // Get the ranges into Eigen format
for (size_t i = 0; i < n_pts; ++i) { for (size_t i = 0; i < n_pts; ++i) {
ranges(i, 0) = (double) scan_in.ranges[i]; ranges(i, 0) = static_cast<double>(scan_in.ranges[i]);
ranges(i, 1) = (double) scan_in.ranges[i]; ranges(i, 1) = static_cast<double>(scan_in.ranges[i]);
} }
// Check if our existing co_sine_map is valid // Check if our existing co_sine_map is valid
if (co_sine_map_.rows() != (int)n_pts || angle_min_ != scan_in.angle_min || if (co_sine_map_.rows() != static_cast<int>(n_pts) || angle_min_ != scan_in.angle_min ||
angle_max_ != scan_in.angle_max) angle_max_ != scan_in.angle_max)
{ {
ROS_DEBUG("[projectLaser] No precomputed map given. Computing one."); // ROS_DEBUG("[projectLaser] No precomputed map given. Computing one.");
co_sine_map_ = Eigen::ArrayXXd(n_pts, 2); co_sine_map_ = Eigen::ArrayXXd(n_pts, 2);
angle_min_ = scan_in.angle_min; angle_min_ = scan_in.angle_min;
angle_max_ = scan_in.angle_max; angle_max_ = scan_in.angle_max;
// Spherical->Cartesian projection // Spherical->Cartesian projection
for (size_t i = 0; i < n_pts; ++i) { for (size_t i = 0; i < n_pts; ++i) {
co_sine_map_(i, 0) = cos(scan_in.angle_min + (double) i * scan_in.angle_increment); co_sine_map_(i, 0) =
co_sine_map_(i, 1) = sin(scan_in.angle_min + (double) i * scan_in.angle_increment); cos(scan_in.angle_min + static_cast<double>(i) * scan_in.angle_increment);
co_sine_map_(i, 1) =
sin(scan_in.angle_min + static_cast<double>(i) * scan_in.angle_increment);
} }
} }
@ -184,7 +188,7 @@ void LaserProjection::projectLaser_(
// check to see if we want to keep the point // check to see if we want to keep the point
const float range = scan_in.ranges[i]; const float range = scan_in.ranges[i];
if (range < range_cutoff && range >= scan_in.range_min) { if (range < range_cutoff && range >= scan_in.range_min) {
float * pstep = (float *)&cloud_out.data[count * cloud_out.point_step]; auto pstep = reinterpret_cast<float *>(&cloud_out.data[count * cloud_out.point_step]);
// Copy XYZ // Copy XYZ
pstep[0] = output(i, 0); pstep[0] = output(i, 0);
@ -198,7 +202,7 @@ void LaserProjection::projectLaser_(
// Copy index // Copy index
if (idx_index != -1) { if (idx_index != -1) {
((int *)(pstep))[idx_index] = i; reinterpret_cast<int *>(pstep)[idx_index] = i;
} }
// Copy distance // Copy distance
@ -222,8 +226,8 @@ void LaserProjection::projectLaser_(
++count; ++count;
} }
/* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values /* TODO(anonymous): Why was this done in this way, I don't get this at all, you end up with a
* why can't you just leave them out? * ton of points with NaN values why can't you just leave them out?
* *
// Invalid measurement? // Invalid measurement?
if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min) if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
@ -307,27 +311,28 @@ void LaserProjection::transformLaserScanToPointCloud_(
} }
} }
ROS_ASSERT(index_offset > 0); assert(index_offset > 0);
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
tf2::Transform cur_transform; tf2::Transform cur_transform;
double ranges_norm = 1 / ((double) scan_in.ranges.size() - 1.0); double ranges_norm = 1 / (static_cast<double>(scan_in.ranges.size()) - 1.0);
// we want to loop through all the points in the cloud // we want to loop through all the points in the cloud
for (size_t i = 0; i < cloud_out.width; ++i) { for (size_t i = 0; i < cloud_out.width; ++i) {
// Apply the transform to the current point // Apply the transform to the current point
float * pstep = (float *)&cloud_out.data[i * cloud_out.point_step + 0]; float * pstep = reinterpret_cast<float *>(&cloud_out.data[i * cloud_out.point_step + 0]);
// find the index of the point // find the index of the point
uint32_t pt_index; uint32_t pt_index;
memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t)); memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
// Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms // Assume constant motion during the laser-scan and use slerp to compute intermediate transforms
tfScalar ratio = pt_index * ranges_norm; tfScalar ratio = pt_index * ranges_norm;
//! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector) // TODO(anon): Make a function that performs both the slerp and linear interpolation needed to
// interpolate a Full Transform (Quaternion + Vector)
// Interpolate translation // Interpolate translation
tf2::Vector3 v(0, 0, 0); tf2::Vector3 v(0, 0, 0);
v.setInterpolate3(origin_start, origin_end, ratio); v.setInterpolate3(origin_start, origin_end, ratio);
@ -346,7 +351,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
// Convert the viewpoint as well // Convert the viewpoint as well
if (has_viewpoint) { if (has_viewpoint) {
float * vpstep = (float *)&cloud_out.data[i * cloud_out.point_step + vp_x_offset]; 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 = tf2::Vector3(vpstep[0], vpstep[1], vpstep[2]);
point_out = cur_transform * point_in; point_out = cur_transform * point_in;
@ -418,7 +424,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
{ {
TIME start_time = scan_in.header.stamp; TIME start_time = scan_in.header.stamp;
TIME end_time = scan_in.header.stamp; TIME end_time = scan_in.header.stamp;
// TODO: reconcile all the different time constructs // TODO(anonymous): reconcile all the different time constructs
if (!scan_in.ranges.empty()) { if (!scan_in.ranges.empty()) {
end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0); end_time = end_time + rclcpp::Duration((scan_in.ranges.size() - 1) * scan_in.time_increment, 0);
} }
@ -456,4 +462,4 @@ void LaserProjection::transformLaserScanToPointCloud_(
channel_options); channel_options);
} }
} //laser_geometry } // namespace laser_geometry

88
test/projection_test.cpp
View File

@ -30,6 +30,7 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <sys/time.h> #include <sys/time.h>
#include <math.h> #include <math.h>
#include <vector>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
@ -86,11 +87,17 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.intensities.push_back(options.intensity_); scan.intensities.push_back(options.intensity_);
} }
scan.time_increment = options.scan_time_.nanoseconds() / (double)i; scan.time_increment = options.scan_time_.nanoseconds() / static_cast<double>(i);
return scan; return scan;
} }
template<typename T>
T cloudData(sensor_msgs::msg::PointCloud2 cloud_out, uint32_t index)
{
return *reinterpret_cast<T *>(&cloud_out.data[index]);
}
TEST(laser_geometry, projectLaser2) { TEST(laser_geometry, projectLaser2) {
double tolerance = 1e-12; double tolerance = 1e-12;
laser_geometry::LaserProjection projector; laser_geometry::LaserProjection projector;
@ -124,7 +131,7 @@ TEST(laser_geometry, projectLaser2) {
max_angles.push_back(M_PI / 4); max_angles.push_back(M_PI / 4);
max_angles.push_back(M_PI / 8); max_angles.push_back(M_PI / 8);
// angle_increments.push_back(-M_PI/180); // -one degree angle_increments.push_back(-M_PI / 180); // -one degree
angle_increments.push_back(M_PI / 180); // one degree angle_increments.push_back(M_PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree angle_increments.push_back(M_PI / 720); // quarter degree
@ -156,28 +163,28 @@ TEST(laser_geometry, projectLaser2) {
sensor_msgs::msg::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(), 4u);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.projectLaser(scan, cloud_out, -1.0); projector.projectLaser(scan, cloud_out, -1.0);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.projectLaser(scan, cloud_out, -1.0, projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index); laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.projectLaser(scan, cloud_out, -1.0, projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance); laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); EXPECT_EQ(cloud_out.fields.size(), 6u);
projector.projectLaser(scan, cloud_out, -1.0, projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance | laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp); laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); EXPECT_EQ(cloud_out.fields.size(), 7u);
unsigned int valid_points = 0; unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++) { for (unsigned int i = 0; i < scan.ranges.size(); i++) {
@ -210,21 +217,22 @@ TEST(laser_geometry, projectLaser2) {
} }
for (unsigned int i = 0; i < cloud_out.width; i++) { for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset], static_cast<float>(static_cast<double>(scan.ranges[i]) *
(float)((double)(scan.ranges[i]) * cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset], EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
(float)((double)(scan.ranges[i]) * static_cast<float>(static_cast<double>(scan.ranges[i]) *
sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance); tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset], EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset),
scan.intensities[i], tolerance); // intensity scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i, EXPECT_NEAR(cloudData<uint32_t>(cloud_out, i * cloud_out.point_step + index_offset), i,
tolerance); // index tolerance); // index
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset], EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + distance_offset),
scan.ranges[i], tolerance); // ranges scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset], EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + stamps_offset),
(float)i * scan.time_increment, tolerance); // timestamps (float)i * scan.time_increment, tolerance); // timestamps
} }
} catch (BuildScanException & ex) { } catch (BuildScanException & ex) {
@ -237,7 +245,6 @@ TEST(laser_geometry, projectLaser2) {
} }
TEST(laser_geometry, transformLaserScanToPointCloud2) { TEST(laser_geometry, transformLaserScanToPointCloud2) {
tf2::BufferCore tf2; tf2::BufferCore tf2;
double tolerance = 1e-12; double tolerance = 1e-12;
@ -307,31 +314,31 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
sensor_msgs::msg::PointCloud2 cloud_out; sensor_msgs::msg::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::None); laser_geometry::channel_option::None);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); EXPECT_EQ(cloud_out.fields.size(), 3u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Index); laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity); laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); 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, tf2);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index); laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance); laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6); EXPECT_EQ(cloud_out.fields.size(), 6u);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance | laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp); laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7); EXPECT_EQ(cloud_out.fields.size(), 7u);
EXPECT_EQ(cloud_out.is_dense, false); EXPECT_EQ(cloud_out.is_dense, false);
@ -365,22 +372,23 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
} }
for (unsigned int i = 0; i < cloud_out.width; i++) { for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset], EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
(float)((double)(scan.ranges[i]) * static_cast<float>(static_cast<double>(scan.ranges[i]) *
cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset], tolerance);
(float)((double)(scan.ranges[i]) * EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); static_cast<float>(static_cast<double>(scan.ranges[i]) *
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance); sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset], tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset),
scan.intensities[i], tolerance); // intensity scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i, EXPECT_NEAR(cloudData<uint32_t>(cloud_out, i * cloud_out.point_step + index_offset), i,
tolerance); // index tolerance); // index
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset], EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + distance_offset),
scan.ranges[i], tolerance); // ranges scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset], EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + stamps_offset),
(float)i * scan.time_increment, tolerance); // timestamps (float)i * scan.time_increment, tolerance); // timestamps
} }
} catch (BuildScanException & ex) { } catch (BuildScanException & ex) {
// make sure it is not a false exception // make sure it is not a false exception