diff --git a/include/laser_geometry/laser_geometry.hpp b/include/laser_geometry/laser_geometry.hpp index acf26cf..6859c47 100644 --- a/include/laser_geometry/laser_geometry.hpp +++ b/include/laser_geometry/laser_geometry.hpp @@ -27,26 +27,20 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef LASER_SCAN_UTILS_LASERSCAN_H -#define LASER_SCAN_UTILS_LASERSCAN_H +#ifndef LASER_GEOMETRY__LASER_GEOMETRY_HPP_ +#define LASER_GEOMETRY__LASER_GEOMETRY_HPP_ #include #include #include +#include + +#include // NOLINT (cpplint cannot handle include order here) #include "tf2/buffer_core.h" #include "sensor_msgs/msg/laser_scan.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 - namespace laser_geometry { // 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 { -//! Enumerated output channels options. +// Enumerated output channels options. /*! * An OR'd set of these options is passed as the final argument of * the projectLaser and transformLaserScanToPointCloud calls to @@ -72,7 +66,7 @@ enum ChannelOption Viewpoint = 0x10, //!< Enable "viewpoint" channel Default = (Intensity | Index) //!< Enable "intensities" and "index" channels }; -} +} // namespace channel_option //! \brief A Class to Project Laser Scan /*! @@ -98,12 +92,11 @@ enum ChannelOption */ class LaserProjection { - public: LaserProjection() : 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 * 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); } - //! 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 * point cloud, accounting for movement of the laser over the @@ -163,14 +156,14 @@ public: } private: - //! Internal hidden representation of projectLaser + // Internal hidden representation of projectLaser 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 transformLaserScanToPointCloud2 + // Internal hidden representation of transformLaserScanToPointCloud2 void transformLaserScanToPointCloud_( const std::string & target_frame, const sensor_msgs::msg::LaserScan & scan_in, @@ -179,7 +172,7 @@ private: double range_cutoff, 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, const sensor_msgs::msg::LaserScan & scan_in, @@ -191,12 +184,12 @@ private: double range_cutoff, int channel_options); - //! Internal map of pointers to stored values + // Internal map of pointers to stored values float angle_min_; float angle_max_; Eigen::ArrayXXd co_sine_map_; }; -} +} // namespace laser_geometry -#endif //LASER_SCAN_UTILS_LASERSCAN_H +#endif // LASER_GEOMETRY__LASER_GEOMETRY_HPP_ diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index d030398..6a721a7 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -30,15 +30,17 @@ #include "laser_geometry/laser_geometry.hpp" #include +#include + #include "rclcpp/time.hpp" + #define TIME rclcpp::Time #define POINT_FIELD sensor_msgs::msg::PointField -// TODO: fix definitions typedef double tfScalar; -#include +#include "tf2/LinearMath/Transform.h" namespace laser_geometry { @@ -54,22 +56,24 @@ void LaserProjection::projectLaser_( // Get the ranges into Eigen format for (size_t i = 0; i < n_pts; ++i) { - ranges(i, 0) = (double) scan_in.ranges[i]; - ranges(i, 1) = (double) scan_in.ranges[i]; + ranges(i, 0) = static_cast(scan_in.ranges[i]); + ranges(i, 1) = static_cast(scan_in.ranges[i]); } // 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(n_pts) || angle_min_ != scan_in.angle_min || 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); angle_min_ = scan_in.angle_min; angle_max_ = scan_in.angle_max; // Spherical->Cartesian projection 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, 1) = sin(scan_in.angle_min + (double) i * scan_in.angle_increment); + co_sine_map_(i, 0) = + cos(scan_in.angle_min + static_cast(i) * scan_in.angle_increment); + co_sine_map_(i, 1) = + sin(scan_in.angle_min + static_cast(i) * scan_in.angle_increment); } } @@ -97,7 +101,7 @@ void LaserProjection::projectLaser_( int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1; - //now, we need to check what fields we need to store + // now, we need to check what fields we need to store int offset = 12; if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) { int field_size = cloud_out.fields.size(); @@ -181,10 +185,10 @@ void LaserProjection::projectLaser_( unsigned int count = 0; for (size_t i = 0; i < n_pts; ++i) { - //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]; if (range < range_cutoff && range >= scan_in.range_min) { - float * pstep = (float *)&cloud_out.data[count * cloud_out.point_step]; + auto pstep = reinterpret_cast(&cloud_out.data[count * cloud_out.point_step]); // Copy XYZ pstep[0] = output(i, 0); @@ -196,9 +200,9 @@ void LaserProjection::projectLaser_( pstep[idx_intensity] = scan_in.intensities[i]; } - //Copy index + // Copy index if (idx_index != -1) { - ((int *)(pstep))[idx_index] = i; + reinterpret_cast(pstep)[idx_index] = i; } // Copy distance @@ -218,12 +222,12 @@ void LaserProjection::projectLaser_( pstep[idx_vpz] = 0; } - //make sure to increment count + // make sure to increment 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 - * why can't you just leave them out? + /* TODO(anonymous): 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 why can't you just leave them out? * // Invalid measurement? if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min) @@ -254,7 +258,7 @@ void LaserProjection::projectLaser_( */ } - //resize if necessary + // resize if necessary cloud_out.width = count; cloud_out.row_step = cloud_out.point_step * cloud_out.width; cloud_out.data.resize(cloud_out.row_step * cloud_out.height); @@ -271,63 +275,64 @@ void LaserProjection::transformLaserScanToPointCloud_( double range_cutoff, int channel_options) { - //check if the user has requested the index field + // check if the user has requested the index field bool requested_index = false; if ((channel_options & channel_option::Index)) { requested_index = true; } - //we'll enforce that we get index values for the laser scan so that we - //ensure that we use the correct timestamps + // we'll enforce that we get index values for the laser scan so that we + // ensure that we use the correct timestamps channel_options |= channel_option::Index; projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); - //we'll assume no associated viewpoint by default + // we'll assume no associated viewpoint by default bool has_viewpoint = false; uint32_t vp_x_offset = 0; - //we need to find the offset of the intensity field in the point cloud - //we also know that the index field is guaranteed to exist since we - //set the channel option above. To be really safe, it might be worth - //putting in a check at some point, but I'm just going to put in an - //assert for now + // we need to find the offset of the intensity field in the point cloud + // we also know that the index field is guaranteed to exist since we + // set the channel option above. To be really safe, it might be worth + // putting in a check at some point, but I'm just going to put in an + // assert for now uint32_t index_offset = 0; for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) { if (cloud_out.fields[i].name == "index") { index_offset = cloud_out.fields[i].offset; } - //we want to check if the cloud has a viewpoint associated with it - //checking vp_x should be sufficient since vp_x, vp_y, and vp_z all - //get put in together + // we want to check if the cloud has a viewpoint associated with it + // checking vp_x should be sufficient since vp_x, vp_y, and vp_z all + // get put in together if (cloud_out.fields[i].name == "vp_x") { has_viewpoint = true; vp_x_offset = cloud_out.fields[i].offset; } } - ROS_ASSERT(index_offset > 0); + assert(index_offset > 0); cloud_out.header.frame_id = target_frame; tf2::Transform cur_transform; - double ranges_norm = 1 / ((double) scan_in.ranges.size() - 1.0); + double ranges_norm = 1 / (static_cast(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) { // Apply the transform to the current point - float * pstep = (float *)&cloud_out.data[i * cloud_out.point_step + 0]; + float * pstep = reinterpret_cast(&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; 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; - //! \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 tf2::Vector3 v(0, 0, 0); v.setInterpolate3(origin_start, origin_end, ratio); @@ -346,7 +351,8 @@ void LaserProjection::transformLaserScanToPointCloud_( // Convert the viewpoint as well if (has_viewpoint) { - float * vpstep = (float *)&cloud_out.data[i * cloud_out.point_step + vp_x_offset]; + auto vpstep = + reinterpret_cast(&cloud_out.data[i * cloud_out.point_step + vp_x_offset]); point_in = tf2::Vector3(vpstep[0], vpstep[1], vpstep[2]); point_out = cur_transform * point_in; @@ -357,18 +363,18 @@ void LaserProjection::transformLaserScanToPointCloud_( } } - //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) { 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.width = cloud_out.width; cloud_without_index.height = cloud_out.height; cloud_without_index.is_bigendian = cloud_out.is_bigendian; cloud_without_index.is_dense = cloud_out.is_dense; - //copy the fields + // copy the fields cloud_without_index.fields.resize(cloud_out.fields.size()); unsigned int field_count = 0; unsigned int offset_shift = 0; @@ -378,22 +384,22 @@ void LaserProjection::transformLaserScanToPointCloud_( cloud_without_index.fields[field_count].offset -= offset_shift; ++field_count; } else { - //once we hit the index, we'll set the shift + // once we hit the index, we'll set the shift offset_shift = 4; } } - //resize the fields + // resize the fields cloud_without_index.fields.resize(field_count); - //compute the size of the new data + // compute the size of the new data cloud_without_index.point_step = cloud_out.point_step - offset_shift; cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width; cloud_without_index.data.resize(cloud_without_index.row_step * cloud_without_index.height); uint32_t i = 0; uint32_t j = 0; - //copy over the data from one cloud to the other + // copy over the data from one cloud to the other while (i < cloud_out.data.size()) { if ((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4)) @@ -403,7 +409,7 @@ void LaserProjection::transformLaserScanToPointCloud_( i++; } - //make sure to actually set the output + // make sure to actually set the output cloud_out = cloud_without_index; } } @@ -418,7 +424,7 @@ void LaserProjection::transformLaserScanToPointCloud_( { TIME start_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()) { 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); } -} //laser_geometry +} // namespace laser_geometry diff --git a/test/projection_test.cpp b/test/projection_test.cpp index b221ff6..98832c5 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #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.time_increment = options.scan_time_.nanoseconds() / (double)i; + scan.time_increment = options.scan_time_.nanoseconds() / static_cast(i); return scan; } +template +T cloudData(sensor_msgs::msg::PointCloud2 cloud_out, uint32_t index) +{ + return *reinterpret_cast(&cloud_out.data[index]); +} + TEST(laser_geometry, projectLaser2) { double tolerance = 1e-12; laser_geometry::LaserProjection projector; @@ -124,10 +131,10 @@ TEST(laser_geometry, projectLaser2) { max_angles.push_back(M_PI / 4); 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 / 360); // half degree - angle_increments.push_back(M_PI / 720); // quarter 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 / 720); // quarter degree scan_times.push_back(rclcpp::Duration(1 / 40)); scan_times.push_back(rclcpp::Duration(1 / 20)); @@ -156,28 +163,28 @@ TEST(laser_geometry, projectLaser2) { 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); + EXPECT_EQ(cloud_out.fields.size(), 4u); 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); - EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5); + EXPECT_EQ(cloud_out.fields.size(), 5u); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | 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, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | 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, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | 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; 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++) { - - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset], - (float)((double)(scan.ranges[i]) * - cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset], - (float)((double)(scan.ranges[i]) * - sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance); - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset], + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + x_offset), + static_cast(static_cast(scan.ranges[i]) * + cos(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + y_offset), + static_cast(static_cast(scan.ranges[i]) * + sin(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + intensity_offset), scan.intensities[i], tolerance); // intensity - EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i, + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + index_offset), i, tolerance); // index - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset], + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + distance_offset), scan.ranges[i], tolerance); // ranges - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset], + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + stamps_offset), (float)i * scan.time_increment, tolerance); // timestamps } } catch (BuildScanException & ex) { @@ -237,7 +245,6 @@ TEST(laser_geometry, projectLaser2) { } TEST(laser_geometry, transformLaserScanToPointCloud2) { - tf2::BufferCore tf2; double tolerance = 1e-12; @@ -272,10 +279,10 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { max_angles.push_back(M_PI / 4); 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 / 360); // half degree - angle_increments.push_back(M_PI / 720); // quarter 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 / 720); // quarter degree scan_times.push_back(rclcpp::Duration(1 / 40)); scan_times.push_back(rclcpp::Duration(1 / 20)); @@ -307,31 +314,31 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { sensor_msgs::msg::PointCloud2 cloud_out; projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, 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, 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, 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); - 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, laser_geometry::channel_option::Intensity | 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, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | 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, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | 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); @@ -365,22 +372,23 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { } for (unsigned int i = 0; i < cloud_out.width; i++) { - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset], - (float)((double)(scan.ranges[i]) * - cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset], - (float)((double)(scan.ranges[i]) * - sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance); - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance); - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset], + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + x_offset), + static_cast(static_cast(scan.ranges[i]) * + cos(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + y_offset), + static_cast(static_cast(scan.ranges[i]) * + sin(static_cast(scan.angle_min) + i * static_cast(scan.angle_increment))), + tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + z_offset), 0, tolerance); + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + intensity_offset), scan.intensities[i], tolerance); // intensity - EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i, + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + index_offset), i, tolerance); // index - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset], + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + distance_offset), scan.ranges[i], tolerance); // ranges - EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset], + EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + stamps_offset), (float)i * scan.time_increment, tolerance); // timestamps - } } catch (BuildScanException & ex) { // make sure it is not a false exception