roi_to_position_converter.h

Go to the documentation of this file.

#pragma once

#include "aerial_autonomy/common/atomic.h"
#include "aerial_autonomy/trackers/base_tracker.h"
#include "aerial_autonomy/trackers/simple_tracking_strategy.h"
#include "aerial_autonomy/types/position.h"

#include <ros/ros.h>

#include <cv_bridge/cv_bridge.h>

#include <image_transport/image_transport.h>

#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/RegionOfInterest.h>

#include <Eigen/Dense>

#include <boost/thread/mutex.hpp>

class RoiToPositionConverter : public BaseTracker {
public:
  RoiToPositionConverter(ros::NodeHandle &nh)
      : BaseTracker(new SimpleTrackingStrategy()), it_(nh),
        roi_subscriber_(nh.subscribe(
            "roi", 10, &RoiToPositionConverter::roiCallback, this)),
        camera_info_subscriber_(
            nh.subscribe("camera_info", 1,
                         &RoiToPositionConverter::cameraInfoCallback, this)),
        depth_subscriber_(nh.subscribe(
            "depth", 1, &RoiToPositionConverter::depthCallback, this)),
        image_subscriber_(it_.subscribe(
            "image", 1, &RoiToPositionConverter::imageCallback, this)) {}

  bool getTrackingVectors(std::unordered_map<uint32_t, Position> &pos);
  static void computeTrackingVector(const sensor_msgs::RegionOfInterest &roi,
                                    const cv::Mat &depth,
                                    const sensor_msgs::CameraInfo &cam_info,
                                    double max_distance,
                                    double foreground_percent, Position &pos);
  bool trackingIsValid();

  bool isConnected();

private:
  bool cameraInfoIsValid();
  bool roiIsValid();
  void roiCallback(const sensor_msgs::RegionOfInterest &roi_msg);

  void imageCallback(const sensor_msgs::ImageConstPtr &img_msg);

  void cameraInfoCallback(const sensor_msgs::CameraInfo &cam_info_msg);

  void depthCallback(const sensor_msgs::ImageConstPtr &depth_msg);

  static bool compare(Eigen::Vector3d a, Eigen::Vector3d b);

  image_transport::ImageTransport it_;
  ros::Subscriber roi_subscriber_;
  ros::Subscriber camera_info_subscriber_;
  ros::Subscriber depth_subscriber_;
  image_transport::Subscriber image_subscriber_;
  Atomic<sensor_msgs::CameraInfo> camera_info_;
  Atomic<sensor_msgs::RegionOfInterest> roi_rect_;
  Atomic<Position> object_position_;
  double max_object_distance_ = 3.0;
  double foreground_percent_ = 0.25;

  Atomic<ros::Time> last_roi_update_time_;
};