C++ (Cpp) Cortex_FreeFrame Examples

Example #1

File: cortex_bridge.cpp Project: unr-arl/cortex_ros_bridge

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

Example #2

File: cortex.cpp Project: riscmaster/risc_maap

/** 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;
}