pr2_interactive_object_detection: pr2_interactive_object_detection: interactive_object_detection

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_object_detection/interactive_object_detection_frame.h"
00031 
00032 using namespace pr2_interactive_object_detection;
00033 
00034 using actionlib::SimpleActionClient;
00035 
00036 
00037 namespace pr2_interactive_object_detection {
00038 
00039 InteractiveObjectDetectionFrame::InteractiveObjectDetectionFrame
00040   (InteractiveObjectDetectionDisplay *display, wxWindow *parent) :
00041   InteractiveObjectDetectionFrameBase(parent),
00042   user_cmd_action_client_("object_detection_user_command", true),
00043   action_requested_(false),
00044   root_nh_(""),
00045   priv_nh_("~"),
00046   executing_thread_(0)
00047 {
00048 }
00049 
00050 
00051 InteractiveObjectDetectionFrame::~InteractiveObjectDetectionFrame()
00052 {
00053   if (executing_thread_)
00054   {
00055     if (!mutex_.try_lock())
00056     {
00057       ROS_INFO("Waiting for execution thread to finish");
00058       mutex_.lock();
00059       ROS_INFO("Execution thread finished.");
00060     }
00061     mutex_.unlock();
00062     executing_thread_->join();
00063     delete executing_thread_;
00064   }
00065 }
00066 
00067 void InteractiveObjectDetectionFrame::requestUserCommand( int8_t request, bool interactive )
00068 {
00069   if (!mutex_.try_lock())
00070   {
00071     ROS_WARN( "User command already executing. Action canceled." );
00072     return;
00073   }
00074   mutex_.unlock();
00075   if (executing_thread_ != 0)
00076   {
00077     executing_thread_->join();
00078     delete executing_thread_;
00079   }
00080 
00081   boost::function< void () > user_command =
00082       boost::bind( &InteractiveObjectDetectionFrame::executeRequest, this, request, interactive );
00083 
00084   executing_thread_ = new boost::thread( user_command );
00085 }
00086 
00087 
00088 void InteractiveObjectDetectionFrame::update()
00089 {
00090   if (!mutex_.try_lock())
00091   {
00092     Enable(false);
00093   }
00094   else
00095   {
00096     Enable( true );
00097 
00098     seg_button_->Enable( !action_requested_ );
00099     rec_button_->Enable( !action_requested_ );
00100     det_button_->Enable( !action_requested_ );
00101 
00102     cancel_button_->Enable( action_requested_ );
00103 
00104     status_bar_->SetLabel( wxString::FromAscii(status_.c_str()) );
00105 
00106     if ( !action_requested_ )
00107     {
00108       seg_status_label_->SetLabel( wxString::FromAscii(seg_status_.c_str()) );
00109       rec_status_label_->SetLabel( wxString::FromAscii(rec_status_.c_str()) );
00110       det_status_label_->SetLabel( wxString::FromAscii(det_status_.c_str()) );
00111     }
00112 
00113     mutex_.unlock();
00114   }
00115 }
00116 
00117 
00118 void InteractiveObjectDetectionFrame::userCmdDone(const actionlib::SimpleClientGoalState& state,
00119             const UserCommandResultConstPtr& result)
00120 {
00121   boost::mutex::scoped_lock l1( mutex_ );
00122 
00123   ROS_INFO("Action finished in state [%s]", state.toString().c_str());
00124   action_requested_ = false;
00125   status_ = "[" + state.toString() + "]";
00126 
00127   switch (user_command_goal_.request)
00128   {
00129     case UserCommandGoal::SEGMENT:
00130       seg_status_ = state.getText();
00131       break;
00132     case UserCommandGoal::RECOGNIZE:
00133       rec_status_ = state.getText();
00134       break;
00135     case UserCommandGoal::DETECT:
00136       det_status_ = state.getText();
00137       break;    
00138     default:
00139       break;
00140   }
00141 }
00142 
00143 
00144 // Called once when the goal becomes active
00145 void InteractiveObjectDetectionFrame::userCmdActive()
00146 {
00147   boost::mutex::scoped_lock l1( mutex_ );
00148 }
00149 
00150 
00151 // Called every time feedback is received for the goal
00152 void InteractiveObjectDetectionFrame::userCmdFeedback(const UserCommandFeedbackConstPtr& feedback)
00153 {
00154   boost::mutex::scoped_lock l1( mutex_ );
00155   if ( action_requested_ )
00156   {
00157     ROS_INFO_STREAM( feedback->status );
00158     status_ = feedback->status;
00159   }
00160 }
00161 
00162 
00163 void InteractiveObjectDetectionFrame::cancelButtonClicked( wxCommandEvent& event )
00164 {
00165   boost::mutex::scoped_lock l1( mutex_ );
00166   ROS_WARN( "Canceling action.." );
00167 
00168   user_cmd_action_client_.cancelGoal();
00169   ROS_INFO( "Action canceled." );
00170 }
00171 
00172 
00173 void InteractiveObjectDetectionFrame::resetButtonClicked( wxCommandEvent& event )
00174 {
00175   seg_status_ = "";
00176   rec_status_ = "";
00177   det_status_ = "";
00178   requestUserCommand( UserCommandGoal::RESET, 0 );  
00179   ROS_INFO( "Detection markers reset." );
00180 }
00181 
00182 
00183 void InteractiveObjectDetectionFrame::executeRequest( int8_t request, bool interactive )
00184 {
00185   boost::mutex::scoped_lock l1( mutex_ );
00186 
00187   if ( action_requested_ )
00188   {
00189     ROS_ERROR( "Action is currently running. Cannot start new one." );
00190     return;
00191   }
00192 
00193   if ( !user_cmd_action_client_.waitForServer( ros::Duration(2.0) ) )
00194   {
00195     status_ = "ERROR: Action server not found.";
00196     return;
00197   }
00198 
00199   status_ = "Action requested.";
00200 
00201   ROS_INFO( "Requesting user command %d, interactive =%d", request, interactive );
00202   action_requested_ = true;
00203 
00204   user_command_goal_.request = request;
00205   user_command_goal_.interactive = interactive;
00206 
00207   user_cmd_action_client_.sendGoal(user_command_goal_,
00208       boost::bind(&InteractiveObjectDetectionFrame::userCmdDone, this, _1, _2),
00209       boost::bind(&InteractiveObjectDetectionFrame::userCmdActive, this),
00210       boost::bind(&InteractiveObjectDetectionFrame::userCmdFeedback, this, _1) );
00211 }
00212 
00213 
00214 void InteractiveObjectDetectionFrame::segButtonClicked( wxCommandEvent& event )
00215 {
00216   seg_status_ = "Working..";
00217   rec_status_ = "";
00218   det_status_ = "";
00219   requestUserCommand( UserCommandGoal::SEGMENT, interactive_cb_->IsChecked() );
00220 }
00221 
00222 void InteractiveObjectDetectionFrame::recButtonClicked( wxCommandEvent& event )
00223 {
00224   rec_status_ = "Working..";
00225   det_status_ = "";
00226   requestUserCommand( UserCommandGoal::RECOGNIZE, interactive_cb_->IsChecked() );
00227 }
00228 
00229 void InteractiveObjectDetectionFrame::detButtonClicked( wxCommandEvent& event )
00230 {
00231   seg_status_ = "";
00232   rec_status_ = "";
00233   det_status_ = "Working..";
00234   requestUserCommand( UserCommandGoal::DETECT, interactive_cb_->IsChecked() );
00235 }
00236 
00237 }
interactive_object_detection_frame.cpp
