Natnet and server version can now be set in config file

Added a way to set both natnet and server version from yaml file, so you don't have to restart the broadcast each time you launch the mocap node. Add "version: [3,0,0,0]" in the optitrack_config-part of the config file

include/mocap_optitrack/mocap_config.h

@@ -52,6 +52,7 @@ struct ServerDescription
   int commandPort;
   int dataPort;
   std::string multicastIpAddress;
+  std::vector<int> version;
 };
 
 /// \brief ROS publisher configuration

src/mocap_config.cpp

@@ -74,6 +74,7 @@ namespace rosparam
     const std::string MulticastIpAddress = "optitrack_config/multicast_address";
     const std::string CommandPort = "optitrack_config/command_port";
     const std::string DataPort = "optitrack_config/data_port";
+    const std::string Version = "optitrack_config/version";
     const std::string RigidBodies = "rigid_bodies";
     const std::string PoseTopicName = "pose";
     const std::string Pose2dTopicName = "pose2d";
@@ -124,6 +125,15 @@ void NodeConfiguration::fromRosParam(
       serverDescription.dataPort);
   }
 
+  if (nh.hasParam(rosparam::keys::Version) )
+  {
+    nh.getParam(rosparam::keys::Version, serverDescription.version);
+  }
+  else
+  {
+    ROS_WARN_STREAM("Could not get server version, using auto");
+  }
+
   // Parse rigid bodies section
   if (nh.hasParam(rosparam::keys::RigidBodies))
   {

src/mocap_node.cpp

@@ -61,6 +61,11 @@ namespace mocap_optitrack
         new UdpMulticastSocket(serverDescription.dataPort, 
           serverDescription.multicastIpAddress)); 
 
+      if (!serverDescription.version.empty())
+      {
+        dataModel.setVersions(&serverDescription.version[0], &serverDescription.version[0]);
+      }
+
       // Need verion information from the server to properly decode any of their packets.
       // If we have not recieved that yet, send another request.  
       while(ros::ok() && !dataModel.hasServerInfo())