RSMControls.cpp
Go to the documentation of this file.
1 #include "RSMControls.h"
2 
3 namespace rsm {
4 
6  rviz::Panel(parent), _gui(new Ui::rsm_controls()) {
7  _gui->setupUi(this);
9  connectSlots();
11  _exploration_running = false;
13  _emergency_stop_active = false;
14  _operation_mode = rsm_msgs::OperationMode::STOPPED;
16  getStateInfo();
17 }
18 
20  ros::NodeHandle nh("rsm");
21  _start_stop_exploration_client = nh.serviceClient<std_srvs::SetBool>(
22  "startStopExploration");
23  _set_exploration_mode_client = nh.serviceClient<std_srvs::SetBool>(
24  "setExplorationMode");
25 
27  "startStopWaypointFollowing");
28  _waypoint_reset_client = nh.serviceClient<std_srvs::Trigger>(
29  "resetWaypoints");
31  rsm_msgs::SetWaypointFollowingMode>("setWaypointFollowingMode");
32 
33  _state_info_subscriber = nh.subscribe("stateInfo", 10,
35  _state_info_client = nh.serviceClient<std_srvs::Trigger>("stateInfo");
36  _set_operation_mode_client = nh.serviceClient<rsm_msgs::SetOperationMode>(
37  "setOperationMode");
38  _operation_mode_subcriber = nh.subscribe<rsm_msgs::OperationMode>(
39  "operationMode", 1, &RSMControlPanel::operationModeCallback, this);
40 
41  _set_reverse_mode_client = nh.serviceClient<std_srvs::SetBool>(
42  "setReverseMode");
43  _reverse_mode_subscriber = nh.subscribe<std_msgs::Bool>("reverseMode", 10,
45  _stop_2d_nav_goal_client = nh.serviceClient<std_srvs::Trigger>(
46  "stop2DNavGoal");
47 
48  _add_waypoint_client = nh.serviceClient<rsm_msgs::AddWaypoint>(
49  "addWaypoint");
51  rsm_msgs::GetWaypointRoutines>("getWaypointRoutines");
52 
53  _get_robot_pose_client = nh.serviceClient<rsm_msgs::GetRobotPose>(
54  "getRobotPose");
55 }
56 
58 connect(_gui->start_exploration_button, SIGNAL(clicked(bool)), this, SLOT(startStopExploration()));
59 
60 connect(_gui->start_waypoint_following_button, SIGNAL(clicked(bool)), this, SLOT(startStopWaypointFollowing()));
61 connect(_gui->reset_waypoints_button, SIGNAL(clicked(bool)), this, SLOT(resetWaypoints()));
62 
63 connect(_gui->set_waypoint_button, SIGNAL(clicked(bool)), this, SLOT(addWaypoint()));
64 
65 connect(_gui->reverse_checkbox, SIGNAL(clicked(bool)), this, SLOT(setReverseMode()));
66 
67 connect(_gui->emergency_stop_button, SIGNAL(clicked(bool)), this, SLOT(emergencyStop()));
68 connect(_gui->stopped_radio_button, SIGNAL(clicked(bool)), this, SLOT(stopOperation()));
69 connect(_gui->autonomy_radio_button, SIGNAL(clicked(bool)), this, SLOT(setAutonomyOperation()));
70 connect(_gui->teleoperation_radio_button, SIGNAL(clicked(bool)), this, SLOT(setTeleoperation()));
71 
72 connect(_gui->stop_2d_nav_goal_button, SIGNAL(clicked(bool)), this, SLOT(stop2dNavGoal()));
73 }
74 
76 std_srvs::SetBool srv;
78  srv.request.data = false;
79 } else {
80  srv.request.data = true;
81 }
83  //ROS_INFO("State was changed: %s -> message: %s", srv.response.success ? "true" : "false", srv.response.message.c_str() );
84  if (srv.response.success) {
86  _gui->start_exploration_button->setText("Start");
87  _exploration_running = false;
88  _gui->exploration_info_text->setText("Exploration stopped");
89  _gui->exploration_mode_box->setEnabled(true);
90  } else {
91  _gui->start_exploration_button->setText("Stop");
92  _exploration_running = true;
93  _gui->exploration_info_text->setText("Exploration running");
95  _gui->exploration_mode_box->setEnabled(false);
96  }
97  } else {
98  QString text = QString("Call unsuccessful: %1").arg(
99  srv.response.message.c_str());
100  _gui->exploration_info_text->setText(text);
101  }
102 } else {
103  ROS_ERROR("Failed to call service Start/Stop Exploration");
104  _gui->exploration_info_text->setText("Exploration service not available");
105 }
106 }
107 
109 std_srvs::SetBool srv;
111  srv.request.data = false;
112 } else {
113  srv.request.data = true;
114 }
116  //ROS_INFO("State was changed: %s -> message: %s", srv.response.success ? "true" : "false", srv.response.message.c_str() );
117  if (srv.response.success) {
119  _gui->start_waypoint_following_button->setText("Start");
121  _gui->waypoint_following_info_text->setText(
122  "Waypoint following stopped");
123  _gui->reset_waypoints_button->setEnabled(true);
124  _gui->mode_box->setEnabled(true);
125  } else {
126  _gui->start_waypoint_following_button->setText("Stop");
128  _gui->waypoint_following_info_text->setText("Waypoint running");
129  _gui->reset_waypoints_button->setEnabled(false);
130  _gui->mode_box->setEnabled(false);
132  }
133  } else {
134  QString text = QString("Call unsuccessful: %1").arg(
135  srv.response.message.c_str());
136  _gui->waypoint_following_info_text->setText(text);
137  }
138 } else {
139  ROS_ERROR("Failed to call service Start/Stop Waypoint Following");
140  _gui->waypoint_following_info_text->setText(
141  "Waypoint Following service not available");
142 }
143 }
144 
146 std_srvs::Trigger srv;
147 if (_waypoint_reset_client.call(srv)) {
148  //ROS_INFO("State was changed: %s -> message: %s", srv.response.success ? "true" : "false", srv.response.message.c_str() );
149  if (srv.response.success) {
150  _gui->waypoint_following_info_text->setText("Waypoints reset");
151  } else {
152  QString text = QString("Reset unsuccessful: %1").arg(
153  srv.response.message.c_str());
154  _gui->waypoint_following_info_text->setText(text);
155  }
156 } else {
157  ROS_ERROR("Failed to call service Reset Waypoints");
158  _gui->waypoint_following_info_text->setText(
159  "Reset Waypoints service not available");
160 }
161 }
162 
164 rsm_msgs::GetRobotPose srv;
165 if (!_get_robot_pose_client.call(srv)) {
166  ROS_ERROR("Failed to call Get Robot Pose service");
167  _gui->waypoint_following_info_text->setText(
168  "Get Robot Pose service not available");
169 } else {
170  rsm_msgs::AddWaypoint srv2;
171  rsm_msgs::Waypoint waypoint;
172  waypoint.pose = srv.response.pose;
173  if (_gui->routine_combo_box->currentIndex() == 0) {
174  waypoint.routine = "";
175  } else {
176  waypoint.routine =
177  _gui->routine_combo_box->currentText().toUtf8().constData();
178  }
179  srv2.request.waypoint = waypoint;
180  srv2.request.position = -1;
181  if (!_add_waypoint_client.call(srv2)) {
182  ROS_ERROR("Failed to call Add Waypoint service");
183  _gui->waypoint_following_info_text->setText(
184  "Add Waypoint service not available");
185  }
186 }
187 }
188 
190 std_srvs::SetBool srv;
191 srv.request.data = !_reverse_mode;
192 if (_set_reverse_mode_client.call(srv)) {
193  if (srv.response.success) {
195  _gui->reverse_checkbox->setChecked(_reverse_mode);
196  } else {
197  _gui->movement_label->setText("Control: Failed to change reverse mode");
198  }
199 } else {
200  ROS_ERROR("Failed to call Set Reverse Mode service");
201  _gui->movement_label->setText(
202  "Control: Set Reverse Mode service not available");
203 }
204 }
205 
208 _operation_mode = rsm_msgs::OperationMode::STOPPED;
210 }
211 
213 if (_operation_mode != rsm_msgs::OperationMode::STOPPED) {
214  _operation_mode = rsm_msgs::OperationMode::STOPPED;
216 }
217 }
218 
220 if (_operation_mode != rsm_msgs::OperationMode::AUTONOMOUS) {
221  _operation_mode = rsm_msgs::OperationMode::AUTONOMOUS;
223 }
224 }
225 
227 if (_operation_mode != rsm_msgs::OperationMode::TELEOPERATION) {
228  _operation_mode = rsm_msgs::OperationMode::TELEOPERATION;
230 }
231 }
232 
234 std_srvs::Trigger srv;
235 if (!_stop_2d_nav_goal_client.call(srv)) {
236  ROS_ERROR("Failed to call Stop 2D Nav Goal service");
237  _gui->movement_label->setText(
238  "Control: Stop 2D Nav Goal service not available");
239 }
240 }
241 
243 rsm_msgs::SetOperationMode srv;
244 srv.request.operationMode.emergencyStop = _emergency_stop_active;
245 srv.request.operationMode.mode = _operation_mode;
249 } else {
250  ROS_ERROR("Failed to call service Set Operation Mode");
251  _gui->control_label->setText(
252  "Control: Set Operation Mode service not available");
253 }
254 }
255 
257 ROS_INFO("set waypoint following mode");
258 rsm_msgs::SetWaypointFollowingMode srv;
259 srv.request.mode = _gui->mode_box->currentIndex();
261  if (!srv.response.success) {
262  QString text =
263  QString("Set Waypoint Following Mode unsuccessful: %1").arg(
264  srv.response.message.c_str());
265  _gui->waypoint_following_info_text->setText(text);
266  }
267 
268 } else {
269  ROS_ERROR("Failed to call service Set Waypoint Following Mode");
270  _gui->waypoint_following_info_text->setText(
271  "Set Waypoint Following Mode service not available");
272 }
273 }
274 
276 std_srvs::SetBool srv;
277 srv.request.data = _gui->exploration_mode_box->currentIndex();
279  ROS_ERROR("Failed to call service Set Operation Mode");
280  _gui->exploration_info_text->setText(
281  "Set Exploration Mode service not available");
282 }
283 }
284 
286  const std_msgs::String::ConstPtr& state_info) {
287 QString text = QString("Current state: %1").arg(state_info->data.c_str());
288 _gui->current_state_text->setText(text);
289 if (state_info->data.rfind("E:") == 0) {
290  if (!_exploration_running) {
291  _gui->start_exploration_button->setText("Stop");
292  _exploration_running = true;
293  _gui->exploration_info_text->setText("Exploration running");
294  _gui->exploration_mode_box->setEnabled(false);
295  }
296 } else if (state_info->data.rfind("W:") == 0) {
298  _gui->start_waypoint_following_button->setText("Stop");
300  _gui->waypoint_following_info_text->setText(
301  "Waypoint Following running");
302  }
303 } else {
304  _gui->start_exploration_button->setText("Start");
305  _exploration_running = false;
306  _gui->exploration_info_text->setText("");
307  _gui->exploration_mode_box->setEnabled(true);
308  _gui->start_waypoint_following_button->setText("Start");
309  _gui->reset_waypoints_button->setEnabled(true);
310  _gui->mode_box->setEnabled(true);
312  _gui->waypoint_following_info_text->setText("");
313 }
314 }
315 
317  const std_msgs::Bool::ConstPtr& reverse_mode) {
318 _reverse_mode = reverse_mode->data;
319 _gui->reverse_checkbox->setChecked(_reverse_mode);
320 }
321 
323  const rsm_msgs::OperationMode::ConstPtr& operation_mode) {
326 } else {
327  _emergency_stop_active = operation_mode->emergencyStop;
328  _operation_mode = operation_mode->mode;
330 }
331 }
332 
334 QStringList list;
335 list.append("None");
336 rsm_msgs::GetWaypointRoutines srv;
338  _waypoint_routines = srv.response.waypointRoutines;
339  for (auto it : _waypoint_routines) {
340  list.append(it.c_str());
341  }
342 } else {
343  ROS_ERROR("Failed to call Get Waypoint Routines service");
344 }
345 _gui->routine_combo_box->addItems(list);
346 }
347 
349 std_srvs::Trigger srv;
350 if (_state_info_client.call(srv)) {
351  QString text = QString("Current state: %1").arg(
352  srv.response.message.c_str());
353  _gui->current_state_text->setText(text);
354 } else {
355  ROS_ERROR("Failed to call State Info service");
356 }
357 }
358 
360 _gui->stopped_radio_button->setChecked(_operation_mode == 0);
361 _gui->autonomy_radio_button->setChecked(_operation_mode == 1);
362 _gui->teleoperation_radio_button->setChecked(_operation_mode == 2);
364  _gui->emergency_stop_button->setStyleSheet(
365  "background-color: rgba(150,75,75,1);color: rgba(0,0,0,1);");
366  _gui->autonomy_radio_button->setEnabled(false);
367  _gui->teleoperation_radio_button->setEnabled(false);
368 } else {
369  _gui->emergency_stop_button->setStyleSheet(
370  "background-color: rgba(150,0,0,1);color: rgba(255,255,255,1);");
371  _gui->autonomy_radio_button->setEnabled(true);
372  _gui->teleoperation_radio_button->setEnabled(true);
373 }
374 }
375 
377 rviz::Panel::save(config);
378 }
379 
380 void RSMControlPanel::load(const rviz::Config& config) {
381 rviz::Panel::load(config);
382 }
383 
384 } // end namespace rsm
385 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::ServiceClient _set_waypoint_following_mode_client
Definition: RSMControls.h:71
ros::ServiceClient _state_info_client
Definition: RSMControls.h:78
ros::ServiceClient _start_stop_waypoint_following_client
Definition: RSMControls.h:69
ros::ServiceClient _waypoint_reset_client
Definition: RSMControls.h:70
ros::ServiceClient _set_reverse_mode_client
Definition: RSMControls.h:74
void reverseModeCallback(const std_msgs::Bool::ConstPtr &reverse_mode)
ros::ServiceClient _add_waypoint_client
Definition: RSMControls.h:72
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void startStopWaypointFollowing()
ros::Subscriber _state_info_subscriber
Definition: RSMControls.h:80
bool call(MReq &req, MRes &res)
bool _exploration_running
Is the exploration currently running.
Definition: RSMControls.h:92
ros::ServiceClient _set_operation_mode_client
Definition: RSMControls.h:76
ros::Subscriber _reverse_mode_subscriber
Definition: RSMControls.h:81
void stateInfoCallback(const std_msgs::String::ConstPtr &state_info)
ros::ServiceClient _set_exploration_mode_client
Definition: RSMControls.h:77
virtual void load(const rviz::Config &config)
Saves configuration data from this panel to the Config object.
#define ROS_INFO(...)
ros::ServiceClient _get_robot_pose_client
Definition: RSMControls.h:75
bool _operation_mode_button_pushed
Was an operation mode button just pushed.
Definition: RSMControls.h:88
Ui::rsm_controls * _gui
Definition: RSMControls.h:64
ros::Subscriber _operation_mode_subcriber
Definition: RSMControls.h:82
bool _waypoint_following_running
Is waypoint following currently running.
Definition: RSMControls.h:96
ros::ServiceClient _stop_2d_nav_goal_client
Definition: RSMControls.h:79
virtual void save(Config config) const
virtual void load(const Config &config)
void operationModeCallback(const rsm_msgs::OperationMode::ConstPtr &operation_mode)
std::vector< std::string > _waypoint_routines
Definition: RSMControls.h:112
ros::ServiceClient _start_stop_exploration_client
Definition: RSMControls.h:68
ros::ServiceClient _get_waypoint_routines_client
Definition: RSMControls.h:73
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
Panel plugin for RViz which adds buttons to interface the RSM.
Definition: RSMControls.h:31
RSMControlPanel(QWidget *parent=0)
Constructor.
Definition: RSMControls.cpp:5
virtual void save(rviz::Config config) const
Load configuration data for this panel from Config object.


rsm_rviz_plugins
Author(s): Marco Steinbrink
autogenerated on Tue Mar 16 2021 02:44:40