cliff_detector.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 CLIFF_DETECTOR
#define CLIFF_DETECTOR

#include <ros/console.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>
#include <depth_nav_msgs/Point32List.h>
#include <geometry_msgs/Point32.h>

#include <sstream>
#include <limits.h>
#include <math.h>
#include <cmath>
#include <string>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>
#include <utility>
#include <cstdlib>

#include <vector>

namespace cliff_detector
{ 
class CliffDetector
{
  //---------------------------------------------------------------------------------------------
public: // Public methods
  CliffDetector();
  ~CliffDetector() { }
  void detectCliff( const sensor_msgs::ImageConstPtr& depth_msg,
                         const sensor_msgs::CameraInfoConstPtr& info_msg);
  void setRangeLimits(const float rmin, const float rmax);
  void setSensorMountHeight (const float height);
  float getSensorMountHeight () { return sensor_mount_height_; }
  void setSensorTiltAngle (const float angle);
  float getSensorTiltAngle () { return sensor_tilt_angle_; }
  void setPublishDepthEnable (const bool enable) { publish_depth_enable_ = enable; }
  bool getPublishDepthEnable () const { return publish_depth_enable_; }
  void setCamModelUpdate (const bool u) { cam_model_update_ = u; }
  void setUsedDepthHeight(const unsigned int height);
  void setBlockSize(const int size);
  void setBlockPointsThresh(const int thresh);
  void setDepthImgStepRow(const int step);
  void setDepthImgStepCol(const int step);
  void setGroundMargin (const float margin);
  void setParametersConfigurated (const bool u) { depth_sensor_params_update = u; }

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


private: // Private fields

  //-----------------------------------------------------------------------------------------------
  // ROS parameters configurated with config file or dynamic_reconfigure
  std::string  outputFrameId_;        
  float        range_min_;            
  float        range_max_;            
  float        sensor_mount_height_;  
  float        sensor_tilt_angle_;    
  bool         publish_depth_enable_; 
  bool         cam_model_update_;     
  unsigned int used_depth_height_;    
  unsigned int block_size_;           
  unsigned int block_points_thresh_;  
  unsigned int depth_image_step_row_; 
  unsigned int depth_image_step_col_; 
  float        ground_margin_;        
  //-----------------------------------------------------------------------------------------------

  bool depth_sensor_params_update;
  image_geometry::PinholeCameraModel camera_model_;
  std::vector<unsigned int> dist_to_ground_;
  std::vector<double> tilt_compensation_factor_;

  std::vector<double> delta_row_;

  //-----------------------------------------------------------------------------------------------
public: // Public fields

  sensor_msgs::Image new_depth_msg_;
  sensor_msgs::ImageConstPtr depth_msg_to_pub_;

  depth_nav_msgs::Point32List stairs_points_msg_;

};
}; // cliff_detector

#endif