00001 00006 #ifndef ROBOTIQ_C_MODEL_ACTION_SERVER_H 00007 #define ROBOTIQ_C_MODEL_ACTION_SERVER_H 00008 00009 // STL 00010 #include <string> 00011 // ROS standard 00012 #include <ros/ros.h> 00013 #include <actionlib/server/simple_action_server.h> 00014 #include <control_msgs/GripperCommandAction.h> 00015 // Repo specific includes 00016 #include <robotiq_c_model_control/CModel_robot_input.h> 00017 #include <robotiq_c_model_control/CModel_robot_output.h> 00018 00019 00020 namespace robotiq_action_server 00021 { 00022 00023 typedef robotiq_c_model_control::CModel_robot_input GripperInput; 00024 typedef robotiq_c_model_control::CModel_robot_output GripperOutput; 00025 00026 typedef control_msgs::GripperCommandGoal GripperCommandGoal; 00027 typedef control_msgs::GripperCommandFeedback GripperCommandFeedback; 00028 typedef control_msgs::GripperCommandResult GripperCommandResult; 00029 00038 struct CModelGripperParams 00039 { 00040 double min_gap_; // meters 00041 double max_gap_; 00042 double min_effort_; // N / (Nm) 00043 double max_effort_; 00044 }; 00045 00052 class CModelGripperActionServer 00053 { 00054 public: 00055 CModelGripperActionServer(const std::string& name, const CModelGripperParams& params); 00056 00057 // These functions are meant to be called by simple action server 00058 void goalCB(); 00059 void preemptCB(); 00060 void analysisCB(const GripperInput::ConstPtr& msg); 00061 00062 private: 00063 void issueActivation(); 00064 00065 ros::NodeHandle nh_; 00066 actionlib::SimpleActionServer<control_msgs::GripperCommandAction> as_; 00067 00068 ros::Subscriber state_sub_; // Subs to grippers "input" topic 00069 ros::Publisher goal_pub_; // Pubs to grippers "output" topic 00070 00071 GripperOutput goal_reg_state_; // Goal information in gripper-register form 00072 GripperInput current_reg_state_; // State info in gripper-register form 00073 00074 /* Used to translate GripperCommands in engineering units 00075 * to/from register states understood by gripper itself. Different 00076 * for different models/generations of Robotiq grippers */ 00077 CModelGripperParams gripper_params_; 00078 00079 std::string action_name_; 00080 }; 00081 00082 } 00083 #endif