00001 #include "tfConnector.h"
00002 #include <ros/time.h>
00003 #include <math.h>
00004 #include <iostream>
00005
00006
00007 using namespace tf;
00008 using namespace std;
00009
00010 tfConnector* tfConnector::instance = NULL;
00011
00012
00013
00014 tfConnector::tfConnector() {
00015 listener = new tf::TransformListener();
00016 }
00017
00018
00019 int tfConnector::lookupTransform(char* goal, char* origin, double* out) {
00020 tf::StampedTransform transform;
00021 try{
00022 listener->lookupTransform(goal, origin, ros::Time(0), transform);
00023 } catch (tf::TransformException ex) {
00024 ROS_ERROR("%s",ex.what());
00025 return 0;
00026 }
00027 out[0] = transform.getOrigin().getX();
00028 out[1] = transform.getOrigin().getY();
00029 out[2] = transform.getOrigin().getZ();
00030
00031 out[3] = transform.getRotation().getX();
00032 out[4] = transform.getRotation().getY();
00033 out[5] = transform.getRotation().getZ();
00034 out[6] = transform.getRotation().getW();
00035
00036
00037 return 1;
00038 }
00039
00040 extern "C" void Initialize() {
00041 if(tfConnector::instance==NULL) tfConnector::instance = new tfConnector();
00042 }
00043
00044
00045 extern "C" int lookupTransformInternal(char* goal, char* origin, double* ptr) {
00046 return tfConnector::instance->lookupTransform(goal, origin, ptr);
00047 }
