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
00038 #ifndef MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE
00039 #define MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE
00040
00041 #include <moveit_simple_controller_manager/action_based_controller_handle.h>
00042 #include <control_msgs/GripperCommandAction.h>
00043
00044 namespace moveit_simple_controller_manager
00045 {
00046
00047
00048
00049
00050
00051 class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs::GripperCommandAction>
00052 {
00053 public:
00054
00055 GripperControllerHandle(const std::string &name, const std::string &ns) :
00056 ActionBasedControllerHandle<control_msgs::GripperCommandAction>(name, ns),
00057 allow_failure_(false)
00058 {
00059 }
00060
00061 virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
00062 {
00063 ROS_DEBUG_STREAM("GripperController: new trajectory to " << name_);
00064
00065 if (!controller_action_client_)
00066 return false;
00067
00068 if (!trajectory.multi_dof_joint_trajectory.points.empty())
00069 {
00070 ROS_ERROR("GripperController: cannot execute multi-dof trajectories.");
00071 return false;
00072 }
00073
00074 if (trajectory.joint_trajectory.points.size() != 1)
00075 {
00076 ROS_ERROR("GripperController: expects a joint trajectory with one point only, but %u provided)", (unsigned int)trajectory.joint_trajectory.points.size());
00077 return false;
00078 }
00079
00080
00081
00082 if (trajectory.joint_trajectory.points[0].positions.empty())
00083 {
00084 ROS_ERROR("GripperController: expects a joint trajectory with one point that specifies at least one position, but 0 positions provided)");
00085 return false;
00086 }
00087
00088
00089 control_msgs::GripperCommandGoal goal;
00090 if (!trajectory.joint_trajectory.points[0].velocities.empty())
00091 goal.command.max_effort = trajectory.joint_trajectory.points[0].velocities[0];
00092 goal.command.position = trajectory.joint_trajectory.points[0].positions[0];
00093 controller_action_client_->sendGoal(goal,
00094 boost::bind(&GripperControllerHandle::controllerDoneCallback, this, _1, _2),
00095 boost::bind(&GripperControllerHandle::controllerActiveCallback, this),
00096 boost::bind(&GripperControllerHandle::controllerFeedbackCallback, this, _1));
00097 done_ = false;
00098 last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
00099 return true;
00100 }
00101
00102 void setCommandJoint(const std::string& name) { command_joint_ = name; }
00103 void allowFailure(bool allow) { allow_failure_ = allow; }
00104
00105 private:
00106
00107 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
00108 const control_msgs::GripperCommandResultConstPtr& result)
00109 {
00110 if (state == actionlib::SimpleClientGoalState::ABORTED && allow_failure_)
00111 finishControllerExecution(actionlib::SimpleClientGoalState::SUCCEEDED);
00112 else
00113 finishControllerExecution(state);
00114 }
00115
00116 void controllerActiveCallback()
00117 {
00118 ROS_DEBUG_STREAM("GripperController: " << name_ << " started execution");
00119 }
00120
00121 void controllerFeedbackCallback(const control_msgs::GripperCommandFeedbackConstPtr& feedback)
00122 {
00123
00124 }
00125
00126
00127
00128
00129
00130 bool allow_failure_;
00131
00132
00133
00134
00135
00136
00137
00138 std::string command_joint_;
00139 };
00140
00141
00142 }
00143
00144 #endif // MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE