vicon_recv_direct.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, UC Regents
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the University of California nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <Client.h>
00036 #include <ros/ros.h>
00037 #include <diagnostic_updater/diagnostic_updater.h>
00038 #include <diagnostic_updater/update_functions.h>
00039 #include <tf/tf.h>
00040 #include <tf/transform_broadcaster.h>
00041 #include <geometry_msgs/TransformStamped.h>
00042 #include <vicon_mocap/viconGrabPose.h>
00043 #include <iostream>
00044 #include <algorithm>
00045 
00046 #include <vicon_mocap/Markers.h>
00047 #include <vicon_mocap/Marker.h>
00048 
00049 using std::min;
00050 using std::max;
00051 using std::string;
00052 
00053 using namespace ViconDataStreamSDK::CPP;
00054 
00055 string Adapt(const Direction::Enum i_Direction)
00056 {
00057   switch (i_Direction)
00058   {
00059     case Direction::Forward:
00060       return "Forward";
00061     case Direction::Backward:
00062       return "Backward";
00063     case Direction::Left:
00064       return "Left";
00065     case Direction::Right:
00066       return "Right";
00067     case Direction::Up:
00068       return "Up";
00069     case Direction::Down:
00070       return "Down";
00071     default:
00072       return "Unknown";
00073   }
00074 }
00075 
00076 class ViconReceiver
00077 {
00078 private:
00079   ros::NodeHandle nh;
00080   ros::NodeHandle nh_priv;
00081   // Diagnostic Updater
00082   diagnostic_updater::Updater diag_updater;
00083   double min_freq;
00084   double max_freq;
00085   diagnostic_updater::FrequencyStatus freq_status;
00086   // Parameters:
00087   string StreamMode;
00088   string HostName;
00089   string SubjectName;
00090   string SegmentName;
00091   string tf_ref_frame_id;
00092   string tf_tracked_frame_id;
00093   double update_rate;
00094   double vicon_capture_rate;
00095   // Publisher
00096   ros::Publisher pose_pub;
00097   ros::Publisher markers_pub;
00098   // Timer
00099   ros::Timer updateTimer;
00100   // TF Broadcaster
00101   tf::TransformBroadcaster tf_broadcast;
00102   //geometry_msgs::PoseStamped vicon_pose;
00103   tf::Transform flyer_transform;
00104   ros::Time now_time;
00105   // TODO: Make the following configurable:
00106   ros::ServiceServer m_grab_vicon_pose_service_server;
00107   ViconDataStreamSDK::CPP::Client MyClient;
00108   double max_period_between_updates;
00109   double latest_dt;
00110   double latest_jitter;
00111   double max_abs_jitter;
00112   double last_callback_duration;
00113   unsigned int lastFrameNumber;
00114   unsigned int frameCount;
00115   unsigned int droppedFrameCount;
00116   ros::Time time_datum;
00117   unsigned int frame_datum;
00118   double latest_time_bias;
00119   unsigned int time_bias_reset_count;
00120   unsigned int n_markers;
00121   unsigned int n_unlabeled_markers;
00122   double time_bias_reset;
00123   bool segment_data_enabled;
00124   bool marker_data_enabled;
00125   bool unlabeled_marker_data_enabled;
00126   bool enable_tf_broadcast;
00127 
00128 public:
00129   ViconReceiver() :
00130     nh_priv("~"), diag_updater(), min_freq(0.1), max_freq(1000),
00131         freq_status(diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq)), StreamMode("ClientPullPreFetch"),
00132         HostName(""), SubjectName(""), SegmentName(""), tf_ref_frame_id("/enu"),
00133         tf_tracked_frame_id("/pelican1/flyer_vicon"), update_rate(100), vicon_capture_rate(50),
00134         max_period_between_updates(0), max_abs_jitter(0), lastFrameNumber(0), frameCount(0), droppedFrameCount(0),
00135         frame_datum(0), latest_time_bias(0), time_bias_reset_count(0), n_markers(0), n_unlabeled_markers(0),
00136         time_bias_reset(0.05), segment_data_enabled(false), marker_data_enabled(false),
00137         unlabeled_marker_data_enabled(false), enable_tf_broadcast(false)
00138 
00139   {
00140     // Diagnostics
00141     diag_updater.add("ViconReceiver Status", this, &ViconReceiver::diagnostics);
00142     diag_updater.add(freq_status);
00143     diag_updater.setHardwareID("none");
00144     diag_updater.force_update();
00145     // Parameters
00146     nh_priv.param("stream_mode", StreamMode, StreamMode);
00147     nh_priv.param("datastream_hostport", HostName, HostName);
00148     nh_priv.param("subject_name", SubjectName, SubjectName);
00149     nh_priv.param("segment_name", SegmentName, SegmentName);
00150     nh_priv.param("update_rate", update_rate, update_rate);
00151     nh_priv.param("vicon_capture_rate", vicon_capture_rate, vicon_capture_rate);
00152     nh_priv.param("tf_ref_frame_id", tf_ref_frame_id, tf_ref_frame_id);
00153     nh_priv.param("tf_tracked_frame_id", tf_tracked_frame_id, tf_tracked_frame_id);
00154     nh_priv.param("time_bias_reset", time_bias_reset, time_bias_reset);
00155     nh_priv.param("enable_tf_broadcast", enable_tf_broadcast, enable_tf_broadcast);
00156     ROS_ASSERT(init_vicon());
00157     // Service Server
00158     ROS_INFO("setting up grab_vicon_pose service server ... ");
00159     m_grab_vicon_pose_service_server = nh_priv.advertiseService("grab_vicon_pose",
00160                                                                 &ViconReceiver::grab_vicon_pose_callback, this);
00161     // Publishers
00162     pose_pub = nh_priv.advertise<geometry_msgs::TransformStamped> ("output", 10);
00163     markers_pub = nh_priv.advertise<vicon_mocap::Markers> ("markers", 10);
00164     // Timer
00165     double update_timer_period = 1 / update_rate;
00166     min_freq = 0.95 * vicon_capture_rate;
00167     max_freq = 1.05 * vicon_capture_rate;
00168     updateTimer = nh.createTimer(ros::Duration(update_timer_period), &ViconReceiver::updateCallback, this);
00169   }
00170 
00171   ~ViconReceiver()
00172   {
00173     ROS_ASSERT(shutdown_vicon());
00174   }
00175 
00176 private:
00177   void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
00178   {
00179     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
00180     stat.add("max dt", max_period_between_updates);
00181     stat.add("latest dt", latest_dt);
00182     stat.add("latest jitter", latest_jitter);
00183     stat.add("max abs jitter", max_abs_jitter);
00184     stat.add("latest callback runtime", last_callback_duration);
00185     stat.add("latest VICON frame number", lastFrameNumber);
00186     stat.add("dropped frames", droppedFrameCount);
00187     stat.add("framecount", frameCount);
00188     stat.add("latest time bias", latest_time_bias);
00189     stat.add("time bias reset count", time_bias_reset_count);
00190     stat.add("# markers", n_markers);
00191     stat.add("# unlabeled markers", n_unlabeled_markers);
00192   }
00193 
00194   bool init_vicon()
00195   {
00196     ROS_INFO_STREAM("Connecting to Vicon DataStream SDK at " << HostName << " ...");
00197 
00198     while (!MyClient.IsConnected().Connected)
00199     {
00200       MyClient.Connect(HostName);
00201       ROS_INFO(".");
00202       sleep(1);
00203       ros::spinOnce();
00204       if (!ros::ok())
00205         return false;
00206     }
00207     ROS_ASSERT(MyClient.IsConnected().Connected);
00208     ROS_INFO_STREAM("... connected!");
00209 
00210     ROS_INFO_STREAM("Setting Stream Mode to " << StreamMode);
00211     {
00212       Output_SetStreamMode result;
00213 
00214       if (StreamMode == "ServerPush")
00215       {
00216         result = MyClient.SetStreamMode(StreamMode::ServerPush);
00217       }
00218       else if (StreamMode == "ClientPull")
00219       {
00220         result = MyClient.SetStreamMode(StreamMode::ClientPull);
00221       }
00222       else if (StreamMode == "ClientPullPreFetch")
00223       {
00224         result = MyClient.SetStreamMode(StreamMode::ClientPullPreFetch);
00225       }
00226       else
00227       {
00228         ROS_FATAL("Unknown stream mode -- options are ServerPush, ClientPull, ClientPullPreFetch");
00229         ros::shutdown();
00230       }
00231 
00232       if (result.Result != Result::Success)
00233       {
00234         ROS_FATAL("Set stream mode call failed -- shutting down");
00235         ros::shutdown();
00236       }
00237     }
00238     {
00239       Output_SetAxisMapping result;
00240       result = MyClient.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up); // 'Z-up'
00241       if (result.Result != Result::Success)
00242       {
00243         ROS_FATAL("Set axis mapping call failed -- shutting down");
00244         ros::shutdown();
00245       }
00246       Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
00247       ROS_INFO_STREAM(
00248                       "Axis Mapping: X-" << Adapt(_Output_GetAxisMapping.XAxis) << " Y-"
00249                           << Adapt(_Output_GetAxisMapping.YAxis) << " Z-" << Adapt(_Output_GetAxisMapping.ZAxis));
00250       Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
00251       ROS_INFO_STREAM(
00252                       "Version: " << _Output_GetVersion.Major << "." << _Output_GetVersion.Minor << "."
00253                           << _Output_GetVersion.Point);
00254     }
00255     return true;
00256   }
00257 
00258   void updateCallback(const ros::TimerEvent& e)
00259   {
00260     static bool got_first = false;
00261     latest_dt = (e.current_real - e.last_real).toSec();
00262     latest_jitter = (e.current_real - e.current_expected).toSec();
00263     max_abs_jitter = max(max_abs_jitter, fabs(latest_jitter));
00264     Result::Enum the_result;
00265     bool was_new_frame = true;
00266 
00267     if ((not segment_data_enabled) //
00268         and (pose_pub.getNumSubscribers() > 0 or markers_pub.getNumSubscribers() > 0))
00269     {
00270       the_result = MyClient.EnableSegmentData().Result;
00271       if (the_result != Result::Success)
00272       {
00273         ROS_ERROR("Enable segment data call failed");
00274       }
00275       else
00276       {
00277         ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
00278         ROS_INFO("Segment data enabled");
00279         segment_data_enabled = true;
00280       }
00281     }
00282 
00283     if (segment_data_enabled)
00284     {
00285       while (was_new_frame and (the_result = MyClient.GetFrame().Result) == Result::Success)
00286         ;
00287       {
00288         now_time = ros::Time::now(); // try to grab as close to getting message as possible
00289         was_new_frame = process_frame();
00290         got_first = true;
00291       }
00292 
00293       if (the_result != Result::NoFrame)
00294       {
00295         ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
00296       }
00297 
00298       if (got_first)
00299       {
00300         max_period_between_updates = max(max_period_between_updates, latest_dt);
00301         last_callback_duration = e.profile.last_duration.toSec();
00302       }
00303     }
00304     diag_updater.update();
00305   }
00306 
00307   bool shutdown_vicon()
00308   {
00309     ROS_INFO_STREAM("Disconnecting from Vicon DataStream SDK");
00310     MyClient.Disconnect();
00311     ROS_ASSERT(!MyClient.IsConnected().Connected);
00312     ROS_INFO_STREAM("... disconnected.");
00313     return true;
00314   }
00315 
00316   bool process_frame()
00317   {
00318     static ros::Time lastTime;
00319     Output_GetFrameNumber OutputFrameNum = MyClient.GetFrameNumber();
00320     if ((OutputFrameNum.Result != Result::Success) and (OutputFrameNum.Result != Result::NoFrame))
00321     {
00322       ROS_ERROR("GetFrameNumber() returned %d", int(OutputFrameNum.Result));
00323       return false;
00324     }
00325     if (frame_datum == 0 or (fabs(latest_time_bias) > time_bias_reset)) // this is the first frame we've gotten
00326     {
00327       frame_datum = OutputFrameNum.FrameNumber;
00328       time_datum = now_time;
00329       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,
00330                time_datum.nsec);
00331       time_bias_reset_count++;
00332     }
00333 
00334     //frameCount++;
00335     //    ROS_INFO_STREAM("Grabbed a frame: " << OutputFrameNum.FrameNumber);
00336     int frameDiff = 0;
00337     if (lastFrameNumber != 0)
00338     {
00339       frameDiff = OutputFrameNum.FrameNumber - lastFrameNumber;
00340       frameCount += frameDiff;
00341       if ((frameDiff) > 1)
00342       {
00343         droppedFrameCount += frameDiff - 1;
00344         double droppedFramePct = (double)droppedFrameCount / frameCount * 100;
00345         ROS_DEBUG_STREAM(
00346                          (frameDiff - 1) << " more (total " << droppedFrameCount << "/" << frameCount << ", "
00347                              << droppedFramePct << "%) frame(s) dropped. Consider adjusting rates.");
00348       }
00349     }
00350     lastFrameNumber = OutputFrameNum.FrameNumber;
00351 
00352     if (frameDiff == 0)
00353     {
00354       return false;
00355     }
00356     else
00357     {
00358       freq_status.tick();
00359       //double latencyInMs = MyClient.GetLatencyTotal().Total * 1000;
00360       ros::Time frame_time = time_datum + ros::Duration((lastFrameNumber - frame_datum) / vicon_capture_rate);
00361       latest_time_bias = (frame_time - now_time).toSec();
00362 
00363       process_subjects(frame_time);
00364       process_markers(frame_time, lastFrameNumber);
00365 
00366       lastTime = now_time;
00367       return true;
00368     }
00369   }
00370 
00371   void process_subjects(const ros::Time& frame_time)
00372   {
00373     if (pose_pub.getNumSubscribers() > 0)
00374     {
00375 
00376       // We know the subject and segment names a priori, so don't bother enumerating, just grab the data:
00377       // Flyer:
00378       Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(SubjectName, SegmentName);
00379       Output_GetSegmentGlobalRotationQuaternion quat = MyClient.GetSegmentGlobalRotationQuaternion(SubjectName,
00380                                                                                                    SegmentName);
00381       if (trans.Result == Result::Success and quat.Result == Result::Success)
00382       {
00383         if ((not trans.Occluded) and (not quat.Occluded))
00384         {
00385           flyer_transform.setOrigin(
00386                                     tf::Vector3(trans.Translation[0] / 1000, trans.Translation[1] / 1000,
00387                                                 trans.Translation[2] / 1000));
00388           flyer_transform.setRotation(
00389                                       tf::Quaternion(quat.Rotation[0], quat.Rotation[1], quat.Rotation[2],
00390                                                      quat.Rotation[3]));
00391           //ros::Time thisTime = now_time - ros::Duration(latencyInMs / 1000);
00392           tf::StampedTransform transform = tf::StampedTransform(flyer_transform, frame_time, tf_ref_frame_id,
00393                                                                 tf_tracked_frame_id);
00394           if (enable_tf_broadcast)
00395             tf_broadcast.sendTransform(transform);
00396 
00397           geometry_msgs::TransformStamped pose_msg;
00398           tf::transformStampedTFToMsg(transform, pose_msg);
00399           pose_pub.publish(pose_msg);
00400 
00401         }
00402         else
00403         {
00404           ROS_WARN_STREAM("occluded, not publishing...");
00405         }
00406       }
00407       else
00408       {
00409         ROS_WARN("GetSegmentGlobalTranslation/Rotation failed (result = %d, %d), not publishing...",
00410                  (int)(trans.Result), (int)(quat.Result));
00411       }
00412     }
00413   }
00414 
00415   void process_markers(const ros::Time& frame_time, unsigned int vicon_frame_num)
00416   {
00417     if (markers_pub.getNumSubscribers() > 0)
00418     {
00419       if (not marker_data_enabled)
00420       {
00421         MyClient.EnableMarkerData();
00422         ROS_ASSERT(MyClient.IsMarkerDataEnabled().Enabled);
00423         marker_data_enabled = true;
00424       }
00425       if (not unlabeled_marker_data_enabled)
00426       {
00427         MyClient.EnableUnlabeledMarkerData();
00428         ROS_ASSERT(MyClient.IsUnlabeledMarkerDataEnabled().Enabled);
00429         unlabeled_marker_data_enabled = true;
00430       }
00431       n_markers = 0;
00432       vicon_mocap::Markers markers_msg;
00433       markers_msg.header.stamp = frame_time;
00434       markers_msg.frame_number = vicon_frame_num;
00435       markers_msg.header.frame_id = tf_ref_frame_id;
00436       // Count the number of subjects
00437       unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
00438       // Get labeled markers
00439       for (unsigned int SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex)
00440       {
00441         std::string this_subject_name = MyClient.GetSubjectName(SubjectIndex).SubjectName;
00442         // Count the number of markers
00443         unsigned int num_subject_markers = MyClient.GetMarkerCount(SubjectName).MarkerCount;
00444         n_markers += num_subject_markers;
00445         //std::cout << "    Markers (" << MarkerCount << "):" << std::endl;
00446         for (unsigned int MarkerIndex = 0; MarkerIndex < num_subject_markers; ++MarkerIndex)
00447         {
00448           vicon_mocap::Marker this_marker;
00449           this_marker.marker_name = MyClient.GetMarkerName(this_subject_name, MarkerIndex).MarkerName;
00450           this_marker.subject_name = this_subject_name;
00451           this_marker.segment_name
00452               = MyClient.GetMarkerParentName(this_subject_name, this_marker.marker_name).SegmentName;
00453 
00454           // Get the global marker translation
00455           Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation =
00456               MyClient.GetMarkerGlobalTranslation(this_subject_name, this_marker.marker_name);
00457 
00458           this_marker.translation.x = _Output_GetMarkerGlobalTranslation.Translation[0] / 1000;
00459           this_marker.translation.y = _Output_GetMarkerGlobalTranslation.Translation[1] / 1000;
00460           this_marker.translation.z = _Output_GetMarkerGlobalTranslation.Translation[2] / 1000;
00461           this_marker.occluded = _Output_GetMarkerGlobalTranslation.Occluded;
00462 
00463           markers_msg.markers.push_back(this_marker);
00464         }
00465       }
00466       // get unlabeled markers
00467       unsigned int UnlabeledMarkerCount = MyClient.GetUnlabeledMarkerCount().MarkerCount;
00468       //ROS_INFO("# unlabeled markers: %d", UnlabeledMarkerCount);
00469       n_markers += UnlabeledMarkerCount;
00470       n_unlabeled_markers = UnlabeledMarkerCount;
00471       for (unsigned int UnlabeledMarkerIndex = 0; UnlabeledMarkerIndex < UnlabeledMarkerCount; ++UnlabeledMarkerIndex)
00472       {
00473         // Get the global marker translation
00474         Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation =
00475             MyClient.GetUnlabeledMarkerGlobalTranslation(UnlabeledMarkerIndex);
00476 
00477         if (_Output_GetUnlabeledMarkerGlobalTranslation.Result == Result::Success)
00478         {
00479           vicon_mocap::Marker this_marker;
00480           this_marker.translation.x = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[0] / 1000;
00481           this_marker.translation.y = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[1] / 1000;
00482           this_marker.translation.z = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[2] / 1000;
00483           this_marker.occluded = false; // unlabeled markers can't be occluded
00484           markers_msg.unlabeled_markers.push_back(this_marker);
00485         }
00486         else
00487         {
00488           ROS_WARN("GetUnlabeledMarkerGlobalTranslation failed (result = %d)",
00489                    (int)(_Output_GetUnlabeledMarkerGlobalTranslation.Result));
00490 
00491         }
00492       }
00493       markers_pub.publish(markers_msg);
00494     }
00495   }
00496 
00497   bool grab_vicon_pose_callback(vicon_mocap::viconGrabPose::Request& req, vicon_mocap::viconGrabPose::Response& resp)
00498   {
00499     ROS_INFO("Got request for a VICON pose");
00500     updateTimer.stop();
00501     tf::StampedTransform transform;
00502     //tf::Quaternion quat;
00503 
00504     if (not segment_data_enabled)
00505     {
00506       MyClient.EnableSegmentData();
00507       ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
00508       segment_data_enabled = true;
00509     }
00510 
00511     // Gather data:
00512     int N = req.n_measurements;
00513     double accum_trans[3] = {0, 0, 0};
00514     double accum_quat[4] = {0, 0, 0, 0};
00515     Result::Enum the_result;
00516     int n_success = 0;
00517     for (int k = 0; k < N; k++)
00518     {
00519       ros::Duration(1 / vicon_capture_rate).sleep(); // Wait at least as long as vicon needs to capture the next frame
00520       if ((the_result = MyClient.GetFrame().Result) == Result::Success)
00521       {
00522         try
00523         {
00524           Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(req.subject_name,
00525                                                                                           req.segment_name);
00526           Output_GetSegmentGlobalRotationQuaternion quat =
00527               MyClient.GetSegmentGlobalRotationQuaternion(req.subject_name, req.segment_name);
00528           if ((!trans.Occluded) && (!quat.Occluded))
00529           {
00530             accum_trans[0] += trans.Translation[0] / 1000;
00531             accum_trans[1] += trans.Translation[1] / 1000;
00532             accum_trans[2] += trans.Translation[2] / 1000;
00533             accum_quat[0] += quat.Rotation[3];
00534             accum_quat[1] += quat.Rotation[0];
00535             accum_quat[2] += quat.Rotation[1];
00536             accum_quat[3] += quat.Rotation[2];
00537             n_success++;
00538           }
00539         }
00540         catch (tf::TransformException ex)
00541         {
00542           ROS_ERROR("%s", ex.what());
00543           resp.success = false;
00544           return false; // TODO: should we really bail here, or just try again?
00545         }
00546       }
00547       else
00548       {
00549         if (the_result != Result::NoFrame)
00550         {
00551           ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
00552         }
00553       }
00554     }
00555 
00556     ROS_INFO("Averaging %d measurements", n_success);
00557     // Average the data:
00558     double normalized_quat[4];
00559     double quat_norm = sqrt(
00560                             accum_quat[0] * accum_quat[0] + accum_quat[1] * accum_quat[1] + accum_quat[2]
00561                                 * accum_quat[2] + accum_quat[3] * accum_quat[3]);
00562     for (int i = 0; i < 4; i++)
00563     {
00564       normalized_quat[i] = accum_quat[i] / quat_norm;
00565     }
00566     double normalized_vector[3];
00567     // Copy to inputs:
00568     for (int i = 0; i < 3; i++)
00569     {
00570       normalized_vector[i] = accum_trans[i] / n_success;
00571     }
00572 
00573     // copy what we used to service call response:
00574     resp.success = true;
00575     resp.pose.header.stamp = ros::Time::now();
00576     resp.pose.header.frame_id = tf_ref_frame_id;
00577     resp.pose.pose.position.x = normalized_vector[0];
00578     resp.pose.pose.position.y = normalized_vector[1];
00579     resp.pose.pose.position.z = normalized_vector[2];
00580     resp.pose.pose.orientation.w = normalized_quat[0];
00581     resp.pose.pose.orientation.x = normalized_quat[1];
00582     resp.pose.pose.orientation.y = normalized_quat[2];
00583     resp.pose.pose.orientation.z = normalized_quat[3];
00584 
00585     updateTimer.start();
00586     return true;
00587   }
00588 
00589 };
00590 
00591 int main(int argc, char** argv)
00592 {
00593   ros::init(argc, argv, "vicon_recv_direct");
00594   ViconReceiver vr;
00595   ros::spin();
00596   return 0;
00597 }


vicon_mocap
Author(s): Patrick Bouffard
autogenerated on Sun Jan 5 2014 11:38:20