Implement issue moveit#630

Author: corot

planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h

@@ -38,6 +38,7 @@
 #define MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_
 
 #include <moveit/robot_state/robot_state.h>
+#include <moveit_msgs/ObjectColor.h>
 #include <moveit_msgs/CollisionObject.h>
 #include <moveit_msgs/AttachedCollisionObject.h>
 
@@ -72,13 +73,21 @@ class PlanningSceneInterface
     return getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, empty_vector_string);
   };
 
+  /** \brief Get the poses from the objects identified by the given object ids list. */
   std::map<std::string, geometry_msgs::Pose> getObjectPoses(const std::vector<std::string> &object_ids);
 
-  /** \brief Add collision objects to the world
-      Make sure object.operation is set to object.ADD*/
-  void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects) const;
+  /** \brief Get the objects identified by the given object ids list. If no ids are provided, return all the known objects. */
+  std::map<std::string, moveit_msgs::CollisionObject> getObjects(const std::vector<std::string> &object_ids = std::vector<std::string>());
 
-  /** \brief Remove collision objects from the world*/
+  /** \brief Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects. */
+  std::map<std::string, moveit_msgs::AttachedCollisionObject> getAttachedObjects(const std::vector<std::string> &object_ids = std::vector<std::string>());
+
+  /** \brief Add collision objects to the world.
+      Make sure object.operation is set to object.ADD. */
+  void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects,
+                           const std::vector<moveit_msgs::ObjectColor> &object_colors = std::vector<moveit_msgs::ObjectColor>()) const;
+
+  /** \brief Remove collision objects from the world */
   void removeCollisionObjects(const std::vector<std::string> &object_ids) const;
 
   /**@}*/

planning_interface/planning_scene_interface/src/planning_scene_interface.cpp

@@ -38,6 +38,7 @@
 #include <moveit/move_group/capability_names.h>
 #include <moveit_msgs/GetPlanningScene.h>
 #include <ros/ros.h>
+#include <algorithm>
 
 namespace moveit
 {
@@ -140,29 +141,73 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl
       return result;
     }
 
-    for(std::size_t i=0; i < object_ids.size(); ++i)
+    for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
     {
-      for (std::size_t j=0; j < response.scene.world.collision_objects.size() ; ++j)
+      if (std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) != object_ids.end())
       {
-        if(response.scene.world.collision_objects[j].id == object_ids[i])
-        {
-          if (response.scene.world.collision_objects[j].mesh_poses.empty() &&
-              response.scene.world.collision_objects[j].primitive_poses.empty())
-            continue;
-          if(!response.scene.world.collision_objects[j].mesh_poses.empty())
-            result[response.scene.world.collision_objects[j].id] = response.scene.world.collision_objects[j].mesh_poses[0];
-          else
-            result[response.scene.world.collision_objects[j].id] = response.scene.world.collision_objects[j].primitive_poses[0];
-        }
+        if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
+            response.scene.world.collision_objects[i].primitive_poses.empty())
+          continue;
+        if(!response.scene.world.collision_objects[i].mesh_poses.empty())
+          result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i].mesh_poses[0];
+        else
+          result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i].primitive_poses[0];
+      }
+    }
+    return result;
+  }
+
+  std::map<std::string, moveit_msgs::CollisionObject> getObjects(const std::vector<std::string> &object_ids)
+  {
+    moveit_msgs::GetPlanningScene::Request request;
+    moveit_msgs::GetPlanningScene::Response response;
+    std::map<std::string, moveit_msgs::CollisionObject> result;
+    request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
+    if (!planning_scene_service_.call(request, response))
+    {
+      ROS_WARN("Could not call planning scene service to get object geometries");
+      return result;
+    }
+
+    for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
+    {
+      if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) != object_ids.end())
+      {
+        result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i];
+      }
+    }
+    return result;
+  }
+
+  std::map<std::string, moveit_msgs::AttachedCollisionObject> getAttachedObjects(const std::vector<std::string> &object_ids)
+  {
+    moveit_msgs::GetPlanningScene::Request request;
+    moveit_msgs::GetPlanningScene::Response response;
+    std::map<std::string, moveit_msgs::AttachedCollisionObject> result;
+    request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS;
+    if (!planning_scene_service_.call(request, response))
+    {
+      ROS_WARN("Could not call planning scene service to get attached object geometries");
+      return result;
+    }
+
+    for (std::size_t i = 0; i < response.scene.robot_state.attached_collision_objects.size(); ++i)
+    {
+      if (object_ids.empty() ||
+          std::find(object_ids.begin(), object_ids.end(), response.scene.robot_state.attached_collision_objects[i].object.id) != object_ids.end())
+      {
+        result[response.scene.robot_state.attached_collision_objects[i].object.id] = response.scene.robot_state.attached_collision_objects[i];
       }
     }
     return result;
   }
 
-  void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects) const
+  void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects,
+                           const std::vector<moveit_msgs::ObjectColor> &object_colors) const
   {
     moveit_msgs::PlanningScene planning_scene;
     planning_scene.world.collision_objects = collision_objects;
+    planning_scene.object_colors = object_colors;
     planning_scene.is_diff = true;
     planning_scene_diff_publisher_.publish(planning_scene);
   }
@@ -214,9 +259,20 @@ std::map<std::string, geometry_msgs::Pose> PlanningSceneInterface::getObjectPose
   return impl_->getObjectPoses(object_ids);
 }
 
-void PlanningSceneInterface::addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects) const
+std::map<std::string, moveit_msgs::CollisionObject> PlanningSceneInterface::getObjects(const std::vector<std::string> &object_ids)
+{
+  return impl_->getObjects(object_ids);
+}
+
+std::map<std::string, moveit_msgs::AttachedCollisionObject> PlanningSceneInterface::getAttachedObjects(const std::vector<std::string> &object_ids)
+{
+  return impl_->getAttachedObjects(object_ids);
+}
+
+void PlanningSceneInterface::addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects,
+                                                 const std::vector<moveit_msgs::ObjectColor> &object_colors) const
 {
-  return impl_->addCollisionObjects(collision_objects);
+  return impl_->addCollisionObjects(collision_objects, object_colors);
 }
 
 void PlanningSceneInterface::removeCollisionObjects(const std::vector<std::string> &object_ids) const

planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp

@@ -37,6 +37,7 @@
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit/py_bindings_tools/roscpp_initializer.h>
 #include <moveit/py_bindings_tools/py_conversions.h>
+#include <moveit/py_bindings_tools/serialize_msg.h>
 
 #include <boost/function.hpp>
 #include <boost/python.hpp>
@@ -64,12 +65,42 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial
 
   bp::list getKnownObjectNamesPython(bool with_type = false)
   {
-    return moveit::py_bindings_tools::listFromString(getKnownObjectNames(with_type));
+    return py_bindings_tools::listFromString(getKnownObjectNames(with_type));
   }
 
   bp::list getKnownObjectNamesInROIPython(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type = false)
   {
-    return moveit::py_bindings_tools::listFromString(getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type));
+    return py_bindings_tools::listFromString(getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type));
+  }
+
+  bp::dict getObjectPosesPython(bp::list object_ids)
+  {
+    std::map<std::string, geometry_msgs::Pose> ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids));
+    std::map<std::string, std::string> ser_ops;
+    for (std::map<std::string, geometry_msgs::Pose>::const_iterator it = ops.begin(); it != ops.end(); ++it)
+      ser_ops[it->first] = py_bindings_tools::serializeMsg(it->second);
+
+    return py_bindings_tools::dictFromType(ser_ops);
+  }
+
+  bp::dict getObjectsPython(bp::list object_ids)
+  {
+    std::map<std::string, moveit_msgs::CollisionObject> objs = getObjects(py_bindings_tools::stringFromList(object_ids));
+    std::map<std::string, std::string> ser_objs;
+    for (std::map<std::string, moveit_msgs::CollisionObject>::const_iterator it = objs.begin(); it != objs.end(); ++it)
+      ser_objs[it->first] = py_bindings_tools::serializeMsg(it->second);
+
+    return py_bindings_tools::dictFromType(ser_objs);
+  }
+
+  bp::dict getAttachedObjectsPython(const bp::list &object_ids)
+  {
+    std::map<std::string, moveit_msgs::AttachedCollisionObject> aobjs = getAttachedObjects(py_bindings_tools::stringFromList(object_ids));
+    std::map<std::string, std::string> ser_aobjs;
+    for (std::map<std::string, moveit_msgs::AttachedCollisionObject>::const_iterator it = aobjs.begin(); it != aobjs.end(); ++it)
+      ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second);
+
+    return py_bindings_tools::dictFromType(ser_aobjs);
   }
 
 };
@@ -80,6 +111,9 @@ static void wrap_planning_scene_interface()
 
   PlanningSceneClass.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython);
   PlanningSceneClass.def("get_known_object_names_in_roi", &PlanningSceneInterfaceWrapper::getKnownObjectNamesInROIPython);
+  PlanningSceneClass.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython);
+  PlanningSceneClass.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython);
+  PlanningSceneClass.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython);
 }
 
 }

planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h

@@ -40,6 +40,7 @@
 #include <boost/python.hpp>
 #include <string>
 #include <vector>
+#include <map>
 
 namespace moveit
 {
@@ -65,6 +66,15 @@ boost::python::list listFromType(const std::vector<T>& v)
   return l;
 }
 
+template<typename T>
+boost::python::dict dictFromType(const std::map<std::string, T>& v)
+{
+  boost::python::dict d;
+  for (typename std::map<std::string, T>::const_iterator it = v.begin(); it != v.end(); ++it)
+    d[it->first] = it->second;
+  return d;
+}
+
 std::vector<double> doubleFromList(const boost::python::list &values)
 {
   return typeFromList<double>(values);