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();
}
