ViconSubject.cpp

Go to the documentation of this file.

/*
 * ViconSubject.cpp
 *
 *  Created on: Oct 22, 2011
 *      Author: mriedel
 */

#include <telekyb_vicon/ViconSubject.hpp>

// local
#include <telekyb_vicon/ViconTypes.hpp>
#include <ros/console.h>

#include <telekyb_vicon/ViconBridge.hpp>

namespace TELEKYB_NAMESPACE
{

ViconSubject::ViconSubject()
        : options(ViconTrackerOptions::Instance()), subjectID(-1)
{

}

ViconSubject::ViconSubject(int subjectID_)
        : options(ViconTrackerOptions::Instance()), subjectID(subjectID_)
{

}

ViconSubject& ViconSubject::operator= (const ViconSubject & other)
{
        subjectID = other.subjectID;
        return *this;
}

ViconSubject::~ViconSubject()
{

}

std::string ViconSubject::getName() const
{
        return subjectName;
}

unsigned int ViconSubject::getNSegments() const
{
        return nSegments;
}

void ViconSubject::init()
{
        subjectName = ViconBridge::Instance().getClient().GetSubjectName(subjectID).SubjectName;
        nSegments = ViconBridge::Instance().getClient().GetSegmentCount(subjectName).SegmentCount;
//      segments.empty();

    for (unsigned int i = 0; i < nSegments; i++) {
        std::string segmentName = ViconBridge::Instance().getClient().GetSegmentName(subjectName, i).SegmentName;
        segments.push_back(ViconSegment(*this, i, segmentName));
    }
}

void ViconSubject::process()
{
    for (unsigned int i = 0; i < nSegments; i++) {
        Vicon::Output_GetSegmentGlobalTranslation trans =
                        ViconBridge::Instance().getClient().GetSegmentGlobalTranslation(subjectName, segments[i].getSegmentName());
        Vicon::Output_GetSegmentGlobalRotationQuaternion quat =
                        ViconBridge::Instance().getClient().GetSegmentGlobalRotationQuaternion(subjectName, segments[i].getSegmentName());

        if (trans.Result != Vicon::Result::Success || quat.Result != Vicon::Result::Success) {
            ROS_WARN("GetSegmentGlobalTranslation/Rotation failed (result = %s, %s), not publishing...",//"a","b");
                ViconResult(trans.Result).str(), ViconResult(quat.Result).str());
                continue;
        }

        if (trans.Occluded || quat.Occluded) {
            ROS_WARN_STREAM("Segment "<< segments.at(i).getSegmentName() << " of "
                        << subjectName <<" occluded, not publishing... " );
                continue;
        }

        // everything ok.

        Eigen::Vector3d globalTranslation(trans.Translation[0] / 1000, trans.Translation[1] / 1000, trans.Translation[2] / 1000);
        Eigen::Quaterniond globalRotation(quat.Rotation[3], quat.Rotation[0], quat.Rotation[1], quat.Rotation[2]);

        if (options.tPublishTF->getValue()) {
                        tf::Transform lastTransform;
                        // ok publish
                        lastTransform.setOrigin(tf::Vector3(globalTranslation(0), globalTranslation(1), globalTranslation(2)));
                        lastTransform.setRotation(tf::Quaternion(globalRotation.x(), globalRotation.y(), globalRotation.z(),
                                        globalRotation.w()));

                        tfBroadCaster.sendTransform(
                                        tf::StampedTransform(lastTransform, ros::Time::now(), "world", segments[i].getSegmentName() + "/" + subjectName)
                        );
        }

        segments[i].publish(globalTranslation, globalRotation);
    }
}

} //namespace