ViconMarkerProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotComponents::ArmarXObjects::ViconMarkerProvider
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ViconMarkerProvider.h"
 
 
using namespace armarx;
 
 
void ViconMarkerProvider::onInitComponent()
{
    offeringTopic(getProperty<std::string>("ViconDataTopicName").getValue());
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
}
 
 
void ViconMarkerProvider::onConnectComponent()
{
    listener = getTopic<ViconMarkerProviderListenerInterfacePrx>(getProperty<std::string>("ViconDataTopicName").getValue());
    debugDrawer = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    std::string address = getProperty<std::string>("Hostname").getValue();
    address += ":" + to_string(getProperty<int>("Port").getValue());
 
    ARMARX_INFO << "Connecting to VICON server at " << address << "...";
 
    try
    {
        dsClient.Connect(address);
        if (dsClient.IsConnected().Connected)
        {
            ARMARX_INFO << "Connected to VICON server. Setting up configuration...";
 
            if (dsClient.EnableMarkerData().Result && dsClient.EnableUnlabeledMarkerData().Result && dsClient.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull).Result)
            {
                ARMARX_INFO << "Setup complete. Starting polling thread.";
 
                publisherThread = new RunningTask<ViconMarkerProvider>(this, &ViconMarkerProvider::publish, "ViconPublishThread");
                publisherThread->start();
            }
            else
            {
                ARMARX_ERROR << "Failed to set up configuration!";
            }
        }
        else
        {
            ARMARX_ERROR << "Connection failed!";
        }
    }
    catch (...)
    {
        ARMARX_ERROR << "Exception while connecting to vicon server!";
        handleExceptions();
    }
}
 
 
void ViconMarkerProvider::onDisconnectComponent()
{
    publisherThread->stop();
    dsClient.Disconnect();
}
 
 
void ViconMarkerProvider::onExitComponent()
{
 
}
 
void ViconMarkerProvider::publish()
{
    while (publisherThread->isRunning())
    {
        if (!dsClient.IsConnected().Connected)
        {
            ARMARX_ERROR << "Connection to VICON server broken!";
            return;
        }
 
        // Wait for frame (blocking call)
        if (dsClient.GetFrame().Result != ViconDataStreamSDK::CPP::Result::Success)
        {
            ARMARX_WARNING << "Failed to receive frame, will retry in one second! (Is VICON Nexus currently running in online or playback mode?)";
            sleep(1);
            continue;
        }
 
        DebugDrawerColoredPointCloud pointCloud;
        pointCloud.pointSize = 10;
 
        // Receive labeled markers
        std::map<std::string, Vector3f> labeledMarkers;
        int labeledMarkerCount = 0;
        int subjectCount = dsClient.GetSubjectCount().SubjectCount;
 
        for (int curSubj = 0; curSubj < subjectCount; ++curSubj)
        {
            std::string subjectName = dsClient.GetSubjectName(curSubj).SubjectName;
 
            int subjectMarkerCount = dsClient.GetMarkerCount(subjectName).MarkerCount;
            for (int i = 0; i < subjectMarkerCount; ++i)
            {
                std::string markerName = dsClient.GetMarkerName(subjectName, i).MarkerName;
                double* translation = dsClient.GetMarkerGlobalTranslation(subjectName, markerName).Translation;
 
                Vector3f v;
                v.e0 = translation[0];
                v.e1 = translation[1];
                v.e2 = translation[2];
                labeledMarkers[subjectName + ":" + markerName] = v;
 
                DebugDrawerColoredPointCloudElement p;
                p.x = v.e0;
                p.y = v.e1;
                p.z = v.e2;
                p.color.r = 0.0f;
                p.color.g = 0.0f;
                p.color.b = 1.0f;
                pointCloud.points.push_back(p);
 
                ++labeledMarkerCount;
            }
        }
 
        //        for (auto& it : labeledMarkers)
        //        {
        //            Vector3f v = it.second;
        //            ARMARX_INFO << it.first << " " << v.e0 << " " << v.e1 << " " << v.e2;
        //        }
        listener->reportLabeledViconMarkerFrame(labeledMarkers);
 
        // Receive unlabeled markers
        std::vector<Vector3f> unlabeledMarkers;
        int unlabeledMarkerCount = dsClient.GetUnlabeledMarkerCount().MarkerCount;
 
        for (int i = 0; i < unlabeledMarkerCount; ++i)
        {
            double* translation = dsClient.GetUnlabeledMarkerGlobalTranslation(i).Translation;
 
            Vector3f v;
            v.e0 = translation[0];
            v.e1 = translation[1];
            v.e2 = translation[2];
            unlabeledMarkers.push_back(v);
 
            DebugDrawerColoredPointCloudElement p;
            p.x = v.e0;
            p.y = v.e1;
            p.z = v.e2;
            p.color.r = 1.0f;
            p.color.g = 0.0f;
            p.color.b = 0.0f;
            pointCloud.points.push_back(p);
        }
 
        listener->reportUnlabeledViconMarkerFrame(unlabeledMarkers);
 
        ARMARX_DEBUG << "Frame processed: " << subjectCount << " subjects, " << labeledMarkerCount << " labeled markers, " << unlabeledMarkers.size() << " unlabeled markers";
 
        debugDrawer->setColoredPointCloudVisu("ViconMarkerProvider", "MarkerCloud", pointCloud);
    }
}
 
armarx::PropertyDefinitionsPtr ViconMarkerProvider::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(new ViconMarkerProviderPropertyDefinitions(
            getConfigIdentifier()));
}