tfConnector.cpp

Go to the documentation of this file.

#include "tfConnector.h"
#include <ros/time.h>
#include <math.h>
#include <iostream>


using namespace tf;
using namespace std;

tfConnector* tfConnector::instance = NULL;



tfConnector::tfConnector() {
        listener = new tf::TransformListener();
}


int tfConnector::lookupTransform(char* goal, char* origin, double* out) {
        tf::StampedTransform transform;
        try{
                listener->lookupTransform(goal, origin,  ros::Time(0), transform);                                                                                                                                      
        } catch (tf::TransformException ex) { 
                ROS_ERROR("%s",ex.what());
                return 0;
    }
    out[0] = transform.getOrigin().getX();
    out[1] = transform.getOrigin().getY();
    out[2] = transform.getOrigin().getZ();
    
    out[3] = transform.getRotation().getX();
    out[4] = transform.getRotation().getY();
    out[5] = transform.getRotation().getZ();
    out[6] = transform.getRotation().getW();
    
    
        return 1;
}

extern "C" void Initialize() {                  
        if(tfConnector::instance==NULL) tfConnector::instance = new tfConnector();
}


extern "C" int lookupTransformInternal(char* goal, char* origin, double* ptr) {
        return tfConnector::instance->lookupTransform(goal, origin, ptr);
}