├── include ├── NatNetRepeater.h ├── LinearKalmanFilter.h ├── NatNetWrapper.h ├── NatNetRequests.h ├── NatNetClient.h ├── NatNetCAPI.h └── NatNetTypes.h ├── lib └── libNatNetLibShared.so ├── .idea ├── optitrack_bridge.iml ├── misc.xml ├── encodings.xml ├── modules.xml ├── codeStyles │ └── Project.xml └── workspace.xml ├── src ├── optitrack_bridge_node.cpp ├── LinearKalmanFilter.cpp ├── SampleClient.cpp └── NatNetWrapper.cpp ├── launch └── optitrack.launch ├── CMakeLists.txt ├── README.md └── package.xml /include/NatNetRepeater.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/optitrack_bridge/HEAD/include/NatNetRepeater.h -------------------------------------------------------------------------------- /lib/libNatNetLibShared.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/qwerty35/optitrack_bridge/HEAD/lib/libNatNetLibShared.so -------------------------------------------------------------------------------- /.idea/optitrack_bridge.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /src/optitrack_bridge_node.cpp: -------------------------------------------------------------------------------- 1 | #include "NatNetWrapper.h" 2 | 3 | int main( int argc, char* argv[] ) 4 | { 5 | ros::init(argc, argv, "optitrack_bridge_node"); 6 | NatNetWrapper client; 7 | client.run(); 8 | } -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | -------------------------------------------------------------------------------- /.idea/encodings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /launch/optitrack.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(optitrack_bridge) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | find_package(Eigen3 REQUIRED) 9 | include_directories(${EIGEN3_INCLUDE_DIR}) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | std_msgs 14 | tf 15 | ) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS include 19 | # LIBRARIES optitrack_bridge 20 | CATKIN_DEPENDS roscpp std_msgs tf 21 | # DEPENDS system_lib 22 | ) 23 | 24 | ########### 25 | ## Build ## 26 | ########### 27 | 28 | ## Specify additional locations of header files 29 | ## Your package locations should be listed before other locations 30 | include_directories( 31 | ${PROJECT_SOURCE_DIR}/include 32 | ${catkin_INCLUDE_DIRS} 33 | ) 34 | 35 | ## Declare a C++ executable 36 | add_executable(SampleClient src/SampleClient.cpp) 37 | target_link_libraries(SampleClient 38 | ${PROJECT_SOURCE_DIR}/lib/libNatNetLibShared.so 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | add_executable(optitrack_bridge_node 43 | src/optitrack_bridge_node.cpp 44 | src/NatNetWrapper.cpp 45 | src/LinearKalmanFilter.cpp 46 | ) 47 | target_link_libraries(optitrack_bridge_node 48 | ${PROJECT_SOURCE_DIR}/lib/libNatNetLibShared.so 49 | ${catkin_LIBRARIES} 50 | ) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # optitrack_bridge 2 | 3 | This package converts Optitrack rigidBody message to ROS1 pose/odometry/tf message. 4 | 5 | Tested in 6 | - Ubuntu 20.04, ROS Noetic 7 | - Ubuntu 18.04, ROS Melodic 8 | - Ubuntu 16.04, ROS Kinetic 9 | 10 | 11 | 1.Prerequisite 12 | ------ 13 | This code is for below environment. 14 | - C++14 15 | - Motive 2.2 or 2.3 16 | - NatNet 3.1 17 | 18 | 2.Installation 19 | ------ 20 | cd ~/catkin_ws/src 21 | 22 | git clone https://github.com/qwerty35/optitrack_bridge.git 23 | 24 | cd .. && catkin_make 25 | 26 | It assumes that your work space is in `~/catkin_ws/src` 27 | 28 | 3.Usage 29 | ------ 30 | roslaunch optitrack_bridge optitrack.launch 31 | 32 | 33 | 4.Parameters 34 | ----- 35 | `frame_id`: Set frame id of message. 36 | 37 | `message_type`: Ros message type. 38 | 39 | + pose - It returns the object's pose as geometry::poseStamped message. 40 | + odometry - It returns object's pose+twist(velocity) as nav_msgs::odometry message. The twist(velocity) of the object is computed by linear Kalman filter. (Error covariance is not supported yet.) 41 | + tf - It returns the object's pose as tf message. 42 | 43 | `show_latency`: Print latency on the screen. 44 | 45 | `publish_labeled_marker_pose_array`: If true, publish pose of object's markers as geometry::poseArray 46 | 47 | `publish_unlabeled_marker_pose_array`: If true, publish unlabeled markers as geometry::poseArray 48 | 49 | (If labeled or unlabeled markers are not published, check Motive -> Streaming Pane -> Labeled or Unlabeld Markers) 50 | -------------------------------------------------------------------------------- /.idea/codeStyles/Project.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 15 | 16 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /include/LinearKalmanFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef OPTITRACK_BRIDGE_LINEARKALMANFILTER_H 2 | #define OPTITRACK_BRIDGE_LINEARKALMANFILTER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define PUBLISH_RAW_TWIST false 12 | 13 | #define Row 6 14 | #define Col 3 15 | 16 | using namespace Eigen; 17 | typedef Matrix FMatrix; 18 | typedef Matrix GMatrix; 19 | typedef Matrix QMatrix; 20 | typedef Matrix PMatrix; 21 | typedef Matrix RMatrix; 22 | typedef Matrix HMatrix; 23 | typedef Matrix NVector; 24 | typedef Matrix MVector; 25 | 26 | class LinearKalmanFilter { 27 | public: 28 | LinearKalmanFilter(); 29 | nav_msgs::Odometry pose_cb(const geometry_msgs::PoseStamped& msg); 30 | 31 | private: 32 | FMatrix F; 33 | GMatrix G; 34 | QMatrix Q; 35 | // RMatrix R; 36 | HMatrix H; 37 | 38 | NVector x_old; 39 | NVector x_predict; 40 | NVector x_estimate; 41 | PMatrix P_old; 42 | PMatrix P_predict; 43 | PMatrix P_estimate; 44 | 45 | MVector sigma_Q; 46 | MVector sigma_R; 47 | 48 | geometry_msgs::PoseStamped pose_old, pose_new; 49 | geometry_msgs::TwistStamped twist, twist_raw; 50 | nav_msgs::Odometry odom; 51 | 52 | ros::WallTime t_old, t_new; 53 | bool initialized = false; 54 | 55 | void predict(const double &dt); 56 | static FMatrix computeF(const double &dt); 57 | static GMatrix computeG(const double &dt); 58 | static QMatrix computeQ(const GMatrix &G, const MVector &sigma_Q); 59 | void update(const double &dt, const geometry_msgs::PoseStamped& msg); 60 | RMatrix computeR(); 61 | }; 62 | 63 | 64 | #endif //OPTITRACK_BRIDGE_LINEARKALMANFILTER_H 65 | -------------------------------------------------------------------------------- /include/NatNetWrapper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #ifdef _WIN32 13 | # include 14 | #else 15 | # include 16 | # include 17 | #endif 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | enum MessageType{ 28 | POSE, 29 | ODOMETRY, 30 | TF 31 | }; 32 | 33 | class NatNetWrapper { 34 | public: 35 | NatNetWrapper(); 36 | int run(); 37 | 38 | void NATNET_CALLCONV ServerDiscoveredCallback( const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext ); 39 | void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData); // receives data from the server 40 | void NATNET_CALLCONV MessageHandler(Verbosity msgType, const char* msg); // receives NatNet error messages 41 | 42 | private: 43 | ros::NodeHandle nh; 44 | tf::TransformBroadcaster br; 45 | std::vector pubs_vision_pose; 46 | std::vector pubs_vision_odom; 47 | std::vector pubs_labeled_marker_pose_array; 48 | ros::Publisher pub_unlabeled_marker_pose_array; 49 | std::vector> linearKalmanFilters; 50 | std::vector model_ids; 51 | std::vector model_names; 52 | std::string prefix; 53 | std::string frame_id; 54 | MessageType message_type; 55 | int verbose_level; 56 | bool show_latency = false; 57 | bool publish_labeled_marker_pose_array = false; 58 | bool publish_unlabeled_marker_pose_array = false; 59 | bool is_ServerDiscovered = false; 60 | 61 | static void sigintCallback(int signum); 62 | void resetClient(); 63 | int ConnectClient(); 64 | geometry_msgs::PoseStamped rigidBodyToPose(const sRigidBodyData& rigid_body_data); 65 | void publishPose(int idx, const sRigidBodyData& rigid_body_data); 66 | void publishOdom(int idx, const sRigidBodyData& rigid_body_data); 67 | void publishTF(int idx, const sRigidBodyData& rigid_body_data); 68 | 69 | static const ConnectionType kDefaultConnectionType = ConnectionType_Multicast; 70 | 71 | char getch(); 72 | NatNetClient* g_pClient = NULL; 73 | 74 | std::vector< sNatNetDiscoveredServer > g_discoveredServers; 75 | sNatNetClientConnectParams g_connectParams; 76 | char g_discoveredMulticastGroupAddr[kNatNetIpv4AddrStrLenMax] = NATNET_DEFAULT_MULTICAST_ADDRESS; 77 | int g_analogSamplesPerMocapFrame = 0; 78 | sServerDescription g_serverDescription; 79 | }; 80 | 81 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | optitrack_bridge 4 | 0.0.0 5 | The optitrack_bridge package 6 | 7 | 8 | 9 | 10 | jungwon 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | tf 55 | roscpp 56 | std_msgs 57 | tf 58 | roscpp 59 | std_msgs 60 | tf 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /include/NatNetRequests.h: -------------------------------------------------------------------------------- 1 | //=============================================================================---- 2 | // Copyright © 2016 NaturalPoint, Inc. All Rights Reserved. 3 | // 4 | // This software is provided by the copyright holders and contributors "as is" and 5 | // any express or implied warranties, including, but not limited to, the implied 6 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 7 | // In no event shall NaturalPoint, Inc. or contributors be liable for any direct, 8 | // indirect, incidental, special, exemplary, or consequential damages 9 | // (including, but not limited to, procurement of substitute goods or services; 10 | // loss of use, data, or profits; or business interruption) however caused 11 | // and on any theory of liability, whether in contract, strict liability, 12 | // or tort (including negligence or otherwise) arising in any way out of 13 | // the use of this software, even if advised of the possibility of such damage. 14 | //=============================================================================---- 15 | 16 | #pragma once 17 | 18 | 19 | // Parameters: (none) 20 | // Returns: float32_t value representing current system's unit scale in terms of millimeters. 21 | #define NATNET_REQUEST_GETUNITSTOMILLIMETERS "UnitsToMillimeters" 22 | 23 | // Parameters: (none) 24 | // Returns: float32_t value representing current system's tracking frame rate in frames per second. 25 | #define NATNET_REQUEST_GETFRAMERATE "FrameRate" 26 | 27 | // Parameters: (none) 28 | // Returns: int32_t value representing number of analog samples per mocap frame of data. 29 | #define NATNET_REQUEST_GETANALOGSAMPLESPERMOCAPFRAME "AnalogSamplesPerMocapFrame" 30 | 31 | // Parameters: (none) 32 | // Returns: int32_t value representing length of current take in frames. 33 | #define NATNET_REQUEST_GETCURRENTTAKELENGTH "CurrentTakeLength" 34 | 35 | // Parameters: (none) 36 | // Returns: int32_t value representing the current mode. (0 = Live, 1 = Recording, 2 = Edit) 37 | #define NATNET_REQUEST_GETCURRENTMODE "CurrentMode" 38 | 39 | // Parameters: (none) 40 | // Returns: (none) 41 | #define NATNET_REQUEST_STARTRECORDING "StartRecording" 42 | 43 | // Parameters: (none) 44 | // Returns: (none) 45 | #define NATNET_REQUEST_STOPRECORDING "StopRecording" 46 | 47 | // Parameters: (none) 48 | // Returns: (none) 49 | #define NATNET_REQUEST_SWITCHTOLIVEMODE "LiveMode" 50 | 51 | // Parameters: (none) 52 | // Returns: (none) 53 | #define NATNET_REQUEST_SWITCHTOEDITMODE "EditMode" 54 | 55 | // Parameters: (none) 56 | // Returns: (none) 57 | #define NATNET_REQUEST_TIMELINEPLAY "TimelinePlay" 58 | 59 | // Parameters: (none) 60 | // Returns: (none) 61 | #define NATNET_REQUEST_TIMELINESTOP "TimelineStop" 62 | 63 | // Parameters: New playback take name. 64 | // Returns: (none) 65 | #define NATNET_REQUEST_SETPLAYBACKTAKENAME "SetPlaybackTakeName" 66 | 67 | // Parameters: New record take name. 68 | // Returns: (none) 69 | #define NATNET_REQUEST_SETRECORDTAKENAME "SetRecordTakeName" 70 | 71 | // Parameters: Session name to either switch to or to create. 72 | // Returns: (none) 73 | #define NATNET_REQUEST_SETCURRENTSESSION "SetCurrentSession" 74 | 75 | // Parameters: The new start frame for the playback range. 76 | // Returns: (none) 77 | #define NATNET_REQUEST_SETPLAYBACKSTARTFRAME "SetPlaybackStartFrame" 78 | 79 | // Parameters: The new end frame for the playback range. 80 | // Returns: (none) 81 | #define NATNET_REQUEST_SETPLAYBACKSTOPFRAME "SetPlaybackStopFrame" 82 | 83 | // Parameters: The new current frame for playback. 84 | // Returns: (none) 85 | #define NATNET_REQUEST_SETPLAYBACKCURRENTFRAME "SetPlaybackCurrentFrame" 86 | 87 | // Parameters: Node (asset) name 88 | // Returns: int32_t value indicating whether the command successfully enabled the asset. 0 if succeeded. 89 | // (e.g. string command = "EnableAsset,Rigid Body 1") 90 | #define NATNET_REQUEST_ENABLEASSET "EnableAsset" 91 | 92 | // Parameters: Node (asset) name 93 | // Returns: int32_t value indicating whether the command successfully disabled the asset. 0 if succeeded. 94 | #define NATNET_REQUEST_DISABLEASSET "DisableAsset" 95 | 96 | // Parameters: Node name (if applicable leave it empty if not) and property name. 97 | // Returns: string value representing corresponding property settings 98 | #define NATNET_REQUEST_GETPROPERTY "GetProperty" 99 | 100 | // Parameters: Node name (if applicable leave it empty if not), property name, and desired value. 101 | // Returns: int32_t value indicating whether the command successfully updated the data. 0 if succeeded. 102 | // (e.g. string command = "SetProperty,,Unlabeled Markers,False") 103 | #define NATNET_REQUEST_SETPROPETRY "SetProperty" 104 | 105 | -------------------------------------------------------------------------------- /include/NatNetClient.h: -------------------------------------------------------------------------------- 1 | //=============================================================================---- 2 | // Copyright © 2016 NaturalPoint, Inc. All Rights Reserved. 3 | // 4 | // This software is provided by the copyright holders and contributors "as is" and 5 | // any express or implied warranties, including, but not limited to, the implied 6 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 7 | // In no event shall NaturalPoint, Inc. or contributors be liable for any direct, 8 | // indirect, incidental, special, exemplary, or consequential damages 9 | // (including, but not limited to, procurement of substitute goods or services; 10 | // loss of use, data, or profits; or business interruption) however caused 11 | // and on any theory of liability, whether in contract, strict liability, 12 | // or tort (including negligence or otherwise) arising in any way out of 13 | // the use of this software, even if advised of the possibility of such damage. 14 | //=============================================================================---- 15 | 16 | #pragma once 17 | 18 | #include "NatNetTypes.h" 19 | 20 | 21 | // NatNetClient is a complete C++ class for connecting to NatNet server 22 | // applications such as OptiTrack Motive. 23 | class NATNET_API NatNetClient 24 | { 25 | public: 26 | NatNetClient(); 27 | ~NatNetClient(); 28 | 29 | ErrorCode Connect( const sNatNetClientConnectParams& connectParams ); 30 | ErrorCode Disconnect(); 31 | 32 | ErrorCode SetFrameReceivedCallback( NatNetFrameReceivedCallback pfnDataCallback, void* pUserContext = NULL ); 33 | ErrorCode SetUnknownMessageCallback( NatNetUnknownMessageCallback pfnMsgCallback, void* pUserContext = NULL ); 34 | 35 | ErrorCode SendMessageAndWait( const char* szRequest, void** ppServerResponse, int* pResponseSize ); 36 | ErrorCode SendMessageAndWait( const char* szRequest, int tries, int timeout, void** ppServerResponse, int* pResponseSize ); 37 | 38 | ErrorCode GetServerDescription( sServerDescription* pServerDescription ); 39 | ErrorCode GetDataDescriptionList( sDataDescriptions** ppDataDescriptions ); 40 | 41 | void ValidateAuthenticationToken( const char* challengeToken, char* authToken); 42 | 43 | double SecondsSinceHostTimestamp( uint64_t hostTimestamp ) const; 44 | 45 | ////////////////////////////////////////////////////////////////////////// 46 | // Deprecated methods 47 | 48 | NATNET_DEPRECATED( "The constructor specifying connection type is deprecated; use the sNatNetClientConnectParams::connectionType field instead." ) 49 | explicit NatNetClient( int connectionType ); 50 | 51 | NATNET_DEPRECATED( "This method is deprecated; use Connect instead." ) 52 | ErrorCode Initialize( const char* szLocalAddress, const char* szServerAddress, int HostCommandPort = 0, int HostDataPort = 0 ); 53 | 54 | NATNET_DEPRECATED( "This method has been renamed; use Disconnect instead." ) 55 | ErrorCode Uninitialize() 56 | { 57 | return Disconnect(); 58 | } 59 | 60 | NATNET_DEPRECATED( "This method has been renamed; use SetFrameReceivedCallback instead." ) 61 | ErrorCode SetDataCallback( void (*CallbackFunction)(sFrameOfMocapData* pFrameOfData, void* pUserData), void* pUserData = NULL ) 62 | { 63 | return SetFrameReceivedCallback( CallbackFunction, pUserData ); 64 | } 65 | 66 | NATNET_DEPRECATED( "This method is deprecated; the log callback is installed globally, and should be set using the function NatNet_SetLogCallback from NatNetCAPI.h instead." ) 67 | int SetMessageCallback( void (*CallbackFunction)(int id, char* szTraceMessage) ); 68 | 69 | NATNET_DEPRECATED( "This method is deprecated; use GetDataDescriptionList, which returns an ErrorCode instead of the number of descriptions (which is still available as sDataDescriptions::nDataDescriptions)." ) 70 | int GetDataDescriptions( sDataDescriptions** ppDataDescriptions ); 71 | 72 | NATNET_DEPRECATED( "This functionality is no longer supported by Motive servers." ) 73 | sFrameOfMocapData* GetLastFrameOfData(); 74 | 75 | NATNET_DEPRECATED( "This method is deprecated; use the function NatNet_GetVersion from NatNetCAPI.h instead." ) 76 | void NatNetVersion( unsigned char Version[4] ); 77 | 78 | NATNET_DEPRECATED( "This method is obsolete and does nothing; implement filtering logic in your log callback handler instead." ) 79 | void SetVerbosityLevel( int iLevel ); 80 | 81 | NATNET_DEPRECATED( "This method is deprecated; use the function NatNet_DecodeTimecode from NatNetCAPI.h instead." ) 82 | bool DecodeTimecode( unsigned int timecode, unsigned int timecodeSubframe, int* pOutHour, int* pOutMinute, int* pOutSecond, int* pOutFrame, int* pOutSubframe ); 83 | 84 | NATNET_DEPRECATED( "This method is deprecated; use the function NatNet_TimecodeStringify from NatNetCAPI.h instead." ) 85 | bool TimecodeStringify( unsigned int timecode, unsigned int timecodeSubframe, char* outBuffer, int outBufferSize ); 86 | 87 | NATNET_DEPRECATED( "This method is deprecated; use the function NatNet_DecodeID from NatNetCAPI.h instead." ) 88 | void DecodeID( unsigned int compositeId, int* pOutEntityId, int* pOutMemeberId ); 89 | 90 | NATNET_DEPRECATED( "This method is deprecated; use the function NatNet_CopyFrame from NatNetCAPI.h instead." ) 91 | void CopyFrame( sFrameOfMocapData* pSrc, sFrameOfMocapData* pDst ); 92 | 93 | NATNET_DEPRECATED( "This method is deprecated; use the function NatNet_FreeFrame from NatNetCAPI.h instead." ) 94 | void FreeFrame( sFrameOfMocapData* pFrame ); 95 | 96 | NATNET_DEPRECATED("This method is deprecated; use the function NatNet_FreeDescriptions from NatNetCAPI.h instead.") 97 | void FreeDescriptions( sDataDescriptions* pDesc ); 98 | 99 | private: 100 | class ClientCore* m_pClientCore; 101 | }; 102 | -------------------------------------------------------------------------------- /src/LinearKalmanFilter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jungwon on 20. 8. 31.. 3 | // 4 | 5 | #include "../include/LinearKalmanFilter.h" 6 | 7 | LinearKalmanFilter::LinearKalmanFilter() { 8 | F = FMatrix::Zero(); 9 | G = GMatrix::Zero(); 10 | Q = QMatrix::Zero(); 11 | // R = RMatrix::Zero(); 12 | H = HMatrix::Zero(); 13 | 14 | x_old = NVector::Zero(); 15 | x_predict = NVector::Zero(); 16 | x_estimate = NVector::Zero(); 17 | P_old = PMatrix::Zero(); 18 | P_predict = PMatrix::Zero(); 19 | P_estimate = PMatrix::Zero(); 20 | 21 | sigma_Q = MVector::Zero(); 22 | sigma_R = MVector::Zero(); 23 | 24 | sigma_Q(0,0) = 20.0; 25 | sigma_Q(1,0) = 20.0; 26 | sigma_Q(2,0) = 20.0; 27 | sigma_R(0,0) = 0.001; 28 | sigma_R(1,0) = 0.001; 29 | sigma_R(2,0) = 0.001; 30 | 31 | H(0,0) = 1.0; 32 | H(1,1) = 1.0; 33 | H(2,2) = 1.0; 34 | } 35 | 36 | nav_msgs::Odometry LinearKalmanFilter::pose_cb(const geometry_msgs::PoseStamped& msg) 37 | { 38 | if(initialized) 39 | { 40 | t_new = ros::WallTime::now(); 41 | double dt = (t_new - t_old).toSec(); 42 | //if(dt>0.02) std::cout << " " << dt << std::endl; 43 | //else if(dt<0.005) std::cout << " " << dt << std::endl; 44 | //else std::cout << dt << std::endl; 45 | predict(dt); 46 | update(dt, msg); 47 | ros::WallTime t_now_wall = ros::WallTime::now(); 48 | ros::Time t_now; 49 | t_now.sec = t_now_wall.sec; 50 | t_now.nsec = t_now_wall.nsec; 51 | twist.header.stamp = t_now; 52 | twist.twist.linear.x = x_estimate(3,0); 53 | twist.twist.linear.y = x_estimate(4,0); 54 | twist.twist.linear.z = x_estimate(5,0); 55 | //twist_pub.publish(twist); 56 | odom.pose.pose = msg.pose; 57 | 58 | //convert from inertial velocity to body velocity 59 | double q0 = msg.pose.orientation.w; 60 | double q1 = msg.pose.orientation.x; 61 | double q2 = msg.pose.orientation.y; 62 | double q3 = msg.pose.orientation.z; 63 | double phi = atan2(2*(q0*q1 + q2*q3),1-2*(q1*q1+q2*q2)); 64 | double theta = asin(2*(q0*q2-q3*q1)); 65 | double psi = atan2(2*(q0*q3 + q1*q2),1-2*(q2*q2+q3*q3)); 66 | 67 | Eigen::Matrix R; 68 | R(0,0) = cos(psi)*cos(theta); 69 | R(0,1) = cos(psi)*sin(phi)*sin(theta) - cos(phi)*sin(psi); 70 | R(0,2) = sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta); 71 | R(1,0) = cos(theta)*sin(psi); 72 | R(1,1) = cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta); 73 | R(1,2) = cos(phi)*sin(psi)*sin(theta) - cos(psi)*sin(phi); 74 | R(2,0) = -sin(theta); 75 | R(2,1) = cos(theta)*sin(phi); 76 | R(2,2) = cos(phi)*cos(theta); 77 | Eigen::Vector3d twist_vector; 78 | twist_vector(0) = twist.twist.linear.x; 79 | twist_vector(1) = twist.twist.linear.y; 80 | twist_vector(2) = twist.twist.linear.z; 81 | 82 | //twist_vector = R.transpose()*twist_vector; //TODO: add param to control this 83 | 84 | odom.twist.twist.linear.x = twist_vector(0); 85 | odom.twist.twist.linear.y = twist_vector(1); 86 | odom.twist.twist.linear.z = twist_vector(2); 87 | // odom_pub.publish(odom); 88 | 89 | pose_new = msg; 90 | twist_raw.header.stamp = t_now; 91 | twist_raw.twist.linear.x = (pose_new.pose.position.x-pose_old.pose.position.x)/dt; 92 | twist_raw.twist.linear.y = (pose_new.pose.position.y-pose_old.pose.position.y)/dt; 93 | twist_raw.twist.linear.z = (pose_new.pose.position.z-pose_old.pose.position.z)/dt; 94 | //twist_pub_raw.publish(twist_raw); 95 | 96 | x_old = x_estimate; 97 | P_old = P_estimate; 98 | t_old = t_new; 99 | pose_old = pose_new; 100 | } 101 | else 102 | { 103 | t_old = ros::WallTime::now(); 104 | x_old(0,0) = msg.pose.position.x; 105 | x_old(1,0) = msg.pose.position.y; 106 | x_old(2,0) = msg.pose.position.z; 107 | 108 | P_old(0,0) = 0.1; 109 | P_old(1,1) = 0.1; 110 | P_old(2,2) = 0.1; 111 | P_old(3,3) = 1.1; 112 | P_old(4,4) = 1.1; 113 | P_old(5,5) = 1.1; 114 | 115 | if(PUBLISH_RAW_TWIST) { 116 | pose_old = msg; 117 | } 118 | 119 | initialized = true; 120 | } 121 | 122 | return odom; 123 | } 124 | 125 | void LinearKalmanFilter::predict(const double &dt) 126 | { 127 | F = computeF(dt); 128 | G = computeG(dt); 129 | Q = computeQ(G, sigma_Q); 130 | 131 | x_predict = F*x_old; 132 | P_predict = F*P_old*F.transpose() + Q; 133 | } 134 | 135 | void LinearKalmanFilter::update(const double &dt, const geometry_msgs::PoseStamped& msg) 136 | { 137 | MVector measure = MVector::Zero(); 138 | measure(0,0) = msg.pose.position.x; 139 | measure(1,0) = msg.pose.position.y; 140 | measure(2,0) = msg.pose.position.z; 141 | 142 | MVector residual = measure - H*x_predict; 143 | RMatrix R = computeR(); 144 | RMatrix innovation = R + H*P_predict*H.transpose(); 145 | GMatrix K = P_predict*H.transpose()*innovation.inverse(); 146 | 147 | x_estimate = x_predict + K*residual; 148 | P_estimate = P_predict - K*innovation*K.transpose(); 149 | } 150 | 151 | FMatrix LinearKalmanFilter::computeF(const double &dt) 152 | { 153 | FMatrix temp = FMatrix::Zero(); 154 | for(int i=0; i < Row; i++) 155 | { 156 | temp(i,i) = 1.0; 157 | } 158 | temp(0,3) = dt; 159 | temp(1,4) = dt; 160 | temp(2,5) = dt; 161 | 162 | return temp; 163 | } 164 | 165 | GMatrix LinearKalmanFilter::computeG(const double &dt) 166 | { 167 | GMatrix temp = GMatrix::Zero(); 168 | for(int i=0; i<3; i++) 169 | { 170 | temp(i,i) = 0.5*dt*dt; 171 | temp(i+3,i) = dt; 172 | } 173 | 174 | return temp; 175 | } 176 | 177 | QMatrix LinearKalmanFilter::computeQ(const GMatrix &G, const MVector &sigma_Q) 178 | { 179 | RMatrix temp = RMatrix::Zero(); 180 | for(int i=0; i < Col; i++) 181 | { 182 | temp(i,i) = sigma_Q(i,0)*sigma_Q(i,0); 183 | } 184 | 185 | return G*temp*G.transpose(); 186 | } 187 | 188 | RMatrix LinearKalmanFilter::computeR() 189 | { 190 | RMatrix temp = RMatrix::Zero(); 191 | for(int i=0; i < Col; i++) 192 | { 193 | temp(i,i) = sigma_R(i,0); 194 | } 195 | 196 | return temp; 197 | } 198 | 199 | -------------------------------------------------------------------------------- /include/NatNetCAPI.h: -------------------------------------------------------------------------------- 1 | //=============================================================================---- 2 | // Copyright © 2016 NaturalPoint, Inc. All Rights Reserved. 3 | // 4 | // This software is provided by the copyright holders and contributors "as is" and 5 | // any express or implied warranties, including, but not limited to, the implied 6 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 7 | // In no event shall NaturalPoint, Inc. or contributors be liable for any direct, 8 | // indirect, incidental, special, exemplary, or consequential damages 9 | // (including, but not limited to, procurement of substitute goods or services; 10 | // loss of use, data, or profits; or business interruption) however caused 11 | // and on any theory of liability, whether in contract, strict liability, 12 | // or tort (including negligence or otherwise) arising in any way out of 13 | // the use of this software, even if advised of the possibility of such damage. 14 | //=============================================================================---- 15 | 16 | #pragma once 17 | 18 | #include "NatNetTypes.h" 19 | 20 | //! @file 21 | //! @brief C language compatible helper functions for the NatNet SDK. 22 | 23 | #if defined( __cplusplus ) 24 | extern "C" { 25 | #endif 26 | 27 | 28 | typedef struct NatNetDiscovery_t* NatNetDiscoveryHandle; 29 | 30 | 31 | NATNET_API void NATNET_CALLCONV NatNet_GetVersion( unsigned char outVersion[4] ); 32 | NATNET_API void NATNET_CALLCONV NatNet_SetLogCallback( NatNetLogCallback pfnLogCallback ); 33 | 34 | NATNET_API void NATNET_CALLCONV NatNet_DecodeID( int compositeId, int* pOutEntityId, int* pOutMemberId ); 35 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_DecodeTimecode( unsigned int timecode, unsigned int timecodeSubframe, int* pOutHour, int* pOutMinute, int* pOutSecond, int* pOutFrame, int* pOutSubframe ); 36 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_TimecodeStringify( unsigned int timecode, unsigned int timecodeSubframe, char* outBuffer, int outBufferSize ); 37 | 38 | /// 39 | /// Helper that performs a deep copy from into . 40 | /// 41 | /// 42 | /// Some members of will be dynamically allocated. Call to 43 | /// deallocate them. 44 | /// 45 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_CopyFrame( sFrameOfMocapData* pSrc, sFrameOfMocapData* pDst ); 46 | 47 | /// 48 | /// Frees the dynamically allocated members of a frame copy created using . 49 | /// 50 | /// 51 | /// The object pointed to by itself is NOT de-allocated, only its nested members which 52 | /// were dynamically allocated. 53 | /// 54 | /// Warning: Do not call this on any that was not the destination of a call to 55 | /// . 56 | /// 57 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_FreeFrame( sFrameOfMocapData* pFrame ); 58 | 59 | 60 | /// 61 | /// Deallocates and all of its members; after this call, the object is no longer valid. 62 | /// 63 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_FreeDescriptions( sDataDescriptions* pDesc ); 64 | 65 | 66 | // "xxx.xxx.xxx.xxx" + null terminator 67 | enum { kNatNetIpv4AddrStrLenMax = 16 }; 68 | 69 | typedef struct sNatNetDiscoveredServer 70 | { 71 | char localAddress[kNatNetIpv4AddrStrLenMax]; 72 | char serverAddress[kNatNetIpv4AddrStrLenMax]; 73 | uint16_t serverCommandPort; 74 | sServerDescription serverDescription; 75 | } sNatNetDiscoveredServer; 76 | 77 | typedef void (NATNET_CALLCONV* NatNetServerDiscoveryCallback)( const sNatNetDiscoveredServer* pNewServer, void* pUserContext ); 78 | 79 | /// 80 | /// Sends broadcast messages to discover active NatNet servers and blocks for a specified time to gather responses. 81 | /// 82 | /// 83 | /// Array of length equal to the input value of . This array will receive the 84 | /// details of all servers discovered by the broadcast. 85 | /// 86 | /// 87 | /// Pointer to an integer containing the length of the array . After this function 88 | /// returns, the integer is modified to contain the total number of servers that responded to the broadcast 89 | /// inquiry. If the modified number is larger than the original number passed to the function, there was 90 | /// insufficient space for those additional servers. 91 | /// 92 | /// 93 | /// Amount of time, in milliseconds, to wait for responses to the broadcast before returning. 94 | /// 95 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_BroadcastServerDiscovery( sNatNetDiscoveredServer* outServers, int* pInOutNumServers, unsigned int timeoutMillisec = 1000 ); 96 | 97 | /// 98 | /// Begins sending periodic broadcast messages to discover active NatNet servers in the background. 99 | /// 100 | /// 101 | /// Out pointer that will receive a handle representing the asynchronous discovery process. The handle returned 102 | /// should be passed to later for cleanup. 103 | /// 104 | /// 105 | /// A function pointer that will be invoked once for every new server 106 | /// that's discovered by the asynchronous search. The callback will also be passed the provided 107 | /// argument. 108 | /// 109 | /// 110 | /// User-specified context data to be passed to the provided when invoked. 111 | /// 112 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_CreateAsyncServerDiscovery( NatNetDiscoveryHandle* pOutDiscovery, NatNetServerDiscoveryCallback pfnCallback, void* pUserContext = NULL ); 113 | 114 | /// 115 | /// Ends a previously created asynchronous server discovery process, and cleans up the associated resources. 116 | /// 117 | /// 118 | /// The handle representing the asynchronous discovery process. Returned by 119 | /// . 120 | /// 121 | NATNET_API ErrorCode NATNET_CALLCONV NatNet_FreeAsyncServerDiscovery( NatNetDiscoveryHandle discovery ); 122 | 123 | 124 | #if defined( __cplusplus ) 125 | } // extern "C" 126 | #endif 127 | -------------------------------------------------------------------------------- /include/NatNetTypes.h: -------------------------------------------------------------------------------- 1 | //=============================================================================---- 2 | // Copyright © 2016 NaturalPoint, Inc. All Rights Reserved. 3 | // 4 | // This software is provided by the copyright holders and contributors "as is" and 5 | // any express or implied warranties, including, but not limited to, the implied 6 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 7 | // In no event shall NaturalPoint, Inc. or contributors be liable for any direct, 8 | // indirect, incidental, special, exemplary, or consequential damages 9 | // (including, but not limited to, procurement of substitute goods or services; 10 | // loss of use, data, or profits; or business interruption) however caused 11 | // and on any theory of liability, whether in contract, strict liability, 12 | // or tort (including negligence or otherwise) arising in any way out of 13 | // the use of this software, even if advised of the possibility of such damage. 14 | //=============================================================================---- 15 | 16 | /* 17 | NatNetTypes defines the public, common data structures and types 18 | used when working with NatNetServer and NatNetClient objects. 19 | 20 | version 3.0.0.0 21 | */ 22 | 23 | #pragma once 24 | 25 | 26 | #include 27 | 28 | #if !defined( NULL ) 29 | # include 30 | #endif 31 | 32 | 33 | #ifdef _WIN32 34 | # define NATNET_CALLCONV __cdecl 35 | #else 36 | # define NATNET_CALLCONV 37 | #endif 38 | 39 | 40 | #ifdef _MSC_VER 41 | # define NATNET_DEPRECATED( msg ) __declspec(deprecated(msg)) 42 | #else 43 | # define NATNET_DEPRECATED( msg ) __attribute__((deprecated(msg))) 44 | #endif 45 | 46 | 47 | // storage class specifier 48 | // - to link to NatNet dynamically, define NATNETLIB_IMPORTS and link to the NatNet import library. 49 | // - to link to NatNet statically, link to the NatNet static library. 50 | #if defined( _WIN32 ) 51 | # if defined( NATNETLIB_EXPORTS ) 52 | # define NATNET_API __declspec(dllexport) 53 | # elif defined( NATNETLIB_IMPORTS ) 54 | # define NATNET_API __declspec(dllimport) 55 | # else 56 | # define NATNET_API 57 | # endif 58 | #else 59 | # if defined( NATNETLIB_EXPORTS ) 60 | # define NATNET_API __attribute((visibility("default"))) 61 | # elif defined( NATNETLIB_IMPORTS ) 62 | # define NATNET_API 63 | # else 64 | # define NATNET_API 65 | # endif 66 | #endif 67 | 68 | 69 | #define NATNET_DEFAULT_PORT_COMMAND 1510 70 | #define NATNET_DEFAULT_PORT_DATA 1511 71 | #define NATNET_DEFAULT_MULTICAST_ADDRESS "239.255.42.99" // IANA, local network 72 | 73 | 74 | // model limits 75 | #define MAX_MODELS 2000 // maximum number of total models (data descriptions) 76 | #define MAX_MARKERSETS 1000 // maximum number of MarkerSets 77 | #define MAX_RIGIDBODIES 1000 // maximum number of RigidBodies 78 | #define MAX_NAMELENGTH 256 // maximum length for strings 79 | #define MAX_MARKERS 200 // maximum number of markers per MarkerSet 80 | #define MAX_RBMARKERS 20 // maximum number of markers per RigidBody 81 | #define MAX_SKELETONS 100 // maximum number of skeletons 82 | #define MAX_SKELRIGIDBODIES 200 // maximum number of RididBodies per Skeleton 83 | #define MAX_LABELED_MARKERS 1000 // maximum number of labeled markers per frame 84 | #define MAX_UNLABELED_MARKERS 1000 // maximum number of unlabeled (other) markers per frame 85 | 86 | #define MAX_FORCEPLATES 8 // maximum number of force plates 87 | #define MAX_DEVICES 32 // maximum number of peripheral devices 88 | #define MAX_ANALOG_CHANNELS 32 // maximum number of data channels (signals) per analog/force plate device 89 | #define MAX_ANALOG_SUBFRAMES 30 // maximum number of analog/force plate frames per mocap frame 90 | 91 | #define MAX_PACKETSIZE 100000 // max size of packet (actual packet size is dynamic) 92 | 93 | 94 | // Client/server message ids 95 | #define NAT_CONNECT 0 96 | #define NAT_SERVERINFO 1 97 | #define NAT_REQUEST 2 98 | #define NAT_RESPONSE 3 99 | #define NAT_REQUEST_MODELDEF 4 100 | #define NAT_MODELDEF 5 101 | #define NAT_REQUEST_FRAMEOFDATA 6 102 | #define NAT_FRAMEOFDATA 7 103 | #define NAT_MESSAGESTRING 8 104 | #define NAT_DISCONNECT 9 105 | #define NAT_KEEPALIVE 10 106 | #define NAT_DISCONNECTBYTIMEOUT 11 107 | #define NAT_ECHOREQUEST 12 108 | #define NAT_ECHORESPONSE 13 109 | #define NAT_DISCOVERY 14 110 | #define NAT_UNRECOGNIZED_REQUEST 100 111 | 112 | 113 | #define UNDEFINED 999999.9999 114 | 115 | 116 | // NatNet uses to set reporting level of messages. 117 | // Clients use to set level of messages to receive. 118 | typedef enum Verbosity 119 | { 120 | Verbosity_None = 0, 121 | Verbosity_Debug, 122 | Verbosity_Info, 123 | Verbosity_Warning, 124 | Verbosity_Error, 125 | } Verbosity; 126 | 127 | 128 | // NatNet error reporting codes 129 | typedef enum ErrorCode 130 | { 131 | ErrorCode_OK = 0, 132 | ErrorCode_Internal, 133 | ErrorCode_External, 134 | ErrorCode_Network, 135 | ErrorCode_Other, 136 | ErrorCode_InvalidArgument, 137 | ErrorCode_InvalidOperation 138 | } ErrorCode; 139 | 140 | 141 | // NatNet connection types 142 | typedef enum ConnectionType 143 | { 144 | ConnectionType_Multicast = 0, 145 | ConnectionType_Unicast 146 | } ConnectionType; 147 | 148 | 149 | // NatNet data types 150 | typedef enum DataDescriptors 151 | { 152 | Descriptor_MarkerSet = 0, 153 | Descriptor_RigidBody, 154 | Descriptor_Skeleton, 155 | Descriptor_ForcePlate, 156 | Descriptor_Device 157 | } DataDescriptors; 158 | 159 | 160 | typedef float MarkerData[3]; // posX, posY, posZ 161 | 162 | 163 | #pragma pack(push, 1) 164 | 165 | // sender 166 | typedef struct sSender 167 | { 168 | char szName[MAX_NAMELENGTH]; // host app's name 169 | uint8_t Version[4]; // host app's version [major.minor.build.revision] 170 | uint8_t NatNetVersion[4]; // host app's NatNet version [major.minor.build.revision] 171 | } sSender; 172 | 173 | 174 | typedef struct sSender_Server 175 | { 176 | sSender Common; 177 | 178 | uint64_t HighResClockFrequency; // host's high resolution clock frequency (ticks per second) 179 | uint16_t DataPort; 180 | bool IsMulticast; 181 | uint8_t MulticastGroupAddress[4]; 182 | } sSender_Server; 183 | 184 | 185 | // packet 186 | // note : only used by clients who are depacketizing NatNet packets directly 187 | typedef struct sPacket 188 | { 189 | uint16_t iMessage; // message ID (e.g. NAT_FRAMEOFDATA) 190 | uint16_t nDataBytes; // Num bytes in payload 191 | union 192 | { 193 | uint8_t cData[MAX_PACKETSIZE]; 194 | char szData[MAX_PACKETSIZE]; 195 | uint32_t lData[MAX_PACKETSIZE/sizeof(uint32_t)]; 196 | float fData[MAX_PACKETSIZE/sizeof(float)]; 197 | sSender Sender; 198 | sSender_Server SenderServer; 199 | } Data; // payload - statically allocated for convenience. Actual packet size is determined by nDataBytes 200 | } sPacket; 201 | 202 | #pragma pack(pop) 203 | 204 | 205 | // Mocap server application description 206 | typedef struct sServerDescription 207 | { 208 | bool HostPresent; // host is present and accounted for 209 | char szHostComputerName[MAX_NAMELENGTH];// host computer name 210 | uint8_t HostComputerAddress[4]; // host IP address 211 | char szHostApp[MAX_NAMELENGTH]; // name of host app 212 | uint8_t HostAppVersion[4]; // version of host app 213 | uint8_t NatNetVersion[4]; // host app's version of NatNet 214 | 215 | // Clock and connection info is only provided by NatNet 3.0+ servers. 216 | uint64_t HighResClockFrequency; // host's high resolution clock frequency (ticks per second) 217 | 218 | bool bConnectionInfoValid; // If the server predates NatNet 3.0, this will be false, and the other Connection* fields invalid. 219 | uint16_t ConnectionDataPort; // The data port this server is configured to use. 220 | bool ConnectionMulticast; // Whether this server is streaming in multicast. If false, connect in unicast instead. 221 | uint8_t ConnectionMulticastAddress[4]; // The multicast group address to use for a multicast connection. 222 | } sServerDescription; 223 | 224 | 225 | // Marker 226 | typedef struct sMarker 227 | { 228 | int32_t ID; // Unique identifier: 229 | // For active markers, this is the Active ID. For passive markers, this is the PointCloud assigned ID. 230 | // For legacy assets that are created prior to 2.0, this is both AssetID (High-bit) and Member ID (Lo-bit) 231 | 232 | float x; // x position 233 | float y; // y position 234 | float z; // z position 235 | float size; // marker size 236 | int16_t params; // host defined parameters 237 | float residual; // marker error residual, in mm/ray 238 | } sMarker; 239 | 240 | 241 | // MarkerSet Definition 242 | typedef struct sMarkerSetDescription 243 | { 244 | char szName[MAX_NAMELENGTH]; // MarkerSet name 245 | int32_t nMarkers; // # of markers in MarkerSet 246 | char** szMarkerNames; // array of marker names 247 | } sMarkerSetDescription; 248 | 249 | 250 | // MarkerSet Data (single frame of one MarkerSet) 251 | typedef struct sMarkerSetData 252 | { 253 | char szName[MAX_NAMELENGTH]; // MarkerSet name 254 | int32_t nMarkers; // # of markers in MarkerSet 255 | MarkerData* Markers; // Array of marker data ( [nMarkers][3] ) 256 | } sMarkerSetData; 257 | 258 | 259 | // Rigid Body Definition 260 | typedef struct sRigidBodyDescription 261 | { 262 | char szName[MAX_NAMELENGTH]; // RigidBody name 263 | int32_t ID; // RigidBody identifier: Streaming ID value for rigid body assets, and Bone index value for skeleton rigid bodies. 264 | int32_t parentID; // ID of parent Rigid Body (in case hierarchy exists; otherwise -1) 265 | float offsetx, offsety, offsetz; // offset position relative to parent 266 | int32_t nMarkers; // Number of markers associated with this rigid body 267 | MarkerData* MarkerPositions; // Array of marker locations ( [nMarkers][3] ) 268 | int32_t* MarkerRequiredLabels; // Array of expected marker active labels - 0 if not specified. ( [nMarkers] ) 269 | } sRigidBodyDescription; 270 | 271 | 272 | // Rigid Body Data (single frame of one rigid body) 273 | typedef struct sRigidBodyData 274 | { 275 | int32_t ID; // RigidBody identifier: 276 | // For rigid body assets, this is the Streaming ID value. 277 | // For skeleton assets, this combines both skeleton ID (High-bit) and Bone ID (Low-bit). 278 | 279 | float x, y, z; // Position 280 | float qx, qy, qz, qw; // Orientation 281 | float MeanError; // Mean measure-to-solve deviation 282 | int16_t params; // Host defined tracking flags 283 | 284 | #if defined(__cplusplus) 285 | sRigidBodyData() 286 | : ID( 0 ) 287 | , params( 0 ) 288 | { 289 | } 290 | #endif 291 | } sRigidBodyData; 292 | 293 | 294 | // Skeleton Description 295 | typedef struct sSkeletonDescription 296 | { 297 | char szName[MAX_NAMELENGTH]; // Skeleton name 298 | int32_t skeletonID; // Skeleton unqiue identifier 299 | int32_t nRigidBodies; // # of rigid bodies (bones) in skeleton 300 | sRigidBodyDescription RigidBodies[MAX_SKELRIGIDBODIES]; // array of rigid body (bone) descriptions 301 | } sSkeletonDescription; 302 | 303 | 304 | // Skeleton Data 305 | typedef struct sSkeletonData 306 | { 307 | int32_t skeletonID; // Skeleton unique identifier 308 | int32_t nRigidBodies; // # of rigid bodies 309 | sRigidBodyData* RigidBodyData; // Array of RigidBody data 310 | } sSkeletonData; 311 | 312 | // FrocePlate description 313 | typedef struct sForcePlateDescription 314 | { 315 | int32_t ID; // used for order, and for identification in the data stream 316 | char strSerialNo[128]; // for unique plate identification 317 | float fWidth; // plate physical width (manufacturer supplied) 318 | float fLength; // plate physical length (manufacturer supplied) 319 | float fOriginX, fOriginY, fOriginZ; // electrical center offset (from electrical center to geometric center-top of force plate) (manufacturer supplied) 320 | float fCalMat[12][12]; // force plate calibration matrix (for raw analog voltage channel type only) 321 | float fCorners[4][3]; // plate corners, in world (aka Mocap System) coordinates, clockwise from plate +x,+y (refer to C3D spec for details) 322 | int32_t iPlateType; // force plate 'type' (refer to C3D spec for details) 323 | int32_t iChannelDataType; // 0=Calibrated force data, 1=Raw analog voltages 324 | int32_t nChannels; // # of channels (signals) 325 | char szChannelNames[MAX_ANALOG_CHANNELS][MAX_NAMELENGTH]; // channel names 326 | } sForcePlateDescription; 327 | 328 | // Peripheral Device description (e.g. NIDAQ) 329 | typedef struct sDeviceDescription 330 | { 331 | int32_t ID; // used for order, and for identification in the data stream 332 | char strName[128]; // device name as appears in Motive 333 | char strSerialNo[128]; // for unique device identification 334 | int32_t iDeviceType; // device 'type' code 335 | int32_t iChannelDataType; // channel data type code 336 | int32_t nChannels; // # of currently enabled/active channels (signals) 337 | char szChannelNames[MAX_ANALOG_CHANNELS][MAX_NAMELENGTH]; // channel names 338 | } sDeviceDescription; 339 | 340 | // Tracked Object data description. 341 | // A Mocap Server application (e.g. Arena or TrackingTools) may contain multiple 342 | // tracked "objects (e.g. RigidBody, MarkerSet). Each object will have its 343 | // own DataDescription. 344 | typedef struct sDataDescription 345 | { 346 | int32_t type; 347 | union 348 | { 349 | sMarkerSetDescription* MarkerSetDescription; 350 | sRigidBodyDescription* RigidBodyDescription; 351 | sSkeletonDescription* SkeletonDescription; 352 | sForcePlateDescription* ForcePlateDescription; 353 | sDeviceDescription* DeviceDescription; 354 | } Data; 355 | } sDataDescription; 356 | 357 | 358 | // All data descriptions for current session (as defined by host app) 359 | typedef struct sDataDescriptions 360 | { 361 | int32_t nDataDescriptions; 362 | sDataDescription arrDataDescriptions[MAX_MODELS]; 363 | } sDataDescriptions; 364 | 365 | 366 | typedef struct sAnalogChannelData 367 | { 368 | int32_t nFrames; // # of analog frames of data in this channel data packet (typically # of subframes per mocap frame) 369 | float Values[MAX_ANALOG_SUBFRAMES]; // values 370 | } sAnalogChannelData; 371 | 372 | typedef struct sForcePlateData 373 | { 374 | int32_t ID; // ForcePlate ID (from data description) 375 | int32_t nChannels; // # of channels (signals) for this force plate 376 | sAnalogChannelData ChannelData[MAX_ANALOG_CHANNELS];// Channel (signal) data (e.g. Fx[], Fy[], Fz[]) 377 | int16_t params; // Host defined flags 378 | } sForcePlateData; 379 | 380 | typedef struct sDeviceData 381 | { 382 | int32_t ID; // Device ID (from data description) 383 | int32_t nChannels; // # of active channels (signals) for this device 384 | sAnalogChannelData ChannelData[MAX_ANALOG_CHANNELS];// Channel (signal) data (e.g. ai0, ai1, ai2) 385 | int16_t params; // Host defined flags 386 | } sDeviceData; 387 | 388 | // Single frame of data (for all tracked objects) 389 | typedef struct sFrameOfMocapData 390 | { 391 | int32_t iFrame; // host defined frame number 392 | 393 | int32_t nMarkerSets; // # of marker sets in this frame of data 394 | sMarkerSetData MocapData[MAX_MARKERSETS]; // MarkerSet data 395 | 396 | int32_t nOtherMarkers; // # of undefined markers 397 | MarkerData* OtherMarkers; // undefined marker data 398 | 399 | int32_t nRigidBodies; // # of rigid bodies 400 | sRigidBodyData RigidBodies[MAX_RIGIDBODIES]; // Rigid body data 401 | 402 | int32_t nSkeletons; // # of Skeletons 403 | sSkeletonData Skeletons[MAX_SKELETONS]; // Skeleton data 404 | 405 | int32_t nLabeledMarkers; // # of Labeled Markers 406 | sMarker LabeledMarkers[MAX_LABELED_MARKERS]; // Labeled Marker data (labeled markers not associated with a "MarkerSet") 407 | 408 | int32_t nForcePlates; // # of force plates 409 | sForcePlateData ForcePlates[MAX_FORCEPLATES]; // Force plate data 410 | 411 | int32_t nDevices; // # of devices 412 | sDeviceData Devices[MAX_DEVICES]; // Device data 413 | 414 | uint32_t Timecode; // SMPTE timecode (if available) 415 | uint32_t TimecodeSubframe; // timecode sub-frame data 416 | double fTimestamp; // timestamp since software start ( software timestamp ) 417 | uint64_t CameraMidExposureTimestamp; // Given in host's high resolution ticks (from e.g. QueryPerformanceCounter). 418 | uint64_t CameraDataReceivedTimestamp; // Given in host's high resolution ticks (from e.g. QueryPerformanceCounter). 419 | uint64_t TransmitTimestamp; // Given in host's high resolution ticks (from e.g. QueryPerformanceCounter). 420 | int16_t params; // host defined parameters 421 | } sFrameOfMocapData; 422 | 423 | 424 | typedef struct sNatNetClientConnectParams 425 | { 426 | ConnectionType connectionType; 427 | uint16_t serverCommandPort; 428 | uint16_t serverDataPort; 429 | const char* serverAddress; 430 | const char* localAddress; 431 | const char* multicastAddress; 432 | 433 | #if defined(__cplusplus) 434 | sNatNetClientConnectParams() 435 | : connectionType( ConnectionType_Multicast ) 436 | , serverCommandPort( 0 ) 437 | , serverDataPort( 0 ) 438 | , serverAddress( NULL ) 439 | , localAddress( NULL ) 440 | , multicastAddress( NULL ) 441 | { 442 | } 443 | #endif 444 | } sNatNetClientConnectParams; 445 | 446 | 447 | // Callback function pointer types 448 | typedef void (NATNET_CALLCONV* NatNetLogCallback)( Verbosity level, const char* message ); 449 | typedef void (NATNET_CALLCONV* NatNetFrameReceivedCallback)( sFrameOfMocapData* pFrameOfData, void* pUserData ); 450 | typedef void (NATNET_CALLCONV* NatNetUnknownMessageCallback)( sPacket* pPacket, void* pUserData ); 451 | -------------------------------------------------------------------------------- /src/SampleClient.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright � 2012 NaturalPoint Inc. 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. */ 15 | 16 | 17 | /* 18 | 19 | SampleClient.cpp 20 | 21 | This program connects to a NatNet server, receives a data stream, and writes that data stream 22 | to an ascii file. The purpose is to illustrate using the NatNetClient class. 23 | 24 | Usage [optional]: 25 | 26 | SampleClient [ServerIP] [LocalIP] [OutputFilename] 27 | 28 | [ServerIP] IP address of the server (e.g. 192.168.0.107) ( defaults to local machine) 29 | [OutputFilename] Name of points file (pts) to write out. defaults to Client-output.pts 30 | 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #ifdef _WIN32 39 | # include 40 | #else 41 | # include 42 | # include 43 | #endif 44 | 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | #ifndef _WIN32 52 | char getch(); 53 | #endif 54 | void _WriteHeader(FILE* fp, sDataDescriptions* pBodyDefs); 55 | void _WriteFrame(FILE* fp, sFrameOfMocapData* data); 56 | void _WriteFooter(FILE* fp); 57 | void NATNET_CALLCONV ServerDiscoveredCallback( const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext ); 58 | void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData); // receives data from the server 59 | void NATNET_CALLCONV MessageHandler(Verbosity msgType, const char* msg); // receives NatNet error messages 60 | void resetClient(); 61 | int ConnectClient(); 62 | 63 | static const ConnectionType kDefaultConnectionType = ConnectionType_Multicast; 64 | 65 | NatNetClient* g_pClient = NULL; 66 | FILE* g_outputFile; 67 | 68 | std::vector< sNatNetDiscoveredServer > g_discoveredServers; 69 | sNatNetClientConnectParams g_connectParams; 70 | char g_discoveredMulticastGroupAddr[kNatNetIpv4AddrStrLenMax] = NATNET_DEFAULT_MULTICAST_ADDRESS; 71 | int g_analogSamplesPerMocapFrame = 0; 72 | sServerDescription g_serverDescription; 73 | 74 | 75 | int main( int argc, char* argv[] ) 76 | { 77 | // print version info 78 | unsigned char ver[4]; 79 | NatNet_GetVersion( ver ); 80 | printf( "NatNet Sample Client (NatNet ver. %d.%d.%d.%d)\n", ver[0], ver[1], ver[2], ver[3] ); 81 | 82 | // Install logging callback 83 | NatNet_SetLogCallback( MessageHandler ); 84 | 85 | // create NatNet client 86 | g_pClient = new NatNetClient(); 87 | 88 | // set the frame callback handler 89 | g_pClient->SetFrameReceivedCallback( DataHandler, g_pClient ); // this function will receive data from the server 90 | 91 | // If no arguments were specified on the command line, 92 | // attempt to discover servers on the local network. 93 | if ( argc == 1 ) 94 | { 95 | // An example of synchronous server discovery. 96 | #if 0 97 | const unsigned int kDiscoveryWaitTimeMillisec = 5 * 1000; // Wait 5 seconds for responses. 98 | const int kMaxDescriptions = 10; // Get info for, at most, the first 10 servers to respond. 99 | sNatNetDiscoveredServer servers[kMaxDescriptions]; 100 | int actualNumDescriptions = kMaxDescriptions; 101 | NatNet_BroadcastServerDiscovery( servers, &actualNumDescriptions ); 102 | 103 | if ( actualNumDescriptions < kMaxDescriptions ) 104 | { 105 | // If this happens, more servers responded than the array was able to store. 106 | } 107 | #endif 108 | 109 | // Do asynchronous server discovery. 110 | printf( "Looking for servers on the local network.\n" ); 111 | printf( "Press the number key that corresponds to any discovered server to connect to that server.\n" ); 112 | printf( "Press Q at any time to quit.\n\n" ); 113 | 114 | NatNetDiscoveryHandle discovery; 115 | NatNet_CreateAsyncServerDiscovery( &discovery, ServerDiscoveredCallback ); 116 | 117 | while ( const int c = getch() ) 118 | { 119 | if ( c >= '1' && c <= '9' ) 120 | { 121 | const size_t serverIndex = c - '1'; 122 | if ( serverIndex < g_discoveredServers.size() ) 123 | { 124 | const sNatNetDiscoveredServer& discoveredServer = g_discoveredServers[serverIndex]; 125 | 126 | if ( discoveredServer.serverDescription.bConnectionInfoValid ) 127 | { 128 | // Build the connection parameters. 129 | #ifdef _WIN32 130 | _snprintf_s( 131 | #else 132 | snprintf( 133 | #endif 134 | g_discoveredMulticastGroupAddr, sizeof g_discoveredMulticastGroupAddr, 135 | "%" PRIu8 ".%" PRIu8".%" PRIu8".%" PRIu8"", 136 | discoveredServer.serverDescription.ConnectionMulticastAddress[0], 137 | discoveredServer.serverDescription.ConnectionMulticastAddress[1], 138 | discoveredServer.serverDescription.ConnectionMulticastAddress[2], 139 | discoveredServer.serverDescription.ConnectionMulticastAddress[3] 140 | ); 141 | 142 | g_connectParams.connectionType = discoveredServer.serverDescription.ConnectionMulticast ? ConnectionType_Multicast : ConnectionType_Unicast; 143 | g_connectParams.serverCommandPort = discoveredServer.serverCommandPort; 144 | g_connectParams.serverDataPort = discoveredServer.serverDescription.ConnectionDataPort; 145 | g_connectParams.serverAddress = discoveredServer.serverAddress; 146 | g_connectParams.localAddress = discoveredServer.localAddress; 147 | g_connectParams.multicastAddress = g_discoveredMulticastGroupAddr; 148 | } 149 | else 150 | { 151 | // We're missing some info because it's a legacy server. 152 | // Guess the defaults and make a best effort attempt to connect. 153 | g_connectParams.connectionType = kDefaultConnectionType; 154 | g_connectParams.serverCommandPort = discoveredServer.serverCommandPort; 155 | g_connectParams.serverDataPort = 0; 156 | g_connectParams.serverAddress = discoveredServer.serverAddress; 157 | g_connectParams.localAddress = discoveredServer.localAddress; 158 | g_connectParams.multicastAddress = NULL; 159 | } 160 | 161 | break; 162 | } 163 | } 164 | else if ( c == 'q' ) 165 | { 166 | return 0; 167 | } 168 | } 169 | 170 | NatNet_FreeAsyncServerDiscovery( discovery ); 171 | } 172 | else 173 | { 174 | g_connectParams.connectionType = kDefaultConnectionType; 175 | 176 | if ( argc >= 2 ) 177 | { 178 | g_connectParams.serverAddress = argv[1]; 179 | } 180 | 181 | if ( argc >= 3 ) 182 | { 183 | g_connectParams.localAddress = argv[2]; 184 | } 185 | } 186 | 187 | int iResult; 188 | 189 | // Connect to Motive 190 | iResult = ConnectClient(); 191 | if (iResult != ErrorCode_OK) 192 | { 193 | printf("Error initializing client. See log for details. Exiting"); 194 | return 1; 195 | } 196 | else 197 | { 198 | printf("Client initialized and ready.\n"); 199 | } 200 | 201 | 202 | // Send/receive test request 203 | void* response; 204 | int nBytes; 205 | printf("[SampleClient] Sending Test Request\n"); 206 | iResult = g_pClient->SendMessageAndWait("TestRequest", &response, &nBytes); 207 | if (iResult == ErrorCode_OK) 208 | { 209 | printf("[SampleClient] Received: %s", (char*)response); 210 | } 211 | 212 | // Retrieve Data Descriptions from Motive 213 | printf("\n\n[SampleClient] Requesting Data Descriptions..."); 214 | sDataDescriptions* pDataDefs = NULL; 215 | iResult = g_pClient->GetDataDescriptionList(&pDataDefs); 216 | if (iResult != ErrorCode_OK || pDataDefs == NULL) 217 | { 218 | printf("[SampleClient] Unable to retrieve Data Descriptions."); 219 | } 220 | else 221 | { 222 | printf("[SampleClient] Received %d Data Descriptions:\n", pDataDefs->nDataDescriptions ); 223 | for(int i=0; i < pDataDefs->nDataDescriptions; i++) 224 | { 225 | printf("Data Description # %d (type=%d)\n", i, pDataDefs->arrDataDescriptions[i].type); 226 | if(pDataDefs->arrDataDescriptions[i].type == Descriptor_MarkerSet) 227 | { 228 | // MarkerSet 229 | sMarkerSetDescription* pMS = pDataDefs->arrDataDescriptions[i].Data.MarkerSetDescription; 230 | printf("MarkerSet Name : %s\n", pMS->szName); 231 | for(int i=0; i < pMS->nMarkers; i++) 232 | printf("%s\n", pMS->szMarkerNames[i]); 233 | 234 | } 235 | else if(pDataDefs->arrDataDescriptions[i].type == Descriptor_RigidBody) 236 | { 237 | // RigidBody 238 | sRigidBodyDescription* pRB = pDataDefs->arrDataDescriptions[i].Data.RigidBodyDescription; 239 | printf("RigidBody Name : %s\n", pRB->szName); 240 | printf("RigidBody ID : %d\n", pRB->ID); 241 | printf("RigidBody Parent ID : %d\n", pRB->parentID); 242 | printf("Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx, pRB->offsety, pRB->offsetz); 243 | 244 | if ( pRB->MarkerPositions != NULL && pRB->MarkerRequiredLabels != NULL ) 245 | { 246 | for ( int markerIdx = 0; markerIdx < pRB->nMarkers; ++markerIdx ) 247 | { 248 | const MarkerData& markerPosition = pRB->MarkerPositions[markerIdx]; 249 | const int markerRequiredLabel = pRB->MarkerRequiredLabels[markerIdx]; 250 | 251 | printf( "\tMarker #%d:\n", markerIdx ); 252 | printf( "\t\tPosition: %.2f, %.2f, %.2f\n", markerPosition[0], markerPosition[1], markerPosition[2] ); 253 | 254 | if ( markerRequiredLabel != 0 ) 255 | { 256 | printf( "\t\tRequired active label: %d\n", markerRequiredLabel ); 257 | } 258 | } 259 | } 260 | } 261 | else if(pDataDefs->arrDataDescriptions[i].type == Descriptor_Skeleton) 262 | { 263 | // Skeleton 264 | sSkeletonDescription* pSK = pDataDefs->arrDataDescriptions[i].Data.SkeletonDescription; 265 | printf("Skeleton Name : %s\n", pSK->szName); 266 | printf("Skeleton ID : %d\n", pSK->skeletonID); 267 | printf("RigidBody (Bone) Count : %d\n", pSK->nRigidBodies); 268 | for(int j=0; j < pSK->nRigidBodies; j++) 269 | { 270 | sRigidBodyDescription* pRB = &pSK->RigidBodies[j]; 271 | printf(" RigidBody Name : %s\n", pRB->szName); 272 | printf(" RigidBody ID : %d\n", pRB->ID); 273 | printf(" RigidBody Parent ID : %d\n", pRB->parentID); 274 | printf(" Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx, pRB->offsety, pRB->offsetz); 275 | } 276 | } 277 | else if(pDataDefs->arrDataDescriptions[i].type == Descriptor_ForcePlate) 278 | { 279 | // Force Plate 280 | sForcePlateDescription* pFP = pDataDefs->arrDataDescriptions[i].Data.ForcePlateDescription; 281 | printf("Force Plate ID : %d\n", pFP->ID); 282 | printf("Force Plate Serial : %s\n", pFP->strSerialNo); 283 | printf("Force Plate Width : %3.2f\n", pFP->fWidth); 284 | printf("Force Plate Length : %3.2f\n", pFP->fLength); 285 | printf("Force Plate Electrical Center Offset (%3.3f, %3.3f, %3.3f)\n", pFP->fOriginX,pFP->fOriginY, pFP->fOriginZ); 286 | for(int iCorner=0; iCorner<4; iCorner++) 287 | printf("Force Plate Corner %d : (%3.4f, %3.4f, %3.4f)\n", iCorner, pFP->fCorners[iCorner][0],pFP->fCorners[iCorner][1],pFP->fCorners[iCorner][2]); 288 | printf("Force Plate Type : %d\n", pFP->iPlateType); 289 | printf("Force Plate Data Type : %d\n", pFP->iChannelDataType); 290 | printf("Force Plate Channel Count : %d\n", pFP->nChannels); 291 | for(int iChannel=0; iChannelnChannels; iChannel++) 292 | printf("\tChannel %d : %s\n", iChannel, pFP->szChannelNames[iChannel]); 293 | } 294 | else if (pDataDefs->arrDataDescriptions[i].type == Descriptor_Device) 295 | { 296 | // Peripheral Device 297 | sDeviceDescription* pDevice = pDataDefs->arrDataDescriptions[i].Data.DeviceDescription; 298 | printf("Device Name : %s\n", pDevice->strName); 299 | printf("Device Serial : %s\n", pDevice->strSerialNo); 300 | printf("Device ID : %d\n", pDevice->ID); 301 | printf("Device Channel Count : %d\n", pDevice->nChannels); 302 | for (int iChannel = 0; iChannel < pDevice->nChannels; iChannel++) 303 | printf("\tChannel %d : %s\n", iChannel, pDevice->szChannelNames[iChannel]); 304 | } 305 | else 306 | { 307 | printf("Unknown data type."); 308 | // Unknown 309 | } 310 | } 311 | } 312 | 313 | 314 | // Create data file for writing received stream into 315 | const char* szFile = "Client-output.pts"; 316 | if (argc > 3) 317 | szFile = argv[3]; 318 | 319 | g_outputFile = fopen(szFile, "w"); 320 | if(!g_outputFile) 321 | { 322 | printf("error opening output file %s. Exiting.", szFile); 323 | exit(1); 324 | } 325 | 326 | if ( pDataDefs ) 327 | { 328 | _WriteHeader( g_outputFile, pDataDefs ); 329 | NatNet_FreeDescriptions( pDataDefs ); 330 | pDataDefs = NULL; 331 | } 332 | 333 | // Ready to receive marker stream! 334 | printf("\nClient is connected to server and listening for data...\n"); 335 | int c; 336 | bool bExit = false; 337 | while(c=getch()) 338 | { 339 | switch(c) 340 | { 341 | case 'q': 342 | bExit = true; 343 | break; 344 | case 'r': 345 | resetClient(); 346 | break; 347 | case 'p': 348 | sServerDescription ServerDescription; 349 | memset(&ServerDescription, 0, sizeof(ServerDescription)); 350 | g_pClient->GetServerDescription(&ServerDescription); 351 | if(!ServerDescription.HostPresent) 352 | { 353 | printf("Unable to connect to server. Host not present. Exiting."); 354 | return 1; 355 | } 356 | break; 357 | case 's': 358 | { 359 | printf("\n\n[SampleClient] Requesting Data Descriptions..."); 360 | sDataDescriptions* pDataDefs = NULL; 361 | iResult = g_pClient->GetDataDescriptionList(&pDataDefs); 362 | if (iResult != ErrorCode_OK || pDataDefs == NULL) 363 | { 364 | printf("[SampleClient] Unable to retrieve Data Descriptions."); 365 | } 366 | else 367 | { 368 | printf("[SampleClient] Received %d Data Descriptions:\n", pDataDefs->nDataDescriptions); 369 | } 370 | } 371 | break; 372 | case 'm': // change to multicast 373 | g_connectParams.connectionType = ConnectionType_Multicast; 374 | iResult = ConnectClient(); 375 | if(iResult == ErrorCode_OK) 376 | printf("Client connection type changed to Multicast.\n\n"); 377 | else 378 | printf("Error changing client connection type to Multicast.\n\n"); 379 | break; 380 | case 'u': // change to unicast 381 | g_connectParams.connectionType = ConnectionType_Unicast; 382 | iResult = ConnectClient(); 383 | if(iResult == ErrorCode_OK) 384 | printf("Client connection type changed to Unicast.\n\n"); 385 | else 386 | printf("Error changing client connection type to Unicast.\n\n"); 387 | break; 388 | case 'c' : // connect 389 | iResult = ConnectClient(); 390 | break; 391 | case 'd' : // disconnect 392 | // note: applies to unicast connections only - indicates to Motive to stop sending packets to that client endpoint 393 | iResult = g_pClient->SendMessageAndWait("Disconnect", &response, &nBytes); 394 | if (iResult == ErrorCode_OK) 395 | printf("[SampleClient] Disconnected"); 396 | break; 397 | default: 398 | break; 399 | } 400 | if(bExit) 401 | break; 402 | } 403 | 404 | // Done - clean up. 405 | if (g_pClient) 406 | { 407 | g_pClient->Disconnect(); 408 | delete g_pClient; 409 | g_pClient = NULL; 410 | } 411 | 412 | if (g_outputFile) 413 | { 414 | _WriteFooter(g_outputFile); 415 | fclose(g_outputFile); 416 | g_outputFile = NULL; 417 | } 418 | 419 | return ErrorCode_OK; 420 | } 421 | 422 | 423 | void NATNET_CALLCONV ServerDiscoveredCallback( const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext ) 424 | { 425 | char serverHotkey = '.'; 426 | if ( g_discoveredServers.size() < 9 ) 427 | { 428 | serverHotkey = static_cast('1' + g_discoveredServers.size()); 429 | } 430 | 431 | const char* warning = ""; 432 | 433 | if ( pDiscoveredServer->serverDescription.bConnectionInfoValid == false ) 434 | { 435 | warning = " (WARNING: Legacy server, could not autodetect settings. Auto-connect may not work reliably.)"; 436 | } 437 | 438 | printf( "[%c] %s %d.%d at %s%s\n", 439 | serverHotkey, 440 | pDiscoveredServer->serverDescription.szHostApp, 441 | pDiscoveredServer->serverDescription.HostAppVersion[0], 442 | pDiscoveredServer->serverDescription.HostAppVersion[1], 443 | pDiscoveredServer->serverAddress, 444 | warning ); 445 | 446 | g_discoveredServers.push_back( *pDiscoveredServer ); 447 | } 448 | 449 | // Establish a NatNet Client connection 450 | int ConnectClient() 451 | { 452 | // Release previous server 453 | g_pClient->Disconnect(); 454 | 455 | // Init Client and connect to NatNet server 456 | int retCode = g_pClient->Connect( g_connectParams ); 457 | if (retCode != ErrorCode_OK) 458 | { 459 | printf("Unable to connect to server. Error code: %d. Exiting", retCode); 460 | return ErrorCode_Internal; 461 | } 462 | else 463 | { 464 | // connection succeeded 465 | 466 | void* pResult; 467 | int nBytes = 0; 468 | ErrorCode ret = ErrorCode_OK; 469 | 470 | // print server info 471 | memset( &g_serverDescription, 0, sizeof( g_serverDescription ) ); 472 | ret = g_pClient->GetServerDescription( &g_serverDescription ); 473 | if ( ret != ErrorCode_OK || ! g_serverDescription.HostPresent ) 474 | { 475 | printf("Unable to connect to server. Host not present. Exiting."); 476 | return 1; 477 | } 478 | printf("\n[SampleClient] Server application info:\n"); 479 | printf("Application: %s (ver. %d.%d.%d.%d)\n", g_serverDescription.szHostApp, g_serverDescription.HostAppVersion[0], 480 | g_serverDescription.HostAppVersion[1], g_serverDescription.HostAppVersion[2], g_serverDescription.HostAppVersion[3]); 481 | printf("NatNet Version: %d.%d.%d.%d\n", g_serverDescription.NatNetVersion[0], g_serverDescription.NatNetVersion[1], 482 | g_serverDescription.NatNetVersion[2], g_serverDescription.NatNetVersion[3]); 483 | printf("Client IP:%s\n", g_connectParams.localAddress ); 484 | printf("Server IP:%s\n", g_connectParams.serverAddress ); 485 | printf("Server Name:%s\n", g_serverDescription.szHostComputerName); 486 | 487 | // get mocap frame rate 488 | ret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes); 489 | if (ret == ErrorCode_OK) 490 | { 491 | float fRate = *((float*)pResult); 492 | printf("Mocap Framerate : %3.2f\n", fRate); 493 | } 494 | else 495 | printf("Error getting frame rate.\n"); 496 | 497 | // get # of analog samples per mocap frame of data 498 | ret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame", &pResult, &nBytes); 499 | if (ret == ErrorCode_OK) 500 | { 501 | g_analogSamplesPerMocapFrame = *((int*)pResult); 502 | printf("Analog Samples Per Mocap Frame : %d\n", g_analogSamplesPerMocapFrame); 503 | } 504 | else 505 | printf("Error getting Analog frame rate.\n"); 506 | } 507 | 508 | return ErrorCode_OK; 509 | } 510 | 511 | // DataHandler receives data from the server 512 | // This function is called by NatNet when a frame of mocap data is available 513 | void NATNET_CALLCONV DataHandler(sFrameOfMocapData* data, void* pUserData) 514 | { 515 | NatNetClient* pClient = (NatNetClient*) pUserData; 516 | 517 | // Software latency here is defined as the span of time between: 518 | // a) The reception of a complete group of 2D frames from the camera system (CameraDataReceivedTimestamp) 519 | // and 520 | // b) The time immediately prior to the NatNet frame being transmitted over the network (TransmitTimestamp) 521 | // 522 | // This figure may appear slightly higher than the "software latency" reported in the Motive user interface, 523 | // because it additionally includes the time spent preparing to stream the data via NatNet. 524 | const uint64_t softwareLatencyHostTicks = data->TransmitTimestamp - data->CameraDataReceivedTimestamp; 525 | const double softwareLatencyMillisec = (softwareLatencyHostTicks * 1000) / static_cast(g_serverDescription.HighResClockFrequency); 526 | 527 | // Transit latency is defined as the span of time between Motive transmitting the frame of data, and its reception by the client (now). 528 | // The SecondsSinceHostTimestamp method relies on NatNetClient's internal clock synchronization with the server using Cristian's algorithm. 529 | const double transitLatencyMillisec = pClient->SecondsSinceHostTimestamp( data->TransmitTimestamp ) * 1000.0; 530 | 531 | if (g_outputFile) 532 | { 533 | _WriteFrame( g_outputFile, data ); 534 | } 535 | 536 | int i=0; 537 | 538 | printf("FrameID : %d\n", data->iFrame); 539 | printf("Timestamp : %3.2lf\n", data->fTimestamp); 540 | printf("Software latency : %.2lf milliseconds\n", softwareLatencyMillisec); 541 | 542 | // Only recent versions of the Motive software in combination with ethernet camera systems support system latency measurement. 543 | // If it's unavailable (for example, with USB camera systems, or during playback), this field will be zero. 544 | const bool bSystemLatencyAvailable = data->CameraMidExposureTimestamp != 0; 545 | 546 | if ( bSystemLatencyAvailable ) 547 | { 548 | // System latency here is defined as the span of time between: 549 | // a) The midpoint of the camera exposure window, and therefore the average age of the photons (CameraMidExposureTimestamp) 550 | // and 551 | // b) The time immediately prior to the NatNet frame being transmitted over the network (TransmitTimestamp) 552 | const uint64_t systemLatencyHostTicks = data->TransmitTimestamp - data->CameraMidExposureTimestamp; 553 | const double systemLatencyMillisec = (systemLatencyHostTicks * 1000) / static_cast(g_serverDescription.HighResClockFrequency); 554 | 555 | // Client latency is defined as the sum of system latency and the transit time taken to relay the data to the NatNet client. 556 | // This is the all-inclusive measurement (photons to client processing). 557 | const double clientLatencyMillisec = pClient->SecondsSinceHostTimestamp( data->CameraMidExposureTimestamp ) * 1000.0; 558 | 559 | // You could equivalently do the following (not accounting for time elapsed since we calculated transit latency above): 560 | //const double clientLatencyMillisec = systemLatencyMillisec + transitLatencyMillisec; 561 | 562 | printf( "System latency : %.2lf milliseconds\n", systemLatencyMillisec ); 563 | printf( "Total client latency : %.2lf milliseconds (transit time +%.2lf ms)\n", clientLatencyMillisec, transitLatencyMillisec ); 564 | } 565 | else 566 | { 567 | printf( "Transit latency : %.2lf milliseconds\n", transitLatencyMillisec ); 568 | } 569 | 570 | // FrameOfMocapData params 571 | bool bIsRecording = ((data->params & 0x01)!=0); 572 | bool bTrackedModelsChanged = ((data->params & 0x02)!=0); 573 | if(bIsRecording) 574 | printf("RECORDING\n"); 575 | if(bTrackedModelsChanged) 576 | printf("Models Changed.\n"); 577 | 578 | 579 | // timecode - for systems with an eSync and SMPTE timecode generator - decode to values 580 | int hour, minute, second, frame, subframe; 581 | NatNet_DecodeTimecode( data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe ); 582 | // decode to friendly string 583 | char szTimecode[128] = ""; 584 | NatNet_TimecodeStringify( data->Timecode, data->TimecodeSubframe, szTimecode, 128 ); 585 | printf("Timecode : %s\n", szTimecode); 586 | 587 | // Rigid Bodies 588 | printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies); 589 | for(i=0; i < data->nRigidBodies; i++) 590 | { 591 | // params 592 | // 0x01 : bool, rigid body was successfully tracked in this frame 593 | bool bTrackingValid = data->RigidBodies[i].params & 0x01; 594 | 595 | printf("Rigid Body [ID=%d Error=%3.2f Valid=%d]\n", data->RigidBodies[i].ID, data->RigidBodies[i].MeanError, bTrackingValid); 596 | printf("\tx\ty\tz\tqx\tqy\tqz\tqw\n"); 597 | printf("\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n", 598 | data->RigidBodies[i].x, 599 | data->RigidBodies[i].y, 600 | data->RigidBodies[i].z, 601 | data->RigidBodies[i].qx, 602 | data->RigidBodies[i].qy, 603 | data->RigidBodies[i].qz, 604 | data->RigidBodies[i].qw); 605 | } 606 | 607 | // Skeletons 608 | printf("Skeletons [Count=%d]\n", data->nSkeletons); 609 | for(i=0; i < data->nSkeletons; i++) 610 | { 611 | sSkeletonData skData = data->Skeletons[i]; 612 | printf("Skeleton [ID=%d Bone count=%d]\n", skData.skeletonID, skData.nRigidBodies); 613 | for(int j=0; j< skData.nRigidBodies; j++) 614 | { 615 | sRigidBodyData rbData = skData.RigidBodyData[j]; 616 | printf("Bone %d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n", 617 | rbData.ID, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw ); 618 | } 619 | } 620 | 621 | // labeled markers - this includes all markers (Active, Passive, and 'unlabeled' (markers with no asset but a PointCloud ID) 622 | bool bOccluded; // marker was not visible (occluded) in this frame 623 | bool bPCSolved; // reported position provided by point cloud solve 624 | bool bModelSolved; // reported position provided by model solve 625 | bool bHasModel; // marker has an associated asset in the data stream 626 | bool bUnlabeled; // marker is 'unlabeled', but has a point cloud ID that matches Motive PointCloud ID (In Motive 3D View) 627 | bool bActiveMarker; // marker is an actively labeled LED marker 628 | 629 | printf("Markers [Count=%d]\n", data->nLabeledMarkers); 630 | for(i=0; i < data->nLabeledMarkers; i++) 631 | { 632 | bOccluded = ((data->LabeledMarkers[i].params & 0x01)!=0); 633 | bPCSolved = ((data->LabeledMarkers[i].params & 0x02)!=0); 634 | bModelSolved = ((data->LabeledMarkers[i].params & 0x04) != 0); 635 | bHasModel = ((data->LabeledMarkers[i].params & 0x08) != 0); 636 | bUnlabeled = ((data->LabeledMarkers[i].params & 0x10) != 0); 637 | bActiveMarker = ((data->LabeledMarkers[i].params & 0x20) != 0); 638 | 639 | sMarker marker = data->LabeledMarkers[i]; 640 | 641 | // Marker ID Scheme: 642 | // Active Markers: 643 | // ID = ActiveID, correlates to RB ActiveLabels list 644 | // Passive Markers: 645 | // If Asset with Legacy Labels 646 | // AssetID (Hi Word) 647 | // MemberID (Lo Word) 648 | // Else 649 | // PointCloud ID 650 | int modelID, markerID; 651 | NatNet_DecodeID( marker.ID, &modelID, &markerID ); 652 | 653 | char szMarkerType[512]; 654 | if (bActiveMarker) 655 | strcpy(szMarkerType, "Active"); 656 | else if(bUnlabeled) 657 | strcpy(szMarkerType, "Unlabeled"); 658 | else 659 | strcpy(szMarkerType, "Labeled"); 660 | 661 | printf("%s Marker [ModelID=%d, MarkerID=%d, Occluded=%d, PCSolved=%d, ModelSolved=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n", 662 | szMarkerType, modelID, markerID, bOccluded, bPCSolved, bModelSolved, marker.size, marker.x, marker.y, marker.z); 663 | } 664 | 665 | // force plates 666 | printf("Force Plate [Count=%d]\n", data->nForcePlates); 667 | for(int iPlate=0; iPlate < data->nForcePlates; iPlate++) 668 | { 669 | printf("Force Plate %d\n", data->ForcePlates[iPlate].ID); 670 | for(int iChannel=0; iChannel < data->ForcePlates[iPlate].nChannels; iChannel++) 671 | { 672 | printf("\tChannel %d:\t", iChannel); 673 | if(data->ForcePlates[iPlate].ChannelData[iChannel].nFrames == 0) 674 | { 675 | printf("\tEmpty Frame\n"); 676 | } 677 | else if(data->ForcePlates[iPlate].ChannelData[iChannel].nFrames != g_analogSamplesPerMocapFrame) 678 | { 679 | printf("\tPartial Frame [Expected:%d Actual:%d]\n", g_analogSamplesPerMocapFrame, data->ForcePlates[iPlate].ChannelData[iChannel].nFrames); 680 | } 681 | for(int iSample=0; iSample < data->ForcePlates[iPlate].ChannelData[iChannel].nFrames; iSample++) 682 | printf("%3.2f\t", data->ForcePlates[iPlate].ChannelData[iChannel].Values[iSample]); 683 | printf("\n"); 684 | } 685 | } 686 | 687 | // devices 688 | printf("Device [Count=%d]\n", data->nDevices); 689 | for (int iDevice = 0; iDevice < data->nDevices; iDevice++) 690 | { 691 | printf("Device %d\n", data->Devices[iDevice].ID); 692 | for (int iChannel = 0; iChannel < data->Devices[iDevice].nChannels; iChannel++) 693 | { 694 | printf("\tChannel %d:\t", iChannel); 695 | if (data->Devices[iDevice].ChannelData[iChannel].nFrames == 0) 696 | { 697 | printf("\tEmpty Frame\n"); 698 | } 699 | else if (data->Devices[iDevice].ChannelData[iChannel].nFrames != g_analogSamplesPerMocapFrame) 700 | { 701 | printf("\tPartial Frame [Expected:%d Actual:%d]\n", g_analogSamplesPerMocapFrame, data->Devices[iDevice].ChannelData[iChannel].nFrames); 702 | } 703 | for (int iSample = 0; iSample < data->Devices[iDevice].ChannelData[iChannel].nFrames; iSample++) 704 | printf("%3.2f\t", data->Devices[iDevice].ChannelData[iChannel].Values[iSample]); 705 | printf("\n"); 706 | } 707 | } 708 | } 709 | 710 | 711 | // MessageHandler receives NatNet error/debug messages 712 | void NATNET_CALLCONV MessageHandler( Verbosity msgType, const char* msg ) 713 | { 714 | // Optional: Filter out debug messages 715 | if ( msgType < Verbosity_Info ) 716 | { 717 | return; 718 | } 719 | 720 | printf( "\n[NatNetLib]" ); 721 | 722 | switch ( msgType ) 723 | { 724 | case Verbosity_Debug: 725 | printf( " [DEBUG]" ); 726 | break; 727 | case Verbosity_Info: 728 | printf( " [INFO]" ); 729 | break; 730 | case Verbosity_Warning: 731 | printf( " [WARN]" ); 732 | break; 733 | case Verbosity_Error: 734 | printf( " [ERROR]" ); 735 | break; 736 | default: 737 | printf( " [?????]" ); 738 | break; 739 | } 740 | 741 | printf( ": %s\n", msg ); 742 | } 743 | 744 | 745 | /* File writing routines */ 746 | void _WriteHeader(FILE* fp, sDataDescriptions* pBodyDefs) 747 | { 748 | int i=0; 749 | 750 | if(!pBodyDefs->arrDataDescriptions[0].type == Descriptor_MarkerSet) 751 | return; 752 | 753 | sMarkerSetDescription* pMS = pBodyDefs->arrDataDescriptions[0].Data.MarkerSetDescription; 754 | 755 | fprintf(fp, "\n\n"); 756 | fprintf(fp, "\n%s\n\n\n", pMS->szName); 757 | 758 | fprintf(fp, "\n"); 759 | for(i=0; i < pMS->nMarkers; i++) 760 | { 761 | fprintf(fp, "%s\n", pMS->szMarkerNames[i]); 762 | } 763 | fprintf(fp, "\n\n"); 764 | 765 | fprintf(fp, "\n"); 766 | fprintf(fp, "Frame#\t"); 767 | for(i=0; i < pMS->nMarkers; i++) 768 | { 769 | fprintf(fp, "M%dX\tM%dY\tM%dZ\t", i, i, i); 770 | } 771 | fprintf(fp,"\n"); 772 | 773 | } 774 | 775 | 776 | void _WriteFrame(FILE* fp, sFrameOfMocapData* data) 777 | { 778 | fprintf(fp, "%d", data->iFrame); 779 | for(int i =0; i < data->MocapData->nMarkers; i++) 780 | { 781 | fprintf(fp, "\t%.5f\t%.5f\t%.5f", data->MocapData->Markers[i][0], data->MocapData->Markers[i][1], data->MocapData->Markers[i][2]); 782 | } 783 | fprintf(fp, "\n"); 784 | } 785 | 786 | 787 | void _WriteFooter(FILE* fp) 788 | { 789 | fprintf(fp, "\n\n"); 790 | fprintf(fp, "\n"); 791 | } 792 | 793 | 794 | void resetClient() 795 | { 796 | int iSuccess; 797 | 798 | printf("\n\nre-setting Client\n\n."); 799 | 800 | iSuccess = g_pClient->Disconnect(); 801 | if(iSuccess != 0) 802 | printf("error un-initting Client\n"); 803 | 804 | iSuccess = g_pClient->Connect( g_connectParams ); 805 | if(iSuccess != 0) 806 | printf("error re-initting Client\n"); 807 | } 808 | 809 | 810 | #ifndef _WIN32 811 | char getch() 812 | { 813 | char buf = 0; 814 | termios old = { 0 }; 815 | 816 | fflush( stdout ); 817 | 818 | if ( tcgetattr( 0, &old ) < 0 ) 819 | perror( "tcsetattr()" ); 820 | 821 | old.c_lflag &= ~ICANON; 822 | old.c_lflag &= ~ECHO; 823 | old.c_cc[VMIN] = 1; 824 | old.c_cc[VTIME] = 0; 825 | 826 | if ( tcsetattr( 0, TCSANOW, &old ) < 0 ) 827 | perror( "tcsetattr ICANON" ); 828 | 829 | if ( read( 0, &buf, 1 ) < 0 ) 830 | perror( "read()" ); 831 | 832 | old.c_lflag |= ICANON; 833 | old.c_lflag |= ECHO; 834 | 835 | if ( tcsetattr( 0, TCSADRAIN, &old ) < 0 ) 836 | perror( "tcsetattr ~ICANON" ); 837 | 838 | //printf( "%c\n", buf ); 839 | 840 | return buf; 841 | } 842 | #endif 843 | -------------------------------------------------------------------------------- /src/NatNetWrapper.cpp: -------------------------------------------------------------------------------- 1 | #include "NatNetWrapper.h" 2 | 3 | std::atomic quit(false); // signal flag 4 | 5 | NatNetWrapper* NatNetWrapper_ptr; 6 | void logCallbackWrapper( Verbosity level, const char* message ){ 7 | return NatNetWrapper_ptr->MessageHandler(level, message); 8 | } 9 | void dataCallbackWrapper( sFrameOfMocapData* pFrameOfData, void* pUserData ){ 10 | return NatNetWrapper_ptr->DataHandler(pFrameOfData, pUserData); 11 | } 12 | void serverCallbackWrapper( const sNatNetDiscoveredServer* pNewServer, void* pUserContext ){ 13 | return NatNetWrapper_ptr->ServerDiscoveredCallback(pNewServer, pUserContext); 14 | } 15 | 16 | NatNetWrapper::NatNetWrapper(){ 17 | NatNetWrapper_ptr = this; 18 | std::string message_type_; 19 | nh = ros::NodeHandle("~"); 20 | nh.param("frame_id", frame_id, "/world"); 21 | nh.param("message_type", message_type_, "pose"); 22 | nh.param("show_latency", show_latency, false); 23 | nh.param("publish_labeled_marker_pose_array", publish_labeled_marker_pose_array, false); 24 | nh.param("publish_unlabeled_marker_pose_array", publish_unlabeled_marker_pose_array, false); 25 | prefix = "/optitrack/"; 26 | verbose_level = Verbosity_Error + 1; // Do not listen NatNetlib message 27 | 28 | if(message_type_ == "pose"){ 29 | message_type = MessageType::POSE; 30 | } 31 | else if(message_type_ == "odom"){ 32 | message_type = MessageType::ODOMETRY; 33 | } 34 | else if(message_type_ == "tf"){ 35 | message_type = MessageType::TF; 36 | } 37 | else{ 38 | ROS_ERROR("[NatNetWrapper] Invalid message type. message type must be pose, twist, or tf"); 39 | throw std::invalid_argument("message_type"); 40 | } 41 | } 42 | 43 | int NatNetWrapper::run() { 44 | // print version info 45 | unsigned char ver[4]; 46 | NatNet_GetVersion( ver ); 47 | printf( "NatNetWrapper (NatNet ver. %d.%d.%d.%d)\n", ver[0], ver[1], ver[2], ver[3] ); 48 | 49 | // Install logging callback 50 | NatNet_SetLogCallback( logCallbackWrapper ); 51 | 52 | // create NatNet client 53 | g_pClient = new NatNetClient(); 54 | 55 | // set the frame callback handler 56 | g_pClient->SetFrameReceivedCallback( dataCallbackWrapper, g_pClient ); // this function will receive data from the server 57 | 58 | // If no arguments were specified on the command line, 59 | // attempt to discover servers on the local network. 60 | 61 | // Do asynchronous server discovery. 62 | printf( "Looking for servers on the local network.\n" ); 63 | is_ServerDiscovered = false; 64 | NatNetDiscoveryHandle discovery; 65 | NatNet_CreateAsyncServerDiscovery( &discovery, serverCallbackWrapper ); 66 | 67 | ros::Rate server_wait_rate(10); 68 | while(ros::ok() and !is_ServerDiscovered) { 69 | server_wait_rate.sleep(); 70 | } 71 | 72 | const size_t serverIndex = 0; 73 | 74 | const sNatNetDiscoveredServer& discoveredServer = g_discoveredServers[serverIndex]; 75 | 76 | if ( discoveredServer.serverDescription.bConnectionInfoValid ) 77 | { 78 | // Build the connection parameters. 79 | #ifdef _WIN32 80 | _snprintf_s( 81 | #else 82 | snprintf( 83 | #endif 84 | g_discoveredMulticastGroupAddr, sizeof g_discoveredMulticastGroupAddr, 85 | "%" PRIu8 ".%" PRIu8".%" PRIu8".%" PRIu8"", 86 | discoveredServer.serverDescription.ConnectionMulticastAddress[0], 87 | discoveredServer.serverDescription.ConnectionMulticastAddress[1], 88 | discoveredServer.serverDescription.ConnectionMulticastAddress[2], 89 | discoveredServer.serverDescription.ConnectionMulticastAddress[3] 90 | ); 91 | 92 | g_connectParams.connectionType = discoveredServer.serverDescription.ConnectionMulticast ? ConnectionType_Multicast : ConnectionType_Unicast; 93 | g_connectParams.serverCommandPort = discoveredServer.serverCommandPort; 94 | g_connectParams.serverDataPort = discoveredServer.serverDescription.ConnectionDataPort; 95 | g_connectParams.serverAddress = discoveredServer.serverAddress; 96 | g_connectParams.localAddress = discoveredServer.localAddress; 97 | g_connectParams.multicastAddress = g_discoveredMulticastGroupAddr; 98 | } 99 | else 100 | { 101 | // We're missing some info because it's a legacy server. 102 | // Guess the defaults and make a best effort attempt to connect. 103 | g_connectParams.connectionType = kDefaultConnectionType; 104 | g_connectParams.serverCommandPort = discoveredServer.serverCommandPort; 105 | g_connectParams.serverDataPort = 0; 106 | g_connectParams.serverAddress = discoveredServer.serverAddress; 107 | g_connectParams.localAddress = discoveredServer.localAddress; 108 | g_connectParams.multicastAddress = NULL; 109 | } 110 | 111 | NatNet_FreeAsyncServerDiscovery( discovery ); 112 | 113 | int iResult; 114 | 115 | // Connect to Motive 116 | iResult = ConnectClient(); 117 | if (iResult != ErrorCode_OK) 118 | { 119 | printf("Error initializing client. See log for details. Exiting"); 120 | return 1; 121 | } 122 | else 123 | { 124 | printf("Client initialized and ready.\n"); 125 | } 126 | 127 | 128 | // Send/receive test request 129 | void* response; 130 | int nBytes; 131 | // printf("[NatNetWrapper] Sending Test Request\n"); 132 | iResult = g_pClient->SendMessageAndWait("TestRequest", &response, &nBytes); 133 | if (iResult == ErrorCode_OK) 134 | { 135 | printf("[NatNetWrapper] Received: %s", (char*)response); 136 | } 137 | 138 | // Retrieve Data Descriptions from Motive 139 | printf("\n[NatNetWrapper] Requesting Data Descriptions...\n"); 140 | sDataDescriptions* pDataDefs = NULL; 141 | iResult = g_pClient->GetDataDescriptionList(&pDataDefs); 142 | if (iResult != ErrorCode_OK || pDataDefs == NULL) 143 | { 144 | printf("[NatNetWrapper] Unable to retrieve Data Descriptions."); 145 | } 146 | else 147 | { 148 | // printf("[NatNetWrapper] Received %d Data Descriptions:\n", pDataDefs->nDataDescriptions ); 149 | for(int i=0; i < pDataDefs->nDataDescriptions; i++) 150 | { 151 | // printf("Data Description # %d (type=%d)\n", i, pDataDefs->arrDataDescriptions[i].type); 152 | if(pDataDefs->arrDataDescriptions[i].type == Descriptor_MarkerSet) 153 | { 154 | // MarkerSet 155 | sMarkerSetDescription* pMS = pDataDefs->arrDataDescriptions[i].Data.MarkerSetDescription; 156 | printf("MarkerSet Name : %s\n", pMS->szName); 157 | for(int j = 0; j < pMS->nMarkers; j++) 158 | printf("%s\n", pMS->szMarkerNames[j]); 159 | 160 | } 161 | if(pDataDefs->arrDataDescriptions[i].type == Descriptor_RigidBody) 162 | { 163 | // RigidBody 164 | sRigidBodyDescription* pRB = pDataDefs->arrDataDescriptions[i].Data.RigidBodyDescription; 165 | printf("RigidBody Name : %s\n", pRB->szName); 166 | printf("RigidBody ID : %d\n", pRB->ID); 167 | // printf("RigidBody Parent ID : %d\n", pRB->parentID); 168 | // printf("Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx, pRB->offsety, pRB->offsetz); 169 | 170 | model_ids.emplace_back(pRB->ID); 171 | model_names.emplace_back(std::string(pRB->szName)); 172 | 173 | // Initialize publisher by message type 174 | if(message_type == MessageType::POSE){ 175 | pubs_vision_pose.emplace_back(nh.advertise(prefix + std::string(pRB->szName) + "/poseStamped", 10)); 176 | } 177 | else if(message_type == MessageType::ODOMETRY){ 178 | pubs_vision_odom.emplace_back(nh.advertise(prefix + std::string(pRB->szName) + "/odometry", 10)); 179 | linearKalmanFilters.emplace_back(std::make_unique()); 180 | } 181 | 182 | if(publish_labeled_marker_pose_array){ 183 | pubs_labeled_marker_pose_array.emplace_back(nh.advertise(prefix + std::string(pRB->szName) + "/markerPoseArray", 10)); 184 | } 185 | 186 | // if ( pRB->MarkerPositions != NULL && pRB->MarkerRequiredLabels != NULL ) 187 | // { 188 | // for ( int markerIdx = 0; markerIdx < pRB->nMarkers; ++markerIdx ) 189 | // { 190 | // const MarkerData& markerPosition = pRB->MarkerPositions[markerIdx]; 191 | // const int markerRequiredLabel = pRB->MarkerRequiredLabels[markerIdx]; 192 | // 193 | // printf( "\tMarker #%d:\n", markerIdx ); 194 | // printf( "\t\tPosition: %.2f, %.2f, %.2f\n", markerPosition[0], markerPosition[1], markerPosition[2] ); 195 | // 196 | // if ( markerRequiredLabel != 0 ) 197 | // { 198 | // printf( "\t\tRequired active label: %d\n", markerRequiredLabel ); 199 | // } 200 | // } 201 | // } 202 | } 203 | // else if(pDataDefs->arrDataDescriptions[i].type == Descriptor_Skeleton) 204 | // { 205 | // // Skeleton 206 | // sSkeletonDescription* pSK = pDataDefs->arrDataDescriptions[i].Data.SkeletonDescription; 207 | // printf("Skeleton Name : %s\n", pSK->szName); 208 | // printf("Skeleton ID : %d\n", pSK->skeletonID); 209 | // printf("RigidBody (Bone) Count : %d\n", pSK->nRigidBodies); 210 | // for(int j=0; j < pSK->nRigidBodies; j++) 211 | // { 212 | // sRigidBodyDescription* pRB = &pSK->RigidBodies[j]; 213 | // printf(" RigidBody Name : %s\n", pRB->szName); 214 | // printf(" RigidBody ID : %d\n", pRB->ID); 215 | // printf(" RigidBody Parent ID : %d\n", pRB->parentID); 216 | // printf(" Parent Offset : %3.2f,%3.2f,%3.2f\n", pRB->offsetx, pRB->offsety, pRB->offsetz); 217 | // } 218 | // } 219 | // else if(pDataDefs->arrDataDescriptions[i].type == Descriptor_ForcePlate) 220 | // { 221 | // // Force Plate 222 | // sForcePlateDescription* pFP = pDataDefs->arrDataDescriptions[i].Data.ForcePlateDescription; 223 | // printf("Force Plate ID : %d\n", pFP->ID); 224 | // printf("Force Plate Serial : %s\n", pFP->strSerialNo); 225 | // printf("Force Plate Width : %3.2f\n", pFP->fWidth); 226 | // printf("Force Plate Length : %3.2f\n", pFP->fLength); 227 | // printf("Force Plate Electrical Center Offset (%3.3f, %3.3f, %3.3f)\n", pFP->fOriginX,pFP->fOriginY, pFP->fOriginZ); 228 | // for(int iCorner=0; iCorner<4; iCorner++) 229 | // printf("Force Plate Corner %d : (%3.4f, %3.4f, %3.4f)\n", iCorner, pFP->fCorners[iCorner][0],pFP->fCorners[iCorner][1],pFP->fCorners[iCorner][2]); 230 | // printf("Force Plate Type : %d\n", pFP->iPlateType); 231 | // printf("Force Plate Data Type : %d\n", pFP->iChannelDataType); 232 | // printf("Force Plate Channel Count : %d\n", pFP->nChannels); 233 | // for(int iChannel=0; iChannelnChannels; iChannel++) 234 | // printf("\tChannel %d : %s\n", iChannel, pFP->szChannelNames[iChannel]); 235 | // } 236 | // else if (pDataDefs->arrDataDescriptions[i].type == Descriptor_Device) 237 | // { 238 | // // Peripheral Device 239 | // sDeviceDescription* pDevice = pDataDefs->arrDataDescriptions[i].Data.DeviceDescription; 240 | // printf("Device Name : %s\n", pDevice->strName); 241 | // printf("Device Serial : %s\n", pDevice->strSerialNo); 242 | // printf("Device ID : %d\n", pDevice->ID); 243 | // printf("Device Channel Count : %d\n", pDevice->nChannels); 244 | // for (int iChannel = 0; iChannel < pDevice->nChannels; iChannel++) 245 | // printf("\tChannel %d : %s\n", iChannel, pDevice->szChannelNames[iChannel]); 246 | // } 247 | // else 248 | // { 249 | // printf("Unknown data type."); 250 | // // Unknown 251 | // } 252 | } 253 | if(publish_unlabeled_marker_pose_array){ 254 | pub_unlabeled_marker_pose_array = nh.advertise(prefix + "unlabeled/markerPoseArray", 10); 255 | } 256 | } 257 | 258 | // Ready to receive marker stream! 259 | signal(SIGINT, NatNetWrapper::sigintCallback); 260 | printf("\nClient is connected to server and listening for data...\n"); 261 | int c; 262 | bool bExit = false; 263 | ros::Rate main_rate(100); 264 | while(ros::ok()) 265 | { 266 | // c=getch(); 267 | // switch(c) 268 | // { 269 | // case 'q': 270 | // bExit = true; 271 | // break; 272 | // case 'r': 273 | // resetClient(); 274 | // break; 275 | // case 'p': 276 | // sServerDescription ServerDescription; 277 | // memset(&ServerDescription, 0, sizeof(ServerDescription)); 278 | // g_pClient->GetServerDescription(&ServerDescription); 279 | // if(!ServerDescription.HostPresent) 280 | // { 281 | // printf("Unable to connect to server. Host not present. Exiting."); 282 | // return 1; 283 | // } 284 | // break; 285 | // case 's': 286 | // { 287 | // printf("\n\n[NatNetWrapper] Requesting Data Descriptions..."); 288 | // sDataDescriptions* pDataDefs = NULL; 289 | // iResult = g_pClient->GetDataDescriptionList(&pDataDefs); 290 | // if (iResult != ErrorCode_OK || pDataDefs == NULL) 291 | // { 292 | // printf("[NatNetWrapper] Unable to retrieve Data Descriptions."); 293 | // } 294 | // else 295 | // { 296 | // printf("[NatNetWrapper] Received %d Data Descriptions:\n", pDataDefs->nDataDescriptions); 297 | // } 298 | // } 299 | // break; 300 | // case 'm': // change to multicast 301 | // g_connectParams.connectionType = ConnectionType_Multicast; 302 | // iResult = ConnectClient(); 303 | // if(iResult == ErrorCode_OK) 304 | // printf("Client connection type changed to Multicast.\n\n"); 305 | // else 306 | // printf("Error changing client connection type to Multicast.\n\n"); 307 | // break; 308 | // case 'u': // change to unicast 309 | // g_connectParams.connectionType = ConnectionType_Unicast; 310 | // iResult = ConnectClient(); 311 | // if(iResult == ErrorCode_OK) 312 | // printf("Client connection type changed to Unicast.\n\n"); 313 | // else 314 | // printf("Error changing client connection type to Unicast.\n\n"); 315 | // break; 316 | // case 'c' : // connect 317 | // iResult = ConnectClient(); 318 | // break; 319 | // case 'd' : // disconnect 320 | // // note: applies to unicast connections only - indicates to Motive to stop sending packets to that client endpoint 321 | // iResult = g_pClient->SendMessageAndWait("Disconnect", &response, &nBytes); 322 | // if (iResult == ErrorCode_OK) 323 | // printf("[NatNetWrapper] Disconnected"); 324 | // break; 325 | // default: 326 | // break; 327 | // } 328 | // if(bExit) 329 | // break; 330 | 331 | if(quit.load()) 332 | break; 333 | 334 | main_rate.sleep(); 335 | } 336 | 337 | // Done - clean up. 338 | if (g_pClient) 339 | { 340 | g_pClient->Disconnect(); 341 | delete g_pClient; 342 | g_pClient = NULL; 343 | } 344 | 345 | return ErrorCode_OK; 346 | } 347 | 348 | // DataHandler receives data from the server 349 | // This function is called by NatNet when a frame of mocap data is available 350 | void NATNET_CALLCONV NatNetWrapper::DataHandler(sFrameOfMocapData* data, void* pUserData) 351 | { 352 | NatNetClient* pClient = (NatNetClient*) pUserData; 353 | 354 | // Software latency here is defined as the span of time between: 355 | // a) The reception of a complete group of 2D frames from the camera system (CameraDataReceivedTimestamp) 356 | // and 357 | // b) The time immediately prior to the NatNet frame being transmitted over the network (TransmitTimestamp) 358 | // 359 | // This figure may appear slightly higher than the "software latency" reported in the Motive user interface, 360 | // because it additionally includes the time spent preparing to stream the data via NatNet. 361 | const uint64_t softwareLatencyHostTicks = data->TransmitTimestamp - data->CameraDataReceivedTimestamp; 362 | const double softwareLatencyMillisec = (softwareLatencyHostTicks * 1000) / static_cast(g_serverDescription.HighResClockFrequency); 363 | 364 | // Transit latency is defined as the span of time between Motive transmitting the frame of data, and its reception by the client (now). 365 | // The SecondsSinceHostTimestamp method relies on NatNetClient's internal clock synchronization with the server using Cristian's algorithm. 366 | const double transitLatencyMillisec = pClient->SecondsSinceHostTimestamp( data->TransmitTimestamp ) * 1000.0; 367 | 368 | int i=0; 369 | 370 | // printf("FrameID : %d\n", data->iFrame); 371 | // printf("Timestamp : %3.2lf\n", data->fTimestamp); 372 | 373 | 374 | // Only recent versions of the Motive software in combination with ethernet camera systems support system latency measurement. 375 | // If it's unavailable (for example, with USB camera systems, or during playback), this field will be zero. 376 | const bool bSystemLatencyAvailable = data->CameraMidExposureTimestamp != 0; 377 | 378 | if(show_latency) { 379 | if (bSystemLatencyAvailable) { 380 | // System latency here is defined as the span of time between: 381 | // a) The midpoint of the camera exposure window, and therefore the average age of the photons (CameraMidExposureTimestamp) 382 | // and 383 | // b) The time immediately prior to the NatNet frame being transmitted over the network (TransmitTimestamp) 384 | const uint64_t systemLatencyHostTicks = data->TransmitTimestamp - data->CameraMidExposureTimestamp; 385 | const double systemLatencyMillisec = 386 | (systemLatencyHostTicks * 1000) / static_cast(g_serverDescription.HighResClockFrequency); 387 | 388 | // Client latency is defined as the sum of system latency and the transit time taken to relay the data to the NatNet client. 389 | // This is the all-inclusive measurement (photons to client processing). 390 | const double clientLatencyMillisec = 391 | pClient->SecondsSinceHostTimestamp(data->CameraMidExposureTimestamp) * 1000.0; 392 | 393 | // You could equivalently do the following (not accounting for time elapsed since we calculated transit latency above): 394 | //const double clientLatencyMillisec = systemLatencyMillisec + transitLatencyMillisec; 395 | 396 | printf("Software latency : %.2lf milliseconds\n", softwareLatencyMillisec); 397 | printf("System latency : %.2lf milliseconds\n", systemLatencyMillisec); 398 | printf("Total client latency : %.2lf milliseconds (transit time +%.2lf ms)\n", clientLatencyMillisec, 399 | transitLatencyMillisec); 400 | } else { 401 | printf("Transit latency : %.2lf milliseconds\n", transitLatencyMillisec); 402 | } 403 | } 404 | 405 | // FrameOfMocapData params 406 | bool bIsRecording = ((data->params & 0x01)!=0); 407 | bool bTrackedModelsChanged = ((data->params & 0x02)!=0); 408 | if(bIsRecording) 409 | printf("RECORDING\n"); 410 | if(bTrackedModelsChanged) 411 | printf("Models Changed.\n"); 412 | 413 | 414 | // timecode - for systems with an eSync and SMPTE timecode generator - decode to values 415 | int hour, minute, second, frame, subframe; 416 | NatNet_DecodeTimecode( data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe ); 417 | // decode to friendly string 418 | char szTimecode[128] = ""; 419 | NatNet_TimecodeStringify( data->Timecode, data->TimecodeSubframe, szTimecode, 128 ); 420 | // printf("Timecode : %s\n", szTimecode); 421 | 422 | // Rigid Bodies 423 | // printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies); 424 | 425 | 426 | for(i = 0; i < data->nRigidBodies; i++) { 427 | if(model_ids.size() != data->nRigidBodies){ // Do not publish when number of data is not matched 428 | break; 429 | } 430 | 431 | // params 432 | // 0x01 : bool, rigid body was successfully tracked in this frame 433 | bool bTrackingValid = data->RigidBodies[i].params & 0x01; 434 | // printf("Rigid Body [ID=%d Error=%3.2f Valid=%d]\n", data->RigidBodies[i].ID, data->RigidBodies[i].MeanError, bTrackingValid); 435 | if (bTrackingValid) { 436 | switch (message_type){ 437 | case POSE: 438 | publishPose(i, data->RigidBodies[i]); 439 | break; 440 | case ODOMETRY: 441 | publishOdom(i, data->RigidBodies[i]); 442 | break; 443 | case TF: 444 | publishTF(i, data->RigidBodies[i]); 445 | break; 446 | default: 447 | break; 448 | } 449 | } else { 450 | ROS_WARN_STREAM("[NatNetWrapper] " << model_names[i] << " is not published"); 451 | } 452 | } 453 | 454 | // // Skeletons 455 | // printf("Skeletons [Count=%d]\n", data->nSkeletons); 456 | // for(i=0; i < data->nSkeletons; i++) 457 | // { 458 | // sSkeletonData skData = data->Skeletons[i]; 459 | // printf("Skeleton [ID=%d Bone count=%d]\n", skData.skeletonID, skData.nRigidBodies); 460 | // for(int j=0; j< skData.nRigidBodies; j++) 461 | // { 462 | // sRigidBodyData rbData = skData.RigidBodyData[j]; 463 | // printf("Bone %d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n", 464 | // rbData.ID, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw ); 465 | // } 466 | // } 467 | 468 | // labeled markers - this includes all markers (Active, Passive, and 'unlabeled' (markers with no asset but a PointCloud ID) 469 | if(publish_labeled_marker_pose_array || publish_unlabeled_marker_pose_array){ 470 | std::vector labeled_marker_pose_array; 471 | labeled_marker_pose_array.resize(model_ids.size()); 472 | geometry_msgs::PoseArray unlabeled_marker_pose_array; 473 | bool bOccluded; // marker was not visible (occluded) in this frame 474 | bool bPCSolved; // reported position provided by point cloud solve 475 | bool bModelSolved; // reported position provided by model solve 476 | bool bHasModel; // marker has an associated asset in the data stream 477 | bool bUnlabeled; // marker is 'unlabeled', but has a point cloud ID that matches Motive PointCloud ID (In Motive 3D View) 478 | bool bActiveMarker; // marker is an actively labeled LED marker 479 | 480 | // printf("Markers [Count=%d]\n", data->nLabeledMarkers); 481 | for(i=0; i < data->nLabeledMarkers; i++) { 482 | bOccluded = ((data->LabeledMarkers[i].params & 0x01) != 0); 483 | bPCSolved = ((data->LabeledMarkers[i].params & 0x02) != 0); 484 | bModelSolved = ((data->LabeledMarkers[i].params & 0x04) != 0); 485 | bHasModel = ((data->LabeledMarkers[i].params & 0x08) != 0); 486 | bUnlabeled = ((data->LabeledMarkers[i].params & 0x10) != 0); 487 | bActiveMarker = ((data->LabeledMarkers[i].params & 0x20) != 0); 488 | 489 | sMarker marker = data->LabeledMarkers[i]; 490 | 491 | // Marker ID Scheme: 492 | // Active Markers: 493 | // ID = ActiveID, correlates to RB ActiveLabels list 494 | // Passive Markers: 495 | // If Asset with Legacy Labels 496 | // AssetID (Hi Word) 497 | // MemberID (Lo Word) 498 | // Else 499 | // PointCloud ID 500 | int modelID, markerID; 501 | NatNet_DecodeID(marker.ID, &modelID, &markerID); 502 | 503 | // char szMarkerType[512]; 504 | // if (bActiveMarker) 505 | // strcpy(szMarkerType, "Active"); 506 | // else if(bUnlabeled) 507 | // strcpy(szMarkerType, "Unlabeled"); 508 | // else 509 | // strcpy(szMarkerType, "Labeled"); 510 | // 511 | // printf("%s Marker [ModelID=%d, MarkerID=%d, Occluded=%d, PCSolved=%d, ModelSolved=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n", 512 | // szMarkerType, modelID, markerID, bOccluded, bPCSolved, bModelSolved, marker.size, marker.x, marker.y, marker.z); 513 | 514 | geometry_msgs::Pose marker_pose; 515 | marker_pose.position.x = marker.x; 516 | marker_pose.position.y = marker.y; 517 | marker_pose.position.z = marker.z; 518 | marker_pose.orientation.x = 0; 519 | marker_pose.orientation.y = 0; 520 | marker_pose.orientation.z = 0; 521 | marker_pose.orientation.w = 1; 522 | 523 | if (bActiveMarker) { 524 | //TODO: ActiveMarker 525 | } else if (bUnlabeled && publish_unlabeled_marker_pose_array) { 526 | unlabeled_marker_pose_array.header.stamp = ros::Time::now(); 527 | unlabeled_marker_pose_array.header.frame_id = frame_id; 528 | unlabeled_marker_pose_array.poses.push_back(marker_pose); 529 | } else if(publish_labeled_marker_pose_array){ 530 | int labeled_marker_index = -1; 531 | for(int i_model = 0; i_model < model_ids.size(); i_model++){ 532 | if(modelID == model_ids[i_model]){ 533 | labeled_marker_index = i_model; 534 | break; 535 | } 536 | } 537 | if(labeled_marker_index == -1){ 538 | // data is not ready 539 | break; 540 | } 541 | 542 | labeled_marker_pose_array[labeled_marker_index].header.stamp = ros::Time::now(); 543 | labeled_marker_pose_array[labeled_marker_index].header.frame_id = frame_id; 544 | labeled_marker_pose_array[labeled_marker_index].poses.emplace_back(marker_pose); 545 | } 546 | } 547 | if(publish_labeled_marker_pose_array && !pubs_labeled_marker_pose_array.empty()){ 548 | for(int i_model=0; i_model < model_ids.size(); i_model++){ 549 | pubs_labeled_marker_pose_array[i_model].publish(labeled_marker_pose_array[i_model]); 550 | } 551 | } 552 | if(publish_unlabeled_marker_pose_array){ 553 | pub_unlabeled_marker_pose_array.publish(unlabeled_marker_pose_array); 554 | } 555 | } 556 | 557 | // // force plates 558 | // printf("Force Plate [Count=%d]\n", data->nForcePlates); 559 | // for(int iPlate=0; iPlate < data->nForcePlates; iPlate++) 560 | // { 561 | // printf("Force Plate %d\n", data->ForcePlates[iPlate].ID); 562 | // for(int iChannel=0; iChannel < data->ForcePlates[iPlate].nChannels; iChannel++) 563 | // { 564 | // printf("\tChannel %d:\t", iChannel); 565 | // if(data->ForcePlates[iPlate].ChannelData[iChannel].nFrames == 0) 566 | // { 567 | // printf("\tEmpty Frame\n"); 568 | // } 569 | // else if(data->ForcePlates[iPlate].ChannelData[iChannel].nFrames != g_analogSamplesPerMocapFrame) 570 | // { 571 | // printf("\tPartial Frame [Expected:%d Actual:%d]\n", g_analogSamplesPerMocapFrame, data->ForcePlates[iPlate].ChannelData[iChannel].nFrames); 572 | // } 573 | // for(int iSample=0; iSample < data->ForcePlates[iPlate].ChannelData[iChannel].nFrames; iSample++) 574 | // printf("%3.2f\t", data->ForcePlates[iPlate].ChannelData[iChannel].Values[iSample]); 575 | // printf("\n"); 576 | // } 577 | // } 578 | // 579 | // // devices 580 | // printf("Device [Count=%d]\n", data->nDevices); 581 | // for (int iDevice = 0; iDevice < data->nDevices; iDevice++) 582 | // { 583 | // printf("Device %d\n", data->Devices[iDevice].ID); 584 | // for (int iChannel = 0; iChannel < data->Devices[iDevice].nChannels; iChannel++) 585 | // { 586 | // printf("\tChannel %d:\t", iChannel); 587 | // if (data->Devices[iDevice].ChannelData[iChannel].nFrames == 0) 588 | // { 589 | // printf("\tEmpty Frame\n"); 590 | // } 591 | // else if (data->Devices[iDevice].ChannelData[iChannel].nFrames != g_analogSamplesPerMocapFrame) 592 | // { 593 | // printf("\tPartial Frame [Expected:%d Actual:%d]\n", g_analogSamplesPerMocapFrame, data->Devices[iDevice].ChannelData[iChannel].nFrames); 594 | // } 595 | // for (int iSample = 0; iSample < data->Devices[iDevice].ChannelData[iChannel].nFrames; iSample++) 596 | // printf("%3.2f\t", data->Devices[iDevice].ChannelData[iChannel].Values[iSample]); 597 | // printf("\n"); 598 | // } 599 | // } 600 | } 601 | 602 | // MessageHandler receives NatNet error/debug messages 603 | void NATNET_CALLCONV NatNetWrapper::MessageHandler( Verbosity msgType, const char* msg ) 604 | { 605 | 606 | // Optional: Filter out debug messages 607 | if ( msgType < verbose_level ) 608 | { 609 | return; 610 | } 611 | 612 | printf( "\n[NatNetLib]" ); 613 | 614 | switch ( msgType ) 615 | { 616 | case Verbosity_Debug: 617 | printf( " [DEBUG]" ); 618 | break; 619 | case Verbosity_Info: 620 | printf( " [INFO]" ); 621 | break; 622 | case Verbosity_Warning: 623 | printf( " [WARN]" ); 624 | break; 625 | case Verbosity_Error: 626 | printf( " [ERROR]" ); 627 | break; 628 | default: 629 | printf( " [?????]" ); 630 | break; 631 | } 632 | 633 | printf( ": %s\n", msg ); 634 | } 635 | 636 | void NATNET_CALLCONV NatNetWrapper::ServerDiscoveredCallback( const sNatNetDiscoveredServer* pDiscoveredServer, void* pUserContext ) 637 | { 638 | char serverHotkey = '.'; 639 | if ( g_discoveredServers.size() < 9 ) 640 | { 641 | serverHotkey = static_cast('1' + g_discoveredServers.size()); 642 | } 643 | 644 | const char* warning = ""; 645 | 646 | if ( pDiscoveredServer->serverDescription.bConnectionInfoValid == false ) 647 | { 648 | warning = " (WARNING: Legacy server, could not autodetect settings. Auto-connect may not work reliably.)"; 649 | } 650 | 651 | printf( "[%c] %s %d.%d at %s%s\n", 652 | serverHotkey, 653 | pDiscoveredServer->serverDescription.szHostApp, 654 | pDiscoveredServer->serverDescription.HostAppVersion[0], 655 | pDiscoveredServer->serverDescription.HostAppVersion[1], 656 | pDiscoveredServer->serverAddress, 657 | warning ); 658 | 659 | g_discoveredServers.push_back( *pDiscoveredServer ); 660 | is_ServerDiscovered = true; //TODO: mutex to protect this 661 | } 662 | 663 | // Establish a NatNet Client connection 664 | int NatNetWrapper::ConnectClient() 665 | { 666 | // Release previous server 667 | g_pClient->Disconnect(); 668 | 669 | // Init Client and connect to NatNet server 670 | int retCode = g_pClient->Connect( g_connectParams ); 671 | if (retCode != ErrorCode_OK) 672 | { 673 | printf("Unable to connect to server. Error code: %d. Exiting", retCode); 674 | return ErrorCode_Internal; 675 | } 676 | else 677 | { 678 | // connection succeeded 679 | 680 | void* pResult; 681 | int nBytes = 0; 682 | ErrorCode ret = ErrorCode_OK; 683 | 684 | // print server info 685 | memset( &g_serverDescription, 0, sizeof( g_serverDescription ) ); 686 | ret = g_pClient->GetServerDescription( &g_serverDescription ); 687 | if ( ret != ErrorCode_OK || ! g_serverDescription.HostPresent ) 688 | { 689 | printf("Unable to connect to server. Host not present. Exiting."); 690 | return 1; 691 | } 692 | printf("\n[NatNetWrapper] Server application info:\n"); 693 | printf("Application: %s (ver. %d.%d.%d.%d)\n", g_serverDescription.szHostApp, g_serverDescription.HostAppVersion[0], 694 | g_serverDescription.HostAppVersion[1], g_serverDescription.HostAppVersion[2], g_serverDescription.HostAppVersion[3]); 695 | printf("NatNet Version: %d.%d.%d.%d\n", g_serverDescription.NatNetVersion[0], g_serverDescription.NatNetVersion[1], 696 | g_serverDescription.NatNetVersion[2], g_serverDescription.NatNetVersion[3]); 697 | printf("Client IP:%s\n", g_connectParams.localAddress ); 698 | printf("Server IP:%s\n", g_connectParams.serverAddress ); 699 | printf("Server Name:%s\n", g_serverDescription.szHostComputerName); 700 | 701 | // get mocap frame rate 702 | ret = g_pClient->SendMessageAndWait("FrameRate", &pResult, &nBytes); 703 | if (ret == ErrorCode_OK) 704 | { 705 | float fRate = *((float*)pResult); 706 | printf("Mocap Framerate : %3.2f\n", fRate); 707 | } 708 | else 709 | printf("Error getting frame rate.\n"); 710 | 711 | // // get # of analog samples per mocap frame of data 712 | // ret = g_pClient->SendMessageAndWait("AnalogSamplesPerMocapFrame", &pResult, &nBytes); 713 | // if (ret == ErrorCode_OK) 714 | // { 715 | // g_analogSamplesPerMocapFrame = *((int*)pResult); 716 | // printf("Analog Samples Per Mocap Frame : %d\n", g_analogSamplesPerMocapFrame); 717 | // } 718 | // else 719 | // printf("Error getting Analog frame rate.\n"); 720 | } 721 | 722 | return ErrorCode_OK; 723 | } 724 | 725 | void NatNetWrapper::sigintCallback(int signum){ 726 | quit.store(true); 727 | } 728 | 729 | void NatNetWrapper::resetClient() 730 | { 731 | int iSuccess; 732 | 733 | printf("\n\nre-setting Client\n\n."); 734 | 735 | iSuccess = g_pClient->Disconnect(); 736 | if(iSuccess != 0) 737 | printf("error un-initting Client\n"); 738 | 739 | iSuccess = g_pClient->Connect( g_connectParams ); 740 | if(iSuccess != 0) 741 | printf("error re-initting Client\n"); 742 | } 743 | 744 | #ifndef _WIN32 745 | char NatNetWrapper::getch() 746 | { 747 | char buf = 0; 748 | termios old = { 0 }; 749 | 750 | fflush( stdout ); 751 | 752 | if ( tcgetattr( 0, &old ) < 0 ) 753 | perror( "tcsetattr()" ); 754 | 755 | old.c_lflag &= ~ICANON; 756 | old.c_lflag &= ~ECHO; 757 | old.c_cc[VMIN] = 1; 758 | old.c_cc[VTIME] = 0; 759 | 760 | if ( tcsetattr( 0, TCSANOW, &old ) < 0 ) 761 | perror( "tcsetattr ICANON" ); 762 | 763 | if ( read( 0, &buf, 1 ) < 0 ) 764 | perror( "read()" ); 765 | 766 | old.c_lflag |= ICANON; 767 | old.c_lflag |= ECHO; 768 | 769 | if ( tcsetattr( 0, TCSADRAIN, &old ) < 0 ) 770 | perror( "tcsetattr ~ICANON" ); 771 | 772 | //printf( "%c\n", buf ); 773 | 774 | return buf; 775 | } 776 | 777 | geometry_msgs::PoseStamped NatNetWrapper::rigidBodyToPose(const sRigidBodyData& rigid_body_data) { 778 | geometry_msgs::PoseStamped pose; 779 | pose.pose.position.x = rigid_body_data.x; 780 | pose.pose.position.y = rigid_body_data.y; 781 | pose.pose.position.z = rigid_body_data.z; 782 | pose.pose.orientation.x = rigid_body_data.qx; 783 | pose.pose.orientation.y = rigid_body_data.qy; 784 | pose.pose.orientation.z = rigid_body_data.qz; 785 | pose.pose.orientation.w = rigid_body_data.qw; 786 | return pose; 787 | } 788 | 789 | void NatNetWrapper::publishPose(int idx, const sRigidBodyData& rigid_body_data) { 790 | geometry_msgs::PoseStamped pose = rigidBodyToPose(rigid_body_data); 791 | pose.header.stamp = ros::Time::now(); 792 | pose.header.frame_id = frame_id; 793 | pubs_vision_pose[idx].publish(pose); 794 | } 795 | 796 | void NatNetWrapper::publishOdom(int idx, const sRigidBodyData& rigid_body_data) { 797 | geometry_msgs::PoseStamped pose = rigidBodyToPose(rigid_body_data); 798 | nav_msgs::Odometry vision_odom = linearKalmanFilters[idx].get()->pose_cb(pose); 799 | vision_odom.header.stamp = ros::Time::now(); 800 | vision_odom.header.frame_id = frame_id; 801 | vision_odom.child_frame_id = frame_id; 802 | pubs_vision_odom[idx].publish(vision_odom); 803 | } 804 | 805 | void NatNetWrapper::publishTF(int idx, const sRigidBodyData& rigid_body_data) { 806 | tf::Transform transform; 807 | transform.setOrigin(tf::Vector3(rigid_body_data.x, rigid_body_data.y, rigid_body_data.z)); 808 | tf::Quaternion q(rigid_body_data.qx, rigid_body_data.qy, rigid_body_data.qz, rigid_body_data.qw); 809 | transform.setRotation(q); 810 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, model_names[idx])); 811 | } 812 | 813 | 814 | #endif 815 | -------------------------------------------------------------------------------- /.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | execut 176 | ServerDiscoveredCallback 177 | NatNetFrameReceivedCallback 178 | error 179 | CONNECTCLI 180 | nRi 181 | prefix 182 | Total cl 183 | library 184 | dataCallbackWrapper 185 | publish 186 | software latency 187 | soft 188 | Marker 189 | analogsample 190 | NatNetlib 191 | 192 | 193 | 194 | 205 | 206 | 207 | 208 | 209 | true 210 | DEFINITION_ORDER 211 | 212 | 213 | 214 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 |