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 
39 #include <boost/algorithm/string/predicate.hpp>
40 #include <ros/console.h>
41 
42 namespace collision_detection
43 {
45 {
46 }
47 
48 World::World(const World& other)
49 {
50  objects_ = other.objects_;
51 }
52 
54 {
55  while (!observers_.empty())
56  removeObserver(observers_.front());
57 }
58 
59 inline void World::addToObjectInternal(const ObjectPtr& obj, const shapes::ShapeConstPtr& shape,
60  const Eigen::Isometry3d& shape_pose)
61 {
62  obj->shapes_.push_back(shape);
63  ASSERT_ISOMETRY(shape_pose) // unsanitized input, could contain a non-isometry
64  obj->shape_poses_.push_back(shape_pose);
65  obj->global_shape_poses_.push_back(obj->pose_ * shape_pose);
66 }
67 
68 void World::addToObject(const std::string& object_id, const Eigen::Isometry3d& pose,
69  const std::vector<shapes::ShapeConstPtr>& shapes,
70  const EigenSTL::vector_Isometry3d& shape_poses)
71 {
72  if (shapes.size() != shape_poses.size())
73  {
74  ROS_ERROR_NAMED("collision_detection", "Number of shapes and number of poses do not match. "
75  "Not adding this object to collision world.");
76  return;
77  }
78 
79  if (shapes.empty())
80  return;
81 
82  int action = ADD_SHAPE;
83 
84  ObjectPtr& obj = objects_[object_id];
85  if (!obj)
86  {
87  obj = std::make_shared<Object>(object_id);
88  action |= CREATE;
89  obj->pose_ = pose;
90  }
91  else
92  ensureUnique(obj);
93 
94  for (std::size_t i = 0; i < shapes.size(); ++i)
95  addToObjectInternal(obj, shapes[i], shape_poses[i]);
96 
97  notify(obj, Action(action));
98 }
99 
100 std::vector<std::string> World::getObjectIds() const
101 {
102  std::vector<std::string> ids;
103  for (const auto& object : objects_)
104  ids.push_back(object.first);
105  return ids;
106 }
107 
108 World::ObjectConstPtr World::getObject(const std::string& object_id) const
109 {
110  auto it = objects_.find(object_id);
111  if (it == objects_.end())
112  return ObjectConstPtr();
113  else
114  return it->second;
115 }
116 
117 void World::ensureUnique(ObjectPtr& obj)
118 {
119  if (obj && !obj.unique())
120  obj = std::make_shared<Object>(*obj);
121 }
122 
123 bool World::hasObject(const std::string& object_id) const
124 {
125  return objects_.find(object_id) != objects_.end();
126 }
127 
128 bool World::knowsTransform(const std::string& name) const
129 {
130  // Check object names first
131  std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
132  if (it != objects_.end())
133  return true;
134  else // Then objects' subframes
135  {
136  for (const std::pair<const std::string, ObjectPtr>& object : objects_)
137  {
138  // if "object name/" matches start of object_id, we found the matching object
139  if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
140  {
141  return object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1)) !=
142  object.second->global_subframe_poses_.end();
143  }
144  }
145  }
146  return false;
147 }
148 
149 const Eigen::Isometry3d& World::getTransform(const std::string& name) const
150 {
151  bool found;
152  const Eigen::Isometry3d& result = getTransform(name, found);
153  if (!found)
154  throw std::runtime_error("No transform found with name: " + name);
155  return result;
156 }
157 
158 const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& frame_found) const
159 {
160  // assume found
161  frame_found = true;
162 
163  std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
164  if (it != objects_.end())
165  {
166  return it->second->pose_;
167  }
168  else // Search within subframes
169  {
170  for (const std::pair<const std::string, ObjectPtr>& object : objects_)
171  {
172  // if "object name/" matches start of object_id, we found the matching object
173  if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
174  {
175  auto it = object.second->global_subframe_poses_.find(name.substr(object.first.length() + 1));
176  if (it != object.second->global_subframe_poses_.end())
177  {
178  return it->second;
179  }
180  }
181  }
182  }
183 
184  // we need a persisting isometry for the API
185  static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
186  frame_found = false;
187  return IDENTITY_TRANSFORM;
188 }
189 
190 const Eigen::Isometry3d& World::getGlobalShapeTransform(const std::string& object_id, int shape_index) const
191 {
192  auto it = objects_.find(object_id);
193  if (it != objects_.end())
194  {
195  return it->second->global_shape_poses_[shape_index];
196  }
197  else
198  {
199  ROS_ERROR_STREAM("Could not find global shape transform for object " << object_id);
200  static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
201  return IDENTITY_TRANSFORM;
202  }
203 }
204 
205 const EigenSTL::vector_Isometry3d& World::getGlobalShapeTransforms(const std::string& object_id) const
206 {
207  auto it = objects_.find(object_id);
208  if (it != objects_.end())
209  {
210  return it->second->global_shape_poses_;
211  }
212  else
213  {
214  ROS_ERROR_STREAM("Could not find global shape transforms for object " << object_id);
215  static const EigenSTL::vector_Isometry3d IDENTITY_TRANSFORM_VECTOR;
216  return IDENTITY_TRANSFORM_VECTOR;
217  }
218 }
219 
220 bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape,
221  const Eigen::Isometry3d& shape_pose)
222 {
223  auto it = objects_.find(object_id);
224  if (it != objects_.end())
225  {
226  unsigned int n = it->second->shapes_.size();
227  for (unsigned int i = 0; i < n; ++i)
228  if (it->second->shapes_[i] == shape)
229  {
230  ensureUnique(it->second);
231  ASSERT_ISOMETRY(shape_pose) // unsanitized input, could contain a non-isometry
232  it->second->shape_poses_[i] = shape_pose;
233  it->second->global_shape_poses_[i] = it->second->pose_ * shape_pose;
234 
235  notify(it->second, MOVE_SHAPE);
236  return true;
237  }
238  }
239  return false;
240 }
241 
242 bool World::moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses)
243 {
244  auto it = objects_.find(object_id);
245  if (it != objects_.end())
246  {
247  if (shape_poses.size() == it->second->shapes_.size())
248  {
249  for (std::size_t i = 0; i < shape_poses.size(); ++i)
250  {
251  ASSERT_ISOMETRY(shape_poses[i]) // unsanitized input, could contain a non-isometry
252  it->second->shape_poses_[i] = shape_poses[i];
253  it->second->global_shape_poses_[i] = it->second->pose_ * shape_poses[i];
254  }
255  notify(it->second, MOVE_SHAPE);
256  return true;
257  }
258  }
259  return false;
260 }
261 
262 bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform)
263 {
264  auto it = objects_.find(object_id);
265  if (it == objects_.end())
266  return false;
267  if (transform.isApprox(Eigen::Isometry3d::Identity()))
268  return true; // object already at correct location
269 
270  ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry
271  return setObjectPose(object_id, transform * it->second->pose_);
272 }
273 
274 bool World::setObjectPose(const std::string& object_id, const Eigen::Isometry3d& pose)
275 {
276  ASSERT_ISOMETRY(pose); // unsanitized input, could contain a non-isometry
277  ObjectPtr& obj = objects_[object_id];
278  int action;
279  if (!obj)
280  {
281  obj = std::make_shared<Object>(object_id);
282  action = CREATE;
283  }
284  else
285  {
286  ensureUnique(obj);
287  action = obj->shapes_.empty() ? 0 : MOVE_SHAPE;
288  }
289 
290  obj->pose_ = pose;
292  notify(obj, Action(action));
293  return true;
294 }
295 
296 bool World::removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape)
297 {
298  auto it = objects_.find(object_id);
299  if (it != objects_.end())
300  {
301  unsigned int n = it->second->shapes_.size();
302  for (unsigned int i = 0; i < n; ++i)
303  if (it->second->shapes_[i] == shape)
304  {
305  ensureUnique(it->second);
306  it->second->shapes_.erase(it->second->shapes_.begin() + i);
307  it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
308  it->second->global_shape_poses_.erase(it->second->global_shape_poses_.begin() + i);
309 
310  if (it->second->shapes_.empty())
311  {
312  notify(it->second, DESTROY);
313  objects_.erase(it);
314  }
315  else
316  {
317  notify(it->second, REMOVE_SHAPE);
318  }
319  return true;
320  }
321  }
322  return false;
323 }
324 
325 bool World::removeObject(const std::string& object_id)
326 {
327  auto it = objects_.find(object_id);
328  if (it != objects_.end())
329  {
330  notify(it->second, DESTROY);
331  objects_.erase(it);
332  return true;
333  }
334  return false;
335 }
336 
337 void World::clearObjects()
338 {
340  objects_.clear();
341 }
342 
343 bool World::setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses)
344 {
345  auto obj_pair = objects_.find(object_id);
346  if (obj_pair == objects_.end())
347  {
348  return false;
349  }
350  for (const auto& t : subframe_poses)
351  {
352  ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry
353  }
354  obj_pair->second->subframe_poses_ = subframe_poses;
355  obj_pair->second->global_subframe_poses_ = subframe_poses;
356  updateGlobalPosesInternal(obj_pair->second, false, true);
357  return true;
358 }
359 
360 void World::updateGlobalPosesInternal(ObjectPtr& obj, bool update_shape_poses, bool update_subframe_poses)
361 {
362  // Update global shape poses
363  if (update_shape_poses)
364  for (unsigned int i = 0; i < obj->global_shape_poses_.size(); ++i)
365  obj->global_shape_poses_[i] = obj->pose_ * obj->shape_poses_[i];
366 
367  // Update global subframe poses
368  if (update_subframe_poses)
369  {
370  for (auto it_global_pose = obj->global_subframe_poses_.begin(), it_local_pose = obj->subframe_poses_.begin(),
371  end_poses = obj->global_subframe_poses_.end();
372  it_global_pose != end_poses; ++it_global_pose, ++it_local_pose)
373  {
374  it_global_pose->second = obj->pose_ * it_local_pose->second;
375  }
376  }
377 }
378 
379 World::ObserverHandle World::addObserver(const ObserverCallbackFn& callback)
380 {
381  auto o = new Observer(callback);
382  observers_.push_back(o);
383  return ObserverHandle(o);
384 }
385 
386 void World::removeObserver(ObserverHandle observer_handle)
387 {
388  for (auto obs = observers_.begin(); obs != observers_.end(); ++obs)
389  {
390  if (*obs == observer_handle.observer_)
391  {
392  delete *obs;
393  observers_.erase(obs);
394  return;
395  }
396  }
397 }
398 
399 void World::notifyAll(Action action)
400 {
401  for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin(); it != objects_.end(); ++it)
402  notify(it->second, action);
403 }
404 
405 void World::notify(const ObjectConstPtr& obj, Action action)
406 {
407  for (Observer* observer : observers_)
408  observer->callback_(obj, action);
409 }
410 
411 void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
412 {
413  for (auto observer : observers_)
414  {
415  if (observer == observer_handle.observer_)
416  {
417  // call the callback for each object
418  for (const auto& object : objects_)
419  observer->callback_(object.second, action);
420  break;
421  }
422  }
423 }
424 
425 } // end of 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:411
collision_detection::World::ADD_SHAPE
@ ADD_SHAPE
Definition: world.h:261
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::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:375
collision_detection::World::notifyAll
void notifyAll(Action action)
Definition: world.cpp:431
collision_detection::World::moveShapesInObject
bool moveShapesInObject(const std::string &object_id, const EigenSTL::vector_Isometry3d &shape_poses)
Update the pose of all shapes in an object. Shape size is verified. Returns true on success.
Definition: world.cpp:274
Eigen
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:259
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
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:294
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
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:328
collision_detection::World::observers_
std::vector< Observer * > observers_
All registered observers of this world representation.
Definition: world.h:353
check_isometry.h
world.h
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:392
collision_detection::World::ObserverHandle::observer_
const Observer * observer_
Definition: world.h:301
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:268
collision_detection::World::ObserverHandle
Definition: world.h:290
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
obj
CollisionObject< S > * obj
collision_detection::World::notifyObserverAllObjects
void notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
Definition: world.cpp:443
console.h
collision_detection::World::removeObserver
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:418
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
name
std::string name
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:305
collision_detection::World::World
World()
Constructor.
Definition: world.cpp:76
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
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
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:437
collision_detection::World::objects_
std::map< std::string, ObjectPtr > objects_
Definition: world.h:340
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
pose_
const geometry_msgs::Pose * pose_
Definition: planning_scene.cpp:799
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:260
std
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:306
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:357
collision_detection::World::CREATE
@ CREATE
Definition: world.h:258
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:369
t
geometry_msgs::TransformStamped t
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition: world.h:262
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 Sat Dec 21 2024 03:23:42