include/tracker_impl.h

Go to the documentation of this file.

#ifndef TRACKER_H
#define TRACKER_H

//MACROS
#define DIEonERR(value) if (value<0) { \
 fprintf(stderr,"%i DIED in %s line %i with error %i\n",getpid(),__FILE__,__LINE__,-value);exit(1); }

/* system includes */
#include <string>

/* my includes */
#include "ros/ros.h"
#include <geometry_msgs/Pose.h>
#include <tf/transform_broadcaster.h>
#include <asr_msgs/AsrObject.h>

#include "transform_coords.h"

typedef enum{rightTracker=1, leftTracker}trackerID;

//for reference purposes: the old data structure
/*
    // Definition of a calibration data set
  struct fulcrum{
    double sensorVal[3];
    double sensorAngle[3];
        double worldVal[3];
        double worldAngle[3];
  };
  //typedef sequence<fulcrum> oneSensorCal;
  typedef sequence<fulcrum> allSensorCal;
*/

class tracker_impl{
 public:
  tracker_impl(trackerID t, BirdTrack_impl* b, std::string trackerName);
  ~tracker_impl();

  void start();
  void stop();
  
  bool loadCalibFile(const char *srcFileName);


private:
  trackerID myID;
  BirdTrack_impl* myBird;
  transformCoords* myTransformer;
  unsigned int seqId;
  
  struct fulcrum{
    double sensorVal[3];
    double sensorAngle[3];
    double worldVal[3];
    double worldAngle[3];
  };
  
  void workerThread();
  bool threadRunning;
  bool writeNotification(trackerID id, double* raw, double *calibrated);

  pthread_t workerThreadHandle;
  static void *s_workerThread(void* arg);

  void printFulcrum(fulcrum fulc);

  bool loadCalibration(const fulcrum cal[255], unsigned int length);
  
  void getPbdObject(std::string name, double *data, asr_msgs::AsrObject &object);
 void getTransform(double *data, tf::Transform &transform);
  
  tf::TransformBroadcaster transformPublisher;
  ros::NodeHandle node;
  ros::Publisher generalPublisher;
};

#endif /* TRACKER_H */