wrap_python_planning_scene_interface.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
41 
42 #include <boost/function.hpp>
43 #include <boost/python.hpp>
44 #include <Python.h>
45 
48 namespace bp = boost::python;
49 
50 namespace moveit
51 {
52 namespace planning_interface
53 {
54 class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public PlanningSceneInterface
55 {
56 public:
57  // ROSInitializer is constructed first, and ensures ros::init() was called, if needed
58  PlanningSceneInterfaceWrapper(const std::string& ns = "")
59  : py_bindings_tools::ROScppInitializer(), PlanningSceneInterface(ns)
60  {
61  }
62 
63  bp::list getKnownObjectNamesPython(bool with_type = false)
64  {
65  return py_bindings_tools::listFromString(getKnownObjectNames(with_type));
66  }
67 
68  bp::list getKnownObjectNamesInROIPython(double minx, double miny, double minz, double maxx, double maxy, double maxz,
69  bool with_type = false)
70  {
71  return py_bindings_tools::listFromString(getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type));
72  }
73 
74  bp::dict getObjectPosesPython(bp::list object_ids)
75  {
76  std::map<std::string, geometry_msgs::Pose> ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids));
77  std::map<std::string, std::string> ser_ops;
78  for (std::map<std::string, geometry_msgs::Pose>::const_iterator it = ops.begin(); it != ops.end(); ++it)
79  ser_ops[it->first] = py_bindings_tools::serializeMsg(it->second);
80 
81  return py_bindings_tools::dictFromType(ser_ops);
82  }
83 
84  bp::dict getObjectsPython(bp::list object_ids)
85  {
86  std::map<std::string, moveit_msgs::CollisionObject> objs =
87  getObjects(py_bindings_tools::stringFromList(object_ids));
88  std::map<std::string, std::string> ser_objs;
89  for (std::map<std::string, moveit_msgs::CollisionObject>::const_iterator it = objs.begin(); it != objs.end(); ++it)
90  ser_objs[it->first] = py_bindings_tools::serializeMsg(it->second);
91 
92  return py_bindings_tools::dictFromType(ser_objs);
93  }
94 
95  bp::dict getAttachedObjectsPython(const bp::list& object_ids)
96  {
97  std::map<std::string, moveit_msgs::AttachedCollisionObject> aobjs =
98  getAttachedObjects(py_bindings_tools::stringFromList(object_ids));
99  std::map<std::string, std::string> ser_aobjs;
100  for (std::map<std::string, moveit_msgs::AttachedCollisionObject>::const_iterator it = aobjs.begin();
101  it != aobjs.end(); ++it)
102  ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second);
103 
104  return py_bindings_tools::dictFromType(ser_aobjs);
105  }
106 
107  bool applyPlanningScenePython(const std::string& ps_str)
108  {
109  moveit_msgs::PlanningScene ps_msg;
110  py_bindings_tools::deserializeMsg(ps_str, ps_msg);
111  return applyPlanningScene(ps_msg);
112  }
113 };
114 
115 static void wrap_planning_scene_interface()
116 {
117  bp::class_<PlanningSceneInterfaceWrapper> PlanningSceneClass("PlanningSceneInterface",
118  bp::init<bp::optional<std::string>>());
119 
120  PlanningSceneClass.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython);
121  PlanningSceneClass.def("get_known_object_names_in_roi",
122  &PlanningSceneInterfaceWrapper::getKnownObjectNamesInROIPython);
123  PlanningSceneClass.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython);
124  PlanningSceneClass.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython);
125  PlanningSceneClass.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython);
126  PlanningSceneClass.def("apply_planning_scene", &PlanningSceneInterfaceWrapper::applyPlanningScenePython);
127 }
128 }
129 }
130 
131 BOOST_PYTHON_MODULE(_moveit_planning_scene_interface)
132 {
133  using namespace moveit::planning_interface;
134  wrap_planning_scene_interface();
135 }
136 
boost::python::dict dictFromType(const std::map< std::string, T > &v)
BOOST_PYTHON_MODULE(_moveit_roscpp_initializer)
std::vector< std::string > stringFromList(const boost::python::object &values)
Simple interface to MoveIt! components.
boost::python::list listFromString(const std::vector< std::string > &v)
std::string serializeMsg(const T &msg)
Convert a ROS message to a string.
Definition: serialize_msg.h:48
void deserializeMsg(const std::string &data, T &msg)
Convert a string to a ROS message.
Definition: serialize_msg.h:66


planning_interface
Author(s): Ioan Sucan
autogenerated on Mon Jul 16 2018 19:29:14