cob_command_gui_rviz: cob_command_gui_rviz: command_gui_rviz

00001 #include "cob_command_gui_rviz/command_gui_rviz_panel.h"
00002 
00003 #include <wx/event.h>
00004 #include <XmlRpcValue.h>
00005 
00006 namespace rviz
00007 {       
00008         
00009         //constructor
00010         CommandGuiRvizPanel::CommandGuiRvizPanel(wxWindow *parent, const wxString& title):
00011         wxPanel(parent, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, title),
00012         planning_enabled(false),
00013         base_diff_enabled(false)
00014         {               
00015                 //try to load button parameters from ROS parameter server
00016                 if(false != Getparams())
00017                 {
00018                         //create graphical user interface
00019                         Creategui();            
00020                 
00021                         //initialze the action client
00022                         action_client_ = new actionlib::SimpleActionClient<cob_script_server::ScriptAction>("cob_command_gui_rviz", true);              
00023                 }
00024                 else
00025                 {
00026                         //no parameter available -> Error message
00027                         statuslabel_ = new wxStaticText(this, wxID_ANY, wxT("\n\tstatus: Error! - parameter does not exist on ROS parameter server! \n\n"));    
00028                 }       
00029         }
00030 
00031         //destructor
00032         CommandGuiRvizPanel::~CommandGuiRvizPanel() 
00033         {
00034         }
00035         
00036         //try to load button parameters from ROS parameter server
00037         bool CommandGuiRvizPanel::Getparams()
00038         {       
00039                 std::string param_prefix = "/control_buttons";          
00040                 
00041                 if(false == (ros::param::has(param_prefix)))
00042                 {
00043                         ROS_DEBUG_ONCE("parameter '%s' does not exist on ROS parameter server, aborting...\n", param_prefix.c_str());                   
00044                         return false;                   
00045                 }
00046                 
00047                 XmlRpc::XmlRpcValue group_param;
00048                 ros::param::get(param_prefix, group_param);
00049                                 
00050                 XmlRpc::XmlRpcValue current_group;
00051                 XmlRpc::XmlRpcValue buttons;
00052                 XmlRpc::XmlRpcValue group_name;
00053                 XmlRpc::XmlRpcValue component_name;
00054                 
00055                 std::string groupkey = "groupx";
00056 
00057                 //create groups
00058                 for(int i = 0; i < group_param.size(); i++)
00059                 {       
00060                         groupkey[5] = (i+'0'+1);  
00061                          
00062                         current_group = group_param[groupkey];
00063                          
00064                         buttons  = current_group["buttons"];
00065                         group_name = current_group["group_name"];
00066                         component_name  = current_group["component_name"];      
00067                         static int id = 101;                                                
00068                          
00069                         Buttonlist current_buttons;     
00070                                          
00071                         // create buttons in group               
00072                         for(int j=0; j < buttons.size(); j++)
00073                         {
00074                                 if(buttons[j][1] == "move")
00075                                 {
00076                                         current_buttons.push_back(Descripe_button(id, buttons[j], component_name));
00077                                 }                               
00078                                 else if(buttons[j][1] == "move_base_rel")
00079                                 {
00080                                         current_buttons.push_back(Descripe_button(id, buttons[j], component_name));
00081                                 }
00082                                 else if(buttons[j][1] == "trigger")
00083                                 {
00084                                         current_buttons.push_back(Descripe_button(id, buttons[j], component_name));
00085                                         
00086                                         if(buttons[j][2] == "stop")
00087                                         {
00088                                                 stop_buttons_.push_back(component_name);
00089                                         }
00090                                         else if(buttons[j][2] == "init")
00091                                         {
00092                                                 init_buttons_.push_back(component_name);
00093                                         }
00094                                         else if(buttons[j][2] == "recover")
00095                                         {
00096                                                 recover_buttons_.push_back(component_name);
00097                                         }
00098                                         
00099                                 }
00100                                 else if(buttons[j][1] == "stop")
00101                                 {
00102                                         current_buttons.push_back(Descripe_button(id, buttons[j], component_name));
00103                                         stop_buttons_.push_back(component_name);
00104                                 }
00105                                 else if(buttons[j][1] == "mode")
00106                                 {
00107                                         current_buttons.push_back(Descripe_button(id, buttons[j], component_name));
00108                                 }
00109                                 else 
00110                                 {
00111                                         std::string fail = buttons[j][1];
00112                                         ROS_DEBUG_ONCE("function %s not known to rviz_movement_buttons panel", fail.c_str());
00113                                 }               
00114                                                 
00115                                 id++;                                                                                            
00116                         }       
00117                         
00118                         
00119                         
00120                         //add nav buttons (optional)            
00121                         if("base" == TostlString(component_name))
00122                         {
00123                                 param_prefix = "/nav_buttons";
00124                                 
00125                                 if(false != (ros::param::has(param_prefix)))
00126                                 {
00127                                         XmlRpc::XmlRpcValue nav_buttons_param;
00128                                         XmlRpc::XmlRpcValue nav_buttons;
00129                                         
00130                                         ros::param::get(param_prefix, nav_buttons_param);
00131                                         
00132                                         nav_buttons = nav_buttons_param["buttons"];
00133                                         
00134                                         for(int k=0; k < nav_buttons.size(); k++)
00135                                         {
00136                                                 current_buttons.push_back(Descripe_button(id, nav_buttons[k], component_name)); 
00137                                                 id++;
00138                                         }
00139                                 } 
00140                                 else
00141                                 {
00142                                         ROS_DEBUG_ONCE("parameter %s does not exist on ROS parameter server, no nav buttons will be available!\n", param_prefix.c_str());               
00143                                 }
00144                         }
00145                         
00146                         groups_.push_back(std::pair<std::string, Buttonlist>(TostlString(group_name),current_buttons));
00147                 }                               
00148                 
00149                 //uniquify lists
00150                 stop_buttons_.unique();
00151                 init_buttons_.unique();
00152                 recover_buttons_.unique();
00153                 
00154                 return true;
00155         }       
00156         
00157         //creates the graphical user interface
00158         void CommandGuiRvizPanel::Creategui()
00159         {       
00160                 //main sizer
00161                 wxSizer *mainsizer = new wxBoxSizer(wxHORIZONTAL);
00162                 
00163                 //create general box
00164                 sizers_.push_back(CreatesbGeneral());
00165                 
00166                 //create boxes and buttons inside them
00167                 for(Grouplist::const_iterator group_ci = groups_.begin(); group_ci != groups_.end(); group_ci++)
00168                 {
00169                         wxSizer *vsizer = new wxStaticBoxSizer(wxVERTICAL, this, TowxString(group_ci->first));
00170                         
00171                         for(Buttonlist::const_iterator button_ci = (group_ci->second).begin(); button_ci != (group_ci->second).end(); button_ci++)
00172                         {                               
00173                                 vsizer->Add(AddButton(button_ci->id, TowxString(button_ci->button_name)), 0, wxEXPAND);         
00174                         }
00175                         
00176                         sizers_.push_back(vsizer);
00177                 }
00178                 
00179                 //add each groupsizer to the mainsizer
00180                 for(unsigned int i = 0; i < sizers_.size(); i++)
00181                 {
00182                         mainsizer->Add(sizers_[i], 1, wxEXPAND);
00183                 }       
00184                 
00185                 mainsizer->SetSizeHints(this);
00186                 this->SetSizerAndFit(mainsizer);
00187         }
00188         
00189         //creates the 'general' box with predefined widgets
00190         wxSizer* CommandGuiRvizPanel::CreatesbGeneral()
00191         {                       
00192                 //widget ids
00193                 const int genids[5] = {001, 002, 003, 004, 005};                
00194                 
00195                 wxSizer *vsizer = new wxStaticBoxSizer(wxVERTICAL, this, wxT("general"));
00196                 
00197                 statuslabel_    = new wxStaticText(this, wxID_ANY, wxT("\n Status: OK"));
00198                 cbPlanning_             = new wxCheckBox(this, genids[0], wxT("planning"));
00199                 cbBaseDiff_             = new wxCheckBox(this, genids[1], wxT("base diff"));
00200                 
00201                 wxButton *btnstopall    = new wxButton(this, genids[2], wxT("stop all"), wxDefaultPosition, /*wxDefaultSize*/wxSize(30,29), wxBU_EXACTFIT);
00202                 wxButton *btninitall    = new wxButton(this, genids[3], wxT("init all"), wxDefaultPosition, /*wxDefaultSize*/wxSize(30,29), wxBU_EXACTFIT);
00203                 wxButton *btnrecoverall = new wxButton(this, genids[4], wxT("recover all"), wxDefaultPosition, /*wxDefaultSize*/wxSize(30,29), wxBU_EXACTFIT);          
00204                 
00205                 btnstopall->Enable(true);
00206                 btninitall->Enable(true);       
00207                 btnrecoverall->Enable(true);
00208                 
00209                 //connect checkboxes to event handler
00210                 Connect(genids[0], wxEVT_COMMAND_CHECKBOX_CLICKED, wxCommandEventHandler(CommandGuiRvizPanel::planned_toggle));
00211                 Connect(genids[1], wxEVT_COMMAND_CHECKBOX_CLICKED, wxCommandEventHandler(CommandGuiRvizPanel::base_mode_toggle));
00212                 
00213                 //connect buttons to event handler
00214                 Connect(genids[2], wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler(CommandGuiRvizPanel::OnStopAll));
00215                 Connect(genids[3], wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler(CommandGuiRvizPanel::OnInitAll));
00216                 Connect(genids[4], wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler(CommandGuiRvizPanel::OnRecoverAll));
00217 
00218 
00219                 vsizer->Add(statuslabel_, 0, wxEXPAND);
00220                 vsizer->Add(btnstopall, 0, wxEXPAND);
00221                 vsizer->Add(btninitall, 0, wxEXPAND);
00222                 vsizer->Add(btnrecoverall, 0, wxEXPAND);
00223                 
00224                 vsizer->Add(cbPlanning_, 0, wxEXPAND);
00225                 vsizer->Add(cbBaseDiff_, 0, wxEXPAND);  
00226                 
00227                 return vsizer;
00228         }
00229  
00230         //creates a 'Button_Description'
00231         Button_Description CommandGuiRvizPanel::Descripe_button(const int &id, XmlRpc::XmlRpcValue data, const std::string &component_name) const
00232         {
00233                 Button_Description btn;
00234                 
00235                 btn.id = id;
00236                 btn.button_name   = TostlString(data[0]);
00237                 btn.function_name = TostlString(data[1]);
00238                 btn.args.first    = component_name;
00239                 btn.args.second   = TostlString(data[2]);
00240                 
00241         return btn;
00242         }
00243         
00244         //creates a wxButton and connects it to a event-handler
00245         wxButton* CommandGuiRvizPanel::AddButton(const int &id, const wxString &name)
00246         {
00247                 wxButton *but = new wxButton(this, id, name, wxDefaultPosition, wxSize(30,29)/*wxDefaultSize*/, wxBU_EXACTFIT, wxDefaultValidator, name);
00248                 
00249                 but->Enable(true);
00250                 
00251                 Connect(id, wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler(CommandGuiRvizPanel::OnClick));
00252                  
00253                 return but;
00254         }
00255 
00256         //event handler for buttons from the config file on ROS parameter server
00257         void CommandGuiRvizPanel::OnClick(wxCommandEvent& event)
00258         {                               
00259                 Button_Description pressed_button;
00260                 std::string group_name;
00261                 
00262                 pressed_button.id = event.GetId();
00263                 
00264                 //use the button_id to get more information about the pressed button            
00265                 for(Grouplist::const_iterator group_ci = groups_.begin(); group_ci != groups_.end(); group_ci++)
00266                 {
00267                         for(Buttonlist::const_iterator button_ci = group_ci->second.begin(); button_ci != group_ci->second.end(); button_ci++)
00268                         {
00269                                 if((button_ci->id == pressed_button.id))
00270                                 {
00271                                         pressed_button.button_name = button_ci->button_name;
00272                                         pressed_button.function_name = button_ci->function_name;
00273                                         pressed_button.args = button_ci->args;
00274                                         group_name = group_ci->first;
00275                                 }
00276                         }
00277                 }
00278                                 
00279                 ROS_INFO("'%s-%s' clicked", group_name.c_str(), pressed_button.button_name.c_str());
00280                 
00281                 cob_script_server::ScriptGoal goal;
00282                 goal.function_name  = pressed_button.function_name;
00283                 goal.component_name = pressed_button.args.first;
00284                 goal.parameter_name = pressed_button.args.second;
00285                 
00286                 if("arm" == goal.component_name)
00287                 {
00288                         if(false != planning_enabled)
00289                         {
00290                                 goal.mode = "planned";
00291                         }
00292                 }
00293                 
00294                 if("base" == goal.component_name)
00295                 {
00296                         if(false != base_diff_enabled)
00297                         {
00298                                 goal.mode = "diff";
00299                         }       
00300                 }
00301                 
00302                 //fill in goal here
00303                 action_client_->sendGoal(goal);
00304                 action_client_->waitForResult(ros::Duration(1.0));
00305                 if(action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00306                 {
00307                         //set status to 'ok'
00308                         statuslabel_->SetLabel(wxT("\n Status: OK"));
00309                         
00310                         this->GetSizer()->Layout();
00311                 }
00312                 else
00313                 {
00314                         //set status to 'no connection'
00315                         statuslabel_->SetLabel(wxT("\nstatus: \nno connect. to action server!\n\n"));
00316                         
00317                         this->GetSizer()->Layout();
00318                         
00319                         ROS_DEBUG_ONCE("Error! - no connection to action server!\n");
00320                 }
00321                 
00322                 ROS_INFO("current state: %s\n", action_client_->getState().toString().c_str());
00323         }
00324         
00325         //event handler for the 'stop all' button
00326         void CommandGuiRvizPanel::OnStopAll(wxCommandEvent &event)
00327         {
00328                 cob_script_server::ScriptGoal goal;
00329                 bool success = true; 
00330                 
00331                 goal.function_name  = "stop";
00332                 
00333                 ROS_INFO("'stop all' clicked");
00334                 
00335                 for(Stringlist::const_iterator ci = stop_buttons_.begin(); ci != stop_buttons_.end(); ci++)
00336                 {
00337                         goal.component_name = *ci;
00338                         
00339                         action_client_->sendGoal(goal);
00340                         action_client_->waitForResult(ros::Duration(1.0));
00341                     
00342                         if("SUCCEEDED" != action_client_->getState().toString())
00343                         {
00344                                 success = false;
00345                                 ROS_INFO("warning: component %s not properly stopped\n", goal.component_name.c_str());
00346                         }
00347                         
00348                         ROS_INFO("current state: %s\n", action_client_->getState().toString().c_str());
00349                 }
00350                 
00351                 if(false != success)
00352                 { 
00353                         ROS_INFO("all components stopped\n");
00354                         
00355                         statuslabel_->SetLabel(wxT("\n Status: OK"));
00356                         this->GetSizer()->Layout();
00357                 }
00358                 else
00359                 {
00360                         //set status to 'no connection'
00361                         statuslabel_->SetLabel(wxT("\nstatus: \nno connect. to action server!\n\n"));
00362                         
00363                         this->GetSizer()->Layout();
00364                         
00365                         ROS_DEBUG_ONCE("Error! - no connection to action server!\n");
00366                 }
00367         }
00368         
00369         //event handler for the 'init all' button
00370         void CommandGuiRvizPanel::OnInitAll(wxCommandEvent &event)
00371         {
00372                 cob_script_server::ScriptGoal goal;
00373                 bool success = true; 
00374                 
00375                 goal.function_name = "init";
00376                 
00377                 ROS_INFO("'init all' clicked");
00378                 
00379                 for(Stringlist::const_iterator ci = init_buttons_.begin(); ci != init_buttons_.end(); ci++)
00380                 {
00381                         goal.component_name = *ci;
00382                         
00383                         action_client_->sendGoal(goal);
00384                         action_client_->waitForResult(ros::Duration(1.0));
00385                     
00386                         if("SUCCEEDED" != action_client_->getState().toString())
00387                         {
00388                                 success = false;
00389                                 ROS_INFO("warning: component %s not properly initialized\n", goal.component_name.c_str());
00390                         }
00391                         
00392                         ROS_INFO("current state: %s\n", action_client_->getState().toString().c_str());
00393                 }
00394                 
00395                 if(false != success)
00396                 { 
00397                         statuslabel_->SetLabel(wxT("\n Status: OK"));
00398                         this->GetSizer()->Layout();
00399                         
00400                         ROS_INFO("all components initialized\n");
00401                 }
00402                 else
00403                 {
00404                         //set status to 'no connection'
00405                         statuslabel_->SetLabel(wxT("\nstatus: \nno connect. to action server!\n\n"));
00406                         
00407                         this->GetSizer()->Layout();
00408                         
00409                         ROS_DEBUG_ONCE("Error! - no connection to action server!\n");
00410                 }
00411         }
00412         
00413         //event handler for the 'recover all' button
00414         void CommandGuiRvizPanel::OnRecoverAll(wxCommandEvent &event)
00415         {
00416                 cob_script_server::ScriptGoal goal;
00417                 bool success = true; 
00418                 
00419                 goal.function_name = "recover";
00420                 
00421                 ROS_INFO("'recover all' clicked");
00422                 
00423                 for(Stringlist::const_iterator ci = recover_buttons_.begin(); ci != recover_buttons_.end(); ci++)
00424                 {
00425                         goal.component_name = *ci;
00426                         
00427                         action_client_->sendGoal(goal);
00428                         action_client_->waitForResult(ros::Duration(1.0));
00429                         
00430                         if("SUCCEEDED" != action_client_->getState().toString())
00431                         {
00432                                 success = false;
00433                                 ROS_INFO("warning: component %s not properly recovered\n", goal.component_name.c_str());
00434                         }
00435                         
00436                         ROS_INFO("current state: %s\n", action_client_->getState().toString().c_str());
00437                 }
00438                 
00439                 if(false != success)
00440                 { 
00441                         statuslabel_->SetLabel(wxT("\n Status: OK"));
00442                         this->GetSizer()->Layout();
00443                         
00444                         ROS_INFO("all components recovered\n");
00445                 }
00446                 else
00447                 {
00448                         //set status to 'no connection'
00449                         statuslabel_->SetLabel(wxT("\nstatus: \nno connect. to action server!\n\n"));
00450                         
00451                         this->GetSizer()->Layout();
00452                         
00453                         ROS_DEBUG_ONCE("Error! - no connection to action server!\n");
00454                 }
00455         }
00456         
00457         //event handler for the 'planning' checkbox
00458         inline void CommandGuiRvizPanel::planned_toggle(wxCommandEvent &event)
00459         {
00460                 planning_enabled = !planning_enabled;
00461         }
00462         
00463         //event handler for the 'base diff' checkbox
00464         inline void CommandGuiRvizPanel::base_mode_toggle(wxCommandEvent &event)
00465         {
00466                 base_diff_enabled = !base_diff_enabled;
00467         }
00468 
00469         //type casts a std::string into a wxString
00470         inline wxString CommandGuiRvizPanel::TowxString(const std::string &temp) const
00471         {
00472                 return wxString(temp.c_str(), wxConvUTF8);
00473         }
00474         
00475         //type casts a XmlRpcValue of 'typestring' or a wxString into std::string
00476         inline std::string CommandGuiRvizPanel::TostlString(XmlRpc::XmlRpcValue temp) const
00477         {
00478                 return static_cast<std::string>(temp);
00479         }
00480         
00481         //type casts a wxString into std::string
00482         inline std::string CommandGuiRvizPanel::TostlString(wxString temp) const
00483         {
00484                 return std::string(temp.mb_str());
00485         }
00486 
00487 }
command_gui_rviz_panel.cpp
