POSITION message support

modules/openigtlink/processors/openigtlinkclient.cpp

@@ -25,6 +25,7 @@
 #include "openigtlinkclient.h"
 
 #include "transformdata.h"
+#include "positiondata.h"
 
 #include <igtlTransformMessage.h>
 #include <igtlPositionMessage.h>
@@ -48,6 +49,8 @@ namespace campvis {
         , p_targetTransformID("targetTransformName", "Target Transform ID", "OpenIGTLinkClient.transform", DataNameProperty::WRITE)
         , p_imageOffset("ImageOffset", "Image Offset in mm", tgt::vec3(0.f), tgt::vec3(-10000.f), tgt::vec3(10000.f), tgt::vec3(0.1f))
         , p_voxelSize("VoxelSize", "Voxel Size in mm", tgt::vec3(1.f), tgt::vec3(-100.f), tgt::vec3(100.f), tgt::vec3(0.1f))
+        , p_receivePositions("ReceivePositions", "Receive POSITION Messages", true)
+        , p_targetPositionID("targetPositionsID", "Target Position ID", "OpenIGTLinkClient.position", DataNameProperty::WRITE)
     {
         _lastReceivedTransform = 0;
         _lastReceivedImageMessage = 0;
@@ -55,14 +58,17 @@ namespace campvis {
         addProperty(p_address, VALID);
         addProperty(p_port, VALID);
         addProperty(p_deviceName, VALID);
+
+        addProperty(p_connect, VALID);
+
         addProperty(p_receiveTransforms, INVALID_RESULT | INVALID_PROPERTIES);
         addProperty(p_receiveImages, INVALID_RESULT | INVALID_PROPERTIES);
         addProperty(p_targetTransformID, VALID);
         addProperty(p_targetImageID, VALID);
         addProperty(p_imageOffset, VALID);
         addProperty(p_voxelSize, VALID);
-        
-        addProperty(p_connect, VALID);
+        addProperty(p_receivePositions, INVALID_RESULT | INVALID_PROPERTIES);
+        addProperty(p_targetPositionID, VALID);
     }
 
     OpenIGTLinkClient::~OpenIGTLinkClient() {
@@ -142,6 +148,15 @@ namespace campvis {
             }
         }
 
+        if(p_receivePositions.getValue())
+        {
+            _lastReceivedPositionMutex.lock();
+            PositionData * pd = new PositionData(_lastReceivedPosition, _lastReceivedQuaternion);
+            _lastReceivedPositionMutex.unlock();
+
+            data.addData(p_targetPositionID.getValue(), pd);
+        }
+
         validate(INVALID_RESULT);
     }
 
@@ -231,17 +246,16 @@ namespace campvis {
 
         if (c & igtl::MessageHeader::UNPACK_BODY) // if CRC check is OK
         {
-            // Retrive the transform data
-            float position[3];
-            float quaternion[4];
+            _lastReceivedPositionMutex.lock();
+            positionMsg->GetPosition(_lastReceivedPosition.elem);
+            positionMsg->GetQuaternion(_lastReceivedQuaternion.elem);
+            _lastReceivedPositionMutex.unlock();
 
-            positionMsg->GetPosition(position);
-            positionMsg->GetQuaternion(quaternion);
-
-            std::cerr << "position   = (" << position[0] << ", " << position[1] << ", " << position[2] << ")" << std::endl;
-            std::cerr << "quaternion = (" << quaternion[0] << ", " << quaternion[1] << ", "
-                << quaternion[2] << ", " << quaternion[3] << ")" << std::endl << std::endl;
+            std::cerr << "position   = (" << _lastReceivedPosition[0] << ", " << _lastReceivedPosition[1] << ", " << _lastReceivedPosition[2] << ")" << std::endl;
+            std::cerr << "quaternion = (" << _lastReceivedQuaternion[0] << ", " << _lastReceivedQuaternion[1] << ", "
+                << _lastReceivedQuaternion[2] << ", " << _lastReceivedQuaternion[3] << ")" << std::endl << std::endl;
 
+            invalidate(INVALID_RESULT);
             return 1;
         }
 
@@ -368,10 +382,13 @@ namespace campvis {
                 else
                     _socket->Skip(headerMsg->GetBodySizeToRead(), 0);
             }
-            /*else if (strcmp(headerMsg->GetDeviceType(), "POSITION") == 0)
+            else if (strcmp(headerMsg->GetDeviceType(), "POSITION") == 0)
             {
-                ReceivePosition(_socket, headerMsg);
-            }*/
+                if(p_receivePositions.getValue())
+                    ReceivePosition(_socket, headerMsg);
+                else
+                    _socket->Skip(headerMsg->GetBodySizeToRead(), 0);
+            }
             else if (strcmp(headerMsg->GetDeviceType(), "IMAGE") == 0)
             {
                 if(p_receiveImages.getValue())

modules/openigtlink/processors/openigtlinkclient.h

@@ -92,7 +92,8 @@ namespace campvis {
         Vec3Property p_imageOffset;         ///< Image Offset in mm
         Vec3Property p_voxelSize;           ///< Voxel Size in mm
 
-        
+        BoolProperty p_receivePositions;       ///< toggle receiving IMAGE messages
+        DataNameProperty p_targetPositionID;   ///< image ID for read image        
 
         /**
          * Updates the data container with the latest received frame/transformation
@@ -123,6 +124,10 @@ namespace campvis {
         igtl::ImageMessage::Pointer _lastReceivedImageMessage;  ///< last received igtl image message
         tbb::mutex _lastReceivedImageMessageMutex;              ///< mutex to control access to the _lastReceivedImageMessage pointer
 
+        tbb::mutex _lastReceivedPositionMutex;                  ///< mutex to control access to _lastReceivedPosition and _lastReceivedQuaternion
+        tgt::vec3 _lastReceivedPosition;                        ///< last received position in the position message
+        tgt::vec4 _lastReceivedQuaternion;                      ///< last received quaternion/orientation in the position message
+
     };
 
 }

modules/openigtlink/processors/positiondata.h

+// ================================================================================================
+// 
+// This file is part of the CAMPVis Software Framework.
+// 
+// If not explicitly stated otherwise: Copyright (C) 2012-2014, all rights reserved,
+//      Christian Schulte zu Berge <christian.szb@in.tum.de>
+//      Chair for Computer Aided Medical Procedures
+//      Technische Universitaet Muenchen
+//      Boltzmannstr. 3, 85748 Garching b. Muenchen, Germany
+// 
+// For a full list of authors and contributors, please refer to the file "AUTHORS.txt".
+// 
+// Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file 
+// except in compliance with the License. You may obtain a copy of the License at
+// 
+// http://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software distributed under the 
+// License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 
+// either express or implied. See the License for the specific language governing permissions 
+// and limitations under the License.
+// 
+// ================================================================================================
+
+#ifndef POSITIONDATA_H__
+#define POSITIONDATA_H__
+
+#include "tgt/vector.h"
+
+#include "core/datastructures/abstractdata.h"
+
+namespace campvis {
+
+    /**
+     * Abstract base class for data handled by a DataHandle and stored in a DataContainer.
+     * 
+     * \todo 
+     */
+    class PositionData : public AbstractData {
+    public:
+        /**
+         * Constructor, Creates a new light source.
+         * \param   position       The initial position
+         * \param   quaternion     The initial quaternion/orientation
+         */
+        explicit PositionData(const tgt::vec3& position, const tgt::vec4& quaternion) 
+            : _position(position)
+            , _quaternion(quaternion) 
+        { };
+
+        /**
+         * Virtual destructor
+         */
+        virtual ~PositionData() {};
+
+        /// \see AbstractData::clone()
+        virtual PositionData* clone() const {
+            return new PositionData(*this);
+        };
+
+        /// \see AbstractData::getLocalMemoryFootprint()
+        virtual size_t getLocalMemoryFootprint() const {
+            return sizeof(PositionData);
+        };
+        /// \see AbstractData::getVideoMemoryFootprint()
+        virtual size_t getVideoMemoryFootprint() const {
+            return 0;
+        };
+
+        /**
+        * Gets the quaternion
+        * \return _quaternion
+        **/
+        tgt::vec4 getQuaternion() const { return _quaternion; }
+
+        /**
+        * Sets the quaternion.
+        * \param val New quaternion matrix
+        **/
+        void setQuaternion(tgt::vec4 val) { _quaternion = val; }
+        
+        /**
+        * Gets the position
+        * \return _position
+        **/
+        tgt::vec3 getPosition() const { return _position; }
+
+        /**
+        * Sets the position.
+        * \param val New position matrix
+        **/
+        void setPosition(tgt::vec3 val) { _position = val; }
+        
+
+    protected:
+        tgt::vec3 _position;                ///< the position
+        tgt::vec4 _quaternion;              ///< The orientation quaternion
+    };
+
+}
+
+#endif // POSITION_H__
\ No newline at end of file