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

Uncrustify

Browse Source
This commit is contained in:
Martin Idel 2018-03-14 13:36:50 +01:00
parent b88330b64c
commit 433181e63e
3 changed files with 730 additions and 689 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
include/laser_geometry/laser_geometry.hpp
View File

@ -100,8 +100,8 @@ namespace laser_geometry
{ {
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
/*! /*!
@ -119,7 +119,8 @@ 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::msg::LaserScan& scan_in, void projectLaser(
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::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)
@ -149,26 +150,29 @@ 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 transformLaserScanToPointCloud(const std::string &target_frame, void transformLaserScanToPointCloud(
const std::string & target_frame,
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,
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)
{ {
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: private:
//! Internal hidden representation of projectLaser //! Internal hidden representation of projectLaser
void projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, void projectLaser_(
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_ (const std::string &target_frame, void transformLaserScanToPointCloud_(
const std::string & target_frame,
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,
tf2::BufferCore & tf, tf2::BufferCore & tf,
@ -176,7 +180,8 @@ namespace laser_geometry
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::msg::LaserScan & scan_in, const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out, sensor_msgs::msg::PointCloud2 & cloud_out,
tf2::Quaternion quat_start, tf2::Quaternion quat_start,

108
src/laser_geometry.cpp
View File

@ -42,7 +42,8 @@ typedef double tfScalar;
namespace laser_geometry namespace laser_geometry
{ {
void LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in, void LaserProjection::projectLaser_(
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)
@ -52,22 +53,21 @@ namespace laser_geometry
Eigen::ArrayXXd output(n_pts, 2); Eigen::ArrayXXd output(n_pts, 2);
// 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) = (double) scan_in.ranges[i];
ranges(i, 1) = (double) scan_in.ranges[i]; ranges(i, 1) = (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 || angle_max_ != scan_in.angle_max ) if (co_sine_map_.rows() != (int)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); 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) = 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, 1) = sin(scan_in.angle_min + (double) i * scan_in.angle_increment);
} }
@ -94,12 +94,12 @@ namespace laser_geometry
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
int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1; 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; int offset = 12;
if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0) {
{
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";
@ -110,8 +110,7 @@ namespace laser_geometry
idx_intensity = field_size; idx_intensity = field_size;
} }
if ((channel_options & channel_option::Index)) if ((channel_options & channel_option::Index)) {
{
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";
@ -122,8 +121,7 @@ namespace laser_geometry
idx_index = field_size; idx_index = field_size;
} }
if ((channel_options & channel_option::Distance)) if ((channel_options & channel_option::Distance)) {
{
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";
@ -134,8 +132,7 @@ namespace laser_geometry
idx_distance = field_size; idx_distance = field_size;
} }
if ((channel_options & channel_option::Timestamp)) if ((channel_options & channel_option::Timestamp)) {
{
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";
@ -146,8 +143,7 @@ namespace laser_geometry
idx_timestamp = field_size; idx_timestamp = field_size;
} }
if ((channel_options & channel_option::Viewpoint)) if ((channel_options & channel_option::Viewpoint)) {
{
int field_size = cloud_out.fields.size(); int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 3); cloud_out.fields.resize(field_size + 3);
@ -179,16 +175,15 @@ namespace laser_geometry
cloud_out.data.resize(cloud_out.row_step * cloud_out.height); cloud_out.data.resize(cloud_out.row_step * cloud_out.height);
cloud_out.is_dense = false; cloud_out.is_dense = false;
if (range_cutoff < 0) if (range_cutoff < 0) {
range_cutoff = scan_in.range_max; range_cutoff = scan_in.range_max;
}
unsigned int count = 0; unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i) 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]; 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]; float * pstep = (float *)&cloud_out.data[count * cloud_out.point_step];
// Copy XYZ // Copy XYZ
@ -197,24 +192,27 @@ namespace laser_geometry
pstep[2] = 0; pstep[2] = 0;
// Copy intensity // Copy intensity
if(idx_intensity != -1) if (idx_intensity != -1) {
pstep[idx_intensity] = scan_in.intensities[i]; pstep[idx_intensity] = scan_in.intensities[i];
}
//Copy index //Copy index
if(idx_index != -1) if (idx_index != -1) {
((int *)(pstep))[idx_index] = i; ((int *)(pstep))[idx_index] = i;
}
// Copy distance // Copy distance
if(idx_distance != -1) if (idx_distance != -1) {
pstep[idx_distance] = range; pstep[idx_distance] = range;
}
// Copy timestamp // Copy timestamp
if(idx_timestamp != -1) if (idx_timestamp != -1) {
pstep[idx_timestamp] = i * scan_in.time_increment; pstep[idx_timestamp] = i * scan_in.time_increment;
}
// Copy viewpoint (0, 0, 0) // Copy viewpoint (0, 0, 0)
if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1) if (idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1) {
{
pstep[idx_vpx] = 0; pstep[idx_vpx] = 0;
pstep[idx_vpy] = 0; pstep[idx_vpy] = 0;
pstep[idx_vpz] = 0; pstep[idx_vpz] = 0;
@ -262,7 +260,8 @@ namespace laser_geometry
cloud_out.data.resize(cloud_out.row_step * cloud_out.height); cloud_out.data.resize(cloud_out.row_step * cloud_out.height);
} }
void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame, void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame,
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,
tf2::Quaternion quat_start, tf2::Quaternion quat_start,
@ -274,8 +273,9 @@ namespace laser_geometry
{ {
//check if the user has requested the index field //check if the user has requested the index field
bool requested_index = false; bool requested_index = false;
if ((channel_options & channel_option::Index)) if ((channel_options & channel_option::Index)) {
requested_index = true; requested_index = true;
}
//we'll enforce that we get index values for the laser scan so that we //we'll enforce that we get index values for the laser scan so that we
//ensure that we use the correct timestamps //ensure that we use the correct timestamps
@ -293,18 +293,15 @@ namespace laser_geometry
//putting in a check at some point, but I'm just going to put in an //putting in a check at some point, but I'm just going to put in an
//assert for now //assert for now
uint32_t index_offset = 0; uint32_t index_offset = 0;
for(unsigned int i = 0; i < cloud_out.fields.size(); ++i) for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) {
{ if (cloud_out.fields[i].name == "index") {
if(cloud_out.fields[i].name == "index")
{
index_offset = cloud_out.fields[i].offset; index_offset = cloud_out.fields[i].offset;
} }
//we want to check if the cloud has a viewpoint associated with it //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 //checking vp_x should be sufficient since vp_x, vp_y, and vp_z all
//get put in together //get put in together
if(cloud_out.fields[i].name == "vp_x") if (cloud_out.fields[i].name == "vp_x") {
{
has_viewpoint = true; has_viewpoint = true;
vp_x_offset = cloud_out.fields[i].offset; vp_x_offset = cloud_out.fields[i].offset;
} }
@ -319,8 +316,7 @@ namespace laser_geometry
double ranges_norm = 1 / ((double) scan_in.ranges.size() - 1.0); double ranges_norm = 1 / ((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 = (float *)&cloud_out.data[i * cloud_out.point_step + 0];
@ -349,8 +345,7 @@ namespace laser_geometry
pstep[2] = point_out.z(); pstep[2] = point_out.z();
// 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]; float * vpstep = (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;
@ -363,8 +358,7 @@ namespace laser_geometry
} }
//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::msg::PointCloud2 cloud_without_index; sensor_msgs::msg::PointCloud2 cloud_without_index;
//copy basic meta data //copy basic meta data
@ -378,16 +372,12 @@ namespace laser_geometry
cloud_without_index.fields.resize(cloud_out.fields.size()); cloud_without_index.fields.resize(cloud_out.fields.size());
unsigned int field_count = 0; unsigned int field_count = 0;
unsigned int offset_shift = 0; unsigned int offset_shift = 0;
for(unsigned int i = 0; i < cloud_out.fields.size(); ++i) for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) {
{ if (cloud_out.fields[i].name != "index") {
if(cloud_out.fields[i].name != "index")
{
cloud_without_index.fields[field_count] = cloud_out.fields[i]; cloud_without_index.fields[field_count] = cloud_out.fields[i];
cloud_without_index.fields[field_count].offset -= offset_shift; cloud_without_index.fields[field_count].offset -= offset_shift;
++field_count; ++field_count;
} } else {
else
{
//once we hit the index, we'll set the shift //once we hit the index, we'll set the shift
offset_shift = 4; offset_shift = 4;
} }
@ -404,9 +394,9 @@ namespace laser_geometry
uint32_t i = 0; uint32_t i = 0;
uint32_t j = 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()) while (i < cloud_out.data.size()) {
{ if ((i % cloud_out.point_step) < index_offset ||
if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4)) (i % cloud_out.point_step) >= (index_offset + 4))
{ {
cloud_without_index.data[j++] = cloud_out.data[i]; cloud_without_index.data[j++] = cloud_out.data[i];
} }
@ -418,7 +408,8 @@ namespace laser_geometry
} }
} }
void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame,
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,
tf2::BufferCore & tf, tf2::BufferCore & tf,
@ -428,17 +419,20 @@ namespace laser_geometry
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: 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);
} }
std::chrono::milliseconds start(start_time.nanoseconds()); std::chrono::milliseconds start(start_time.nanoseconds());
std::chrono::time_point<std::chrono::system_clock> st(start); 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); 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::milliseconds end(end_time.nanoseconds());
std::chrono::time_point<std::chrono::system_clock> e(end); 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); 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,

182
test/projection_test.cpp
View File

@ -40,9 +40,12 @@
#define PROJECTION_TEST_RANGE_MAX (40.0) #define PROJECTION_TEST_RANGE_MAX (40.0)
class BuildScanException { }; class BuildScanException
{
};
struct ScanOptions { struct ScanOptions
{
double range_; double range_;
double intensity_; double intensity_;
double ang_min_; double ang_min_;
@ -50,10 +53,11 @@ struct ScanOptions {
double ang_increment_; double ang_increment_;
rclcpp::Duration scan_time_; rclcpp::Duration scan_time_;
ScanOptions(double range, double intensity, ScanOptions(
double range, double intensity,
double ang_min, double ang_max, double ang_increment, double ang_min, double ang_max, double ang_increment,
rclcpp::Duration scan_time) : rclcpp::Duration scan_time)
range_(range), : range_(range),
intensity_(intensity), intensity_(intensity),
ang_min_(ang_min), ang_min_(ang_min),
ang_max_(ang_max), ang_max_(ang_max),
@ -63,8 +67,9 @@ struct ScanOptions {
sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options) sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
{ {
if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) {
throw (BuildScanException()); throw (BuildScanException());
}
sensor_msgs::msg::LaserScan scan; sensor_msgs::msg::LaserScan scan;
scan.header.stamp = rclcpp::Clock().now(); scan.header.stamp = rclcpp::Clock().now();
@ -76,8 +81,7 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.range_min = PROJECTION_TEST_RANGE_MIN; scan.range_min = PROJECTION_TEST_RANGE_MIN;
scan.range_max = PROJECTION_TEST_RANGE_MAX; scan.range_max = PROJECTION_TEST_RANGE_MAX;
uint32_t i = 0; uint32_t i = 0;
for(; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++) for (; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++) {
{
scan.ranges.push_back(options.range_); scan.ranges.push_back(options.range_);
scan.intensities.push_back(options.intensity_); scan.intensities.push_back(options.intensity_);
} }
@ -85,10 +89,9 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.time_increment = options.scan_time_.nanoseconds() / (double)i; scan.time_increment = options.scan_time_.nanoseconds() / (double)i;
return scan; return scan;
}; }
TEST(laser_geometry, projectLaser2) TEST(laser_geometry, projectLaser2) {
{
double tolerance = 1e-12; double tolerance = 1e-12;
laser_geometry::LaserProjection projector; laser_geometry::LaserProjection projector;
@ -146,9 +149,9 @@ TEST(laser_geometry, projectLaser2)
} }
for (auto option : options) { for (auto option : options) {
try try {
{ // printf("%f %f %f %f %f %f\n",
// printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::msg::LaserScan scan = build_constant_scan(option); sensor_msgs::msg::LaserScan scan = build_constant_scan(option);
sensor_msgs::msg::PointCloud2 cloud_out; sensor_msgs::msg::PointCloud2 cloud_out;
@ -159,19 +162,31 @@ TEST(laser_geometry, projectLaser2)
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(), (unsigned int)5);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); 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(), (unsigned int)5);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); 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(), (unsigned int)6);
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); 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(), (unsigned int)7);
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++) {
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX &&
scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
{
valid_points++; valid_points++;
}
}
EXPECT_EQ(valid_points, cloud_out.width); EXPECT_EQ(valid_points, cloud_out.width);
@ -182,29 +197,37 @@ 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::msg::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();
if (f->name == "x") x_offset = f->offset; f != cloud_out.fields.end(); f++)
if (f->name == "y") y_offset = f->offset; {
if (f->name == "z") z_offset = f->offset; if (f->name == "x") {x_offset = f->offset;}
if (f->name == "intensity") intensity_offset = f->offset; if (f->name == "y") {y_offset = f->offset;}
if (f->name == "index") index_offset = f->offset; if (f->name == "z") {z_offset = f->offset;}
if (f->name == "distances") distance_offset = f->offset; if (f->name == "intensity") {intensity_offset = f->offset;}
if (f->name == "stamps") stamps_offset = f->offset; if (f->name == "index") {index_offset = f->offset;}
if (f->name == "distances") {distance_offset = f->offset;}
if (f->name == "stamps") {stamps_offset = f->offset;}
} }
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] , (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 + x_offset],
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); (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 + z_offset], 0, tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset],
EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps tolerance); // index
}; EXPECT_NEAR(*(float *)&cloud_out.data[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],
(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
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
FAIL(); FAIL();
@ -213,8 +236,7 @@ TEST(laser_geometry, projectLaser2)
} }
} }
TEST(laser_geometry, transformLaserScanToPointCloud2) TEST(laser_geometry, transformLaserScanToPointCloud2) {
{
tf2::BufferCore tf2; tf2::BufferCore tf2;
@ -258,7 +280,6 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
scan_times.push_back(rclcpp::Duration(1 / 40)); scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20)); scan_times.push_back(rclcpp::Duration(1 / 20));
std::vector<ScanOptions> options; std::vector<ScanOptions> options;
for (auto range : ranges) { for (auto range : ranges) {
for (auto intensity : intensities) { for (auto intensity : intensities) {
@ -276,38 +297,52 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
} }
for (auto option : options) { for (auto option : options) {
try try {
{ // printf("%f %f %f %f %f %f\n",
// printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::msg::LaserScan scan = build_constant_scan(option); sensor_msgs::msg::LaserScan scan = build_constant_scan(option);
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, laser_geometry::channel_option::None); 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(), (unsigned int)3);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index); 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(), (unsigned int)4);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity); 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(), (unsigned int)4);
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(), (unsigned int)5);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); 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(), (unsigned int)5);
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); 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(), (unsigned int)6);
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); 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(), (unsigned int)7);
EXPECT_EQ(cloud_out.is_dense, false); EXPECT_EQ(cloud_out.is_dense, false);
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++) {
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN) if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX &&
scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
{
valid_points++; valid_points++;
}
}
EXPECT_EQ(valid_points, cloud_out.width); EXPECT_EQ(valid_points, cloud_out.width);
uint32_t x_offset = 0; uint32_t x_offset = 0;
@ -317,31 +352,37 @@ 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::msg::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;}
if (f->name == "z") z_offset = f->offset; if (f->name == "z") {z_offset = f->offset;}
if (f->name == "intensity") intensity_offset = f->offset; if (f->name == "intensity") {intensity_offset = f->offset;}
if (f->name == "index") index_offset = f->offset; if (f->name == "index") {index_offset = f->offset;}
if (f->name == "distances") distance_offset = f->offset; if (f->name == "distances") {distance_offset = f->offset;}
if (f->name == "stamps") stamps_offset = f->offset; if (f->name == "stamps") {stamps_offset = f->offset;}
} }
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(*(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); (float)((double)(scan.ranges[i]) *
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); 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 + z_offset], 0, tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset],
EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps tolerance); // index
EXPECT_NEAR(*(float *)&cloud_out.data[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],
(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
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
FAIL(); FAIL();
@ -351,7 +392,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
} }
int main(int argc, char **argv){ int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }