alvar_tracker.h

Go to the documentation of this file.

#pragma once
#include "aerial_autonomy/trackers/base_tracker.h"
#include "aerial_autonomy/trackers/closest_tracking_strategy.h"

#include "aerial_autonomy/common/atomic.h"

#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include <ros/ros.h>

class AlvarTracker : public BaseTracker {
public:
  AlvarTracker(ros::NodeHandle &nh)
      : BaseTracker(new ClosestTrackingStrategy(20)), nh_(nh),
        alvar_sub_(nh_.subscribe("ar_pose_marker", 1,
                                 &AlvarTracker::markerCallback, this)),
        timeout_(0.2) {}
  virtual bool getTrackingVectors(std::unordered_map<uint32_t, Position> &pos);
  virtual bool trackingIsValid();

  bool isConnected();

private:
  void markerCallback(const ar_track_alvar_msgs::AlvarMarkers &marker_msg);

  ros::NodeHandle nh_;
  ros::Subscriber alvar_sub_;
  Atomic<ros::Time> last_valid_time_;
  Atomic<std::unordered_map<uint32_t, Position>> object_positions_;
  const double timeout_;
};