[mitk-users] mitkIGT and MedSAFE integration

[Kostas Pachopoulos]

Hallo everybody,
i am trying to add support for the Ascension MedSAFE device
(http://www.ascension-tech.com/medical/medSAFE.php) to mitkIGT.
Although this should be straightforward, since the API of the MedSAFE
device is 90+ % the same with the MicroBird
one, i face some problems. These are probably related to the threading
support. Please feel free to  correct or do any additions.


MitkIGT architecture/funcionality
==================================
Jochen Neuhaus kindly informed me for the following, regarding the
architecture/funcionality of
mitkIGT.
** "Tracking" (reading data from the sensors) is done by a separate thread
** These data are afterwards passed to the "Tools" and handled by them
(1 tool <-> 1 sensor)
** A "NavigationDataRecorder", running on the Main thread, reads  the
navigation data and
writes them to a stream.
        ** The two threads do not interact with each other in any way. It is
up to the scheduler to
           decide what will be run next; this will change in the future.
** If there is a filter between the "tracking" and the "recording", it
is executed by another thread.


Additional header and source files
===================================
Support for MedSAFE is based on the following file additions:
-mitkMedSAFETool.h/cpp (mitkMedSAFETool.h should be removed, since it
adds no more to
 mitkInternalTrackingTool.h)
-mitkMedSAFETrackingDevice.h/cpp

and creating an "IGTMedSAFETest" test project with:
-IGTMedSAFETest.cpp file


The following existing IGT file have been edited:
-mitkTrackingTypes.h:
        enum TrackingDeviceType {
        ...
        AscensionMedSAFE,
        ...     }
-mitkInternalTrackingTool.h:
        class InternalTrackingTool: public TrackingTool {
        ...
        friend class MedSAFETrackingDevice;
        ...     }
-mitkTrackingDeviceSource.cpp:
        Connect() {
        ...
        if (m_TrackingDevice->GetType()==mitk::AscensionMedSAFE)
                this->CreateOutputs();
        ...     }

note: this is necessary for the same reason NDIAurora requires it.


CMake Settings
==================================
cmake settings have been made in MITKIGTHardware.cmake and in
CMakeLists.txt (Core/IGT)



Issues
==================================
The problem i face is the following:
** if i run my sample program, which is exactly the same as
IGTTutorialStep1.cpp without
the DisplacementFilter, i get an XML output, which repeats the values
of the starting position
of the sensor. That is case whether the sensors are in movement or not.
** if i place 2 breakpoints; one inside the
"mitk::MedSAFETrackingDevice::TrackTools()" loop and
one inside the "mitk::NavigationDataRecorder::Update()" function, the
XML data DO change and follow
the sensor movement
There is possibly a race condition somwhere. I try work this out.

Unfortunately using a Thread Profiler like the Intel one, is not much
of a help, since after
instrumenting the program an exception is thrown.


I include attachments with all the files mentioned above, except the
IGT files the changes in which
i have already pointed out (mitkTrackingTypes.h,
mitkInternalTrackingTool.h, mitkTrackingDeviceSource.cpp)


Any help appreciated.

Thanks,
Kostas

INCLUDE(MITKIGTHardware.cmake)

####
#INCLUDES
####
INCLUDE_DIRECTORIES(IGTFilters IGTTrackingDevices)
IF(MITK_USE_MICRON_TRACKER)
  INCLUDE_DIRECTORIES(${MITK_MICRON_TRACKER_INCLUDE_DIR})
ENDIF(MITK_USE_MICRON_TRACKER)
IF(MITK_USE_MICROBIRD_TRACKER)
  INCLUDE_DIRECTORIES(${MITK_USE_MICROBIRD_TRACKER_INCLUDE_DIR})
ENDIF(MITK_USE_MICROBIRD_TRACKER)
IF(MITK_USE_MEDSAFE_TRACKER)                                    #MedSAFE 
tracker include directories
  INCLUDE_DIRECTORIES(${MITK_USE_MEDSAFE_TRACKER_INCLUDE_DIR})
ENDIF(MITK_USE_MEDSAFE_TRACKER)

IF(MITK_BUILD_SHARED_CORE)
  CREATE_CORE_LIB(SHARED)
ELSE(MITK_BUILD_SHARED_CORE)
  CREATE_CORE_LIB()  
ENDIF(MITK_BUILD_SHARED_CORE)

TARGET_LINK_LIBRARIES(mitkIGT mitkCore tinyxml)


####
#LINKS
####
# Link MICRON TRACKER to IGT
IF(MITK_USE_MICRON_TRACKER)
  TARGET_LINK_LIBRARIES(mitkIGT ${MITK_MICRON_TRACKER_LIB})
ENDIF(MITK_USE_MICRON_TRACKER)

# Link Ascension MicroBird to IGT
IF(MITK_USE_MICROBIRD_TRACKER)
  TARGET_LINK_LIBRARIES(mitkIGT ${MITK_USE_MICROBIRD_TRACKER_LIB})
ENDIF(MITK_USE_MICROBIRD_TRACKER)

IF(MITK_USE_MEDSAFE_TRACKER)                                    #Link Ascension 
MedSAFE to IGT
  TARGET_LINK_LIBRARIES(mitkIGT ${MITK_USE_MEDSAFE_TRACKER_LIB})
ENDIF(MITK_USE_MEDSAFE_TRACKER)

## create IGT config
CONFIGURE_FILE(mitkIGTConfig.h.in ${PROJECT_BINARY_DIR}/mitkIGTConfig.h @ONLY)

# add test programm for serial communication class
#ADD_EXECUTABLE(SerialCommunicationTest 
IGTTrackingDevices/mitkSerialCommunicationTest.cpp)
#TARGET_LINK_LIBRARIES(SerialCommunicationTest mitkIGT mitkCore tinyxml 
PocoXML) 

ADD_EXECUTABLE (IGTTutorialStep1 IGTTutorial/IGTTutorialStep1.cpp)
TARGET_LINK_LIBRARIES(IGTTutorialStep1 mitkIGT mitkCore tinyxml PocoXML)

ADD_EXECUTABLE (IGTMedSAFETest IGTMedSAFETest/IGTMedSAFETest.cpp)
TARGET_LINK_LIBRARIES(IGTMedSAFETest  mitkIGT mitkCore tinyxml PocoXML)

files.cmake
Description: Binary data

#include <mitkMedSAFETrackingDevice.h>
#include <mitkInternalTrackingTool.h>

#include <mitkNavigationData.h>
#include <mitkTrackingDeviceSource.h>
#include "mitkNavigationDataDisplacementFilter.h"
#include <mitkNavigationDataRecorder.h>
#include <mitkNavigationDataPlayer.h>

#include <itksys/SystemTools.hxx>
#include <iostream>					//TODO: remove after  testing

using namespace std;

int main(int argc, char* argv[])
{
	//create MedSAFETracking device; register available tools to the device
	mitk::MedSAFETrackingDevice::Pointer tracker= mitk::MedSAFETrackingDevice::New();	

	//a MedSAFETrackingDevice is encapsulated in TrackingDeviceSource; see TrackingDeviceSource class description
	mitk::TrackingDeviceSource::Pointer source = mitk::TrackingDeviceSource::New();
	source->SetTrackingDevice(tracker);
	source->Connect(); 							
	source->StartTracking();	

	////////////Displacement filter - NavigationDataToNavigationDataFilter
	//mitk::NavigationDataDisplacementFilter::Pointer displacer = mitk::NavigationDataDisplacementFilter::New();
	//mitk::Vector3D offset;
	//mitk::FillVector3D(offset, 10.0, 100.0, 1.0); //initialize the offset
	//displacer->SetOffset(offset); //now set the offset in the NavigationDataDisplacementFilter object

	////now every output of the source object is connected to the displacer object
	////for (int i = 0; i < source->GetNumberOfOutputs(); i++)
	//for (int i = 0; i <100; i++)
	//{
	//	displacer->SetInput(i, source->GetOutput(i));  //here we connect to the displacement filter 
	//}


    //////////record data - NavigationDataRecorder
	//we need the stringstream for building up our filename
	std::stringstream filename;  
	filename << "C:\\Documents and Settings\\kpachopo\\My Documents\\medsafe\\test.xml" << std::endl;
	std::cout << "Record to file: " << filename.str() << std::endl;

	mitk::NavigationDataRecorder::Pointer recorder = mitk::NavigationDataRecorder::New();
	recorder->SetFileName(filename.str());

	//now every output of the displacer object is connected to the recorder object
	for (int i = 0; i < source->GetNumberOfOutputs(); i++)
	{
		recorder->AddNavigationData(source->GetOutput(i));  // here we connect to the recorder
	}

	recorder->StartRecording(); //TODO: should i move it above/below the loop?

	for (int i = 0; i<4000; i++)
	{		
		recorder->Update(); //the update causes one line in the XML file for every tool
			                //in this case two lines
		itksys::SystemTools::Delay(100);         //sleep a little
		//Sleep(static_cast<DWORD>(100));
	}
	

	recorder->StopRecording(); 


	////////////play data - NavigationDataPlayer
	////filename << "C:\\Documents and Settings\\kpachopo\\My Documents\\medsafe\\test.xml"  << std::endl;

	//mitk::NavigationDataPlayer::Pointer player = mitk::NavigationDataPlayer::New();	
	//player->SetFileName(filename.str()); 
	//player->StartPlaying(); 
 // 	
	//mitk::NavigationData::Pointer nd = player->GetOutput(0); 
	//mitk::NavigationData::Pointer nd2 = player->GetOutput(1); 

	//  for (unsigned int x=0; x<100; x++) 
	//  {
	//	if (nd.IsNotNull()) //check if the output is not null
	//	{
	//	  //With this update the NavigationData object propagates through the pipeline to get a new value.
	//	  //In this case we only have a source (NavigationDataPlayer).
	//	  nd->Update(); 

	//	  std::cout << x << ": 1:" << nd->GetPosition() << std::endl;
	//	  std::cout << x << ": 2:" << nd2->GetPosition() << std::endl;
	//	  std::cout << x << ": 1:" << nd->GetOrientation() << std::endl;
	//	  std::cout << x << ": 2:" << nd2->GetOrientation() << std::endl;

	//	  itksys::SystemTools::Delay(100); //sleep a little like in the recorder part
	//	}
	//  }

	//player->StopPlaying(); //This stops the player
 //                        //With another call of StartPlaying the player will start again at the beginning of the file

 // itksys::SystemTools::Delay(2000);
  std::cout << "finished" << std::endl;
}

MITKIGTHardware.cmake
Description: Binary data

/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date: 2007-07-10 17:24:20 +0200 (Di, 10 Jul 2007) $
Version:   $Revision: 11220 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkMedSAFETool.h"

mitk::MedSAFETool::MedSAFETool() 
: InternalTrackingTool()
{
}

mitk::MedSAFETool::~MedSAFETool()
{
}

/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date: 2007-07-10 17:24:20 +0200 (Di, 10 Jul 2007) $
Version:   $Revision: 11220 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/


#ifndef MITKMEDSAFETOOL_H_HEADER_INCLUDED_
#define MITKMEDSAFETOOL_H_HEADER_INCLUDED_

#include <mitkInternalTrackingTool.h>
//#include "mitkTrackingTypes.h"

namespace mitk {
    //##Documentation
    //## \brief Implementation of a MicroBird tool
    //##
    //## 
    //## \ingroup IGT

    class MedSAFETool : public InternalTrackingTool
    {
    public:
      mitkClassMacro(MedSAFETool, InternalTrackingTool);
      itkNewMacro(Self);
    protected:
      MedSAFETool();
      ~MedSAFETool();
    };
} // namespace mitk

#endif /* MITKMEDSAFETOOL_H_HEADER_INCLUDED_ */

/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date: 2008-07-03 12:25:58 +0200 (Do, 03 Jul 2008) $
Version:   $Revision: 14720 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkMedSAFETrackingDevice.h"

#include <itksys/SystemTools.hxx>


#include <iostream>	//TODO: remove after testing
	using namespace std;

mitk::MedSAFETrackingDevice::MedSAFETrackingDevice() : TrackingDevice(),
m_ErrorMessage(""),
m_ThreadID(0),
m_pl(50), // 50 Hz for Europe
m_metric(true),
m_agcModeBoth(false),
m_measurementRate(120), // 68.3 for mid-range transmitter, 40.5 for flat transmitter
m_TransmitterConfig(NULL),
m_SensorConfig(NULL)
{
  // Flat transmitter needs measurement rate: 40.5
  // Mid-range transmitter needs measurement rate: 68.3;

  // Set the tracker type
  this->m_Type = AscensionMedSAFE;

  // Set the tracking volume
  this->m_TrackingVolume->SetTrackingDeviceType(this->m_Type);

  // Clear tools vector
  m_Tools.clear();

  // Create tools vector mutex
  m_ToolsMutex = itk::FastMutexLock::New();

  // Prepare multi-threading
  m_MultiThreader = itk::MultiThreader::New();

  // Pointer to record member variable
  pRecord = &record;
}


mitk::MedSAFETrackingDevice::~MedSAFETrackingDevice()
{
  if (m_MultiThreader)
    m_MultiThreader->TerminateThread(m_ThreadID);
  m_MultiThreader = NULL;
  if (m_ToolsMutex)
    m_ToolsMutex->Unlock();
  m_ToolsMutex = NULL;


  if (m_TransmitterConfig)
    delete [] m_TransmitterConfig;
  if (m_SensorConfig)
    delete [] m_SensorConfig;

  //\TODO: Do we need to clean up the pointers to PCIBird data like DOUBLE_POSITION_QUATERNION_TIME_Q_RECORD?
}


bool mitk::MedSAFETrackingDevice::OpenConnection()
{
  /* Grab mode mutex */
  this->m_ModeMutex->Lock();

  /* Check whether in setup mode */
  if (this->m_Mode != Setup)
  {
    this->SetErrorMessage("Can only try to open the connection if in setup mode");
    this->m_ModeMutex->Unlock();
    return false;
  }

  int errorCode; // Holds error code

  /* Initialize the PCIBIRD driver and DLL */
  errorCode = InitializeBIRDSystem(); // this function can take a few seconds
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    this->m_ModeMutex->Unlock();
    return false;
  }
  /// @todo : check for transmitter serial numbers here?
  // Serial numbers could be compared to known ones for some simple
  //    parameters sanity check (measurement frequency etc.)

  /* Get system configuration */
  errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    this->m_ModeMutex->Unlock();
    return false;
  }

  /* use metric measurements in mm */
  errorCode = SetSystemParameter(METRIC, &m_metric, sizeof(m_metric)); 
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    this->m_ModeMutex->Unlock();
    return false;
  }

  /* Set the measurement rate to m_measurementRate */
  if ((m_measurementRate > 30) && (m_measurementRate < 80))
  {
    errorCode = SetSystemParameter(MEASUREMENT_RATE, &m_measurementRate, sizeof(m_measurementRate)); 
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
      this->m_ModeMutex->Unlock();
      return false;
    }
  }

  /* Set power line frequency */
  if ((m_pl >= 50) && (m_pl <= 60))
  {
    errorCode = SetSystemParameter(POWER_LINE_FREQUENCY, &m_pl, sizeof(m_pl));
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
      this->m_ModeMutex->Unlock();
      return false;
    }
  }

  /* Set AGC mode */
  m_agc = m_agcModeBoth ? TRANSMITTER_AND_SENSOR_AGC : SENSOR_AGC_ONLY;
  errorCode = SetSystemParameter(AGC_MODE, &m_agc, sizeof(m_agc));
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    HandleError(errorCode);
    this->m_ModeMutex->Unlock();
    return false;
  }


  /* Get sensor information */
  m_SensorConfig = new SENSOR_CONFIGURATION[m_SystemConfig.numberSensors];
  for (int i = 0; i < m_SystemConfig.numberSensors; i++)
  {
    errorCode = GetSensorConfiguration(i, &(m_SensorConfig[i]));
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
    }

    /* Initialize the quality parameter structure */
    QUALITY_PARAMETERS qualityParameters; // = { 164, 0, 32, 3072 };
    GetSensorParameter(i, QUALITY, &qualityParameters, sizeof(qualityParameters));

    /* Set data format to matrix format */
    //DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_MATRIX_TIME_Q;
    /* Set data format to quaternion format */
    DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_ANGLES_TIME_Q;

    /* Set data format for sensor */
    DATA_FORMAT_TYPE *pTempBuffer = &tempBuffer;
    errorCode = SetSensorParameter(i, DATA_FORMAT, pTempBuffer, sizeof(tempBuffer));	
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
    }
  }

  /* Initialise tools vector */
  m_ToolsMutex->Lock();
  for (int i = 0; i < m_SystemConfig.numberSensors; i++)
  {
    if (m_SensorConfig[i].attached)
      m_Tools.push_back(ToolType::New());

  }
  m_ToolsMutex->Unlock();

  /* Get transmitter configuration */
  m_TransmitterConfig = new TRANSMITTER_CONFIGURATION[m_SystemConfig.numberTransmitters];
  for (int i = 0; i < m_SystemConfig.numberTransmitters; i++)
  {
    errorCode = GetTransmitterConfiguration(i, &(m_TransmitterConfig[i]));
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
    }
  }

  /* Switch off transmitter */
  SwitchTransmitter(true);
  SwitchTransmitter(false);

  // @todo : set up error scaling?

  /* finish  - now all tools should be added, initialized and enabled, so that tracking can be started */
  this->SetMode(Ready);
  this->SetErrorMessage("");
  this->m_ModeMutex->Unlock();

  return true; // Return success
}


bool mitk::MedSAFETrackingDevice::SwitchTransmitter(bool switchOn)
{ 
  if (switchOn)
  {
    /* Search for the first attached transmitter and turn it on */
    for (short id = 0; id < m_SystemConfig.numberTransmitters; id++)
    {
      if (m_TransmitterConfig[id].attached)
      {
        // Transmitter selection is a system function.
        // Using the SELECT_TRANSMITTER parameter we send the id of the
        // transmitter that we want to run with the SetSystemParameter() call
        int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
        if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
        {
          HandleError(errorCode);
          return false;
        }
        else 
          return true; //break; // \TODO: Stop after the first attached transmitter was turned off?
      }
    }
  }
  else
  {
    /* Transmitter selection is a system function, Note: a selector of -1 switches off the current transmitter */
    short TRANSMITTER_OFF = -1;
    int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &TRANSMITTER_OFF, sizeof(TRANSMITTER_OFF));
    if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    {
      HandleError(errorCode);
      return false;
    }
    else
      return true;
  }
  // Return success
  return true;
}


bool mitk::MedSAFETrackingDevice::CloseConnection()
{
  this->m_ModeMutex->Lock(); // Grab mode mutex

  SwitchTransmitter(false); // Switch off the transmitter

  int errorCode = CloseBIRDSystem(); // Close connection. This function can take a few seconds

  // Error checking
  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
    HandleError(errorCode);

  // Delete configuration
  if (m_TransmitterConfig)
    delete [] m_TransmitterConfig;
  if (m_SensorConfig)
    delete [] m_SensorConfig;

  // Change mode and release mutex
  this->SetMode(Setup);
  m_ModeMutex->Unlock();

  // Clear error message
  this->SetErrorMessage("");

  return true; 
}


ITK_THREAD_RETURN_TYPE mitk::MedSAFETrackingDevice::ThreadStartTracking(void* pInfoStruct)
{
  /* extract this pointer from Thread Info structure */
  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
  if ((pInfo == NULL) || (pInfo->UserData == NULL))
    return ITK_THREAD_RETURN_VALUE;

  MedSAFETrackingDevice *trackingDevice = (MedSAFETrackingDevice*)pInfo->UserData;
  if (trackingDevice != NULL)
    trackingDevice->TrackTools();             // call TrackTools() from the original object

  return ITK_THREAD_RETURN_VALUE;
}


bool mitk::MedSAFETrackingDevice::StopTracking()
{
  TrackingDevice::StopTracking(); // Call superclass method

  SwitchTransmitter(false); // Switch off transmitter
  InvalidateAll(); // Invalidate all tools
  return true;   // \todo : think about return value
}


bool mitk::MedSAFETrackingDevice::StartTracking()
{
  this->m_ModeMutex->Lock();   // Grab mode mutex
  if (m_Mode != Ready)
  {
    this->m_ModeMutex->Unlock();
    return false;
  }
  this->SetMode(Tracking); // Go to mode Tracking

  /* Switch on transmitter */
  SwitchTransmitter(true);

  /* Update the local copy of m_StopTracking */
  this->m_StopTrackingMutex->Lock();
  this->m_StopTracking = false;
  this->m_StopTrackingMutex->Unlock();

  m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread 
  m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method

  this->m_ModeMutex->Unlock(); // Release mode mutex
  return true;
}


void mitk::MedSAFETrackingDevice::TrackTools()
{
  // lock the TrackingFinishedMutex to signal that the execution rights
  //   are now transfered to the tracking thread
  m_TrackingFinishedMutex->Lock();

  // Because m_StopTracking is used by two threads, access has to be guarded
  //   by a mutex. To minimize thread locking, a local copy is used here 
  bool localStopTracking;

  /* update the local copy of m_StopTracking */
  this->m_StopTrackingMutex->Lock();
  localStopTracking = this->m_StopTracking;
  this->m_StopTrackingMutex->Unlock();

  /* Frequency configuration */
  double updateRate = 1000.0 / m_SystemConfig.measurementRate;
  double measurementDuration = 0.0;

  /* Tracking loop */
  while ((this->GetMode() == Tracking) && (localStopTracking == false))
  //while (true)
  {
    int errorCode;
    unsigned int nOfAttachedSensors = 0;
    double timeStamp = 0.0;   
    int toolNumber = 0; // Numbers for attached sensors only

    for (int sensorID = 0; sensorID < m_SystemConfig.numberSensors; sensorID++) // for each sensor grep data
    {
      if (!m_SensorConfig[sensorID].attached)
        continue;

      // sensor attached so get record
      errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record));
      if (CompareError(errorCode, BIRD_ERROR_SUCCESS))      
      { // On SUCCESS, parse sensor information
        nOfAttachedSensors++;       
        timeStamp += record.time; // Get timestamp from record
        ToolType* tool = GetMedSAFETool(toolNumber); /// Get tool (current sensor)
        if (tool != NULL)
        {
          tool->SetTrackingError(record.quality); // Set tracking error (quality) from record
          mitk::Point3D position;
          position[0] = pRecord->x;
          position[1] = pRecord->y;
          position[2] = pRecord->z;
		  cout << pRecord->time <<" "<<pRecord->x<<" "<<pRecord->y<<" "<<pRecord->z<< endl; //TODO: remove after testing
		  cout << record.time <<" "<<record.x<<" "<<record.y<<" "<<record.z<< endl; //TODO: remove after testing

          tool->SetPosition(position);  // Set position
          //mitk::Quaternion orientation(record.q[1], record.q[2], record.q[3],record.q[0]);
          //tool->SetOrientation(orientation); // Set orientation as quaternion \todo : verify quaternion order q(r,x,y,z)
          tool->SetDataValid(true); // Set data state to valid
        }        
        toolNumber++; // Increment tool number
      }	
      else
      { // ERROR while reading sensor information
        HandleError(errorCode);
      }      
    }

    /// @todo : is there any synchronisation?
    // Average timestamp: timeStamp/nOfAttachedSensors

    // Compute sleep time
    double sleepTime = updateRate - measurementDuration;
    // Sleep
    if (sleepTime > 0.0 && sleepTime < 500.0)
    {
      // Note: we only have to approximately sleep one measurement cycle,
      //    since the tracker keeps track of the measurement rate itself
      Sleep(static_cast<DWORD>(sleepTime));
    }

    // Update the local copy of m_StopTracking
    this->m_StopTrackingMutex->Lock();  
    localStopTracking = m_StopTracking;
    this->m_StopTrackingMutex->Unlock();
	Sleep(100);
  }

  // @bug (#1813) : maybe we need to check for localStopTracking=true here?
  //    m_StopTracking should only ever be updated by StopTracking(), so
  //    maybe we should not unlock a mutex that nobody is waiting for?

  m_TrackingFinishedMutex->Unlock(); // transfer control back to main thread (last action here) 
  return; // returning from this function (and ThreadStartTracking()) this will end the thread
}


mitk::TrackingTool* mitk::MedSAFETrackingDevice::GetTool(unsigned int toolNumber)
{
  return static_cast<TrackingTool*>(GetMedSAFETool(toolNumber));
}


mitk::MedSAFETrackingDevice::ToolType* mitk::MedSAFETrackingDevice::GetMedSAFETool(unsigned int toolNumber)
{
  ToolType* t = NULL;

  m_ToolsMutex->Lock();
  if (toolNumber < m_Tools.size())
  {
    t = m_Tools.at(toolNumber);
  }
  m_ToolsMutex->Unlock();
  return t;
}


unsigned int mitk::MedSAFETrackingDevice::GetToolCount() const
{
  unsigned int s = 0;
  m_ToolsMutex->Lock();
  s = m_Tools.size();
  m_ToolsMutex->Unlock();
  return s;
}


bool mitk::MedSAFETrackingDevice::CompareError(int errorCode, int errorConstant)
{
  return ((errorCode & 0xffff) == errorConstant);
}


void mitk::MedSAFETrackingDevice::HandleError(int errorCode)
{
  char buffer[1024];
  char* pBuffer = &buffer[0];

  while(!CompareError(errorCode, BIRD_ERROR_SUCCESS))
  {
    // Print error number on screen
    //cout << "MicroBIRD Error Code: " << errorCode << endl;
    // Print error message on screen
    errorCode = GetErrorText(errorCode, pBuffer, sizeof(buffer), SIMPLE_MESSAGE);
    /// @todo : set error message, does it work?
    this->SetErrorMessage(buffer);
  }
}


void mitk::MedSAFETrackingDevice::InvalidateAll()
{
  m_ToolsMutex->Lock();
  for (ToolContainerType::iterator iterator = m_Tools.begin(); iterator != m_Tools.end(); ++iterator)
    (*iterator)->SetDataValid(false);
  m_ToolsMutex->Unlock();
}

/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date: 2008-06-26 19:19:58 +0200 (Do, 26 Jun 2008) $
Version:   $Revision: 14656 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#ifndef MITKMEDSAFETRACKINGDEVICE_H_HEADER_INCLUDED
#define MITKMEDSAFETRACKINGDEVICE_H_HEADER_INCLUDED

#include "mitkTrackingDevice.h"

#include <mitkCommon.h>
#include <itkMultiThreader.h>

#include <list>

#include "mitkTrackingTypes.h"
#include "mitkMedSAFETool.h"

#include "ATC3DGm.h"

namespace mitk {
  /**Documentation
  * \brief superclass for specific medSAFE tracking Devices
  *
  * \ingroup IGT
  */
  class MedSAFETrackingDevice : public TrackingDevice
  {
  public:
    mitkClassMacro(MedSAFETrackingDevice, TrackingDevice);
    itkNewMacro(Self);

    /**Documentation
    * \brief Set the type of the medSAFE Tracking Device because it can not yet 
handle this itself
    */
    itkSetMacro(Type,TrackingDeviceType);

    /**Documentation
    * \brief Builds up the connection (loads tools, initializes and enables 
them)
    */
    virtual bool OpenConnection();

    /**Documentation
    * \brief Closes the connection
    **/
    virtual bool CloseConnection();

    /**Documentation
    * \brief Start the tracking. 
    * 
    * A new thread is created, which reads the position and orientation 
information of each tool and stores them inside the tools.
    **/
    virtual bool StartTracking();

    /**Documentation
    * \brief here we use the superclass method.
    **/
    virtual bool StopTracking();

    /**Documentation
    * \brief returns a tracking tool that contains positional information about 
one of the sensors
    **/
    virtual TrackingTool* GetTool(unsigned int toolNumber);

    /**Documentation
    * \brief returns a the number of attached sensors
    **/
    virtual unsigned int GetToolCount() const;

    /**Documentation
    * \brief returns description of most recent error.
    **/
    itkGetStringMacro(ErrorMessage);

  protected:
    void HandleError(int errorCode);
    bool CompareError(int errorCode, int errorConstant);

    typedef InternalTrackingTool ToolType;
    typedef std::vector<ToolType::Pointer> ToolContainerType;
    MedSAFETrackingDevice();
    virtual ~MedSAFETrackingDevice();

    /**Documentation
    * \brief returns a tracking tool that contains positional information about 
one of the sensors
    **/
    ToolType* GetMedSAFETool(unsigned int toolNumber);

    virtual void InvalidateAll();         ///< invalidates all tools (on 
stoptracking, closeconnection)     
    bool SwitchTransmitter(bool switchOn);///< Switches the transmitter on 
resp. off

    /**Documentation
    * \brief  tracks the position and orientation of all tools until 
StopTracking() is called.
    *
    * This function should only be executed by a new thread (through 
StartTracking() and ThreadStartTracking())
    */
    virtual void TrackTools();    

    static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void* data);  ///< Helper 
function, because the itk::MultiThreader can only start a new thread with a 
static member function

    itkSetStringMacro(ErrorMessage);

    itk::FastMutexLock::Pointer m_ToolsMutex;
    ToolContainerType m_Tools;  

    std::string m_ErrorMessage;

    itk::MultiThreader::Pointer m_MultiThreader;
    int m_ThreadID;

    //DOUBLE_POSITION_MATRIX_TIME_Q_RECORD record, *pRecord;  ///< One tracking 
data record (matrix orientation format)
    DOUBLE_POSITION_ANGLES_TIME_Q_RECORD record, *pRecord; ///< One tracking 
data record (quaternion orientation format)
    SYSTEM_CONFIGURATION                m_SystemConfig;         ///< The system 
configuration - used to specify its use
    SENSOR_CONFIGURATION                *m_SensorConfig;        ///< The sensor 
configuration - used to get and set the sensor properties
    TRANSMITTER_CONFIGURATION   *m_TransmitterConfig; ///< The transmitter 
configuration - used to get and set the transmitter properties
    BOOL m_metric;            ///< Specifies whether metric measurement is used
    double m_measurementRate; ///< Specifies the measurement rate - default set 
to maximum
    double m_pl;              ///< Specifies the power line frequency (Europe 
50Hz, USA 60Hz)
    bool m_agcModeBoth;       ///< AGC (automatic gain control) mode flag
    AGC_MODE_TYPE m_agc;
  };
} // namespace mitk
#endif /* MITKMEDSAFETRACKINGDEVICE_H_HEADER_INCLUDED*/

------------------------------------------------------------------------------
Register Now & Save for Velocity, the Web Performance & Operations 
Conference from O'Reilly Media. Velocity features a full day of 
expert-led, hands-on workshops and two days of sessions from industry 
leaders in dedicated Performance & Operations tracks. Use code vel09scf 
and Save an extra 15% before 5/3. http://p.sf.net/sfu/velocityconf
_______________________________________________
mitk-users mailing list
mitk-users@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/mitk-users