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 <console_bridge/console.h>
39 
41 {
42 }
43 
45 {
46  objects_ = other.objects_;
47 }
48 
50 {
51  while (!observers_.empty())
52  removeObserver(observers_.front());
53 }
54 
55 inline void collision_detection::World::addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape,
56  const Eigen::Affine3d& pose)
57 {
58  obj->shapes_.push_back(shape);
59  obj->shape_poses_.push_back(pose);
60 }
61 
62 void collision_detection::World::addToObject(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
63  const EigenSTL::vector_Affine3d& poses)
64 {
65  if (shapes.size() != poses.size())
66  {
67  CONSOLE_BRIDGE_logError("Number of shapes and number of poses do not match. Not adding this object to collision "
68  "world.");
69  return;
70  }
71 
72  if (shapes.empty())
73  return;
74 
75  int action = ADD_SHAPE;
76 
77  ObjectPtr& obj = objects_[id];
78  if (!obj)
79  {
80  obj.reset(new Object(id));
81  action |= CREATE;
82  }
83 
84  ensureUnique(obj);
85 
86  for (std::size_t i = 0; i < shapes.size(); ++i)
87  addToObjectInternal(obj, shapes[i], poses[i]);
88 
89  notify(obj, Action(action));
90 }
91 
92 void collision_detection::World::addToObject(const std::string& id, const shapes::ShapeConstPtr& shape,
93  const Eigen::Affine3d& pose)
94 {
95  int action = ADD_SHAPE;
96 
97  ObjectPtr& obj = objects_[id];
98  if (!obj)
99  {
100  obj.reset(new Object(id));
101  action |= CREATE;
102  }
103 
104  ensureUnique(obj);
105  addToObjectInternal(obj, shape, pose);
106 
107  notify(obj, Action(action));
108 }
109 
110 std::vector<std::string> collision_detection::World::getObjectIds() const
111 {
112  std::vector<std::string> id;
113  for (const auto& object : objects_)
114  id.push_back(object.first);
115  return id;
116 }
117 
118 collision_detection::World::ObjectConstPtr collision_detection::World::getObject(const std::string& id) const
119 {
120  auto it = objects_.find(id);
121  if (it == objects_.end())
122  return ObjectConstPtr();
123  else
124  return it->second;
125 }
126 
128 {
129  if (obj && !obj.unique())
130  obj.reset(new Object(*obj));
131 }
132 
133 bool collision_detection::World::hasObject(const std::string& id) const
134 {
135  return objects_.find(id) != objects_.end();
136 }
137 
139  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 
159 {
160  auto it = objects_.find(id);
161  if (it != objects_.end())
162  {
163  unsigned int n = it->second->shapes_.size();
164  for (unsigned int i = 0; i < n; ++i)
165  if (it->second->shapes_[i] == shape)
166  {
167  ensureUnique(it->second);
168  it->second->shapes_.erase(it->second->shapes_.begin() + i);
169  it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
170 
171  if (it->second->shapes_.empty())
172  {
173  notify(it->second, DESTROY);
174  objects_.erase(it);
175  }
176  else
177  {
178  notify(it->second, REMOVE_SHAPE);
179  }
180  return true;
181  }
182  }
183  return false;
184 }
185 
186 bool collision_detection::World::removeObject(const std::string& id)
187 {
188  auto it = objects_.find(id);
189  if (it != objects_.end())
190  {
191  notify(it->second, DESTROY);
192  objects_.erase(it);
193  return true;
194  }
195  return false;
196 }
197 
199 {
201  objects_.clear();
202 }
203 
205 {
206  auto o = new Observer(callback);
207  observers_.push_back(o);
208  return ObserverHandle(o);
209 }
210 
212 {
213  for (auto obs = observers_.begin(); obs != observers_.end(); ++obs)
214  {
215  if (*obs == observer_handle.observer_)
216  {
217  delete *obs;
218  observers_.erase(obs);
219  return;
220  }
221  }
222 }
223 
225 {
226  for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin(); it != objects_.end(); ++it)
227  notify(it->second, action);
228 }
229 
230 void collision_detection::World::notify(const ObjectConstPtr& obj, Action action)
231 {
232  for (std::vector<Observer*>::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs)
233  (*obs)->callback_(obj, action);
234 }
235 
237 {
238  for (auto observer : observers_)
239  {
240  if (observer == observer_handle.observer_)
241  {
242  // call the callback for each object
243  for (const auto& object : objects_)
244  observer->callback_(object.second, action);
245  break;
246  }
247  }
248 }
void notifyAll(Action action)
Definition: world.cpp:224
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:186
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:198
std::vector< Observer * > observers_
Definition: world.h:272
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:192
std::vector< std::string > getObjectIds() const
Get the list of Object ids.
Definition: world.cpp:110
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:62
void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
Definition: world.cpp:236
ObserverHandle addObserver(const ObserverCallbackFn &callback)
register a callback function for notification of changes. callback will be called right after any cha...
Definition: world.cpp:204
ObjectConstPtr getObject(const std::string &id) const
Get a particular object.
Definition: world.cpp:118
bool hasObject(const std::string &id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:133
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
Definition: world.h:229
World()
Constructor.
Definition: world.cpp:40
A representation of an object.
Definition: world.h:80
void notify(const ObjectConstPtr &, Action)
Definition: world.cpp:230
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:211
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:158
std::map< std::string, ObjectPtr > objects_
Definition: world.h:261
virtual void addToObjectInternal(const ObjectPtr &obj, const shapes::ShapeConstPtr &shape, const Eigen::Affine3d &pose)
Definition: world.cpp:55
#define CONSOLE_BRIDGE_logError(fmt,...)
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:138
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:127
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Apr 21 2018 02:54:52