bool shutdown_vicon() { ROS_INFO_STREAM("Disconnecting from Vicon DataStream SDK"); MyClient.Disconnect(); ROS_ASSERT(!MyClient.IsConnected().Connected); ROS_INFO_STREAM("... disconnected."); return true; }
// mocap position thread void DataStreamClient::run(void) { while (true) //TODO: exit cleanly? { // get frame while (_vicon_client.GetFrame().Result != Result::Success) { dbg("Couldn't GetFrame()\n"); usleep(1000); } // get timecode and populate subject message // Output_GetTimecode time_code = _vicon_client.GetTimecode(); vicon::body_t msg; msg.utime = _timestamp_now(); vicon::vicon_t system; // get subject count system.nummodels = _vicon_client.GetSubjectCount().SubjectCount; // Uh oh, models disappeared - realloc the system if (system.nummodels < system.models.size()) { system.models.clear(); system.models.reserve(system.nummodels); } if (system.nummodels == 0) { std::cout << "no models detected" << std::endl; continue; } // loop through subjects for (uint subject_index = 0; subject_index < system.nummodels; subject_index++) { //////////////////////////////////////////////////////////////////////// /// Marker vicon::model_t& model = findModel(system.models, _vicon_client.GetSubjectName(subject_index).SubjectName); model.nummarkers = _vicon_client.GetMarkerCount(model.name).MarkerCount; if (model.nummarkers < model.markers.size()) { model.markers.clear(); model.markers.reserve(model.nummarkers); } for(int32_t j = 0; j < model.nummarkers; j++) { vicon::marker_t& marker = findMarker(model.markers, _vicon_client.GetMarkerName(model.name, j).MarkerName); Output_GetMarkerGlobalTranslation translation = _vicon_client.GetMarkerGlobalTranslation(model.name, marker.name); marker.o = translation.Occluded; memcpy(marker.xyz, translation.Translation, 3*sizeof(double)); } //////////////////////////////////////////////////////////////////////// /// Segments // get number of segments model.numsegments = _vicon_client.GetSegmentCount(model.name).SegmentCount; if (model.numsegments < model.segments.size()) { model.segments.clear(); model.segments.reserve(model.numsegments); } // get subject name std::string subject_name = _vicon_client.GetSubjectName(subject_index).SubjectName; // get root segment name std::string root_segment_name = _vicon_client.GetSubjectRootSegmentName(subject_name).SegmentName; // loop through segments for (uint segment_index = 0; segment_index < model.numsegments; segment_index++) { // get segment name std::string segment_name = _vicon_client.GetSegmentName(subject_name, segment_index).SegmentName; vicon::segment_t& segment = findSegment(model.segments, segment_name); Output_GetSegmentGlobalRotationEulerXYZ A = _vicon_client.GetSegmentGlobalRotationEulerXYZ(model.name, segment.name); Output_GetSegmentGlobalTranslation T = _vicon_client.GetSegmentGlobalTranslation(model.name, segment.name); Output_GetSegmentLocalRotationEulerXYZ ba = _vicon_client.GetSegmentLocalRotationEulerXYZ(model.name, segment.name); Output_GetSegmentLocalTranslation bt = _vicon_client.GetSegmentLocalTranslation(model.name, segment.name); memcpy(segment.A, A.Rotation, 3*sizeof(double)); memcpy(segment.T, T.Translation, 3*sizeof(double)); memcpy(segment.ba, ba.Rotation, 3*sizeof(double)); memcpy(segment.bt, bt.Translation, 3*sizeof(double)); // check if root segment if (segment_name == root_segment_name) { //TODO: handle articulated bodies // get segment translation // Output_GetSegmentStaticTranslation segment_translation = _vicon_client.GetSegmentStaticTranslation(subject_name, segment_name); Output_GetSegmentGlobalTranslation segment_translation = _vicon_client.GetSegmentGlobalTranslation( subject_name, segment_name); // get segment rotation //Output_GetSegmentStaticRotationQuaternion segment_rotation = _vicon_client.GetSegmentStaticRotationQuaternion(subject_name, segment_name); Output_GetSegmentGlobalRotationQuaternion segment_rotation = _vicon_client.GetSegmentGlobalRotationQuaternion(subject_name, segment_name); // Output_GetSegmentGlobalRotationEulerXYZ segment_rotation = _vicon_client.GetSegmentGlobalRotationEulerXYZ(subject_name, segment_name); // populate message with position for (int i = 0; i < 3; i++) msg.trans[i] = segment_translation.Translation[i]/1000.0; //vicon data is in mm msg.quat[0] = segment_rotation.Rotation[3]; //vicon is x,y,z,w msg.quat[1] = segment_rotation.Rotation[0]; msg.quat[2] = segment_rotation.Rotation[1]; msg.quat[3] = segment_rotation.Rotation[2]; std::string channel = "VICON_" + subject_name; _lcm.publish(channel.c_str(), &msg); // break from segment for loop break; } // root segment } // for each segment (model.numsegments) } // for each subject (system.nummodels) _lcm.publish("VICON_MARKERS", &system); } // disconnect client dbg("Subject position thread terminated properly, disconnecting client\n"); _vicon_client.Disconnect(); }