1
1
import rclpy
2
2
from rclpy .node import Node as RclpyNode
3
3
from autonav_shared .types import DeviceState , LogLevel , SystemState
4
- from autonav_msgs .msg import SystemState as SystemStateMsg , DeviceState as DeviceStateMsg , Performance , Log
4
+ from autonav_msgs .msg import SystemState as SystemStateMsg , DeviceState as DeviceStateMsg , Performance , Log , ConfigurationBroadcast , ConfigurationUpdate
5
5
import sty
6
6
import time
7
7
import inspect
8
+ import json
8
9
9
10
10
11
class Node (RclpyNode ):
@@ -16,13 +17,19 @@ def __init__(self, name: str) -> None:
16
17
self .mobility = False
17
18
self .device_states = {}
18
19
self .start_times = {}
20
+ self .config = {}
21
+ self .other_cfgs = {}
19
22
self .device_states [name ] = DeviceState .OFF
20
23
21
24
# TODO: Setup all relevant publishers, subscribers, services, clients, etc
22
25
self .system_state_sub = self .create_subscription (SystemStateMsg , "/autonav/shared/system" , self .system_state_callback , 10 )
23
26
self .device_state_sub = self .create_subscription (DeviceStateMsg , "/autonav/shared/device" , self .device_state_callback , 10 )
24
27
self .system_state_pub = self .create_publisher (SystemStateMsg , "/autonav/shared/system" , 10 )
25
28
self .device_state_pub = self .create_publisher (DeviceStateMsg , "/autonav/shared/device" , 10 )
29
+ self .config_sub = self .create_subscription (ConfigurationUpdate , "/autonav/shared/config/updates" , self .config_callback , 10 )
30
+ self .config_pub = self .create_publisher (ConfigurationUpdate , "/autonav/shared/config/updates" , 10 )
31
+ self .config_broadcast_sub = self .create_subscription (ConfigurationBroadcast , "/autonav/shared/config/requests" , self .config_broadcast_callback , 10 )
32
+ self .config_broadcast_pub = self .create_publisher (ConfigurationBroadcast , "/autonav/shared/config/requests" , 10 )
26
33
27
34
self .performance_pub = self .create_publisher (Performance , "/autonav/shared/performance" , 10 )
28
35
self .log_pub = self .create_publisher (Log , "/autonav/shared/log" , 10 )
@@ -35,6 +42,68 @@ def init(self) -> None:
35
42
"""
36
43
pass
37
44
45
+ def _smart_dump_config (self , config ) -> str :
46
+ if isinstance (config , dict ):
47
+ return json .dumps (config )
48
+ else :
49
+ try :
50
+ return json .dumps (config .__dict__ )
51
+ except :
52
+ return str (config )
53
+
54
+ def config_callback (self , msg : ConfigurationUpdate ) -> None :
55
+ """
56
+ Callback for the configuration update topic.
57
+ """
58
+ if msg .device == self .get_name ():
59
+ self .log (f"Received update on our own configuration" , LogLevel .DEBUG )
60
+ self .config = json .loads (msg .json )
61
+ else :
62
+ self .log (f"Received updated on { msg .device } 's configuration" , LogLevel .DEBUG )
63
+
64
+ self .other_cfgs [msg .device ] = json .loads (msg .json )
65
+
66
+ def config_broadcast_callback (self , msg : ConfigurationBroadcast ) -> None :
67
+ """
68
+ Callback for the configuration broadcast topic.
69
+ """
70
+ if msg .target_nodes == [] or self .get_name () in msg .target_nodes :
71
+ self .log (f"Received request for our own configuration to be broadcasted" , LogLevel .DEBUG )
72
+ self .broadcast_config ()
73
+
74
+ def broadcast_config (self ) -> None :
75
+ """
76
+ Broadcast our configuration to the system.
77
+ """
78
+ msg = ConfigurationUpdate ()
79
+ msg .device = self .get_name ()
80
+ msg .json = self ._smart_dump_config (self .config )
81
+ self .config_pub .publish (msg )
82
+
83
+ def request_all_configs (self ) -> None :
84
+ """
85
+ Request the configuration of all devices.
86
+ """
87
+ self .request_config (None )
88
+
89
+ def request_config (self , device : str ) -> None :
90
+ """
91
+ Request the configuration of a specific device.
92
+ """
93
+ msg = ConfigurationBroadcast ()
94
+ if device != None :
95
+ msg .target_nodes = [device ]
96
+ else :
97
+ msg .target_nodes = []
98
+ self .config_broadcast_pub .publish (msg )
99
+
100
+ def write_config (self , config ) -> None :
101
+ """
102
+ Register a configuration object.
103
+ """
104
+ self .config = config
105
+ self .broadcast_config ()
106
+
38
107
def perf_start (self , name : str ) -> None :
39
108
"""
40
109
Start a performance measurement.
@@ -174,4 +243,4 @@ def log(self, message: str, level: LogLevel = LogLevel.INFO) -> None:
174
243
print (f"{ sty .fg (99 , 150 , 79 )} { current_time } { sty .fg .white } | { sty .bg (207 , 62 , 97 )} { level_str } { sty .bg .rs } { sty .fg .white } | { sty .fg (90 , 60 , 146 )} { self .get_name ()} { sty .fg .white } :{ sty .fg (90 , 60 , 146 )} { calling_function } { sty .fg .white } :{ sty .fg (90 , 60 , 146 )} { line_number } { sty .fg .white } - { sty .bg (207 , 62 , 97 )} { message } { sty .rs .all } " )
175
244
return
176
245
177
- print (f"{ sty .fg (99 , 150 , 79 )} { current_time } { sty .fg .white } | { sty .fg (r , g , b )} { level_str } { sty .fg .white } | { sty .fg (90 , 60 , 146 )} { self .get_name ()} { sty .fg .white } :{ sty .fg (90 , 60 , 146 )} { calling_function } { sty .fg .white } :{ sty .fg (90 , 60 , 146 )} { line_number } { sty .fg .white } - { sty .fg (r , g , b )} { message } { sty .rs .all } " )
246
+ print (f"{ sty .fg (99 , 150 , 79 )} { current_time } { sty .fg .white } | { sty .fg (r , g , b )} { level_str } { sty .fg .white } | { sty .fg (90 , 60 , 146 )} { self .get_name ()} { sty .fg .white } :{ sty .fg (90 , 60 , 146 )} { calling_function } { sty .fg .white } :{ sty .fg (90 , 60 , 146 )} { line_number } { sty .fg .white } - { sty .fg (r , g , b )} { message } { sty .rs .all } " )
0 commit comments