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)
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
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 }