00001 00046 #ifndef JACO_DRIVER_JACO_FINGERS_ACTION_H 00047 #define JACO_DRIVER_JACO_FINGERS_ACTION_H 00048 00049 #include <ros/ros.h> 00050 #include <actionlib/server/simple_action_server.h> 00051 00052 #include <jaco_msgs/SetFingersPositionAction.h> 00053 00054 #include "jaco_driver/jaco_comm.h" 00055 00056 00057 namespace jaco 00058 { 00059 00060 class JacoFingersActionServer 00061 { 00062 public: 00063 JacoFingersActionServer(JacoComm &, const ros::NodeHandle &n); 00064 ~JacoFingersActionServer(); 00065 00066 void actionCallback(const jaco_msgs::SetFingersPositionGoalConstPtr &); 00067 00068 private: 00069 ros::NodeHandle node_handle_; 00070 JacoComm &arm_comm_; 00071 actionlib::SimpleActionServer<jaco_msgs::SetFingersPositionAction> action_server_; 00072 00073 ros::Time last_nonstall_time_; 00074 jaco::FingerAngles last_nonstall_finger_positions_; 00075 00076 // Parameters 00077 double stall_interval_seconds_; 00078 double stall_threshold_; 00079 double rate_hz_; 00080 float tolerance_; 00081 }; 00082 00083 } // namespace jaco 00084 #endif // JACO_DRIVER_JACO_FINGERS_ACTION_H