robot_command_interface.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 
3 #include "rviz/config.h"
5 #include "ros/time.h"
6 #include <ros/package.h>
7 #include <boost/format.hpp>
8 #include <exception>
9 #include <std_srvs/Empty.h>
10 
11 namespace jsk_rviz_plugins
12 {
13 
14  // Exception class
15  class RobotCommandParseException: public std::runtime_error
16  {
17  public:
18  RobotCommandParseException(const std::string& text): std::runtime_error(text) {}
19  };
20 
22  : rviz::Panel( parent )
23  {
25  signal_mapper_ = new QSignalMapper(this);
26  ros::NodeHandle nh("~");
27  QHBoxLayout* layout = new QHBoxLayout();
28  // Parse yaml file from parameter
29  if (nh.hasParam("robot_command_buttons")) {
30  try {
31  XmlRpc::XmlRpcValue robot_command_buttons_xmlrpc;
32  nh.param("robot_command_buttons", robot_command_buttons_xmlrpc, robot_command_buttons_xmlrpc);
33  if (robot_command_buttons_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray) {
34  throw RobotCommandParseException("~robot_comamnd_buttons should be an array");
35  }
36  else {
37  for (size_t i = 0; i < robot_command_buttons_xmlrpc.size(); i++) {
38  XmlRpc::XmlRpcValue button_xmlrpc = robot_command_buttons_xmlrpc[i];
39  if (button_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
40  throw RobotCommandParseException("element of ~robot_comamnd_buttons should be an struct");
41  }
42  else {
43  std::string name;
44  QToolButton* button = new QToolButton();
45  //button->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
46  button->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
47 
48  if (button_xmlrpc.hasMember("name")) {
49  name = (std::string)button_xmlrpc["name"];
50  }
51  else {
52  throw RobotCommandParseException("element of ~robot_comamnd_buttons should have name field");
53  }
54  button->setText(QString(name.c_str()));
55  if (button_xmlrpc.hasMember("icon")) {
56  // TODO: resolve path
57  std::string icon;
58  icon = (std::string)button_xmlrpc["icon"];
59  if (icon.find("package://") == 0) {
60  icon.erase(0, strlen("package://"));
61  size_t package_end = icon.find("/");
62  std::string package = icon.substr(0, package_end);
63  icon.erase(0, package_end);
64  std::string package_path;
65  package_path = ros::package::getPath(package);
66  icon = package_path + icon;
67  }
68  button->setIcon(QIcon(QPixmap(QString(icon.c_str()))));
69  button->setIconSize(QSize(80, 80));
70  button->setToolButtonStyle(Qt::ToolButtonTextUnderIcon);
71  }
72  std::string type;
73  if (button_xmlrpc.hasMember("type")) {
74  type = (std::string)button_xmlrpc["type"];
75  }
76  if (type == "euscommand") {
77  if (button_xmlrpc.hasMember("command")) {
78  euscommand_mapping_[i] = (std::string)button_xmlrpc["command"];
79  button->setToolTip(euscommand_mapping_[i].c_str());
80  }
81  else {
82  throw RobotCommandParseException("type: euscommand requires command field");
83  }
84  }
85  else if (type == "emptysrv") {
86  if (button_xmlrpc.hasMember("srv")) {
87  emptyservice_mapping_[i] = (std::string)button_xmlrpc["srv"];
88  button->setToolTip(emptyservice_mapping_[i].c_str());
89  }
90  else {
91  throw RobotCommandParseException("type: emptysrv requires srv field");
92  }
93  }
94  else {
95  throw RobotCommandParseException("type field is required");
96  }
97  // connect
98  connect(button, SIGNAL(clicked()), signal_mapper_, SLOT(map()));
99  signal_mapper_->setMapping(button, i);
100  layout->addWidget(button);
101  }
102  }
103  }
104  }
105  catch (RobotCommandParseException& e) {
106  popupDialog((boost::format("Malformed ~robot_command_buttons parameter.\n"
107  "%s\n"
108  "See package://jsk_rviz_plugins/config/default_robot_command.yaml")
109  % e.what()).str().c_str());
110  }
111  }
112  else {
113  popupDialog("You need to specify ~robot_command_buttons parameter.\n"
114  "See package://jsk_rviz_plugins/launch/robot_command_interface_sample.launch");
115  }
116  layout->addStretch();
117  connect(signal_mapper_, SIGNAL(mapped(int)), this, SLOT(buttonCallback(int)));
118  // QToolButton* button = new QToolButton();
119 
120  // // button->setPopupMode(QToolButton::MenuButtonPopup);
121  // button->setToolButtonStyle(Qt::ToolButtonTextUnderIcon);
122  // button->setIcon(QIcon(QPixmap(QString("/home/garaemon/ros/hydro/src/jsk-ros-pkg/jsk_visualization/jsk_rviz_plugins/icons/stop-imp.png"))));
123 
124  // button->setText("Hello worpld");
125  // layout->addWidget(button);
126  this->setLayout(layout);
127  }
128 
129  bool RobotCommandInterfaceAction::callRequestEusCommand(const std::string& command){
130  ros::ServiceClient client = nh_.serviceClient<jsk_rviz_plugins::EusCommand>("/eus_command", true);
131  jsk_rviz_plugins::EusCommand srv;
132  srv.request.command = command;
133  return client.call(srv);
134  }
135 
137  {
138  ROS_INFO("buttonCallback(%d)", i);
139  if (euscommand_mapping_.find(i) != euscommand_mapping_.end()) {
141  popupDialog((boost::format("Failed to call %s") % euscommand_mapping_[i]).str().c_str());
142  }
143  }
144  else if (emptyservice_mapping_.find(i) != emptyservice_mapping_.end()) {
145  std_srvs::Empty emp;
147  popupDialog((boost::format("Failed to call %s") % emptyservice_mapping_[i]).str().c_str());
148  }
149  }
150  else {
151  popupDialog((boost::format("Failed to find corresponding command for %d") % i).str().c_str());
152  }
153  }
154 
156  {
157  QMessageBox msg_box;
158  msg_box.setText("Unexpected error");
159  msg_box.setText(QString(text.c_str()));
160  msg_box.exec();
161  }
162 }
163 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool callRequestEusCommand(const std::string &command)
string package
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
bool call(const std::string &service_name, MReq &req, MRes &res)
bool call(MReq &req, MRes &res)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Type const & getType() const
#define ROS_INFO(...)
bool hasParam(const std::string &key) const
ROSLIB_DECL std::string getPath(const std::string &package_name)
string str
bool hasMember(const std::string &name) const


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58