void projectLaser(const sensor_msgs::LaserScan& scan_in,
std::vector<Point> &points_out, const float range_cutoff = 30.0f)
{
size_t n_points = scan_in.ranges.size();
if (co_sine_map_.rows() != n_points || angle_min_ != scan_in.angle_min
|| angle_max_ != scan_in.angle_max)
{
ROS_INFO("Precomputed map built");
co_sine_map_ = Eigen::ArrayXXf(n_points, 2);
angle_min_ = scan_in.angle_min;
angle_max_ = scan_in.angle_max;
for (size_t i = 0; i < n_points; ++i)
{
co_sine_map_(i, 0) =
cos(
angle_min_
+ static_cast<float>(i
* scan_in.angle_increment));
co_sine_map_(i, 1) =
sin(
angle_min_
+ static_cast<float>(i
* scan_in.angle_increment));
}
}
if (points_out.size() != n_points)
{
ROS_INFO("Resize points container");
points_out.clear();
points_out.resize(n_points);
}
float range;
for (size_t i = 0; i < n_points; i++)
{
if(std::isinf(scan_in.ranges[i]))
{
range = 0;
}
else if(std::isnan(scan_in.ranges[i]))
{
ROS_ERROR("Laser data has a nan value!");
}
else
{
range = scan_in.ranges[i] > range_cutoff ?
range_cutoff : scan_in.ranges[i];
}
points_out[i].x() = range * co_sine_map_(i, 0);
points_out[i].y() = range * co_sine_map_(i, 1);
}
return;
}
void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::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 = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[1].name = "y";
cloud_out.fields[1].offset = 4;
cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[2].name = "z";
cloud_out.fields[2].offset = 8;
cloud_out.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
// 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 = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
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 = sensor_msgs::PointField::INT32;
cloud_out.fields[field_size].offset = offset;
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 = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
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 = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
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 = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
offset += 4;
cloud_out.fields[field_size + 1].name = "vp_y";
cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size + 1].offset = offset;
offset += 4;
cloud_out.fields[field_size + 2].name = "vp_z";
cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size + 2].offset = offset;
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;
//TODO: Find out why this was needed
//float bad_point = std::numeric_limits<float>::quiet_NaN ();
if (range_cutoff < 0)
range_cutoff = scan_in.range_max;
else
range_cutoff = std::min(range_cutoff, (double)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
if (scan_in.ranges[i] <= range_cutoff && scan_in.ranges[i] >= 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] = scan_in.ranges[i];
// 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);
}
