Skip to content

Commit f83dd68

Browse files
dylanzemlindylanzemlin
authored
Merge pull request #11 from SoonerRobotics/feat/shared_configuration
2 parents 2e82362 + 7989474 commit f83dd68

File tree

9 files changed

+24969
-8
lines changed

9 files changed

+24969
-8
lines changed

‎autonav_ws/src/autonav_example_cpp/src/example.cpp

+30
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,36 @@
11
#include "autonav_shared/node.hpp"
22

3+
4+
struct ExampleCPPConfig
5+
{
6+
float alpha;
7+
std::string beta;
8+
int gamma;
9+
bool delta;
10+
std::vector<float> epsilon;
11+
std::vector<std::string> zeta;
12+
std::map<std::string, std::string> eta;
13+
int rid;
14+
15+
NLOHMANN_DEFINE_TYPE_INTRUSIVE(ExampleCPPConfig, alpha, beta, gamma, delta, epsilon, zeta)
16+
};
17+
318
class ExampleCPP : public AutoNav::Node
419
{
520
public:
621
ExampleCPP() : AutoNav::Node("example_cpp")
722
{
23+
auto config = ExampleCPPConfig();
24+
config.alpha = 0.5;
25+
config.beta = "Hello";
26+
config.gamma = 42;
27+
config.delta = true;
28+
config.epsilon = {0.1, 0.2, 0.3};
29+
config.zeta = {"A", "B", "C"};
30+
config.eta = {{"A", "1"}, {"B", "2"}, {"C", "3"}};
31+
config.rid = 1;
32+
write_config(config);
33+
834
log("Hello from ExampleCPP", AutoNav::Logging::DEBUG);
935
log("Hello from ExampleCPP", AutoNav::Logging::INFO);
1036
log("Hello from ExampleCPP", AutoNav::Logging::WARN);
@@ -20,6 +46,10 @@ class ExampleCPP : public AutoNav::Node
2046

2147
perf_start("ExampleCPP::init");
2248
perf_stop("ExampleCPP::init", true);
49+
50+
request_all_configs();
51+
52+
log("Beta (CONFIG): " + config["beta"].get<std::string>(), AutoNav::Logging::DEBUG);
2353
}
2454
};
2555

‎autonav_ws/src/autonav_example_py/src/example.py

+19-3
Original file line numberDiff line numberDiff line change
@@ -3,12 +3,24 @@
33
from autonav_shared.node import Node
44
from autonav_shared.types import LogLevel, DeviceState, SystemState
55
import rclpy
6-
import time
76

87

9-
class Example(Node):
8+
class ExamplePyConfig:
9+
def __init__(self):
10+
self.alpha = 0.5
11+
self.beta = "Hello"
12+
self.gamma = 42
13+
self.delta = True
14+
self.epsilon = [0.1, 0.2, 0.3]
15+
self.zeta = ["A", "B", "C"]
16+
self.eta = {"A": 1, "A": 2, "A": 3}
17+
self.rid = 2
18+
19+
20+
class ExamplePy(Node):
1021
def __init__(self):
1122
super().__init__("autonav_example_py")
23+
self.write_config(ExamplePyConfig())
1224

1325
self.log("Hello from ExamplePy", LogLevel.DEBUG)
1426
self.log("Hello from ExamplePy", LogLevel.INFO)
@@ -22,10 +34,14 @@ def init(self):
2234

2335
self.perf_start("example")
2436
self.perf_stop("example", True)
37+
38+
self.request_all_configs()
39+
40+
self.log(f"Beta (CONFIG): {self.config.beta}", LogLevel.DEBUG)
2541

2642
def main():
2743
rclpy.init()
28-
example = Example()
44+
example = ExamplePy()
2945
rclpy.spin(example)
3046
rclpy.shutdown()
3147

‎autonav_ws/src/autonav_msgs/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@ set(msg_files
2626
"msg/SystemState.msg"
2727
"msg/Ultrasonic.msg"
2828
"msg/Performance.msg"
29+
"msg/ConfigurationBroadcast.msg"
30+
"msg/ConfigurationUpdate.msg"
2931
)
3032

3133
set(srv_files

‎autonav_ws/src/autonav_msgs/msg/ConfigurationBroadcast.msg

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
string[] target_nodes

‎autonav_ws/src/autonav_msgs/msg/ConfigurationUpdate.msg

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
string device
2+
string json

‎autonav_ws/src/autonav_shared/autonav_shared/node.py

+71-2
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
11
import rclpy
22
from rclpy.node import Node as RclpyNode
33
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
55
import sty
66
import time
77
import inspect
8+
import json
89

910

1011
class Node(RclpyNode):
@@ -16,13 +17,19 @@ def __init__(self, name: str) -> None:
1617
self.mobility = False
1718
self.device_states = {}
1819
self.start_times = {}
20+
self.config = {}
21+
self.other_cfgs = {}
1922
self.device_states[name] = DeviceState.OFF
2023

2124
# TODO: Setup all relevant publishers, subscribers, services, clients, etc
2225
self.system_state_sub = self.create_subscription(SystemStateMsg, "/autonav/shared/system", self.system_state_callback, 10)
2326
self.device_state_sub = self.create_subscription(DeviceStateMsg, "/autonav/shared/device", self.device_state_callback, 10)
2427
self.system_state_pub = self.create_publisher(SystemStateMsg, "/autonav/shared/system", 10)
2528
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)
2633

2734
self.performance_pub = self.create_publisher(Performance, "/autonav/shared/performance", 10)
2835
self.log_pub = self.create_publisher(Log, "/autonav/shared/log", 10)
@@ -35,6 +42,68 @@ def init(self) -> None:
3542
"""
3643
pass
3744

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+
38107
def perf_start(self, name: str) -> None:
39108
"""
40109
Start a performance measurement.
@@ -174,4 +243,4 @@ def log(self, message: str, level: LogLevel = LogLevel.INFO) -> None:
174243
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}")
175244
return
176245

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

Comments
 (0)