diff --git a/planning_interface/move_group_interface/src/move_group.cpp b/planning_interface/move_group_interface/src/move_group.cpp index d3029b5aa5..beba96b913 100644 --- 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 #include #include +#include #include #include #include @@ -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(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(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(move_group::EXECUTE_SERVICE_NAME); + // execute_service_ = node_handle_.serviceClient(move_group::EXECUTE_SERVICE_NAME); query_service_ = node_handle_.serviceClient(move_group::QUERY_PLANNERS_SERVICE_NAME); cartesian_path_service_ = node_handle_.serviceClient(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 &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 > move_action_client_; + boost::scoped_ptr > execute_action_client_; boost::scoped_ptr > pick_action_client_; boost::scoped_ptr > place_action_client_;