1 // Include roscpp, the ROS C++ client library
   2 #include <ros/ros.h>
   3 // Include the action client infrastructure, so that we can send goals 
   4 // to action servers
   5 #include <actionlib/client/simple_action_client.h>
   6 
   7 // Include messages that we'll use
   8 #include <approach_table_tools/GetCheckerboardPose.h>
   9 #include <approach_table_tools/GetApproachPose.h>
  10 #include <geometry_msgs/PoseStamped.h>
  11 #include <move_base_msgs/MoveBaseAction.h>
  12 #include <pr2_common_action_msgs/TuckArmsAction.h>
  13 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
  14 
  15 int
  16 main(int argc, char** argv)
  17 {
  18   // Initialize roscpp, naming our node
  19   ros::init(argc, argv, "nav_executive");
  20   // Create a NodeHandle, which we'll use to create publishers and subscribers
  21   ros::NodeHandle n;
  22 
  23   //////////////////////////////////////
  24   // Lower the torso, because navigation works better that way.
  25   actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> torso_client("/torso_controller/position_joint_action", true);
  26   // Wait for the action server to become available
  27   torso_client.waitForServer();
  28   // Create and fill in a goal for this action
  29   pr2_controllers_msgs::SingleJointPositionGoal torso_goal;
  30   // We send it to 0.03, because it can't actually reach 0.0
  31   torso_goal.position = 0.03;
  32   torso_goal.min_duration = ros::Duration(2.0);
  33   torso_goal.max_velocity = 1.0;
  34   ROS_INFO("lowering the torso");
  35   torso_client.sendGoal(torso_goal);
  36   torso_client.waitForResult();
  37   if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
  38     ROS_BREAK();
  39   //////////////////////////////////////
  40 
  41   //////////////////////////////////////
  42   // Tuck the arms, because navigation requires it
  43   actionlib::SimpleActionClient<pr2_common_action_msgs::TuckArmsAction> tuck_arms_client("tuck_arms", true);
  44   tuck_arms_client.waitForServer();
  45   pr2_common_action_msgs::TuckArmsGoal tuck_arms_goal;
  46   tuck_arms_goal.tuck_left = true;
  47   tuck_arms_goal.tuck_right = true;
  48   tuck_arms_client.sendGoal(tuck_arms_goal);
  49   tuck_arms_client.waitForResult();
  50   if(tuck_arms_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
  51     ROS_BREAK();
  52   //////////////////////////////////////
  53   
  54   //////////////////////////////////////
  55   // Get the checkerboard pose
  56 
  57   // TODO
  58   // Call the service provided by the checkerboard detector, 
  59   // to get the pose of the big checkerboard that is attached to the table.  
  60   // The service is defined in approach_table_tools/srv/GetCheckerboardPose.srv.
  61   // The service is called "get_checkerboard_pose".
  62   // That checkerboard is 5 X 4, with 0.08m X 0.08m squares.  You should
  63   // end up with a geometry_msgs::PoseStamped that tells you where the
  64   // checkerboard is.
  65   //////////////////////////////////////
  66 
  67   //////////////////////////////////////
  68   // Get the approach pose
  69 
  70   // TODO
  71   // Call the service provided by the approach pose finder
  72   // to get a pose that is 1m in front of the checkerboard.
  73   // The service is defined in approach_table_tools/srv/GetApproachPose.srv.
  74   // The service is called "get_approach_pose".
  75   // You should provide the pose that you got from the checkerboard
  76   // detector, and get in return the approach pose.  Once you have the
  77   // approach pose, create a ros::Publisher, publish the pose (as a
  78   // geometry_msgs::PoseStamped), and use rviz to visualize it (hint: make
  79   // your Publisher latched, to avoid the chance of rviz missing the initial
  80   // publication).
  81   //////////////////////////////////////
  82 
  83   //////////////////////////////////////
  84   // Approach the table
  85 
  86   // TODO
  87   // Send the robot to the approach pose that you got from the approach 
  88   // pose finder.  Use the navigation stack, which is an action server that
  89   // accepts a PoseStamped goal.  The action is defined in 
  90   // move_base_msgs/action/MoveBase.action.
  91   // The action is called "move_base_local".
  92   //////////////////////////////////////
  93 
  94   //////////////////////////////////////
  95   // Untuck the arms, to prepare for manipulation
  96 
  97   // TODO
  98   // Reuse the tuck_arms_client, and send a new goal with 
  99   // tuck_left and tuck_right set to false.
 100   //////////////////////////////////////
 101 
 102   //////////////////////////////////////
 103   // Raise the torso, to prepare for manipulation
 104   // We set the goal to 0.195, because that's almost all the way at the top
 105   torso_goal.position = 0.195;
 106   torso_goal.min_duration = ros::Duration(2.0);
 107   torso_goal.max_velocity = 1.0;
 108   ROS_INFO("Raising the torso");
 109   torso_client.sendGoal(torso_goal);
 110   torso_client.waitForResult();
 111   if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
 112     ROS_BREAK();
 113   //////////////////////////////////////
 114 
 115   //////////////////////////////////////
 116   // Move close to the table, using local navigation (NO OBSTACLE AVOIDANCE!)
 117   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> drive_base_client("drive_base_action", true);
 118   drive_base_client.waitForServer();
 119   move_base_msgs::MoveBaseGoal drive_base_goal;
 120   // Construct a goal for local navigation that is 0.3m straight forward
 121   drive_base_goal.target_pose.header.frame_id = "base_link";
 122   drive_base_goal.target_pose.pose.position.x = 0.3;
 123   drive_base_goal.target_pose.pose.position.y = 0.0;
 124   drive_base_goal.target_pose.pose.position.z = 0.0;
 125   drive_base_goal.target_pose.pose.orientation.x = 0.0;
 126   drive_base_goal.target_pose.pose.orientation.y = 0.0;
 127   drive_base_goal.target_pose.pose.orientation.z = 0.0;
 128   drive_base_goal.target_pose.pose.orientation.w = 1.0;
 129   ROS_INFO("Moving close to the table");
 130   drive_base_client.sendGoal(drive_base_goal);
 131   drive_base_client.waitForResult();
 132   if(drive_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
 133     ROS_BREAK();
 134   //////////////////////////////////////
 135 
 136   //////////////////////////////////////
 137   // Move to the right-hand table, using local navigation (NO OBSTACLE AVOIDANCE!)
 138   // TODO
 139   // Reuse the drive_base_client to send a new goal that is 1.1m to the 
 140   // robot's right (hint: that's negative in Y).
 141   //////////////////////////////////////
 142 
 143   ROS_INFO("Finished");
 144   return 0;
 145 }