1 /***********************************************************
   2 * Software License Agreement (BSD License)
   3 *
   4 *  Copyright (c) 2009, Willow Garage, Inc.
   5 *  All rights reserved.
   6 *
   7 *  Redistribution and use in source and binary forms, with or without
   8 *  modification, are permitted provided that the following conditions
   9 *  are met:
  10 *
  11 *   * Redistributions of source code must retain the above copyright
  12 *     notice, this list of conditions and the following disclaimer.
  13 *   * Redistributions in binary form must reproduce the above
  14 *     copyright notice, this list of conditions and the following
  15 *     disclaimer in the documentation and/or other materials provided
  16 *     with the distribution.
  17 *   * Neither the name of Willow Garage, Inc. nor the names of its
  18 *     contributors may be used to endorse or promote products derived
  19 *     from this software without specific prior written permission.
  20 *
  21 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  25 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  29 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  31 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  32 *  POSSIBILITY OF SUCH DAMAGE.
  33 *
  34 * Author: Brian Gerkey
  35 ***********************************************************/
  36 
  37 #include <ros/ros.h>
  38 #include <actionlib/client/simple_action_client.h>
  39 
  40 // Messages
  41 #include <approach_table_tools/GetCheckerboardPose.h>
  42 #include <approach_table_tools/GetApproachPose.h>
  43 #include <geometry_msgs/PoseStamped.h>
  44 #include <move_base_msgs/MoveBaseAction.h>
  45 #include <pr2_common_action_msgs/TuckArmsAction.h>
  46 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
  47 
  48 int
  49 main(int argc, char** argv)
  50 {
  51   ros::init(argc, argv, "executive");
  52   ros::NodeHandle n;
  53 
  54   // Lower the torso
  55   actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> torso_client("/torso_controller/position_joint_action", true);
  56   ROS_INFO("waiting for torso action server");
  57   torso_client.waitForServer();
  58   ROS_INFO("found action server");
  59   pr2_controllers_msgs::SingleJointPositionGoal torso_goal;
  60   torso_goal.position = 0.03;
  61   torso_goal.min_duration = ros::Duration(2.0);
  62   torso_goal.max_velocity = 1.0;
  63   ROS_INFO("lowering the torso");
  64   torso_client.sendGoal(torso_goal);
  65   torso_client.waitForResult();
  66   if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
  67     ROS_BREAK();
  68 
  69   // Tuck the arms
  70   actionlib::SimpleActionClient<pr2_common_action_msgs::TuckArmsAction> tuck_arms_client("tuck_arms", true);
  71   tuck_arms_client.waitForServer();
  72   pr2_common_action_msgs::TuckArmsGoal tuck_arms_goal;
  73   tuck_arms_goal.tuck_left = true;
  74   tuck_arms_goal.tuck_right = true;
  75   tuck_arms_client.sendGoal(tuck_arms_goal);
  76   tuck_arms_client.waitForResult();
  77   if(tuck_arms_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
  78     ROS_BREAK();
  79   
  80   // Get the checkerboard pose
  81   ros::service::waitForService("get_checkerboard_pose");
  82   approach_table_tools::GetCheckerboardPoseRequest cb_req;
  83   approach_table_tools::GetCheckerboardPoseResponse cb_resp;
  84   cb_req.corners_x = 5;
  85   cb_req.corners_y = 4;
  86   cb_req.spacing_x = 0.08;
  87   cb_req.spacing_y = 0.08;
  88   ROS_INFO("Attempting to get the pose of the board");
  89   if(!ros::service::call("get_checkerboard_pose", cb_req, cb_resp))
  90     ROS_BREAK();
  91   ROS_INFO("Got board pose: %.3f %.3f %.3f", 
  92            cb_resp.board_pose.pose.position.x,
  93            cb_resp.board_pose.pose.position.y,
  94            cb_resp.board_pose.pose.position.z);
  95 
  96   // Get the approach pose
  97   ros::service::waitForService("get_approach_pose");
  98   approach_table_tools::GetApproachPoseRequest ap_req;
  99   approach_table_tools::GetApproachPoseResponse ap_resp;
 100   ap_req.board_pose = cb_resp.board_pose;
 101   ROS_INFO("Attempting to get approach pose");
 102   if(!ros::service::call("get_approach_pose", ap_req, ap_resp))
 103     ROS_BREAK();
 104   ROS_INFO("Got approach pose: %.3f %.3f %.3f", 
 105            ap_resp.nav_pose.pose.position.x,
 106            ap_resp.nav_pose.pose.position.y,
 107            ap_resp.nav_pose.pose.position.z);
 108   // For visualiation
 109   ros::Publisher nav_pose_pub = n.advertise<geometry_msgs::PoseStamped>("nav_pose", 1, true);
 110   nav_pose_pub.publish(ap_resp.nav_pose);
 111 
 112   // Approach the table
 113   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_local_client("move_base_local", true);
 114   move_base_local_client.waitForServer();
 115   move_base_msgs::MoveBaseGoal move_base_local_goal;
 116   move_base_local_goal.target_pose = ap_resp.nav_pose;
 117   move_base_local_client.sendGoal(move_base_local_goal);
 118   move_base_local_client.waitForResult();
 119   if(move_base_local_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
 120     ROS_BREAK();
 121 
 122   // Untuck the arms
 123   tuck_arms_goal.tuck_left = false;
 124   tuck_arms_goal.tuck_right = false;
 125   tuck_arms_client.sendGoal(tuck_arms_goal);
 126   tuck_arms_client.waitForResult();
 127   if(tuck_arms_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
 128     ROS_BREAK();
 129 
 130   // Raise the torso
 131   torso_goal.position = 0.195;
 132   torso_goal.min_duration = ros::Duration(2.0);
 133   torso_goal.max_velocity = 1.0;
 134   ROS_INFO("Raising the torso");
 135   torso_client.sendGoal(torso_goal);
 136   torso_client.waitForResult();
 137   if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
 138     ROS_BREAK();
 139 
 140   // Move close to the table, using local navigation (NO OBSTACLE AVOIDANCE!)
 141   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> drive_base_client("drive_base_action", true);
 142   drive_base_client.waitForServer();
 143   move_base_msgs::MoveBaseGoal drive_base_goal;
 144   drive_base_goal.target_pose.header.frame_id = "base_link";
 145   drive_base_goal.target_pose.pose.position.x = 0.3;
 146   drive_base_goal.target_pose.pose.position.y = 0.0;
 147   drive_base_goal.target_pose.pose.position.z = 0.0;
 148   drive_base_goal.target_pose.pose.orientation.x = 0.0;
 149   drive_base_goal.target_pose.pose.orientation.y = 0.0;
 150   drive_base_goal.target_pose.pose.orientation.z = 0.0;
 151   drive_base_goal.target_pose.pose.orientation.w = 1.0;
 152   ROS_INFO("Moving close to the table");
 153   drive_base_client.sendGoal(drive_base_goal);
 154   drive_base_client.waitForResult();
 155   if(drive_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
 156     ROS_BREAK();
 157 
 158   // Move to the right-hand table, using local navigation (NO OBSTACLE AVOIDANCE!)
 159   drive_base_goal.target_pose.header.frame_id = "base_link";
 160   drive_base_goal.target_pose.pose.position.x = 0.0;
 161   drive_base_goal.target_pose.pose.position.y = -1.1;
 162   drive_base_goal.target_pose.pose.position.z = 0.0;
 163   drive_base_goal.target_pose.pose.orientation.x = 0.0;
 164   drive_base_goal.target_pose.pose.orientation.y = 0.0;
 165   drive_base_goal.target_pose.pose.orientation.z = 0.0;
 166   drive_base_goal.target_pose.pose.orientation.w = 1.0;
 167   ROS_INFO("Moving to the right table");
 168   drive_base_client.sendGoal(drive_base_goal);
 169   drive_base_client.waitForResult();
 170   if(drive_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
 171     ROS_BREAK();
 172   ROS_INFO("Finished");
 173   ros::spin();
 174   return 0;
 175 }