md10c_serial_bridge
読み取り中…
検索中…
一致する文字列を見つけられません
node.py
[詳解]
1#!/usr/bin/env python3
2"""ROSで受信した最新のPWM指令を、シリアル通信でMD10C制御用Arduino Unoへ送る。"""
3
4import time
5
6import rclpy
7import serial
8from rclpy.node import Node
9from std_msgs.msg import Int16
10
11
13 """PWM指令を上限・下限の範囲に収めて送信し、ROS指令が途絶えた場合は停止指令を送る。"""
14
15 def __init__(self) -> None:
16 super().__init__('md10c_serial_bridge')
17 self.declare_parameter('device', '/dev/ttyACM0')
18 self.declare_parameter('baud_rate', 115200)
19 self.declare_parameter('timeout_ms', 500)
20 self.declare_parameter('publish_period_ms', 50)
21 self.declare_parameter('topic', '/md10c/cmd_pwm')
22
23 device = self.get_parameter('device').value
24 baud_rate = int(self.get_parameter('baud_rate').value)
25 self._timeout_s = float(self.get_parameter('timeout_ms').value) / 1000.0
26 period_ms = int(self.get_parameter('publish_period_ms').value)
27 topic = self.get_parameter('topic').value
28 if self._timeout_s <= 0.0 or period_ms <= 0:
29 raise ValueError('timeout_ms and publish_period_ms must be positive')
30
31 self._serial = serial.Serial(device, baud_rate, timeout=0.1, write_timeout=0.1)
32 # Opening an Uno serial port may reset it. Allow its bootloader to finish
33 # before the first command is sent; startup state is already motor-off.
34 time.sleep(2.0)
35 self._command = 0
36 self._last_command_time = time.monotonic()
37 self._last_write_failed = False
38 self._subscription = self.create_subscription(Int16, topic, self._on_command, 10)
39 self._timer = self.create_timer(period_ms / 1000.0, self._send_latest)
40 self.get_logger().info(
41 f'MD10C serial bridge ready: {device} at {baud_rate} baud, topic={topic}'
42 )
43
44 def _on_command(self, message: Int16) -> None:
45 self._command = max(-255, min(255, int(message.data)))
46 self._last_command_time = time.monotonic()
47
48 def _send_latest(self) -> None:
49 command = self._command
50 if time.monotonic() - self._last_command_time >= self._timeout_s:
51 command = 0
52 try:
53 self._serial.write(f'{command}\n'.encode('ascii'))
54 self._last_write_failed = False
55 except (serial.SerialException, OSError) as error:
56 if not self._last_write_failed:
57 self.get_logger().error(f'Serial write failed: {error}')
58 self._last_write_failed = True
59
60 def destroy_node(self) -> bool:
61 try:
62 if self._serial.is_open:
63 self._serial.write(b'0\n')
64 self._serial.close()
65 except (serial.SerialException, OSError):
66 pass
67 return super().destroy_node()
68
69
70def main(args=None) -> None:
71 rclpy.init(args=args)
72 node = None
73 try:
74 node = Md10cSerialBridge()
75 rclpy.spin(node)
76 except (serial.SerialException, OSError, ValueError) as error:
77 if node is not None:
78 node.get_logger().fatal(f'Could not start MD10C serial bridge: {error}')
79 else:
80 print(f'Could not start MD10C serial bridge: {error}')
81 finally:
82 if node is not None:
83 node.destroy_node()
84 rclpy.shutdown()
85
86
87if __name__ == '__main__':
88 main()
PWM指令を上限・下限の範囲に収めて送信し、ROS指令が途絶えた場合は停止指令を送る。
定義 node.py:12
None _on_command(self, Int16 message)
定義 node.py:44
None main(args=None)
定義 node.py:70