VRPNTrackerClient.cpp

Go to the documentation of this file.

/*
 * VRPNTrackerClient.cpp
 *
 *  Created on: Nov 30, 2012
 *      Author: vgrabe
 */

#include <vrpn_tracker/VRPNTrackerClient.hpp>

void VRPN_CALLBACK viconTracker_handleTracker(void* userData, const vrpn_TRACKERCB t ) {
        VRPNTrackerClient* client = (VRPNTrackerClient *) userData;
        client->handleVRPNTrackerCB(t);
}


VRPNTrackerClient::VRPNTrackerClient(const std::string& objectName)
{
        opttrackRotationMatrix.resize(9);
        opttrackRotationMatrix[0] = 1.0; opttrackRotationMatrix[1] = 0; opttrackRotationMatrix[2] = 0;
        opttrackRotationMatrix[3] = 0; opttrackRotationMatrix[4] = 0; opttrackRotationMatrix[5] = -1.0;
        opttrackRotationMatrix[6] = 0; opttrackRotationMatrix[7] = 1.0; opttrackRotationMatrix[8] = 0;

        options = new VRPNTrackerClientOptions();
        std::string vrpnObjectName = objectName + "@" + options->tVRPNHostname;
        if(options->tTopicName.compare(VRPN_TOPIC_UNDEF_STRING) == 0){ //check whether the topic name was defined
                pub = nodeHandle.advertise<geometry_msgs::PoseStamped>(objectName, 10); //use just the object name if not
        }
        else{
                pub = nodeHandle.advertise<geometry_msgs::PoseStamped>(options->tTopicName, 10); //otherwise use the specified topic name
        }

        childFrameID = objectName;
        poseStamped.header.frame_id = options->tChildFrameID;

        // Tracker
        vrpnTracker = new vrpn_Tracker_Remote(vrpnObjectName.c_str());
        vrpnTracker->register_change_handler(this, viconTracker_handleTracker);
}

VRPNTrackerClient::~VRPNTrackerClient()
{
        vrpnTracker->unregister_change_handler(this, viconTracker_handleTracker);
        delete vrpnTracker;
}

void VRPNTrackerClient::spin()
{
        vrpnTracker->mainloop();
}

void VRPNTrackerClient::handleVRPNTrackerCB(const vrpn_TRACKERCB t)
{

        //TODO: Reenable the code to allow the user to directly apply a transformation
//      Eigen::Vector3d pose(t.pos[0], t.pos[1], t.pos[2]);
        std::vector<float> pose(3);
        pose[0] = t.pos[0]; pose[1] = t.pos[1]; pose[2] = t.pos[2];
        std::vector<float> rotPose(3);

        if(options->useOptitrackToENURot){
                rotPose = multMatrixVector(opttrackRotationMatrix, pose);
        }
        else{
                rotPose = pose;
        }

        if(options->useVRPNServerTime)
                poseStamped.header.stamp = ros::Time(t.msg_time.tv_sec, t.msg_time.tv_usec*1000);
        else
                poseStamped.header.stamp = ros::Time::now();

        if(options->useNEDconvention){
                poseStamped.pose.position.x = rotPose[0];
                poseStamped.pose.position.y = -rotPose[1];
                poseStamped.pose.position.z = -rotPose[2];
        }
        else{
                poseStamped.pose.position.x = rotPose[0];
                poseStamped.pose.position.y = rotPose[1];
                poseStamped.pose.position.z = rotPose[2];
        }

        //      Eigen::Vector3d ori(t.quat[0], t.quat[1], t.quat[2]);
        std::vector<float> ori(3);
        ori[0] = t.quat[0]; ori[1] = t.quat[1]; ori[2] = t.quat[2];
        std::vector<float> rotOri(3);
        if(options->useOptitrackToENURot){
                rotOri = multMatrixVector(opttrackRotationMatrix, ori);
        }
        else{
                rotOri = ori;
        }

        if(options->useNEDconvention){
                poseStamped.pose.orientation.x = rotOri[0];
                poseStamped.pose.orientation.y = -rotOri[1];
                poseStamped.pose.orientation.z = -rotOri[2];
                poseStamped.pose.orientation.w = t.quat[3];
        }
        else{
                poseStamped.pose.orientation.x = rotOri[0];
                poseStamped.pose.orientation.y = rotOri[1];
                poseStamped.pose.orientation.z = rotOri[2];
                poseStamped.pose.orientation.w = t.quat[3];
        }
        pub.publish(poseStamped);


        if (options->tEnableTF) {
                tf::Transform transform;
                transform.setOrigin(tf::Vector3(poseStamped.pose.position.x, poseStamped.pose.position.y, poseStamped.pose.position.z));
                transform.setRotation(tf::Quaternion(poseStamped.pose.orientation.x, poseStamped.pose.orientation.y, poseStamped.pose.orientation.z, poseStamped.pose.orientation.w));

                tf::StampedTransform tfStampedTransform(transform, poseStamped.header.stamp, poseStamped.header.frame_id, childFrameID);
                transBroadcaster.sendTransform(tfStampedTransform);
        }
}

std::vector<float> VRPNTrackerClient::multMatrixVector(const std::vector<float> &mat, const std::vector<float> &vec){
        std::vector<float> ret;
        if(vec.size() != 3 || mat.size() != 9)
                return ret;

        ret.resize(3);
        ret[0] = mat[0]*vec[0]+mat[1]*vec[1]+mat[2]*vec[2];
        ret[1] = mat[3]*vec[0]+mat[4]*vec[1]+mat[5]*vec[2];
        ret[2] = mat[6]*vec[0]+mat[7]*vec[1]+mat[8]*vec[2];

        return ret;

}