world.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: Ioan Sucan, Acorn Pooley, Sachin Chitta */
36 
37 #pragma once
38 
40 
41 #include <string>
42 #include <vector>
43 #include <map>
44 #include <boost/function.hpp>
45 #include <Eigen/Geometry>
46 #include <eigen_stl_containers/eigen_stl_vector_container.h>
48 
49 namespace shapes
50 {
51 MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc
52 }
53 
54 namespace collision_detection
55 {
56 MOVEIT_CLASS_FORWARD(World); // Defines WorldPtr, ConstPtr, WeakPtr... etc
57 
59 class World
60 {
61 public:
63  World();
64 
68  World(const World& other);
69 
70  virtual ~World();
71 
72  /**********************************************************************/
73  /* Collision Bodies */
74  /**********************************************************************/
75 
77 
79  struct Object
80  {
81  Object(const std::string& id) : id_(id)
82  {
83  }
84 
86 
88  std::string id_;
89 
101  std::vector<shapes::ShapeConstPtr> shapes_;
102 
106  EigenSTL::vector_Isometry3d shape_poses_;
107 
113  };
114 
116  std::vector<std::string> getObjectIds() const;
117 
119  ObjectConstPtr getObject(const std::string& object_id) const;
120 
122  using const_iterator = std::map<std::string, ObjectPtr>::const_iterator;
125  {
126  return objects_.begin();
127  }
130  {
131  return objects_.end();
132  }
134  std::size_t size() const
135  {
136  return objects_.size();
137  }
139  const_iterator find(const std::string& id) const
140  {
141  return objects_.find(id);
142  }
143 
145  bool hasObject(const std::string& object_id) const;
146 
149  bool knowsTransform(const std::string& name) const;
150 
155  const Eigen::Isometry3d& getTransform(const std::string& name) const;
156 
161  const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const;
162 
168  void addToObject(const std::string& object_id, const std::vector<shapes::ShapeConstPtr>& shapes,
169  const EigenSTL::vector_Isometry3d& poses);
170 
175  void addToObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& pose);
176 
179  bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape,
180  const Eigen::Isometry3d& pose);
181 
183  bool moveObject(const std::string& object_id, const Eigen::Isometry3d& transform);
184 
191  bool removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape);
192 
197  bool removeObject(const std::string& object_id);
198 
200  bool setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses);
201 
205  void clearObjects();
206 
208  {
210  CREATE = 1,
211  DESTROY = 2,
213  ADD_SHAPE = 8,
215  };
216 
220  class Action
221  {
222  public:
224  {
225  }
226  Action(int v) : action_(v)
227  {
228  }
229  operator ActionBits() const
230  {
231  return ActionBits(action_);
232  }
233 
234  private:
235  int action_;
236  };
237 
238 private:
239  class Observer;
240 
241 public:
243  {
244  public:
246  {
247  }
248 
249  private:
251  {
252  }
254  friend class World;
255  };
256 
257  using ObserverCallbackFn = boost::function<void(const ObjectConstPtr&, Action)>;
258 
264 
266  void removeObserver(const ObserverHandle observer_handle);
267 
270  void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const;
271 
272 private:
274  void notify(const ObjectConstPtr& /*obj*/, Action /*action*/);
275 
277  void notifyAll(Action action);
278 
282  void ensureUnique(ObjectPtr& obj);
283 
284  /* Add a shape with no checking */
285  virtual void addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape,
286  const Eigen::Isometry3d& pose);
287 
289  std::map<std::string, ObjectPtr> objects_;
290 
292  class Observer
293  {
294  public:
295  Observer(const ObserverCallbackFn& callback) : callback_(callback)
296  {
297  }
299  };
300 
302  std::vector<Observer*> observers_;
303 };
304 } // namespace collision_detection
collision_detection::World::addObserver
ObserverHandle addObserver(const ObserverCallbackFn &callback)
register a callback function for notification of changes. callback will be called right after any cha...
Definition: world.cpp:333
collision_detection::World::ADD_SHAPE
@ ADD_SHAPE
Definition: world.h:213
collision_detection::World::Object::shape_poses_
EigenSTL::vector_Isometry3d shape_poses_
The poses of the corresponding entries in shapes_.
Definition: world.h:106
collision_detection::World::setSubframesOfObject
bool setSubframesOfObject(const std::string &object_id, const moveit::core::FixedTransformsMap &subframe_poses)
Set subframes on an object.
Definition: world.cpp:318
collision_detection::World::notifyAll
void notifyAll(Action action)
Definition: world.cpp:353
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:211
shapes
collision_detection::World::hasObject
bool hasObject(const std::string &object_id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:169
collision_detection::World::moveObject
bool moveObject(const std::string &object_id, const Eigen::Isometry3d &transform)
Move all shapes in an object according to the given transform specified in world frame.
Definition: world.cpp:255
collision_detection::World::Object::Object
Object(const std::string &id)
Definition: world.h:81
collision_detection::World::size
std::size_t size() const
Definition: world.h:134
collision_detection::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
collision_detection::World::removeShapeFromObject
bool removeShapeFromObject(const std::string &object_id, const shapes::ShapeConstPtr &shape)
Remove shape from object. Shape equality is verified by comparing pointers. Ownership of the object i...
Definition: world.cpp:272
collision_detection::World::observers_
std::vector< Observer * > observers_
All registered observers of this world representation.
Definition: world.h:302
collision_detection::World::find
const_iterator find(const std::string &id) const
Definition: world.h:139
collision_detection::World::moveShapeInObject
bool moveShapeInObject(const std::string &object_id, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &pose)
Update the pose of a shape in an object. Shape equality is verified by comparing pointers....
Definition: world.cpp:234
collision_detection::World::ObserverHandle::ObserverHandle
ObserverHandle()
Definition: world.h:245
collision_detection::World::addToObject
void addToObject(const std::string &object_id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &poses)
Add shapes to an object in the map. This function makes repeated calls to addToObjectInternal() to ad...
Definition: world.cpp:99
collision_detection::World::Object::shapes_
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition: world.h:101
collision_detection::World::ObserverHandle::observer_
const Observer * observer_
Definition: world.h:253
collision_detection::World::Observer::callback_
ObserverCallbackFn callback_
Definition: world.h:298
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
collision_detection::World
Maintain a representation of the environment.
Definition: world.h:59
collision_detection::World::Action
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:220
collision_detection::World::ObserverHandle
Definition: world.h:242
class_forward.h
shapes::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(Shape)
collision_detection::World::Action::action_
int action_
Definition: world.h:235
collision_detection::World::UNINITIALIZED
@ UNINITIALIZED
Definition: world.h:209
collision_detection::World::MOVEIT_STRUCT_FORWARD
MOVEIT_STRUCT_FORWARD(Object)
collision_detection::World::begin
const_iterator begin() const
Definition: world.h:124
collision_detection::World::notifyObserverAllObjects
void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
Definition: world.cpp:365
collision_detection::World::end
const_iterator end() const
Definition: world.h:129
transforms.h
collision_detection::World::removeObserver
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:340
collision_detection::World::ObserverHandle::ObserverHandle
ObserverHandle(const Observer *o)
Definition: world.h:250
collision_detection::World::ActionBits
ActionBits
Definition: world.h:207
collision_detection::World::getObject
ObjectConstPtr getObject(const std::string &object_id) const
Get a particular object.
Definition: world.cpp:154
collision_detection::World::ObserverCallbackFn
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:257
collision_detection::World::World
World()
Constructor.
Definition: world.cpp:76
collision_detection::World::Object::subframe_poses_
moveit::core::FixedTransformsMap subframe_poses_
Transforms to subframes on the object. Use them to define points of interest on an object to plan wit...
Definition: world.h:112
collision_detection::World::knowsTransform
bool knowsTransform(const std::string &name) const
Check if an object or subframe with given name exists in the collision world. A subframe name needs t...
Definition: world.cpp:174
collision_detection::World::addToObjectInternal
virtual void addToObjectInternal(const ObjectPtr &obj, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &pose)
Definition: world.cpp:91
collision_detection::World::Object::id_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
Definition: world.h:88
moveit::core::FixedTransformsMap
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:117
collision_detection::World::notify
void notify(const ObjectConstPtr &, Action)
Definition: world.cpp:359
collision_detection::World::const_iterator
std::map< std::string, ObjectPtr >::const_iterator const_iterator
Definition: world.h:122
collision_detection::World::objects_
std::map< std::string, ObjectPtr > objects_
Definition: world.h:289
collision_detection::World::Object
A representation of an object.
Definition: world.h:79
collision_detection::World::ensureUnique
void ensureUnique(ObjectPtr &obj)
Make sure that the object named id is known only to this instance of the World. If the object is know...
Definition: world.cpp:163
collision_detection::World::MOVE_SHAPE
@ MOVE_SHAPE
Definition: world.h:212
collision_detection::World::Action::Action
Action()
Definition: world.h:223
collision_detection::World::Action::Action
Action(int v)
Definition: world.h:226
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::World::getTransform
const Eigen::Isometry3d & getTransform(const std::string &name) const
Get the transform to an object or subframe with given name. If name does not exist,...
Definition: world.cpp:194
collision_detection::World::removeObject
bool removeObject(const std::string &object_id)
Remove a particular object. If there are no external pointers to the corresponding instance of Object...
Definition: world.cpp:300
collision_detection::World::CREATE
@ CREATE
Definition: world.h:210
collision_detection::World::Observer
Definition: world.h:292
collision_detection::World::Observer::Observer
Observer(const ObserverCallbackFn &callback)
Definition: world.h:295
collision_detection::World::~World
virtual ~World()
Definition: world.cpp:85
collision_detection::World::clearObjects
void clearObjects()
Clear all objects. If there are no other pointers to corresponding instances of Objects,...
Definition: world.cpp:312
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition: world.h:214
collision_detection::World::getObjectIds
std::vector< std::string > getObjectIds() const
Get the list of Object ids.
Definition: world.cpp:146
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 12 2020 03:25:45