diff --git a/include/laser_geometry/laser_geometry.hpp b/include/laser_geometry/laser_geometry.hpp index 98306721..f7ee51b7 100644 --- a/include/laser_geometry/laser_geometry.hpp +++ b/include/laser_geometry/laser_geometry.hpp @@ -159,8 +159,8 @@ class LaserProjection double range_cutoff = -1.0, int channel_options = channel_option::Default) { - transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, - channel_options); + transformLaserScanToPointCloud_( + target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); } private: diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 948b37c0..70312901 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -433,14 +433,12 @@ void LaserProjection::transformLaserScanToPointCloud_( std::chrono::nanoseconds 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); + geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform( + target_frame, scan_in.header.frame_id, st); std::chrono::nanoseconds 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); + 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, @@ -457,7 +455,8 @@ void LaserProjection::transformLaserScanToPointCloud_( tf2::Vector3 origin_end(end_transform.transform.translation.x, end_transform.transform.translation.y, end_transform.transform.translation.z); - transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, + transformLaserScanToPointCloud_( + target_frame, scan_in, cloud_out, quat_start, origin_start, quat_end, origin_end, range_cutoff, diff --git a/test/projection_test.cpp b/test/projection_test.cpp index 62939a7b..e5c8e0f6 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -139,7 +139,8 @@ TEST(laser_geometry, projectLaser2) { for (auto max_angle : max_angles) { for (auto angle_increment : angle_increments) { for (auto scan_time : scan_times) { - options.push_back(ScanOptions( + options.push_back( + ScanOptions( range, intensity, min_angle, max_angle, angle_increment, scan_time)); } } @@ -162,17 +163,20 @@ TEST(laser_geometry, projectLaser2) { projector.projectLaser(scan, cloud_out, -1.0); 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); 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::Distance); 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::Index | laser_geometry::channel_option::Distance | @@ -210,22 +214,28 @@ TEST(laser_geometry, projectLaser2) { } for (unsigned int i = 0; i < cloud_out.width; i++) { - EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + x_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), + 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), + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + intensity_offset), scan.intensities[i], tolerance); // intensity - EXPECT_NEAR(cloudData(cloud_out, 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(cloudData(cloud_out, 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(cloudData(cloud_out, 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 (const BuildScanException & ex) { @@ -291,7 +301,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { for (auto max_angle : max_angles) { for (auto angle_increment : angle_increments) { for (auto scan_time : scan_times) { - options.push_back(ScanOptions( + options.push_back( + ScanOptions( range, intensity, min_angle, max_angle, angle_increment, scan_time)); } } @@ -309,29 +320,35 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { scan.header.frame_id = "laser_frame"; 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); 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); 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); EXPECT_EQ(cloud_out.fields.size(), 4u); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2); 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); 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::Distance); 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::Distance | laser_geometry::channel_option::Timestamp); @@ -369,22 +386,28 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { } for (unsigned int i = 0; i < cloud_out.width; i++) { - EXPECT_NEAR(cloudData(cloud_out, i * cloud_out.point_step + x_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), + 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), + EXPECT_NEAR( + cloudData(cloud_out, i * cloud_out.point_step + intensity_offset), scan.intensities[i], tolerance); // intensity - EXPECT_NEAR(cloudData(cloud_out, 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(cloudData(cloud_out, 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(cloudData(cloud_out, 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) {