mocap_datapackets.cpp
Go to the documentation of this file.
00001 #include "mocap_optitrack/mocap_datapackets.h"
00002 
00003 #include <stdio.h>
00004 #include <string>
00005 #include <iostream>
00006 #include <ros/console.h>
00007 using namespace std;
00008 
00009 RigidBody::RigidBody() 
00010   : NumberOfMarkers(0), marker(0)
00011 {
00012 }
00013 
00014 RigidBody::~RigidBody()
00015 {
00016   delete[] marker;
00017 }
00018 
00019 const geometry_msgs::Pose RigidBody::get_ros_pose()
00020 {
00021   geometry_msgs::Pose ros_pose;
00022 
00023   // y & z axes are swapped in the Optitrack coordinate system
00024   ros_pose.position.x = pose.position.x;
00025   ros_pose.position.y = -pose.position.z;
00026   ros_pose.position.z = pose.position.y;
00027 
00028   ros_pose.orientation.x = pose.orientation.x;
00029   ros_pose.orientation.y = -pose.orientation.z;
00030   ros_pose.orientation.z = pose.orientation.y;
00031   ros_pose.orientation.w = pose.orientation.w;
00032 
00033   return ros_pose;
00034 }
00035 
00036 bool RigidBody::has_data()
00037 {
00038     static const char zero[sizeof(pose)] = { 0 };
00039     return memcmp(zero, (char*) &pose, sizeof(pose));
00040 }
00041 
00042 ModelDescription::ModelDescription()
00043   : numMarkers(0), markerNames(0)
00044 {
00045 }
00046 
00047 ModelDescription::~ModelDescription()
00048 {
00049   delete[] markerNames;
00050 }
00051 
00052 ModelFrame::ModelFrame()
00053   : markerSets(0), otherMarkers(0), rigidBodies(0), 
00054     numMarkerSets(0), numOtherMarkers(0), numRigidBodies(0),
00055     latency(0.0)
00056 {
00057 }
00058 
00059 ModelFrame::~ModelFrame()
00060 {
00061   delete[] markerSets;
00062   delete[] otherMarkers;
00063   delete[] rigidBodies;
00064 }
00065 
00066 MoCapDataFormat::MoCapDataFormat(const char *packet, unsigned short length) 
00067   : packet(packet), length(length), frameNumber(0)
00068 {
00069 }
00070 
00071 MoCapDataFormat::~MoCapDataFormat()
00072 {
00073 }
00074 
00075 void MoCapDataFormat::seek(size_t count)
00076 {
00077   packet += count;
00078   length -= count;
00079 }
00080 
00081 void MoCapDataFormat::parse()
00082 {
00083   seek(4);
00084 
00085   // parse frame number
00086   read_and_seek(frameNumber);
00087 
00088   // count number of packetsets
00089   read_and_seek(model.numMarkerSets);
00090   model.markerSets = new MarkerSet[model.numMarkerSets];
00091   ROS_DEBUG("Number of marker sets: %d\n", model.numMarkerSets);
00092 
00093   for (int i = 0; i < model.numMarkerSets; i++)
00094   {
00095     strcpy(model.markerSets[i].name, packet);
00096     seek(strlen(model.markerSets[i].name) + 1);
00097 
00098     ROS_DEBUG("Parsing marker set named: %s\n", model.markerSets[i].name);
00099 
00100     // read number of markers that belong to the model
00101     read_and_seek(model.markerSets[i].numMarkers);
00102     ROS_DEBUG("Number of markers in set: %d\n", model.markerSets[i].numMarkers);
00103 
00104     model.markerSets[i].markers = new Marker[model.markerSets[i].numMarkers];
00105     for (int k = 0; k < model.markerSets[i].numMarkers; k++)
00106     {
00107       // read marker positions
00108       read_and_seek(model.markerSets[i].markers[k]);
00109     }
00110   }
00111 
00112   // read number of 'other' markers (cf. NatNet specs)
00113   read_and_seek(model.numOtherMarkers);
00114   model.otherMarkers = new Marker[model.numOtherMarkers];
00115   ROS_DEBUG("Number of markers not in sets: %d\n", model.numOtherMarkers);
00116   for (int l = 0; l < model.numOtherMarkers; l++)
00117   {
00118     // read positions of 'other' markers
00119     read_and_seek(model.otherMarkers[l]);
00120   }
00121 
00122   // read number of rigid bodies of the model
00123   read_and_seek(model.numRigidBodies);
00124   ROS_DEBUG("Number of rigid bodies: %d\n", model.numRigidBodies);
00125 
00126   model.rigidBodies = new RigidBody[model.numRigidBodies];
00127   for (int m = 0; m < model.numRigidBodies; m++)
00128   {
00129     // read id, position and orientation of each rigid body
00130     read_and_seek(model.rigidBodies[m].ID);
00131     read_and_seek(model.rigidBodies[m].pose);
00132 
00133     // get number of markers per rigid body
00134     read_and_seek(model.rigidBodies[m].NumberOfMarkers);
00135     ROS_DEBUG("Rigid body ID: %d\n", model.rigidBodies[m].ID);
00136     ROS_DEBUG("Number of rigid body markers: %d\n", model.rigidBodies[m].NumberOfMarkers);
00137     if (model.rigidBodies[m].NumberOfMarkers > 0)
00138     {
00139       model.rigidBodies[m].marker = new Marker [model.rigidBodies[m].NumberOfMarkers];
00140       size_t byte_count = model.rigidBodies[m].NumberOfMarkers * sizeof(Marker);
00141       memcpy(model.rigidBodies[m].marker, packet, byte_count);
00142       seek(byte_count);
00143 
00144       // skip marker IDs
00145       byte_count = model.rigidBodies[m].NumberOfMarkers * sizeof(int);
00146       seek(byte_count);
00147 
00148       // skip marker sizes
00149       byte_count = model.rigidBodies[m].NumberOfMarkers * sizeof(float);
00150       seek(byte_count);
00151     }
00152 
00153     // skip mean marker error
00154     seek(sizeof(float));
00155   }
00156 
00157   // TODO: read skeletons
00158   int numSkeletons = 0;
00159   read_and_seek(numSkeletons);
00160 
00161   // get latency
00162   read_and_seek(model.latency);
00163 }
00164 
00165 


mocap_optitrack
Author(s): Kathrin Gräve/graeve@ais.uni-bonn.de, Alex Bencz/abencz@clearpathrobotics.com
autogenerated on Mon Oct 6 2014 02:22:13