transform_provider.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Suat Gedikli */
36 
38 #include <tf/transform_listener.h>
40 
41 using namespace std;
42 using namespace boost;
43 using namespace Eigen;
44 using namespace tf;
45 using namespace mesh_filter;
46 
47 TransformProvider::TransformProvider(unsigned long interval_us) : stop_(true), interval_us_(interval_us)
48 {
49  tf_.reset(new TransformListener);
50  psm_.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf_));
51  psm_->startStateMonitor();
52 }
53 
55 {
56  stop();
57 }
58 
60 {
61  stop_ = false;
62  thread_ = thread(&TransformProvider::run, this);
63 }
64 
66 {
67  stop_ = true;
68  thread_.join();
69 }
70 
71 void TransformProvider::addHandle(MeshHandle handle, const string& name)
72 {
73  if (!stop_)
74  throw runtime_error("Can not add handles if TransformProvider is running");
75 
76  handle2context_[handle].reset(new TransformContext(name));
77 }
78 
79 void TransformProvider::setFrame(const string& frame)
80 {
81  if (frame_id_ != frame)
82  {
83  frame_id_ = frame;
84  for (map<MeshHandle, shared_ptr<TransformContext> >::iterator contextIt = handle2context_.begin();
85  contextIt != handle2context_.end(); ++contextIt)
86  {
87  // invalidate transformations
88  contextIt->second->mutex_.lock();
89  contextIt->second->transformation_.matrix().setZero();
90  contextIt->second->mutex_.unlock();
91  }
92  }
93 }
94 
95 bool TransformProvider::getTransform(MeshHandle handle, Affine3d& transform) const
96 {
97  map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.find(handle);
98 
99  if (contextIt == handle2context_.end())
100  {
101  ROS_ERROR("Unable to find mesh with handle %d", handle);
102  return false;
103  }
104  contextIt->second->mutex_.lock();
105  transform = contextIt->second->transformation_;
106  contextIt->second->mutex_.unlock();
107  return !(transform.matrix().isZero(0));
108 }
109 
111 {
112  if (handle2context_.empty())
113  throw runtime_error("TransformProvider is listening to empty list of frames!");
114 
115  while (!stop_)
116  {
118  usleep(interval_us_);
119  }
120 }
121 
122 void TransformProvider::setUpdateInterval(unsigned long usecs)
123 {
124  interval_us_ = usecs;
125 }
126 
128 {
129  static Affine3d transformation;
130  static robot_state::RobotStatePtr robot_state;
131  robot_state = psm_->getStateMonitor()->getCurrentState();
132 
133  static string error;
134  static Stamped<Pose> input_pose, out_pose;
135  tf_->getLatestCommonTime(frame_id_, psm_->getPlanningScene()->getPlanningFrame(), input_pose.stamp_, &error);
136  input_pose.frame_id_ = psm_->getPlanningScene()->getPlanningFrame();
137 
138  for (map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt = handle2context_.begin();
139  contextIt != handle2context_.end(); ++contextIt)
140  {
141  transformation = robot_state->getLinkState(contextIt->second->frame_id_)->getGlobalCollisionBodyTransform();
142  poseEigenToTF(transformation, input_pose);
143  try
144  {
145  tf_->transformPose(frame_id_, input_pose, out_pose);
146  }
147  catch (const tf::TransformException& ex)
148  {
149  handle2context_[contextIt->first]->mutex_.lock();
150  handle2context_[contextIt->first]->transformation_.matrix().setZero();
151  handle2context_[contextIt->first]->mutex_.unlock();
152  continue;
153  }
154  catch (std::exception& ex)
155  {
156  ROS_ERROR("Caught %s while updating transforms", ex.what());
157  }
158  poseTFToEigen(out_pose, transformation);
159  handle2context_[contextIt->first]->mutex_.lock();
160  handle2context_[contextIt->first]->transformation_ = transformation;
161  handle2context_[contextIt->first]->mutex_.unlock();
162  }
163 }
std::map< mesh_filter::MeshHandle, TransformContextPtr > handle2context_
mapping between the mesh handle and its context
~TransformProvider()
Destructor.
void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
void start()
starts the updating process. Done in a seperate thread
void updateTransforms()
this method is called periodically by the dedicated thread and updates all the transformations of the...
unsigned long interval_us_
update interval in micro seconds
void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e)
planning_scene_monitor::PlanningSceneMonitorPtr psm_
SceneMonitor used to get current states.
boost::thread thread_
thread object
std::string frame_id_
the camera frame id
bool getTransform(mesh_filter::MeshHandle handle, Eigen::Affine3d &transform) const
returns the current transformation of a mesh given by its handle
void addHandle(mesh_filter::MeshHandle handle, const std::string &name)
registers a mesh with its handle
void setUpdateInterval(unsigned long usecs)
sets the update interval in micro seconds. This should be low enough to reduce the system load but hi...
void run()
The entry point of the dedicated thread that updates the transformations periodically.
ros::Time stamp_
std::string frame_id_
Context Object for registered frames.
TransformProvider(unsigned long interval_us=30000)
Constructor.
void setFrame(const std::string &frame)
sets the camera frame id. The returned transformations are in respect to this coordinate frame ...
#define ROS_ERROR(...)
void stop()
stops the update process/thread.
boost::shared_ptr< tf::TransformListener > tf_
TransformListener used to listen and update transformations.
unsigned int MeshHandle


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23