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& object_id) : id_(object_id)
82  {
83  }
84 
86 
88  std::string id_;
89 
92  Eigen::Isometry3d pose_;
93 
97  std::vector<shapes::ShapeConstPtr> shapes_;
98 
102  EigenSTL::vector_Isometry3d shape_poses_;
103 
107  EigenSTL::vector_Isometry3d global_shape_poses_;
108 
114 
118  };
119 
121  std::vector<std::string> getObjectIds() const;
122 
124  ObjectConstPtr getObject(const std::string& object_id) const;
125 
127  using const_iterator = std::map<std::string, ObjectPtr>::const_iterator;
130  {
131  return objects_.begin();
132  }
135  {
136  return objects_.end();
137  }
139  std::size_t size() const
140  {
141  return objects_.size();
142  }
144  const_iterator find(const std::string& object_id) const
145  {
146  return objects_.find(object_id);
147  }
148 
150  bool hasObject(const std::string& object_id) const;
151 
154  bool knowsTransform(const std::string& name) const;
155 
161  const Eigen::Isometry3d& getTransform(const std::string& name) const;
162 
168  const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const;
169 
173  const Eigen::Isometry3d& getGlobalShapeTransform(const std::string& object_id, int shape_index) const;
174 
177  const EigenSTL::vector_Isometry3d& getGlobalShapeTransforms(const std::string& object_id) const;
178 
182  void addToObject(const std::string& object_id, const Eigen::Isometry3d& pose,
183  const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses);
184 
188  void addToObject(const std::string& object_id, const std::vector<shapes::ShapeConstPtr>& shapes,
189  const EigenSTL::vector_Isometry3d& shape_poses)
190  {
191  addToObject(object_id, Eigen::Isometry3d::Identity(), shapes, shape_poses);
192  }
193 
199  void addToObject(const std::string& object_id, const Eigen::Isometry3d& pose, const shapes::ShapeConstPtr& shape,
200  const Eigen::Isometry3d& shape_pose)
201  {
202  addToObject(object_id, pose, std::vector<shapes::ShapeConstPtr>{ shape }, EigenSTL::vector_Isometry3d{ shape_pose });
203  }
204 
210  void addToObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& shape_pose)
211  {
212  addToObject(object_id, Eigen::Isometry3d::Identity(), std::vector<shapes::ShapeConstPtr>{ shape },
213  EigenSTL::vector_Isometry3d{ shape_pose });
214  }
215 
218  bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape,
219  const Eigen::Isometry3d& shape_pose);
220 
225  bool moveObject(const std::string& object_id, const Eigen::Isometry3d& transform);
226 
228  bool setObjectPose(const std::string& object_id, const Eigen::Isometry3d& pose);
229 
236  bool removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape);
237 
242  bool removeObject(const std::string& object_id);
243 
245  bool setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses);
246 
250  void clearObjects();
251 
253  {
255  CREATE = 1,
256  DESTROY = 2,
258  ADD_SHAPE = 8,
260  };
261 
265  class Action
266  {
267  public:
269  {
270  }
271  Action(int v) : action_(v)
272  {
273  }
274  operator ActionBits() const
275  {
276  return ActionBits(action_);
277  }
278 
279  private:
280  int action_;
281  };
282 
283 private:
284  class Observer;
285 
286 public:
288  {
289  public:
291  {
292  }
293 
294  private:
296  {
297  }
299  friend class World;
300  };
301 
302  using ObserverCallbackFn = boost::function<void(const ObjectConstPtr&, Action)>;
303 
309 
311  void removeObserver(const ObserverHandle observer_handle);
312 
315  void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const;
316 
317 private:
319  void notify(const ObjectConstPtr& /*obj*/, Action /*action*/);
320 
322  void notifyAll(Action action);
323 
327  void ensureUnique(ObjectPtr& obj);
328 
329  /* Add a shape with no checking */
330  virtual void addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape,
331  const Eigen::Isometry3d& shape_pose);
332 
334  void updateGlobalPosesInternal(ObjectPtr& obj, bool update_shape_poses = true, bool update_subframe_poses = true);
335 
337  std::map<std::string, ObjectPtr> objects_;
338 
340  class Observer
341  {
342  public:
343  Observer(const ObserverCallbackFn& callback) : callback_(callback)
344  {
345  }
347  };
348 
350  std::vector<Observer*> observers_;
351 };
352 } // 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:391
collision_detection::World::ADD_SHAPE
@ ADD_SHAPE
Definition: world.h:258
collision_detection::World::getGlobalShapeTransforms
const EigenSTL::vector_Isometry3d & getGlobalShapeTransforms(const std::string &object_id) const
Get the global transforms to the shapes of an object. This function is used to construct the collisio...
Definition: world.cpp:237
collision_detection::World::Object::shape_poses_
EigenSTL::vector_Isometry3d shape_poses_
The poses of the corresponding entries in shapes_, relative to the object pose.
Definition: world.h:102
collision_detection::World::setSubframesOfObject
bool setSubframesOfObject(const std::string &object_id, const moveit::core::FixedTransformsMap &subframe_poses)
Set subframes on an object. The frames are relative to the object pose.
Definition: world.cpp:355
collision_detection::World::notifyAll
void notifyAll(Action action)
Definition: world.cpp:411
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:256
collision_detection::World::addToObject
void addToObject(const std::string &object_id, const Eigen::Isometry3d &pose, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &shape_pose)
Add a pose and shape to an object. If the object already exists, this call will add the shape to the ...
Definition: world.h:199
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:155
collision_detection::World::moveObject
bool moveObject(const std::string &object_id, const Eigen::Isometry3d &transform)
Move the object pose (thus moving all shapes and subframes in the object) according to the given tran...
Definition: world.cpp:274
collision_detection::World::size
std::size_t size() const
Definition: world.h:139
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:308
collision_detection::World::observers_
std::vector< Observer * > observers_
All registered observers of this world representation.
Definition: world.h:350
collision_detection::World::ObserverHandle::ObserverHandle
ObserverHandle()
Definition: world.h:290
collision_detection::World::moveShapeInObject
bool moveShapeInObject(const std::string &object_id, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &shape_pose)
Update the pose of a shape in an object. Shape equality is verified by comparing pointers....
Definition: world.cpp:252
collision_detection::World::updateGlobalPosesInternal
void updateGlobalPosesInternal(ObjectPtr &obj, bool update_shape_poses=true, bool update_subframe_poses=true)
Updates the global shape and subframe poses.
Definition: world.cpp:372
collision_detection::World::Object::shapes_
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition: world.h:97
collision_detection::World::ObserverHandle::observer_
const Observer * observer_
Definition: world.h:298
collision_detection::World::Object::Object
Object(const std::string &object_id)
Definition: world.h:81
collision_detection::World::Observer::callback_
ObserverCallbackFn callback_
Definition: world.h:346
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:265
collision_detection::World::ObserverHandle
Definition: world.h:287
class_forward.h
shapes::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(Shape)
collision_detection::World::Action::action_
int action_
Definition: world.h:280
collision_detection::World::UNINITIALIZED
@ UNINITIALIZED
Definition: world.h:254
collision_detection::World::MOVEIT_STRUCT_FORWARD
MOVEIT_STRUCT_FORWARD(Object)
collision_detection::World::begin
const_iterator begin() const
Definition: world.h:129
collision_detection::World::notifyObserverAllObjects
void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
Definition: world.cpp:423
collision_detection::World::end
const_iterator end() const
Definition: world.h:134
transforms.h
collision_detection::World::removeObserver
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:398
collision_detection::World::ObserverHandle::ObserverHandle
ObserverHandle(const Observer *o)
Definition: world.h:295
collision_detection::World::addToObject
void addToObject(const std::string &object_id, const Eigen::Isometry3d &pose, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &shape_poses)
Add a pose and shapes to an object in the map. This function makes repeated calls to addToObjectInter...
Definition: world.cpp:100
collision_detection::World::ActionBits
ActionBits
Definition: world.h:252
collision_detection::World::getObject
ObjectConstPtr getObject(const std::string &object_id) const
Get a particular object.
Definition: world.cpp:140
collision_detection::World::ObserverCallbackFn
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:302
collision_detection::World::World
World()
Constructor.
Definition: world.cpp:76
collision_detection::World::addToObject
void addToObject(const std::string &object_id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &shape_poses)
Add shapes to an object in the map. This function makes repeated calls to addToObjectInternal() to ad...
Definition: world.h:188
collision_detection::World::Object::subframe_poses_
moveit::core::FixedTransformsMap subframe_poses_
Transforms from the object pose to subframes on the object. Use them to define points of interest on ...
Definition: world.h:113
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:160
collision_detection::World::find
const_iterator find(const std::string &object_id) const
Definition: world.h:144
collision_detection::World::Object::id_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
Definition: world.h:88
collision_detection::World::Object::global_subframe_poses_
moveit::core::FixedTransformsMap global_subframe_poses_
Transforms from the world frame to the object subframes.
Definition: world.h:117
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:417
collision_detection::World::const_iterator
std::map< std::string, ObjectPtr >::const_iterator const_iterator
Definition: world.h:127
collision_detection::World::objects_
std::map< std::string, ObjectPtr > objects_
Definition: world.h:337
collision_detection::World::Object
A representation of an object.
Definition: world.h:79
collision_detection::World::Object::pose_
Eigen::Isometry3d pose_
The object's pose. All shapes and subframes are defined relative to this frame. This frame is returne...
Definition: world.h:92
collision_detection::World::getGlobalShapeTransform
const Eigen::Isometry3d & getGlobalShapeTransform(const std::string &object_id, int shape_index) const
Get the global transform to a shape of an object with multiple shapes. shape_index is the index of th...
Definition: world.cpp:222
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:149
collision_detection::World::MOVE_SHAPE
@ MOVE_SHAPE
Definition: world.h:257
collision_detection::World::Action::Action
Action()
Definition: world.h:268
collision_detection::World::Object::global_shape_poses_
EigenSTL::vector_Isometry3d global_shape_poses_
The poses of the corresponding entries in shapes_, relative to the world frame.
Definition: world.h:107
collision_detection::World::addToObject
void addToObject(const std::string &object_id, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &shape_pose)
Add a shape to an object. If the object already exists, this call will add the shape to the object at...
Definition: world.h:210
collision_detection::World::Action::Action
Action(int v)
Definition: world.h:271
collision_detection::World::addToObjectInternal
virtual void addToObjectInternal(const ObjectPtr &obj, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &shape_pose)
Definition: world.cpp:91
collision_detection::World::setObjectPose
bool setObjectPose(const std::string &object_id, const Eigen::Isometry3d &pose)
Set the pose of an object. The pose is specified in the world frame.
Definition: world.cpp:286
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:181
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:337
collision_detection::World::CREATE
@ CREATE
Definition: world.h:255
collision_detection::World::Observer
Definition: world.h:340
collision_detection::World::Observer::Observer
Observer(const ObserverCallbackFn &callback)
Definition: world.h:343
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:349
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition: world.h:259
collision_detection::World::getObjectIds
std::vector< std::string > getObjectIds() const
Get the list of Object ids.
Definition: world.cpp:132
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Sep 15 2021 02:23:16