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) : stop_(true), interval_us_(interval_us)
00048 {
00049   tf_.reset(new TransformListener);
00050   psm_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf_));
00051   psm_->startStateMonitor();
00052 }
00053 
00054 TransformProvider::~TransformProvider()
00055 {
00056   stop();
00057 }
00058 
00059 void TransformProvider::start()
00060 {
00061   stop_ = false;
00062   thread_ = thread(&TransformProvider::run, this);
00063 }
00064 
00065 void TransformProvider::stop()
00066 {
00067   stop_ = true;
00068   thread_.join();
00069 }
00070 
00071 void TransformProvider::addHandle(MeshHandle handle, const string& name)
00072 {
00073   if (!stop_)
00074     throw runtime_error("Can not add handles if TransformProvider is running");
00075 
00076   handle2context_[handle].reset(new TransformContext(name));
00077 }
00078 
00079 void TransformProvider::setFrame(const string& frame)
00080 {
00081   if (frame_id_ != frame)
00082   {
00083     frame_id_ = frame;
00084     for (map<MeshHandle, shared_ptr<TransformContext> >::iterator contextIt = handle2context_.begin();
00085          contextIt != handle2context_.end(); ++contextIt)
00086     {
00087       // invalidate transformations
00088       contextIt->second->mutex_.lock();
00089       contextIt->second->transformation_.matrix().setZero();
00090       contextIt->second->mutex_.unlock();
00091     }
00092   }
00093 }
00094 
00095 bool TransformProvider::getTransform(MeshHandle handle, Affine3d& transform) const
00096 {
00097   map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.find(handle);
00098 
00099   if (contextIt == handle2context_.end())
00100   {
00101     ROS_ERROR("Unable to find mesh with handle %d", handle);
00102     return false;
00103   }
00104   contextIt->second->mutex_.lock();
00105   transform = contextIt->second->transformation_;
00106   contextIt->second->mutex_.unlock();
00107   return !(transform.matrix().isZero(0));
00108 }
00109 
00110 void TransformProvider::run()
00111 {
00112   if (handle2context_.empty())
00113     throw runtime_error("TransformProvider is listening to empty list of frames!");
00114 
00115   while (!stop_)
00116   {
00117     updateTransforms();
00118     usleep(interval_us_);
00119   }
00120 }
00121 
00122 void TransformProvider::setUpdateInterval(unsigned long usecs)
00123 {
00124   interval_us_ = usecs;
00125 }
00126 
00127 void TransformProvider::updateTransforms()
00128 {
00129   static Affine3d transformation;
00130   static robot_state::RobotStatePtr robot_state;
00131   robot_state = psm_->getStateMonitor()->getCurrentState();
00132 
00133   static string error;
00134   static Stamped<Pose> input_pose, out_pose;
00135   tf_->getLatestCommonTime(frame_id_, psm_->getPlanningScene()->getPlanningFrame(), input_pose.stamp_, &error);
00136   input_pose.frame_id_ = psm_->getPlanningScene()->getPlanningFrame();
00137 
00138   for (map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.begin();
00139        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 Jun 19 2019 19:24:12