transform_provider.h
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 
37 #ifndef MOVEIT_MESH_FILTER_TRANSFORM_PROVIDER_
38 #define MOVEIT_MESH_FILTER_TRANSFORM_PROVIDER_
39 
40 #include <string>
41 #include <boost/thread/thread.hpp>
42 #include <boost/thread/mutex.hpp>
43 #include <boost/shared_ptr.hpp>
45 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
47 #include <map>
48 
49 namespace tf
50 {
51 class TransformListener;
52 }
53 
59 {
60 public:
66  TransformProvider(unsigned long interval_us = 30000);
67 
70 
78  bool getTransform(mesh_filter::MeshHandle handle, Eigen::Affine3d& transform) const;
79 
86  void addHandle(mesh_filter::MeshHandle handle, const std::string& name);
87 
93  void setFrame(const std::string& frame);
94 
99  void start();
100 
105  void stop();
106 
114  void setUpdateInterval(unsigned long usecs);
115 
116 private:
122  void updateTransforms();
123 
125 
131  {
132  TransformContext(const std::string& name) : frame_id_(name)
133  {
134  transformation_.matrix().setZero();
135  }
137  std::string frame_id_;
138  Eigen::Affine3d transformation_;
139  boost::mutex mutex_;
140  };
141 
146  void run();
147 
149  std::map<mesh_filter::MeshHandle, TransformContextPtr> handle2context_;
150 
153 
155  planning_scene_monitor::PlanningSceneMonitorPtr psm_;
156 
158  std::string frame_id_;
159 
161  boost::thread thread_;
162 
164  bool stop_;
165 
167  unsigned long interval_us_;
168 };
169 #endif
std::map< mesh_filter::MeshHandle, TransformContextPtr > handle2context_
mapping between the mesh handle and its context
unsigned long interval_us_
update interval in micro seconds
planning_scene_monitor::PlanningSceneMonitorPtr psm_
SceneMonitor used to get current states.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string frame_id_
boost::thread thread_
thread object
std::string frame_id_
the camera frame id
MOVEIT_CLASS_FORWARD(Job)
TransformContext(const std::string &name)
Class that caches and updates transformations for given frames.
Context Object for registered frames.
void run(ClassLoader *loader)
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