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::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform)
243 {
244  auto it = objects_.find(object_id);
245  if (it == objects_.end())
246  return false;
247  if (transform.isApprox(Eigen::Isometry3d::Identity()))
248  return true; // object already at correct location
249 
250  ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry
251  return setObjectPose(object_id, transform * it->second->pose_);
252 }
253 
254 bool World::setObjectPose(const std::string& object_id, const Eigen::Isometry3d& pose)
255 {
256  ASSERT_ISOMETRY(pose); // unsanitized input, could contain a non-isometry
257  ObjectPtr& obj = objects_[object_id];
258  int action;
259  if (!obj)
260  {
261  obj = std::make_shared<Object>(object_id);
262  action = CREATE;
263  }
264  else
265  {
266  ensureUnique(obj);
267  action = obj->shapes_.empty() ? 0 : MOVE_SHAPE;
268  }
269 
270  obj->pose_ = pose;
272  notify(obj, Action(action));
273  return true;
274 }
275 
276 bool World::removeShapeFromObject(const std::string& object_id, const shapes::ShapeConstPtr& shape)
277 {
278  auto it = objects_.find(object_id);
279  if (it != objects_.end())
280  {
281  unsigned int n = it->second->shapes_.size();
282  for (unsigned int i = 0; i < n; ++i)
283  if (it->second->shapes_[i] == shape)
284  {
285  ensureUnique(it->second);
286  it->second->shapes_.erase(it->second->shapes_.begin() + i);
287  it->second->shape_poses_.erase(it->second->shape_poses_.begin() + i);
288  it->second->global_shape_poses_.erase(it->second->global_shape_poses_.begin() + i);
289 
290  if (it->second->shapes_.empty())
291  {
292  notify(it->second, DESTROY);
293  objects_.erase(it);
294  }
295  else
296  {
297  notify(it->second, REMOVE_SHAPE);
298  }
299  return true;
300  }
301  }
302  return false;
303 }
304 
305 bool World::removeObject(const std::string& object_id)
306 {
307  auto it = objects_.find(object_id);
308  if (it != objects_.end())
309  {
310  notify(it->second, DESTROY);
311  objects_.erase(it);
312  return true;
313  }
314  return false;
315 }
316 
317 void World::clearObjects()
318 {
320  objects_.clear();
321 }
322 
323 bool World::setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses)
324 {
325  auto obj_pair = objects_.find(object_id);
326  if (obj_pair == objects_.end())
327  {
328  return false;
329  }
330  for (const auto& t : subframe_poses)
331  {
332  ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry
333  }
334  obj_pair->second->subframe_poses_ = subframe_poses;
335  obj_pair->second->global_subframe_poses_ = subframe_poses;
336  updateGlobalPosesInternal(obj_pair->second, false, true);
337  return true;
338 }
339 
340 void World::updateGlobalPosesInternal(ObjectPtr& obj, bool update_shape_poses, bool update_subframe_poses)
341 {
342  // Update global shape poses
343  if (update_shape_poses)
344  for (unsigned int i = 0; i < obj->global_shape_poses_.size(); ++i)
345  obj->global_shape_poses_[i] = obj->pose_ * obj->shape_poses_[i];
346 
347  // Update global subframe poses
348  if (update_subframe_poses)
349  {
350  for (auto it_global_pose = obj->global_subframe_poses_.begin(), it_local_pose = obj->subframe_poses_.begin(),
351  end_poses = obj->global_subframe_poses_.end();
352  it_global_pose != end_poses; ++it_global_pose, ++it_local_pose)
353  {
354  it_global_pose->second = obj->pose_ * it_local_pose->second;
355  }
356  }
357 }
358 
359 World::ObserverHandle World::addObserver(const ObserverCallbackFn& callback)
360 {
361  auto o = new Observer(callback);
362  observers_.push_back(o);
363  return ObserverHandle(o);
364 }
365 
366 void World::removeObserver(ObserverHandle observer_handle)
367 {
368  for (auto obs = observers_.begin(); obs != observers_.end(); ++obs)
369  {
370  if (*obs == observer_handle.observer_)
371  {
372  delete *obs;
373  observers_.erase(obs);
374  return;
375  }
376  }
377 }
378 
379 void World::notifyAll(Action action)
380 {
381  for (std::map<std::string, ObjectPtr>::const_iterator it = objects_.begin(); it != objects_.end(); ++it)
382  notify(it->second, action);
383 }
384 
385 void World::notify(const ObjectConstPtr& obj, Action action)
386 {
387  for (Observer* observer : observers_)
388  observer->callback_(obj, action);
389 }
390 
391 void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Action action) const
392 {
393  for (auto observer : observers_)
394  {
395  if (observer == observer_handle.observer_)
396  {
397  // call the callback for each object
398  for (const auto& object : objects_)
399  observer->callback_(object.second, action);
400  break;
401  }
402  }
403 }
404 
405 } // 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:391
collision_detection::World::ADD_SHAPE
@ ADD_SHAPE
Definition: world.h:258
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:355
collision_detection::World::notifyAll
void notifyAll(Action action)
Definition: world.cpp:411
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:256
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:274
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:308
collision_detection::World::observers_
std::vector< Observer * > observers_
All registered observers of this world representation.
Definition: world.h:350
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:372
collision_detection::World::ObserverHandle::observer_
const Observer * observer_
Definition: world.h:298
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:265
collision_detection::World::ObserverHandle
Definition: world.h:287
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:423
console.h
collision_detection::World::removeObserver
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:398
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::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:417
collision_detection::World::objects_
std::map< std::string, ObjectPtr > objects_
Definition: world.h:337
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
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:257
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:286
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:337
collision_detection::World::CREATE
@ CREATE
Definition: world.h:255
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:349
t
geometry_msgs::TransformStamped t
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition: world.h:259
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 Wed May 18 2022 02:23:53