mocap_datapackets.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, University of Bonn, Computer Science Institute VI
00005  *  Author: Kathrin Gräve, 01/2011
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of University of Bonn, Computer Science Institute
00019  *     VI nor the names of its contributors may be used to endorse or
00020  *     promote products derived from this software without specific
00021  *     prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 
00039 
00040 #ifndef __MOCAP_DATAPACKETS_H__
00041 #define __MOCAP_DATAPACKETS_H__
00042 
00043 #include <sys/types.h>
00044 #include <iostream>
00045 #include <string>
00046 #include <geometry_msgs/Pose.h>
00047 
00048 using namespace std;
00049 
00051 class Marker
00052 {
00053   public:
00054     float positionX;
00055     float positionY;
00056     float positionZ;
00057 };
00058 
00059 class Pose
00060 {
00061   public:
00062     struct {
00063       float x;
00064       float y;
00065       float z;
00066     } position;
00067     struct {
00068       float x;
00069       float y;
00070       float z;
00071       float w;
00072     } orientation;
00073 };
00074 
00076 class RigidBody
00077 {
00078   public:
00079     RigidBody();
00080     ~RigidBody();
00081 
00082     int ID;
00083     
00084     Pose pose; 
00085 
00086     int NumberOfMarkers;
00087     Marker *marker;
00088 
00089     const geometry_msgs::Pose get_ros_pose();
00090     bool has_data();
00091 };
00092 
00094 class ModelDescription
00095 {
00096   public:
00097     ModelDescription();
00098     ~ModelDescription();
00099 
00100     string name;
00101     int numMarkers;
00102     string *markerNames;
00103 };
00104 
00105 class MarkerSet
00106 {
00107   public:
00108     MarkerSet() : numMarkers(0), markers(0) {}
00109     ~MarkerSet() { delete[] markers; }
00110     char name[256];
00111     int numMarkers;
00112     Marker *markers;
00113 };
00114 
00116 class ModelFrame
00117 {
00118   public:
00119     ModelFrame();
00120     ~ModelFrame();
00121 
00122     MarkerSet *markerSets;
00123     Marker *otherMarkers;
00124     RigidBody *rigidBodies;
00125 
00126     int numMarkerSets;
00127     int numOtherMarkers;
00128     int numRigidBodies;
00129 
00130     float latency;
00131 };
00132 
00134 class MoCapDataFormat
00135 {
00136   public:
00137     MoCapDataFormat(const char *packet, unsigned short length);
00138     ~MoCapDataFormat();
00139 
00141     void parse ();
00142 
00143 
00144     const char *packet;
00145     unsigned short length;
00146 
00147     int frameNumber;
00148     ModelFrame model;
00149 
00150   private:
00151     void seek(size_t count);
00152     template <typename T> void read_and_seek(T& target)
00153     {
00154         target = *((T*) packet);
00155         seek(sizeof(T));
00156     }
00157 };
00158 
00159 #endif  /*__MOCAP_DATAPACKETS_H__*/


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