00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00145 void InteractiveObjectDetectionFrame::userCmdActive()
00146 {
00147 boost::mutex::scoped_lock l1( mutex_ );
00148 }
00149
00150
00151
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 }