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

removes trailing white spaces and empty lines

Browse Source
This commit is contained in:
enriquefernandez 2014-06-04 13:48:30 +02:00
parent 072561a2f3
commit 3453bce486
2 changed files with 21 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
include/laser_geometry/laser_geometry.h
View File

@ -82,7 +82,7 @@ namespace laser_geometry
* By default all range values less than the scanner min_range, and * By default all range values less than the scanner min_range, and
* greater than the scanner max_range are removed from the generated * greater than the scanner max_range are removed from the generated
* point cloud, as these are assumed to be invalid. * point cloud, as these are assumed to be invalid.
* *
* If it is important to preserve a mapping between the index of * If it is important to preserve a mapping between the index of
* range values and points in the cloud, the recommended approach is * range values and points in the cloud, the recommended approach is
* to pre-filter your laser_scan message to meet the requiremnt that all * to pre-filter your laser_scan message to meet the requiremnt that all
@ -105,7 +105,6 @@ namespace laser_geometry
//! Destructor to deallocate stored unit vectors //! Destructor to deallocate stored unit vectors
~LaserProjection(); ~LaserProjection();
//! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud
/*! /*!
* Project a single laser scan from a linear array into a 3D * Project a single laser scan from a linear array into a 3D
@ -146,7 +145,7 @@ 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::LaserScan& scan_in, void projectLaser (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::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)
@ -236,8 +235,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 transformLaserScanToPointCloud(const std::string &target_frame, void transformLaserScanToPointCloud(const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff = -1.0, double range_cutoff = -1.0,
@ -269,7 +268,7 @@ namespace laser_geometry
int channel_options); int channel_options);
//! Internal hidden representation of projectLaser //! Internal hidden representation of projectLaser
void projectLaser_ (const sensor_msgs::LaserScan& scan_in, void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
@ -283,10 +282,10 @@ namespace laser_geometry
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::LaserScan &scan_in, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);

28
src/laser_geometry.cpp
View File

@ -1,10 +1,10 @@
/* /*
* Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved. * All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met: * modification, are permitted provided that the following conditions are met:
* *
* * Redistributions of source code must retain the above copyright * * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. * notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright * * Redistributions in binary form must reproduce the above copyright
@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. nor the names of its * * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from * contributors may be used to endorse or promote products derived from
* this software without specific prior written permission. * this software without specific prior written permission.
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@ -35,7 +35,7 @@ namespace laser_geometry
{ {
void void
LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff,
bool preservative, int mask) bool preservative, int mask)
{ {
boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size()); boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
@ -46,7 +46,6 @@ namespace laser_geometry
ranges(0,index) = (double) scan_in.ranges[index]; ranges(0,index) = (double) scan_in.ranges[index];
ranges(1,index) = (double) scan_in.ranges[index]; ranges(1,index) = (double) scan_in.ranges[index];
} }
//Do the projection //Do the projection
// NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment)); // NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
@ -59,7 +58,7 @@ namespace laser_geometry
// 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; int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
cloud_out.channels.resize(0); cloud_out.channels.resize(0);
// Check if the intensity bit is set // Check if the intensity bit is set
if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0) if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
@ -70,7 +69,7 @@ namespace laser_geometry
cloud_out.channels[0].values.resize (scan_in.intensities.size()); cloud_out.channels[0].values.resize (scan_in.intensities.size());
idx_intensity = 0; idx_intensity = 0;
} }
// Check if the index bit is set // Check if the index bit is set
if (mask & channel_option::Index) if (mask & channel_option::Index)
{ {
@ -141,7 +140,6 @@ namespace laser_geometry
count++; count++;
} }
} }
//downsize if necessary //downsize if necessary
cloud_out.points.resize (count); cloud_out.points.resize (count);
@ -168,7 +166,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
(*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment); (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment);
(*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment); (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment);
} }
//store //store
unit_vector_map_[anglestring.str()] = tempPtr; unit_vector_map_[anglestring.str()] = tempPtr;
//and return //and return
return *tempPtr; return *tempPtr;
@ -273,7 +271,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx); cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx);
} }
void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
@ -496,10 +494,10 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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::LaserScan &scan_in, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -519,7 +517,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
uint32_t vp_x_offset = 0; uint32_t vp_x_offset = 0;
//we need to find the offset of the intensity field in the point cloud //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 //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 //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 //putting in a check at some point, but I'm just going to put in an
//assert for now //assert for now
@ -565,7 +563,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
//find the index of the point //find the index of the point
uint32_t pt_index; uint32_t pt_index;
memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t)); 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 // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
tfScalar ratio = pt_index * ranges_norm; tfScalar ratio = pt_index * ranges_norm;