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()); }