goto_plug.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>

#include <move_arm_msgs/MoveArmAction.h>
#include <move_arm_msgs/utils.h>

#include <nav_msgs/Odometry.h>
#include <boost/thread.hpp>

#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
#include <tf/transform_listener.h>


// for opening gripper
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
#include <actionlib/client/simple_action_client.h>

// Our Action interface type, provided as a typedef for convenience
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> GripperClient;

class Gripper{
private:
  GripperClient* gripper_client_;  

public:
  //Action client initialization
  Gripper(){

    //Initialize the client for the Action interface to the gripper controller
    //and tell the action client that we want to spin a thread by default
    gripper_client_ = new GripperClient("r_gripper_controller/gripper_action", true);
    
    //wait for the gripper action server to come up 
    while(!gripper_client_->waitForServer(ros::Duration(30.0))){
      ROS_INFO("Waiting for the r_gripper_controller/gripper_action action server to come up");
    }
  }

  ~Gripper(){
    delete gripper_client_;
  }

  //Open the gripper
  void open(){
    pr2_controllers_msgs::Pr2GripperCommandGoal open;
    open.command.position = 0.08;
    open.command.max_effort = -1.0;  // Do not limit effort (negative)
    
    ROS_INFO("Sending open goal");
    gripper_client_->sendGoal(open);
    gripper_client_->waitForResult();
    if(gripper_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper opened!");
    else
      ROS_INFO("The gripper failed to open.");
  }

  //Close the gripper
  void close(){
    pr2_controllers_msgs::Pr2GripperCommandGoal squeeze;
    squeeze.command.position = 0.0;
    squeeze.command.max_effort = 50.0;  // Close gently
    
/*
    ROS_INFO("Sending squeeze goal, no block, just continue");
    gripper_client_->sendGoal(squeeze);
*/
    ROS_INFO("Sending squeeze goal, wait for 5 seconds, check for stall, then continue");
    if(gripper_client_->sendGoalAndWait(squeeze,ros::Duration(5.0)) == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("The gripper closed!");
    else
    {
      gripper_client_->sendGoal(squeeze);
      if (gripper_client_->getResult()->stalled)
        ROS_INFO("The gripper failed to close and is stalled, possibly holding an object.");
      else
        ROS_INFO("The gripper failed to close and is not stalled, something is wrong.");
    }
  }
};

class objectPose
{
    public:
      bool got_pose_;
      nav_msgs::Odometry pose_;
      ros::Subscriber sub_;

      objectPose(ros::NodeHandle& nh,std::string topic)
      {
        this->got_pose_ = false;
        this->sub_ =  nh.subscribe(topic, 1, &objectPose::poseCallback, this);
      };
      void poseCallback(const nav_msgs::OdometryConstPtr& pose)
      {
         this->pose_ = *pose;
         this->got_pose_ = true;
      };
};


int main(int argc, char **argv){
  ros::init (argc, argv, "move_arm_pose_goal_test");
  ros::NodeHandle nh;
  // spin some threaded spinners
  ros::MultiThreadedSpinner s(4);
  boost::thread* ros_spinner = new boost::thread(boost::bind(&ros::spin,s));

  /*****************************************************/
  /*                                                   */
  /*   open gripper                                    */
  /*                                                   */
  /*****************************************************/
  Gripper gripper;
  gripper.open();


  /*****************************************************/
  /*                                                   */
  /*   get tool frame fixed transform                  */
  /*                                                   */
  /*****************************************************/
  // offset pose by transform from r_wrist_roll_link to r_gripper_tool_frame
  // get the fixed transform from r_wrist_roll_link to r_gripper_tool_frame
  std::string target_frame = "r_gripper_tool_frame";
  std::string command_link = "r_wrist_roll_link";
  ROS_INFO("waiting for transform from %s to %s",command_link.c_str(),target_frame.c_str());
  tf::TransformListener listener;
  tf::StampedTransform target_to_command_link; // tool frame in wrist_tool_link frame
  listener.waitForTransform(target_frame,command_link,ros::Time(),ros::Duration(100.0));
  listener.lookupTransform(target_frame,command_link,ros::Time(0),target_to_command_link);


  /*****************************************************/
  /*                                                   */
  /*   connect to server                               */
  /*                                                   */
  /*****************************************************/
  actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true);
  ROS_INFO("Connecting to move arm server");
  move_arm.waitForServer();


  /*****************************************************/
  /*                                                   */
  /*   goalA: goal pose                                */
  /*                                                   */
  /*****************************************************/
  // get plug pose from sim
  objectPose pcb(nh,"/plug/plug_pose_ground_truth");

  ROS_INFO("waiting for /plug/plug_pose_ground_truth from sim");
  while (!pcb.got_pose_)
  {
    ROS_DEBUG("checking for plug pose (%d)",  pcb.got_pose_ );
    ros::Duration(1.0).sleep();
  }

  // get reference frame of plug_pose
  std::string plug_reference_frame = pcb.pose_.header.frame_id;
  // rotate target goal by 90 degrees yaw
  btQuaternion target_rot_90yaw; target_rot_90yaw.setEuler(0,0,M_PI/2);
  // construct target transform from plug_reference_frame to plug
  // and transform target pose rotation
  btTransform plug_transform(btQuaternion(pcb.pose_.pose.pose.orientation.x,
                                          pcb.pose_.pose.pose.orientation.y,
                                          pcb.pose_.pose.pose.orientation.z,
                                          pcb.pose_.pose.pose.orientation.w)*target_rot_90yaw,
                               btVector3(pcb.pose_.pose.pose.position.x,
                                         pcb.pose_.pose.pose.position.y,
                                         pcb.pose_.pose.pose.position.z));
  plug_transform =  plug_transform*(btTransform)target_to_command_link; 

  move_arm_msgs::MoveArmGoal goalA;

  goalA.group_name = "right_arm";
  goalA.num_planning_attempts = 1;
  goalA.planner_id = std::string("");
  goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalA.allowed_planning_time = 5.0;
  
  motion_planning_msgs::SimplePoseConstraint desired_poseA;

  desired_poseA.header = pcb.pose_.header;
  desired_poseA.link_name = command_link;
  //desired_poseA.pose = pcb.pose_.pose.pose;
  desired_poseA.pose.position.x = plug_transform.getOrigin().x();
  desired_poseA.pose.position.y = plug_transform.getOrigin().y() - 0.05;
  desired_poseA.pose.position.z = plug_transform.getOrigin().z();
  desired_poseA.pose.orientation.x = plug_transform.getRotation().x();
  desired_poseA.pose.orientation.y = plug_transform.getRotation().y();
  desired_poseA.pose.orientation.z = plug_transform.getRotation().z();
  desired_poseA.pose.orientation.w = plug_transform.getRotation().w();

  // additional parameters
  desired_poseA.absolute_position_tolerance.x = 0.02;
  desired_poseA.absolute_position_tolerance.y = 0.02;
  desired_poseA.absolute_position_tolerance.z = 0.02;

  desired_poseA.absolute_roll_tolerance = 0.04;
  desired_poseA.absolute_pitch_tolerance = 0.04;
  desired_poseA.absolute_yaw_tolerance = 0.04;
  
  ROS_INFO("sending approach pose goal to server");
  move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_poseA,goalA);

  // add another goal

  if (nh.ok())
  {
    bool finished_within_time = false;
    int retries = 0;
    int max_retries = 10;
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));

    while (!finished_within_time && retries <= max_retries)
    {
      retries++;
      move_arm.sendGoal(goalA);
      finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    }

    if (!finished_within_time)
    {
      ROS_INFO("Timed out achieving goal A");
      move_arm.cancelGoal();
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }

  /*****************************************************/
  /*                                                   */
  /*   goalB: goal pose                                */
  /*                                                   */
  /*****************************************************/
  move_arm_msgs::MoveArmGoal goalB;

  goalB.group_name = "right_arm";
  goalB.num_planning_attempts = 1;
  goalB.planner_id = std::string("");
  goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalB.allowed_planning_time = 5.0;
  
  motion_planning_msgs::SimplePoseConstraint desired_poseB;

  desired_poseB.header = pcb.pose_.header;
  desired_poseB.link_name = command_link;
  //desired_poseB.pose = pcb.pose_.pose.pose;
  desired_poseB.pose.position.x = plug_transform.getOrigin().x();
  desired_poseB.pose.position.y = plug_transform.getOrigin().y();
  desired_poseB.pose.position.z = plug_transform.getOrigin().z();
  desired_poseB.pose.orientation.x = plug_transform.getRotation().x();
  desired_poseB.pose.orientation.y = plug_transform.getRotation().y();
  desired_poseB.pose.orientation.z = plug_transform.getRotation().z();
  desired_poseB.pose.orientation.w = plug_transform.getRotation().w();

  // additional parameters
  desired_poseB.absolute_position_tolerance.x = 0.02;
  desired_poseB.absolute_position_tolerance.y = 0.02;
  desired_poseB.absolute_position_tolerance.z = 0.02;

  desired_poseB.absolute_roll_tolerance = 0.04;
  desired_poseB.absolute_pitch_tolerance = 0.04;
  desired_poseB.absolute_yaw_tolerance = 0.04;
  
  ROS_INFO("sending plug pose goal to server");
  move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_poseB,goalB);

  // add another goal

  if (nh.ok())
  {
    bool finished_within_time = false;
    int retries = 0;
    int max_retries = 10;
    move_arm.sendGoal(goalB);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    while (!finished_within_time && retries <= max_retries)
    {
      retries++;
      move_arm.sendGoal(goalB);
      finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    }

    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal B");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }

  /*****************************************************/
  /*                                                   */
  /*   close gripper                                   */
  /*                                                   */
  /*****************************************************/
  ROS_INFO("closing gripper around object");
  gripper.close();

  /*****************************************************/
  /*                                                   */
  /*   goalC: goal pose                                */
  /*                                                   */
  /*****************************************************/
  // getting outlet pose from sim
  objectPose ocb(nh,"/outlet/outlet_pose_ground_truth");
  ROS_INFO("waiting for /outlet/outlet_pose_ground_truth from sim");
  while (!ocb.got_pose_)
  {
    ROS_DEBUG("checking for outlet pose (%d)",  ocb.got_pose_ );
    ros::Duration(1.0).sleep();
  }
  // get reference frame of outlet_pose
  std::string outlet_reference_frame = ocb.pose_.header.frame_id;

  // set goal message
  btTransform outlet_transform(btQuaternion(ocb.pose_.pose.pose.orientation.x,
                                            ocb.pose_.pose.pose.orientation.y,
                                            ocb.pose_.pose.pose.orientation.z,
                                            ocb.pose_.pose.pose.orientation.w),
                               btVector3(ocb.pose_.pose.pose.position.x,
                                         ocb.pose_.pose.pose.position.y,
                                         ocb.pose_.pose.pose.position.z));
  outlet_transform =  outlet_transform*(btTransform)target_to_command_link; 

  move_arm_msgs::MoveArmGoal goalC;

  goalC.group_name = "right_arm";
  goalC.num_planning_attempts = 100;
  goalC.planner_id = std::string("");
  goalC.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalC.allowed_planning_time = 5.0;
  
  motion_planning_msgs::SimplePoseConstraint desired_poseC;

  desired_poseC.header = ocb.pose_.header;
  desired_poseC.link_name = command_link;
  desired_poseC.pose.position.x = outlet_transform.getOrigin().x()-0.10;
  desired_poseC.pose.position.y = outlet_transform.getOrigin().y();
  desired_poseC.pose.position.z = outlet_transform.getOrigin().z();
  desired_poseC.pose.orientation.x = outlet_transform.getRotation().x();
  desired_poseC.pose.orientation.y = outlet_transform.getRotation().y();
  desired_poseC.pose.orientation.z = outlet_transform.getRotation().z();
  desired_poseC.pose.orientation.w = outlet_transform.getRotation().w();

/*
  desired_poseC.header.frame_id = "torso_lift_link";
  desired_poseC.pose.position.x = .75;
  desired_poseC.pose.position.y = -.188;
  desired_poseC.pose.position.z = 0.0;
  desired_poseC.pose.orientation.x = 0;
  desired_poseC.pose.orientation.y = 0;
  desired_poseC.pose.orientation.z = 0;
  desired_poseC.pose.orientation.w = 1.0;
*/

  // additional parameters
  desired_poseC.absolute_position_tolerance.x = 0.02;
  desired_poseC.absolute_position_tolerance.y = 0.02;
  desired_poseC.absolute_position_tolerance.z = 0.02;

  desired_poseC.absolute_roll_tolerance = 0.04;
  desired_poseC.absolute_pitch_tolerance = 0.04;
  desired_poseC.absolute_yaw_tolerance = 0.04;
  
  ROS_INFO("sending outlet pose goal to server");
  move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_poseC,goalC);

  // add another goal

  if (nh.ok())
  {
    bool finished_within_time = false;
    int retries = 0;
    int max_retries = 10;
    move_arm.sendGoal(goalC);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    while (!finished_within_time && retries <= max_retries)
    {
      retries++;
      move_arm.sendGoal(goalC);
      finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    }
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal C");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }




  ros::shutdown();
  ros_spinner->join();
}