vicon_bridge.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, UC Regents
00005  *  Copyright (c) 2011, Markus Achtelik, ETH Zurich, Autonomous Systems Lab (modifications)
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the University of California nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <Client.h>
00037 #include <ros/ros.h>
00038 #include <diagnostic_updater/diagnostic_updater.h>
00039 #include <diagnostic_updater/update_functions.h>
00040 #include <tf/tf.h>
00041 #include <tf/transform_broadcaster.h>
00042 #include <geometry_msgs/TransformStamped.h>
00043 #include <vicon_bridge/viconGrabPose.h>
00044 #include <iostream>
00045 
00046 #include <vicon_bridge/Markers.h>
00047 #include <vicon_bridge/Marker.h>
00048 
00049 #include "msvc_bridge.h"
00050 #include <map>
00051 #include <boost/thread.hpp>
00052 #include <vicon_bridge/viconCalibrateSegment.h>
00053 #include <tf/transform_listener.h>
00054 
00055 using std::min;
00056 using std::max;
00057 using std::string;
00058 using std::map;
00059 
00060 using namespace ViconDataStreamSDK::CPP;
00061 
00062 string Adapt(const Direction::Enum i_Direction)
00063 {
00064   switch (i_Direction)
00065   {
00066     case Direction::Forward:
00067       return "Forward";
00068     case Direction::Backward:
00069       return "Backward";
00070     case Direction::Left:
00071       return "Left";
00072     case Direction::Right:
00073       return "Right";
00074     case Direction::Up:
00075       return "Up";
00076     case Direction::Down:
00077       return "Down";
00078     default:
00079       return "Unknown";
00080   }
00081 }
00082 
00083 string Adapt(const Result::Enum i_result)
00084 {
00085   switch (i_result)
00086   {
00087     case Result::ClientAlreadyConnected:
00088       return "ClientAlreadyConnected";
00089     case Result::ClientConnectionFailed:
00090       return "";
00091     case Result::CoLinearAxes:
00092       return "CoLinearAxes";
00093     case Result::InvalidDeviceName:
00094       return "InvalidDeviceName";
00095     case Result::InvalidDeviceOutputName:
00096       return "InvalidDeviceOutputName";
00097     case Result::InvalidHostName:
00098       return "InvalidHostName";
00099     case Result::InvalidIndex:
00100       return "InvalidIndex";
00101     case Result::InvalidLatencySampleName:
00102       return "InvalidLatencySampleName";
00103     case Result::InvalidMarkerName:
00104       return "InvalidMarkerName";
00105     case Result::InvalidMulticastIP:
00106       return "InvalidMulticastIP";
00107     case Result::InvalidSegmentName:
00108       return "InvalidSegmentName";
00109     case Result::InvalidSubjectName:
00110       return "InvalidSubjectName";
00111     case Result::LeftHandedAxes:
00112       return "LeftHandedAxes";
00113     case Result::NoFrame:
00114       return "NoFrame";
00115     case Result::NotConnected:
00116       return "NotConnected";
00117     case Result::NotImplemented:
00118       return "NotImplemented";
00119     case Result::ServerAlreadyTransmittingMulticast:
00120       return "ServerAlreadyTransmittingMulticast";
00121     case Result::ServerNotTransmittingMulticast:
00122       return "ServerNotTransmittingMulticast";
00123     case Result::Success:
00124       return "Success";
00125     case Result::Unknown:
00126       return "Unknown";
00127     default:
00128       return "unknown";
00129   }
00130 }
00131 
00132 class SegmentPublisher
00133 {
00134 public:
00135   ros::Publisher pub;
00136   bool is_ready;
00137   tf::Transform calibration_pose;
00138   bool calibrated;
00139   SegmentPublisher() :
00140     is_ready(false), calibration_pose(tf::Pose::getIdentity()),
00141         calibrated(false)
00142   {
00143   }
00144   ;
00145 };
00146 
00147 typedef map<string, SegmentPublisher> SegmentMap;
00148 
00149 class ViconReceiver
00150 {
00151 private:
00152   ros::NodeHandle nh;
00153   ros::NodeHandle nh_priv;
00154   // Diagnostic Updater
00155   diagnostic_updater::Updater diag_updater;
00156   double min_freq_;
00157   double max_freq_;
00158   diagnostic_updater::FrequencyStatus freq_status_;
00159   // Parameters:
00160   string stream_mode_;
00161   string host_name_;
00162   string tf_ref_frame_id_;
00163   string tracked_frame_suffix_;
00164   // Publisher
00165   ros::Publisher marker_pub_;
00166   // TF Broadcaster
00167   tf::TransformBroadcaster tf_broadcaster_;
00168   //geometry_msgs::PoseStamped vicon_pose;
00169   tf::Transform flyer_transform;
00170   ros::Time now_time;
00171   // TODO: Make the following configurable:
00172   ros::ServiceServer m_grab_vicon_pose_service_server;
00173   ros::ServiceServer calibrate_segment_server_;
00174   //  ViconDataStreamSDK::CPP::Client MyClient;
00175   unsigned int lastFrameNumber;
00176   unsigned int frameCount;
00177   unsigned int droppedFrameCount;
00178   ros::Time time_datum;
00179   unsigned int frame_datum;
00180   unsigned int n_markers;
00181   unsigned int n_unlabeled_markers;
00182   bool segment_data_enabled;
00183   bool marker_data_enabled;
00184   bool unlabeled_marker_data_enabled;
00185 
00186   bool broadcast_tf_, publish_tf_, publish_markers_;
00187 
00188   bool grab_frames_;
00189   boost::thread grab_frames_thread_;
00190   SegmentMap segment_publishers_;
00191   boost::mutex segments_mutex_;
00192   std::vector<std::string> time_log_;
00193 
00194 public:
00195   void startGrabbing()
00196   {
00197     grab_frames_ = true;
00198     // test grabbing in the main loop and run an asynchronous spinner instead
00199     grabThread();
00200     //grab_frames_thread_ = boost::thread(&ViconReceiver::grabThread, this);
00201   }
00202 
00203   void stopGrabbing()
00204   {
00205     grab_frames_ = false;
00206     //grab_frames_thread_.join();
00207   }
00208 
00209   ViconReceiver() :
00210     nh_priv("~"), diag_updater(), min_freq_(0.1), max_freq_(1000),
00211         freq_status_(diagnostic_updater::FrequencyStatusParam(&min_freq_, &max_freq_)), stream_mode_("ClientPull"),
00212         host_name_(""), tf_ref_frame_id_("world"), tracked_frame_suffix_("vicon"),
00213         lastFrameNumber(0), frameCount(0), droppedFrameCount(0), frame_datum(0), n_markers(0), n_unlabeled_markers(0),
00214         marker_data_enabled(false), unlabeled_marker_data_enabled(false), grab_frames_(false)
00215 
00216   {
00217     // Diagnostics
00218     diag_updater.add("ViconReceiver Status", this, &ViconReceiver::diagnostics);
00219     diag_updater.add(freq_status_);
00220     diag_updater.setHardwareID("none");
00221     diag_updater.force_update();
00222     // Parameters
00223     nh_priv.param("stream_mode", stream_mode_, stream_mode_);
00224     nh_priv.param("datastream_hostport", host_name_, host_name_);
00225     nh_priv.param("tf_ref_frame_id", tf_ref_frame_id_, tf_ref_frame_id_);
00226     nh_priv.param("broadcast_transform", broadcast_tf_, true);
00227     nh_priv.param("publish_transform", publish_tf_, true);
00228     nh_priv.param("publish_markers", publish_markers_, true);
00229     if (init_vicon() == false){
00230       ROS_ERROR("Error while connecting to Vicon. Exiting now.");
00231       return;
00232     }
00233     // Service Server
00234     ROS_INFO("setting up grab_vicon_pose service server ... ");
00235     m_grab_vicon_pose_service_server = nh_priv.advertiseService("grab_vicon_pose", &ViconReceiver::grabPoseCallback,
00236                                                                 this);
00237 
00238     ROS_INFO("setting up segment calibration service server ... ");
00239     calibrate_segment_server_ = nh_priv.advertiseService("calibrate_segment", &ViconReceiver::calibrateSegmentCallback,
00240                                                          this);
00241 
00242     // Publishers
00243     if(publish_markers_)
00244     {
00245       marker_pub_ = nh.advertise<vicon_bridge::Markers>(tracked_frame_suffix_ + "/markers", 10);
00246     }
00247     startGrabbing();
00248   }
00249 
00250   ~ViconReceiver()
00251   {
00252     for (size_t i = 0; i < time_log_.size(); i++)
00253     {
00254       std::cout << time_log_[i] << std::endl;
00255     }
00256     if (shutdown_vicon() == false){
00257       ROS_ERROR("Error while shutting down Vicon.");
00258     }
00259   }
00260 
00261 private:
00262   void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
00263   {
00264     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
00265     stat.add("latest VICON frame number", lastFrameNumber);
00266     stat.add("dropped frames", droppedFrameCount);
00267     stat.add("framecount", frameCount);
00268     stat.add("# markers", n_markers);
00269     stat.add("# unlabeled markers", n_unlabeled_markers);
00270   }
00271 
00272   bool init_vicon()
00273   {
00274     ROS_INFO_STREAM("Connecting to Vicon DataStream SDK at " << host_name_ << " ...");
00275 
00276     ros::Duration d(1);
00277     Result::Enum result(Result::Unknown);
00278 
00279     while (!msvcbridge::IsConnected().Connected)
00280     {
00281       msvcbridge::Connect(host_name_);
00282       ROS_INFO(".");
00283       d.sleep();
00284       ros::spinOnce();
00285       if (!ros::ok())
00286         return false;
00287     }
00288     ROS_ASSERT(msvcbridge::IsConnected().Connected);
00289     ROS_INFO_STREAM("... connected!");
00290 
00291     // ClientPullPrefetch doesn't make much sense here, since we're only forwarding the data
00292     if (stream_mode_ == "ServerPush")
00293     {
00294       result = msvcbridge::SetStreamMode(StreamMode::ServerPush).Result;
00295     }
00296     else if (stream_mode_ == "ClientPull")
00297     {
00298       result = msvcbridge::SetStreamMode(StreamMode::ClientPull).Result;
00299     }
00300     else
00301     {
00302       ROS_FATAL("Unknown stream mode -- options are ServerPush, ClientPull");
00303       ros::shutdown();
00304     }
00305 
00306     ROS_INFO_STREAM("Setting Stream Mode to " << stream_mode_<< ": "<< Adapt(result));
00307 
00308     msvcbridge::SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up); // 'Z-up'
00309     Output_GetAxisMapping _Output_GetAxisMapping = msvcbridge::GetAxisMapping();
00310 
00311     ROS_INFO_STREAM("Axis Mapping: X-" << Adapt(_Output_GetAxisMapping.XAxis) << " Y-"
00312         << Adapt(_Output_GetAxisMapping.YAxis) << " Z-" << Adapt(_Output_GetAxisMapping.ZAxis));
00313 
00314     msvcbridge::EnableSegmentData();
00315     ROS_ASSERT(msvcbridge::IsSegmentDataEnabled().Enabled);
00316 
00317     Output_GetVersion _Output_GetVersion = msvcbridge::GetVersion();
00318     ROS_INFO_STREAM("Version: " << _Output_GetVersion.Major << "." << _Output_GetVersion.Minor << "."
00319         << _Output_GetVersion.Point);
00320     return true;
00321   }
00322 
00323   void createSegmentThread(const string subject_name, const string segment_name)
00324   {
00325     ROS_INFO("creating new object %s/%s ...",subject_name.c_str(), segment_name.c_str() );
00326     boost::mutex::scoped_lock lock(segments_mutex_);
00327     SegmentPublisher & spub = segment_publishers_[subject_name + "/" + segment_name];
00328 
00329     // we don't need the lock anymore, since rest is protected by is_ready
00330     lock.unlock();
00331 
00332     if(publish_tf_)
00333     {
00334       spub.pub = nh.advertise<geometry_msgs::TransformStamped>(tracked_frame_suffix_ + "/" + subject_name + "/"
00335                                                                                                             + segment_name, 10);
00336     }
00337     // try to get zero pose from parameter server
00338     string param_suffix(subject_name + "/" + segment_name + "/zero_pose/");
00339     double qw, qx, qy, qz, x, y, z;
00340     bool have_params = true;
00341     have_params = have_params && nh_priv.getParam(param_suffix + "orientation/w", qw);
00342     have_params = have_params && nh_priv.getParam(param_suffix + "orientation/x", qx);
00343     have_params = have_params && nh_priv.getParam(param_suffix + "orientation/y", qy);
00344     have_params = have_params && nh_priv.getParam(param_suffix + "orientation/z", qz);
00345     have_params = have_params && nh_priv.getParam(param_suffix + "position/x", x);
00346     have_params = have_params && nh_priv.getParam(param_suffix + "position/y", y);
00347     have_params = have_params && nh_priv.getParam(param_suffix + "position/z", z);
00348 
00349     if (have_params)
00350     {
00351       ROS_INFO("loaded zero pose for %s/%s", subject_name.c_str(), segment_name.c_str());
00352       spub.calibration_pose.setRotation(tf::Quaternion(qx, qy, qz, qw));
00353       spub.calibration_pose.setOrigin(tf::Vector3(x, y, z));
00354       spub.calibration_pose = spub.calibration_pose.inverse();
00355     }
00356     else
00357     {
00358       ROS_WARN("unable to load zero pose for %s/%s", subject_name.c_str(), segment_name.c_str());
00359       spub.calibration_pose.setIdentity();
00360     }
00361 
00362     spub.is_ready = true;
00363     ROS_INFO("... done, advertised as \" %s/%s/%s\" ", tracked_frame_suffix_.c_str(), subject_name.c_str(), segment_name.c_str());
00364 
00365   }
00366 
00367   void createSegment(const string subject_name, const string segment_name)
00368   {
00369     boost::thread(&ViconReceiver::createSegmentThread, this, subject_name, segment_name);
00370   }
00371 
00372   void grabThread()
00373   {
00374     ros::Duration d(1.0 / 240.0);
00375 //    ros::Time last_time = ros::Time::now();
00376 //    double fps = 100.;
00377 //    ros::Duration diff;
00378 //    std::stringstream time_log;
00379     while (ros::ok() && grab_frames_)
00380     {
00381       while (msvcbridge::GetFrame().Result != Result::Success && ros::ok())
00382       {
00383         ROS_INFO("getFrame returned false");
00384         d.sleep();
00385       }
00386       now_time = ros::Time::now();
00387 //      diff = now_time-last_time;
00388 //      fps = 1.0/(0.9/fps + 0.1*diff.toSec());
00389 //      time_log.clear();
00390 //      time_log.str("");
00391 //      time_log <<"timings: dt="<<diff<<" fps=" <<fps;
00392 //      time_log_.push_back(time_log.str());
00393 //      last_time = now_time;
00394 
00395       bool was_new_frame = process_frame();
00396       ROS_WARN_COND(!was_new_frame, "grab frame returned false");
00397 
00398       diag_updater.update();
00399     }
00400   }
00401 
00402   bool shutdown_vicon()
00403   {
00404     ROS_INFO_STREAM("stopping grabbing thread");
00405     stopGrabbing();
00406     ROS_INFO_STREAM("Disconnecting from Vicon DataStream SDK");
00407     msvcbridge::Disconnect();
00408     ROS_ASSERT(!msvcbridge::IsConnected().Connected);
00409     ROS_INFO_STREAM("... disconnected.");
00410     return true;
00411   }
00412 
00413   bool process_frame()
00414   {
00415     static ros::Time lastTime;
00416     Output_GetFrameNumber OutputFrameNum = msvcbridge::GetFrameNumber();
00417 
00418     //frameCount++;
00419     //ROS_INFO_STREAM("Grabbed a frame: " << OutputFrameNum.FrameNumber);
00420     int frameDiff = 0;
00421     if (lastFrameNumber != 0)
00422     {
00423       frameDiff = OutputFrameNum.FrameNumber - lastFrameNumber;
00424       frameCount += frameDiff;
00425       if ((frameDiff) > 1)
00426       {
00427         droppedFrameCount += frameDiff;
00428         double droppedFramePct = (double)droppedFrameCount / frameCount * 100;
00429         ROS_DEBUG_STREAM(frameDiff << " more (total " << droppedFrameCount << "/" << frameCount << ", "
00430             << droppedFramePct << "%) frame(s) dropped. Consider adjusting rates.");
00431       }
00432     }
00433     lastFrameNumber = OutputFrameNum.FrameNumber;
00434 
00435     if (frameDiff == 0)
00436     {
00437       return false;
00438     }
00439     else
00440     {
00441       freq_status_.tick();
00442       ros::Duration vicon_latency(msvcbridge::GetLatencyTotal().Total);
00443 
00444       if(publish_tf_ || broadcast_tf_)
00445       {
00446         process_subjects(now_time - vicon_latency);
00447       }
00448 
00449       if(publish_markers_)
00450       {
00451         process_markers(now_time - vicon_latency, lastFrameNumber);
00452       }
00453 
00454       lastTime = now_time;
00455       return true;
00456     }
00457   }
00458 
00459   void process_subjects(const ros::Time& frame_time)
00460   {
00461     string tracked_frame, subject_name, segment_name;
00462     unsigned int n_subjects = msvcbridge::GetSubjectCount().SubjectCount;
00463     SegmentMap::iterator pub_it;
00464     tf::Transform transform;
00465     std::vector<tf::StampedTransform, std::allocator<tf::StampedTransform> > transforms;
00466     geometry_msgs::TransformStampedPtr pose_msg(new geometry_msgs::TransformStamped);
00467     static unsigned int cnt = 0;
00468 
00469     for (unsigned int i_subjects = 0; i_subjects < n_subjects; i_subjects++)
00470     {
00471 
00472       subject_name = msvcbridge::GetSubjectName(i_subjects).SubjectName;
00473       unsigned int n_segments = msvcbridge::GetSegmentCount(subject_name).SegmentCount;
00474 
00475       for (unsigned int i_segments = 0; i_segments < n_segments; i_segments++)
00476       {
00477         segment_name = msvcbridge::GetSegmentName(subject_name, i_segments).SegmentName;
00478 
00479         Output_GetSegmentGlobalTranslation trans = msvcbridge::GetSegmentGlobalTranslation(subject_name, segment_name);
00480         Output_GetSegmentGlobalRotationQuaternion quat = msvcbridge::GetSegmentGlobalRotationQuaternion(subject_name,
00481                                                                                                         segment_name);
00482 
00483         if (trans.Result == Result::Success && quat.Result == Result::Success)
00484         {
00485           if (!trans.Occluded && !quat.Occluded)
00486           {
00487             transform.setOrigin(tf::Vector3(trans.Translation[0] / 1000, trans.Translation[1] / 1000,
00488                                                   trans.Translation[2] / 1000));
00489             transform.setRotation(tf::Quaternion(quat.Rotation[0], quat.Rotation[1], quat.Rotation[2],
00490                                                        quat.Rotation[3]));
00491 
00492             tracked_frame = tracked_frame_suffix_ + "/" + subject_name + "/" + segment_name;
00493 
00494             boost::mutex::scoped_try_lock lock(segments_mutex_);
00495 
00496             if (lock.owns_lock())
00497             {
00498               pub_it = segment_publishers_.find(subject_name + "/" + segment_name);
00499               if (pub_it != segment_publishers_.end())
00500               {
00501                 SegmentPublisher & seg = pub_it->second;
00502                 //ros::Time thisTime = now_time - ros::Duration(latencyInMs / 1000);
00503 
00504                 if (seg.is_ready)
00505                 {
00506                   transform = transform * seg.calibration_pose;
00507                   transforms.push_back(tf::StampedTransform(transform, frame_time, tf_ref_frame_id_, tracked_frame));
00508 //                  transform = tf::StampedTransform(flyer_transform, frame_time, tf_ref_frame_id_, tracked_frame);
00509 //                  tf_broadcaster_.sendTransform(transform);
00510 
00511                   if(publish_tf_)
00512                   {
00513                     tf::transformStampedTFToMsg(transforms.back(), *pose_msg);
00514                     seg.pub.publish(pose_msg);
00515                   }
00516                 }
00517               }
00518               else
00519               {
00520                 lock.unlock();
00521                 createSegment(subject_name, segment_name);
00522               }
00523             }
00524           }
00525           else
00526           {
00527             if (cnt % 100 == 0)
00528               ROS_WARN_STREAM("" << subject_name <<" occluded, not publishing... " );
00529           }
00530         }
00531         else
00532         {
00533           ROS_WARN("GetSegmentGlobalTranslation/Rotation failed (result = %s, %s), not publishing...",
00534               Adapt(trans.Result).c_str(), Adapt(quat.Result).c_str());
00535         }
00536       }
00537     }
00538 
00539     if(broadcast_tf_)
00540     {
00541       tf_broadcaster_.sendTransform(transforms);
00542     }
00543     cnt++;
00544   }
00545 
00546   void process_markers(const ros::Time& frame_time, unsigned int vicon_frame_num)
00547   {
00548     if (marker_pub_.getNumSubscribers() > 0)
00549     {
00550       if (not marker_data_enabled)
00551       {
00552         msvcbridge::EnableMarkerData();
00553         ROS_ASSERT(msvcbridge::IsMarkerDataEnabled().Enabled);
00554         marker_data_enabled = true;
00555       }
00556       if (not unlabeled_marker_data_enabled)
00557       {
00558         msvcbridge::EnableUnlabeledMarkerData();
00559         ROS_ASSERT(msvcbridge::IsUnlabeledMarkerDataEnabled().Enabled);
00560         unlabeled_marker_data_enabled = true;
00561       }
00562       n_markers = 0;
00563       vicon_bridge::Markers markers_msg;
00564       markers_msg.header.stamp = frame_time;
00565       markers_msg.frame_number = vicon_frame_num;
00566       // Count the number of subjects
00567       unsigned int SubjectCount = msvcbridge::GetSubjectCount().SubjectCount;
00568       // Get labeled markers
00569       for (unsigned int SubjectIndex = 0; SubjectIndex < SubjectCount; ++SubjectIndex)
00570       {
00571         std::string this_subject_name = msvcbridge::GetSubjectName(SubjectIndex).SubjectName;
00572         // Count the number of markers
00573         unsigned int num_subject_markers = msvcbridge::GetMarkerCount(this_subject_name).MarkerCount;
00574         n_markers += num_subject_markers;
00575         //std::cout << "    Markers (" << MarkerCount << "):" << std::endl;
00576         for (unsigned int MarkerIndex = 0; MarkerIndex < num_subject_markers; ++MarkerIndex)
00577         {
00578           vicon_bridge::Marker this_marker;
00579           this_marker.marker_name = msvcbridge::GetMarkerName(this_subject_name, MarkerIndex).MarkerName;
00580           this_marker.subject_name = this_subject_name;
00581           this_marker.segment_name
00582               = msvcbridge::GetMarkerParentName(this_subject_name, this_marker.marker_name).SegmentName;
00583 
00584           // Get the global marker translation
00585           Output_GetMarkerGlobalTranslation _Output_GetMarkerGlobalTranslation =
00586               msvcbridge::GetMarkerGlobalTranslation(this_subject_name, this_marker.marker_name);
00587 
00588           this_marker.translation.x = _Output_GetMarkerGlobalTranslation.Translation[0];
00589           this_marker.translation.y = _Output_GetMarkerGlobalTranslation.Translation[1];
00590           this_marker.translation.z = _Output_GetMarkerGlobalTranslation.Translation[2];
00591           this_marker.occluded = _Output_GetMarkerGlobalTranslation.Occluded;
00592 
00593           markers_msg.markers.push_back(this_marker);
00594         }
00595       }
00596       // get unlabeled markers
00597       unsigned int UnlabeledMarkerCount = msvcbridge::GetUnlabeledMarkerCount().MarkerCount;
00598       //ROS_INFO("# unlabeled markers: %d", UnlabeledMarkerCount);
00599       n_markers += UnlabeledMarkerCount;
00600       n_unlabeled_markers = UnlabeledMarkerCount;
00601       for (unsigned int UnlabeledMarkerIndex = 0; UnlabeledMarkerIndex < UnlabeledMarkerCount; ++UnlabeledMarkerIndex)
00602       {
00603         // Get the global marker translation
00604         Output_GetUnlabeledMarkerGlobalTranslation _Output_GetUnlabeledMarkerGlobalTranslation =
00605             msvcbridge::GetUnlabeledMarkerGlobalTranslation(UnlabeledMarkerIndex);
00606 
00607         if (_Output_GetUnlabeledMarkerGlobalTranslation.Result == Result::Success)
00608         {
00609           vicon_bridge::Marker this_marker;
00610           this_marker.translation.x = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[0];
00611           this_marker.translation.y = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[1];
00612           this_marker.translation.z = _Output_GetUnlabeledMarkerGlobalTranslation.Translation[2];
00613           this_marker.occluded = false; // unlabeled markers can't be occluded
00614           markers_msg.markers.push_back(this_marker);
00615         }
00616         else
00617         {
00618           ROS_WARN("GetUnlabeledMarkerGlobalTranslation failed (result = %s)",
00619               Adapt(_Output_GetUnlabeledMarkerGlobalTranslation.Result).c_str());
00620 
00621         }
00622       }
00623       marker_pub_.publish(markers_msg);
00624     }
00625   }
00626 
00627   bool grabPoseCallback(vicon_bridge::viconGrabPose::Request& req, vicon_bridge::viconGrabPose::Response& resp)
00628   {
00629     ROS_INFO("Got request for a VICON pose");
00630     tf::TransformListener tf_listener;
00631     tf::StampedTransform transform;
00632     tf::Quaternion orientation(0, 0, 0, 0);
00633     tf::Vector3 position(0, 0, 0);
00634 
00635     string tracked_segment = tracked_frame_suffix_ + "/" + req.subject_name + "/" + req.segment_name;
00636 
00637     // Gather data:
00638     int N = req.n_measurements;
00639     int n_success = 0;
00640     ros::Duration timeout(0.1);
00641     ros::Duration poll_period(1.0 / 240.0);
00642 
00643     for (int k = 0; k < N; k++)
00644     {
00645       try
00646       {
00647         if (tf_listener.waitForTransform(tf_ref_frame_id_, tracked_segment, ros::Time::now(), timeout, poll_period))
00648         {
00649           tf_listener.lookupTransform(tf_ref_frame_id_, tracked_segment, ros::Time(0), transform);
00650           orientation += transform.getRotation();
00651           position += transform.getOrigin();
00652           n_success++;
00653         }
00654       }
00655       catch (tf::TransformException ex)
00656       {
00657         ROS_ERROR("%s", ex.what());
00658         //              resp.success = false;
00659         //              return false; // TODO: should we really bail here, or just try again?
00660       }
00661     }
00662 
00663     // Average the data
00664     orientation /= n_success;
00665     orientation.normalize();
00666     position /= n_success;
00667 
00668     // copy what we used to service call response:
00669     resp.success = true;
00670     resp.pose.header.stamp = ros::Time::now();
00671     resp.pose.header.frame_id = tf_ref_frame_id_;
00672     resp.pose.pose.position.x = position.x();
00673     resp.pose.pose.position.y = position.y();
00674     resp.pose.pose.position.z = position.z();
00675     resp.pose.pose.orientation.w = orientation.w();
00676     resp.pose.pose.orientation.x = orientation.x();
00677     resp.pose.pose.orientation.y = orientation.y();
00678     resp.pose.pose.orientation.z = orientation.z();
00679 
00680     return true;
00681   }
00682 
00683   bool calibrateSegmentCallback(vicon_bridge::viconCalibrateSegment::Request& req,
00684                                 vicon_bridge::viconCalibrateSegment::Response& resp)
00685   {
00686 
00687     std::string full_name = req.subject_name + "/" + req.segment_name;
00688     ROS_INFO("trying to calibrate %s", full_name.c_str());
00689 
00690     SegmentMap::iterator seg_it = segment_publishers_.find(full_name);
00691 
00692     if (seg_it == segment_publishers_.end())
00693     {
00694       ROS_WARN("frame %s not found --> not calibrating", full_name.c_str());
00695       resp.success = false;
00696       resp.status = "segment " + full_name + " not found";
00697       return false;
00698     }
00699 
00700     SegmentPublisher & seg = seg_it->second;
00701 
00702     if (seg.calibrated)
00703     {
00704       ROS_INFO("%s already calibrated, deleting old calibration", full_name.c_str());
00705       seg.calibration_pose.setIdentity();
00706     }
00707 
00708     vicon_bridge::viconGrabPose::Request grab_req;
00709     vicon_bridge::viconGrabPose::Response grab_resp;
00710 
00711     grab_req.n_measurements = req.n_measurements;
00712     grab_req.subject_name = req.subject_name;
00713     grab_req.segment_name = req.segment_name;
00714 
00715     bool ret = grabPoseCallback(grab_req, grab_resp);
00716 
00717     if (!ret)
00718     {
00719       resp.success = false;
00720       resp.status = "error while grabbing pose from Vicon";
00721       return false;
00722     }
00723 
00724     tf::Transform t;
00725     t.setOrigin(tf::Vector3(grab_resp.pose.pose.position.x, grab_resp.pose.pose.position.y,
00726                             grab_resp.pose.pose.position.z - req.z_offset));
00727     t.setRotation(tf::Quaternion(grab_resp.pose.pose.orientation.x, grab_resp.pose.pose.orientation.y,
00728                                  grab_resp.pose.pose.orientation.z, grab_resp.pose.pose.orientation.w));
00729 
00730     seg.calibration_pose = t.inverse();
00731 
00732     // write zero_pose to parameter server
00733     string param_suffix(full_name + "/zero_pose/");
00734     nh_priv.setParam(param_suffix + "orientation/w", t.getRotation().w());
00735     nh_priv.setParam(param_suffix + "orientation/x", t.getRotation().x());
00736     nh_priv.setParam(param_suffix + "orientation/y", t.getRotation().y());
00737     nh_priv.setParam(param_suffix + "orientation/z", t.getRotation().z());
00738 
00739     nh_priv.setParam(param_suffix + "position/x", t.getOrigin().x());
00740     nh_priv.setParam(param_suffix + "position/y", t.getOrigin().y());
00741     nh_priv.setParam(param_suffix + "position/z", t.getOrigin().z());
00742 
00743     ROS_INFO_STREAM("calibration completed");
00744     resp.pose = grab_resp.pose;
00745     resp.success = true;
00746     resp.status = "calibration successful";
00747     seg.calibrated = true;
00748 
00749     return true;
00750   }
00751 
00752 };
00753 
00754 int main(int argc, char** argv)
00755 {
00756   ros::init(argc, argv, "vicon");
00757 //  ViconReceiver vr;
00758 //  ros::spin();
00759 
00760   ros::AsyncSpinner aspin(1);
00761   aspin.start();
00762   ViconReceiver vr;
00763   aspin.stop();
00764   return 0;
00765 }


vicon_bridge
Author(s):
autogenerated on Mon Sep 14 2015 03:57:50