robot_command_interface.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 
00003 #include "rviz/config.h"
00004 #include "robot_command_interface.h"
00005 #include "ros/time.h"
00006 #include <ros/package.h>
00007 #include <boost/format.hpp>
00008 #include <exception>
00009 #include <std_srvs/Empty.h>
00010 
00011 namespace jsk_rviz_plugins
00012 {
00013 
00014   // Exception class
00015   class RobotCommandParseException: public std::runtime_error
00016   {
00017   public:
00018     RobotCommandParseException(const std::string& text): std::runtime_error(text) {}
00019   };
00020 
00021   RobotCommandInterfaceAction::RobotCommandInterfaceAction( QWidget* parent )
00022     : rviz::Panel( parent )
00023   {
00024     resource_retriever::Retriever r;
00025     signal_mapper_ = new QSignalMapper(this);
00026     ros::NodeHandle nh("~");
00027     QHBoxLayout* layout = new QHBoxLayout();
00028     // Parse yaml file from parameter
00029     if (nh.hasParam("robot_command_buttons")) {
00030       try {
00031         XmlRpc::XmlRpcValue robot_command_buttons_xmlrpc;
00032         nh.param("robot_command_buttons", robot_command_buttons_xmlrpc, robot_command_buttons_xmlrpc);
00033         if (robot_command_buttons_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00034           throw RobotCommandParseException("~robot_comamnd_buttons should be an array");
00035         }
00036         else {
00037           for (size_t i = 0; i < robot_command_buttons_xmlrpc.size(); i++) {
00038             XmlRpc::XmlRpcValue button_xmlrpc = robot_command_buttons_xmlrpc[i];
00039             if (button_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
00040               throw RobotCommandParseException("element of ~robot_comamnd_buttons should be an struct");
00041             }
00042             else {
00043               std::string name;
00044               QToolButton* button = new QToolButton();
00045               //button->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
00046               button->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
00047 
00048               if (button_xmlrpc.hasMember("name")) {
00049                 name = (std::string)button_xmlrpc["name"];
00050               }
00051               else {
00052                 throw RobotCommandParseException("element of ~robot_comamnd_buttons should have name field");
00053               }
00054               button->setText(QString(name.c_str()));
00055               if (button_xmlrpc.hasMember("icon")) {
00056                 // TODO: resolve path
00057                 std::string icon;
00058                 icon = (std::string)button_xmlrpc["icon"];
00059                 if (icon.find("package://") == 0) {
00060                   icon.erase(0, strlen("package://"));
00061                   size_t package_end = icon.find("/");
00062                   std::string package = icon.substr(0, package_end);
00063                   icon.erase(0, package_end);
00064                   std::string package_path;
00065                   package_path = ros::package::getPath(package);
00066                   icon = package_path + icon;
00067                 }
00068                 button->setIcon(QIcon(QPixmap(QString(icon.c_str()))));
00069                 button->setIconSize(QSize(80, 80));
00070                 button->setToolButtonStyle(Qt::ToolButtonTextUnderIcon);
00071               }
00072               std::string type;
00073               if (button_xmlrpc.hasMember("type")) {
00074                 type = (std::string)button_xmlrpc["type"];
00075               }
00076               if (type == "euscommand") {
00077                 if (button_xmlrpc.hasMember("command")) {
00078                   euscommand_mapping_[i] = (std::string)button_xmlrpc["command"];
00079                   button->setToolTip(euscommand_mapping_[i].c_str());
00080                 }
00081                 else {
00082                   throw RobotCommandParseException("type: euscommand requires command field");
00083                 }
00084               }
00085               else if (type == "emptysrv") {
00086                 if (button_xmlrpc.hasMember("srv")) {
00087                   emptyservice_mapping_[i] = (std::string)button_xmlrpc["srv"];
00088                   button->setToolTip(emptyservice_mapping_[i].c_str());
00089                 }
00090                 else {
00091                   throw RobotCommandParseException("type: emptysrv requires srv field");
00092                 }
00093               }
00094               else {
00095                 throw RobotCommandParseException("type field is required");
00096               }
00097               // connect
00098               connect(button, SIGNAL(clicked()), signal_mapper_, SLOT(map()));
00099               signal_mapper_->setMapping(button, i);
00100               layout->addWidget(button);
00101             }
00102           }
00103         }
00104       }
00105       catch (RobotCommandParseException& e) {
00106         popupDialog((boost::format("Malformed ~robot_command_buttons parameter.\n"
00107                                   "%s\n"
00108                                   "See package://jsk_rviz_plugins/config/default_robot_command.yaml")
00109                      % e.what()).str().c_str());
00110       }
00111     }
00112     else {
00113       popupDialog("You need to specify ~robot_command_buttons parameter.\n"
00114                   "See package://jsk_rviz_plugins/launch/robot_command_interface_sample.launch");
00115     }
00116     layout->addStretch();
00117     connect(signal_mapper_, SIGNAL(mapped(int)), this, SLOT(buttonCallback(int)));
00118     // QToolButton* button = new QToolButton();
00119     
00120     // // button->setPopupMode(QToolButton::MenuButtonPopup);
00121     // button->setToolButtonStyle(Qt::ToolButtonTextUnderIcon);
00122     // button->setIcon(QIcon(QPixmap(QString("/home/garaemon/ros/hydro/src/jsk-ros-pkg/jsk_visualization/jsk_rviz_plugins/icons/stop-imp.png"))));
00123     
00124     // button->setText("Hello worpld");
00125     // layout->addWidget(button);
00126     this->setLayout(layout);
00127   }
00128 
00129   bool RobotCommandInterfaceAction::callRequestEusCommand(const std::string& command){
00130     ros::ServiceClient client = nh_.serviceClient<jsk_rviz_plugins::EusCommand>("/eus_command", true);
00131     jsk_rviz_plugins::EusCommand srv;
00132     srv.request.command = command;
00133     return client.call(srv);
00134   }
00135 
00136   void RobotCommandInterfaceAction::buttonCallback(int i)
00137   {
00138     ROS_INFO("buttonCallback(%d)", i);
00139     if (euscommand_mapping_.find(i) != euscommand_mapping_.end()) {
00140       if(!callRequestEusCommand(euscommand_mapping_[i])) {
00141         popupDialog((boost::format("Failed to call %s") % euscommand_mapping_[i]).str().c_str());
00142       }
00143     }
00144     else if (emptyservice_mapping_.find(i) != emptyservice_mapping_.end()) {
00145       std_srvs::Empty emp;
00146       if (!ros::service::call(emptyservice_mapping_[i], emp)) {
00147         popupDialog((boost::format("Failed to call %s") % emptyservice_mapping_[i]).str().c_str());
00148       }
00149     }
00150     else {
00151       popupDialog((boost::format("Failed to find corresponding command for %d") % i).str().c_str());
00152     }
00153   }
00154 
00155   void RobotCommandInterfaceAction::popupDialog(const std::string& text)
00156   {
00157     QMessageBox msg_box;
00158     msg_box.setText("Unexpected error");
00159     msg_box.setText(QString(text.c_str()));
00160     msg_box.exec();
00161   }
00162 }
00163 
00164 #include <pluginlib/class_list_macros.h>
00165 PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::RobotCommandInterfaceAction, rviz::Panel )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03