diff --git a/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 4d4882ab62..cb87129c90 100644 --- a/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/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 +#include #include #include @@ -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 getObjectPoses(const std::vector &object_ids); - /** \brief Add collision objects to the world - Make sure object.operation is set to object.ADD*/ - void addCollisionObjects(const std::vector &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 getObjects(const std::vector &object_ids = std::vector()); - /** \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 getAttachedObjects(const std::vector &object_ids = std::vector()); + + /** \brief Add collision objects to the world. + Make sure object.operation is set to object.ADD. */ + void addCollisionObjects(const std::vector &collision_objects, + const std::vector &object_colors = std::vector()) const; + + /** \brief Remove collision objects from the world */ void removeCollisionObjects(const std::vector &object_ids) const; /**@}*/ diff --git a/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 638353e5f3..ff1492463a 100644 --- a/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -38,6 +38,7 @@ #include #include #include +#include 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 getObjects(const std::vector &object_ids) + { + moveit_msgs::GetPlanningScene::Request request; + moveit_msgs::GetPlanningScene::Response response; + std::map 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 getAttachedObjects(const std::vector &object_ids) + { + moveit_msgs::GetPlanningScene::Request request; + moveit_msgs::GetPlanningScene::Response response; + std::map 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 &collision_objects) const + void addCollisionObjects(const std::vector &collision_objects, + const std::vector &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 PlanningSceneInterface::getObjectPose return impl_->getObjectPoses(object_ids); } -void PlanningSceneInterface::addCollisionObjects(const std::vector &collision_objects) const +std::map PlanningSceneInterface::getObjects(const std::vector &object_ids) +{ + return impl_->getObjects(object_ids); +} + +std::map PlanningSceneInterface::getAttachedObjects(const std::vector &object_ids) +{ + return impl_->getAttachedObjects(object_ids); +} + +void PlanningSceneInterface::addCollisionObjects(const std::vector &collision_objects, + const std::vector &object_colors) const { - return impl_->addCollisionObjects(collision_objects); + return impl_->addCollisionObjects(collision_objects, object_colors); } void PlanningSceneInterface::removeCollisionObjects(const std::vector &object_ids) const diff --git a/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp b/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp index 80612eff67..ba95a0a388 100644 --- a/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp +++ b/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -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 ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids)); + std::map ser_ops; + for (std::map::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 objs = getObjects(py_bindings_tools::stringFromList(object_ids)); + std::map ser_objs; + for (std::map::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 aobjs = getAttachedObjects(py_bindings_tools::stringFromList(object_ids)); + std::map ser_aobjs; + for (std::map::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); } } diff --git a/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h b/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h index e3c56b191c..2c26c44f21 100644 --- a/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h +++ b/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h @@ -40,6 +40,7 @@ #include #include #include +#include namespace moveit { @@ -65,6 +66,15 @@ boost::python::list listFromType(const std::vector& v) return l; } +template +boost::python::dict dictFromType(const std::map& v) +{ + boost::python::dict d; + for (typename std::map::const_iterator it = v.begin(); it != v.end(); ++it) + d[it->first] = it->second; + return d; +} + std::vector doubleFromList(const boost::python::list &values) { return typeFromList(values);