turtlebot_block_manipulation: turtlebot_block_manipulation: pick_and_place_action

00001 /* 
00002  * Copyright (c) 2011, Vanadium Labs LLC
00003  * All Rights Reserved
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Vanadium Labs LLC nor the names of its 
00014  *       contributors may be used to endorse or promote products derived 
00015  *       from this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020  * DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00021  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027  * 
00028  * Author: Michael Ferguson, Helen Oleynikova
00029  */
00030 
00031 #include <ros/ros.h>
00032 #include <tf/tf.h>
00033 
00034 #include <actionlib/server/simple_action_server.h>
00035 #include <turtlebot_block_manipulation/PickAndPlaceAction.h>
00036 
00037 #include <actionlib/client/simple_action_client.h>
00038 #include <simple_arm_server/MoveArmAction.h>
00039 
00040 #include <geometry_msgs/PoseArray.h>
00041 
00042 namespace turtlebot_block_manipulation
00043 {
00044 
00045 class PickAndPlaceServer
00046 {
00047 private:
00048     
00049   ros::NodeHandle nh_;
00050   actionlib::SimpleActionServer<turtlebot_block_manipulation::PickAndPlaceAction> as_;
00051   std::string action_name_;
00052   
00053   turtlebot_block_manipulation::PickAndPlaceFeedback     feedback_;
00054   turtlebot_block_manipulation::PickAndPlaceResult       result_;
00055   turtlebot_block_manipulation::PickAndPlaceGoalConstPtr goal_;
00056   
00057   actionlib::SimpleActionClient<simple_arm_server::MoveArmAction> client_;
00058   
00059   ros::Subscriber pick_and_place_sub_;
00060   
00061   // Parameters from goal
00062   std::string arm_link;
00063   double gripper_open;
00064   double gripper_closed;
00065   double z_up;
00066 public:
00067   PickAndPlaceServer(const std::string name) : 
00068     nh_("~"), as_(name, false), action_name_(name), client_("/move_arm", true)
00069   {
00070   
00071     //register the goal and feeback callbacks
00072     as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
00073     as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
00074     
00075     as_.start();
00076   }
00077 
00078   void goalCB()
00079   {
00080     ROS_INFO("[pick and place] Received goal!");
00081     goal_ = as_.acceptNewGoal();
00082     arm_link = goal_->frame;
00083     gripper_open = goal_->gripper_open;
00084     gripper_closed = goal_->gripper_closed;
00085     z_up = goal_->z_up;
00086     
00087     if (goal_->topic.length() < 1)
00088       pickAndPlace(goal_->pickup_pose, goal_->place_pose);
00089     else
00090       pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
00091   }
00092 
00093   void sendGoalFromTopic(const geometry_msgs::PoseArrayConstPtr& msg)
00094   {
00095     ROS_INFO("[pick and place] Got goal from topic! %s", goal_->topic.c_str());
00096     pickAndPlace(msg->poses[0], msg->poses[1]);
00097     pick_and_place_sub_.shutdown();
00098   }
00099 
00100   void preemptCB()
00101   {
00102     ROS_INFO("%s: Preempted", action_name_.c_str());
00103     // set the action state to preempted
00104     as_.setPreempted();
00105   }
00106   
00107   void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
00108   {
00109     ROS_INFO("[pick and place] Picking. Also placing.");
00110   
00111     simple_arm_server::MoveArmGoal goal;
00112     simple_arm_server::ArmAction action;
00113     simple_arm_server::ArmAction grip;
00114     
00115     /* open gripper */
00116     grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
00117     grip.move_time.sec = 1.0;
00118     grip.command = gripper_open;
00119     goal.motions.push_back(grip);
00120     
00121     /* arm straight up */
00122     btQuaternion temp;
00123     temp.setRPY(0,1.57,0);
00124     action.goal.orientation.x = temp.getX();
00125     action.goal.orientation.y = temp.getY();
00126     action.goal.orientation.z = temp.getZ();
00127     action.goal.orientation.w = temp.getW();
00128 
00129     /* hover over */
00130     action.goal.position.x = start_pose.position.x;
00131     action.goal.position.y = start_pose.position.y;
00132     action.goal.position.z = z_up;
00133     action.move_time.sec = 0.25;
00134     goal.motions.push_back(action);
00135 
00136     /* go down */
00137     action.goal.position.z = start_pose.position.z;
00138     action.move_time.sec = 1.5;
00139     goal.motions.push_back(action);
00140 
00141     /* close gripper */
00142     grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
00143     grip.command = gripper_closed;
00144     grip.move_time.sec = 1.0;
00145     goal.motions.push_back(grip);
00146 
00147     /* go up */
00148     action.goal.position.z = z_up;
00149     action.move_time.sec = 1.0;
00150     goal.motions.push_back(action);
00151 
00152     /* hover over */
00153     action.goal.position.x = end_pose.position.x;
00154     action.goal.position.y = end_pose.position.y;
00155     action.goal.position.z = z_up;
00156     action.move_time.sec = 1.0;
00157     goal.motions.push_back(action);
00158 
00159     /* go down */
00160     action.goal.position.z = end_pose.position.z;
00161     action.move_time.sec = 1.5;
00162     goal.motions.push_back(action);
00163 
00164     /* open gripper */
00165     grip.command = gripper_open;
00166     goal.motions.push_back(grip);
00167 
00168     /* go up */
00169     action.goal.position.z = z_up;
00170     action.move_time.sec = 1.0;
00171     goal.motions.push_back(action);
00172   
00173     goal.header.frame_id = arm_link;
00174     client_.sendGoal(goal);
00175     ROS_INFO("[pick and place] Sent goal. Waiting.");
00176     client_.waitForResult(ros::Duration(30.0));
00177     ROS_INFO("[pick and place] Received result.");
00178     if (client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00179       as_.setSucceeded(result_);
00180     else
00181     {
00182       ROS_INFO("Actual state: %s", client_.getState().toString().c_str());
00183       as_.setAborted(result_);
00184     }
00185   }
00186 };
00187 
00188 };
00189 
00190 
00191 int main(int argc, char** argv)
00192 {
00193   ros::init(argc, argv, "pick_and_place_action_server");
00194 
00195   turtlebot_block_manipulation::PickAndPlaceServer server("pick_and_place");
00196   ros::spin();
00197 
00198   return 0;
00199 }
00200
pick_and_place_action_server.cpp
