Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/planning_scene_interface/planning_scene_interface.h>
00038 #include <moveit/py_bindings_tools/roscpp_initializer.h>
00039 #include <moveit/py_bindings_tools/py_conversions.h>
00040 #include <moveit/py_bindings_tools/serialize_msg.h>
00041
00042 #include <boost/function.hpp>
00043 #include <boost/python.hpp>
00044 #include <Python.h>
00045
00048 namespace bp = boost::python;
00049
00050 namespace moveit
00051 {
00052 namespace planning_interface
00053 {
00054 class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public PlanningSceneInterface
00055 {
00056 public:
00057
00058 PlanningSceneInterfaceWrapper() : py_bindings_tools::ROScppInitializer(), PlanningSceneInterface()
00059 {
00060 }
00061
00062 bp::list getKnownObjectNamesPython(bool with_type = false)
00063 {
00064 return py_bindings_tools::listFromString(getKnownObjectNames(with_type));
00065 }
00066
00067 bp::list getKnownObjectNamesInROIPython(double minx, double miny, double minz, double maxx, double maxy, double maxz,
00068 bool with_type = false)
00069 {
00070 return py_bindings_tools::listFromString(getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type));
00071 }
00072
00073 bp::dict getObjectPosesPython(bp::list object_ids)
00074 {
00075 std::map<std::string, geometry_msgs::Pose> ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids));
00076 std::map<std::string, std::string> ser_ops;
00077 for (std::map<std::string, geometry_msgs::Pose>::const_iterator it = ops.begin(); it != ops.end(); ++it)
00078 ser_ops[it->first] = py_bindings_tools::serializeMsg(it->second);
00079
00080 return py_bindings_tools::dictFromType(ser_ops);
00081 }
00082
00083 bp::dict getObjectsPython(bp::list object_ids)
00084 {
00085 std::map<std::string, moveit_msgs::CollisionObject> objs =
00086 getObjects(py_bindings_tools::stringFromList(object_ids));
00087 std::map<std::string, std::string> ser_objs;
00088 for (std::map<std::string, moveit_msgs::CollisionObject>::const_iterator it = objs.begin(); it != objs.end(); ++it)
00089 ser_objs[it->first] = py_bindings_tools::serializeMsg(it->second);
00090
00091 return py_bindings_tools::dictFromType(ser_objs);
00092 }
00093
00094 bp::dict getAttachedObjectsPython(const bp::list& object_ids)
00095 {
00096 std::map<std::string, moveit_msgs::AttachedCollisionObject> aobjs =
00097 getAttachedObjects(py_bindings_tools::stringFromList(object_ids));
00098 std::map<std::string, std::string> ser_aobjs;
00099 for (std::map<std::string, moveit_msgs::AttachedCollisionObject>::const_iterator it = aobjs.begin();
00100 it != aobjs.end(); ++it)
00101 ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second);
00102
00103 return py_bindings_tools::dictFromType(ser_aobjs);
00104 }
00105 };
00106
00107 static void wrap_planning_scene_interface()
00108 {
00109 bp::class_<PlanningSceneInterfaceWrapper> PlanningSceneClass("PlanningSceneInterface");
00110
00111 PlanningSceneClass.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython);
00112 PlanningSceneClass.def("get_known_object_names_in_roi",
00113 &PlanningSceneInterfaceWrapper::getKnownObjectNamesInROIPython);
00114 PlanningSceneClass.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython);
00115 PlanningSceneClass.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython);
00116 PlanningSceneClass.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython);
00117 }
00118 }
00119 }
00120
00121 BOOST_PYTHON_MODULE(_moveit_planning_scene_interface)
00122 {
00123 using namespace moveit::planning_interface;
00124 wrap_planning_scene_interface();
00125 }
00126