laserscan_kinect.h

Go to the documentation of this file.

#pragma once

#include <vector>
#include <string>
#include <mutex>

#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 <laserscan_kinect/math.h>

namespace laserscan_kinect {

class LaserScanKinect {
 public:
  LaserScanKinect(): scan_msg_(new sensor_msgs::LaserScan()) { }
  ~LaserScanKinect() = default;

  sensor_msgs::LaserScanPtr getLaserScanMsg(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 configured) { is_scan_msg_configured_ = configured; }
  void setPublishDbgImgEnable(const bool enable) { publish_dbg_image_ = enable; }

  void setThreadsNum(unsigned threads_num);

  bool getPublishDbgImgEnable() const { return publish_dbg_image_; }

  sensor_msgs::ImageConstPtr getDbgImage() const;

 protected:

  void calcGroundDistancesForImgRows(double vertical_fov);
  void calcTiltCompensationFactorsForImgRows(double vertical_fov);
  void calcScanMsgIndexForImgCols(const sensor_msgs::ImageConstPtr& depth_msg);
  template <typename T>
  float getSmallestValueInColumn(const sensor_msgs::ImageConstPtr &depth_msg, int col);
  template <typename T>
  void convertDepthToPolarCoords(const sensor_msgs::ImageConstPtr& depth_msg);

  sensor_msgs::ImagePtr prepareDbgImage(const sensor_msgs::ImageConstPtr& depth_msg,
    const std::list<std::pair<int, int>>& min_dist_points_indices);

private:
  // ROS parameters configurated with configuration file or dynamic_reconfigure
  std::string output_frame_id_;           
  float range_min_{0};                    
  float range_max_{0};                    
  unsigned scan_height_{0};               
  unsigned depth_img_row_step_{0};        
  bool  cam_model_update_{false};         
  float sensor_mount_height_{0};          
  float sensor_tilt_angle_{0};            
  bool  ground_remove_enable_{false};     
  float ground_margin_{0};                
  bool  tilt_compensation_enable_{false}; 
  bool  publish_dbg_image_{false};        
  unsigned threads_num_{1};                

  sensor_msgs::LaserScanPtr scan_msg_;

  image_geometry::PinholeCameraModel cam_model_;

  bool is_scan_msg_configured_{false};

  std::vector<unsigned> scan_msg_index_;

  std::vector<float> dist_to_ground_corrected;

  std::vector<float> tilt_compensation_factor_;

  int image_vertical_offset_{0};

  sensor_msgs::ImagePtr dbg_image_;
  std::list<std::pair<int, int>> min_dist_points_indices_;

  std::mutex points_indices_mutex_;
  std::mutex scan_msg_mutex_;
};

} // namespace laserscan_kinect