transform_provider.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
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 Willow Garage 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 /* Author: Suat Gedikli */
00036 
00037 #include <moveit/mesh_filter/transform_provider.h>
00038 #include <tf/transform_listener.h>
00039 #include <tf_conversions/tf_eigen.h>
00040 
00041 using namespace std;
00042 using namespace boost;
00043 using namespace Eigen;
00044 using namespace tf;
00045 using namespace mesh_filter;
00046 
00047 TransformProvider::TransformProvider (unsigned long interval_us)
00048 : stop_ (true)
00049 , interval_us_ (interval_us)
00050 {
00051   tf_.reset (new TransformListener);
00052   psm_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf_));
00053   psm_->startStateMonitor();
00054 }
00055 
00056 TransformProvider::~TransformProvider ()
00057 {
00058   stop ();
00059 }
00060 
00061 void TransformProvider::start ()
00062 {
00063   stop_ = false;
00064   thread_ = thread (&TransformProvider::run, this);
00065 }
00066 
00067 void TransformProvider::stop ()
00068 {
00069   stop_ = true;
00070   thread_.join ();
00071 }
00072 
00073 void TransformProvider::addHandle (MeshHandle handle, const string& name)
00074 {
00075   if (!stop_)
00076     throw runtime_error ("Can not add handles if TransformProvider is running");
00077 
00078   handle2context_ [handle].reset (new TransformContext (name));
00079 }
00080 
00081 void TransformProvider::setFrame (const string& frame)
00082 {
00083   if (frame_id_ != frame)
00084   {
00085     frame_id_ = frame;
00086     for (map<MeshHandle, shared_ptr<TransformContext> >::iterator contextIt = handle2context_.begin (); contextIt != handle2context_.end(); ++contextIt)
00087     {
00088       // invalidate transformations
00089       contextIt->second->mutex_.lock ();
00090       contextIt->second->transformation_.matrix().setZero ();
00091       contextIt->second->mutex_.unlock ();
00092     }
00093   }
00094 }
00095 
00096 bool TransformProvider::getTransform (MeshHandle handle, Affine3d& transform) const
00097 {
00098   map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.find(handle);
00099 
00100   if (contextIt == handle2context_.end())
00101   {
00102     ROS_ERROR("Unable to find mesh with handle %d", handle);
00103     return false;
00104   }
00105   contextIt->second->mutex_.lock ();
00106   transform = contextIt->second->transformation_;
00107   contextIt->second->mutex_.unlock ();
00108   return !(transform.matrix ().isZero (0));
00109 }
00110 
00111 void TransformProvider::run ()
00112 {
00113   if (handle2context_.empty ())
00114     throw runtime_error ("TransformProvider is listening to empty list of frames!");
00115 
00116   while (!stop_)
00117   {
00118     updateTransforms ();
00119     usleep (interval_us_);
00120   }
00121 }
00122 
00123 void TransformProvider::setUpdateInterval (unsigned long usecs)
00124 {
00125   interval_us_ = usecs;
00126 }
00127 
00128 void TransformProvider::updateTransforms ()
00129 {
00130   static Affine3d transformation;
00131   static robot_state::RobotStatePtr robot_state;
00132   robot_state = psm_->getStateMonitor()->getCurrentState();
00133 
00134   static string error;
00135   static Stamped<Pose> input_pose, out_pose;
00136   tf_->getLatestCommonTime(frame_id_, psm_->getPlanningScene()->getPlanningFrame(), input_pose.stamp_, &error);
00137   input_pose.frame_id_ = psm_->getPlanningScene()->getPlanningFrame();
00138 
00139   for (map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.begin(); contextIt != handle2context_.end (); ++contextIt)
00140   {
00141     transformation = robot_state->getLinkState(contextIt->second->frame_id_)->getGlobalCollisionBodyTransform();
00142     poseEigenToTF(transformation, input_pose);
00143     try
00144     {
00145       tf_->transformPose(frame_id_, input_pose, out_pose);
00146     }
00147     catch (const tf::TransformException& ex)
00148     {
00149       handle2context_ [contextIt->first]->mutex_.lock ();
00150       handle2context_ [contextIt->first]->transformation_.matrix ().setZero ();
00151       handle2context_ [contextIt->first]->mutex_.unlock ();
00152       continue;
00153     }
00154     catch(...)
00155     {
00156       ROS_ERROR("unknwon tf error");
00157     }
00158     poseTFToEigen(out_pose, transformation);
00159     handle2context_ [contextIt->first]->mutex_.lock ();
00160     handle2context_ [contextIt->first]->transformation_ = transformation;
00161     handle2context_ [contextIt->first]->mutex_.unlock ();
00162   }
00163 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Aug 26 2015 12:43:21