00001 00011 #ifndef JACO_MANIPULATION_H_ 00012 #define JACO_MANIPULATION_H_ 00013 00014 #include <ros/ros.h> 00015 00016 #include <actionlib/client/simple_action_client.h> 00017 #include <actionlib/server/simple_action_server.h> 00018 #include <control_msgs/GripperCommandAction.h> 00019 #include <rail_manipulation_msgs/GripperAction.h> 00020 #include <rail_manipulation_msgs/LiftAction.h> 00021 #include <std_srvs/Empty.h> 00022 #include <wpi_jaco_msgs/AngularCommand.h> 00023 #include <wpi_jaco_msgs/CartesianCommand.h> 00024 #include <wpi_jaco_msgs/GetCartesianPosition.h> 00025 #include <sensor_msgs/JointState.h> 00026 00027 #define NUM_JACO_JOINTS 6 00028 00029 #define MAX_FINGER_VEL 30 //maximum finger actuator velocity 00030 #define DEFAULT_LIFT_VEL .1 //the default velocity for lifting objects during pickup (m/s) 00031 #define LIFT_HEIGHT .15 //height for object pickup (m) 00032 #define LIFT_TIMEOUT 5 //timeout for pickup action (s) 00033 #define GRIPPER_OPEN_THRESHOLD .02 //gripper position where the fingers are considered "open" 00034 00042 class JacoManipulation 00043 { 00044 public: 00045 typedef actionlib::SimpleActionClient<control_msgs::GripperCommandAction> GripperClient; 00046 typedef actionlib::SimpleActionServer<rail_manipulation_msgs::GripperAction> GripperServer; 00047 typedef actionlib::SimpleActionServer<rail_manipulation_msgs::LiftAction> LiftServer; 00051 JacoManipulation(); 00052 00057 void execute_gripper(const rail_manipulation_msgs::GripperGoalConstPtr &goal); 00058 00063 void execute_lift(const rail_manipulation_msgs::LiftGoalConstPtr &goal); 00064 00069 void jointStateCallback(const sensor_msgs::JointState msg); 00070 00071 private: 00072 bool loadParameters(const ros::NodeHandle n); 00073 00074 ros::NodeHandle n, pnh; 00075 00076 // Parameters 00077 std::string arm_name_; 00078 std::string topic_prefix_; 00079 double gripper_closed_; 00080 double gripper_open_; 00081 int num_fingers_; 00082 int num_joints_; 00083 bool kinova_gripper_; 00084 00085 // Messages 00086 ros::Publisher cartesianCmdPublisher; 00087 ros::Publisher angularCmdPublisher; 00088 ros::Subscriber jointStateSubscriber; 00089 00090 // Services 00091 ros::ServiceClient cartesianPositionClient; 00092 ros::ServiceClient eraseTrajectoriesClient; 00093 00094 // Actionlib 00095 GripperClient* acGripper; 00096 GripperServer* asGripper; 00097 LiftServer* asLift; 00098 00099 std::vector<double> joint_pos_; 00100 00101 00102 }; 00103 00104 #endif