md10c_serial_bridge
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
node.py
[詳解]
1
#!/usr/bin/env python3
2
"""ROSで受信した最新のPWM指令を、シリアル通信でMD10C制御用Arduino Unoへ送る。"""
3
4
import
time
5
6
import
rclpy
7
import
serial
8
from
rclpy.node
import
Node
9
from
std_msgs.msg
import
Int16
10
11
12
class
Md10cSerialBridge
(Node):
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
70
def
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
87
if
__name__ ==
'__main__'
:
88
main
()
md10c_serial_bridge.node.Md10cSerialBridge
PWM指令を上限・下限の範囲に収めて送信し、ROS指令が途絶えた場合は停止指令を送る。
定義
node.py:12
md10c_serial_bridge.node.Md10cSerialBridge.__init__
None __init__(self)
定義
node.py:15
md10c_serial_bridge.node.Md10cSerialBridge._timer
_timer
定義
node.py:39
md10c_serial_bridge.node.Md10cSerialBridge._last_command_time
_last_command_time
定義
node.py:36
md10c_serial_bridge.node.Md10cSerialBridge._send_latest
None _send_latest(self)
定義
node.py:48
md10c_serial_bridge.node.Md10cSerialBridge._subscription
_subscription
定義
node.py:38
md10c_serial_bridge.node.Md10cSerialBridge._serial
_serial
定義
node.py:31
md10c_serial_bridge.node.Md10cSerialBridge._timeout_s
float _timeout_s
定義
node.py:25
md10c_serial_bridge.node.Md10cSerialBridge.destroy_node
bool destroy_node(self)
定義
node.py:60
md10c_serial_bridge.node.Md10cSerialBridge._last_write_failed
bool _last_write_failed
定義
node.py:37
md10c_serial_bridge.node.Md10cSerialBridge._command
int _command
定義
node.py:35
md10c_serial_bridge.node.Md10cSerialBridge._on_command
None _on_command(self, Int16 message)
定義
node.py:44
md10c_serial_bridge.node.main
None main(args=None)
定義
node.py:70
md10c_serial_bridge
node.py
構築:
1.18.0