1
+ /*
2
+ * Copyright(c) 2019, seiga-k
3
+ * All rights reserved.
4
+ */
5
+
1
6
#include < ros/ros.h>
2
7
#include < ros/console.h>
3
8
#include < std_msgs/Float32.h>
15
20
#include < boost/phoenix.hpp>
16
21
#include < boost/spirit/include/qi.hpp>
17
22
18
- #include " util.h"
23
+ #include " sysmon_ros/ util.h"
19
24
20
- namespace Sysmon {
25
+ namespace Sysmon
26
+ {
21
27
22
- class Cpumon {
28
+ class Cpumon
29
+ {
23
30
public:
24
-
25
- Cpumon () :
26
- nh (" ~" ),
27
- hz (1.0 ),
28
- do_loop (true ),
29
- proc_name(" /proc/stat" ),
30
- cpun(0 )
31
- {
32
- nh.getParam (" hz" , hz);
33
- struct stat f_stat;
34
- if (stat (proc_name.c_str (), &f_stat) == 0 ) {
35
- do_loop = true ;
36
- std::string str;
37
- int32_t line (0 );
38
- while (Util::readSingleLine (proc_name, str, line++)) {
39
- int32_t total, worker;
40
- std::string name;
41
- if (parse (str, total, worker, name)) {
42
- pub_usages.insert (std::make_pair (name, nh.advertise <std_msgs::Float32>(name, 1 )));
43
- totals_prev.insert (std::make_pair (name, total));
44
- workers_prev.insert (std::make_pair (name, worker));
45
- }
46
- }
47
-
48
- ROS_INFO (" CPU Usage report is enaabled." );
49
- } else {
50
- do_loop = false ;
51
- ROS_INFO (" CPU Usage report is disabled." );
52
- }
53
-
54
- ROS_INFO (" Start cpumon node" );
55
- }
56
-
57
- ~Cpumon ()
58
- {
59
- }
60
-
61
- void run ()
62
- {
63
- ros::Rate rate (hz);
64
- while (ros::ok () && do_loop) {
65
- ros::spinOnce ();
66
-
67
- std::string str;
68
- int32_t line (0 );
69
- while (Util::readSingleLine (proc_name, str, line++)) {
70
- int32_t total, worker;
71
- std::string name;
72
- if (parse (str, total, worker, name)) {
73
- std_msgs::Float32 usage;
74
- usage.data = (float )(worker - workers_prev[name]) / (float )(total - totals_prev[name]) * 100 .;
75
- pub_usages[name].publish (usage);
76
- workers_prev[name] = worker;
77
- totals_prev[name] = total;
78
- }
79
- }
80
-
81
- rate.sleep ();
82
- }
83
- }
31
+ Cpumon () :
32
+ nh (" ~" ),
33
+ hz (1.0 ),
34
+ do_loop (true ),
35
+ proc_name(" /proc/stat" ),
36
+ cpun(0 )
37
+ {
38
+ nh.getParam (" hz" , hz);
39
+ struct stat f_stat;
40
+ if (stat (proc_name.c_str (), &f_stat) == 0 )
41
+ {
42
+ do_loop = true ;
43
+ std::string str;
44
+ int32_t line (0 );
45
+ while (Util::readSingleLine (proc_name, &str, line++))
46
+ {
47
+ int32_t total, worker;
48
+ std::string name;
49
+ if (parse (str, &total, &worker, &name))
50
+ {
51
+ pub_usages.insert (std::make_pair (name, nh.advertise <std_msgs::Float32>(name, 1 )));
52
+ totals_prev.insert (std::make_pair (name, total));
53
+ workers_prev.insert (std::make_pair (name, worker));
54
+ }
55
+ }
56
+
57
+ ROS_INFO (" CPU Usage report is enaabled." );
58
+ }
59
+ else
60
+ {
61
+ do_loop = false ;
62
+ ROS_INFO (" CPU Usage report is disabled." );
63
+ }
64
+
65
+ ROS_INFO (" Start cpumon node" );
66
+ }
67
+
68
+ ~Cpumon ()
69
+ {
70
+ }
71
+
72
+ void run ()
73
+ {
74
+ ros::Rate rate (hz);
75
+ while (ros::ok () && do_loop)
76
+ {
77
+ ros::spinOnce ();
78
+
79
+ std::string str;
80
+ int32_t line (0 );
81
+ while (Util::readSingleLine (proc_name, &str, line++))
82
+ {
83
+ int32_t total, worker;
84
+ std::string name;
85
+ if (parse (str, &total, &worker, &name))
86
+ {
87
+ std_msgs::Float32 usage;
88
+ usage.data = static_cast <float >(worker - workers_prev[name])
89
+ / static_cast <float >(total - totals_prev[name])
90
+ * 100 .;
91
+ pub_usages[name].publish (usage);
92
+ workers_prev[name] = worker;
93
+ totals_prev[name] = total;
94
+ }
95
+ }
96
+
97
+ rate.sleep ();
98
+ }
99
+ }
84
100
85
101
private:
86
- bool parse (const std::string line, int32_t &total, int32_t &worker, std::string &cpu_name) {
87
- namespace qi = boost::spirit::qi;
88
- namespace bp = boost::phoenix;
89
-
90
- auto first = line.begin ();
91
- auto last = line.end ();
92
-
93
- std::vector<int32_t > results;
94
- std::string name;
95
-
96
- if (qi::parse (
97
- first, last,
98
- (
99
- qi::as_string[qi::string (" cpu" ) >> *qi::alnum][bp::ref (name) = qi::_1]
100
- >> +(+qi::blank >> qi::int_[bp::push_back (bp::ref (results), qi::_1)])
101
- )
102
- )) {
103
- // std::cout << "Match!! " << name << " " << results.size() << std::endl;
104
- if (results.size () == 10 ) {
105
- worker = results[0 ] + results[1 ] + results[2 ];
106
- total = std::accumulate (results.begin (), results.end (), 0 );
107
- cpu_name = name;
108
- }
109
-
110
- return true ;
111
- }
112
- return false ;
113
- }
114
-
115
- ros::NodeHandle nh;
116
- std::map<std::string, ros::Publisher> pub_usages;
117
- std::map<std::string, int32_t > totals_prev;
118
- std::map<std::string, int32_t > workers_prev;
119
- double hz;
120
- bool do_loop;
121
- int32_t cpun;
122
- const std::string proc_name;
102
+ bool parse (const std::string line, int32_t *total, int32_t *worker, std::string *cpu_name)
103
+ {
104
+ namespace qi = boost::spirit::qi;
105
+ namespace bp = boost::phoenix;
106
+
107
+ auto first = line.begin ();
108
+ auto last = line.end ();
109
+
110
+ std::vector<int32_t > results;
111
+ std::string name;
112
+
113
+ if (qi::parse (
114
+ first, last,
115
+ (
116
+ qi::as_string[qi::string (" cpu" ) >> *qi::alnum][bp::ref (name) = qi::_1]
117
+ >> +(+qi::blank >> qi::int_[bp::push_back (bp::ref (results), qi::_1)])
118
+ ))) // NOLINT
119
+ {
120
+ if (results.size () == 10 )
121
+ {
122
+ *worker = results[0 ] + results[1 ] + results[2 ];
123
+ *total = std::accumulate (results.begin (), results.end (), 0 );
124
+ *cpu_name = name;
125
+ }
126
+
127
+ return true ;
128
+ }
129
+ return false ;
130
+ }
131
+
132
+ ros::NodeHandle nh;
133
+ std::map<std::string, ros::Publisher> pub_usages;
134
+ std::map<std::string, int32_t > totals_prev;
135
+ std::map<std::string, int32_t > workers_prev;
136
+ double hz;
137
+ bool do_loop;
138
+ int32_t cpun;
139
+ const std::string proc_name;
123
140
};
124
- }
141
+ } // namespace Sysmon
125
142
126
143
int main (int argc, char ** argv)
127
144
{
128
- ros::init (argc, argv, " cpumon" );
129
- Sysmon::Cpumon cpumon;
130
- cpumon.run ();
131
- ros::spin ();
132
- return 0 ;
133
- }
145
+ ros::init (argc, argv, " cpumon" );
146
+ Sysmon::Cpumon cpumon;
147
+ cpumon.run ();
148
+ ros::spin ();
149
+ return 0 ;
150
+ }
0 commit comments