cliff_detector.h

Go to the documentation of this file.

#pragma once

#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 <iostream>
#include <fstream>
#include <utility>
#include <cstdlib>
#include <vector>

namespace cliff_detector {
class CliffDetector {
 public:
  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; }

 protected:
  double lengthOfVector(const cv::Point3d& vec) const;
  double angleBetweenRays(const cv::Point3d& ray1, const cv::Point3d& ray2) const;
  template<typename T>
  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();

 private:
  // 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:
  sensor_msgs::Image new_depth_msg_;
  sensor_msgs::ImageConstPtr depth_msg_to_pub_;

  depth_nav_msgs::Point32List stairs_points_msg_;
};

} // namespace cliff_detector