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::Isometry3d& 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_Isometry3d& 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::Isometry3d& 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& object_id) const
120 {
121  auto it = objects_.find(object_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& object_id) const
135 {
136  return objects_.find(object_id) != objects_.end();
137 }
138 
139 bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape,
140  const Eigen::Isometry3d& pose)
141 {
142  auto it = objects_.find(object_id);
143  if (it != objects_.end())
144  {
145  unsigned int n = it->second->shapes_.size();
146  for (unsigned int i = 0; i < n; ++i)
147  if (it->second->shapes_[i] == shape)
148  {
149  ensureUnique(it->second);
150  it->second->shape_poses_[i] = pose;
151 
152  notify(it->second, MOVE_SHAPE);
153  return true;
154  }
155  }
156  return false;
157 }
158 
159 bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform)
160 {
161  auto it = objects_.find(object_id);
162  if (it == objects_.end())
163  return false;
164  if (transform.isApprox(Eigen::Isometry3d::Identity()))
165  return true; // object already at correct location
166  ensureUnique(it->second);
167  for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i)
168  {
169  it->second->shape_poses_[i] = transform * it->second->shape_poses_[i];
170  }
171  notify(it->second, MOVE_SHAPE);
172  return true;
173 }
174 
175 bool World::removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape)
176 {
177  auto it = objects_.find(object_id);
178  if (it != objects_.end())
179  {
180  unsigned int n = it->second->shapes_.size();
181  for (unsigned int i = 0; i < n; ++i)
182  if (it->second->shapes_[i] == shape)
183  {
184  ensureUnique(it->second);
185  it->second->shapes_.erase(it->second->shapes_.begin() + i);
186  it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
187 
188  if (it->second->shapes_.empty())
189  {
190  notify(it->second, DESTROY);
191  objects_.erase(it);
192  }
193  else
194  {
195  notify(it->second, REMOVE_SHAPE);
196  }
197  return true;
198  }
199  }
200  return false;
201 }
202 
203 bool World::removeObject(const std::string& object_id)
204 {
205  auto it = objects_.find(object_id);
206  if (it != objects_.end())
207  {
208  notify(it->second, DESTROY);
209  objects_.erase(it);
210  return true;
211  }
212  return false;
213 }
214 
216 {
218  objects_.clear();
219 }
220 
222 {
223  auto o = new Observer(callback);
224  observers_.push_back(o);
225  return ObserverHandle(o);
226 }
227 
229 {
230  for (auto obs = observers_.begin(); obs != observers_.end(); ++obs)
231  {
232  if (*obs == observer_handle.observer_)
233  {
234  delete *obs;
235  observers_.erase(obs);
236  return;
237  }
238  }
239 }
240 
242 {
243  for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin(); it != objects_.end(); ++it)
244  notify(it->second, action);
245 }
246 
247 void World::notify(const ObjectConstPtr& obj, Action action)
248 {
249  for (std::vector<Observer*>::const_iterator obs = observers_.begin(); obs != observers_.end(); ++obs)
250  (*obs)->callback_(obj, action);
251 }
252 
253 void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
254 {
255  for (auto observer : observers_)
256  {
257  if (observer == observer_handle.observer_)
258  {
259  // call the callback for each object
260  for (const auto& object : objects_)
261  observer->callback_(object.second, action);
262  break;
263  }
264  }
265 }
266 
267 } // end of namespace collision_detection
std::vector< std::string > getObjectIds() const
Get the list of Object ids.
Definition: world.cpp:111
void notifyAll(Action action)
Definition: world.cpp:241
bool hasObject(const std::string &object_id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:134
void clearObjects()
Clear all objects. If there are no other pointers to corresponding instances of Objects, the memory is freed.
Definition: world.cpp:215
std::vector< Observer * > observers_
All registered observers of this world representation.
Definition: world.h:277
ObserverHandle addObserver(const ObserverCallbackFn &callback)
register a callback function for notification of changes. callback will be called right after any cha...
Definition: world.cpp:221
Maintain a representation of the environment.
Definition: world.h:59
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:175
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:195
void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
Definition: world.cpp:253
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. Returns true on success.
Definition: world.cpp:139
ObjectConstPtr getObject(const std::string &object_id) const
Get a particular object.
Definition: world.cpp:119
Generic interface to collision detection.
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:64
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:79
void notify(const ObjectConstPtr &, Action)
Definition: world.cpp:247
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:228
std::map< std::string, ObjectPtr > objects_
Definition: world.h:264
action
virtual void addToObjectInternal(const ObjectPtr &obj, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &pose)
Definition: world.cpp:57
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:203
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,...)
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
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:159
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 23 2020 03:52:30