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

code style only: wrap after open parenthesis if not in one line (#52)

Browse Source 
Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>
This commit is contained in:
Dirk Thomas 2020-02-04 11:43:50 -08:00 committed by GitHub
parent c2bdad5f01
commit f59e3fea7e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 54 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/laser_geometry/laser_geometry.hpp
View File

@ -159,8 +159,8 @@ public:
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
{ {
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, transformLaserScanToPointCloud_(
channel_options); target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
} }
private: private:

13
src/laser_geometry.cpp
View File

@ -433,14 +433,12 @@ void LaserProjection::transformLaserScanToPointCloud_(
std::chrono::nanoseconds start(start_time.nanoseconds()); std::chrono::nanoseconds start(start_time.nanoseconds());
std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> st(start); std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> st(start);
geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform(target_frame, geometry_msgs::msg::TransformStamped start_transform = tf.lookupTransform(
scan_in.header.frame_id, target_frame, scan_in.header.frame_id, st);
st);
std::chrono::nanoseconds end(end_time.nanoseconds()); std::chrono::nanoseconds end(end_time.nanoseconds());
std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> e(end); std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> e(end);
geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform(target_frame, geometry_msgs::msg::TransformStamped end_transform = tf.lookupTransform(
scan_in.header.frame_id, target_frame, scan_in.header.frame_id, e);
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,
@ -457,7 +455,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
tf2::Vector3 origin_end(end_transform.transform.translation.x, tf2::Vector3 origin_end(end_transform.transform.translation.x,
end_transform.transform.translation.y, end_transform.transform.translation.y,
end_transform.transform.translation.z); end_transform.transform.translation.z);
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, transformLaserScanToPointCloud_(
target_frame, scan_in, cloud_out,
quat_start, origin_start, quat_start, origin_start,
quat_end, origin_end, quat_end, origin_end,
range_cutoff, range_cutoff,

69
test/projection_test.cpp
View File

@ -139,7 +139,8 @@ TEST(laser_geometry, projectLaser2) {
for (auto max_angle : max_angles) { for (auto max_angle : max_angles) {
for (auto angle_increment : angle_increments) { for (auto angle_increment : angle_increments) {
for (auto scan_time : scan_times) { for (auto scan_time : scan_times) {
options.push_back(ScanOptions( options.push_back(
ScanOptions(
range, intensity, min_angle, max_angle, angle_increment, scan_time)); 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); projector.projectLaser(scan, cloud_out, -1.0);
EXPECT_EQ(cloud_out.fields.size(), 5u); 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(), 5u); 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(), 6u); 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 |
@ -210,22 +214,28 @@ 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(
cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) * static_cast<float>(static_cast<double>(scan.ranges[i]) *
cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))), cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance); tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset), EXPECT_NEAR(
cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) * static_cast<float>(static_cast<double>(scan.ranges[i]) *
sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))), sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance); 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 + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset), 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(cloudData<uint32_t>(cloud_out, 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(cloudData<float>(cloud_out, 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(cloudData<float>(cloud_out, 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 (const BuildScanException & ex) { } catch (const BuildScanException & ex) {
@ -291,7 +301,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
for (auto max_angle : max_angles) { for (auto max_angle : max_angles) {
for (auto angle_increment : angle_increments) { for (auto angle_increment : angle_increments) {
for (auto scan_time : scan_times) { for (auto scan_time : scan_times) {
options.push_back(ScanOptions( options.push_back(
ScanOptions(
range, intensity, min_angle, max_angle, angle_increment, scan_time)); range, intensity, min_angle, max_angle, angle_increment, scan_time));
} }
} }
@ -309,29 +320,35 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
scan.header.frame_id = "laser_frame"; scan.header.frame_id = "laser_frame";
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(), 3u); 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(), 4u); 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(), 4u); 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(), 5u); 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(), 5u); 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(), 6u); 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);
@ -369,22 +386,28 @@ 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(cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset), EXPECT_NEAR(
cloudData<float>(cloud_out, i * cloud_out.point_step + x_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) * static_cast<float>(static_cast<double>(scan.ranges[i]) *
cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))), cos(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance); tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset), EXPECT_NEAR(
cloudData<float>(cloud_out, i * cloud_out.point_step + y_offset),
static_cast<float>(static_cast<double>(scan.ranges[i]) * static_cast<float>(static_cast<double>(scan.ranges[i]) *
sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))), sin(static_cast<double>(scan.angle_min) + i * static_cast<double>(scan.angle_increment))),
tolerance); 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 + z_offset), 0, tolerance);
EXPECT_NEAR(cloudData<float>(cloud_out, i * cloud_out.point_step + intensity_offset), 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(cloudData<uint32_t>(cloud_out, 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(cloudData<float>(cloud_out, 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(cloudData<float>(cloud_out, 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) {