Initial merge of dual camera

.vscode/settings.json

@@ -92,5 +92,6 @@
         "coroutine": "cpp",
         "source_location": "cpp"
     },
-    "cmake.configureOnOpen": false
+    "cmake.configureOnOpen": false,
+    "ros.distro": "humble"
 }

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
@@ -62,7 +62,9 @@ def __init__(self):
 		self.limiter.setLimit("/autonav/position", 5)
 		self.limiter.setLimit("/autonav/camera/compressed/left", 2)
 		self.limiter.setLimit("/autonav/camera/compressed/right", 2)
-		self.limiter.setLimit("/autonav/cfg_space/raw/image", 5)
+		self.limiter.setLimit("/autonav/cfg_space/raw/image/left", 5)
+		self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5)
+		self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5)
 		self.limiter.setLimit("/autonav/debug/astar/image", 5)
 
 		self.get_logger().info("Broadcasting on ws://{}:{}".format(self.host, self.port))
@@ -72,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)
@@ -87,9 +90,12 @@ def __init__(self):
 
 		self.systemStateService = self.create_client(SetSystemState, "/scr/state/set_system_state")
 
-		self.cameraSubscriber = self.create_subscription(CompressedImage, "/autonav/camera/compressed", self.cameraCallback, 20)
-		self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image", self.filteredCallback, 20)
-		self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, 20)
+		self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.cameraCallbackLeft, 20)
+		self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.cameraCallbackRight, 20)
+		self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, 20)
+		self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, 20)
+		self.bigboiSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.bigboiCallback, 20)
+		self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/expanded/image", self.debugAStarCallback, 20)
 
 		self.get_logger().info("Starting event loop")
 
@@ -137,22 +143,25 @@ 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()
 				for i in range(len(nodes)):
 					nodes[i] = nodes[i].replace("/", "")
+				node_states = {}
+				for identifier in nodes:
+					node_states[identifier] = self.device_states[identifier] if identifier in self.device_states else 0
 				self.pushSendQueue(json.dumps({
 					"op": "get_nodes_callback",
-					"nodes": nodes
+					"nodes": nodes,
+					"states": node_states,
+					"configs": self.node_configs
 				}), unique_id)
 
 			if obj["op"] == "set_system_state":
@@ -336,30 +345,72 @@ def motorControllerDebugCallback(self, msg: MotorControllerDebug):
 			"right_motor_output": msg.right_motor_output
 		}))
 
-	def cameraCallback(self, msg: CompressedImage):
-		if not self.limiter.use("/autonav/camera/compressed"):
+	def cameraCallbackLeft(self, msg: CompressedImage):
+		if not self.limiter.use("/autonav/camera/compressed/left"):
+			return
+
+		byts = msg.data.tobytes()
+		base64_str = base64.b64encode(byts).decode("utf-8")
+
+		self.pushSendQueue(json.dumps({
+			"op": "data",
+			"topic": "/autonav/camera/compressed/left",
+			"format": msg.format,
+			"data": base64_str
+		}))
+
+	def cameraCallbackRight(self, msg: CompressedImage):
+		if not self.limiter.use("/autonav/camera/compressed/right"):
+			return
+
+		byts = msg.data.tobytes()
+		base64_str = base64.b64encode(byts).decode("utf-8")
+
+		self.pushSendQueue(json.dumps({
+			"op": "data",
+			"topic": "/autonav/camera/compressed/right",
+			"format": msg.format,
+			"data": base64_str
+		}))
+
+	def filteredCallbackLeft(self, msg: CompressedImage):
+		if not self.limiter.use("/autonav/cfg_space/raw/image/left"):
+			return
+
+		byts = msg.data.tobytes()
+		base64_str = base64.b64encode(byts).decode("utf-8")
+
+		self.pushSendQueue(json.dumps({
+			"op": "data",
+			"topic": "/autonav/cfg_space/raw/image/left",
+			"format": msg.format,
+			"data": base64_str
+		}))
+
+	def filteredCallbackRight(self, msg: CompressedImage):
+		if not self.limiter.use("/autonav/cfg_space/raw/image/right"):
 			return
 
 		byts = msg.data.tobytes()
 		base64_str = base64.b64encode(byts).decode("utf-8")
 
 		self.pushSendQueue(json.dumps({
 			"op": "data",
-			"topic": "/autonav/camera/compressed",
+			"topic": "/autonav/cfg_space/raw/image/right",
 			"format": msg.format,
 			"data": base64_str
 		}))
 
-	def filteredCallback(self, msg: CompressedImage):
-		if not self.limiter.use("/autonav/cfg_space/raw/image"):
+	def bigboiCallback(self, msg: CompressedImage):
+		if not self.limiter.use("/autonav/cfg_space/raw/debug"):
 			return
 
 		byts = msg.data.tobytes()
 		base64_str = base64.b64encode(byts).decode("utf-8")
 
 		self.pushSendQueue(json.dumps({
 			"op": "data",
-			"topic": "/autonav/cfg_space/raw/image",
+			"topic": "/autonav/cfg_space/raw/debug",
 			"format": msg.format,
 			"data": base64_str
 		}))

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_launch/launch/simulation.xml

@@ -15,4 +15,5 @@
     <node pkg="autonav_vision" exec="transformations.py" />
     <node pkg="autonav_vision" exec="expandify" />
     <node pkg="autonav_display" exec="display.py" />
+    <node pkg="autonav_vision" exec="combination.py" />
 </launch>

autonav_ws/src/autonav_launch/launch/test.xml

@@ -10,6 +10,7 @@
         <param name="default_type" value="1" />
     </node>
     <node pkg="autonav_vision" exec="transformations.py" />
+    <node pkg="autonav_vision" exec="combination.py" />
     <node pkg="autonav_vision" exec="expandify" />
     <node pkg="autonav_display" exec="display.py" />
 </launch>

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/autonav_vision/CMakeLists.txt

@@ -18,17 +18,12 @@ find_package(sensor_msgs REQUIRED)
 find_package(scr REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(image_transport REQUIRED)
-
-# Install Python executables
-install(PROGRAMS
-  src/transformations.py
-  DESTINATION lib/${PROJECT_NAME}
-)
 
 # Install Python programs
 install(PROGRAMS
   src/circumscriber.py
   src/transformations.py
+  src/combination.py
   DESTINATION lib/${PROJECT_NAME}
 )