example_grasp.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <iri_wam_common_msgs/SimpleBhandPickUpAction.h>
00003 #include <geometry_msgs/PoseStamped.h>
00004 #include <object_manipulation_msgs/GripperTranslation.h>
00005 #include <actionlib/client/simple_action_client.h>
00006 
00007 typedef actionlib::SimpleActionClient< iri_wam_common_msgs::SimpleBhandPickUpAction > graspClient;
00008 
00009 class RobotGrasp
00010 {
00011 private:
00012   // Action client for the joint trajectory action 
00013   // used to trigger the arm movement action
00014   graspClient * grasp_client_;
00015 
00016 public:
00018   RobotGrasp() 
00019   {
00020     // tell the action client that we want to spin a thread by default
00021     grasp_client_ = new graspClient("/estirabot/skills/grasp/pickup", true);
00022 
00023     // wait for action server to come up
00024     while(!grasp_client_->waitForServer(ros::Duration(5.0))){
00025       ROS_INFO("Waiting for the pick action server");
00026     }
00027   }
00028 
00030   ~RobotGrasp()
00031   {
00032     delete grasp_client_;
00033   }
00034 
00036   void startGrasp(iri_wam_common_msgs::SimpleBhandPickUpGoal goal)
00037   {
00038     grasp_client_->sendGoal(goal);
00039   }
00040 
00041   iri_wam_common_msgs::SimpleBhandPickUpGoal generateGraspExampleMsg()
00042   {
00043       iri_wam_common_msgs::SimpleBhandPickUpGoal goal;
00044       ros::Time now = ros::Time::now();
00045 
00046       geometry_msgs::PoseStamped grasp_pose;
00047 
00048       // Grasp pose is home position -0.2m in Z
00049       grasp_pose.header.frame_id = "/wam_link0";
00050       grasp_pose.header.stamp = now;
00051       grasp_pose.pose.position.x = 0.4;
00052       grasp_pose.pose.position.y = 0;
00053       grasp_pose.pose.position.z = 0.23;
00054       grasp_pose.pose.orientation.x = 0.0;
00055       grasp_pose.pose.orientation.y = 1.0;
00056       grasp_pose.pose.orientation.z = 0.0;
00057       grasp_pose.pose.orientation.w = 0.0;
00058 
00059       // Pre-grasp pose -0.15m above grasp
00060       geometry_msgs::Pose pre_grasp_pose;
00061 
00062       pre_grasp_pose.position.x = 0;
00063       pre_grasp_pose.position.y = 0;
00064       pre_grasp_pose.position.z = -0.15;
00065       pre_grasp_pose.orientation.x = 0;
00066       pre_grasp_pose.orientation.y = 0;
00067       pre_grasp_pose.orientation.z = 0;
00068       pre_grasp_pose.orientation.w = 1;
00069 
00070       // Lift modifier -0.20 on Z
00071       object_manipulation_msgs::GripperTranslation lift;
00072 
00073       lift.direction.header.frame_id = "/wam_link0";
00074       lift.direction.header.stamp = now;
00075       lift.direction.vector.x = 0;
00076       lift.direction.vector.y = 0;
00077       lift.direction.vector.z = 1;
00078       lift.desired_distance = 0.20;
00079 
00080       goal.fingers_position_for_grasp.push_back("close");
00081       goal.fingers_position_for_pre_grasp.push_back("open");
00082       goal.grasp_pose                     = grasp_pose;
00083       goal.pre_grasp_modifier             = pre_grasp_pose;
00084       goal.lift                           = lift;
00085 
00086       return goal;
00087   }
00088 
00090   actionlib::SimpleClientGoalState getState()
00091   {
00092     return grasp_client_->getState();
00093   }
00094  
00095 };
00096 
00097 int main(int argc, char** argv)
00098 {
00099   // Init the ROS node
00100   ros::init(argc, argv, "robot_driver");
00101 
00102   RobotGrasp grasp;
00103   // Start the trajectory
00104   grasp.startGrasp(grasp.generateGraspExampleMsg());
00105   // Wait for trajectory completion
00106   while(!grasp.getState().isDone() && ros::ok())
00107   {
00108     usleep(50000);
00109   }
00110 }
00111 


iri_grasp_actions
Author(s): pmonso
autogenerated on Fri Dec 6 2013 20:14:56