00001 00046 #ifndef JACO_DRIVER_JACO_POSE_ACTION_H_s 00047 #define JACO_DRIVER_JACO_POSE_ACTION_H_s 00048 00049 #include <ros/ros.h> 00050 #include <actionlib/server/simple_action_server.h> 00051 #include <tf/tf.h> 00052 #include <tf/transform_listener.h> 00053 00054 #include <jaco_msgs/ArmPoseAction.h> 00055 00056 #include <string> 00057 #include "jaco_driver/jaco_comm.h" 00058 00059 00060 namespace jaco 00061 { 00062 00063 class JacoPoseActionServer 00064 { 00065 public: 00066 JacoPoseActionServer(JacoComm &, const ros::NodeHandle &n); 00067 ~JacoPoseActionServer(); 00068 00069 void actionCallback(const jaco_msgs::ArmPoseGoalConstPtr &); 00070 00071 private: 00072 ros::NodeHandle node_handle_; 00073 JacoComm &arm_comm_; 00074 actionlib::SimpleActionServer<jaco_msgs::ArmPoseAction> action_server_; 00075 tf::TransformListener listener; 00076 00077 ros::Time last_nonstall_time_; 00078 jaco::JacoPose last_nonstall_pose_; 00079 00080 std::string api_origin_frame_; 00081 00082 // Parameters 00083 double stall_interval_seconds_; 00084 double stall_threshold_; 00085 double rate_hz_; 00086 float tolerance_; 00087 std::string tf_prefix_; 00088 }; 00089 00090 } // namespace jaco 00091 #endif // JACO_DRIVER_JACO_POSE_ACTION_H_s 00092