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  if (transform.isApprox(Eigen::Affine3d::Identity()))
164  return true; // object already at correct location
165  ensureUnique(it->second);
166  for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i)
167  {
168  it->second->shape_poses_[i] = transform * it->second->shape_poses_[i];
169  }
170  notify(it->second, MOVE_SHAPE);
171  return true;
172 }
173 
174 bool World::removeShapeFromObject(const std::string& id, const shapes::ShapeConstPtr& shape)
175 {
176  auto it = objects_.find(id);
177  if (it != objects_.end())
178  {
179  unsigned int n = it->second->shapes_.size();
180  for (unsigned int i = 0; i < n; ++i)
181  if (it->second->shapes_[i] == shape)
182  {
183  ensureUnique(it->second);
184  it->second->shapes_.erase(it->second->shapes_.begin() + i);
185  it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
186 
187  if (it->second->shapes_.empty())
188  {
189  notify(it->second, DESTROY);
190  objects_.erase(it);
191  }
192  else
193  {
194  notify(it->second, REMOVE_SHAPE);
195  }
196  return true;
197  }
198  }
199  return false;
200 }
201 
202 bool World::removeObject(const std::string& id)
203 {
204  auto it = objects_.find(id);
205  if (it != objects_.end())
206  {
207  notify(it->second, DESTROY);
208  objects_.erase(it);
209  return true;
210  }
211  return false;
212 }
213 
215 {
217  objects_.clear();
218 }
219 
221 {
222  auto o = new Observer(callback);
223  observers_.push_back(o);
224  return ObserverHandle(o);
225 }
226 
228 {
229  for (auto obs = observers_.begin(); obs != observers_.end(); ++obs)
230  {
231  if (*obs == observer_handle.observer_)
232  {
233  delete *obs;
234  observers_.erase(obs);
235  return;
236  }
237  }
238 }
239 
241 {
242  for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin(); it != objects_.end(); ++it)
243  notify(it->second, action);
244 }
245 
246 void World::notify(const ObjectConstPtr& obj, Action action)
247 {
248  for (std::vector<Observer*>::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs)
249  (*obs)->callback_(obj, action);
250 }
251 
252 void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
253 {
254  for (auto observer : observers_)
255  {
256  if (observer == observer_handle.observer_)
257  {
258  // call the callback for each object
259  for (const auto& object : objects_)
260  observer->callback_(object.second, action);
261  break;
262  }
263  }
264 }
265 
266 } // end of namespace collision_detection
void notifyAll(Action action)
Definition: world.cpp:240
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:202
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:214
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:220
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:252
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:246
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:227
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:174
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 Sun Oct 18 2020 13:16:33