Use action for trajectory execution in move_group_interface

moveit · Jul 18, 2016 · 4d93afd · 4d93afd
1 parent f75ae75
commit 4d93afd
Showing 1 changed file with 47 additions and 8 deletions.
diff --git a/planning_interface/move_group_interface/src/move_group.cpp b/planning_interface/move_group_interface/src/move_group.cpp
@@ -48,6 +48,7 @@
 #include <moveit/common_planning_interface_objects/common_objects.h>
 #include <moveit/robot_state/conversions.h>
 #include <moveit_msgs/MoveGroupAction.h>
+#include <moveit_msgs/ExecuteKnownTrajectoryAction.h>
 #include <moveit_msgs/PickupAction.h>
 #include <moveit_msgs/PlaceAction.h>
 #include <moveit_msgs/ExecuteKnownTrajectory.h>
@@ -135,6 +136,11 @@ class MoveGroup::MoveGroupImpl
                                                                                               false));
     waitForAction(move_action_client_, wait_for_server, move_group::MOVE_ACTION);
 
+    execute_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::ExecuteKnownTrajectoryAction>(node_handle_,
+                                                                                                              move_group::EXECUTE_ACTION,
+                                                                                                              false));
+    waitForAction(execute_action_client_, wait_for_server, move_group::EXECUTE_ACTION);
+
     pick_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::PickupAction>(node_handle_,
                                                                                            move_group::PICKUP_ACTION,
                                                                                            false));
@@ -145,7 +151,7 @@ class MoveGroup::MoveGroupImpl
                                                                                            false));
     waitForAction(place_action_client_, wait_for_server, move_group::PLACE_ACTION);
 
-    execute_service_ = node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
+    // execute_service_ = node_handle_.serviceClient<moveit_msgs::ExecuteKnownTrajectory>(move_group::EXECUTE_SERVICE_NAME);
     query_service_ = node_handle_.serviceClient<moveit_msgs::QueryPlannerInterfaces>(move_group::QUERY_PLANNERS_SERVICE_NAME);
     cartesian_path_service_ = node_handle_.serviceClient<moveit_msgs::GetCartesianPath>(move_group::CARTESIAN_PATH_SERVICE_NAME);
 
@@ -675,18 +681,50 @@ class MoveGroup::MoveGroupImpl
 
   MoveItErrorCode execute(const Plan &plan, bool wait)
   {
-    moveit_msgs::ExecuteKnownTrajectory::Request req;
-    moveit_msgs::ExecuteKnownTrajectory::Response res;
-    req.trajectory = plan.trajectory_;
-    req.wait_for_execution = wait;
-    if (execute_service_.call(req, res))
+    // moveit_msgs::ExecuteKnownTrajectory::Request req;
+    // moveit_msgs::ExecuteKnownTrajectory::Response res;
+    // req.trajectory = plan.trajectory_;
+    // req.wait_for_execution = wait;
+    // if (execute_service_.call(req, res))
+    // {
+    //   return MoveItErrorCode(res.error_code);
+    // }
+    // else
+    // {
+    //   return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+    // }
+    if (!execute_action_client_)
     {
-      return MoveItErrorCode(res.error_code);
+      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
     }
-    else
+    if (!execute_action_client_->isServerConnected())
     {
       return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
     }
+
+    moveit_msgs::ExecuteKnownTrajectoryGoal goal;
+    goal.trajectory = plan.trajectory_;
+
+    execute_action_client_->sendGoal(goal);
+    if (!wait)
+    {
+      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
+    }
+
+    if (!execute_action_client_->waitForResult())
+    {
+      ROS_INFO_STREAM("ExecuteKnownTrajectory action returned early");
+    }
+
+    if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
+    {
+      return MoveItErrorCode(execute_action_client_->getResult()->error_code);
+    }
+    else
+    {
+      ROS_INFO_STREAM(execute_action_client_->getState().toString() << ": " << execute_action_client_->getState().getText());
+      return MoveItErrorCode(execute_action_client_->getResult()->error_code);
+    }
   }
 
   double computeCartesianPath(const std::vector<geometry_msgs::Pose> &waypoints, double step, double jump_threshold,
@@ -1035,6 +1073,7 @@ class MoveGroup::MoveGroupImpl
   robot_model::RobotModelConstPtr robot_model_;
   planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_;
   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> > move_action_client_;
+  boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteKnownTrajectoryAction> > execute_action_client_;
   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction> > pick_action_client_;
   boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction> > place_action_client_;