pr2_interactive_manipulation: interactive_manipulation

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_interactive_manipulation/interactive_manipulation_frontend.h"
00031 
00032 #include <rviz/visualization_manager.h>
00033 
00034 namespace pr2_interactive_manipulation {
00035 
00036 typedef actionlib::SimpleActionClient<IMGUIAction> Client;
00037 
00038 InteractiveManipulationFrontend::InteractiveManipulationFrontend(wxWindow *parent, 
00039                                                                  rviz::VisualizationManager* manager) : 
00040   InteractiveManipulationFrameBase(parent),
00041   new_object_list_(false),
00042   status_label_text_("idle"),
00043   adv_options_(AdvancedOptionsDialog::getDefaultsMsg()),
00044   active_goal_(false)
00045 {
00046   action_name_ = "imgui_action";  
00047   action_client_ = new actionlib::SimpleActionClient<IMGUIAction>(action_name_, true);
00048 
00049   std::string graspable_objects_topic_name = "interactive_object_recognition_result";
00050   graspable_objects_sub_ = root_nh_.subscribe(graspable_objects_topic_name, 10, 
00051                                               &InteractiveManipulationFrontend::graspableObjectsCallback, this);
00052 
00053   object_selection_frame_ = new ObjectSelectionFrame( manager, this );
00054 }
00055 
00056 InteractiveManipulationFrontend::~InteractiveManipulationFrontend()
00057 {
00058   delete action_client_;
00059   delete object_selection_frame_;
00060 }
00061 
00062 wxString getWxString(std::string str)
00063 {
00064   wxString mystring(str.c_str(), wxConvUTF8);
00065   return mystring;
00066 }
00067 
00068 void InteractiveManipulationFrontend::update()
00069 {
00070   status_label_mutex_.lock();
00071   wxString mystring(status_label_text_.c_str(), wxConvUTF8);  
00072   status_label_mutex_.unlock();
00073   status_label_->SetLabel(mystring);
00074 
00075   if (active_goal_)
00076   {
00077     actionlib::SimpleClientGoalState state = action_client_->getState();
00078     if (state != actionlib::SimpleClientGoalState::PENDING && 
00079         state != actionlib::SimpleClientGoalState::ACTIVE)
00080     {
00081       active_goal_ = false;
00082     }
00083   }
00084   grasp_button_->Enable(!active_goal_);
00085   place_button_->Enable(!active_goal_);
00086   cancel_button_->Enable( active_goal_);
00087   arm_go_button_->Enable(!active_goal_);
00088   look_button_->Enable(!active_goal_);  
00089   model_object_button_->Enable(!active_goal_);  
00090   reset_button_->Enable(!active_goal_ && collision_box_->IsChecked());
00091   new_map_button_->Enable(!active_goal_ && collision_box_->IsChecked());
00092   reset_choice_->Enable(collision_box_->IsChecked());
00093 
00094   boost::mutex::scoped_lock lock(received_objects_mutex_);
00095 
00096   if (new_object_list_)
00097   {
00098     grasp_object_choice_->Clear();
00099     grasp_object_choice_->Insert(getWxString("Gripper Click"),0);
00100     if (!received_objects_->graspable_objects.empty())
00101     {
00102       grasp_object_choice_->Insert(getWxString("Object Selection"),1);
00103     }
00104     grasp_object_choice_->SetSelection(0);
00105     new_object_list_ = false;
00106   }
00107 
00108   object_selection_frame_->update();
00109   switch ( object_selection_frame_->getState() )
00110   {
00111   case ObjectSelectionFrame::ACCEPTED:
00112     {
00113       object_selection_frame_->setState( ObjectSelectionFrame::IDLE );
00114       if (active_goal_)
00115       {
00116         status_label_text_ = "action already in progress";
00117         break;
00118       }
00119       unsigned int selected_object = object_selection_frame_->getSelectedObject();
00120       ROS_INFO( "Object selection was finished. Result: %d", selected_object );
00121       if ( selected_object < received_objects_->graspable_objects.size() )
00122       {
00123         IMGUIGoal goal;
00124         goal.options = getDialogOptions();
00125         goal.options.grasp_selection = 1;
00126         goal.options.selected_object = received_objects_->graspable_objects.at(selected_object);
00127         goal.command.command = goal.command.PICKUP;
00128         ROS_INFO("Frontend: grasp opbject with %d points",(int)goal.options.selected_object.cluster.points.size());
00129         action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00130                                  boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00131         active_goal_ = true;
00132       }
00133       else status_label_text_ = "invalid object selection";
00134       break;
00135     }
00136   case ObjectSelectionFrame::CANCELED:
00137     status_label_text_ = "object selection was canceled";
00138     object_selection_frame_->setState( ObjectSelectionFrame::IDLE );
00139     break;
00140   case ObjectSelectionFrame::IDLE:
00141   case ObjectSelectionFrame::ACTIVE:
00142     break;
00143   } 
00144 }
00145 
00146 void InteractiveManipulationFrontend::feedbackCallback(const IMGUIFeedbackConstPtr &feedback)
00147 {
00148   ROS_DEBUG_STREAM("IM Frontend received feedback" << feedback->status);
00149   boost::mutex::scoped_lock lock(status_label_mutex_);
00150   status_label_text_ = feedback->status;
00151 }
00152 
00153 void InteractiveManipulationFrontend::graspableObjectsCallback(
00154                                            const pr2_interactive_object_detection::GraspableObjectListConstPtr &objects)
00155 {
00156   boost::mutex::scoped_lock lock(received_objects_mutex_);
00157   received_objects_ = objects;
00158   new_object_list_ = true;
00159 }
00160 
00161 void InteractiveManipulationFrontend::cancelButtonClicked( wxCommandEvent& /*event*/ )
00162 {
00163   action_client_->cancelGoal();
00164 }
00165 
00166 void InteractiveManipulationFrontend::graspButtonClicked( wxCommandEvent& /*event*/ )
00167 {
00168   IMGUIGoal goal;
00169   goal.options = getDialogOptions();
00170   if (grasp_object_choice_->GetSelection() == 0)
00171   {
00172     goal.command.command = goal.command.PICKUP;
00173     action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00174                              boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00175     active_goal_ = true;
00176   }
00177   else
00178   {
00179     boost::mutex::scoped_lock lock(received_objects_mutex_);
00180     if (received_objects_->graspable_objects.empty())
00181     {
00182       status_label_text_ = "no perception results received";
00183     }
00184     object_selection_frame_->setData( received_objects_ );
00185   }
00186 }
00187 
00188 void InteractiveManipulationFrontend::placeButtonClicked( wxCommandEvent& /*event*/ )
00189 {
00190   IMGUIGoal goal;
00191   goal.options = getDialogOptions();
00192   goal.command.command = goal.command.PLACE;
00193   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00194                            boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00195   active_goal_ = true;
00196 }
00197 
00198 void InteractiveManipulationFrontend::takeMapButtonClicked( wxCommandEvent& /*event*/ )
00199 {
00200   IMGUIGoal goal;
00201   goal.options = getDialogOptions();
00202   goal.command.command = goal.command.TAKE_MAP;
00203   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00204                            boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00205   active_goal_ = true;
00206 }
00207 
00208 void InteractiveManipulationFrontend::resetButtonClicked( wxCommandEvent& /*event*/ )
00209 {
00210   IMGUIGoal goal;
00211   goal.options = getDialogOptions();
00212   goal.command.command = goal.command.RESET;  
00213   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00214                            boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00215   active_goal_ = true;
00216 }
00217 
00218 void InteractiveManipulationFrontend::armGoButtonClicked( wxCommandEvent& /*event*/ )
00219 {
00220   IMGUIGoal goal;
00221   goal.options = getDialogOptions();
00222   goal.command.command = goal.command.MOVE_ARM;
00223   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00224                            boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00225   active_goal_ = true;
00226 }
00227 
00228 void InteractiveManipulationFrontend::lookButtonClicked( wxCommandEvent& /*event*/ )
00229 {
00230   IMGUIGoal goal;
00231   goal.options = getDialogOptions();
00232   goal.command.command = goal.command.LOOK_AT_TABLE;
00233   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00234                            boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00235   active_goal_ = true;
00236 }
00237 
00238 void InteractiveManipulationFrontend::modelObjectClicked( wxCommandEvent& /*event*/)
00239 {
00240   IMGUIGoal goal;
00241   goal.options = getDialogOptions();
00242   goal.command.command = goal.command.MODEL_OBJECT;
00243   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00244                            boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00245   active_goal_ = true;
00246 }
00247 
00248 void InteractiveManipulationFrontend::gripperSliderScrollChanged( wxScrollEvent& )
00249 {
00250   IMGUIGoal goal;
00251   goal.options = getDialogOptions();
00252   goal.command.command = goal.command.MOVE_GRIPPER;
00253   action_client_->sendGoal(goal,Client::SimpleDoneCallback(), Client::SimpleActiveCallback(),
00254                            boost::bind(&InteractiveManipulationFrontend::feedbackCallback, this, _1));
00255   active_goal_ = true;
00256 }
00257 
00258 void InteractiveManipulationFrontend::advancedOptionsClicked( wxCommandEvent& /*event*/ )
00259 {
00260   AdvancedOptionsDialog *dlg = new AdvancedOptionsDialog(NULL, this);
00261   dlg->setOptionsMsg(adv_options_);
00262   dlg->Show();
00263 }
00264 
00265 IMGUIOptions InteractiveManipulationFrontend::getDialogOptions()
00266 {
00267   IMGUIOptions options;
00268   options.collision_checked = collision_box_->IsChecked();
00269   options.grasp_selection = grasp_object_choice_->GetSelection();
00270   options.arm_selection = arm_choice_->GetSelection();
00271   options.reset_choice = reset_choice_->GetSelection();
00272   options.arm_action_choice = arm_action_choice_->GetSelection();
00273   options.arm_planner_choice = arm_planner_choice_->GetSelection();
00274   options.adv_options = adv_options_;
00275   options.gripper_slider_position = gripper_slider_->GetValue();
00276   return options;
00277 }
00278 
00279 }
interactive_manipulation_frontend.cpp
