world.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: Acorn Pooley, Ioan Sucan */
36 
38 #include <ros/console.h>
39 
40 namespace collision_detection
41 {
43 {
44 }
45 
46 World::World(const World& other)
47 {
48  objects_ = other.objects_;
49 }
50 
52 {
53  while (!observers_.empty())
54  removeObserver(observers_.front());
55 }
56 
57 inline void World::addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape,
58  const Eigen::Affine3d& pose)
59 {
60  obj->shapes_.push_back(shape);
61  obj->shape_poses_.push_back(pose);
62 }
63 
64 void World::addToObject(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
65  const EigenSTL::vector_Affine3d& poses)
66 {
67  if (shapes.size() != poses.size())
68  {
69  ROS_ERROR_NAMED("collision_detection", "Number of shapes and number of poses do not match. "
70  "Not adding this object to collision world.");
71  return;
72  }
73 
74  if (shapes.empty())
75  return;
76 
77  int action = ADD_SHAPE;
78 
79  ObjectPtr& obj = objects_[id];
80  if (!obj)
81  {
82  obj.reset(new Object(id));
83  action |= CREATE;
84  }
85 
86  ensureUnique(obj);
87 
88  for (std::size_t i = 0; i < shapes.size(); ++i)
89  addToObjectInternal(obj, shapes[i], poses[i]);
90 
91  notify(obj, Action(action));
92 }
93 
94 void World::addToObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose)
95 {
96  int action = ADD_SHAPE;
97 
98  ObjectPtr& obj = objects_[id];
99  if (!obj)
100  {
101  obj.reset(new Object(id));
102  action |= CREATE;
103  }
104 
105  ensureUnique(obj);
106  addToObjectInternal(obj, shape, pose);
107 
108  notify(obj, Action(action));
109 }
110 
111 std::vector<std::string> World::getObjectIds() const
112 {
113  std::vector<std::string> id;
114  for (const auto& object : objects_)
115  id.push_back(object.first);
116  return id;
117 }
118 
119 World::ObjectConstPtr World::getObject(const std::string& id) const
120 {
121  auto it = objects_.find(id);
122  if (it == objects_.end())
123  return ObjectConstPtr();
124  else
125  return it->second;
126 }
127 
128 void World::ensureUnique(ObjectPtr& obj)
129 {
130  if (obj && !obj.unique())
131  obj.reset(new Object(*obj));
132 }
133 
134 bool World::hasObject(const std::string& id) const
135 {
136  return objects_.find(id) != objects_.end();
137 }
138 
139 bool World::moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose)
140 {
141  auto it = objects_.find(id);
142  if (it != objects_.end())
143  {
144  unsigned int n = it->second->shapes_.size();
145  for (unsigned int i = 0; i < n; ++i)
146  if (it->second->shapes_[i] == shape)
147  {
148  ensureUnique(it->second);
149  it->second->shape_poses_[i] = pose;
150 
151  notify(it->second, MOVE_SHAPE);
152  return true;
153  }
154  }
155  return false;
156 }
157 
158 bool World::moveObject(const std::string& id, const Eigen::Affine3d& transform)
159 {
160  auto it = objects_.find(id);
161  if (it == objects_.end())
162  return false;
163  ensureUnique(it->second);
164  for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i)
165  {
166  it->second->shape_poses_[i] = transform * it->second->shape_poses_[i];
167  }
168  notify(it->second, MOVE_SHAPE);
169  return true;
170 }
171 
172 bool World::removeShapeFromObject(const std::string& id, const shapes::ShapeConstPtr& shape)
173 {
174  auto it = objects_.find(id);
175  if (it != objects_.end())
176  {
177  unsigned int n = it->second->shapes_.size();
178  for (unsigned int i = 0; i < n; ++i)
179  if (it->second->shapes_[i] == shape)
180  {
181  ensureUnique(it->second);
182  it->second->shapes_.erase(it->second->shapes_.begin() + i);
183  it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
184 
185  if (it->second->shapes_.empty())
186  {
187  notify(it->second, DESTROY);
188  objects_.erase(it);
189  }
190  else
191  {
192  notify(it->second, REMOVE_SHAPE);
193  }
194  return true;
195  }
196  }
197  return false;
198 }
199 
200 bool World::removeObject(const std::string& id)
201 {
202  auto it = objects_.find(id);
203  if (it != objects_.end())
204  {
205  notify(it->second, DESTROY);
206  objects_.erase(it);
207  return true;
208  }
209  return false;
210 }
211 
213 {
215  objects_.clear();
216 }
217 
219 {
220  auto o = new Observer(callback);
221  observers_.push_back(o);
222  return ObserverHandle(o);
223 }
224 
226 {
227  for (auto obs = observers_.begin(); obs != observers_.end(); ++obs)
228  {
229  if (*obs == observer_handle.observer_)
230  {
231  delete *obs;
232  observers_.erase(obs);
233  return;
234  }
235  }
236 }
237 
239 {
240  for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin(); it != objects_.end(); ++it)
241  notify(it->second, action);
242 }
243 
244 void World::notify(const ObjectConstPtr& obj, Action action)
245 {
246  for (std::vector<Observer*>::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs)
247  (*obs)->callback_(obj, action);
248 }
249 
250 void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
251 {
252  for (auto observer : observers_)
253  {
254  if (observer == observer_handle.observer_)
255  {
256  // call the callback for each object
257  for (const auto& object : objects_)
258  observer->callback_(object.second, action);
259  break;
260  }
261  }
262 }
263 
264 } // end of namespace collision_detection
void notifyAll(Action action)
Definition: world.cpp:238
bool removeObject(const std::string &id)
Remove a particular object. If there are no external pointers to the corresponding instance of Object...
Definition: world.cpp:200
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void clearObjects()
Clear all objects. If there are no other pointers to corresponding instances of Objects, the memory is freed.
Definition: world.cpp:212
std::vector< Observer * > observers_
Definition: world.h:275
ObserverHandle addObserver(const ObserverCallbackFn &callback)
register a callback function for notification of changes. callback will be called right after any cha...
Definition: world.cpp:218
Maintain a representation of the environment.
Definition: world.h:60
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:195
std::vector< std::string > getObjectIds() const
Get the list of Object ids.
Definition: world.cpp:111
void addToObject(const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &poses)
Add shapes to an object in the map. This function makes repeated calls to addToObjectInternal() to ad...
Definition: world.cpp:64
void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
Definition: world.cpp:250
Generic interface to collision detection.
bool hasObject(const std::string &id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:134
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:232
World()
Constructor.
Definition: world.cpp:42
A representation of an object.
Definition: world.h:80
void notify(const ObjectConstPtr &, Action)
Definition: world.cpp:244
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:225
bool removeShapeFromObject(const std::string &id, const shapes::ShapeConstPtr &shape)
Remove shape from object. Shape equality is verified by comparing pointers. Ownership of the object i...
Definition: world.cpp:172
std::map< std::string, ObjectPtr > objects_
Definition: world.h:264
virtual void addToObjectInternal(const ObjectPtr &obj, const shapes::ShapeConstPtr &shape, const Eigen::Affine3d &pose)
Definition: world.cpp:57
ObjectConstPtr getObject(const std::string &id) const
Get a particular object.
Definition: world.cpp:119
bool moveShapeInObject(const std::string &id, const shapes::ShapeConstPtr &shape, const Eigen::Affine3d &pose)
Update the pose of a shape in an object. Shape equality is verified by comparing pointers. Returns true on success.
Definition: world.cpp:139
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:128
#define ROS_ERROR_NAMED(name,...)
bool moveObject(const std::string &id, const Eigen::Affine3d &transform)
Move all shapes in an object according to the given transform specified in world frame.
Definition: world.cpp:158
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 18 2018 02:48:31