ros_interface.cpp
Go to the documentation of this file.
1 // Provides a ROS interface for controlling rosmon
2 // Author: Max Schwarz <max.schwarz@uni-bonn.de>
3 
4 #include "ros_interface.h"
5 
6 #include <rosmon_msgs/State.h>
7 
8 #include <algorithm>
9 
10 namespace rosmon
11 {
12 
13 ROSInterface::ROSInterface(monitor::Monitor* monitor, bool enableDiagnostics,
14  const std::string& diagnosticsPrefix)
15  : m_monitor(monitor)
16  , m_nh("~")
17  , m_diagnosticsEnabled(enableDiagnostics)
18 {
20 
21  m_pub_state = m_nh.advertise<rosmon_msgs::State>("state", 10, true);
22 
24 
26  m_diagnosticsPublisher.reset(new DiagnosticsPublisher(diagnosticsPrefix));
27 }
28 
30 {
31  rosmon_msgs::State state;
32  state.header.stamp = ros::Time::now();
33 
36 
37  for(auto& node : m_monitor->nodes())
38  {
39  rosmon_msgs::NodeState nstate;
40  nstate.name = node->name();
41  nstate.ns = node->namespaceString();
42 
43  switch(node->state())
44  {
46  nstate.state = nstate.RUNNING;
47  break;
49  nstate.state = nstate.CRASHED;
50  break;
52  nstate.state = nstate.IDLE;
53  break;
55  nstate.state = nstate.WAITING;
56  break;
57  default:
58  nstate.state = nstate.IDLE;
59  break;
60  }
61 
62  nstate.restart_count = node->restartCount();
63 
64  nstate.user_load = static_cast<float>(node->userLoad());
65  nstate.system_load = static_cast<float>(node->systemLoad());
66 
67  nstate.memory = node->memory();
68 
69  state.nodes.push_back(nstate);
70  }
71 
72  m_pub_state.publish(state);
73 }
74 
75 bool ROSInterface::handleStartStop(rosmon_msgs::StartStopRequest& req, rosmon_msgs::StartStopResponse&)
76 {
77  auto it = std::find_if(
78  m_monitor->nodes().begin(), m_monitor->nodes().end(),
79  [&](const monitor::NodeMonitor::ConstPtr& n){ return (n->name() == req.node) && (n->namespaceString() == req.ns); }
80  );
81 
82  if(it == m_monitor->nodes().end())
83  return false;
84 
85  switch(req.action)
86  {
87  case rosmon_msgs::StartStopRequest::START:
88  (*it)->start();
89  break;
90  case rosmon_msgs::StartStopRequest::STOP:
91  (*it)->stop();
92  break;
93  case rosmon_msgs::StartStopRequest::RESTART:
94  (*it)->restart();
95  break;
96  }
97 
98  return true;
99 }
100 
102 {
104 
105  // Send empty state packet to clear the GUI
106  rosmon_msgs::State state;
107  state.header.stamp = ros::Time::now();
108  m_pub_state.publish(state);
109 
110  // HACK: Currently there is no way to make sure that we sent a message.
111  usleep(200 * 1000);
112 }
113 
114 }
const std::vector< NodeMonitor::Ptr > & nodes() const
Definition: monitor.h:41
ros::NodeHandle m_nh
Definition: ros_interface.h:31
void publish(const boost::shared_ptr< M > &message) const
std::shared_ptr< NodeMonitor > ConstPtr
Definition: node_monitor.h:29
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::unique_ptr< DiagnosticsPublisher > m_diagnosticsPublisher
Definition: ros_interface.h:40
ros::ServiceServer m_srv_startStop
Definition: ros_interface.h:37
Idle (e.g. exited with code 0)
Definition: node_monitor.h:34
state
Definition: basic.py:139
ROSInterface(monitor::Monitor *monitor, bool enableDiagnostics=false, const std::string &diagnosticsPrefix={})
Waiting for automatic restart after crash.
Definition: node_monitor.h:37
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
monitor::Monitor * m_monitor
Definition: ros_interface.h:29
static Time now()
bool handleStartStop(rosmon_msgs::StartStopRequest &req, rosmon_msgs::StartStopResponse &resp)
Crashed (i.e. exited with code != 0)
Definition: node_monitor.h:36
ros::WallTimer m_updateTimer
Definition: ros_interface.h:33
ros::Publisher m_pub_state
Definition: ros_interface.h:35


rosmon_core
Author(s): Max Schwarz
autogenerated on Sat Jan 9 2021 03:35:43