00001 #include <ros/ros.h> 00002 #include <actionlib/client/simple_action_client.h> 00003 #include <actionlib/client/terminal_state.h> 00004 #include <carl_moveit/MoveToPoseAction.h> 00005 #include <carl_moveit/CallIK.h> 00006 #include <carl_moveit/CartesianPath.h> 00007 #include <control_msgs/FollowJointTrajectoryAction.h> 00008 #include <rail_grasping/RequestGrasp.h> 00009 #include <sensor_msgs/JointState.h> 00010 #include <tf/transform_broadcaster.h> 00011 #include <tf/transform_listener.h> 00012 #include <wpi_jaco_msgs/ExecutePickupAction.h> 00013 #include <wpi_jaco_msgs/ExecuteGraspAction.h> 00014 #include <wpi_jaco_msgs/GetCartesianPosition.h> 00015 #include <wpi_jaco_msgs/CartesianCommand.h> 00016 00017 #define NUM_JACO_JOINTS 6 00018 00019 class graspObject 00020 { 00021 public: 00022 //ROS publishers, subscribers, and action servers 00023 ros::NodeHandle n; 00024 00025 ros::Publisher cartesianCommandPub; 00026 00027 ros::Subscriber armJointSubscriber; 00028 00029 ros::ServiceClient IKClient; 00030 ros::ServiceClient cartesianPositionClient; 00031 ros::ServiceClient cartesianPathClient; 00032 00033 ros::ServiceServer requestGraspServer; 00034 ros::ServiceServer requestReleaseServer; 00035 00036 //action clients 00037 actionlib::SimpleActionClient<wpi_jaco_msgs::ExecutePickupAction> acPickup; 00038 actionlib::SimpleActionClient<wpi_jaco_msgs::ExecuteGraspAction> acGrasp; 00039 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> acJointTrajectory; 00040 actionlib::SimpleActionClient<carl_moveit::MoveToPoseAction> acMoveArm; 00041 00042 //tf 00043 tf::TransformBroadcaster tfBroadcaster; 00044 tf::TransformListener tfListener; 00045 tf::Transform graspTransform; 00046 00047 std::vector<double> armJointPos; 00048 std::vector<std::string> armJointNames; 00049 bool jointNamesSet; 00050 00054 graspObject(); 00055 00056 void armJointStatesCallback(const sensor_msgs::JointState &msg); 00057 00058 bool requestGrasp(rail_grasping::RequestGrasp::Request &req, rail_grasping::RequestGrasp::Response &res); 00059 00060 bool requestRelease(rail_grasping::RequestGrasp::Request &req, rail_grasping::RequestGrasp::Response &res); 00061 00062 bool executeGrasp(bool *earlyFailureFlag); 00063 00064 void publishGraspFrame(); 00065 };