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


pr2_interactive_object_detection
Author(s): David Gossow
autogenerated on Fri Jan 3 2014 12:04:26