Add subframes to collision world

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -46,6 +46,7 @@
 #include <boost/function.hpp>
 #include <Eigen/Geometry>
 #include <eigen_stl_containers/eigen_stl_vector_container.h>
+#include <moveit/transforms/transforms.h>
 
 namespace shapes
 {
@@ -105,6 +106,12 @@ class World
      *
      * @copydetails shapes_ */
     EigenSTL::vector_Isometry3d shape_poses_;
+
+    /** \brief Transforms to subframes on the object.
+     *  Use them to define points of interest on an object to plan with
+     *  (e.g. screwdriver/tip, kettle/spout, mug/base).
+     */
+    moveit::core::FixedTransformsMap subframe_poses_;
   };
 
   /** \brief Get the list of Object ids */
@@ -139,6 +146,20 @@ class World
   /** \brief Check if a particular object exists in the collision world*/
   bool hasObject(const std::string& object_id) const;
 
+  /** \brief Check if an object or subframe with given name exists in the collision world.
+   * A subframe name needs to be prefixed with the object's name separated by a slash. */
+  bool knowsTransform(const std::string& name) const;
+
+  /** \brief Get the transform to an object or subframe with given name.
+   * If name does not exist, a std::runtime_error is thrown.
+   * A subframe name needs to be prefixed with the object's name separated by a slash. */
+  const Eigen::Isometry3d& getTransform(const std::string& name) const;
+
+  /** \brief Get the transform to an object or subframe with given name.
+   * If name does not exist, returns an identity transform and sets frame_found to false.
+   * A subframe name needs to be prefixed with the object's name separated by a slash. */
+  const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const;
+
   /** \brief Add shapes to an object in the map.
    * This function makes repeated calls to addToObjectInternal() to add the
    * shapes one by one.
@@ -175,6 +196,9 @@ class World
    * Returns true on success and false if no such object was found. */
   bool removeObject(const std::string& object_id);
 
+  /** \brief Set subframes on an object. */
+  bool setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses);
+
   /** \brief Clear all objects.
    * If there are no other pointers to corresponding instances of Objects,
    * the memory is freed. */

moveit_core/collision_detection/src/world.cpp

@@ -35,6 +35,7 @@
 /* Author: Acorn Pooley, Ioan Sucan */
 
 #include <moveit/collision_detection/world.h>
+#include <boost/algorithm/string/predicate.hpp>
 #include <ros/console.h>
 
 namespace collision_detection
@@ -136,6 +137,62 @@ bool World::hasObject(const std::string& object_id) const
   return objects_.find(object_id) != objects_.end();
 }
 
+bool World::knowsTransform(const std::string& name) const
+{
+  // Check object names first
+  std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
+  if (it != objects_.end())
+    // only accept object name as frame if it is associated to a unique shape
+    return it->second->shape_poses_.size() == 1;
+  else  // Then objects' subframes
+  {
+    for (const std::pair<std::string, ObjectPtr>& object : objects_)
+    {
+      // if "object name/" matches start of object_id, we found the matching object
+      if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
+        return object.second->subframe_poses_.find(name.substr(object.first.length() + 1)) !=
+               object.second->subframe_poses_.end();
+    }
+  }
+  return false;
+}
+
+const Eigen::Isometry3d& World::getTransform(const std::string& name) const
+{
+  bool found;
+  const Eigen::Isometry3d& result = getTransform(name, found);
+  if (!found)
+    throw std::runtime_error("No transform found with name: " + name);
+  return result;
+}
+
+const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& frame_found) const
+{
+  std::map<std::string, ObjectPtr>::const_iterator it = objects_.find(name);
+  if (it != objects_.end())
+    return it->second->shape_poses_[0];
+  else  // Search within subframes
+  {
+    for (const std::pair<std::string, ObjectPtr>& object : objects_)
+    {
+      // if "object name/" matches start of object_id, we found the matching object
+      if (boost::starts_with(name, object.first) && name[object.first.length()] == '/')
+      {
+        auto it = object.second->subframe_poses_.find(name.substr(object.first.length() + 1));
+        if (it != object.second->subframe_poses_.end())
+        {
+          frame_found = true;
+          return it->second;
+        }
+      }
+    }
+  }
+
+  static const Eigen::Isometry3d IDENTITY_TRANSFORM = Eigen::Isometry3d::Identity();
+  frame_found = false;
+  return IDENTITY_TRANSFORM;
+}
+
 bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape,
                               const Eigen::Isometry3d& pose)
 {
@@ -218,6 +275,17 @@ void World::clearObjects()
   objects_.clear();
 }
 
+bool World::setSubframesOfObject(const std::string& object_id, const moveit::core::FixedTransformsMap& subframe_poses)
+{
+  auto obj_pair = objects_.find(object_id);
+  if (obj_pair == objects_.end())
+  {
+    return false;
+  }
+  obj_pair->second->subframe_poses_ = subframe_poses;
+  return true;
+}
+
 World::ObserverHandle World::addObserver(const ObserverCallbackFn& callback)
 {
   auto o = new Observer(callback);