6.3 Контроль температуры сервопривода Dynamixel

Если вы используете сервоприводы Dynamixel на своем роботе, вы знаете, что они могут нагреваться под нагрузкой. Как перегрев, так и чрезмерные нагрузки могут легко повредить сервопривод, который может стать довольно дорогим, если вы не будете осторожны.

Сервомоторы Dynamixel имеют встроенную защиту от таких отказов, автоматически отключаясь, когда температура, нагрузка или напряжение превышают заранее определенный порог. Пороговые значения устанавливаются непосредственно в микропрограммном обеспечении, поэтому этот уровень контроля повреждений происходит независимо от ROS. Заводские значения по умолчанию для этих порогов, как правило, в порядке. Тем не менее, это неплохая идея, чтобы контролировать температуру сервопривода и нагрузки на уровне ROS. Таким образом, мы можем заранее дать сервомотору или группе сервомоторов отдохнуть, когда им может угрожать опасность перегрева или перегрузки. Кроме того, если сервомотор может быть выключен с помощью встроенного механизма, часто необходимо включить питание всей шины, чтобы восстановить контроль над сервомотором даже после того, как он остынет или больше не будет перегружен.

6.3.1 Контроль сервоприводов для поворотно-наклонной головки

Для иллюстрации предположим, что ваш робот имеет наклонно-поворотную головку, использующую пару сервоприводов Dynamixel, как мы рассмотрели в главе 12 тома 1. Мы будем использовать файл запуска pi_robot_head_only.launch в каталоге rbx2_bringup / launch, как мы делали при тестировании драйвера arbotix с реальными сервоприводами в предыдущей главе. Этот файл запуска использует драйвер arbotix для подключения к двум сервомоторам AX-12 Dynamixel с аппаратными идентификаторами 1 и 2 и совместными именами head_pan_joint и head_tilt_joint.

Предполагая, что ваши сервоприводы подключены к контроллеру USB2Dynamixel на USB-порту / dev/ttyUSB0, запустите файл запуска с параметром sim, установленным в false:

$ roslaunch rbx2_bringup pi_robot_head_only.launch sim:=false

Если ваш контроллер USB2Dynamixel подключен к другому порту USB, вы можете запустить файл запуска с аргументом port. Например, если ваш контроллер находится на USB-порту / dev/ttyUSB1, используйте команду:

$ roslaunch rbx2_bringup pi_robot_head_only.launch sim:=false port:=/dev/ttyUSB1

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

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

Этот экран состояния показывает, что головка лотка по существу центрирована (положение =-0.005), температура составляет безопасные 33 градуса Цельсия, входное напряжение 12В (умноженное на 10 почему-то- но это не 120 вольт!), частота ошибок составляет идеальные 0,0% , а крутящий момент в настоящее время выключен, что означает, что сервопривод расслаблен и может быть повернут вручную.

Драйвер arbotix запрограммирован на публикацию диагностических данных для сервоприводов, которыми он управляет в топике /diagnostics. Затем наш стартовый файл запускает узел diagnostic_aggregator и rqt_robot_monitor, чтобы суммировать состояние каждого сервопривода. Если сервомотор перегреется или перегружен, его состояние превратится в ошибку, и он будет отображаться красным цветом вместо зеленого. Если он становится теплым, но еще не слишком горячим, состояние будет предупреждать с отображением желтого цвета. Драйвер arbotix жестко запрограммирован для присвоения статуса ошибки при любой температуре выше 60℃ и статуса предупреждения при температуре 50℃ и более. Далее в этой главе мы узнаем, как написать собственный diagnostics publisher и как установить пороговые значения по своему усмотрению.

Давайте теперь поближе рассмотрим файл pi_robot_head_only.launch, чтобы увидеть, как выполняются узлы агрегатора и монитора rqt. В нижней части файла запуска вы найдете следующие строки:

    <node pkg="diagnostic_aggregator" type="aggregator_node"
name="diagnostic_aggregator" clear_params="true" unless="$(arg sim)">
        <rosparam command="load" file="$(find
rbx2_dynamixels)/config/head_only_diagnostics.yaml" />
</node>

    <node pkg="rqt_robot_monitor" type="rqt_robot_monitor"
name="rqt_robot_monitor" unless="$(arg sim)" />

Здесь мы запускаем узел diagnostic_aggregator, который загружает конфигурационный файл head_only_diagnostics.yaml, который находится в каталоге rbx2_dynamixels/config. Мы скоро посмотрим этот файл вкратце. Мы также удаляем все оставшиеся параметры диагностического агрегатора, которые могут остаться после предыдущих запусков, используя аргумент clear_params= "true".

Далее мы запускаем узел rqt_robot_monitor, который генерирует графический интерфейс, показанный ранее для визуального просмотра состояния сервопривода.

Файл head_only_diagnostics.yaml определяет, какие анализаторы мы хотим запустить и как должна быть организована информация. Вот как выглядит этот файл:

pub_rate: 1.0
analyzers:
joints:
type: GenericAnalyzer
path: 'Joints'
timeout: 5.0
contains: '_joint'

Мы уже описывали этот самый конфигурационный файл в разделе «Конфигурационный файл анализатора». Если вы считаете, что скорость публикации 1 Гц слишком медленная, вы можете увеличить параметр pub_rate. Седьмая строка указывает, что данные, которые мы хотим суммировать, поступают из диагностических записей, имя которых содержит строку '_joint'. Чтобы понять, почему это работает, нам нужно изучить фактические диагностические сообщения, публикуемые в топике /diagnostics. Давайте перейдем к следующему вопросу.

6.3.2 Сообщения топика /diagnostic

Ранее мы уже говорили, что драйвер arbotix заботится о публикации диагностических сообщений ROS для нас. Драйвер напрямую связывается с микропрограммным обеспечением сервопривода через последовательный порт, а затем использует API диагностики ROS для публикации данных в разделе /diagnostics

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

$ rostopic echo /diagnostics | more

Первая часть вывода должна выглядеть примерно так:

header:     seq: 2125
stamp:         secs:
1405518802
nsecs: 322613000
frame_id: '' status:
-         level: 0
name: head_controller
message: OK
hardware_id: ''
values:
-         key: State
value: Not Active
-         level: 0
name: head_pan_joint
message: OK
hardware_id: ''
values:
-         key: Position
value: -1.01242732
        -
        key: Temperature
value: 39
-         key: Voltage
value: 120

Сначала мы видим поля заголовка, а затем начало массива состояния. Символ дефиса ( - ) указывает на начало записи массива. Первый дефис выше указывает на начало первой записи массива состояния. Эта запись имеет индекс массива 0, поэтому эта первая запись будет называться /diagnostics / status[0]. В этом случае запись относится к головному контроллеру, который управляет обоими сервоприводами. Поскольку главный контроллер является программным компонентом, он не имеет положения или температуры, поэтому давайте рассмотрим следующую запись массива

Второй блок выше будет иметь индекс массива 1, так что эта запись будет доступна как /diagnostics / status[1]. В этом случае запись относится к головному суставу. Отступы дефисов под ключевым словом values указывают на записи в массиве ключ-значение для этого диагностического элемента. Таким образом, значение температуры для этого сервопривода будет индексироваться как /diagnostics/status[1].values[1]. Обратите внимание, как мы используем точку (.) для указания подполей элемента массива. Если вы хотите, чтобы эхо просто отражало температуру серво-головки панорамирования, вы можете использовать следующую команду:

$ rostopic echo /diagnostics/status[1].values[1]

Возвращаясь теперь к выводам выше, обратите внимание, что первые несколько полей состояния для соединения головных штифтов являются:

status:
-      level: 0
name: head_pan_joint
message: OK
hardware_id: ''

Критическим результатом здесь является значение для подполя level, которое в данном случае равно 0. Напомним, что значение 0 соответствует состоянию диагностики OK. Соответствующие компоненты должны быть идентифицированы по имени, а иногда и по полям hardware_id. Здесь мы видим, что этот элемент массива ссылается на head_pan_joint, но идентификатор оборудования не указан.

Если вы снова выполните команду "rostopic echo / diagnostics | more" и продолжите нажимать пробел, чтобы прокрутить сообщения, вы увидите сообщения о состоянии для каждого сервопривода. В частности, сообщение для сустава наклона головы начинается так:

-        level: 0
name: head_tilt_joint
message: OK
hardware_id: ''

Таким образом, мы видим, что значение поля name-это просто совместное имя, которое мы назначили каждому сервомотору в нашем конфигурационном файле ArbotiX. Поскольку каждое имя соединения заканчивается строкой "_joint", наш конфигурационный файл анализатора head_only_diagnostics.yaml может использовать эту строку для значения параметра contains, чтобы сообщить агрегатору, что диагностические сообщения, содержащие эту строку в поле их имени, являются теми, которые были заинтересованы.

6.3.3 Защита сервопроводов путём мониторинга раздела /diagnostic

Пакет rbx2_diagnostics содержит сценарий, который называется monitor_dynamixels.py в поддиректории nodes. Этот узел подписывается на раздел /diagnostics, извлекает сообщения, относящиеся к сервомоторам и отключает сервомоторы, если они оказываются в неисправном состоянии. Затем сценарий отсчитывает таймер, чтобы дать сервомоторам остыть, а затем снова включает их, когда их температура возвращается к нормальной.

Сценарий довольно длинный поэтому давайте сосредоточимся на ключевых строках:

4 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
5 from arbotix_msgs.srv import Relax, Enable

В верхней части скрипта мы импортируем типы диагностических сообщений, которые нам понадобятся, а также службы Relax и Enable из пакета arbotix_msgs:

12 # The arbotix controller uses
the /arbotix namespace
13 namespace = '/arbotix' 14
15 # Get the list of joints (servos)
16 self.joints = rospy.get_param(namespace + '/joints', '')
17
18 # Minimum time to rest servos that are hot
19 self.minimum_rest_interval = rospy.get_param('~minimum_rest_interval', 60)
20
21 # Initialize the rest timer
22 self.rest_timer = 0
23
24 # Are we already resting a servo?
25 self.resting = False
26
27 # Are the servos enabled?
28 self.servos_enabled = False 29
30 # Have we issued a warning recently? 31
self.warned = False
32
33 # Connect to the servo services
34 self.connect_servos()
35
36 rospy.Subscriber('diagnostics', DiagnosticArray, self.get_diagnostics)

Список соединений, управляемых узлом arbotix_driver, хранится в параметре ROS /arbotix/joins. На самом деле этот параметр совпадает с параметром joints, который мы определили в нашем конфигурационном файле arbotix. Поэтому мы храним объединенный список (фактически словарь) в переменной self.joints

Мы также читаем параметр minimum_rest_interval (в секундах), чтобы дать сервоприводу время остыть перед повторным включением. Этот параметр можно задать в файле запуска, но мы даем ему значение по умолчанию 60 секунд. Если мы опустим минимальный период охлаждения, то перегрев сервопривода будет отключен ровно на столько, чтобы остыть на один или два градуса, а затем он, вероятно, просто перегреется снова и так далее.

Затем мы инициализируем таймер, чтобы отслеживать, как долго мы отключили сервопривод, а также пару логических флагов, чтобы указать, когда сервоприводы отключены и когда мы отдыхаем. Затем мы вызываем функцию connect_servos () (описанную ниже), которая заботится о подключении к различным темам и службам, связанным с управлением Dynamixels.

Последняя строка выше подписывается на раздел /diagnostics и устанавливает функцию обратного вызова в self.get_diagnostics(), который мы рассмотрим далее:

35 def get_diagnostics(self, msg):
36 if self.rest_timer != 0:
37 if rospy.Time.now() - self.rest_timer < rospy.Duration(self.minimum_rest_interval): 
38 return 
39 else:
40  self.resting = False
41  rest_timer = 0

В первой части функции обратного вызова мы проверяем состояние rest_timer и, если у нас еще осталось немного времени на часах, мы немедленно возвращаемся. В противном случае мы сбрасываем таймер на 0, а флаг resting на False, а затем переходим к следующим строкам:

45 for k in range(len(msg.status)):
46 # Check for the Dynamixel identifying string in the name field 
47 if not '_joint' in msg.status[k].name:
48 # Skip other diagnostic messages
49 continue

Функция обратного вызова получает сообщение DiagnosticArray в качестве аргумента msg. Каждый элемент массива представляет собой индивидуальное диагностическое сообщение, поэтому мы хотим перебрать все такие сообщения. Первое, что мы проверяем, - это то, что сильный '_joint' находится в имени сообщения, и если это не так, мы переходим к следующему сообщению, используя оператор continue.

48 # Check the DiagnosticStatus level for this servo 
49 if msg.status[k].level == DiagnosticStatus.ERROR:
50 # If the servo is overheating, then disable all servos 
51 if not self.resting:
52 rospy.loginfo("DANGER: Overheating servo: " + str(msg.status[k].name))
53 rospy.loginfo("Disabling servos for a minimum of " +
str(self.minimum_rest_interval) + " seconds...")
54
55 self.disable_servos()
56 self.servos_enabled = False
57 self.resting = True
58 break
59 elif msg.status[k].level == DiagnosticStatus.WARN:
60 # If the servo is getting toasty, display a warning
61 rospy.loginfo("WARNING: Servo " + str(msg.status[k].name) + " getting
hot...")
62 self.warned = True
63 warn = True

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

Если сервопривод недостаточно горяч для состояния ошибки, но он достаточно теплый для состояния предупреждения, мы выводим предупреждающее сообщение, но не отключаем сервоприводы. Мы также установили пару флагов, чтобы не повторять одно и то же предупреждающее сообщение.

65 # No servo is overheated so re-enable all servos 
66 if not self.resting and not self.servos_enabled:
67 rospy.loginfo("Dynamixel temperatures OK so enabling")
68 self.enable_servos()
69 self.servos_enabled = True
70 self.resting = False

Наконец, если никакие сервоприводы не перегреваются и мы в данный момент не находимся в состоянии покоя, снова включите сервоприводы.

Теперь давайте рассмотрим три функции, которые мы вызывали ранее в скрипте: connect_servos(), disable_serv() и enable_serv().

78 def connect_servos(self):
79 # Create a dictionary to hold the torque and enable services
80 self.relax = dict()
81 self.enable = dict()
82
83 # Connect to the set_speed services and define a position publisher for
each servo
84 rospy.loginfo("Waiting for joint controllers services...")
85
86 for joint in sorted(self.joints):
87 # A service to relax a servo
88 relax = '/' + joint + '/relax'
89 rospy.wait_for_service(relax)
90 self.relax[joint] = rospy.ServiceProxy(relax, Relax)
91
92 # A service to enable/disable a servo
93 enable_service = '/' + joint + '/enable'
94 rospy.wait_for_service(enable_service)
95 self.enable[joint] = rospy.ServiceProxy(enable_service, Enable)
96
97 rospy.loginfo("Connected to servos.")

Функция connect_servos() работает аналогично функции arbotix_relax_all_servos.py, сценарий которого мы рассмотрели в предыдущей главе. В этом случае мы создаем прокси для relax и включаем сервисы для каждого соединения и храним прокси в словаре, индексируемом по имени соединения. Затем мы можем использовать эти службы в функциях disable_servos() и enable_servos().

99 def disable_servos(self):
100 for joint in sorted(self.joints):
101 self.enable[joint](False)
102
103 def enable_servos(self):
104 for joint in sorted(self.joints):
105 self.enable[joint](True)

После того, как мы определили прокси-серверы relax и enable service, можно просто отключить или включить все сервоприводы, перебрав список соединений и вызвав соответствующую службу. Чтобы расслабить и отключить сервопривод, мы вызываем службу enable со значением запроса, установленным в False, и сервопривод расслабится и проигнорирует любые будущие запросы положения. Чтобы повторно включить сервомотор, мы вызываем службу enable со значением запроса True.

Чтобы опробовать этот скрипт, сначала убедитесь, что вы запускаете файл запуска для своих Dynamixels, например pi_robot_head_only.launch, который мы использовали в предыдущем разделе. Затем запустите файл monitor_dynamixels.launch:

$ roslaunch rbx2_diagnostics monitor_dynamixels.launch

Вы должны увидеть следующую серию информационных сообщений:

process[monitor_dynamixels-1]: started with pid [5797] [INFO]
[WallTime: 1403361617.457575] Waiting for joint controllers
services...
[INFO] [WallTime: 1403361617.471666] Connected to servos.
[INFO] [WallTime: 1403361617.662016] Dynamixel temperatures OK so
enabling

Теперь вы можете запускать любые другие узлы, такие как отслеживание головы, которые используют сервоприводы. Узел monitor_dynamixels будет контролировать температуру сервопривода, и если какой-либо сервопривод станет слишком горячим, он отключит все сервоприводы до тех пор, пока они не вернутся к безопасной температуре. В то же время другие узлы могут продолжать публиковать сервокоманды, но они будут просто игнорироваться до тех пор, пока Dynamixel не будут повторно включены.