example_grasp.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <iri_wam_common_msgs/SimpleBhandPickUpAction.h>
#include <geometry_msgs/PoseStamped.h>
#include <object_manipulation_msgs/GripperTranslation.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient< iri_wam_common_msgs::SimpleBhandPickUpAction > graspClient;

class RobotGrasp
{
private:
  // Action client for the joint trajectory action 
  // used to trigger the arm movement action
  graspClient * grasp_client_;

public:
  RobotGrasp() 
  {
    // tell the action client that we want to spin a thread by default
    grasp_client_ = new graspClient("/estirabot/skills/grasp/pickup", true);

    // wait for action server to come up
    while(!grasp_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the pick action server");
    }
  }

  ~RobotGrasp()
  {
    delete grasp_client_;
  }

  void startGrasp(iri_wam_common_msgs::SimpleBhandPickUpGoal goal)
  {
    grasp_client_->sendGoal(goal);
  }

  iri_wam_common_msgs::SimpleBhandPickUpGoal generateGraspExampleMsg()
  {
      iri_wam_common_msgs::SimpleBhandPickUpGoal goal;
      ros::Time now = ros::Time::now();

      geometry_msgs::PoseStamped grasp_pose;

      // Grasp pose is home position -0.2m in Z
      grasp_pose.header.frame_id = "/wam_link0";
      grasp_pose.header.stamp = now;
      grasp_pose.pose.position.x = 0.4;
      grasp_pose.pose.position.y = 0;
      grasp_pose.pose.position.z = 0.23;
      grasp_pose.pose.orientation.x = 0.0;
      grasp_pose.pose.orientation.y = 1.0;
      grasp_pose.pose.orientation.z = 0.0;
      grasp_pose.pose.orientation.w = 0.0;

      // Pre-grasp pose -0.15m above grasp
      geometry_msgs::Pose pre_grasp_pose;

      pre_grasp_pose.position.x = 0;
      pre_grasp_pose.position.y = 0;
      pre_grasp_pose.position.z = -0.15;
      pre_grasp_pose.orientation.x = 0;
      pre_grasp_pose.orientation.y = 0;
      pre_grasp_pose.orientation.z = 0;
      pre_grasp_pose.orientation.w = 1;

      // Lift modifier -0.20 on Z
      object_manipulation_msgs::GripperTranslation lift;

      lift.direction.header.frame_id = "/wam_link0";
      lift.direction.header.stamp = now;
      lift.direction.vector.x = 0;
      lift.direction.vector.y = 0;
      lift.direction.vector.z = 1;
      lift.desired_distance = 0.20;

      goal.fingers_position_for_grasp.push_back("close");
      goal.fingers_position_for_pre_grasp.push_back("open");
      goal.grasp_pose                     = grasp_pose;
      goal.pre_grasp_modifier             = pre_grasp_pose;
      goal.lift                           = lift;

      return goal;
  }

  actionlib::SimpleClientGoalState getState()
  {
    return grasp_client_->getState();
  }
 
};

int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "robot_driver");

  RobotGrasp grasp;
  // Start the trajectory
  grasp.startGrasp(grasp.generateGraspExampleMsg());
  // Wait for trajectory completion
  while(!grasp.getState().isDone() && ros::ok())
  {
    usleep(50000);
  }
}