Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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 }