unsigned int mitk::IGTLDevice::SendMessagePrivate(igtl::MessageBase::Pointer msg,
igtl::Socket::Pointer socket)
{
//check the input message
if ( msg.IsNull() )
{
MITK_ERROR("IGTLDevice") << "Could not send message because message is not "
"valid. Please check.";
return false;
}
// add the name of this device to the message
msg->SetDeviceName(this->GetName().c_str());
// Pack (serialize) and send
msg->Pack();
// measure the time
AddTrackingMeasurements(5, msg, 0);
int sendSuccess = socket->Send(msg->GetPackPointer(), msg->GetPackSize());
if (sendSuccess)
{
this->InvokeEvent(MessageSentEvent());
return IGTL_STATUS_OK;
}
else
{
return IGTL_STATUS_UNKNOWN_ERROR;
}
}
void OpenIGTLinkProtocol::prepareBody(const igtl::MessageHeader::Pointer &header, igtl::MessageBase::Pointer &body)
{
body = T::New();
body->SetMessageHeader(header);
body->AllocatePack();
//CX_LOG_DEBUG() << "( ) prepareBody, pointer: " << body->GetPackPointer() << " size: " << body->GetPackSize();
//CX_LOG_DEBUG() << "(BODY) prepareBody, pointer: " << body->GetPackBodyPointer() << " size: " << body->GetPackBodySize();
}
void OpenIGTLinkProtocol::translate(const igtl::MessageHeader::Pointer &header, const igtl::MessageBase::Pointer &body)
{
//CX_LOG_DEBUG() << "Incoming message to OpenIGTLinkProtocol";
QString type = QString(header->GetDeviceType()).toUpper();
if (type=="TRANSFORM")
{
const igtl::TransformMessage::Pointer temp = dynamic_cast<igtl::TransformMessage*>(body.GetPointer());
this->translate(temp);
}
else if (type=="POLYDATA")
{
const igtl::PolyDataMessage::Pointer temp = dynamic_cast<igtl::PolyDataMessage*>(body.GetPointer());
this->translate(temp);
}
else if (type=="IMAGE")
{
// //----- CustusX openigtlink server -----
// //there is a special kind of image package coming from custusx
// //server where crc is set to 0.
// QString name(header->GetDeviceName());
// if(name.contains("Sonix", Qt::CaseInsensitive))
// {
// const IGTLinkImageMessage::Pointer temp = dynamic_cast<IGTLinkImageMessage*>(body.GetPointer());
// this->translate(temp);
// }
// //----------
// else
// {
const igtl::ImageMessage::Pointer temp = dynamic_cast<igtl::ImageMessage*>(body.GetPointer());
this->translate(temp);
// }
}
else if (type=="STATUS")
{
const igtl::StatusMessage::Pointer temp = dynamic_cast<igtl::StatusMessage*>(body.GetPointer());
this->translate(temp);
}
else if (type=="STRING")
{
const igtl::StringMessage::Pointer temp = dynamic_cast<igtl::StringMessage*>(body.GetPointer());
this->translate(temp);
}
else if (type=="CX_US_ST")
{
const IGTLinkUSStatusMessage::Pointer temp = dynamic_cast<IGTLinkUSStatusMessage*>(body.GetPointer());
this->translate(temp);
}
else
{
this->writeNotSupportedMessage(body);
}
}
void mitk::IGTLDevice::AddTrackingMeasurements(const int index, const igtl::MessageBase::Pointer msg, const long long timeStamp)
{
//Apparently this is the only "elegant" way to do a class check.. or is it???
if (dynamic_cast<igtl::TrackingDataMessage*>(msg.GetPointer()) != nullptr)
{
igtl::TrackingDataMessage* tdMsg =
(igtl::TrackingDataMessage*)(msg.GetPointer());
igtl::TrackingDataElement::Pointer trackingData = igtl::TrackingDataElement::New();
tdMsg->GetTrackingDataElement(0, trackingData);
float x_pos, y_pos, z_pos;
trackingData->GetPosition(&x_pos, &y_pos, &z_pos);
m_Measurement->AddMeasurement(index, x_pos, timeStamp); //x value is used as index
}
}
bool OpenIGTLinkProtocol::isValid(const igtl::MessageBase::Pointer &msg) const
{
if(this->isSupportedBodyType(msg->GetDeviceType()))
return true;
else
return false;
}
void testMessagesEqual(igtl::MessageBase::Pointer sentMessage, igtl::MessageBase::Pointer receivedMessage)
{
std::string lhs(sentMessage->GetDeviceName());
std::string rhs(receivedMessage->GetDeviceName());
CPPUNIT_ASSERT_MESSAGE("The device names were not the same", lhs == rhs);
igtl::StatusMessage::Pointer receivedStatusMessage = dynamic_cast<igtl::StatusMessage*>(receivedMessage.GetPointer());
igtl::StatusMessage::Pointer sentStatusMessage = dynamic_cast<igtl::StatusMessage*>(sentMessage.GetPointer());
CPPUNIT_ASSERT_MESSAGE("The received message was not of the appropriate type.", receivedStatusMessage != nullptr);
CPPUNIT_ASSERT_MESSAGE("The sent message was not of the appropriate type.", sentStatusMessage != nullptr);
lhs = receivedStatusMessage->GetStatusString();
rhs = sentStatusMessage->GetStatusString();
CPPUNIT_ASSERT_MESSAGE("The sent and received message did not contain the same status message.", lhs == rhs);
CPPUNIT_ASSERT_MESSAGE("The sent message did not contain the correct status message.", lhs == m_Message);
CPPUNIT_ASSERT_MESSAGE("The received message did not contain the correct status message.", m_Message == rhs);
};
/**
* @brief convert a igtl::RawMessage(which contain scalar) to a FwDataObjectType
*
* @return an fwData::Integer converted from an igtl::RawMessage
*/
::fwData::Object::sptr fromIgtlMessage (const ::igtl::MessageBase::Pointer src) const
{
typename FwDataObjectType::sptr obj = FwDataObjectType::New();
RawMessage::Pointer msg = RawMessage::Pointer(dynamic_cast< RawMessage*>(src.GetPointer()));
const ScalarType scalar = helper::ScalarToBytes<ScalarType>::fromBytes(msg->getMessage().data());
obj->setValue(scalar);
return obj;
}
bool OpenIGTLinkProtocol::unpackBody(const igtl::MessageBase::Pointer &body)
{
int c = body->Unpack(this->doCRC());
if (c & igtl::MessageHeader::UNPACK_BODY)
{
this->translate(mHeader, body);
}
else
{
CX_LOG_CHANNEL_ERROR(CX_OPENIGTLINK_CHANNEL_NAME) << "Could not unpack the body of type: " << body->GetDeviceType();
return false;
}
return true;
}
::fwData::Object::sptr DataConverter::fromIgtlMessage(const ::igtl::MessageBase::Pointer src) const
{
::fwData::Object::sptr obj;
const std::string deviceType = src->GetDeviceType();
if (deviceType == "ATOMS")
{
obj = m_defaultConverter->fromIgtlMessage(src);
return obj;
}
for(const converter::IConverter::sptr& converter : m_converters)
{
if (converter->getIgtlType() == deviceType)
{
obj = converter->fromIgtlMessage(src);
return obj;
}
}
SLM_WARN("Message type not supported : " + std::string(src->GetDeviceType()));
return obj;
}
::fwData::Object::sptr PointListConverter::fromIgtlMessage(const ::igtl::MessageBase::Pointer src) const
{
float igtlPos[3];
::igtl::PointElement::Pointer elem;
std::vector< ::fwData::Point::sptr> fwPoints;
::fwData::Point::sptr fwPoint;
::igtl::PointMessage* msg = dynamic_cast< ::igtl::PointMessage* >(src.GetPointer());
::igtl::PointMessage::Pointer srcPoints = ::igtl::PointMessage::Pointer(msg);
::fwData::PointList::sptr dest = ::fwData::PointList::New();
for (int i = 0; i < srcPoints->GetNumberOfPointElement(); ++i)
{
fwPoint = ::fwData::Point::New();
srcPoints->GetPointElement(i, elem);
elem->GetPosition(igtlPos);
std::transform(&igtlPos[0], &igtlPos[3], fwPoint->getCoord().begin(),
::boost::numeric_cast<float, double>);
fwPoints.push_back(fwPoint);
}
dest->setPoints(fwPoints);
return dest;
}
::fwData::Object::sptr MatrixConverter::fromIgtlMessage(const ::igtl::MessageBase::Pointer src) const
{
::igtl::Matrix4x4 matrix;
::igtl::TransformMessage* msg = dynamic_cast< ::igtl::TransformMessage* >(src.GetPointer());
::igtl::TransformMessage::Pointer srcTransform = ::igtl::TransformMessage::Pointer(msg);
::fwData::TransformationMatrix3D::sptr dest = ::fwData::TransformationMatrix3D::New();
srcTransform->GetMatrix(matrix);
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
dest->setCoefficient(i, j, matrix[i][j]);
}
}
return dest;
}
::fwData::Object::sptr CompositeConverter::fromIgtlMessage(const ::igtl::MessageBase::Pointer src) const
{
::igtl::TrackingDataMessage::Pointer trackingMsg;
trackingMsg = ::igtl::TrackingDataMessage::Pointer(dynamic_cast< ::igtl::TrackingDataMessage* >(src.GetPointer()));
const int nbTrckingElement = trackingMsg->GetNumberOfTrackingDataElements();
FW_RAISE_EXCEPTION_IF(::igtlProtocol::exception::Conversion("TrackingDataElements"), nbTrckingElement < 0);
::fwData::Composite::sptr composite = ::fwData::Composite::New();
for(int i = 0; i < nbTrckingElement; ++i)
{
::igtl::TrackingDataElement::Pointer trackElement = ::igtl::TrackingDataElement::New();
trackingMsg->GetTrackingDataElement(i, trackElement);
const std::string name = trackElement->GetName();
::fwData::TransformationMatrix3D::sptr transfoMatrix = ::fwData::TransformationMatrix3D::New();
(*composite)[name] = transfoMatrix;
::igtl::Matrix4x4 matrix;
trackElement->GetMatrix(matrix);
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
transfoMatrix->setCoefficient(i, j, matrix[i][j]);
}
}
}
return composite;
}
void mitk::IGTLMessageQueue::PushMessage(igtl::MessageBase::Pointer msg)
{
this->m_Mutex->Lock();
std::stringstream infolog;
infolog << "Received message of type ";
if (dynamic_cast<igtl::TrackingDataMessage*>(msg.GetPointer()) != nullptr)
{
if (this->m_BufferingType == IGTLMessageQueue::NoBuffering)
m_TrackingDataQueue.clear();
this->m_TrackingDataQueue.push_back(dynamic_cast<igtl::TrackingDataMessage*>(msg.GetPointer()));
infolog << "TDATA";
}
else if (dynamic_cast<igtl::TransformMessage*>(msg.GetPointer()) != nullptr)
{
if (this->m_BufferingType == IGTLMessageQueue::NoBuffering)
m_TransformQueue.clear();
this->m_TransformQueue.push_back(dynamic_cast<igtl::TransformMessage*>(msg.GetPointer()));
infolog << "TRANSFORM";
}
else if (dynamic_cast<igtl::StringMessage*>(msg.GetPointer()) != nullptr)
{
if (this->m_BufferingType == IGTLMessageQueue::NoBuffering)
m_StringQueue.clear();
this->m_StringQueue.push_back(dynamic_cast<igtl::StringMessage*>(msg.GetPointer()));
infolog << "STRING";
}
else if (dynamic_cast<igtl::ImageMessage*>(msg.GetPointer()) != nullptr)
{
igtl::ImageMessage::Pointer imageMsg = dynamic_cast<igtl::ImageMessage*>(msg.GetPointer());
int* dim = new int[3];
imageMsg->GetDimensions(dim);
if (dim[2] > 1)
{
if (this->m_BufferingType == IGTLMessageQueue::NoBuffering)
m_Image3dQueue.clear();
this->m_Image3dQueue.push_back(dynamic_cast<igtl::ImageMessage*>(msg.GetPointer()));
infolog << "IMAGE3D";
}
else
{
if (this->m_BufferingType == IGTLMessageQueue::NoBuffering)
m_Image2dQueue.clear();
this->m_Image2dQueue.push_back(dynamic_cast<igtl::ImageMessage*>(msg.GetPointer()));
infolog << "IMAGE2D";
}
}
else
{
if (this->m_BufferingType == IGTLMessageQueue::NoBuffering)
m_MiscQueue.clear();
this->m_MiscQueue.push_back(msg);
infolog << "OTHER";
}
m_Latest_Message = msg;
//MITK_INFO << infolog.str();
this->m_Mutex->Unlock();
}
mitk::IGTLMessage::IGTLMessage(igtl::MessageBase::Pointer message)
{
this->SetMessage(message);
this->SetName(message->GetDeviceName());
}