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

More laser_geometry deprecations and code fixes.

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@24345 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs 2009-09-23 01:37:30 +00:00
parent f7b67fb1b8
commit ac4a77f57a
3 changed files with 179 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

188
include/laser_geometry/laser_geometry.h
View File

@ -42,91 +42,193 @@
#include "sensor_msgs/PointCloud.h" #include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud.h" #include "sensor_msgs/PointCloud.h"
#if defined(__GNUC__)
#define DEPRECATED __attribute__((deprecated))
#else
#define DEPRECATED
#endif
namespace laser_geometry namespace laser_geometry
{ {
/** \brief Define masks for output channels */ //! Enumerated output channels options.
const int MASK_INTENSITY = 0x01; /*!
const int MASK_INDEX = 0x02; * An OR'd set of these options is passed as the final argument of
const int MASK_DISTANCE = 0x04; * the projectLaser and transformLaserScanToPointCloud calls to
const int MASK_TIMESTAMP = 0x08; * enable generation of the appropriate set of additional channels.
const int DEFAULT_MASK = (MASK_INTENSITY | MASK_INDEX); *
* \b Example
* - To produce a point cloud with indices and timestamp:
\verbatim
laser_geometry::laserProjection p_;
sensor_msgs::PointCloud cloud_;
p.projectLaser(scan_in, cloud_out, -1.0, laser_geometry::channel_option::Index | laser_geometry::channel_option::Timestamp);
\endverbatim
*/
namespace channel_option
{
enum ChannelOption
{
Intensity = 0x01, //!< Enable intensity channel
Index = 0x02, //!< Enable index channel
Distance = 0x04, //!< Enable distance channel
Timestamp = 0x08, //!< Enable timestamp channel
Default = (Intensity | Index) //!< Enable intensity and index channels
};
}
/** \brief A Class to Project Laser Scan DEPRECATED const int MASK_INTENSITY = channel_option::Intensity;
* DEPRECATED const int MASK_INDEX = channel_option::Index;
DEPRECATED const int MASK_DISTANCE = channel_option::Distance;
DEPRECATED const int MASK_TIMESTAMP = channel_option::Timestamp;
DEPRECATED const int DEFAULT_MASK = (channel_option::Intensity | channel_option::Index);
//! \brief A Class to Project Laser Scan
/*!
* This class will project laser scans into point clouds. It caches * This class will project laser scans into point clouds. It caches
* unit vectors between runs (provided the angular resolution of * unit vectors between runs (provided the angular resolution of
* your scanner is not changing) to avoid excess computation. * your scanner is not changing) to avoid excess computation.
* *
* 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. If it is * point cloud, as these are assumed to be invalid.
* important to preserve a mapping between the index of range values *
* and points in the cloud, the functions have an optional * If it is important to preserve a mapping between the index of
* "preservative" argument which will leave placeholder points as * range values and points in the cloud, the recommended approach is
* appropriate. * to pre-filter your laser_scan message to meet the requiremnt that all
* ranges are between min and max_range.
*/ */
class LaserProjection class LaserProjection
{ {
public: public:
/** \brief Destructor to deallocate stored unit vectors */
//! Destructor to deallocate stored unit vectors
~LaserProjection(); ~LaserProjection();
/** \brief Project Laser Scan
*
//! 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
* point cloud. The generated cloud will be in the same frame * point cloud. The generated cloud will be in the same frame
* as the original laser scan. * as the original laser scan.
* *
* \param scan_in The input laser scan * \param scan_in The input laser scan
* \param cloudOut The output point cloud * \param cloud_out The output point cloud
* \param range_cutoff An additional range cutoff which can be * \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan. * applied which is more limiting than max_range in the scan.
* \param A bitwise mask of channels to include * \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Range, channel_option:Timestamp.
*/ */
void projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff=-1.0, int mask = DEFAULT_MASK) void projectLaser (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
{ {
return projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask); return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options);
} }
__attribute__ ((deprecated)) void projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, bool preservative, int mask = DEFAULT_MASK) //! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud 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 tf::Transformer object to use to perform the
* transform applied which is more limiting than max_range in
* the scan.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Range, channel_option:Timestamp.
*/
void transformLaserScanToPointCloud (const std::string& target_frame,
const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out,
tf::Transformer& tf,
int channel_options = channel_option::Default)
{ {
return projectLaser_ (scan_in, cloud_out, range_cutoff, preservative, mask); return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, channel_options, false);
}
DEPRECATED
void projectLaser (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud & cloud_out,
double range_cutoff,
bool preservative,
int channel_options = channel_option::Default)
{
return projectLaser_ (scan_in, cloud_out, range_cutoff, preservative, channel_options);
}
DEPRECATED
void transformLaserScanToPointCloud (const std::string& target_frame,
sensor_msgs::PointCloud & cloud_out,
const sensor_msgs::LaserScan& scan_in,
tf::Transformer & tf,
int channel_options = channel_option::Default,
bool preservative = false)
{
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, channel_options, preservative);
} }
/** \brief Transform a sensor_msgs::LaserScan into a PointCloud in target frame */ DEPRECATED
void transformLaserScanToPointCloud (const std::string& target_frame, sensor_msgs::PointCloud & cloudOut, const sensor_msgs::LaserScan & scanIn, tf::Transformer & tf, int mask = DEFAULT_MASK) const boost::numeric::ublas::matrix<double>& getUnitVectors(float angle_min,
float angle_max,
float angle_increment,
unsigned int length)
{ {
return transformLaserScanToPointCloud_ (target_frame, cloudOut, scanIn, tf, mask, false); return getUnitVectors_(angle_min, angle_max, angle_increment, length);
} }
__attribute__ ((deprecated)) void transformLaserScanToPointCloud (const std::string& target_frame, sensor_msgs::PointCloud & cloudOut, const sensor_msgs::LaserScan & scanIn, tf::Transformer & tf, int mask, bool preservative) protected:
{
return transformLaserScanToPointCloud_ (target_frame, cloudOut, scanIn, tf, mask, preservative);
}
/** \brief Return the unit vectors for this configuration //! Internal protected representation of getUnitVectors
* Return the unit vectors for this configuration. /*!
* These are dynamically generated and allocated on first request * This function should not be used by external users, however,
* and will be valid until destruction of this node. */ * it is left protected so that test code can evaluate it
__attribute__ ((deprecated)) const boost::numeric::ublas::matrix<double>& getUnitVectors(float angle_max, float angle_min, float angle_increment, unsigned int length) * appropriately.
{ */
return getUnitVectors_(angle_max, angle_min, angle_increment, length); const boost::numeric::ublas::matrix<double>& getUnitVectors_(float angle_min,
} float angle_max,
float angle_increment,
unsigned int length);
private: private:
void projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, bool preservative, int mask = DEFAULT_MASK); //! Internal hidden representation of projectLaser
void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out,
double range_cutoff,
bool preservative,
int channel_options);
void transformLaserScanToPointCloud_ (const std::string& target_frame, sensor_msgs::PointCloud & cloudOut, const sensor_msgs::LaserScan & scanIn, tf::Transformer & tf, int mask, bool preservative); //! Internal hidden representation of transformLaserScanToPointCloud
void transformLaserScanToPointCloud_ (const std::string& target_frame,
sensor_msgs::PointCloud& cloud_out,
const sensor_msgs::LaserScan& scan_in,
tf::Transformer & tf,
int channel_options,
bool preservative);
const boost::numeric::ublas::matrix<double>& getUnitVectors_(float angle_max, float angle_min, float angle_increment, unsigned int length); //! Internal map of pointers to stored values
///The map of pointers to stored values
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_; std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
}; };
} }
#undef DEPRECATED
#endif //LASER_SCAN_UTILS_LASERSCAN_H #endif //LASER_SCAN_UTILS_LASERSCAN_H

16
src/laser_geometry.cpp
View File

@ -61,7 +61,7 @@ namespace laser_geometry
cloud_out.set_channels_size(0); cloud_out.set_channels_size(0);
// Check if the intensity bit is set // Check if the intensity bit is set
if ((mask & MASK_INTENSITY) && scan_in.get_intensities_size () > 0) if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1); cloud_out.set_channels_size (chan_size + 1);
@ -71,7 +71,7 @@ namespace laser_geometry
} }
// Check if the index bit is set // Check if the index bit is set
if (mask & MASK_INDEX) if (mask & channel_option::Index)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size +1); cloud_out.set_channels_size (chan_size +1);
@ -81,7 +81,7 @@ namespace laser_geometry
} }
// Check if the distance bit is set // Check if the distance bit is set
if (mask & MASK_DISTANCE) if (mask & channel_option::Distance)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1); cloud_out.set_channels_size (chan_size + 1);
@ -90,7 +90,7 @@ namespace laser_geometry
idx_distance = chan_size; idx_distance = chan_size;
} }
if (mask & MASK_TIMESTAMP) if (mask & channel_option::Timestamp)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1); cloud_out.set_channels_size (chan_size + 1);
@ -207,7 +207,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(fl
cloud_out.set_channels_size(0); cloud_out.set_channels_size(0);
// Check if the intensity bit is set // Check if the intensity bit is set
if ((mask & MASK_INTENSITY) && scan_in.get_intensities_size () > 0) if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1); cloud_out.set_channels_size (chan_size + 1);
@ -217,7 +217,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(fl
} }
// Check if the index bit is set // Check if the index bit is set
if (mask & MASK_INDEX) if (mask & channel_option::Index)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1); cloud_out.set_channels_size (chan_size + 1);
@ -227,7 +227,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(fl
} }
// Check if the distance bit is set // Check if the distance bit is set
if (mask & MASK_DISTANCE) if (mask & channel_option::Distance)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1); cloud_out.set_channels_size (chan_size + 1);
@ -236,7 +236,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(fl
idx_distance = chan_size; idx_distance = chan_size;
} }
if (mask & MASK_TIMESTAMP) if (mask & channel_option::Timestamp)
{ {
int chan_size = cloud_out.get_channels_size (); int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1); cloud_out.set_channels_size (chan_size + 1);

40
test/projection_test.cpp
View File

@ -67,10 +67,22 @@ sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
}; };
void test_getUnitVectors (float angle_min, float angle_max, float angle_increment, unsigned int length) class TestProjection : public laser_geometry::LaserProjection
{
public:
const boost::numeric::ublas::matrix<double>& getUnitVectors(float angle_min,
float angle_max,
float angle_increment,
unsigned int length)
{
return getUnitVectors_(angle_min, angle_max, angle_increment, length);
}
};
void test_getUnitVectors(float angle_min, float angle_max, float angle_increment, unsigned int length)
{ {
double tolerance = 1e-6; double tolerance = 1e-6;
laser_geometry::LaserProjection projector; TestProjection projector;
const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length); const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
@ -211,20 +223,20 @@ TEST(laser_geometry, projectLaser)
sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
sensor_msgs::PointCloud cloud_out; sensor_msgs::PointCloud cloud_out;
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INDEX); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.projectLaser(scan, cloud_out, -1.0, false); projector.projectLaser(scan, cloud_out, -1.0);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_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.channels.size(), (unsigned int)3); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_DISTANCE | laser_geometry::MASK_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.channels.size(), (unsigned int)4); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
unsigned int valid_points = 0; unsigned int valid_points = 0;
@ -326,20 +338,20 @@ TEST(laser_geometry, transformLaserScanToPointCloud)
scan.header.frame_id = "laser_frame"; scan.header.frame_id = "laser_frame";
sensor_msgs::PointCloud cloud_out; sensor_msgs::PointCloud cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INDEX); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_DISTANCE); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_DISTANCE | laser_geometry::MASK_TIMESTAMP); projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4); EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
unsigned int valid_points = 0; unsigned int valid_points = 0;