collision_detection::World::moveObject() (moveit#957)

shadow-robot · Jul 30, 2018 · 1522d88 · 1522d88
1 parent fb38953
commit 1522d88
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 1 deletion.
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h
@@ -157,6 +157,9 @@ class World
    * verified by comparing pointers. Returns true on success. */
   bool moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose);
 
+  /** \brief Move all shapes in an object according to the given transform specified in world frame */
+  bool moveObject(const std::string& id, const Eigen::Affine3d& transform);
+
   /** \brief Remove shape from object.
    * Shape equality is verified by comparing pointers. Ownership of the
    * object is renounced (i.e. object is deleted if no external references

diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp
@@ -155,6 +155,20 @@ bool World::moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr
   return false;
 }
 
+bool World::moveObject(const std::string& id, const Eigen::Affine3d& transform)
+{
+  auto it = objects_.find(id);
+  if (it == objects_.end())
+    return false;
+  ensureUnique(it->second);
+  for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i)
+  {
+    it->second->shape_poses_[i] = transform * it->second->shape_poses_[i];
+  }
+  notify(it->second, MOVE_SHAPE);
+  return true;
+}
+
 bool World::removeShapeFromObject(const std::string& id, const shapes::ShapeConstPtr& shape)
 {
   auto it = objects_.find(id);
@@ -247,4 +261,4 @@ void World::notifyObserverAllObjects(const ObserverHandle observer_handle, Actio
   }
 }
 
-}  // end of namespace collision_detection
+}  // end of namespace collision_detection