mocap_node.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 // Local includes
00010 #include "mocap_optitrack/socket.h"
00011 #include "mocap_optitrack/mocap_datapackets.h"
00012 #include "mocap_optitrack/mocap_config.h"
00013 #include "mocap_optitrack/skeletons.h"
00014 
00015 // ROS includes
00016 #include <ros/ros.h>
00017 #include <tf/transform_datatypes.h>
00018 #include <tf/transform_broadcaster.h>
00019 #include <geometry_msgs/Pose2D.h>
00020 
00021 // System includes
00022 #include <string>
00023 #include <unistd.h>
00024 
00026 // Constants
00027 
00028 // ip on multicast group - cannot be changed in Arena
00029 const std::string MULTICAST_IP = "224.0.0.1";
00030 
00031 const std::string MOCAP_MODEL_KEY = "mocap_model";
00032 const std::string RIGID_BODIES_KEY = "rigid_bodies";
00033 const char ** DEFAULT_MOCAP_MODEL = SKELETON_WITHOUT_TOES;
00034 
00035 const int LOCAL_PORT = 1511;
00036 
00038 
00039 void processMocapData( const char** mocap_model, RigidBodyMap& published_rigid_bodies)
00040 {
00041   UdpMulticastSocket multicast_client_socket( LOCAL_PORT, MULTICAST_IP );
00042 
00043   ushort payload;
00044   int numberOfPackets = 0;
00045   while(ros::ok())
00046   {
00047     bool packetread = false;
00048     int numBytes = 0;
00049 
00050     do
00051     {
00052       // Receive data from mocap device
00053       numBytes = multicast_client_socket.recv();
00054 
00055       // Parse mocap data
00056       if( numBytes > 0 )
00057       {
00058         const char* buffer = multicast_client_socket.getBuffer();
00059         unsigned short header = *((unsigned short*)(&buffer[0]));
00060 
00061         // Look for the beginning of a NatNet package
00062         if (header == 7)
00063         {
00064           payload = *((ushort*) &buffer[2]);
00065           MoCapDataFormat format(buffer, payload);
00066           format.parse();
00067           packetread = true;
00068           numberOfPackets++;
00069 
00070           if( format.model.numRigidBodies > 0 )
00071           {
00072             for( int i = 0; i < format.model.numRigidBodies; i++ )
00073             {
00074               int ID = format.model.rigidBodies[i].ID;
00075               RigidBodyMap::iterator item = published_rigid_bodies.find(ID);
00076 
00077               if (item != published_rigid_bodies.end())
00078               {
00079                   item->second.publish(format.model.rigidBodies[i]);
00080               }
00081             }
00082           }
00083         }
00084         // else skip packet
00085       }
00086     } while( numBytes > 0 );
00087 
00088     // Don't try again immediately
00089     if( !packetread )
00090     {
00091       usleep( 10 );
00092     }
00093   }
00094 }
00095 
00096 
00097 
00099 
00100 int main( int argc, char* argv[] )
00101 { 
00102   
00103   // Initialize ROS node
00104   ros::init(argc, argv, "mocap_node");
00105   ros::NodeHandle n("~");
00106 
00107   // Get configuration from ROS parameter server  
00108   const char** mocap_model( DEFAULT_MOCAP_MODEL );
00109   if( n.hasParam( MOCAP_MODEL_KEY ) )
00110   {    std::string tmp;
00111     if( n.getParam( MOCAP_MODEL_KEY, tmp ) )
00112     {
00113       if( tmp == "SKELETON_WITH_TOES" )
00114         mocap_model = SKELETON_WITH_TOES;
00115       else if( tmp == "SKELETON_WITHOUT_TOES" )
00116         mocap_model = SKELETON_WITHOUT_TOES;
00117       else if( tmp == "OBJECT" )
00118         mocap_model = OBJECT;
00119     }
00120   }
00121 
00122   RigidBodyMap published_rigid_bodies;
00123 
00124   if (n.hasParam(RIGID_BODIES_KEY))
00125   {
00126       XmlRpc::XmlRpcValue body_list;
00127       n.getParam("rigid_bodies", body_list);
00128       if (body_list.getType() == XmlRpc::XmlRpcValue::TypeStruct && body_list.size() > 0)
00129       {
00130           XmlRpc::XmlRpcValue::iterator i;
00131           for (i = body_list.begin(); i != body_list.end(); ++i) {
00132               if (i->second.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
00133                   PublishedRigidBody body(i->second);
00134                   string id = (string&) (i->first);
00135                   RigidBodyItem item(atoi(id.c_str()), body);
00136 
00137                   std::pair<RigidBodyMap::iterator, bool> result = published_rigid_bodies.insert(item);
00138                   if (!result.second)
00139                   {
00140                       ROS_ERROR("Could not insert configuration for rigid body ID %s", id.c_str());
00141                   }
00142               }
00143           }
00144       }
00145   }
00146 
00147   // Process mocap data until SIGINT
00148   processMocapData(mocap_model, published_rigid_bodies);
00149 
00150   return 0;
00151 }


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