Fix the configuration system

autonav_ws/src/autonav_display/src/display.py

@@ -5,7 +5,7 @@
 from scr.states import DeviceStateEnum
 # from scr_msgs.msg import SystemState, DeviceState, Log, ConfigurationInstruction
 from scr_msgs.msg import SystemState, DeviceState, ConfigUpdated
-from scr_msgs.srv import SetSystemState
+from scr_msgs.srv import SetSystemState, UpdateConfig
 from std_msgs.msg import Empty
 from autonav_msgs.msg import Position, MotorFeedback, MotorInput, MotorControllerDebug, ObjectDetection, PathingDebug, GPSFeedback, IMUData, Conbus
 from sensor_msgs.msg import CompressedImage
@@ -74,7 +74,8 @@ def __init__(self):
 		self.broadcastPublisher = self.create_publisher(Empty, "/scr/state/broadcast", 20)
 		self.configurationInstructionSubscriber = self.create_subscription(ConfigUpdated, "/scr/config_updated", self.configurationInstructionCallback, 100)
 		# self.logSubscriber = self.create_subscription(Log, "/scr/logging", self.logCallback, 20)
-		# self.configurationInstructionPublisher = self.create_publisher(ConfigurationInstruction, "/scr/configuration", 100)
+		# self.configurationInstructionPublisher = self.create_publisher(ConfigUpdated, "/scr/config_updated", 100)
+		self.configUpdateClient = self.create_client(UpdateConfig, "/scr/update_config_client")
 
 		self.positionSubscriber = self.create_subscription(Position, "/autonav/position", self.positionCallback, 20)
 		self.motorFeedbackSubscriber = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.motorFeedbackCallback, 20)
@@ -142,14 +143,12 @@ async def consumer(self, websocket):
 			if obj["op"] == "broadcast":
 				self.broadcastPublisher.publish(Empty())
 
-			# if obj["op"] == "configuration" and "device" in obj and "opcode" in obj:
-			# 	msg = ConfigurationInstruction()
-			# 	msg.device = str(obj["device"])
-			# 	msg.opcode = int(obj["opcode"])
-			# 	msg.data = obj["data"] if "data" in obj else []
-			# 	msg.address = str(obj["address"]) if "address" in obj else ""
-			# 	msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0
-			# 	self.configurationInstructionPublisher.publish(msg)
+			if obj["op"] == "configuration" and "device" in obj and "json" in obj:
+				self.log("Received configuration instruction for " + obj["device"])
+				config_packet = UpdateConfig.Request()
+				config_packet.device = obj["device"]
+				config_packet.json = json.dumps(obj["json"])
+				self.configUpdateClient.call_async(config_packet)
 
 			if obj["op"] == "get_nodes":
 				nodes = self.get_node_names()
@@ -161,7 +160,8 @@ async def consumer(self, websocket):
 				self.pushSendQueue(json.dumps({
 					"op": "get_nodes_callback",
 					"nodes": nodes,
-					"states": node_states
+					"states": node_states,
+					"configs": self.node_configs
 				}), unique_id)
 
 			if obj["op"] == "set_system_state":

autonav_ws/src/autonav_filters/src/filters.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python3
 
+import json
+from types import SimpleNamespace
 from autonav_msgs.msg import MotorFeedback, GPSFeedback, Position, IMUData
 from scr.states import DeviceStateEnum, SystemStateEnum, SystemModeEnum
 from particlefilter import ParticleFilter
@@ -15,16 +17,11 @@ class FilterType(IntEnum):
     PARTICLE_FILTER = 1
 
 
-CONFIG_FILTER_TYPE = "filter_type"
-CONFIG_DEGREE_OFFSET = "degree_offset"
-CONFIG_SEED_HEADING = "seed_heading"
-
-
 class FiltersNodeConfig:
     def __init__(self):
-        self.filterType = FilterType.PARTICLE_FILTER
-        self.degreeOffset = 107.0
-        self.seedHeading = False
+        self.filter_type = FilterType.PARTICLE_FILTER
+        self.degree_offset = 107.0
+        self.seed_heading = False
 
 
 class FiltersNode(Node):
@@ -40,10 +37,16 @@ def __init__(self):
 
         self.pf = ParticleFilter(self.latitudeLength, self.longitudeLength)
         self.reckoning = DeadReckoningFilter()
-        self.config = FiltersNodeConfig()
+        self.config = self.get_default_config()
 
         self.onReset()
 
+    def config_updated(self, jsonObject):
+        self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d))
+
+    def get_default_config(self):
+        return FiltersNodeConfig()
+
     def init(self):
         self.create_subscription(GPSFeedback, "/autonav/gps", self.onGPSReceived, 20)
         self.create_subscription(IMUData, "/autonav/imu", self.onIMUReceived, 20);
@@ -59,11 +62,11 @@ def getRealHeading(self, heading: float):
         if heading < 0:
             heading = 360 + -heading
 
-        heading += self.config.degreeOffset
+        heading += self.config.degree_offset
         return heading
 
     def onReset(self):
-        if self.lastIMUReceived is not None and self.config.seedHeading:
+        if self.lastIMUReceived is not None and self.config.seed_heading:
             self.reckoning.reset(self.getRealHeading(self.lastIMUReceived.heading))
             self.pf.init_particles(self.getRealHeading(self.lastIMUReceived.heading), True)
         else:
@@ -86,14 +89,14 @@ def onGPSReceived(self, msg: GPSFeedback):
 
         self.lastGps = msg
 
-        filterType = self.config.filterType
+        filterType = self.config.filter_type
         if filterType == FilterType.PARTICLE_FILTER:
             self.pf.gps(msg)
         elif filterType == FilterType.DEAD_RECKONING:
             self.reckoning.gps(msg)
 
     def onMotorFeedbackReceived(self, msg: MotorFeedback):
-        filterType = self.config.filterType
+        filterType = self.config.filter_type
         averages = None
         if filterType == FilterType.PARTICLE_FILTER:
             averages = self.pf.feedback(msg)

autonav_ws/src/autonav_nav/src/astar.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python3
 
+import json
+from types import SimpleNamespace
 from autonav_msgs.msg import Position, IMUData, PathingDebug, SafetyLights
 from scr_msgs.msg import SystemState
 from scr.node import Node
@@ -78,9 +80,15 @@ def __init__(self):
 
         self.latitudeLength = self.declare_parameter("latitude_length", 111086.2).get_parameter_value().double_value
         self.longitudeLength = self.declare_parameter("longitude_length", 81978.2).get_parameter_value().double_value
-        self.config = AStarNodeConfig()
+        self.config = self.get_default_config()
         self.onReset()
 
+    def config_updated(self, jsonObject):
+        self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d))
+
+    def get_default_config(self):
+        return AStarNodeConfig()
+
     def init(self):
         self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.onConfigSpaceReceived, 20)
         self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.onPoseReceived, 20)

autonav_ws/src/autonav_nav/src/path_resolver.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python3
 
+import json
+from types import SimpleNamespace
 from autonav_msgs.msg import MotorInput, Position, SafetyLights
 from scr_msgs.msg import SystemState
 from scr.node import Node
@@ -36,13 +38,13 @@ def toSafetyLights(autonomous: bool, eco: bool, mode: int, brightness: int, colo
 
 class PathResolverNodeConfig:
     def __init__(self):
-        self.FORWARD_SPEED = 2.1
-        self.REVERSE_SPEED = -0.4
-        self.RADIUS_MULTIPLIER = 1.2
-        self.RADIUS_MAX = 4.0
-        self.RADIUS_START = 0.7
-        self.ANGULAR_AGGRESSINON = 2.2
-        self.MAX_ANGULAR_SPEED = 0.5
+        self.forward_speed = 2.1
+        self.reverse_speed = -0.4
+        self.radius_multiplier = 1.2
+        self.radius_max = 4.0
+        self.radius_start = 0.7
+        self.angular_aggression = 2.2
+        self.max_angular_speed = 0.5
 
 
 class PathResolverNode(Node):
@@ -58,11 +60,17 @@ def init(self):
         self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.on_position_received, 20)
         self.motor_publisher = self.create_publisher(MotorInput, "/autonav/MotorInput", 20)
         self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 20)
-        self.config = PathResolverNodeConfig()
+        self.config = self.get_default_config()
 
         self.create_timer(0.05, self.onResolve)
         self.set_device_state(DeviceStateEnum.READY)
 
+    def config_updated(self, jsonObject):
+        self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d))
+
+    def get_default_config(self):
+        return PathResolverNodeConfig()
+
     def reset(self):
         self.position = None
         self.backCount = -1
@@ -105,10 +113,10 @@ def onResolve(self):
 
         cur_pos = (self.position.x, self.position.y)
         lookahead = None
-        radius = self.config.RADIUS_START
-        while lookahead is None and radius <= self.config.RADIUS_MAX:
+        radius = self.config.radius_start
+        while lookahead is None and radius <= self.config.radius_max:
             lookahead = self.pure_pursuit.get_lookahead_point(cur_pos[0], cur_pos[1], radius)
-            radius *= self.config.RADIUS_MULTIPLIER
+            radius *= self.config.radius_multiplier
 
         motor_packet = MotorInput()
         motor_packet.forward_velocity = 0.0
@@ -117,9 +125,9 @@ def onResolve(self):
         if self.backCount == -1 and (lookahead is not None and ((lookahead[1] - cur_pos[1]) ** 2 + (lookahead[0] - cur_pos[0]) ** 2) > 0.25):
             angle_diff = math.atan2(lookahead[1] - cur_pos[1], lookahead[0] - cur_pos[0])
             error = self.get_angle_difference(angle_diff, self.position.theta) / math.pi
-            forward_speed = self.config.FORWARD_SPEED * (1 - abs(error)) ** 5
+            forward_speed = self.config.forward_speed * (1 - abs(error)) ** 5
             motor_packet.forward_velocity = forward_speed
-            motor_packet.angular_velocity = clamp(error * self.config.ANGULAR_AGGRESSINON, -self.config.MAX_ANGULAR_SPEED, self.config.MAX_ANGULAR_SPEED)
+            motor_packet.angular_velocity = clamp(error * self.config.angular_aggression, -self.config.max_angular_speed, self.config.max_angular_speed)
 
             if self.status == 0:
                 self.safety_lights_publisher.publish(toSafetyLights(True, False, 2, 255, "#FFFFFF"))
@@ -132,7 +140,7 @@ def onResolve(self):
                 self.status = 0
                 self.backCount -= 1
 
-            motor_packet.forward_velocity = self.config.REVERSE_SPEED
+            motor_packet.forward_velocity = self.config.reverse_speed
             motor_packet.angular_velocity = BACK_SPEED if IS_SOUTH else (-1 * BACK_SPEED)
 
         if not self.mobility:

autonav_ws/src/scr/scr/node.py

@@ -41,6 +41,7 @@ def __init__(self, node_name):
         self.system_state = SystemStateEnum.DISABLED
         self.system_mode = SystemModeEnum.COMPETITION
         self.device_states = {}
+        self.node_configs = {}
         self.mobility = False
         self.perf_measurements = {}
 
@@ -115,6 +116,7 @@ def on_system_state(self, msg: SystemState):
         self.mobility = msg.mobility
 
     def on_config_updated(self, msg: ConfigUpdated):
+        self.node_configs[msg.device] = json.loads(msg.json)
         if msg.device is None or msg.device != self.identifier:
             return
 

display/scripts/globals.js

@@ -16,6 +16,7 @@ var deviceStates = {};
 var logs = [];
 var iterator = 0;
 var iterators = [];
+var debug = false;
 
 var addressKeys = {
 	"autonav_serial_imu": {
@@ -34,14 +35,22 @@ var addressKeys = {
 	"autonav_vision_transformer": {
 		"internal_title": "[Vision] Transformer",
 		"lower_hue": "int",
-		"lower_saturation": "int",
-		"lower_value": "int",
+		"lower_sat": "int",
+		"lower_val": "int",
 		"upper_hue": "int",
-		"upper_saturation": "int",
-		"upper_value": "int",
-		"blur": "int",
+		"upper_sat": "int",
+		"upper_val": "int",
+		"blur_weight": "int",
 		"blur_iterations": "int",
-		"region_of_disinterest_offset": "int"
+		"rod_offset": "int",
+		"map_res": "int"
+	},
+
+	"autonav_vision_expandifier": {
+		"internal_title": "[Vision] Expandifier",
+		"horizontal_fov": "int",
+		"map_res": "int",
+		"vertical_fov": "float"
 	},
 
 	"autonav_filters": {
@@ -66,8 +75,8 @@ var addressKeys = {
 
 	"autonav_nav_astar": {
 		"internal_title": "[Navigation] A*",
-		"pop_distance": "float",
-		"direction": {
+		"waypointPopDistance": "float",
+		"waypointDirection": {
 			0: "North",
 			1: "South",
 			2: "Misc 1",
@@ -76,8 +85,8 @@ var addressKeys = {
 			5: "Misc 4",
 			6: "Misc 5",
 		},
-		"use_only_waypoints": "bool",
-		"waypoint_delay": "float",
+		"useOnlyWaypoints": "bool",
+		"waypointDelay": "float",
 	},
 
 	"autonav_nav_resolver": {
@@ -91,17 +100,10 @@ var addressKeys = {
 		"max_angular_speed": "float"
 	},
 
-	"autonav_playback": {
-		"internal_title": "[Playback]",
-		"record_imu": "bool",
-		"record_gps": "bool",
-		"record_position": "bool",
-		"record_feedback": "bool",
-		"record_objectdetection": "bool",
-		"record_manual": "bool",
-		"record_autonomous": "bool",
-		"record_input": "bool",
-		"record_debugfeedback": "bool",
+	"autonav_image_combiner": {
+		"internal_title": "[Image Combiner]",
+		"overlap": "int",
+		"map_res": "int"
 	}
 }