merging main into me

.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:
@@ -90,15 +93,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):
-        #self.get_logger().info("received 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/managed_steam.xml

@@ -1,23 +1,17 @@
 <launch>
     <node pkg="scr_controller" exec="core">
-        <param name="mode" value="2" /> 
-        <param name="state" value="2" /> 
+        <param name="mode" value="1" /> 
+        <param name="state" value="1" /> 
+        <param name="mobility" value="false" /> 
     </node>
-    <node pkg="scr_logging" exec="logging">
-        <param name="log_to_console" value="true" />
-    </node>
-    <node pkg="scr_configuration" exec="config">
-        <param name="preset" value="default" />
-    </node>
-    <node pkg="autonav_serial" exec="serial_node.py" />
-    <node pkg="autonav_serial" exec="camera.py" />
-    <node pkg="autonav_vision" exec="transformations.py" />
-    <node pkg="autonav_vision" exec="expandify" />
     <node pkg="autonav_filters" exec="filters.py">
-        <param name="latitude_length" value="110944.21" />
-        <param name="longitude_length" value="91065.46" />
+        <param name="latitude_length" value="111086.2" />
+        <param name="longitude_length" value="81978.2" />
         <param name="default_type" value="1" />
     </node>
+    <node pkg="autonav_vision" exec="transformations.py" />
+    <node pkg="autonav_display" exec="display.py" />
+    <node pkg="autonav_vision" exec="combination.py" />
     <node pkg="autonav_manual" exec="steam.py" />
     <node pkg="autonav_manual" exec="steamremote" />
 </launch>

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_manual/src/remote_steamcontroller.cpp

@@ -45,8 +45,8 @@ class SteamJoyNode : public SCR::Node
     json get_default_config() override
     {
         SteamJoyNodeConfig defaultConfig;
-        defaultConfig.throttle_deadzone = 0.03f;
-        defaultConfig.steering_deadzone = 0.03f;
+        defaultConfig.throttle_deadzone = 0.15f;
+        defaultConfig.steering_deadzone = 0.15f;
         defaultConfig.forward_speed = 1.8f;
         defaultConfig.turn_speed = 1.0f;
         defaultConfig.max_turn_speed = 3.14159265f;
@@ -66,6 +66,11 @@ class SteamJoyNode : public SCR::Node
             set_device_state(SCR::DeviceState::READY);
         }
     }
+
+    float lerp(float a, float b, float t)
+    {
+        return a + (b - a) * t;
+    }
 
     void onSteamDataReceived(const autonav_msgs::msg::SteamInput &msg)
     {
@@ -82,18 +87,39 @@ class SteamJoyNode : public SCR::Node
 
         if (abs(msg.ltrig) > throttleDeadzone || abs(msg.rtrig) > throttleDeadzone)
         {
-            throttle = msg.rtrig;
-            throttle = throttle - msg.ltrig;
+            target_throttle = msg.rtrig - msg.ltrig;
+            is_working = true;
         }
 
         if (abs(msg.lpad_x) > steeringDeadzone)
         {
-            steering = msg.lpad_x;
+            target_steering = msg.lpad_x;
+            is_working = true;
+        }
+
+        if (abs(msg.ltrig) < throttleDeadzone && abs(msg.rtrig) < throttleDeadzone)
+        {
+            target_throttle = 0;
+            is_working = false;
         }
 
+        if (abs(msg.lpad_x) < steeringDeadzone)
+        {
+            target_steering = 0;
+            is_working = false;
+        }
+
+        // Generate a forward/angular velocity command that ramps up/down smoothly
+        const float throttleRate = 0.03;
+        const float steeringRate = 0.01;
+        current_throttle = lerp(current_throttle, target_throttle * config.forward_speed, throttleRate * (is_working ? 1 : 1.8));
+        current_steering = lerp(current_steering, target_steering * config.turn_speed, steeringRate * (is_working ? 1 : 1.8));
+
         autonav_msgs::msg::MotorInput input;
-        input.forward_velocity = SCR::Utilities::clamp(throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_speed);
-        input.angular_velocity = -1 * SCR::Utilities::clamp(steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed);
+        // input.forward_velocity = SCR::Utilities::clamp(throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_speed);
+        // input.angular_velocity = -1 * SCR::Utilities::clamp(steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed);
+        input.forward_velocity = SCR::Utilities::clamp(current_throttle, -config.max_forward_speed, config.max_forward_speed);
+        input.angular_velocity = -1 * SCR::Utilities::clamp(current_steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed);
         motor_publisher->publish(input);
     }
 
@@ -104,6 +130,11 @@ class SteamJoyNode : public SCR::Node
 
 private:
     SteamJoyNodeConfig config;
+    float target_throttle = 0;
+    float target_steering = 0;
+    float current_throttle = 0;
+    float current_steering = 0;
+    bool is_working = false;
 };
 
 int main(int argc, char *argv[])

autonav_ws/src/autonav_manual/src/steam.py

@@ -96,6 +96,9 @@ def onButtonReleased(self, button: SteamControllerButton, msTime: float):
             self.set_system_state(SystemStateEnum.DISABLED)
             self.safetyLightsPublisher.publish(toSafetyLights(False, False, 2, 100, "#A020F0"))
 
+    def getClockMs(self):
+        return time.time() * 1000
+
     def onSteamControllerInput(self, _, sci: SteamControllerInput):
         if self.device_state != DeviceStateEnum.OPERATING:
             return