6.5 Создание собственных диагностических сообщений

Узел батареи ноутбука, представленный в предыдущем разделе, также публикует информацию о заряде батареи по теме /laptop_charge. Предполагая, что у вас все еще работает узел батареи ноутбука, вы можете просмотреть сообщения, опубликованные в этой теме, с помощью команды:

$ rostopic echo /laptop_charge | more

header:
   seq: 31 stamp:    secs:
1395880223    nsecs:
497864007 frame_id: ''
voltage: 15.1199998856 rate: -
11.0220003128 charge:
21.4249992371 capacity:
33.4599990845 design_capacity:
46.4720001221
percentage: 64
charge_state: 0
present: True ---

Как вы можете видеть на выходе, сообщение включает в себя напряжение, скорость заряда или разряда (в зависимости от знака), уровень заряда, емкость, процент оставшегося заряда, состояние заряда (0 = разрядка, 1 = есть заряд, 2 = полностью заряжен) и наличие или отсутствие его.

То, что мы хотели бы сделать - это превратить процентное значение заряда в диагностический уровень OK, WARN или ERROR и опубликовать его в разделе /diagnostics в качестве стандартного диагностического сообщения ROS. Наш новый сценарий назывался: monitor_laptop_charge.py, подписывается на тему /laptop_charge и преобразует процентную плату в один из трех стандартных уровней состояния диагностики: OK, WARN или ERROR.

Мы будем использовать файл monitor_laptop_charge.launch, чтобы вызвать узел и запустить диагностический агрегатор с соответствующим конфигурационным файлом. Файл запуска выглядит следующим образом:

1 <launch>
2 <node pkg="rbx2_diagnostics" type="monitor_laptop_charge.py"
name="monitor_laptop_charge" output="screen"> 
3 <param name="warn_percent"
value="50" />
4 <param name="error_percent" value="20" />
5 </node>
6
7 <node pkg="diagnostic_aggregator" type="aggregator_node"
name="diagnostic_aggregator" clear_params="true">
8 <rosparam command="load" file="$(find rbx2_diagnostics)/config/power.yaml"
/>
9 </node>
10
11 <node pkg="rqt_robot_monitor" type="rqt_robot_monitor" name="rqt_robot_monitor"
/>
12 </launch>

Строки 2-5 запускают monitor_laptop_charge.py узел и установите предупреждающий уровень заряда батареи на 50% , а уровень ошибки - на 20%. Строки 7-9 вызывают diagnostic_aggregator с конфигурационным файлом power.yaml, найденных в каталоге rbx2_diagnostics/config. Наконец, строка 11 вызывает графический интерфейс rqt_robot_monitor, если он еще не запущен.

Файл power.yaml выглядит следующим образом:

pub_rate: 1.0
analyzers:
power:
type: GenericAnalyzer
path: 'Power System'
timeout: 5.0
contains: ['Robot Battery', 'Robot Charge', 'Laptop Battery', 'Laptop
Charge']

Скорость публикации 1.0 Гц, заданная параметром pub_rate, кажется подходящей для монитора батареи, так как уровень заряда батареи меняется не очень быстро.

Строка 3 указывает на то, что мы будем использовать пространство имен power под пространством имен ~analyzers для хранения агрегированных диагностических данных для питания батареи.

Строка 4 указывает, что мы будем использовать тип плагина Generic Analyzer для анализа этих устройств.

В строке 5 параметр path определяет строку, которая будет использоваться для классификации этих устройств при просмотре в графическом интерфейсе RQT_ROBOT_MONITOR. В этом случае мы хотим, чтобы все наши сервоприводы были перечислены в категории "Power System".

Строка 6 определяет тайм-аут для получения обновления от устройства. Если в течение этого периода времени от устройства не будет получено никаких данных, то устройство будет помечено как "Stale"(устаревшее) в rqt_robot_monitor.

Наконец, в строке 7 мы указываем список строк, которые должны появиться где-то в сообщении диагностического массива, чтобы определить тип устройства, которое мы хотим контролировать.

Чтобы попробовать его, сначала убедитесь, что вы находитесь на ноутбуке, а затем запустите файл laptop_battery.launch, если он еще не запущен:

$ roslaunch rbx2_brinup laptop_battery.launch

Затем запустите файл monitor_laptop_charge.launch:

$ roslaunch rbx2_diagnostics monitor_laptop_charge.launch

После небольшой задержки вы увидите появившееся окно rqt_robot_monitor, которое должно выглядеть следующим образом:

В категории "Power System"(система питания) у нас есть два сообщения о состоянии: одно для батареи в целом (т. е. ее можно обнаружить) и одно для текущего уровня заряда. Первый статус предоставляется компанией laptop_battery.py узел тогда как второй публикуется нашим новым узлом monitor_laptop_charge.py Давайте теперь взглянем на этот сценарий.

Ссылка на источник: monitor_laptop_charge.py

1 #!/usr/bin/env python
2
3 import rospy
4 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
5 from linux_hardware.msg import LaptopChargeStatus 6
7 class MonitorLaptopCharge():
8 def __init__(self):
9 rospy.init_node("monitor_laptop_charge")
10
11 # Get the parameters for determining WARN and ERROR status levels
12 self.warn_percent = rospy.get_param("~warn_percent", 50)
13 self.error_percent = rospy.get_param("~error_percent", 20)
14
15 # A diagnostics publisher
16 self.diag_pub = rospy.Publisher('diagnostics', DiagnosticArray)
17
18 # Subscribe to the laptop_charge topic
19 rospy.Subscriber('laptop_charge', LaptopChargeStatus,
self.pub_diagnostics)
20
21 rospy.loginfo("Monitoring laptop charge...")
22
23 def pub_diagnostics(self, msg):
24 # Pull out the percent charge from the message
25 percent_charge = msg.percentage
26
27 # Initialize the diagnostics array
28 diag_arr = DiagnosticArray()
29
30 # Time stamp the message with the incoming
stamp
31 diag_arr.header.stamp = msg.header.stamp32
33 # Initialize the status message
34 diag_msg = DiagnosticStatus()
35
36 # Make the name field descriptive of what we are measuring 37
diag_msg.name = "Laptop Charge"
38
39 # Add a key-value pair so we can drill down to the percent charge
40 diag_msg.values.append(KeyValue('percent_charge', str(percent_charge)))
41
42 # Set the diagnostics level based on the current charge and the threshold
43 # parameters
44 if percent_charge < self.error_percent:
45 diag_msg.level = DiagnosticStatus.ERROR
46 diag_msg.message = 'Battery needs recharging' 47 elif percent_charge
< self.warn_percent:
48 diag_msg.level = DiagnosticStatus.WARN 49
diag_msg.message = 'Battery is below 50%' 50
else:
51 diag_msg.level = DiagnosticStatus.OK
52 diag_msg.message = 'Battery charge is OK'
53
54 # Append the status message to the diagnostic array
55 diag_arr.status.append(diag_msg)
56
57 # Publish the array
58 self.diag_pub.publish(diag_arr)
59
60 if __name__ == '__main__':
61 MonitorLaptopCharge()
62 rospy.spin()

А теперь давайте разберём всё по частям:

4 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue

Сначала нам нужно несколько типов сообщений из пакета diagnostics_msgs.

5 from linux_hardware.msg import LaptopChargeStatus

Нам также нужен тип сообщения о состоянии зарядки ноутбука, который определен в пакете linux_hardware.

16 self.diag_pub = rospy.Publisher('diagnostics', DiagnosticArray)

Здесь мы создаем издателя, чтобы опубликовать заряд ноутбука в виде сообщения диагностического массива в разделе /diagnostics

19 rospy.Subscriber('laptop_charge', LaptopChargeStatus,self.pub_diagnostics)

И здесь мы создаем подписчика для мониторинга темы /laptop_charge. Функция обратного вызова self.pub_diagnostics (объяснено ниже) преобразует заряд ноутбука в диагностическое сообщение ROS.

23 def pub_diagnostics(self, msg):
24 # Pull out the percent charge from the message
25 percent_charge = msg.percentage

Мы начинаем функцию обратного вызова, назначенную пользователю темы /laptop_charge. Напомним, что сообщение о состоянии заряда ноутбука содержит поле для оставшегося процентного заряда, поэтому мы извлекаем это значение из переменной msg, которая передается в обратный вызов.

27 # Initialize the diagnostics array
28 diag_arr = DiagnosticArray()
29
30 # Time stamp the message with the incoming stamp
31 diag_arr.header.stamp = msg.header.stamp

Мы хотим опубликовать процентный заряд в виде сообщения DiagnosticArray, поэтому сначала создадим пустой массив в виде переменной diag_arr. Затем мы даем массиву ту же временную метку, что и штамп входящего сообщения.

33 # Initialize the status message
34 diag_msg = DiagnosticStatus()
35
36 # Make the name field descriptive of what we are measuring
37 diag_msg.name = "Laptop Charge"

Поскольку диагностический массив состоит из отдельных диагностических сообщений о состоянии, мы инициализируем переменную diag_msg соответствующим образом и присваиваем соответствующую метку полю имени.

39 # Add a key-value pair so we can drill down to the percent charge
40 diag_msg.values.append(KeyValue('percent_charge',str(percent_charge)))

Напомним, что массив значений в диагностическом сообщении о состоянии хранится в виде пар «ключ-значение», где оба члена пары являются строками. Поэтому здесь мы добавим пару значений ключа (KeyValue), где ключ - это строка "percent_charge", а значение-это фактический процент заряда, преобразованный в строку.

44 if percent_charge < self.error_percent:
45 diag_msg.level = DiagnosticStatus.ERROR
46 diag_msg.message = 'Battery needs recharging' 47 elif percent_charge< self.warn_percent:
48 diag_msg.level = DiagnosticStatus.WARN 
49 diag_msg.message = 'Battery is below 50%' 
50 else:
51 diag_msg.level = DiagnosticStatus.OK
52 diag_msg.message = 'Battery charge is OK'

Затем мы устанавливаем значение полей level и message переменной diag_msg в соответствии с пороговыми значениями, заданными в параметрах error_percent и warn_percent.

54 # Append the status message to the diagnostic array
55 diag_arr.status.append(diag_msg)
56
57 # Publish the array
58 self.diag_pub.publish(diag_arr)

Наконец, мы добавляем созданное сообщение о состоянии диагностики в массив диагностики и публикуем результат.