pr2_skill_crane: skill_crane_frontend.cpp Source File

00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
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 the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "pr2_skill_crane/skill_crane_frontend.h"
00031 
00032 #include <rviz/visualization_manager.h>
00033 
00034 using pr2_interactive_manipulation::IMGUIAction;
00035 using pr2_interactive_manipulation::IMGUIGoal;
00036 using pr2_interactive_manipulation::IMGUIOptions;
00037 using pr2_interactive_manipulation::IMGUIAdvancedOptions;
00038 
00039 using object_manipulation_msgs::ManipulationResult;
00040 
00041 namespace pr2_skill_crane {
00042 
00043 typedef actionlib::SimpleActionClient<IMGUIAction> Client;
00044 
00045 SkillCraneFrontend::SkillCraneFrontend(wxWindow *parent, rviz::VisualizationManager* manager) : 
00046   SkillCraneFrameBase(parent),
00047   status_label_text_("idle"),
00048   active_goal_(false)
00049 {
00050   action_name_ = "imgui_action";  
00051   action_client_ = new actionlib::SimpleActionClient<IMGUIAction>(action_name_, true);
00052 }
00053 
00054 SkillCraneFrontend::~SkillCraneFrontend()
00055 {
00056   delete action_client_;
00057 }
00058 
00059 wxString getWxString(std::string str)
00060 {
00061   wxString mystring(str.c_str(), wxConvUTF8);
00062   return mystring;
00063 }
00064 
00065 void SkillCraneFrontend::update()
00066 {
00067   status_label_mutex_.lock();
00068   wxString mystring(status_label_text_.c_str(), wxConvUTF8);  
00069   status_label_mutex_.unlock();
00070   status_label_->SetLabel(mystring);
00071 
00072   if (active_goal_)
00073   {
00074     actionlib::SimpleClientGoalState state = action_client_->getState();
00075     if (state != actionlib::SimpleClientGoalState::PENDING && 
00076         state != actionlib::SimpleClientGoalState::ACTIVE)
00077     {
00078       active_goal_ = false;
00079     }
00080   }
00081   
00082   //check if we are executing the prepare routine
00083   if (!mutex_.try_lock()) active_goal_ = true;
00084   else mutex_.unlock();
00085 
00086   prepare_button_->Enable(!active_goal_);
00087   grasp_button_->Enable(!active_goal_);
00088   place_button_->Enable(!active_goal_);
00089   handoff_button_->Enable(!active_goal_);
00090 }
00091 
00092 void SkillCraneFrontend::setStatusLabel(std::string label)
00093 {
00094   boost::mutex::scoped_lock lock(status_label_mutex_);
00095   status_label_text_ = label;
00096 }
00097 
00098 void SkillCraneFrontend::feedbackCallback(const pr2_interactive_manipulation::IMGUIFeedbackConstPtr &feedback)
00099 {
00100   ROS_DEBUG_STREAM("SSC Frontend received feedback" << feedback->status);
00101   setStatusLabel(feedback->status);
00102 }
00103 
00104 //arm_action_choice is 0 for side, 2 for handoff
00105 bool SkillCraneFrontend::moveArmToSide(int arm_selection, int arm_action_choice)
00106 {
00107   IMGUIGoal goal;
00108   goal.command.command = goal.command.MOVE_ARM;
00109   goal.options.arm_selection = arm_selection;
00110   goal.options.arm_action_choice = arm_action_choice;
00111 
00112   //try with planner first
00113   goal.options.arm_planner_choice = 0;
00114   goal.options.collision_checked = true;
00115   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00116                            boost::bind(&SkillCraneFrontend::feedbackCallback, this, _1));
00117   while ( !action_client_->waitForResult(ros::Duration(1.0)) ) {}
00118   if (action_client_->getResult()->result.value == ManipulationResult::SUCCESS) return true;
00119 
00120   //if we failed, try open loop
00121   goal.options.arm_planner_choice = 1;
00122   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00123                            boost::bind(&SkillCraneFrontend::feedbackCallback, this, _1));
00124   while ( !action_client_->waitForResult(ros::Duration(1.0)) ) {}
00125   if (action_client_->getResult()->result.value == ManipulationResult::SUCCESS) return true;
00126 
00127   return false;
00128 }
00129 
00130 void SkillCraneFrontend::prepare()
00131 {
00132   boost::mutex::scoped_lock lock(mutex_);
00133   if (!moveArmToSide(0, 0))
00134   {
00135     setStatusLabel("an error has occured");
00136     return;
00137   }
00138   if (!moveArmToSide(1, 0))
00139   {
00140     setStatusLabel("an error has occured");
00141     return;
00142   }
00143   IMGUIGoal goal;
00144   goal.command.command = goal.command.TAKE_MAP;
00145   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00146                            boost::bind(&SkillCraneFrontend::feedbackCallback, this, _1));
00147   while ( !action_client_->waitForResult(ros::Duration(1.0)) ) {}
00148   if (action_client_->getResult()->result.value != ManipulationResult::SUCCESS) 
00149   {
00150     setStatusLabel("An error has occured");
00151     return;
00152   }
00153 
00154   goal.command.command = goal.command.LOOK_AT_TABLE;
00155   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00156                            boost::bind(&SkillCraneFrontend::feedbackCallback, this, _1));
00157   while ( !action_client_->waitForResult(ros::Duration(1.0)) ) {}
00158   if (action_client_->getResult()->result.value == ManipulationResult::SUCCESS) setStatusLabel("ready");
00159   else setStatusLabel("An error has occured");
00160 }
00161 
00162 void SkillCraneFrontend::prepareButtonClicked( wxCommandEvent& )
00163 {
00164   if (!mutex_.try_lock()) return;
00165   executing_thread_ = new boost::thread(boost::bind(&SkillCraneFrontend::prepare, this));
00166   mutex_.unlock();
00167 }
00168 
00169 void SkillCraneFrontend::graspButtonClicked( wxCommandEvent& )
00170 {
00171   IMGUIGoal goal;
00172   goal.options.collision_checked = true;
00173   goal.options.arm_selection = arm_choice_->GetSelection();
00174   goal.options.grasp_selection = 0;
00175 
00176   IMGUIAdvancedOptions go;
00177   go.reactive_grasping = false;
00178   go.reactive_force = false;
00179   go.reactive_place = true;
00180   go.lift_steps  = 10;
00181   go.retreat_steps = 10;
00182   go.lift_vertically = 0;
00183   goal.options.adv_options = go;
00184 
00185   goal.command.command = goal.command.PICKUP;
00186   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00187                            boost::bind(&SkillCraneFrontend::feedbackCallback, this, _1));
00188   active_goal_ = true;
00189 }
00190 
00191 void SkillCraneFrontend::placeButtonClicked( wxCommandEvent& )
00192 {
00193   IMGUIGoal goal;
00194   goal.options.collision_checked = true;
00195   goal.options.arm_selection = arm_choice_->GetSelection();
00196   goal.options.grasp_selection = 0;
00197 
00198   IMGUIAdvancedOptions go;
00199   go.reactive_grasping = false;
00200   go.reactive_force = false;
00201   go.reactive_place = true;
00202   go.lift_steps  = 10;
00203   go.retreat_steps = 10;
00204   go.lift_vertically = 0;
00205   goal.options.adv_options = go;
00206 
00207   goal.command.command = goal.command.PLACE;
00208   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00209                            boost::bind(&SkillCraneFrontend::feedbackCallback, this, _1));
00210   active_goal_ = true;
00211 }
00212 
00213 void SkillCraneFrontend::handoff()
00214 {
00215   boost::mutex::scoped_lock lock(mutex_);
00216   if (!moveArmToSide(1, 2))
00217   {
00218     setStatusLabel("an error has occured");
00219     return;
00220   }
00221 
00222   IMGUIGoal goal;
00223   goal.command.command = goal.command.MOVE_GRIPPER;
00224   goal.options.gripper_slider_position = 100;
00225   goal.options.arm_selection = 1;
00226   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00227                            boost::bind(&SkillCraneFrontend::feedbackCallback, this, _1));
00228   while ( !action_client_->waitForResult(ros::Duration(1.0)) ) setStatusLabel("opening gripper");
00229   if (action_client_->getResult()->result.value != ManipulationResult::SUCCESS) 
00230   {
00231     setStatusLabel("an error has occured");
00232     return;
00233   }
00234   setStatusLabel("you're welcome!");
00235 }
00236 
00237 void SkillCraneFrontend::handoffButtonClicked( wxCommandEvent& )
00238 {
00239   if (!mutex_.try_lock()) return;
00240   executing_thread_ = new boost::thread(boost::bind(&SkillCraneFrontend::handoff, this));
00241   mutex_.unlock();
00242 }
00243 
00244 }