srs_assisted_arm_navigation_ui: srs_assisted_arm_navigation_ui: bb_estimation

00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id:$
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Zdenek Materna (imaterna@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 31/7/2012
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00028 #include <srs_assisted_arm_navigation_ui/bb_estimation_controls.h>
00029 #include <rviz/window_manager_interface.h>
00030 
00031 using namespace std;
00032 using namespace srs_assisted_arm_navigation_ui;
00033 using namespace srs_assisted_arm_navigation;
00034 using namespace srs_assisted_arm_navigation_msgs;
00035 
00036 static const string cv_win = "Assisted object detection";
00037 
00038 const int ID_BUTTON_OK(101);
00039 
00040 CButBBEstimationControls * class_ptr;
00041 
00045 CButBBEstimationControls::CButBBEstimationControls(wxWindow *parent, const wxString& title, rviz::WindowManagerInterface * wmi )
00046     : wxPanel( parent, wxID_ANY, wxDefaultPosition, wxSize(280, 180), wxTAB_TRAVERSAL, title)
00047     , m_wmi( wmi ),
00048     as_(nh_,ACT_BB_SELECT,false),
00049     it_(nh_)
00050 {
00051 
00052         ros::NodeHandle nh;
00053 
00054         ros::param::param<bool>("~is_video_flipped", is_video_flipped_ , true);
00055         //ros::param::param<bool>("~disable_video", disable_video_ , false);
00056 
00057         class_ptr = this;
00058 
00059     parent_ = parent;
00060 
00061     m_button_ok_ = new wxButton(this, ID_BUTTON_OK, wxT("Positioning completed"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00062 
00063     m_button_ok_->Enable(false);
00064 
00065     //sub_image_ = it_.subscribe("bb_video_in", 1, &CButBBEstimationControls::imageCallback,this);
00066 
00067     as_.registerGoalCallback(    boost::bind(&CButBBEstimationControls::actionGoalCallback, this));
00068     as_.registerPreemptCallback( boost::bind(&CButBBEstimationControls::actionPreemptCallback, this));
00069 
00070     image_ = NULL;
00071     image_tmp_ = NULL;
00072 
00073     image_width_ = 0;
00074     image_height_ = 0;
00075 
00076     butt_down_x_ = -1;
00077     butt_down_y_ = -1;
00078 
00079     disable_video_  = false;
00080 
00081     data_ready_ = false;
00082     some_data_ready_ = false;
00083     action_in_progress_ = false;
00084 
00085     // 20 Hz timer
00086     /*if (!disable_video_)*/ timer_ = nh.createTimer(ros::Duration(0.04),&CButBBEstimationControls::timerCallback,this);
00087     //else ROS_INFO("Started in (almost) useless mode (video disabled).");
00088 
00089     as_.start();
00090 
00091 
00092 }
00093 
00094 
00095 
00096 
00097 CButBBEstimationControls::~CButBBEstimationControls() {
00098 
00099         delete m_button_ok_;
00100         class_ptr = NULL;
00101 
00102 }
00103 
00104 void CButBBEstimationControls::timerCallback(const ros::TimerEvent& ev) {
00105 
00106         ROS_INFO_ONCE("Timer triggered.");
00107 
00108         if (action_in_progress_ && !disable_video_) {
00109 
00110 
00111                 image_mutex_.lock();
00112 
00113                 if (image_ == NULL) {
00114 
00115                         image_mutex_.unlock();
00116                         return;
00117 
00118                 }
00119 
00120                 if (image_tmp_ != NULL) {
00121 
00122                         image_tmp_->release();
00123                         delete image_tmp_;
00124 
00125                 }
00126 
00127                 //image_tmp_ = new cv::Mat(*image_);
00128 
00129                 image_tmp_ = new cv::Mat();
00130 
00131                 image_->copyTo(*image_tmp_);
00132 
00133                 image_mutex_.unlock();
00134 
00135                 std::string text_line1 = "Please select object.";
00136                 std::string text_line2 = "Press left button, drag and then release.";
00137 
00138                 int fontFace = cv::FONT_HERSHEY_COMPLEX_SMALL;
00139                 double fontScale = 0.8;
00140                 int thickness = 1;
00141 
00142                 int baseline=0;
00143 
00144                 cv::Size size_line1 = cv::getTextSize(text_line1, fontFace, fontScale, thickness, &baseline);
00145                 cv::Size size_line2 = cv::getTextSize(text_line2, fontFace, fontScale, thickness, &baseline);
00146 
00147                 int bigger_w = 0;
00148 
00149                 if (size_line1.width > size_line2.width) bigger_w = size_line1.width;
00150                 else bigger_w = size_line2.width;
00151 
00152                 cv::rectangle(*image_tmp_,                    /* the dest image */
00153                                                                   cv::Point(0,0),        /* top left point */
00154                                                                   cv::Point(bigger_w + 20,size_line1.height + size_line2.height + 40),       /* bottom right point */
00155                                                                   cv::Scalar(250, 250, 250, 125),
00156                                                                   CV_FILLED,
00157                                                                   CV_AA,
00158                                                                   0);
00159 
00160                 cv::putText(*image_tmp_, "Please select object.", cvPoint(10,20), fontFace, fontScale, cvScalar(0,0,0), thickness, CV_AA);
00161                 cv::putText(*image_tmp_, "Press left button, drag and then release.", cvPoint(10,50), fontFace, fontScale, cvScalar(0,0,0), thickness, CV_AA);
00162 
00163 
00164 
00165                 if (data_ready_ || some_data_ready_) {
00166 
00167                         cv::Point tl,br;
00168 
00169                         data_mutex_.lock();
00170 
00171                         if (p1_[0] < p2_[0]) {
00172 
00173                                 tl.x = p1_[0];
00174                                 tl.y = p1_[1];
00175 
00176                                 br.x = p2_[0];
00177                                 br.y = p2_[1];
00178 
00179                         } else {
00180 
00181                                 tl.x = p2_[0];
00182                                 tl.y = p2_[1];
00183 
00184                                 br.x = p1_[0];
00185                                 br.y = p1_[1];
00186 
00187                         }
00188 
00189                         data_mutex_.unlock();
00190 
00191                         cv::Scalar color;
00192 
00193                         if (some_data_ready_) color = cv::Scalar(255, 0, 0, 0); // blue - while selecting
00194                         else color = cv::Scalar(0, 0, 255, 0); // red - when selected
00195 
00196                         cv::rectangle(*image_tmp_,                    /* the dest image */
00197                                                   tl,        /* top left point */
00198                                                   br,       /* bottom right point */
00199                                                   color, /* the color; blue */
00200                                                 1, 8, 0);               /* thickness, line type, shift */
00201 
00202 
00203                 }
00204 
00205                 cv::imshow(cv_win.c_str(), *image_tmp_);
00206                 //cv::waitKey(1);
00207 
00208 
00209         }
00210 
00211 
00212 }
00213 
00214 void CButBBEstimationControls::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
00215 
00216         ROS_INFO_ONCE("Received image");
00217 
00218         if (disable_video_) return;
00219 
00220         //cv_bridge::CvImagePtr cv_ptr;
00221 
00222 
00223         try {
00224 
00225                 /*if (cv_ptr_ == NULL) cv_ptr_ = cv_bridge::toCvCopy(msg,msg->encoding);
00226                 else {
00227 
00228                         cv_ptr_.reset();
00229                         cv_ptr_ = cv_bridge::toCvCopy(msg,msg->encoding);
00230 
00231                 }*/
00232 
00233                 cv_bridge::CvImagePtr cv_ptr;
00234                 cv_ptr = cv_bridge::toCvCopy(msg,msg->encoding);
00235 
00236                 image_mutex_.lock();
00237 
00238                 if (image_ != NULL) {
00239 
00240                         image_->release();
00241                         delete image_;
00242 
00243                 }
00244 
00245                 image_ = new cv::Mat(cv_ptr->image);
00246 
00247                 image_mutex_.unlock();
00248 
00249         } catch (cv_bridge::Exception& e) {
00250 
00251                 ROS_ERROR("cv_bridge exception: %s", e.what());
00252                 return;
00253 
00254         }
00255 
00256         if (image_width_ == 0) {
00257 
00258 
00259                 image_width_ = msg->width;
00260                 image_height_ = msg->height;
00261 
00262         }
00263 
00264 }
00265 
00266 void CButBBEstimationControls::newData(int event, int x, int y) {
00267 
00268         if (!as_.isActive()) return;
00269 
00270         switch(event) {
00271 
00272                 case CV_EVENT_LBUTTONDOWN: {
00273 
00274                         butt_down_x_ = x;
00275                         butt_down_y_ = y;
00276 
00277                    } break;
00278 
00279 
00280                 case CV_EVENT_MOUSEMOVE: {
00281 
00282                         if (butt_down_x_ != -1) {
00283 
00284                                 some_data_ready_ = true;
00285 
00286                                 data_mutex_.lock();
00287                                 p1_[0] = butt_down_x_;
00288                                 p1_[1] = butt_down_y_;
00289 
00290                                 p2_[0] = x;
00291                                 p2_[1] = y;
00292                                 data_mutex_.unlock();
00293 
00294                         }
00295 
00296 
00297                 } break;
00298 
00299                    // Mouse UP (end-point of the ROI)
00300                 case CV_EVENT_LBUTTONUP: {
00301 
00302                         data_mutex_.lock();
00303 
00304                         p1_[0] = butt_down_x_;
00305                     p1_[1] = butt_down_y_;
00306 
00307                     butt_down_x_ = -1;
00308                     butt_down_y_ = -1;
00309 
00310                     p2_[0] = x;
00311                     p2_[1] = y;
00312 
00313                     data_mutex_.unlock();
00314 
00315                     some_data_ready_ = false;
00316                     data_ready_ = true;
00317 
00318                         if (!is_video_flipped_) {
00319 
00320                                 fb_.p1[0] = p1_[0];
00321                                 fb_.p1[1] = p1_[1];
00322                                 fb_.p2[0] = p2_[0];
00323                                 fb_.p2[1] = p2_[1];
00324 
00325                         } else {
00326 
00327                                 fb_.p1[0] = image_width_  - p1_[0];
00328                                 fb_.p1[1] = image_height_ - p1_[1];
00329                                 fb_.p2[0] = image_width_  - p2_[0];
00330                                 fb_.p2[1] = image_height_ - p2_[1];
00331 
00332                         }
00333 
00334                     fb_.timestamp = ros::Time::now();
00335 
00336                     as_.publishFeedback(fb_);
00337 
00338                     ROS_INFO("New ROI at [%d,%d], [%d,%d]",p1_[0],p1_[1],p2_[0],p2_[1]);
00339 
00340                     m_button_ok_->Enable(true);
00341 
00342                 } break;
00343 
00344 
00345                  } // switch
00346 
00347 
00348 
00349 }
00350 
00351 void onMouse(int event, int x, int y, int flags, void* param) {
00352 
00353         if (class_ptr!=NULL)
00354                 class_ptr->newData(event,x,y);
00355 
00356 }
00357 
00358 void CButBBEstimationControls::actionGoalCallback() {
00359 
00360         ROS_INFO("Asking user to select ROI in image.");
00361 
00362         goal_ = as_.acceptNewGoal();
00363 
00364         disable_video_ = goal_->disable_video;
00365 
00366         if (!disable_video_) {
00367 
00368                 sub_image_ = it_.subscribe("bb_video_in", 1, &CButBBEstimationControls::imageCallback,this);
00369 
00370                 wxMessageBox(wxString::FromAscii("Please select unknown object in image. You can select object in image several times. After selecting, it will appear as interactive marker in RVIZ and you can tune it. When it will fit real object, please click on \"Positioning finished\" button."), wxString::FromAscii("Assisted object detection"), wxOK, parent_,-1,-1);
00371 
00372                 cv::namedWindow(cv_win.c_str());
00373                 cv::setMouseCallback(cv_win,onMouse,NULL);
00374 
00375         } else {
00376 
00377                 wxMessageBox(wxString::FromAscii("Please tune position and orientation of interactive marker to fit the object. When finished, please click on \"Positioning finished\" button."), wxString::FromAscii("Assisted object detection"), wxOK, parent_,-1,-1);
00378                 m_button_ok_->Enable(true);
00379 
00380         }
00381 
00382         //max_time_ = ros::Time::now() + ros::Duration(30);
00383 
00384 
00385 
00386 
00387 
00388         action_in_progress_ = true;
00389 
00390 }
00391 
00392 
00393 
00394 void CButBBEstimationControls::actionPreemptCallback() {
00395 
00396         ROS_INFO("%s: Preempted",ACT_BB_SELECT.c_str());
00397         as_.setPreempted();
00398 
00399         action_in_progress_ = false;
00400         disable_video_ = false;
00401 
00402         if (!disable_video_) {
00403 
00404                 cv::destroyWindow(cv_win.c_str());
00405                 sub_image_.shutdown();
00406                 some_data_ready_ = false;
00407                 data_ready_ = false;
00408                 image_width_ = 0;
00409                 image_height_ = 0;
00410 
00411         }
00412 
00413 }
00414 
00415 void CButBBEstimationControls::OnOk(wxCommandEvent& event) {
00416 
00417 
00418         if (!as_.isActive()) return;
00419 
00420         ManualBBEstimationResult result;
00421 
00422         if (!disable_video_) {
00423 
00424                 if (!is_video_flipped_) {
00425 
00426                         result.p1[0] = p1_[0];
00427                         result.p1[1] = p1_[1];
00428                         result.p2[0] = p2_[0];
00429                         result.p2[1] = p2_[1];
00430 
00431                 } else {
00432 
00433                         result.p1[0] = image_width_  - p1_[0];
00434                         result.p1[1] = image_height_ - p1_[1];
00435                         result.p2[0] = image_width_  - p2_[0];
00436                         result.p2[1] = image_height_ - p2_[1];
00437 
00438                 }
00439 
00440         } else {
00441 
00442                 result.p1[0] = 0.0;
00443                 result.p1[1] = 0.0;
00444                 result.p2[0] = 0.0;
00445                 result.p2[1] = 0.0;
00446 
00447         }
00448 
00449         //action_in_progress_ = false;
00450 
00451         as_.setSucceeded(result, "Go on...");
00452 
00453         m_button_ok_->Enable(false);
00454 
00455         if (disable_video_) {
00456 
00457                 cv::destroyWindow(cv_win.c_str());
00458                 sub_image_.shutdown();
00459 
00460                 some_data_ready_ = false;
00461                 data_ready_ = false;
00462                 action_in_progress_ = false;
00463                 image_width_ = 0;
00464                 image_height_ = 0;
00465 
00466         }
00467 
00468         disable_video_ = false;
00469 
00470 }
00471 
00472 
00473 
00474 
00475 BEGIN_EVENT_TABLE(CButBBEstimationControls, wxPanel)
00476         EVT_BUTTON(ID_BUTTON_OK, CButBBEstimationControls::OnOk)
00477 END_EVENT_TABLE()
00478
bb_estimation_controls.cpp
