laser_geometry.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef LASER_SCAN_UTILS_LASERSCAN_H
#define LASER_SCAN_UTILS_LASERSCAN_H

#include <map>
#include <iostream>
#include <sstream>

#include "boost/numeric/ublas/matrix.hpp"
#include "boost/thread/mutex.hpp"

#include <tf/tf.h>
#include <tf2/buffer_core.h>

#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud.h"

#include <Eigen/Core>
#include <sensor_msgs/PointCloud2.h>

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;

  namespace channel_option
  {

    enum ChannelOption
      {
        None = 0x00,      
        Intensity = 0x01, 
        Index     = 0x02, 
        Distance  = 0x04, 
        Timestamp = 0x08, 
        Viewpoint = 0x10, 
        Default   = (Intensity | Index) 
      };
  }


  class LaserProjection
    {

    public:

      LaserProjection() : angle_min_(0), angle_max_(0) {}

      ~LaserProjection();


      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, channel_options);
      }


      void projectLaser (const sensor_msgs::LaserScan& scan_in,
                         sensor_msgs::PointCloud2 &cloud_out,
                         double range_cutoff = -1.0,
                         int channel_options = channel_option::Default)
      {
        projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
      }



      void transformLaserScanToPointCloud (const std::string& target_frame,
                                           const sensor_msgs::LaserScan& scan_in,
                                           sensor_msgs::PointCloud& cloud_out,
                                           tf::Transformer& tf,
                                           double range_cutoff,
                                           int channel_options = channel_option::Default)
      {
        return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options);
      }


      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 transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options);
      }


      void transformLaserScanToPointCloud(const std::string &target_frame,
                                           const sensor_msgs::LaserScan &scan_in,
                                           sensor_msgs::PointCloud2 &cloud_out,
                                           tf::Transformer &tf,
                                           double range_cutoff = -1.0,
                                           int channel_options = channel_option::Default)
      {
        transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
      }


      void transformLaserScanToPointCloud(const std::string &target_frame,
                                          const sensor_msgs::LaserScan &scan_in,
                                          sensor_msgs::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);
      }

    protected:


      const boost::numeric::ublas::matrix<double>& getUnitVectors_(double angle_min,
                                                                   double angle_max,
                                                                   double angle_increment,
                                                                   unsigned int length);

    private:

      void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
                          sensor_msgs::PointCloud& cloud_out,
                          double range_cutoff,
                          bool preservative,
                          int channel_options);

      void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
                          sensor_msgs::PointCloud2 &cloud_out,
                          double range_cutoff,
                          int channel_options);

      void transformLaserScanToPointCloud_ (const std::string& target_frame,
                                            sensor_msgs::PointCloud& cloud_out,
                                            const sensor_msgs::LaserScan& scan_in,
                                            tf::Transformer & tf,
                                            double range_cutoff,
                                            int channel_options);

      void transformLaserScanToPointCloud_ (const std::string &target_frame,
                                            const sensor_msgs::LaserScan &scan_in,
                                            sensor_msgs::PointCloud2 &cloud_out,
                                            tf::Transformer &tf,
                                            double range_cutoff,
                                            int channel_options);

      void transformLaserScanToPointCloud_ (const std::string &target_frame,
                                            const sensor_msgs::LaserScan &scan_in,
                                            sensor_msgs::PointCloud2 &cloud_out,
                                            tf2::BufferCore &tf,
                                            double range_cutoff,
                                            int channel_options);

      void transformLaserScanToPointCloud_ (const std::string &target_frame,
                                            const sensor_msgs::LaserScan &scan_in,
                                            sensor_msgs::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);

      std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
      float angle_min_;
      float angle_max_;
      Eigen::ArrayXXd co_sine_map_;
      boost::mutex guv_mutex_;
    };

}

#endif //LASER_SCAN_UTILS_LASERSCAN_H