Rover example and other changes

Dockerfile

@@ -14,7 +14,7 @@ RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/p
 
 FROM ubuntu:22.04 AS base
 
-LABEL org.opencontainers.image.source=https://github.com/cpslab-asu/gzcm
+LABEL org.opencontainers.image.source=https://github.com/cpslab-asu/multicosim
 LABEL org.opencontainers.image.description="Base image for other GZCM images"
 LABEL org.opencontainers.image.license=BSD-3-Clause
 

Makefile

@@ -27,6 +27,9 @@ gazebo: base
 px4: gazebo
 	make -C px4 images
 
-images: base gazebo px4
+rover: base
+	make -C examples/rover images
+
+images: base gazebo px4 rover
 
 .PHONY: all wheel base gazebo px4 images

examples/rover/controller/Dockerfile

@@ -18,6 +18,7 @@ RUN uv venv \
     --python 3.10 \
     --python-preference only-system \
     --relocatable
+RUN uv lock
 RUN uv sync --frozen --group container --no-install-project
 RUN uv pip install /opt/multicosim
 

examples/rover/controller/pyproject.toml

@@ -8,7 +8,8 @@ version = "0.1.0"
 description = "Add your description here"
 requires-python = ">=3.9"
 dependencies = [
-    "numpy~=1.26.0"
+    "numpy~=1.26.0",
+    "scipy~=1.13"
 ]
 
 [dependency-groups]

examples/rover/controller/src/controller/attacks.py

@@ -10,32 +10,31 @@
 
 
 class Magnet(Protocol):
-    def offset(self, time: float, model: Model) -> float:
+    def offset(self, time: float, model: Model) -> tuple[float,float]:
         ...
 
-
 @dataclass()
 class StationaryMagnet(Magnet):
     magnitude: float
+    x: float = 0.0
+    y: float = 0.0
 
-    def offset(self, time: float, model: Model) -> float:
-        return self.magnitude
+    def offset(self, time: float, model: Model) -> tuple[float,float]:
+        p = (self.x, self.y, 0.0)
+        d = euclidean_distance(p, model.position)
+        scale =  self.magnitude / pow(d, 3)
+        return ((p[0] - model.position[0]) * scale/d, (p[1] - model.position[1]) * scale/d)
 
 
 @dataclass()
-class GaussianMagnet(Magnet):
-    x: float
-    y: float
-    rng: random.Generator
-
-    def offset(self, time: float, model: Model) -> float:
-        mu_0 = 4 * pi * 10e-7
-        m = 0.8
+class GaussianMagnet(StationaryMagnet):
+    rng: random.Generator = random.default_rng()
+
+    def offset(self, time: float, model: Model) -> tuple[float,float]:
         p = (self.x, self.y, 0.0)
         d = euclidean_distance(p, model.position)
-        scale = (mu_0 + m) / pow(d, 3)
-
-        return self.rng.normal(0.0, 1.0) * scale
+        scale =  self.rng.normal(0.0,1.0) * self.magnitude / pow(d, 3)
+        return ((p[0] - model.position[0]) * scale/d, (p[1] - model.position[1]) * scale/d)
 
 
 class SpeedController:

examples/rover/controller/src/controller/automaton.py

@@ -6,6 +6,7 @@
 import logging
 import math
 import typing
+import numpy as np
 
 Position: typing.TypeAlias = tuple[float, float, float]
 Command: typing.TypeAlias = typing.Literal[55, 66]
@@ -45,9 +46,11 @@ class Action(enum.IntEnum):
 class State(abc.ABC):
     """An abstract system state representing a behavior of the system."""
     flags: Flags
+
+    time: float = dc.field(default=0.0)
 
     @abc.abstractmethod
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         """Advance the system to the next state."""
 
         ...
@@ -71,26 +74,24 @@ def _create_state_logger(name: str) -> logging.Logger:
 class S1(State):
     LOGGER: typing.ClassVar[logging.Logger] = _create_state_logger("S1")
 
-    time: float = dc.field()
-    step_size: float = dc.field()
-
     def __post_init__(self):
         assert self.flags.check_position
         assert not self.flags.autodrive
         assert not self.flags.update_compass
         assert not self.flags.update_gps
         assert not self.flags.move
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         if self.time >= 5:
             self.LOGGER.info("Wait time exceeded. Transitioning to S2.")
             return S2(
                 flags=dc.replace(self.flags, autodrive=True),
-                initial_position=model.position,
+                time=0.0,
+                initial_position=model.position
             )
 
         self.LOGGER.info(f"Current time: {self.time}, Time remaining: {5 - self.time}")
-        return S1(self.flags, step_size=self.step_size, time=self.time + self.step_size)
+        return S1(self.flags, time=self.time + step_size)
 
 
 def euclidean_distance(p1: Position, p2: Position) -> float:
@@ -103,7 +104,7 @@ def euclidean_distance(p1: Position, p2: Position) -> float:
 class S2(State):
     LOGGER: typing.ClassVar[logging.Logger] = _create_state_logger("S2")
 
-    initial_position: tuple[float, float, float] = dc.field()
+    initial_position: tuple[float, float, float] = dc.field(default=(0.0,0.0,0.0))
 
     def __post_init__(self):
         assert self.flags.check_position
@@ -116,7 +117,7 @@ def __post_init__(self):
     def action(self) -> Action:
         return Action.DRIVE
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         if cmd == 66:
             self.LOGGER.info(f"Received command {cmd}, transitioning to S6")
             return S6(flags=dc.replace(self.flags, autodrive=False, check_position=False))
@@ -128,20 +129,21 @@ def next(self, model: Model, cmd: Command | None) -> State:
             self.LOGGER.info("Distance threshold exceeded. Transitioning to S3.")
             return S3(
                 flags=dc.replace(self.flags, check_position=False, update_compass=True),
-                initial_heading=model.heading,
+                time=0.0,
+                initial_heading=model.heading
             )
 
         self.LOGGER.info(f"Rover position: <{position[0]:.4f}, {position[1]:.4f}, {position[2]:.4f}>.")
         self.LOGGER.info(f"Remaining distance: {7 - distance:.4f}")
-        return S2(self.flags, self.initial_position)
+        return S2(self.flags, time=0.0, initial_position=self.initial_position)
 
 
 @dc.dataclass(frozen=True, slots=True)
 class S3(State):
     LOGGER: typing.ClassVar[logging.Logger] = _create_state_logger("S3")
 
-    initial_heading: float = dc.field()
-    
+    initial_heading: float = dc.field(default=0.0)
+
     def __post_init__(self):
         assert self.flags.autodrive
         assert self.flags.update_compass
@@ -153,26 +155,37 @@ def __post_init__(self):
     def action(self) -> Action:
         return Action.TURN
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         if cmd == 66:
             self.LOGGER.info(f"Received command {cmd}. Transitioning to S8")
             return S8(flags=dc.replace(self.flags, autodrive=False, check_position=False))
 
-        heading = model.heading
+        heading_delta = model.heading - self.initial_heading
 
-        if heading > self.initial_heading:
-            degrees = self.initial_heading + (360 - heading)
-        else:
-            degrees = self.initial_heading - heading
+        # Determines shortest distance and direction when differencing values between pi and -pi
+        if(heading_delta > np.pi):
+            heading_delta =  heading_delta - 2.0*np.pi
+        elif(heading_delta < -np.pi):
+            heading_delta = 2.0*np.pi + heading_delta 
 
-        self.LOGGER.info(f"Current heading: {heading: 0.4f}. Ground truth heading: {model.heading_real:.4f}")
+        degrees = heading_delta*180/np.pi
+
+        self.LOGGER.info((f"Time: {self.time: 0.4f}. "
+                          f"Current heading: {model.heading*180.0/np.pi: 0.4f}. "
+                          f"Ground truth heading: {model.heading_real*180.0/np.pi:.4f}. "
+                          f"Initial heading: {self.initial_heading*180.0/np.pi:.4f}. "
+                          f"True heading: {model.true_heading*180.0/np.pi:.4f}"))
+        model.log_mag_data()
 
         if degrees >= 70:
             self.LOGGER.info("Transitioning to S4")
             return S4(flags=dc.replace(self.flags, update_compass=False, update_gps=True))
+        elif self.time >= 30:
+            self.LOGGER.info("S3 Timeout. Transitioning to S8")
+            return S8(flags=dc.replace(self.flags, autodrive=False, check_position=False))
 
         self.LOGGER.info(f"Degrees to target heading: {70 - degrees}")
-        return S3(self.flags, self.initial_heading)
+        return S3(self.flags, self.time + step_size, self.initial_heading)
 
 
 @dc.dataclass(frozen=True, slots=True)
@@ -190,10 +203,11 @@ def __post_init__(self):
     def action(self) -> Action:
         return Action.TURN
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         self.LOGGER.info("Transitioning to S5")
         return S5(
             flags=dc.replace(self.flags, update_gps=False, move=True),
+            time=0.0,
             initial_position=model.position,
         )
 
@@ -202,7 +216,7 @@ def next(self, model: Model, cmd: Command | None) -> State:
 class S5(State):
     LOGGER: typing.ClassVar[logging.Logger] = _create_state_logger("S5")
 
-    initial_position: Position
+    initial_position: Position = dc.field(default=(0.0,0.0,0.0))
 
     def __post_init__(self):
         assert self.flags.autodrive
@@ -215,7 +229,7 @@ def __post_init__(self):
     def action(self) -> Action:
         return Action.DRIVE
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         if cmd == 66:
             self.LOGGER.info(f"Received command {cmd}. Transitioning to S7")
             return S7(flags=dc.replace(self.flags, autodrive=False, check_position=False))
@@ -227,9 +241,9 @@ def next(self, model: Model, cmd: Command | None) -> State:
             self.LOGGER.info("Distance threshold exceeded. Transitioning to S6.")
             return S6(flags=dc.replace(self.flags, autodrive=False, move=False))
 
-        self.LOGGER.info(f"Rover position: <{position[0]:.4f}, {position[1]:.4f}, {position[2]:.4f}>.")
+        self.LOGGER.info(f"Rover position: <{position[0]:.4f}, {position[1]:.4f}, {position[2]:.4f}>. Heading: {model.true_heading:.4f}")
         self.LOGGER.info(f"Remaining distance: {7 - distance:.4f}")
-        return S5(self.flags, self.initial_position)
+        return S5(self.flags, 0.0, self.initial_position)
 
 
 @dc.dataclass(frozen=True, slots=True)
@@ -244,7 +258,7 @@ def __post_init__(self):
     def is_terminal(self) -> bool:
         return True
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
          return S6(self.flags)
 
 
@@ -263,7 +277,7 @@ def __post_init__(self):
     def action(self) -> Action:
         return Action.DRIVE
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         if cmd == 55:
             self.LOGGER.info(f"Command receieved: {cmd}. Transitioning to S9")
             return S9(self.flags)
@@ -287,7 +301,7 @@ def __post_init__(self):
     def action(self) -> Action:
         return Action.TURN
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         self.LOGGER.info("Transitioning to S7")
         return S7(flags=dc.replace(self.flags, move=True, update_compass=False))
 
@@ -304,19 +318,20 @@ def __post_init__(self):
     def is_terminal(self) -> bool:
         return True
 
-    def next(self, model: Model, cmd: Command | None) -> State:
+    def next(self, model: Model, cmd: Command | None, step_size: float) -> State:
         return S9(self.flags)
 
 
 class Automaton:
     def __init__(self, model: Model, step_size: float):
         self.model = model
-        self.state: State = S1(flags=Flags(), time=0.0, step_size=step_size)
+        self.state: State = S1(flags=Flags())
+        self.step_size = step_size
         self.history: list[State] = []
 
     def step(self, cmd: Command | None):
         self.history.append(self.state)
-        self.state = self.state.next(self.model, cmd)
+        self.state = self.state.next(self.model, cmd, self.step_size)
 
     @property
     def action(self) -> Action:

examples/rover/controller/src/controller/errors.py

@@ -0,0 +1,7 @@
+from __future__ import annotations
+
+class RoverError(Exception):
+    pass
+
+class TransportError(RoverError):
+    pass

examples/rover/controller/src/main.py

@@ -124,7 +124,7 @@ def serve(port: int):
 @click.option("-w", "--world", default="default")
 @click.option("-f", "--frequency", type=int, default=1)
 @click.option("-s", "--speed", type=float, default=5.0)
-@click.option("-m", "--magnet", nargs=2, type=float, default=None)
+@click.option("-m", "--magnet", nargs=3, type=float, default=None)
 def start(
     ctx: click.Context,
     world: str,
@@ -134,7 +134,7 @@ def start(
 ):
     logger: Logger = ctx.obj["logger"]
     logger.info("No port specified, starting controller using defaults.")
-    magnet_: atk.Magnet = atk.GaussianMagnet(magnet[0], magnet[1], rand.default_rng()) if magnet else atk.StationaryMagnet(0.0)
+    magnet_: atk.Magnet = atk.GaussianMagnet(magnet[0], magnet[1], magnet[2], rand.default_rng()) if magnet else atk.StationaryMagnet(0.0)
     speed_ = atk.FixedSpeed(speed)
     history = run(world, frequency, magnet_, speed_, commands=repeat(None))