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

265
include/laser_geometry/laser_geometry.hpp
View File

@ -49,148 +49,153 @@
namespace laser_geometry
{
// NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
const float LASER_SCAN_INVALID = -1.0;
const float LASER_SCAN_MIN_RANGE = -2.0;
const float LASER_SCAN_MAX_RANGE = -3.0;
// NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
const float LASER_SCAN_INVALID = -1.0;
const float LASER_SCAN_MIN_RANGE = -2.0;
const float LASER_SCAN_MAX_RANGE = -3.0;
namespace channel_option
{
//! Enumerated output channels options.
namespace channel_option
{
//! Enumerated output channels options.
/*!
* An OR'd set of these options is passed as the final argument of
* the projectLaser and transformLaserScanToPointCloud calls to
* enable generation of the appropriate set of additional channels.
*/
enum ChannelOption
{
None = 0x00, //!< Enable no channels
Intensity = 0x01, //!< Enable "intensities" channel
Index = 0x02, //!< Enable "index" channel
Distance = 0x04, //!< Enable "distances" channel
Timestamp = 0x08, //!< Enable "stamps" channel
Viewpoint = 0x10, //!< Enable "viewpoint" channel
Default = (Intensity | Index) //!< Enable "intensities" and "index" channels
};
}
//! \brief A Class to Project Laser Scan
/*!
* This class will project laser scans into point clouds. It caches
* unit vectors between runs (provided the angular resolution of
* your scanner is not changing) to avoid excess computation.
*
* By default all range values less than the scanner min_range, and
* greater than the scanner max_range are removed from the generated
* point cloud, as these are assumed to be invalid.
*
* If it is important to preserve a mapping between the index of
* range values and points in the cloud, the recommended approach is
* to pre-filter your laser_scan message to meet the requiremnt that all
* ranges are between min and max_range.
*
* The generated PointClouds have a number of channels which can be enabled
* through the use of ChannelOption.
* - channel_option::Intensity - Create a channel named "intensities" with the intensity of the return for each point
* - channel_option::Index - Create a channel named "index" containing the index from the original array for each point
* - channel_option::Distance - Create a channel named "distances" containing the distance from the laser to each point
* - channel_option::Timestamp - Create a channel named "stamps" containing the specific timestamp at which each point was measured
*/
class LaserProjection
{
public:
LaserProjection()
: angle_min_(0), angle_max_(0) {}
//! Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2
/*!
* An OR'd set of these options is passed as the final argument of
* the projectLaser and transformLaserScanToPointCloud calls to
* enable generation of the appropriate set of additional channels.
* Project a single laser scan from a linear array into a 3D
* point cloud. The generated cloud will be in the same frame
* as the original laser scan.
*
* \param scan_in The input laser scan
* \param cloud_out The output point cloud
* \param range_cutoff An additional range cutoff which can be
* applied to discard everything above it.
* Defaults to -1.0, which means the laser scan max range.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
enum ChannelOption
{
None = 0x00, //!< Enable no channels
Intensity = 0x01, //!< Enable "intensities" channel
Index = 0x02, //!< Enable "index" channel
Distance = 0x04, //!< Enable "distances" channel
Timestamp = 0x08, //!< Enable "stamps" channel
Viewpoint = 0x10, //!< Enable "viewpoint" channel
Default = (Intensity | Index) //!< Enable "intensities" and "index" channels
};
void projectLaser(
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
{
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
}
//! \brief A Class to Project Laser Scan
//! Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame
/*!
* This class will project laser scans into point clouds. It caches
* unit vectors between runs (provided the angular resolution of
* your scanner is not changing) to avoid excess computation.
* Transform a single laser scan from a linear array into a 3D
* point cloud, accounting for movement of the laser over the
* course of the scan. In order for this transform to be
* meaningful at a single point in time, the target_frame must
* be a fixed reference frame. See the tf documentation for
* more information on fixed frames.
*
* By default all range values less than the scanner min_range, and
* greater than the scanner max_range are removed from the generated
* point cloud, as these are assumed to be invalid.
*
* If it is important to preserve a mapping between the index of
* range values and points in the cloud, the recommended approach is
* to pre-filter your laser_scan message to meet the requiremnt that all
* ranges are between min and max_range.
*
* The generated PointClouds have a number of channels which can be enabled
* through the use of ChannelOption.
* - channel_option::Intensity - Create a channel named "intensities" with the intensity of the return for each point
* - channel_option::Index - Create a channel named "index" containing the index from the original array for each point
* - channel_option::Distance - Create a channel named "distances" containing the distance from the laser to each point
* - channel_option::Timestamp - Create a channel named "stamps" containing the specific timestamp at which each point was measured
* \param target_frame The frame of the resulting point cloud
* \param scan_in The input laser scan
* \param cloud_out The output point cloud
* \param tf a tf2::BufferCore object to use to perform the
* transform
* \param range_cutoff An additional range cutoff which can be
* applied to discard everything above it.
* Defaults to -1.0, which means the laser scan max range.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
class LaserProjection
{
void transformLaserScanToPointCloud(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
tf2::BufferCore & tf,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
{
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff,
channel_options);
}
public:
private:
//! 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);
LaserProjection() : angle_min_(0), angle_max_(0) {}
//! Internal hidden representation of transformLaserScanToPointCloud2
void transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
tf2::BufferCore & tf,
double range_cutoff,
int channel_options);
//! 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
* as the original laser scan.
*
* \param scan_in The input laser scan
* \param cloud_out The output point cloud
* \param range_cutoff An additional range cutoff which can be
* applied to discard everything above it.
* Defaults to -1.0, which means the laser scan max range.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
void projectLaser (const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::msg::PointCloud2 &cloud_out,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
{
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
}
//! Function used by the several forms of transformLaserScanToPointCloud_
void transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
tf2::Quaternion quat_start,
tf2::Vector3 origin_start,
tf2::Quaternion quat_end,
tf2::Vector3 origin_end,
double range_cutoff,
int channel_options);
//! 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
* course of the scan. In order for this transform to be
* meaningful at a single point in time, the target_frame must
* be a fixed reference frame. See the tf documentation for
* more information on fixed frames.
*
* \param target_frame The frame of the resulting point cloud
* \param scan_in The input laser scan
* \param cloud_out The output point cloud
* \param tf a tf2::BufferCore object to use to perform the
* transform
* \param range_cutoff An additional range cutoff which can be
* applied to discard everything above it.
* Defaults to -1.0, which means the laser scan max range.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
void transformLaserScanToPointCloud(const std::string &target_frame,
const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::BufferCore &tf,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
{
transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
}
private:
//! 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
void transformLaserScanToPointCloud_ (const std::string &target_frame,
const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::BufferCore &tf,
double range_cutoff,
int channel_options);
//! Function used by the several forms of transformLaserScanToPointCloud_
void transformLaserScanToPointCloud_ (const std::string &target_frame,
const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::Quaternion quat_start,
tf2::Vector3 origin_start,
tf2::Quaternion quat_end,
tf2::Vector3 origin_end,
double range_cutoff,
int channel_options);
//! Internal map of pointers to stored values
float angle_min_;
float angle_max_;
Eigen::ArrayXXd co_sine_map_;
};
//! Internal map of pointers to stored values
float angle_min_;
float angle_max_;
Eigen::ArrayXXd co_sine_map_;
};
}

774
src/laser_geometry.cpp
View File

@ -42,424 +42,418 @@ typedef double tfScalar;
namespace laser_geometry
{
void LaserProjection::projectLaser_ (const sensor_msgs::msg::LaserScan& scan_in,
sensor_msgs::msg::PointCloud2 &cloud_out,
double range_cutoff,
int channel_options)
{
size_t n_pts = scan_in.ranges.size ();
Eigen::ArrayXXd ranges (n_pts, 2);
Eigen::ArrayXXd output (n_pts, 2);
void LaserProjection::projectLaser_(
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
double range_cutoff,
int channel_options)
{
size_t n_pts = scan_in.ranges.size();
Eigen::ArrayXXd ranges(n_pts, 2);
Eigen::ArrayXXd output(n_pts, 2);
// 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];
}
// 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 )
{
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);
}
}
output = ranges * co_sine_map_;
// Set the output cloud accordingly
cloud_out.header = scan_in.header;
cloud_out.height = 1;
cloud_out.width = scan_in.ranges.size ();
cloud_out.fields.resize (3);
cloud_out.fields[0].name = "x";
cloud_out.fields[0].offset = 0;
cloud_out.fields[0].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[0].count = 1;
cloud_out.fields[1].name = "y";
cloud_out.fields[1].offset = 4;
cloud_out.fields[1].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[1].count = 1;
cloud_out.fields[2].name = "z";
cloud_out.fields[2].offset = 8;
cloud_out.fields[2].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[2].count = 1;
// 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;
//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();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "intensity";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_intensity = field_size;
}
if ((channel_options & channel_option::Index))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "index";
cloud_out.fields[field_size].datatype = POINT_FIELD::INT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_index = field_size;
}
if ((channel_options & channel_option::Distance))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "distances";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_distance = field_size;
}
if ((channel_options & channel_option::Timestamp))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "stamps";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_timestamp = field_size;
}
if ((channel_options & channel_option::Viewpoint))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 3);
cloud_out.fields[field_size].name = "vp_x";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
cloud_out.fields[field_size + 1].name = "vp_y";
cloud_out.fields[field_size + 1].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size + 1].offset = offset;
cloud_out.fields[field_size + 1].count = 1;
offset += 4;
cloud_out.fields[field_size + 2].name = "vp_z";
cloud_out.fields[field_size + 2].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size + 2].offset = offset;
cloud_out.fields[field_size + 2].count = 1;
offset += 4;
idx_vpx = field_size;
idx_vpy = field_size + 1;
idx_vpz = field_size + 2;
}
cloud_out.point_step = offset;
cloud_out.row_step = cloud_out.point_step * cloud_out.width;
cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
cloud_out.is_dense = false;
if (range_cutoff < 0)
range_cutoff = scan_in.range_max;
unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i)
{
//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];
// Copy XYZ
pstep[0] = output (i, 0);
pstep[1] = output (i, 1);
pstep[2] = 0;
// Copy intensity
if(idx_intensity != -1)
pstep[idx_intensity] = scan_in.intensities[i];
//Copy index
if(idx_index != -1)
((int*)(pstep))[idx_index] = i;
// Copy distance
if(idx_distance != -1)
pstep[idx_distance] = range;
// Copy timestamp
if(idx_timestamp != -1)
pstep[idx_timestamp] = i * scan_in.time_increment;
// Copy viewpoint (0, 0, 0)
if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1)
{
pstep[idx_vpx] = 0;
pstep[idx_vpy] = 0;
pstep[idx_vpz] = 0;
}
//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?
*
// Invalid measurement?
if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
{
if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE)
{
for (size_t s = 0; s < cloud_out.fields.size (); ++s)
pstep[s] = bad_point;
}
else
{
// Kind of nasty thing:
// We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid.
// Since we still might need the x value we store it in the distance field
pstep[0] = bad_point; // X -> NAN to mark a bad point
pstep[1] = co_sine_map (i, 1); // Y
pstep[2] = 0; // Z
if (store_intensity)
{
pstep[3] = bad_point; // Intensity -> NAN to mark a bad point
pstep[4] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X
}
else
pstep[3] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X
}
}
*/
}
//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);
// 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];
}
void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame,
const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::Quaternion quat_start,
tf2::Vector3 origin_start,
tf2::Quaternion quat_end,
tf2::Vector3 origin_end,
double range_cutoff,
int channel_options)
// 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)
{
//check if the user has requested the index field
bool requested_index = false;
if ((channel_options & channel_option::Index))
requested_index = true;
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);
}
}
//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;
output = ranges * co_sine_map_;
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
// Set the output cloud accordingly
cloud_out.header = scan_in.header;
cloud_out.height = 1;
cloud_out.width = scan_in.ranges.size();
cloud_out.fields.resize(3);
cloud_out.fields[0].name = "x";
cloud_out.fields[0].offset = 0;
cloud_out.fields[0].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[0].count = 1;
cloud_out.fields[1].name = "y";
cloud_out.fields[1].offset = 4;
cloud_out.fields[1].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[1].count = 1;
cloud_out.fields[2].name = "z";
cloud_out.fields[2].offset = 8;
cloud_out.fields[2].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[2].count = 1;
//we'll assume no associated viewpoint by default
bool has_viewpoint = false;
uint32_t vp_x_offset = 0;
// 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;
//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;
//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();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "intensity";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_intensity = field_size;
}
if ((channel_options & channel_option::Index)) {
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "index";
cloud_out.fields[field_size].datatype = POINT_FIELD::INT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_index = field_size;
}
if ((channel_options & channel_option::Distance)) {
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "distances";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_distance = field_size;
}
if ((channel_options & channel_option::Timestamp)) {
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "stamps";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
idx_timestamp = field_size;
}
if ((channel_options & channel_option::Viewpoint)) {
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 3);
cloud_out.fields[field_size].name = "vp_x";
cloud_out.fields[field_size].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1;
offset += 4;
cloud_out.fields[field_size + 1].name = "vp_y";
cloud_out.fields[field_size + 1].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size + 1].offset = offset;
cloud_out.fields[field_size + 1].count = 1;
offset += 4;
cloud_out.fields[field_size + 2].name = "vp_z";
cloud_out.fields[field_size + 2].datatype = POINT_FIELD::FLOAT32;
cloud_out.fields[field_size + 2].offset = offset;
cloud_out.fields[field_size + 2].count = 1;
offset += 4;
idx_vpx = field_size;
idx_vpy = field_size + 1;
idx_vpz = field_size + 2;
}
cloud_out.point_step = offset;
cloud_out.row_step = cloud_out.point_step * cloud_out.width;
cloud_out.data.resize(cloud_out.row_step * cloud_out.height);
cloud_out.is_dense = false;
if (range_cutoff < 0) {
range_cutoff = scan_in.range_max;
}
unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i) {
//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];
// Copy XYZ
pstep[0] = output(i, 0);
pstep[1] = output(i, 1);
pstep[2] = 0;
// Copy intensity
if (idx_intensity != -1) {
pstep[idx_intensity] = scan_in.intensities[i];
}
//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;
//Copy index
if (idx_index != -1) {
((int *)(pstep))[idx_index] = i;
}
// Copy distance
if (idx_distance != -1) {
pstep[idx_distance] = range;
}
// Copy timestamp
if (idx_timestamp != -1) {
pstep[idx_timestamp] = i * scan_in.time_increment;
}
// Copy viewpoint (0, 0, 0)
if (idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1) {
pstep[idx_vpx] = 0;
pstep[idx_vpy] = 0;
pstep[idx_vpz] = 0;
}
//make sure to increment count
++count;
}
ROS_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);
//we want to loop through all the points in the cloud
for(size_t i = 0; i < cloud_out.width; ++i)
/* 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?
*
// Invalid measurement?
if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
{
// Apply the transform to the current point
float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE)
{
for (size_t s = 0; s < cloud_out.fields.size (); ++s)
pstep[s] = bad_point;
}
else
{
// Kind of nasty thing:
// We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid.
// Since we still might need the x value we store it in the distance field
pstep[0] = bad_point; // X -> NAN to mark a bad point
pstep[1] = co_sine_map (i, 1); // Y
pstep[2] = 0; // Z
//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));
if (store_intensity)
{
pstep[3] = bad_point; // Intensity -> NAN to mark a bad point
pstep[4] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X
}
else
pstep[3] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X
}
}
*/
}
// Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
tfScalar ratio = pt_index * ranges_norm;
//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);
}
//! \todo 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);
cur_transform.setOrigin (v);
void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
tf2::Quaternion quat_start,
tf2::Vector3 origin_start,
tf2::Quaternion quat_end,
tf2::Vector3 origin_end,
double range_cutoff,
int channel_options)
{
//check if the user has requested the index field
bool requested_index = false;
if ((channel_options & channel_option::Index)) {
requested_index = true;
}
// Compute the slerp-ed rotation
cur_transform.setRotation (slerp (quat_start, quat_end , ratio));
//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;
tf2::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
tf2::Vector3 point_out = cur_transform * point_in;
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
//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
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
if (cloud_out.fields[i].name == "vp_x") {
has_viewpoint = true;
vp_x_offset = cloud_out.fields[i].offset;
}
}
ROS_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);
//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];
//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
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)
// Interpolate translation
tf2::Vector3 v(0, 0, 0);
v.setInterpolate3(origin_start, origin_end, ratio);
cur_transform.setOrigin(v);
// Compute the slerp-ed rotation
cur_transform.setRotation(slerp(quat_start, quat_end, ratio));
tf2::Vector3 point_in(pstep[0], pstep[1], pstep[2]);
tf2::Vector3 point_out = cur_transform * point_in;
// Copy transformed point into cloud
pstep[0] = point_out.x();
pstep[1] = point_out.y();
pstep[2] = point_out.z();
// Convert the viewpoint as well
if (has_viewpoint) {
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_out = cur_transform * point_in;
// Copy transformed point into cloud
pstep[0] = point_out.x ();
pstep[1] = point_out.y ();
pstep[2] = point_out.z ();
// Convert the viewpoint as well
if(has_viewpoint)
{
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_out = cur_transform * point_in;
// Copy transformed point into cloud
vpstep[0] = point_out.x ();
vpstep[1] = point_out.y ();
vpstep[2] = point_out.z ();
}
}
//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
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
cloud_without_index.fields.resize(cloud_out.fields.size());
unsigned int field_count = 0;
unsigned int offset_shift = 0;
for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
{
if(cloud_out.fields[i].name != "index")
{
cloud_without_index.fields[field_count] = cloud_out.fields[i];
cloud_without_index.fields[field_count].offset -= offset_shift;
++field_count;
}
else
{
//once we hit the index, we'll set the shift
offset_shift = 4;
}
}
//resize the fields
cloud_without_index.fields.resize(field_count);
//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
while (i < cloud_out.data.size())
{
if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4))
{
cloud_without_index.data[j++] = cloud_out.data[i];
}
i++;
}
//make sure to actually set the output
cloud_out = cloud_without_index;
vpstep[0] = point_out.x();
vpstep[1] = point_out.y();
vpstep[2] = point_out.z();
}
}
void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
const sensor_msgs::msg::LaserScan &scan_in,
sensor_msgs::msg::PointCloud2 &cloud_out,
tf2::BufferCore &tf,
double range_cutoff,
int channel_options)
{
TIME start_time = scan_in.header.stamp;
TIME end_time = scan_in.header.stamp;
// TODO: 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);
//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
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
cloud_without_index.fields.resize(cloud_out.fields.size());
unsigned int field_count = 0;
unsigned int offset_shift = 0;
for (unsigned int i = 0; i < cloud_out.fields.size(); ++i) {
if (cloud_out.fields[i].name != "index") {
cloud_without_index.fields[field_count] = cloud_out.fields[i];
cloud_without_index.fields[field_count].offset -= offset_shift;
++field_count;
} else {
//once we hit the index, we'll set the shift
offset_shift = 4;
}
}
std::chrono::milliseconds start(start_time.nanoseconds());
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);
std::chrono::milliseconds end(end_time.nanoseconds());
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);
//resize the fields
cloud_without_index.fields.resize(field_count);
tf2::Quaternion quat_start(start_transform.transform.rotation.x,
start_transform.transform.rotation.y,
start_transform.transform.rotation.z,
start_transform.transform.rotation.w);
tf2::Quaternion quat_end(end_transform.transform.rotation.x,
end_transform.transform.rotation.y,
end_transform.transform.rotation.z,
end_transform.transform.rotation.w);
//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);
tf2::Vector3 origin_start(start_transform.transform.translation.x,
start_transform.transform.translation.y,
start_transform.transform.translation.z);
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,
quat_start, origin_start,
quat_end, origin_end,
range_cutoff,
channel_options);
uint32_t i = 0;
uint32_t j = 0;
//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))
{
cloud_without_index.data[j++] = cloud_out.data[i];
}
i++;
}
//make sure to actually set the output
cloud_out = cloud_without_index;
}
}
void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
tf2::BufferCore & tf,
double range_cutoff,
int channel_options)
{
TIME start_time = scan_in.header.stamp;
TIME end_time = scan_in.header.stamp;
// TODO: 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);
}
std::chrono::milliseconds start(start_time.nanoseconds());
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);
std::chrono::milliseconds end(end_time.nanoseconds());
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);
tf2::Quaternion quat_start(start_transform.transform.rotation.x,
start_transform.transform.rotation.y,
start_transform.transform.rotation.z,
start_transform.transform.rotation.w);
tf2::Quaternion quat_end(end_transform.transform.rotation.x,
end_transform.transform.rotation.y,
end_transform.transform.rotation.z,
end_transform.transform.rotation.w);
tf2::Vector3 origin_start(start_transform.transform.translation.x,
start_transform.transform.translation.y,
start_transform.transform.translation.z);
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,
quat_start, origin_start,
quat_end, origin_end,
range_cutoff,
channel_options);
}
} //laser_geometry

368
test/projection_test.cpp
View File

@ -40,9 +40,12 @@
#define PROJECTION_TEST_RANGE_MAX (40.0)
class BuildScanException { };
class BuildScanException
{
};
struct ScanOptions {
struct ScanOptions
{
double range_;
double intensity_;
double ang_min_;
@ -50,10 +53,11 @@ struct ScanOptions {
double ang_increment_;
rclcpp::Duration scan_time_;
ScanOptions(double range, double intensity,
ScanOptions(
double range, double intensity,
double ang_min, double ang_max, double ang_increment,
rclcpp::Duration scan_time) :
range_(range),
rclcpp::Duration scan_time)
: range_(range),
intensity_(intensity),
ang_min_(ang_min),
ang_max_(ang_max),
@ -63,8 +67,9 @@ struct ScanOptions {
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());
}
sensor_msgs::msg::LaserScan scan;
scan.header.stamp = rclcpp::Clock().now();
@ -76,19 +81,17 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.range_min = PROJECTION_TEST_RANGE_MIN;
scan.range_max = PROJECTION_TEST_RANGE_MAX;
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.intensities.push_back(options.intensity_);
}
scan.time_increment = options.scan_time_.nanoseconds()/(double)i;
scan.time_increment = options.scan_time_.nanoseconds() / (double)i;
return scan;
};
}
TEST(laser_geometry, projectLaser2)
{
TEST(laser_geometry, projectLaser2) {
double tolerance = 1e-12;
laser_geometry::LaserProjection projector;
@ -110,34 +113,34 @@ TEST(laser_geometry, projectLaser2)
intensities.push_back(5.0);
min_angles.push_back(-M_PI);
min_angles.push_back(-M_PI/1.5);
min_angles.push_back(-M_PI/2);
min_angles.push_back(-M_PI/4);
min_angles.push_back(-M_PI/8);
min_angles.push_back(-M_PI / 1.5);
min_angles.push_back(-M_PI / 2);
min_angles.push_back(-M_PI / 4);
min_angles.push_back(-M_PI / 8);
max_angles.push_back(M_PI);
max_angles.push_back(M_PI/1.5);
max_angles.push_back(M_PI/2);
max_angles.push_back(M_PI/4);
max_angles.push_back(M_PI/8);
max_angles.push_back(M_PI / 1.5);
max_angles.push_back(M_PI / 2);
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 / 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));
scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20));
std::vector<ScanOptions> options;
for(auto range : ranges) {
for(auto intensity : intensities) {
for(auto min_angle : min_angles) {
for(auto max_angle : max_angles) {
for(auto angle_increment : angle_increments) {
for(auto scan_time : scan_times) {
for (auto range : ranges) {
for (auto intensity : intensities) {
for (auto min_angle : min_angles) {
for (auto max_angle : max_angles) {
for (auto angle_increment : angle_increments) {
for (auto scan_time : scan_times) {
options.push_back(ScanOptions(
range, intensity, min_angle, max_angle, angle_increment, scan_time));
range, intensity, min_angle, max_angle, angle_increment, scan_time));
}
}
}
@ -145,66 +148,86 @@ TEST(laser_geometry, projectLaser2)
}
}
for (auto option : options){
try
{
// printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::msg::LaserScan scan = build_constant_scan(option);
for (auto option : options) {
try {
// printf("%f %f %f %f %f %f\n",
// range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::msg::LaserScan scan = build_constant_scan(option);
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);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
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);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.projectLaser(scan, cloud_out, -1.0);
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);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.projectLaser(scan, cloud_out, -1.0);
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);
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);
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);
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);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
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);
unsigned int valid_points = 0;
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)
valid_points ++;
unsigned int valid_points = 0;
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)
{
valid_points++;
}
}
EXPECT_EQ(valid_points, cloud_out.width);
EXPECT_EQ(valid_points, cloud_out.width);
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
uint32_t index_offset = 0;
uint32_t distance_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++) {
if (f->name == "x") x_offset = f->offset;
if (f->name == "y") y_offset = f->offset;
if (f->name == "z") z_offset = f->offset;
if (f->name == "intensity") intensity_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;
}
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
uint32_t index_offset = 0;
uint32_t distance_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++)
{
if (f->name == "x") {x_offset = f->offset;}
if (f->name == "y") {y_offset = f->offset;}
if (f->name == "z") {z_offset = f->offset;}
if (f->name == "intensity") {intensity_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 + 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] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, 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)
{
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],
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
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) {
// make sure it is not a false exception
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
FAIL();
@ -213,8 +236,7 @@ TEST(laser_geometry, projectLaser2)
}
}
TEST(laser_geometry, transformLaserScanToPointCloud2)
{
TEST(laser_geometry, transformLaserScanToPointCloud2) {
tf2::BufferCore tf2;
@ -239,35 +261,34 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
intensities.push_back(5.0);
min_angles.push_back(-M_PI);
min_angles.push_back(-M_PI/1.5);
min_angles.push_back(-M_PI/2);
min_angles.push_back(-M_PI/4);
min_angles.push_back(-M_PI/8);
min_angles.push_back(-M_PI / 1.5);
min_angles.push_back(-M_PI / 2);
min_angles.push_back(-M_PI / 4);
min_angles.push_back(-M_PI / 8);
max_angles.push_back(M_PI);
max_angles.push_back(M_PI/1.5);
max_angles.push_back(M_PI/2);
max_angles.push_back(M_PI/4);
max_angles.push_back(M_PI/8);
max_angles.push_back(M_PI / 1.5);
max_angles.push_back(M_PI / 2);
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
scan_times.push_back(rclcpp::Duration(1/40));
scan_times.push_back(rclcpp::Duration(1/20));
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));
std::vector<ScanOptions> options;
for(auto range : ranges) {
for(auto intensity : intensities) {
for(auto min_angle : min_angles) {
for(auto max_angle : max_angles) {
for(auto angle_increment : angle_increments) {
for(auto scan_time : scan_times) {
for (auto range : ranges) {
for (auto intensity : intensities) {
for (auto min_angle : min_angles) {
for (auto max_angle : max_angles) {
for (auto angle_increment : angle_increments) {
for (auto scan_time : scan_times) {
options.push_back(ScanOptions(
range, intensity, min_angle, max_angle, angle_increment, scan_time));
range, intensity, min_angle, max_angle, angle_increment, scan_time));
}
}
}
@ -275,73 +296,93 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
}
}
for (auto option : options){
try
{
// printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
for (auto option : options) {
try {
// printf("%f %f %f %f %f %f\n",
// range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::msg::LaserScan scan = build_constant_scan(option);
scan.header.frame_id = "laser_frame";
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);
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);
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);
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);
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);
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);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
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);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
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);
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);
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);
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);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
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.is_dense, false);
EXPECT_EQ(cloud_out.is_dense, false);
unsigned int valid_points = 0;
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)
valid_points ++;
EXPECT_EQ(valid_points, cloud_out.width);
unsigned int valid_points = 0;
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)
{
valid_points++;
}
}
EXPECT_EQ(valid_points, cloud_out.width);
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
uint32_t index_offset = 0;
uint32_t distance_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++)
{
if (f->name == "x") x_offset = f->offset;
if (f->name == "y") y_offset = f->offset;
if (f->name == "z") z_offset = f->offset;
if (f->name == "intensity") intensity_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;
}
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
uint32_t index_offset = 0;
uint32_t distance_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++)
{
if (f->name == "x") {x_offset = f->offset;}
if (f->name == "y") {y_offset = f->offset;}
if (f->name == "z") {z_offset = f->offset;}
if (f->name == "intensity") {intensity_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++)
{
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] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, 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
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],
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
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
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
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);
return RUN_ALL_TESTS();
}