laserscan_kinect.h

Go to the documentation of this file.

/******************************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2015, Michal Drwiega (drwiega.michal@gmail.com)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     1. Redistributions of source code must retain the above copyright
 *        notice, this list of conditions and the following disclaimer.
 *     2. 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.
 *     3. Neither the name of the copyright holder 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
 * HOLDER 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 LASERSCAN_KINECT
#define LASERSCAN_KINECT

#include <ros/console.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>

#include <limits.h>
#include <algorithm>
#include <vector>
#include <cmath>
#include <string>
#include <boost/lexical_cast.hpp>

#define MAX_UINT16  65535
#define SCAN_TIME   1.0/30.0

namespace laserscan_kinect
{ 
class LaserScanKinect
{
public: // Public methods
  LaserScanKinect();
  ~LaserScanKinect() { }

  sensor_msgs::LaserScanPtr prepareLaserScanMsg(const sensor_msgs::ImageConstPtr& depth_msg,
                                                const sensor_msgs::CameraInfoConstPtr& info_msg);
  void setOutputFrame (const std::string frame) { output_frame_id_ = frame; }
  void setRangeLimits(const float rmin, const float rmax);
  void setScanHeight(const int scan_height);
  void setDepthImgRowStep(const int row_step);
  void setCamModelUpdate (const bool enable) { cam_model_update_ = enable; }
  void setSensorMountHeight (const float height);
  void setSensorTiltAngle (const float angle);
  void setGroundRemove (const bool enable) { ground_remove_enable_ = enable; }
  void setGroundMargin (const float margin);
  void setTiltCompensation (const bool enable) { tilt_compensation_enable_ = enable; }
  void setScanConfigurated (const bool configurated) { is_scan_msg_configurated_ = configurated; }

private: // Private methods
  double lengthOfVector(const cv::Point3d& ray) const;
  double angleBetweenRays(const cv::Point3d& ray1, const cv::Point3d& ray2) const;
  void fieldOfView( double & min, double & max, double x1, double y1,
                    double xc, double yc, double x2, double y2);
  void calcGroundDistancesForImgRows(double vertical_fov);
  void calcTiltCompensationFactorsForImgRows(double vertical_fov);
  void calcScanMsgIndexForImgCols(const sensor_msgs::ImageConstPtr& depth_msg);
  void convertDepthToPolarCoords(       const sensor_msgs::ImageConstPtr& depth_msg);

private: // Private fields
  //-----------------------------------------------------------------------------------------------
  // ROS parameters configurated with configuration file or dynamic_reconfigure
  std::string output_frame_id_;     
  float range_min_;                 
  float range_max_;                 
  unsigned int  scan_height_;       
  unsigned int depth_img_row_step_; 
  bool  cam_model_update_;          
  float sensor_mount_height_;       
  float sensor_tilt_angle_;         
  bool  ground_remove_enable_;      
  float ground_margin_;             
  bool  tilt_compensation_enable_;  
  //-----------------------------------------------------------------------------------------------

  sensor_msgs::LaserScanPtr scan_msg_;

  image_geometry::PinholeCameraModel camera_model_;

  bool  is_scan_msg_configurated_;

  std::vector<unsigned int> scan_msg_index_;

  std::vector<unsigned int> dist_to_ground_;

  std::vector<double> tilt_compensation_factor_;

}; // end of class
}; // end of namespace laserscan_kinect

#endif