/////////////////////////////////////////////////////////////////////////////////
/////////////////////////// NEW FRAME CALLBACK ///////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
void newFrameCallback(sFrameOfData* FrameOfData)
{
// publish body transforms for all interesting bodies
std::tuple<bool,tf::StampedTransform> transform;
int fIndex;
for ( int i = 0; i < NumBodiesOfInterest; ++i )
{
fIndex = FindBodyFrameIndex ( FrameOfData, bodyOfInterest[i] );
if ( fIndex >= 0 )
{
transform = CreateTransform ( FrameOfData, fIndex, bodyOfInterest[i], bodyOriginOffset[i] );
// Only publish if we got valid data
if ( std::get<0>(transform) )
{
Cortex_broadcaster->sendTransform ( std::get<1>(transform) );
// send delay info
ros::Time cur = ros::Time::now();
ROS_INFO ( "frame delay - %f seconds", (cur - glob).toSec());
glob = cur;
}
else
ROS_WARN ( "Received out of range data." );
}
else
ROS_WARN ( "Body %s not found in frame %i", bodyOfInterest[i], FrameOfData->iFrame );
}
// publish markers
#ifdef PUBLISH_VISUALIZATION_MARKERS
visualization_msgs::MarkerArray marker_array;
marker_array = CreateMarkerArray_vis ( FrameOfData );
Cortex_markers.publish(marker_array);
#else
cortex_bridge::Markers marker_array;
marker_array = CreateMarkerArray_novis ( FrameOfData );
Cortex_markers.publish(marker_array);
#endif
// free data frame memory
Cortex_FreeFrame ( FrameOfData );
}
/** This function copies a frame of data.
*
* The Destination frame should start initialized to all zeros. The CopyFrame
* and FreeFrame functions will handle the memory allocations necessary to fill
* out the data.
*
* \param pSrc - The frame to copy FROM.
* \param pDst - The frame to copy TO
*
* \return RC_Okay, RC_MemoryError
*/
int Cortex_CopyFrame(const sFrameOfData* pSrc, sFrameOfData* pDst)
{
int iBody;
int nBodies = pSrc->nBodies;
const sBodyData* SrcBody;
sBodyData* DstBody;
int n;
void *ptr;
int size;
pDst->iFrame = pSrc->iFrame;
pDst->nBodies = nBodies;
for (iBody = 0; iBody < nBodies; iBody++) {
SrcBody = &pSrc->BodyData[iBody];
DstBody = &pDst->BodyData[iBody];
// Copy Markers
n = SrcBody->nMarkers;
size = n * sizeof(tMarkerData);
if (DstBody->nMarkers != n) {
ptr = realloc(DstBody->Markers, size);
if (size > 0 && ptr == NULL) {
Cortex_FreeFrame(pDst);
return RC_MemoryError;
}
DstBody->nMarkers = n;
DstBody->Markers = (tMarkerData*) ptr;
}
memcpy(DstBody->Markers, SrcBody->Markers, size);
// Copy Segments
n = SrcBody->nSegments;
size = n * sizeof(tSegmentData);
if (DstBody->nSegments != n) {
ptr = realloc(DstBody->Segments, size);
if (size > 0 && ptr == NULL) {
Cortex_FreeFrame(pDst);
return RC_MemoryError;
}
DstBody->nSegments = n;
DstBody->Segments = (tSegmentData*) ptr;
}
memcpy(DstBody->Segments, SrcBody->Segments, size);
// Copy DOFs
n = SrcBody->nDofs;
size = n * sizeof(tDofData);
if (DstBody->nDofs != n) {
ptr = realloc(DstBody->Dofs, size);
if (size > 0 && ptr == NULL) {
Cortex_FreeFrame(pDst);
return RC_MemoryError;
}
DstBody->nDofs = n;
DstBody->Dofs = (tDofData*) ptr;
}
memcpy(DstBody->Dofs, SrcBody->Dofs, size);
// Copy extra items
DstBody->fAvgDofResidual = SrcBody->fAvgDofResidual;
DstBody->nIterations = SrcBody->nIterations;
DstBody->ZoomEncoderValue = SrcBody->ZoomEncoderValue;
DstBody->FocusEncoderValue = SrcBody->FocusEncoderValue;
}
// Copy Unidentified Markers
n = pSrc->nUnidentifiedMarkers;
size = n * sizeof(tMarkerData);
if (pDst->nUnidentifiedMarkers != n) {
ptr = realloc(pDst->UnidentifiedMarkers, size);
if (size > 0 && ptr == NULL) {
Cortex_FreeFrame(pDst);
return RC_MemoryError;
}
pDst->nUnidentifiedMarkers = n;
pDst->UnidentifiedMarkers = (tMarkerData*) ptr;
}
memcpy(pDst->UnidentifiedMarkers, pSrc->UnidentifiedMarkers, size);
// Copy Analog
const sAnalogData* SrcAnalog = &pSrc->AnalogData;
sAnalogData* DstAnalog = &pDst->AnalogData;
// Analog Channels
int nChannels = SrcAnalog->nAnalogChannels;
int nSamples = SrcAnalog->nAnalogSamples;
size = nChannels * nSamples * sizeof(short);
if (DstAnalog->nAnalogChannels != nChannels || DstAnalog->nAnalogSamples
!= nSamples) {
ptr = realloc(DstAnalog->AnalogSamples, size);
if (size > 0 && ptr == NULL) {
Cortex_FreeFrame(pDst);
return RC_MemoryError;
}
DstAnalog->nAnalogChannels = nChannels;
DstAnalog->nAnalogSamples = nSamples;
DstAnalog->AnalogSamples = (short*) ptr;
}
memcpy(DstAnalog->AnalogSamples, SrcAnalog->AnalogSamples, size);
// Forces Data
int nForcePlates = SrcAnalog->nForcePlates;
int nForceSamples = SrcAnalog->nForceSamples;
size = nForcePlates * nForceSamples * sizeof(tForceData);
if (DstAnalog->nForcePlates != nForcePlates || DstAnalog->nForceSamples
!= nForceSamples) {
ptr = realloc(DstAnalog->Forces, size);
if (size > 0 && ptr == NULL) {
Cortex_FreeFrame(pDst);
return RC_MemoryError;
}
DstAnalog->nForcePlates = nForcePlates;
DstAnalog->nForceSamples = nForceSamples;
DstAnalog->Forces = (tForceData*) ptr;
}
memcpy(DstAnalog->Forces, SrcAnalog->Forces, size);
int nAngleEncoders = SrcAnalog->nAngleEncoders;
int nAngleEncoderSamples = SrcAnalog->nAngleEncoderSamples;
size = nAngleEncoders * nAngleEncoderSamples * sizeof(double);
if (DstAnalog->nAngleEncoders != nAngleEncoders
|| DstAnalog->nAngleEncoderSamples != nAngleEncoderSamples) {
ptr = realloc(DstAnalog->AngleEncoderSamples, size);
if (size > 0 && ptr == NULL) {
Cortex_FreeFrame(pDst);
return RC_MemoryError;
}
DstAnalog->nAngleEncoders = nAngleEncoders;
DstAnalog->nAngleEncoderSamples = nAngleEncoderSamples;
DstAnalog->AngleEncoderSamples = (double*) ptr;
}
memcpy(DstAnalog->AngleEncoderSamples, SrcAnalog->AngleEncoderSamples, size);
return RC_Okay;
}