ViconTracker.cpp

Go to the documentation of this file.

/*
 * ViconBridge.cpp
 *
 *  Created on: Oct 17, 2011
 *      Author: mriedel
 */

#include <telekyb_vicon/ViconTracker.hpp>

namespace TELEKYB_NAMESPACE
{

ViconTracker::ViconTracker()
        : options(ViconTrackerOptions::Instance()), totalFrameCount(0), totalDroppedFrameCount(0)
{
        init();
}

ViconTracker::~ViconTracker()
{
// disconnect is done by singleton
}

void ViconTracker::init() {
        ROS_INFO_STREAM("Connecting to Vicon Tracker at " << options.tTrackerHostname->getValue());


        while (!ViconBridge::Instance().getClient().IsConnected().Connected)
        {
                ViconBridge::Instance().getClient().Connect(options.tTrackerHostname->getValue());
                ROS_INFO(".");
                if (!ros::ok()) {
                        return;
                }

                // 1sec
                usleep(1000*1000);
        }
        ROS_ASSERT_MSG(ViconBridge::Instance().getClient().IsConnected().Connected,
                        "Unable to connect to Vicon Tracker at %s", options.tTrackerHostname->getValue().c_str());
    ROS_INFO_STREAM("... connected!");

    // Tracker Version
    Vicon::Output_GetVersion viconVersion = ViconBridge::Instance().getClient().GetVersion();
    ROS_INFO_STREAM("Tracker Version: " << viconVersion.Major << "." << viconVersion.Minor << "."
        << viconVersion.Point);

        if (options.tEnableSegmentData->getValue()) {
                // enable SegmentData
                ROS_INFO_STREAM("Enabling SegmentData");
                ViconBridge::Instance().getClient().EnableSegmentData();
                ROS_ASSERT(ViconBridge::Instance().getClient().IsSegmentDataEnabled().Enabled);
        } else {
                // disable SegmentData
                ROS_INFO_STREAM("Disabling SegmentData");
                ViconBridge::Instance().getClient().DisableSegmentData();
                ROS_ASSERT(!ViconBridge::Instance().getClient().IsSegmentDataEnabled().Enabled);
        }

        if (options.tEnableMarkerData->getValue()) {
                // enable MarkerData
                ROS_INFO_STREAM("Enabling MarkerData");
                ViconBridge::Instance().getClient().EnableMarkerData();
                ROS_ASSERT(ViconBridge::Instance().getClient().IsMarkerDataEnabled().Enabled);
        } else {
                // disable MarkerData
                ROS_INFO_STREAM("Disabling MarkerData");
                ViconBridge::Instance().getClient().DisableMarkerData();
                ROS_ASSERT(!ViconBridge::Instance().getClient().IsMarkerDataEnabled().Enabled);
        }


        // Set Axis Mapping
        ViconDirection xAxisIn = options.tAxisMapping->getValue()[0];;
        ViconDirection yAxisIn = options.tAxisMapping->getValue()[1];;
        ViconDirection zAxisIn = options.tAxisMapping->getValue()[2];;
        if (!options.tAxisMapping->isOnInitialValue()) {
                Vicon::Output_SetAxisMapping setAxisOutput =
                                ViconBridge::Instance().getClient().SetAxisMapping(xAxisIn.value(), yAxisIn.value(), zAxisIn.value());

                ROS_ASSERT_MSG(setAxisOutput.Result == Vicon::Result::Success,
                                        "Unable to set Axis(setAxisOutput.Result): %s",
                                        (*ViconResult::get_by_value(setAxisOutput.Result)).str());


        }

        // Get Axis Mapping
        Vicon::Output_GetAxisMapping axisMapping = ViconBridge::Instance().getClient().GetAxisMapping();
        ViconDirection xAxisOut(axisMapping.XAxis);
        ViconDirection yAxisOut(axisMapping.YAxis);
        ViconDirection zAxisOut(axisMapping.ZAxis);

//      // ASSERT
//      if (!options.tAxisMapping->isOnInitialValue()) {
//              ROS_ASSERT(xAxisIn == xAxisOut);
//              ROS_ASSERT(yAxisIn == yAxisOut);
//              ROS_ASSERT(zAxisIn == zAxisOut);
//      }

        // Print
    ROS_INFO_STREAM("AxisMapping X: " << xAxisOut.str() << " Y: " << yAxisOut.str() << " Z: " << zAxisOut.str());


    // Stream Mode
        ROS_INFO_STREAM("Setting up SteamMode: " << options.tStreamMode->getValue().str() << "("
                        << options.tStreamMode->getValue().value() << ")");
        ViconBridge::Instance().getClient().SetStreamMode(options.tStreamMode->getValue().value());
}

void ViconTracker::spin()
{
        // loop variables
        unsigned int lastFrameNumber = 0;
        unsigned int currentFrameNumber = 0;
        int frameDiff = 1;

        while(ros::ok()) {

                if (ViconBridge::Instance().getClient().GetFrame().Result != Vicon::Result::Success) {
                        ROS_WARN("GetFrame().Result != Vicon::Result::Success");
                        ros::Duration(1.0/240.0).sleep();
                }
                currentFrameNumber = ViconBridge::Instance().getClient().GetFrameNumber().FrameNumber;

                if (lastFrameNumber != 0) {
                        frameDiff = currentFrameNumber - lastFrameNumber;
                        //std::cout << frameDiff << " ";
                        totalFrameCount += frameDiff;
                } else {
                        // frameDiff = 1; already set
                        frameDiff = 1;
                        // sleep a little bit
                        // Initial condition - Fill subject array.
                    int nSubjects = ViconBridge::Instance().getClient().GetSubjectCount().SubjectCount;
                    //std::cout << "Found " << nSubjects << " in Scene." << std::endl;
                    if (nSubjects == 0) {
                        ROS_ERROR("No subjects in Scene. Quitting...");
                        ros::shutdown();
                        return;
                    }
                    subjects.resize(nSubjects);
                    for (int i = 0; i < nSubjects; ++i) {
                        subjects[i] = ViconSubject(i);
                                subjects[i].init();
                        }
                }

                //std::cout << "Framediff: " << frameDiff << std::endl;
                if (frameDiff != 0) {

                        // dropped frames?
                        if (frameDiff > 1) {
                                totalDroppedFrameCount += (frameDiff - 1);

                                // div by 0 cannot happen here.
                        ROS_WARN_STREAM(frameDiff << " more (total "
                                        << totalDroppedFrameCount << "/" << totalFrameCount << ", "
                                        << ((double)totalDroppedFrameCount / totalFrameCount) * 100 <<
                                        "%) frame(s) dropped. Consider adjusting rates.");
                        }

                        // process Segments
                        for (unsigned int i = 0; i < subjects.size(); ++i) {
                                subjects.at(i).process();
                        }

                        // set new lastFrameNumber
                        lastFrameNumber = currentFrameNumber;
                } else {
                        // no new frame
                        ROS_WARN_STREAM("No new Frame. FrameDiff == 0!");
                }

                // sleep a little
                usleep(100);
        }
}


} // namepsace