srs_assisted_arm_navigation_ui: srs_assisted_arm_navigation_ui: arm_navigation

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: 5/4/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 
00036 #include "srs_assisted_arm_navigation_ui/arm_navigation_controls.h"
00037 
00038 #include <rviz/window_manager_interface.h>
00039 
00040 using namespace std;
00041 using namespace srs_assisted_arm_navigation_ui;
00042 using namespace srs_assisted_arm_navigation;
00043 using namespace srs_assisted_arm_navigation_msgs;
00044 
00045 const int ID_BUTTON_NEW(101);
00046 const int ID_BUTTON_PLAN(102);
00047 
00048 const int ID_BUTTON_EXECUTE(104);
00049 const int ID_BUTTON_RESET(105);
00050 const int ID_BUTTON_SUCCESS(106);
00051 const int ID_BUTTON_FAILED(107);
00052 
00053 
00054 //const int ID_BUTTON_SWITCH(113);
00055 const int ID_BUTTON_REPEAT(114);
00056 
00057 const int ID_BUTTON_UNDO(115);
00058 
00059 //const int ID_BUTTON_LIFT(118);
00060 //const int ID_BUTTON_MOVE_TO_HOLD(119);
00061 
00062 
00063 /*const int ID_BUTTON_MLEFT(117);
00064 const int ID_BUTTON_MRIGHT(118);
00065 const int ID_BUTTON_MUP(119);
00066 const int ID_BUTTON_MDOWN(120);
00067 const int ID_BUTTON_MFORW(121);
00068 const int ID_BUTTON_MBACK(122);*/
00069 
00070 const int ID_CHECKBOX_POS(125);
00071 const int ID_CHECKBOX_OR(126);
00072 
00073 const int ID_CHECKBOX_ACO(120);
00074 
00075 
00076 const int ID_BUTTON_STOP_TRAJ(123);
00077 
00078 const int ID_CHOICE(175);
00079 
00083 CArmManipulationControls::CArmManipulationControls(wxWindow *parent, const wxString& title, rviz::WindowManagerInterface * wmi )
00084     : wxPanel( parent, wxID_ANY, wxDefaultPosition, wxSize(280, 180), wxTAB_TRAVERSAL, title)
00085     , m_wmi( wmi )
00086 {
00087 
00088 
00089     parent_ = parent;
00090     
00091     traj_executed_ = 0;
00092 
00093     ros::param::param<bool>("~wait_for_start", wait_for_start_ , WAIT_FOR_START);
00094 
00095     ros::NodeHandle nh("~");
00096 
00097     XmlRpc::XmlRpcValue pres;
00098 
00099         if (nh.getParam("arm_nav_presets",pres)) {
00100 
00101                 ROS_ASSERT(pres.getType() == XmlRpc::XmlRpcValue::TypeArray);
00102 
00103                 ROS_INFO("%d presets for assisted arm navigation plugin.",pres.size());
00104 
00105                 for (int i=0; i < pres.size(); i++) {
00106 
00107                         Preset pr;
00108 
00109                         XmlRpc::XmlRpcValue xpr = pres[i];
00110 
00111                         if (xpr.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00112 
00113                           ROS_ERROR("Wrong syntax in YAML config.");
00114                           continue;
00115 
00116                         }
00117 
00118                         // read the name
00119                         if (!xpr.hasMember("name")) {
00120 
00121                           ROS_ERROR("Preset doesn't have 'name' property defined.");
00122                           continue;
00123 
00124                         } else {
00125 
00126                           XmlRpc::XmlRpcValue name = xpr["name"];
00127 
00128                           std::string tmp = static_cast<std::string>(name);
00129 
00130                           pr.name = tmp;
00131 
00132                         }
00133 
00134                         // read the positions
00135                         if (!xpr.hasMember("position")) {
00136 
00137                           ROS_ERROR("Preset doesn't have 'position' property defined.");
00138                           continue;
00139 
00140                         } else {
00141 
00142                           XmlRpc::XmlRpcValue pos = xpr["position"];
00143 
00144                           if (pos.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00145 
00146                                   if (pos.size() != 3) {
00147 
00148                                           ROS_ERROR("Strange number of positions (%d)",pos.size());
00149                                           continue;
00150 
00151                                   }
00152 
00153 
00154                                           XmlRpc::XmlRpcValue p = pos[0];
00155                                           double tmp = static_cast<double>(p);
00156                                           pr.pose.position.x = tmp;
00157 
00158                                           p = pos[1];
00159                                           tmp = static_cast<double>(p);
00160                                           pr.pose.position.y = tmp;
00161 
00162                                           p = pos[2];
00163                                           tmp = static_cast<double>(p);
00164                                           pr.pose.position.z = tmp;
00165 
00166                           } else {
00167 
00168                                   ROS_ERROR("Property 'forces' is defined in bad way.");
00169                                   continue;
00170 
00171                           }
00172 
00173                         } // else forces
00174 
00175 
00176                         // read the orientation
00177                         if (!xpr.hasMember("orientation")) {
00178 
00179                           ROS_ERROR("Preset doesn't have 'orientation' property defined.");
00180                           continue;
00181 
00182                         } else {
00183 
00184                           XmlRpc::XmlRpcValue ori = xpr["orientation"];
00185 
00186                           if (ori.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00187 
00188                                   // RPY
00189                                   if (ori.size() == 3) {
00190 
00191                                           geometry_msgs::Vector3 rpy;
00192 
00193                                                   XmlRpc::XmlRpcValue p = ori[0];
00194                                                   double tmp = static_cast<double>(p);
00195 
00196                                                   rpy.x = tmp;
00197 
00198                                                   p = ori[1];
00199                                                   tmp = static_cast<double>(p);
00200                                                   rpy.y = tmp;
00201 
00202                                                   p = ori[2];
00203                                                   tmp = static_cast<double>(p);
00204                                                   rpy.z = tmp;
00205 
00206                                           pr.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(rpy.x,rpy.y,rpy.z);
00207 
00208 
00209                                   } else if (ori.size() == 4) {
00210 
00211 
00212 
00213                                                   XmlRpc::XmlRpcValue p = ori[0];
00214                                                   double tmp = static_cast<double>(p);
00215                                                   pr.pose.orientation.x = tmp;
00216 
00217                                                   p = ori[1];
00218                                                   tmp = static_cast<double>(p);
00219                                                   pr.pose.orientation.y = tmp;
00220 
00221                                                   p = ori[2];
00222                                                   tmp = static_cast<double>(p);
00223                                                   pr.pose.orientation.z = tmp;
00224 
00225                                                   p = ori[3];
00226                                                   tmp = static_cast<double>(p);
00227                                                   pr.pose.orientation.w = tmp;
00228 
00229                                   } else {
00230 
00231                                           ROS_ERROR("Prop 'orientation' should have 3 or 4 values (%d).",ori.size());
00232                                           continue;
00233 
00234                                   }
00235 
00236 
00237 
00238 
00239 
00240                           } else {
00241 
00242                                   ROS_ERROR("Property 'target_pos' is defined in bad way.");
00243                                   continue;
00244 
00245                           }
00246 
00247                         } // else target_pos
00248 
00249                         // read the name
00250                                 if (!xpr.hasMember("relative")) {
00251 
00252                                   ROS_ERROR("Preset doesn't have 'relative' property defined.");
00253                                   continue;
00254 
00255                                 } else {
00256 
00257                                   XmlRpc::XmlRpcValue rel = xpr["relative"];
00258 
00259                                   bool tmp = static_cast<bool>(rel);
00260 
00261                                   pr.relative = tmp;
00262 
00263                                 }
00264 
00265 
00266                         presets_.push_back(pr);
00267 
00268                 } // for around all presets
00269 
00270         } else {
00271 
00272                         ROS_ERROR("Can't get presets for predefined IM positions.");
00273 
00274                 }
00275 
00276         wxArrayString prt;
00277 
00278         prt.Add(wxString::FromAscii("none"));
00279 
00280         for(unsigned int i=0; i < presets_.size(); i++) {
00281 
00282                 std::string tmp;
00283 
00284                 tmp = presets_[i].name;
00285 
00286                 if (presets_[i].relative) tmp += " (R)";
00287                 else tmp += " (A)";
00288 
00289                 prt.Add(wxString::FromAscii(tmp.c_str()));
00290 
00291         }
00292 
00293         presets_choice_ = new wxChoice(this, ID_CHOICE,wxDefaultPosition,wxDefaultSize,prt);
00294 
00295     buttons_["stop"] = new wxButton(this, ID_BUTTON_STOP_TRAJ, wxT("Stop"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00296     //b_switch_ = new wxToggleButton(this, ID_BUTTON_SWITCH, wxT("Avoid finger collisions"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00297     buttons_["success"] = new wxButton(this, ID_BUTTON_SUCCESS, wxT("Task completed"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00298     buttons_["failed"] = new wxButton(this, ID_BUTTON_FAILED, wxT("Cannot complete task"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00299     buttons_["repeat"] = new wxButton(this, ID_BUTTON_REPEAT, wxT("Repeat detection"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00300 
00301     //---------------------------------------------------------------
00302     // end effector controls
00303     buttons_["undo"] = new wxButton(this, ID_BUTTON_UNDO, wxT("Undo marker movement"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00304    /* buttons_["move_left"] = new wxButton(this, ID_BUTTON_MLEFT, wxT("L"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00305     buttons_["move_right"] = new wxButton(this, ID_BUTTON_MRIGHT, wxT("R"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00306     buttons_["move_up"] = new wxButton(this, ID_BUTTON_MUP, wxT("U"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00307     buttons_["move_down"] = new wxButton(this, ID_BUTTON_MDOWN, wxT("D"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00308     buttons_["move_forw"] = new wxButton(this, ID_BUTTON_MFORW, wxT("F"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00309     buttons_["move_back"] = new wxButton(this, ID_BUTTON_MBACK, wxT("B"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);*/
00310 
00311     buttons_["new"] = new wxButton(this, ID_BUTTON_NEW, wxT("Start arm planning"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00312     buttons_["plan"] = new wxButton(this, ID_BUTTON_PLAN, wxT("Simulate movement"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00313     buttons_["execute"] = new wxButton(this, ID_BUTTON_EXECUTE, wxT("Execute"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00314 
00315     //buttons_["lift"] = new wxButton(this, ID_BUTTON_LIFT, wxT("Lift the object"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00316     //setButton("lift",false);
00317 
00318     //buttons_["hold"] = new wxButton(this, ID_BUTTON_MOVE_TO_HOLD, wxT("Move to hold pos."),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
00319     //setButton("hold",false);
00320 
00321     buttons_["stop"]->SetForegroundColour (wxColour (255, 255, 255));
00322     buttons_["stop"]->SetBackgroundColour (wxColour (255, 108, 108));
00323 
00324     m_pos_lock_ = new wxCheckBox(this,ID_CHECKBOX_POS,wxT("position"),wxDefaultPosition,wxDefaultSize);
00325     m_or_lock_ = new wxCheckBox(this,ID_CHECKBOX_OR,wxT("orientation"),wxDefaultPosition,wxDefaultSize);
00326 
00327     m_aco_ = new wxCheckBox(this,ID_CHECKBOX_ACO,wxT("Avoid fingers collisions"),wxDefaultPosition,wxDefaultSize);
00328     // TODO read initial state of m_aco_ from (state) topic !!!
00329 
00330     m_aco_->Enable(false);
00331     m_aco_->Set3StateValue(wxCHK_UNDETERMINED);
00332 
00333     m_pos_lock_->Enable(false);
00334     m_or_lock_->Enable(false);
00335 
00336     m_pos_lock_->Set3StateValue(wxCHK_UNDETERMINED);
00337     m_or_lock_->Set3StateValue(wxCHK_UNDETERMINED);
00338 
00339 
00340     /*wxSizer *vsizer_endef = new wxStaticBoxSizer(wxVERTICAL,this,wxT("End effector controls"));
00341     wxSizer *hsizer_endef_top = new wxBoxSizer(wxHORIZONTAL);
00342     wxSizer *hsizer_endef_mid = new wxBoxSizer(wxHORIZONTAL);
00343     wxSizer *hsizer_endef_mid2 = new wxBoxSizer(wxHORIZONTAL);
00344     wxSizer *vsizer_endef_bot = new wxBoxSizer(wxVERTICAL);*/
00345 
00346     //hsizer_endef_top->Add(buttons_["undo"], ID_BUTTON_UNDO);
00347 
00348     /*hsizer_endef_mid->Add(buttons_["move_left"], ID_BUTTON_MLEFT);
00349     hsizer_endef_mid->Add(buttons_["move_right"], ID_BUTTON_MRIGHT);
00350     hsizer_endef_mid->Add(buttons_["move_up"], ID_BUTTON_MUP);
00351     hsizer_endef_mid->Add(buttons_["move_down"], ID_BUTTON_MDOWN);
00352     hsizer_endef_mid->Add(buttons_["move_forw"], ID_BUTTON_MFORW);
00353     hsizer_endef_mid->Add(buttons_["move_back"], ID_BUTTON_MBACK);*/
00354 
00355     /*vsizer_endef->Add(hsizer_endef_top,1,wxEXPAND);
00356     vsizer_endef->Add(hsizer_endef_mid,1,wxEXPAND);
00357     vsizer_endef->Add(hsizer_endef_mid2,1,wxEXPAND);
00358     vsizer_endef->Add(vsizer_endef_bot,1,wxEXPAND);*/
00359 
00360     //---------------------------------------------------------------
00361 
00362     m_text_status = new wxStaticText(this, -1, wxT("status: not initialized yet."));
00363 
00364     m_text_predef_pos_ = new wxStaticText(this, -1, wxT("Move to predefined position"));
00365 
00366     //m_text_object = new wxStaticText(this, -1, wxT("object: none"));
00367     m_text_action_ = new wxStaticText(this, -1, wxT("Current task:"));
00368     m_text_task_ = new wxTextCtrl(this,-1,wxT("None."),wxDefaultPosition,wxDefaultSize,wxTE_MULTILINE|wxTE_READONLY);
00369     m_text_task_->SetSizeHints(200,100);
00370 
00371 
00372     //m_text_timeout = new wxStaticText(this, -1, wxT("timeout: none"));
00373 
00374     disableControls();
00375 
00376     wxSizer *vsizer = new wxBoxSizer(wxVERTICAL); // top sizer
00377 
00378     wxSizer *vsizer_top = new wxStaticBoxSizer(wxVERTICAL,this,wxT("Trajectory planning"));
00379 
00380     wxSizer *vsizer_mes = new wxStaticBoxSizer(wxVERTICAL,this,wxT("Task"));
00381 
00382     // -----------------
00383     wxSizer *vsizer_sn = new wxStaticBoxSizer(wxVERTICAL,this,wxT("SpaceNavigator locks"));
00384     wxSizer *hsizer_sn_in = new wxBoxSizer(wxHORIZONTAL);
00385 
00386     hsizer_sn_in->Add(m_pos_lock_,ID_CHECKBOX_POS);
00387     hsizer_sn_in->Add(m_or_lock_,ID_CHECKBOX_OR);
00388 
00389     vsizer_sn->Add(hsizer_sn_in,1,wxEXPAND);
00390 
00391     // -----------------
00392 
00393     wxSizer *vsizer_add = new wxStaticBoxSizer(wxVERTICAL,this,wxT("Additional controls"));
00394 
00395 
00396 
00397     vsizer_top->Add(buttons_["new"]);
00398     vsizer_top->Add(buttons_["plan"]);
00399     vsizer_top->Add(buttons_["execute"]);
00400     vsizer_top->Add(buttons_["stop"]);
00401     vsizer_top->Add(m_text_status);
00402 
00403 
00404 
00405 
00406 
00407     vsizer_mes->Add(buttons_["success"]);
00408     vsizer_mes->Add(buttons_["repeat"]);
00409     vsizer_mes->Add(buttons_["failed"]);
00410     vsizer_mes->Add(m_text_action_);
00411     vsizer_mes->Add(m_text_task_);
00412 
00413     //vsizer_add->Add(b_switch_);
00414     vsizer_add->Add(m_aco_);
00415     vsizer_add->Add(buttons_["undo"]);
00416     /*vsizer_add->Add(buttons_["lift"]);
00417     vsizer_add->Add(buttons_["hold"]);*/
00418     vsizer_add->Add(m_text_predef_pos_);
00419     vsizer_add->Add(presets_choice_);
00420 
00421 
00422     vsizer->Add(vsizer_top,0,wxEXPAND|wxHORIZONTAL);
00423     vsizer->Add(vsizer_mes,0,wxEXPAND|wxHORIZONTAL);
00424     vsizer->Add(vsizer_add,0,wxEXPAND|wxHORIZONTAL);
00425     vsizer->Add(vsizer_sn,0,wxEXPAND|wxHORIZONTAL);
00426 
00427     allow_repeat_ = false;
00428 
00429     // TODO make it configurable ? - read same parameter from here and from but_arm_manip_node...
00430     aco_ = false;
00431 
00432     //b_switch_->SetValue(aco_);
00433 
00434 
00435     spacenav_pos_lock_ = false;
00436     spacenav_or_lock_ = false;
00437 
00438     /*if (aco_) buttons_["switch"]->SetLabel(wxT("ACO enabled"));
00439     else buttons_["switch"]->SetLabel(wxT("ACO disabled"));*/
00440 
00441 
00442     vsizer->SetSizeHints(this);
00443     this->SetSizerAndFit(vsizer);
00444 
00445     //ros::NodeHandle nh;
00446 
00447     arm_nav_state_sub_ = nh.subscribe("/but_arm_manip/state",10, &CArmManipulationControls::stateCallback,this);
00448 
00449     initialized_ = false;
00450     state_received_ = false;
00451     stop_gui_thread_ = false;
00452     planning_started_ = false;
00453     trajectory_planned_ = false;
00454     arm_nav_called_ = false;
00455 
00456     t_gui_update = boost::thread(&CArmManipulationControls::GuiUpdateThread,this);
00457 
00461     service_start_ = nh.advertiseService(SRV_START,&CArmManipulationControls::nav_start,this);
00462 
00463     //cob_script = new cob_client("/script_server",true);
00464 
00465     //cob_script_inited = false;
00466 
00467 
00468 
00469 }
00471 
00472 
00473 void CArmManipulationControls::setControlsToDefaultState() {
00474 
00475         planning_started_ = false;
00476 
00477         buttons_["new"]->SetLabel(wxT("Start arm planning"));
00478 
00479         if (wait_for_start_) setButton("new",false);
00480         else setButton("new",true);
00481 
00482         if (arm_nav_called_) {
00483 
00484                 setButton("new",true); // if action was called, we should allow new try
00485 
00486                 if (allow_repeat_) setButton("repeat",true);
00487                 else setButton("repeat",false);
00488 
00489                 if (traj_executed_ > 0) {
00490 
00491                         setButton("success",true); // allow success only if at least one trajectory was executed
00492                 }
00493 
00494         } else {
00495 
00496                 setButton("success",false);
00497                 setButton("failed",false);
00498                 setButton("repeat",false);
00499 
00500         }
00501 
00502         setButton("plan",false);
00503         setButton("execute",false);
00504         m_aco_->Enable(true);
00505         setButton("stop",false);
00506         setButton("undo",false);
00507         presets_choice_->Enable(false);
00508         presets_choice_->Select(0);
00509 
00510 }
00511 
00512 void CArmManipulationControls::disableControls() {
00513 
00514         setButton("new",false);
00515 
00516         setButton("plan",false);
00517         setButton("execute",false);
00518         //setButton("switch",false);
00519         m_aco_->Enable(false);
00520 
00521         setButton("success",false);
00522         setButton("failed",false);
00523         setButton("repeat",false);
00524 
00525         setButton("undo",false);
00526 
00527         setButton("stop",false);
00528         /*setButton("lift",false);
00529         setButton("hold",false);*/
00530         presets_choice_->Enable(false);
00531 
00532 }
00533 
00534 
00535 void CArmManipulationControls::stateCallback(const AssistedArmNavigationState::ConstPtr& msg) {
00536 
00537         ROS_INFO_ONCE("State received.");
00538 
00539         if (!state_received_) {
00540 
00541                 state_received_ = true;
00542 
00543                 aco_ = msg->aco_state;
00544 
00545         }
00546 
00547         if ( (spacenav_pos_lock_ != msg->position_locked) || (spacenav_or_lock_ != msg->orientation_locked) ) {
00548 
00549                 spacenav_pos_lock_ = msg->position_locked;
00550                 spacenav_or_lock_ = msg->orientation_locked;
00551 
00552         }
00553 
00554 }
00555 
00556 void CArmManipulationControls::GuiUpdateThread() {
00557 
00558         ROS_INFO("Assisted arm nav. GUI thread started.");
00559 
00560         ros::Rate r(5);
00561 
00562         while(!stop_gui_thread_ && ros::ok()) {
00563 
00564                 if ( (!initialized_) && state_received_) {
00565 
00566                         wxMutexGuiEnter();
00567 
00568                         if (aco_) m_aco_->Set3StateValue(wxCHK_CHECKED);
00569                         else m_aco_->Set3StateValue(wxCHK_UNCHECKED);
00570 
00571                         setControlsToDefaultState();
00572 
00573                         /*if (wait_for_start_) m_text_status = new wxStaticText(this, -1, wxT("status: waiting for task"));
00574                         else m_text_status = new wxStaticText(this, -1, wxT("status: let's start"));*/
00575                         if (wait_for_start_) m_text_status->SetLabel(wxT("status: waiting for task"));
00576                         else m_text_status->SetLabel(wxT("status: let's start"));
00577 
00578                         wxMutexGuiLeave();
00579 
00580                         ROS_INFO("Assisted arm nav. plugin initialized.");
00581 
00582                         initialized_ = true;
00583 
00584                 }
00585 
00586                 if (planning_started_) {
00587 
00588                         // update spacenav
00589                         wxMutexGuiEnter();
00590 
00591                         if (spacenav_pos_lock_) m_pos_lock_->Set3StateValue(wxCHK_CHECKED);
00592                         else m_pos_lock_->Set3StateValue(wxCHK_UNCHECKED);
00593 
00594                         if (spacenav_or_lock_) m_or_lock_->Set3StateValue(wxCHK_CHECKED);
00595                         else m_or_lock_->Set3StateValue(wxCHK_UNCHECKED);
00596 
00597                         wxMutexGuiLeave();
00598 
00599                 }
00600 
00601                 r.sleep();
00602 
00603         }
00604 
00605         ROS_INFO("Stopping assisted arm nav. GUI thread.");
00606         stop_gui_thread_ = false;
00607 
00608 }
00609 
00610 
00611 void CArmManipulationControls::setButton(string but, bool state) {
00612 
00613   if (buttons_[but]!=NULL)
00614     buttons_[but]->Enable(state);
00615 
00616 }
00617 
00618 
00619 
00620 CArmManipulationControls::~CArmManipulationControls() {
00621 
00622   stop_gui_thread_ = true;
00623 
00624   ROS_INFO("Waiting for threads to finish.");
00625 
00626   t_gui_update.join();
00627 
00628   //if (cob_script!=NULL) delete cob_script;
00629 
00630   ButtonsMap::iterator it;
00631 
00632   for (it = buttons_.begin(); it != buttons_.end(); ++it)
00633     delete it->second;
00634 
00635   buttons_.clear();
00636 
00637   delete m_aco_;
00638   delete m_text_status;
00639   //delete m_text_object;
00640   //delete m_text_timeout;
00641 
00642 
00643   ROS_INFO("Exiting...");
00644 
00645 }
00646 
00647 void CArmManipulationControls::NewThread() {
00648 
00649 
00650    // first, let's try to refresh planning scene
00651   if (!refresh()) {
00652 
00653           setControlsToDefaultState();
00654 
00655           return;
00656 
00657   }
00658 
00659   ROS_INFO("Request for new trajectory");
00660 
00661   ArmNavNew srv;
00662   std::string status = "";
00663   bool success = false;
00664 
00665    if (ros::service::call(SRV_NEW,srv) ) {
00666 
00667      if (srv.response.completed) {
00668 
00669          success = true;
00670 
00671          status = "status: Perform requested action.";
00672 
00673          /*if (goal_pregrasp) status = "status: Move arm to desired position.";
00674          if (goal_away) status = "status: Move arm to safe position.";*/
00675 
00676      } else {
00677 
00678        status = "status: " + srv.response.error;
00679 
00680      }
00681 
00682    } else {
00683 
00684      ROS_ERROR("Failed when calling arm_nav_new service");
00685      status = "status: Communication error.";
00686 
00687    }
00688 
00689    wxMutexGuiEnter();
00690 
00691    m_text_status->SetLabel(wxString::FromAscii(status.c_str()));
00692 
00693    if (success) {
00694 
00695          disableControls();
00696 
00697          buttons_["new"]->SetLabel(wxT("Cancel planning"));
00698          planning_started_ = true;
00699 
00700          setButton("new",true);
00701          setButton("plan",true);
00702      setButton("undo",true);
00703      /*setButton("lift",true);
00704      setButton("hold",true);*/
00705      presets_choice_->Enable(true);
00706 
00707 
00708      if (arm_nav_called_) {
00709 
00710          if (allow_repeat_) buttons_["repeat"]->Enable(true);
00711          buttons_["failed"]->Enable(true);
00712 
00713      }
00714 
00715 
00716    } else {
00717 
00718            // handle error on communication
00719            setControlsToDefaultState();
00720 
00721    }
00722 
00723    wxMutexGuiLeave();
00724 
00725 }
00726 
00727 
00728 bool CArmManipulationControls::refresh() {
00729 
00730         ROS_INFO("Refreshing planning scene");
00731 
00732    ArmNavRefresh srv;
00733 
00734    if ( ros::service::exists(SRV_REFRESH,true) && ros::service::call(SRV_REFRESH,srv) ) {
00735 
00736          if (srv.response.completed) {
00737 
00738            wxMutexGuiEnter();
00739            m_text_status->SetLabel(wxString::FromAscii("status: Refreshed. Please wait."));
00740            wxMutexGuiLeave();
00741 
00742          } else {
00743 
00744            wxMutexGuiEnter();
00745            m_text_status->SetLabel(wxString::FromAscii("status: Error on refreshing."));
00746            wxMutexGuiLeave();
00747            return false;
00748 
00749          }
00750 
00751    } else {
00752 
00753          ROS_ERROR("failed when calling arm_nav_refresh service");
00754          wxMutexGuiEnter();
00755          m_text_status->SetLabel(wxString::FromAscii("status: Communication error"));
00756          wxMutexGuiLeave();
00757          return false;
00758 
00759    }
00760 
00761   return true;
00762 
00763 }
00764 
00765 
00766 void CArmManipulationControls::OnNew(wxCommandEvent& event) {
00767 
00768         trajectory_planned_ = false;
00769 
00770         if (!planning_started_) {
00771 
00772                 // planning was not started, so we want to start it
00773 
00774                   if (!ros::service::exists(SRV_NEW,true)) {
00775 
00776                         m_text_status->SetLabel(wxString::FromAscii("status: communication error"));
00777                         ROS_ERROR("Service %s is not ready.",((std::string)SRV_NEW).c_str());
00778                         return;
00779 
00780                   }
00781 
00782                   boost::posix_time::time_duration td = boost::posix_time::milliseconds(100);
00783 
00785                   if (t_new.timed_join(td)) {
00786 
00787                         disableControls();
00788 
00789                         m_text_status->SetLabel(wxString::FromAscii("status: Please wait..."));
00790 
00791                         t_new = boost::thread(&CArmManipulationControls::NewThread,this);
00792 
00793                   } else ROS_INFO("We have to wait until new thread finishes.");
00794 
00795         } else {
00796 
00797            //  planning has been already started, so we want to cancel it
00798            ROS_INFO("Reset planning stuff to initial state");
00799 
00800            ArmNavReset srv;
00801 
00802            if ( ros::service::exists(SRV_RESET,true) && ros::service::call(SRV_RESET,srv) ) {
00803 
00804                  if (srv.response.completed) {
00805 
00806                    m_text_status->SetLabel(wxString::FromAscii("status: Ok, try it again"));
00807 
00808                  } else {
00809 
00810                    m_text_status->SetLabel(wxString::FromAscii(srv.response.error.c_str()));
00811 
00812                  }
00813 
00814            } else {
00815 
00816                  ROS_ERROR("failed when calling arm_nav_reset service");
00817                  m_text_status->SetLabel(wxString::FromAscii("status: Communication error"));
00818 
00819            }
00820 
00821            setControlsToDefaultState();
00822 
00823            buttons_["new"]->SetLabel(wxT("Start arm planning"));
00824            planning_started_ = false;
00825 
00826         }
00827 
00828 }
00829 
00830 
00831 void CArmManipulationControls::PlanThread() {
00832 
00833    ROS_INFO("Starting planning and filtering of new trajectory");
00834 
00835    std::string status = "";
00836    bool success = false;
00837 
00838    ArmNavPlan srv;
00839 
00840    if (ros::service::call(SRV_PLAN,srv) ) {
00841 
00842      success = true;
00843 
00844      if (srv.response.completed) {
00845 
00846          status = "status: Trajectory is ready";
00847          trajectory_planned_ = true;
00848 
00849      }
00850      else status = "status: " + srv.response.error;
00851 
00852    } else {
00853 
00854      success = false;
00855      ROS_ERROR("failed when calling service");
00856      status = "status: Communication error";
00857 
00858    }
00859 
00860    wxMutexGuiEnter();
00861 
00862    m_text_status->SetLabel(wxString::FromAscii(status.c_str()));
00863 
00864    if (success) {
00865 
00866          disableControls();
00867 
00868      setButton("new",true);
00869      setButton("execute",true);
00870      setButton("plan",true);
00871 
00872      if (arm_nav_called_) {
00873 
00874                  if (allow_repeat_) buttons_["repeat"]->Enable(true);
00875                  buttons_["failed"]->Enable(true);
00876 
00877      }
00878 
00879    } else {
00880 
00881          disableControls();
00882          setButton("new",true);
00883          setButton("plan",true);
00884 
00885          if (arm_nav_called_) {
00886 
00887                  if (allow_repeat_) buttons_["repeat"]->Enable(true);
00888                  buttons_["failed"]->Enable(true);
00889 
00890          }
00891 
00892    }
00893 
00894    wxMutexGuiLeave();
00895 
00896 }
00897 
00898 void CArmManipulationControls::OnPlan(wxCommandEvent& event)
00899 {
00900 
00901   if (!trajectory_planned_) {
00902 
00903           if (!ros::service::exists(SRV_PLAN,true)) {
00904 
00905                 m_text_status->SetLabel(wxString::FromAscii("status: communication error"));
00906                 ROS_ERROR("Service %s is not ready.",((std::string)SRV_NEW).c_str());
00907                 return;
00908 
00909           }
00910 
00911           boost::posix_time::time_duration td = boost::posix_time::milliseconds(100);
00912 
00914           if (t_plan.timed_join(td)) {
00915 
00916                 m_text_status->SetLabel(wxString::FromAscii("status: Planning. Please wait..."));
00917 
00918                 disableControls();
00919 
00920                 t_plan = boost::thread(&CArmManipulationControls::PlanThread,this);
00921 
00922           } else ROS_INFO("We have to wait until PLAN thread finishes.");
00923 
00924   } else {
00925 
00926          ArmNavPlay srv;
00927 
00928          if ( ros::service::exists(SRV_PLAY,true) && ros::service::call(SRV_PLAY,srv) ) {
00929 
00930            if (srv.response.completed) {
00931 
00932                  m_text_status->SetLabel(wxString::FromAscii("status: Playing trajectory..."));
00933 
00934            } else {
00935 
00936                  m_text_status->SetLabel(wxString::FromAscii(srv.response.error.c_str()));
00937 
00938            }
00939 
00940          } else {
00941 
00942            ROS_ERROR("failed when calling arm_nav_play service");
00943            m_text_status->SetLabel(wxString::FromAscii("status: Communication error"));
00944 
00945          }
00946 
00947   }
00948 
00949 }
00950 
00951 
00952 
00953 void CArmManipulationControls::OnSwitch(wxCommandEvent& event)
00954 {
00955 
00956   if (aco_) ROS_INFO("Lets switch attached collision object OFF");
00957   else ROS_INFO("Lets switch attached collision object ON");
00958 
00959    ArmNavSwitchAttCO srv;
00960 
00961    srv.request.state = !aco_;
00962 
00963    if ( ros::service::exists(SRV_SWITCH,true) && ros::service::call(SRV_SWITCH,srv) ) {
00964 
00965      if (srv.response.completed) {
00966 
00967        //m_text_status->SetLabel(wxString::FromAscii("status: Playing trajectory..."));
00968 
00969        aco_ = !aco_;
00970 
00971        /*if (aco_) buttons_["switch"]->SetLabel(wxT("ACO enabled"));
00972        else buttons_["switch"]->SetLabel(wxT("ACO disabled"));*/
00973 
00974      } else {
00975 
00976        m_text_status->SetLabel(wxString::FromAscii("Can't switch state of ACO"));
00977        //b_switch_->SetValue(aco_);
00978        if (aco_) m_aco_->Set3StateValue(wxCHK_CHECKED);
00979        else m_aco_->Set3StateValue(wxCHK_UNCHECKED);
00980 
00981      }
00982 
00983    } else {
00984 
00985      std::string tmp = SRV_SWITCH;
00986 
00987      ROS_ERROR("failed when calling %s service",tmp.c_str());
00988      m_text_status->SetLabel(wxString::FromAscii("status: Communication error"));
00989      //b_switch_->SetValue(aco_);
00990      if (aco_) m_aco_->Set3StateValue(wxCHK_CHECKED);
00991      else m_aco_->Set3StateValue(wxCHK_UNCHECKED);
00992 
00993    }
00994 
00995 
00996 }
00997 
00998 
00999 void CArmManipulationControls::OnExecute(wxCommandEvent& event) {
01000 
01001   if (!ros::service::exists(SRV_EXECUTE,true)) {
01002 
01003     m_text_status->SetLabel(wxString::FromAscii("status: communication error"));
01004     ROS_ERROR("Service %s is not ready.",((std::string)SRV_NEW).c_str());
01005     return;
01006 
01007   }
01008 
01009   boost::posix_time::time_duration td = boost::posix_time::milliseconds(100);
01010 
01012   if (t_execute.timed_join(td)) {
01013 
01014     disableControls();
01015     setButton("stop",true);
01016     m_text_status->SetLabel(wxT("status: Executing trajectory..."));
01017 
01018     t_execute = boost::thread(&CArmManipulationControls::ExecuteThread,this);
01019 
01020   } else ROS_INFO("We have to wait until EXECUTE thread finishes.");
01021 
01022 }
01023 
01024 void CArmManipulationControls::ExecuteThread()
01025 {
01026    ROS_INFO("Execution of planned trajectory has been started");
01027 
01028    ArmNavExecute srv;
01029 
01030    std::string status = "";
01031    bool success = false;
01032 
01033    if (ros::service::call(SRV_EXECUTE,srv) ) {
01034 
01035      if (srv.response.completed) {
01036 
01037        success = true;
01038        status = "status: Trajectory was executed.";
01039        traj_executed_++;
01040 
01041      } else {
01042 
01043        success = false;
01044        status = srv.response.error;
01045 
01046      }
01047 
01048    } else {
01049 
01050      ROS_ERROR("failed when calling arm_nav_execute service");
01051      success = false;
01052      status = "status: Communication error";
01053 
01054    }
01055 
01056    wxMutexGuiEnter();
01057 
01058    m_text_status->SetLabel(wxString::FromAscii(status.c_str()));
01059 
01060    //if (success) {
01061 
01062          setControlsToDefaultState();
01063          //setButton("stop",true);
01064 
01065          if (arm_nav_called_) {
01066 
01067                  if (allow_repeat_) setButton("repeat",true);
01068                  setButton("failed",true);
01069                  setButton("success",true);
01070 
01071          }
01072 
01073 
01074    //} else {
01075 
01076 
01077 
01078    //}
01079 
01080    buttons_["new"]->SetLabel(wxT("Start arm planning"));
01081    m_pos_lock_->Set3StateValue(wxCHK_UNDETERMINED);
01082    m_or_lock_->Set3StateValue(wxCHK_UNDETERMINED);
01083 
01084    wxMutexGuiLeave();
01085 
01086 }
01087 
01088 
01089 void CArmManipulationControls::OnSuccess(wxCommandEvent& event)
01090 {
01091    ROS_INFO("Finishing manual arm manipulation task");
01092 
01093    ArmNavSuccess srv;
01094 
01095    if ( ros::service::exists(SRV_SUCCESS,true) && ros::service::call(SRV_SUCCESS,srv) ) {
01096 
01097        m_text_status->SetLabel(wxString::FromAscii("status: Succeeded :-)"));
01098 
01099    } else {
01100 
01101      ROS_ERROR("failed when calling arm_nav_success service");
01102      m_text_status->SetLabel(wxString::FromAscii("Communication error"));
01103 
01104    }
01105 
01106 
01107    //m_text_object->SetLabel(wxString::FromAscii("object: none"));
01108    //m_text_action_->SetLabel(wxString::FromAscii("action: none"));
01109    setTask("None.");
01110 
01111 
01112    arm_nav_called_ = false;
01113    setControlsToDefaultState();
01114 
01115 }
01116 
01117 void CArmManipulationControls::setTask(std::string text) {
01118 
01119         m_text_task_->Clear();
01120         m_text_task_->AppendText(wxString::FromAscii(text.c_str()));
01121 
01122 }
01123 
01124 void CArmManipulationControls::OnFailed(wxCommandEvent& event)
01125 {
01126    ROS_ERROR("Manual arm manipulation task failed");
01127 
01128    ArmNavFailed srv;
01129 
01130    if ( ros::service::exists(SRV_FAILED,true) && ros::service::call(SRV_FAILED,srv) ) {
01131 
01132        m_text_status->SetLabel(wxString::FromAscii("status: Failed :-("));
01133        //m_text_object->SetLabel(wxString::FromAscii("object: none"));
01134        //m_text_action_->SetLabel(wxString::FromAscii("action: none"));
01135        setTask("None.");
01136 
01137    } else {
01138 
01139      ROS_ERROR("failed when calling arm_nav_failed service");
01140      m_text_status->SetLabel(wxString::FromAscii("Communication error"));
01141 
01142 
01143    }
01144 
01145    arm_nav_called_ = false;
01146    setControlsToDefaultState();
01147 
01148 }
01149 
01150 void CArmManipulationControls::OnRepeat(wxCommandEvent& event)
01151 {
01152    ROS_ERROR("Request for repeat of manual arm navigation task");
01153 
01154    ArmNavRepeat srv;
01155 
01156    if ( ros::service::exists(SRV_REPEAT,true) && ros::service::call(SRV_REPEAT,srv) ) {
01157 
01158        m_text_status->SetLabel(wxString::FromAscii("status: Repeating action..."));
01159        //m_text_object->SetLabel(wxString::FromAscii("object: none"));
01160        //m_text_action_->SetLabel(wxString::FromAscii("action: none"));
01161        setTask("None.");
01162 
01163    } else {
01164 
01165      ROS_ERROR("failed when calling arm_nav_repeat service");
01166      m_text_status->SetLabel(wxString::FromAscii("Communication error"));
01167 
01168    }
01169 
01170    arm_nav_called_ = false;
01171    setControlsToDefaultState();
01172 
01173 }
01174 
01175 
01176 bool CArmManipulationControls::nav_start(ArmNavStart::Request &req, ArmNavStart::Response &res) {
01177 
01178   char str[120];
01179 
01180   action_ = req.action;
01181   object_name_ = req.object_name;
01182   allow_repeat_ = req.allow_repeat;
01183   traj_executed_ = 0;
01184 
01185   if (allow_repeat_) ROS_INFO("Received request for action. Repeated detection is allowed.");
01186   else ROS_INFO("Received request for action. Repeated detection is NOT allowed.");
01187 
01188 
01189   if (req.object_name!="") {
01190 
01191     snprintf(str,120,"%s (%s)",req.action.c_str(),req.object_name.c_str());
01192 
01193     std::string tmp;
01194     tmp = std::string("object_name: ") + object_name_;
01195     //m_text_object->SetLabel(wxString::FromAscii(tmp.c_str()));
01196 
01197   } else {
01198 
01199           snprintf(str,120,"%s",req.action.c_str());
01200 
01201           std::string tmp;
01202           tmp = std::string("object_name: none");
01203           //m_text_object->SetLabel(wxString::FromAscii(tmp.c_str()));
01204 
01205   }
01206 
01207   std::string tmp;
01208   tmp = std::string("action: ") + req.action;
01209   //m_text_action_->SetLabel(wxString::FromAscii(tmp.c_str()));
01210   setTask(req.action);
01211 
01212 
01213   wxMessageBox(wxString::FromAscii(str), wxString::FromAscii("Assisted arm navigation"), wxOK, parent_,-1,-1);
01214 
01215   setButton("new",true);
01216   arm_nav_called_ = true;
01217 
01218 
01219   res.completed = true;
01220 
01221   return true;
01222 
01223 };
01224 
01225 
01226 void CArmManipulationControls::OnStepBack(wxCommandEvent& event) {
01227 
01228   ROS_INFO("Undoing last change in IM pose");
01229 
01230   setButton("undo",false);
01231 
01232    ArmNavStep srv;
01233 
01234    srv.request.undo = true;
01235 
01236    if ( ros::service::exists(SRV_STEP,true) && ros::service::call(SRV_STEP,srv) ) {
01237 
01238      if (srv.response.completed) {
01239 
01240        m_text_status->SetLabel(wxString::FromAscii("status: Undo was successful."));
01241 
01242      } else {
01243 
01244        m_text_status->SetLabel(wxString::FromAscii("status: Error on undoing."));
01245 
01246      }
01247 
01248    } else {
01249 
01250      ROS_ERROR("failed when calling arm_nav_step service");
01251      m_text_status->SetLabel(wxString::FromAscii("status: Communication error"));
01252 
01253    }
01254 
01255    setButton("undo",true);
01256 
01257 }
01258 
01259 
01260 
01261 /*void CArmManipulationControls::OnMoveRel(wxCommandEvent& event) {
01262 
01263   int id = event.GetId();
01264 
01265   float step = 0.01; // 10 mm
01266 
01267   geometry_msgs::Point move;
01268 
01269   move.x = 0.0;
01270   move.y = 0.0;
01271   move.z = 0.0;
01272 
01273   ArmNavMovePalmLinkRel srv;
01274 
01275 
01276   switch (id) {
01277 
01278     case ID_BUTTON_MLEFT:    move.y -= step; break;
01279     case ID_BUTTON_MRIGHT:   move.y += step; break;
01280 
01281     case ID_BUTTON_MDOWN:    move.z -= step; break;
01282     case ID_BUTTON_MUP:      move.z += step; break;
01283 
01284     case ID_BUTTON_MBACK:    move.x -= step; break;
01285     case ID_BUTTON_MFORW:    move.x += step; break;
01286 
01287   }
01288 
01289   srv.request.relative_shift = move;
01290 
01291   if ( ros::service::exists(SRV_MOVE_PALM_LINK_REL,true) && ros::service::call(SRV_MOVE_PALM_LINK_REL,srv) ) {
01292 
01293       if (srv.response.completed) {
01294 
01295         m_text_status->SetLabel(wxString::FromAscii("status: Gripper shifted."));
01296 
01297       } else {
01298 
01299         m_text_status->SetLabel(wxString::FromAscii("status: Error on shifting IM."));
01300 
01301       }
01302 
01303     } else {
01304 
01305       ROS_ERROR("failed when calling %s service",SRV_MOVE_PALM_LINK_REL.c_str());
01306       m_text_status->SetLabel(wxString::FromAscii("status: Communication error"));
01307 
01308     }
01309 
01310 
01311 }*/
01312 
01313 
01314 void CArmManipulationControls::OnStopTraj(wxCommandEvent& event) {
01315 
01316   ROS_INFO("Stopping execution of trajectory");
01317 
01318    ArmNavStop srv;
01319 
01320    if ( ros::service::exists(SRV_STOP,true) && ros::service::call(SRV_STOP,srv) ) {
01321 
01322        m_text_status->SetLabel(wxString::FromAscii("status: Execution canceled."));
01323 
01324    } else {
01325 
01326      ROS_ERROR("failed when calling arm_nav_stop service");
01327      m_text_status->SetLabel(wxString::FromAscii("status: Communication error"));
01328 
01329    }
01330 
01331    m_pos_lock_->Set3StateValue(wxCHK_UNDETERMINED);
01332    m_or_lock_->Set3StateValue(wxCHK_UNDETERMINED);
01333 
01334    //planning_started_ = false;
01335 
01336    setControlsToDefaultState();
01337 
01338    if (arm_nav_called_) {
01339 
01340                  if (allow_repeat_) buttons_["repeat"]->Enable(true);
01341                  buttons_["failed"]->Enable(true);
01342 
01343          }
01344 
01345 }
01346 
01347 // to be called from GUI callbacks only
01348 bool CArmManipulationControls::checkService(std::string srv) {
01349 
01350         if (!ros::service::exists(srv,true)) {
01351 
01352                 ROS_ERROR("Service %s is not ready.",srv.c_str());
01353                 m_text_status->SetLabel(wxString::FromAscii("status: communication error"));
01354                 setControlsToDefaultState();
01355                 return false;
01356 
01357         } else return true;
01358 
01359 }
01360 
01361 void CArmManipulationControls::OnChoice(wxCommandEvent& event) {
01362 
01363         unsigned int choice;
01364         if (presets_choice_->GetSelection() > 0) choice = (presets_choice_->GetSelection()) - 1;
01365         else return;
01366 
01367         if (presets_[choice].relative) {
01368 
01369                 if (!checkService(SRV_MOVE_PALM_LINK_REL)) return;
01370 
01371                 boost::posix_time::time_duration td = boost::posix_time::milliseconds(100);
01372 
01374                   if (t_move_rel.timed_join(td)) {
01375 
01376                     disableControls();
01377                     m_text_status->SetLabel(wxT("status: Moving to predefined position."));
01378 
01379                     t_move_rel = boost::thread(&CArmManipulationControls::MoveRel,this);
01380 
01381                   } else ROS_DEBUG("We have to wait until MOVE_REL thread finishes.");
01382 
01383                 //MoveRel(choice);
01384 
01385         } else {
01386 
01387 
01388                 if (!checkService(SRV_MOVE_PALM_LINK)) return;
01389 
01390                 boost::posix_time::time_duration td = boost::posix_time::milliseconds(100);
01391 
01393                   if (t_move_abs.timed_join(td)) {
01394 
01395                         disableControls();
01396                         m_text_status->SetLabel(wxT("status: Moving to predefined position."));
01397 
01398                         t_move_abs = boost::thread(&CArmManipulationControls::MoveAbs,this);
01399 
01400                   } else ROS_DEBUG("We have to wait until MOVE_ABS thread finishes.");
01401 
01402                 //MoveAbs(choice);
01403 
01404         }
01405 
01406 
01407 }
01408 
01409 void CArmManipulationControls::MoveAbs() {
01410 
01411 
01412         unsigned int choice;
01413         wxMutexGuiEnter();
01414         if (presets_choice_->GetSelection() > 0) choice = (presets_choice_->GetSelection()) - 1;
01415         else {
01416 
01417                 wxMutexGuiLeave();
01418                 return;
01419         }
01420         wxMutexGuiLeave();
01421 
01422         geometry_msgs::PoseStamped npose;
01423 
01424         npose.header.stamp = ros::Time::now();
01425         npose.header.frame_id = "/base_link"; // TODO read it from param !!!!!!!!!!!!!!!!!!!!
01426 
01427         npose.pose = presets_[choice].pose;
01428 
01429   ArmNavMovePalmLink srv;
01430 
01431   srv.request.sdh_palm_link_pose = npose;
01432 
01433   bool ret = false;;
01434 
01435   if ( ros::service::call(SRV_MOVE_PALM_LINK,srv) ) {
01436 
01437           if (!srv.response.completed) ret = false;
01438           else ret = true;
01439 
01440         } else {
01441 
01442                 ROS_ERROR("failed when called %s service",SRV_MOVE_PALM_LINK.c_str());
01443                 ret = false;
01444 
01445         }
01446 
01447 
01448   if (!ret) {
01449 
01450           wxMutexGuiEnter();
01451           m_text_status->SetLabel(wxString::FromAscii("status: Error on moving IM to new position."));
01452           setControlsToDefaultState();
01453           wxMutexGuiLeave();
01454           return;
01455 
01456   } else {
01457 
01458           wxMutexGuiEnter();
01459           m_text_status->SetLabel(wxString::FromAscii("status: Gripper is at new position."));
01460 
01461           setButton("new",true); // cancel
01462           setButton("plan",true);
01463           setButton("undo",true);
01464 
01465           presets_choice_->Enable(true);
01466           presets_choice_->Select(0);
01467 
01468           if (arm_nav_called_) {
01469 
01470                  if (allow_repeat_) buttons_["repeat"]->Enable(true);
01471                  buttons_["failed"]->Enable(true);
01472 
01473            }
01474 
01475           wxMutexGuiLeave();
01476           return;
01477 
01478 
01479   }
01480 
01481 }
01482 
01483 void CArmManipulationControls::MoveRel() {
01484 
01485         unsigned int choice;
01486         wxMutexGuiEnter();
01487         if (presets_choice_->GetSelection() > 0) choice = (presets_choice_->GetSelection()) - 1;
01488         else {
01489 
01490                 wxMutexGuiLeave();
01491                 return;
01492         }
01493         wxMutexGuiLeave();
01494 
01495   geometry_msgs::Pose move;
01496   move = presets_[choice].pose;
01497 
01498   ArmNavMovePalmLinkRel srv;
01499 
01500   srv.request.relative_movement = move;
01501 
01502   bool ret = false;
01503   bool err = false;
01504 
01505   if ( ros::service::call(SRV_MOVE_PALM_LINK_REL,srv) ) {
01506 
01507           if (srv.response.completed) ret = true;
01508           else ret = false;
01509 
01510   } else {
01511 
01512           ROS_ERROR("failed when called %s service",SRV_MOVE_PALM_LINK_REL.c_str());
01513           err = true;
01514 
01515         }
01516 
01517 
01518   if (!ret || err) {
01519 
01520           wxMutexGuiEnter();
01521 
01522           if (err) {
01523 
01524                   m_text_status->SetLabel(wxString::FromAscii("status: Comunnication error."));
01525                   setControlsToDefaultState();
01526 
01527           }
01528 
01529           if (!ret) {
01530 
01531                   m_text_status->SetLabel(wxString::FromAscii("status: Error on moving IM to new position."));
01532 
01533                   setButton("new",true); // cancel
01534                   setButton("plan",true);
01535                   setButton("undo",true);
01536 
01537                   presets_choice_->Enable(true);
01538                   presets_choice_->Select(0);
01539 
01540                   if (arm_nav_called_) {
01541 
01542                          if (allow_repeat_) buttons_["repeat"]->Enable(true);
01543                          buttons_["failed"]->Enable(true);
01544 
01545                    }
01546 
01547           }
01548 
01549           wxMutexGuiLeave();
01550           return;
01551 
01552   } else {
01553 
01554           wxMutexGuiEnter();
01555           m_text_status->SetLabel(wxString::FromAscii("status: Gripper is at new position."));
01556 
01557           setButton("new",true); // cancel
01558           setButton("plan",true);
01559           setButton("undo",true);
01560 
01561           presets_choice_->Enable(true);
01562           presets_choice_->Select(0);
01563 
01564           if (arm_nav_called_) {
01565 
01566                  if (allow_repeat_) buttons_["repeat"]->Enable(true);
01567                  buttons_["failed"]->Enable(true);
01568 
01569            }
01570 
01571           wxMutexGuiLeave();
01572           return;
01573 
01574   }
01575 
01576 
01577 }
01578 
01580 
01581 
01583 
01584 
01585 BEGIN_EVENT_TABLE(CArmManipulationControls, wxPanel)
01586     EVT_BUTTON(ID_BUTTON_NEW,  CArmManipulationControls::OnNew)
01587     EVT_BUTTON(ID_BUTTON_PLAN,  CArmManipulationControls::OnPlan)
01588     EVT_BUTTON(ID_BUTTON_EXECUTE,  CArmManipulationControls::OnExecute)
01589     EVT_BUTTON(ID_BUTTON_SUCCESS,  CArmManipulationControls::OnSuccess)
01590     EVT_BUTTON(ID_BUTTON_FAILED,  CArmManipulationControls::OnFailed)
01591     //EVT_TOGGLEBUTTON(ID_BUTTON_SWITCH,  CArmManipulationControls::OnSwitch)
01592     EVT_CHECKBOX(ID_CHECKBOX_ACO,CArmManipulationControls::OnSwitch)
01593     EVT_BUTTON(ID_BUTTON_REPEAT,  CArmManipulationControls::OnRepeat)
01594     EVT_BUTTON(ID_BUTTON_UNDO,  CArmManipulationControls::OnStepBack)
01595     /*EVT_BUTTON(ID_BUTTON_MLEFT,  CArmManipulationControls::OnMoveRel)
01596     EVT_BUTTON(ID_BUTTON_MRIGHT,  CArmManipulationControls::OnMoveRel)
01597     EVT_BUTTON(ID_BUTTON_MUP,  CArmManipulationControls::OnMoveRel)
01598     EVT_BUTTON(ID_BUTTON_MDOWN,  CArmManipulationControls::OnMoveRel)
01599     EVT_BUTTON(ID_BUTTON_MFORW,  CArmManipulationControls::OnMoveRel)
01600     EVT_BUTTON(ID_BUTTON_MBACK,  CArmManipulationControls::OnMoveRel)*/
01601     EVT_BUTTON(ID_BUTTON_STOP_TRAJ,  CArmManipulationControls::OnStopTraj)
01602     /*EVT_BUTTON(ID_BUTTON_LIFT,  CArmManipulationControls::OnLift)
01603     EVT_BUTTON(ID_BUTTON_MOVE_TO_HOLD,  CArmManipulationControls::OnHold)*/
01604     EVT_CHOICE(ID_CHOICE,CArmManipulationControls::OnChoice)
01605 END_EVENT_TABLE()
01606
arm_navigation_controls.cpp
