bool shutdown_vicon()
{
ROS_INFO_STREAM("Disconnecting from Vicon DataStream SDK");
MyClient.Disconnect();
ROS_ASSERT(!MyClient.IsConnected().Connected);
ROS_INFO_STREAM("... disconnected.");
return true;
}
void updateCallback(const ros::TimerEvent& e)
{
static bool got_first = false;
latest_dt = (e.current_real - e.last_real).toSec();
latest_jitter = (e.current_real - e.current_expected).toSec();
max_abs_jitter = max(max_abs_jitter, fabs(latest_jitter));
Result::Enum the_result;
bool was_new_frame = true;
if ((not segment_data_enabled) //
and (pose_pub.getNumSubscribers() > 0 or markers_pub.getNumSubscribers() > 0))
{
the_result = MyClient.EnableSegmentData().Result;
if (the_result != Result::Success)
{
ROS_ERROR("Enable segment data call failed");
}
else
{
ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
ROS_INFO("Segment data enabled");
segment_data_enabled = true;
}
}
if (segment_data_enabled)
{
while (was_new_frame and (the_result = MyClient.GetFrame().Result) == Result::Success)
;
{
now_time = ros::Time::now(); // try to grab as close to getting message as possible
was_new_frame = process_frame();
got_first = true;
}
if (the_result != Result::NoFrame)
{
ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
}
if (got_first)
{
max_period_between_updates = max(max_period_between_updates, latest_dt);
last_callback_duration = e.profile.last_duration.toSec();
}
}
diag_updater.update();
}
void process_subjects(const ros::Time& frame_time)
{
if (pose_pub.getNumSubscribers() > 0)
{
// We know the subject and segment names a priori, so don't bother enumerating, just grab the data:
// Flyer:
Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(SubjectName, SegmentName);
Output_GetSegmentGlobalRotationQuaternion quat = MyClient.GetSegmentGlobalRotationQuaternion(SubjectName,
SegmentName);
if (trans.Result == Result::Success and quat.Result == Result::Success)
{
if ((not trans.Occluded) and (not quat.Occluded))
{
flyer_transform.setOrigin(
tf::Vector3(trans.Translation[0] / 1000, trans.Translation[1] / 1000,
trans.Translation[2] / 1000));
flyer_transform.setRotation(
tf::Quaternion(quat.Rotation[0], quat.Rotation[1], quat.Rotation[2],
quat.Rotation[3]));
//ros::Time thisTime = now_time - ros::Duration(latencyInMs / 1000);
tf::StampedTransform transform = tf::StampedTransform(flyer_transform, frame_time, tf_ref_frame_id,
tf_tracked_frame_id);
if (enable_tf_broadcast)
tf_broadcast.sendTransform(transform);
geometry_msgs::TransformStamped pose_msg;
tf::transformStampedTFToMsg(transform, pose_msg);
pose_pub.publish(pose_msg);
}
else
{
ROS_WARN_STREAM("occluded, not publishing...");
}
}
else
{
ROS_WARN("GetSegmentGlobalTranslation/Rotation failed (result = %d, %d), not publishing...",
(int)(trans.Result), (int)(quat.Result));
}
}
}
bool process_frame()
{
static ros::Time lastTime;
Output_GetFrameNumber OutputFrameNum = MyClient.GetFrameNumber();
if ((OutputFrameNum.Result != Result::Success) and (OutputFrameNum.Result != Result::NoFrame))
{
ROS_ERROR("GetFrameNumber() returned %d", int(OutputFrameNum.Result));
return false;
}
if (frame_datum == 0 or (fabs(latest_time_bias) > time_bias_reset)) // this is the first frame we've gotten
{
frame_datum = OutputFrameNum.FrameNumber;
time_datum = now_time;
ROS_INFO("Time bias reset (bias was %f): marking VICON frame number %d as datum time %d.%d", latest_time_bias, frame_datum, time_datum.sec,
time_datum.nsec);
time_bias_reset_count++;
}
//frameCount++;
// ROS_INFO_STREAM("Grabbed a frame: " << OutputFrameNum.FrameNumber);
int frameDiff = 0;
if (lastFrameNumber != 0)
{
frameDiff = OutputFrameNum.FrameNumber - lastFrameNumber;
frameCount += frameDiff;
if ((frameDiff) > 1)
{
droppedFrameCount += frameDiff - 1;
double droppedFramePct = (double)droppedFrameCount / frameCount * 100;
ROS_DEBUG_STREAM(
(frameDiff - 1) << " more (total " << droppedFrameCount << "/" << frameCount << ", "
<< droppedFramePct << "%) frame(s) dropped. Consider adjusting rates.");
}
}
lastFrameNumber = OutputFrameNum.FrameNumber;
if (frameDiff == 0)
{
return false;
}
else
{
freq_status.tick();
//double latencyInMs = MyClient.GetLatencyTotal().Total * 1000;
ros::Time frame_time = time_datum + ros::Duration((lastFrameNumber - frame_datum) / vicon_capture_rate);
latest_time_bias = (frame_time - now_time).toSec();
process_subjects(frame_time);
process_markers(frame_time, lastFrameNumber);
lastTime = now_time;
return true;
}
}
bool grab_vicon_pose_callback(vicon_mocap::viconGrabPose::Request& req, vicon_mocap::viconGrabPose::Response& resp)
{
ROS_INFO("Got request for a VICON pose");
updateTimer.stop();
tf::StampedTransform transform;
//tf::Quaternion quat;
if (not segment_data_enabled)
{
MyClient.EnableSegmentData();
ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
segment_data_enabled = true;
}
// Gather data:
int N = req.n_measurements;
double accum_trans[3] = {0, 0, 0};
double accum_quat[4] = {0, 0, 0, 0};
Result::Enum the_result;
int n_success = 0;
for (int k = 0; k < N; k++)
{
ros::Duration(1 / vicon_capture_rate).sleep(); // Wait at least as long as vicon needs to capture the next frame
if ((the_result = MyClient.GetFrame().Result) == Result::Success)
{
try
{
Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(req.subject_name,
req.segment_name);
Output_GetSegmentGlobalRotationQuaternion quat =
MyClient.GetSegmentGlobalRotationQuaternion(req.subject_name, req.segment_name);
if ((!trans.Occluded) && (!quat.Occluded))
{
accum_trans[0] += trans.Translation[0] / 1000;
accum_trans[1] += trans.Translation[1] / 1000;
accum_trans[2] += trans.Translation[2] / 1000;
accum_quat[0] += quat.Rotation[3];
accum_quat[1] += quat.Rotation[0];
accum_quat[2] += quat.Rotation[1];
accum_quat[3] += quat.Rotation[2];
n_success++;
}
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
resp.success = false;
return false; // TODO: should we really bail here, or just try again?
}
}
else
{
if (the_result != Result::NoFrame)
{
ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
}
}
}
ROS_INFO("Averaging %d measurements", n_success);
// Average the data:
double normalized_quat[4];
double quat_norm = sqrt(
accum_quat[0] * accum_quat[0] + accum_quat[1] * accum_quat[1] + accum_quat[2]
* accum_quat[2] + accum_quat[3] * accum_quat[3]);
for (int i = 0; i < 4; i++)
{
normalized_quat[i] = accum_quat[i] / quat_norm;
}
double normalized_vector[3];
// Copy to inputs:
for (int i = 0; i < 3; i++)
{
normalized_vector[i] = accum_trans[i] / n_success;
}
// copy what we used to service call response:
resp.success = true;
resp.pose.header.stamp = ros::Time::now();
resp.pose.header.frame_id = tf_ref_frame_id;
resp.pose.pose.position.x = normalized_vector[0];
resp.pose.pose.position.y = normalized_vector[1];
resp.pose.pose.position.z = normalized_vector[2];
resp.pose.pose.orientation.w = normalized_quat[0];
resp.pose.pose.orientation.x = normalized_quat[1];
resp.pose.pose.orientation.y = normalized_quat[2];
resp.pose.pose.orientation.z = normalized_quat[3];
updateTimer.start();
return true;
}
void process_markers(const ros::Time& frame_time, unsigned int vicon_frame_num)
{
if (markers_pub.getNumSubscribers() > 0)
{
if (not marker_data_enabled)
{
MyClient.EnableMarkerData();
ROS_ASSERT(MyClient.IsMarkerDataEnabled().Enabled);
marker_data_enabled = true;
}
if (not unlabeled_marker_data_enabled)
{
MyClient.EnableUnlabeledMarkerData();
ROS_ASSERT(MyClient.IsUnlabeledMarkerDataEnabled().Enabled);
unlabeled_marker_data_enabled = true;
}
n_markers = 0;
vicon_mocap::Markers markers_msg;
markers_msg.header.stamp = frame_time;
markers_msg.frame_number = vicon_frame_num;
markers_msg.header.frame_id = tf_ref_frame_id;
// Count the number of subjects
unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
// Get labeled markers
for (unsigned int SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex)
{
std::string this_subject_name = MyClient.GetSubjectName(SubjectIndex).SubjectName;
// Count the number of markers
unsigned int num_subject_markers = MyClient.GetMarkerCount(SubjectName).MarkerCount;
n_markers += num_subject_markers;
//std::cout << " Markers (" << MarkerCount << "):" << std::endl;
for (unsigned int MarkerIndex = 0; MarkerIndex < num_subject_markers; ++MarkerIndex)
{
vicon_mocap::Marker this_marker;
this_marker.marker_name = MyClient.GetMarkerName(this_subject_name, MarkerIndex).MarkerName;
this_marker.subject_name = this_subject_name;
this_marker.segment_name
= MyClient.GetMarkerParentName(this_subject_name, this_marker.marker_name).SegmentName;
// Get the global marker translation
Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation =
MyClient.GetMarkerGlobalTranslation(this_subject_name, this_marker.marker_name);
this_marker.translation.x = _Output_GetMarkerGlobalTranslation.Translation[0] / 1000;
this_marker.translation.y = _Output_GetMarkerGlobalTranslation.Translation[1] / 1000;
this_marker.translation.z = _Output_GetMarkerGlobalTranslation.Translation[2] / 1000;
this_marker.occluded = _Output_GetMarkerGlobalTranslation.Occluded;
markers_msg.markers.push_back(this_marker);
}
}
// get unlabeled markers
unsigned int UnlabeledMarkerCount = MyClient.GetUnlabeledMarkerCount().MarkerCount;
//ROS_INFO("# unlabeled markers: %d", UnlabeledMarkerCount);
n_markers += UnlabeledMarkerCount;
n_unlabeled_markers = UnlabeledMarkerCount;
for (unsigned int UnlabeledMarkerIndex = 0; UnlabeledMarkerIndex < UnlabeledMarkerCount; ++UnlabeledMarkerIndex)
{
// Get the global marker translation
Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation =
MyClient.GetUnlabeledMarkerGlobalTranslation(UnlabeledMarkerIndex);
if (_Output_GetUnlabeledMarkerGlobalTranslation.Result == Result::Success)
{
vicon_mocap::Marker this_marker;
this_marker.translation.x = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[0] / 1000;
this_marker.translation.y = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[1] / 1000;
this_marker.translation.z = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[2] / 1000;
this_marker.occluded = false; // unlabeled markers can't be occluded
markers_msg.unlabeled_markers.push_back(this_marker);
}
else
{
ROS_WARN("GetUnlabeledMarkerGlobalTranslation failed (result = %d)",
(int)(_Output_GetUnlabeledMarkerGlobalTranslation.Result));
}
}
markers_pub.publish(markers_msg);
}
}
bool init_vicon()
{
ROS_INFO_STREAM("Connecting to Vicon DataStream SDK at " << HostName << " ...");
while (!MyClient.IsConnected().Connected)
{
MyClient.Connect(HostName);
ROS_INFO(".");
sleep(1);
ros::spinOnce();
if (!ros::ok())
return false;
}
ROS_ASSERT(MyClient.IsConnected().Connected);
ROS_INFO_STREAM("... connected!");
ROS_INFO_STREAM("Setting Stream Mode to " << StreamMode);
{
Output_SetStreamMode result;
if (StreamMode == "ServerPush")
{
result = MyClient.SetStreamMode(StreamMode::ServerPush);
}
else if (StreamMode == "ClientPull")
{
result = MyClient.SetStreamMode(StreamMode::ClientPull);
}
else if (StreamMode == "ClientPullPreFetch")
{
result = MyClient.SetStreamMode(StreamMode::ClientPullPreFetch);
}
else
{
ROS_FATAL("Unknown stream mode -- options are ServerPush, ClientPull, ClientPullPreFetch");
ros::shutdown();
}
if (result.Result != Result::Success)
{
ROS_FATAL("Set stream mode call failed -- shutting down");
ros::shutdown();
}
}
{
Output_SetAxisMapping result;
result = MyClient.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up); // 'Z-up'
if (result.Result != Result::Success)
{
ROS_FATAL("Set axis mapping call failed -- shutting down");
ros::shutdown();
}
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
ROS_INFO_STREAM(
"Axis Mapping: X-" << Adapt(_Output_GetAxisMapping.XAxis) << " Y-"
<< Adapt(_Output_GetAxisMapping.YAxis) << " Z-" << Adapt(_Output_GetAxisMapping.ZAxis));
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
ROS_INFO_STREAM(
"Version: " << _Output_GetVersion.Major << "." << _Output_GetVersion.Minor << "."
<< _Output_GetVersion.Point);
}
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();
}