MoveGroupCommander.plan() now returns [success, trajectory, planning_…

…time, error_code]
moveit · Mar 6, 2018 · e70ecd1 · e70ecd1
1 parent 61e2e30
commit e70ecd1
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 2 deletions.
diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py
@@ -34,6 +34,7 @@
 
 from geometry_msgs.msg import Pose, PoseStamped
 from moveit_msgs.msg import RobotTrajectory, Grasp, PlaceLocation, Constraints
+from moveit_msgs.msg import MoveItErrorCodes
 from sensor_msgs.msg import JointState
 import rospy
 import tf
@@ -471,9 +472,16 @@ def plan(self, joints = None):
                 self.set_joint_value_target(self.get_remembered_joint_values()[joints])
             except:
                 self.set_joint_value_target(joints)
+
+        (error_code_msg, trajectory_msg, planning_time) = self._g.plan()
+
+        error_code = MoveItErrorCodes()
+        error_code.deserialize(error_code_msg)
         plan = RobotTrajectory()
-        plan.deserialize(self._g.compute_plan())
-        return plan
+        return (error_code.val == MoveItErrorCodes.SUCCESS,
+                plan.deserialize(trajectory_msg),
+                planning_time,
+                error_code)
 
     def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True):
         """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """

diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp
@@ -395,6 +395,15 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     return py_bindings_tools::serializeMsg(plan.trajectory_);
   }
 
+  bp::tuple planPython()
+  {
+    MoveGroupInterface::Plan plan;
+    moveit_msgs::MoveItErrorCodes res = MoveGroupInterface::plan(plan);
+    return bp::make_tuple(py_bindings_tools::serializeMsg(res),
+                          py_bindings_tools::serializeMsg(plan.trajectory_),
+                          plan.planning_time_);
+  }
+
   bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold,
                                        bool avoid_collisions)
   {
@@ -599,6 +608,7 @@ static void wrap_move_group_interface()
   MoveGroupInterfaceClass.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId);
   MoveGroupInterfaceClass.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts);
   MoveGroupInterfaceClass.def("compute_plan", &MoveGroupInterfaceWrapper::getPlanPython);
+  MoveGroupInterfaceClass.def("plan", &MoveGroupInterfaceWrapper::planPython);
   MoveGroupInterfaceClass.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython);
   MoveGroupInterfaceClass.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName);
   MoveGroupInterfaceClass.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython);