fall_school_executive: fall_school_executive: executive.cpp Source File

00001 /***********************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage, Inc. nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *
00034 * Author: Brian Gerkey
00035 ***********************************************************/
00036 
00037 #include <ros/ros.h>
00038 #include <actionlib/client/simple_action_client.h>
00039 
00040 // Messages
00041 #include <approach_table_tools/GetCheckerboardPose.h>
00042 #include <approach_table_tools/GetApproachPose.h>
00043 #include <geometry_msgs/PoseStamped.h>
00044 #include <move_base_msgs/MoveBaseAction.h>
00045 #include <pr2_common_action_msgs/TuckArmsAction.h>
00046 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
00047 
00048 int
00049 main(int argc, char** argv)
00050 {
00051   ros::init(argc, argv, "executive");
00052   ros::NodeHandle n;
00053 
00054   // Lower the torso
00055   actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> torso_client("/torso_controller/position_joint_action", true);
00056   ROS_INFO("waiting for torso action server");
00057   torso_client.waitForServer();
00058   ROS_INFO("found action server");
00059   pr2_controllers_msgs::SingleJointPositionGoal torso_goal;
00060   torso_goal.position = 0.03;
00061   torso_goal.min_duration = ros::Duration(2.0);
00062   torso_goal.max_velocity = 1.0;
00063   ROS_INFO("lowering the torso");
00064   torso_client.sendGoal(torso_goal);
00065   torso_client.waitForResult();
00066   if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00067     ROS_BREAK();
00068 
00069   // Tuck the arms
00070   actionlib::SimpleActionClient<pr2_common_action_msgs::TuckArmsAction> tuck_arms_client("tuck_arms", true);
00071   tuck_arms_client.waitForServer();
00072   pr2_common_action_msgs::TuckArmsGoal tuck_arms_goal;
00073   tuck_arms_goal.tuck_left = true;
00074   tuck_arms_goal.tuck_right = true;
00075   tuck_arms_client.sendGoal(tuck_arms_goal);
00076   tuck_arms_client.waitForResult();
00077   if(tuck_arms_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00078     ROS_BREAK();
00079   
00080   // Get the checkerboard pose
00081   ros::service::waitForService("get_checkerboard_pose");
00082   approach_table_tools::GetCheckerboardPoseRequest cb_req;
00083   approach_table_tools::GetCheckerboardPoseResponse cb_resp;
00084   cb_req.corners_x = 5;
00085   cb_req.corners_y = 4;
00086   cb_req.spacing_x = 0.08;
00087   cb_req.spacing_y = 0.08;
00088   ROS_INFO("Attempting to get the pose of the board");
00089   if(!ros::service::call("get_checkerboard_pose", cb_req, cb_resp))
00090     ROS_BREAK();
00091   ROS_INFO("Got board pose: %.3f %.3f %.3f", 
00092            cb_resp.board_pose.pose.position.x,
00093            cb_resp.board_pose.pose.position.y,
00094            cb_resp.board_pose.pose.position.z);
00095 
00096   // Get the approach pose
00097   ros::service::waitForService("get_approach_pose");
00098   approach_table_tools::GetApproachPoseRequest ap_req;
00099   approach_table_tools::GetApproachPoseResponse ap_resp;
00100   ap_req.board_pose = cb_resp.board_pose;
00101   ROS_INFO("Attempting to get approach pose");
00102   if(!ros::service::call("get_approach_pose", ap_req, ap_resp))
00103     ROS_BREAK();
00104   ROS_INFO("Got approach pose: %.3f %.3f %.3f", 
00105            ap_resp.nav_pose.pose.position.x,
00106            ap_resp.nav_pose.pose.position.y,
00107            ap_resp.nav_pose.pose.position.z);
00108   // For visualiation
00109   ros::Publisher nav_pose_pub = n.advertise<geometry_msgs::PoseStamped>("nav_pose", 1, true);
00110   nav_pose_pub.publish(ap_resp.nav_pose);
00111 
00112   // Approach the table
00113   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_local_client("move_base_local", true);
00114   move_base_local_client.waitForServer();
00115   move_base_msgs::MoveBaseGoal move_base_local_goal;
00116   move_base_local_goal.target_pose = ap_resp.nav_pose;
00117   move_base_local_client.sendGoal(move_base_local_goal);
00118   move_base_local_client.waitForResult();
00119   if(move_base_local_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00120     ROS_BREAK();
00121 
00122   // Untuck the arms
00123   tuck_arms_goal.tuck_left = false;
00124   tuck_arms_goal.tuck_right = false;
00125   tuck_arms_client.sendGoal(tuck_arms_goal);
00126   tuck_arms_client.waitForResult();
00127   if(tuck_arms_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00128     ROS_BREAK();
00129 
00130   // Raise the torso
00131   torso_goal.position = 0.195;
00132   torso_goal.min_duration = ros::Duration(2.0);
00133   torso_goal.max_velocity = 1.0;
00134   ROS_INFO("Raising the torso");
00135   torso_client.sendGoal(torso_goal);
00136   torso_client.waitForResult();
00137   if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00138     ROS_BREAK();
00139 
00140   // Move close to the table, using local navigation (NO OBSTACLE AVOIDANCE!)
00141   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> drive_base_client("drive_base_action", true);
00142   drive_base_client.waitForServer();
00143   move_base_msgs::MoveBaseGoal drive_base_goal;
00144   drive_base_goal.target_pose.header.frame_id = "base_link";
00145   drive_base_goal.target_pose.pose.position.x = 0.3;
00146   drive_base_goal.target_pose.pose.position.y = 0.0;
00147   drive_base_goal.target_pose.pose.position.z = 0.0;
00148   drive_base_goal.target_pose.pose.orientation.x = 0.0;
00149   drive_base_goal.target_pose.pose.orientation.y = 0.0;
00150   drive_base_goal.target_pose.pose.orientation.z = 0.0;
00151   drive_base_goal.target_pose.pose.orientation.w = 1.0;
00152   ROS_INFO("Moving close to the table");
00153   drive_base_client.sendGoal(drive_base_goal);
00154   drive_base_client.waitForResult();
00155   if(drive_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00156     ROS_BREAK();
00157 
00158   // Move to the right-hand table, using local navigation (NO OBSTACLE AVOIDANCE!)
00159   drive_base_goal.target_pose.header.frame_id = "base_link";
00160   drive_base_goal.target_pose.pose.position.x = 0.0;
00161   drive_base_goal.target_pose.pose.position.y = -1.1;
00162   drive_base_goal.target_pose.pose.position.z = 0.0;
00163   drive_base_goal.target_pose.pose.orientation.x = 0.0;
00164   drive_base_goal.target_pose.pose.orientation.y = 0.0;
00165   drive_base_goal.target_pose.pose.orientation.z = 0.0;
00166   drive_base_goal.target_pose.pose.orientation.w = 1.0;
00167   ROS_INFO("Moving to the right table");
00168   drive_base_client.sendGoal(drive_base_goal);
00169   drive_base_client.waitForResult();
00170   if(drive_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00171     ROS_BREAK();
00172   ROS_INFO("Finished");
00173   ros::spin();
00174   return 0;
00175 }