diff --git a/Dockerfile b/Dockerfile index f91b662..d9785b1 100644 --- a/Dockerfile +++ b/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 diff --git a/Makefile b/Makefile index 146f05f..f34530e 100644 --- a/Makefile +++ b/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 diff --git a/examples/rover/controller/Dockerfile b/examples/rover/controller/Dockerfile index dbccdf7..fc0580d 100644 --- a/examples/rover/controller/Dockerfile +++ b/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 diff --git a/examples/rover/controller/pyproject.toml b/examples/rover/controller/pyproject.toml index c0cbbc8..50a990b 100644 --- a/examples/rover/controller/pyproject.toml +++ b/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] diff --git a/examples/rover/controller/src/controller/attacks.py b/examples/rover/controller/src/controller/attacks.py index 3f6c624..72b2c80 100644 --- a/examples/rover/controller/src/controller/attacks.py +++ b/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: diff --git a/examples/rover/controller/src/controller/automaton.py b/examples/rover/controller/src/controller/automaton.py index a24856e..220f20f 100644 --- a/examples/rover/controller/src/controller/automaton.py +++ b/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,9 +74,6 @@ 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 @@ -81,16 +81,17 @@ def __post_init__(self): 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: diff --git a/examples/rover/controller/src/controller/errors.py b/examples/rover/controller/src/controller/errors.py new file mode 100644 index 0000000..be4a137 --- /dev/null +++ b/examples/rover/controller/src/controller/errors.py @@ -0,0 +1,7 @@ +from __future__ import annotations + +class RoverError(Exception): + pass + +class TransportError(RoverError): + pass \ No newline at end of file diff --git a/examples/rover/controller/src/main.py b/examples/rover/controller/src/main.py index 6cde422..9b8b00b 100644 --- a/examples/rover/controller/src/main.py +++ b/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)) diff --git a/examples/rover/controller/src/rover.py b/examples/rover/controller/src/rover.py index 84a81b5..8903ad9 100644 --- a/examples/rover/controller/src/rover.py +++ b/examples/rover/controller/src/rover.py @@ -5,6 +5,8 @@ from math import atan, pi from threading import Event, Lock from typing import Literal, NewType +import numpy as np +from scipy.spatial.transform import Rotation from gz.transport13 import Node, Publisher, SubscribeOptions from gz.math7 import Quaterniond @@ -15,8 +17,7 @@ from gz.msgs10.magnetometer_pb2 import Magnetometer from gz.msgs10.pose_v_pb2 import Pose_V -from controller import attacks, automaton - +from controller import attacks, automaton, errors def _pose_logger() -> Logger: logger = getLogger("rover.pose") @@ -107,7 +108,7 @@ def clock(self) -> float: @property def heading(self) -> float: with self._lock: - return self._heading * (180 / pi) + return self._heading @property def roll(self) -> float: @@ -149,9 +150,13 @@ def position(self) -> tuple[float, float, float]: return self._pose.position @property - def heading(self) -> float: + def true_heading(self) -> float: return self._pose.heading + @property + def heading(self) -> float: + return self.true_heading + @property def roll(self) -> float: return self._pose.roll @@ -206,28 +211,16 @@ class NGC(Rover): _velocity: float = field(default=0.0, init=False) _steering_angle: float = field(default=0.0, init=False) - @property - def _heading(self) -> float: - x, y, _ = self._magnetometer.vector - - if y > 0: - heading_ = 90 - (atan(x/y) * 180/pi) - elif y < 0: - heading_ = 270 - (atan(x/y) * 180/pi) - elif x > 0: - heading_ = 180.0 - else: - heading_ = 0.0 - - return heading_ - @property def heading_real(self) -> float: - return self._heading + x, y, _ = self._magnetometer.vector + return -np.arctan2(y,x) @property def heading(self) -> float: - return self._heading + self._magnet.offset(self.clock, self) + x, y, _ = self._magnetometer.vector + offset = self.body_offset() + return -np.arctan2(y+offset[1],x+offset[0]) @property def steering_angle(self) -> float: @@ -260,19 +253,23 @@ def velocity(self, target: float): self._velocity = target self._logger.info(f"Setting velocity to {target}") + def body_offset(self) -> tuple[float,float]: + orientation = Rotation.from_euler("xyz",np.array([0.0,0.0,self.true_heading])) + offset = self._magnet.offset(self.clock, self) + body_offset = orientation.inv().apply(np.array([offset[0],offset[1],0.0])) + return (body_offset[0],body_offset[1]) + + def log_mag_data(self): + x, y, _ = self._magnetometer.vector + offset = self.body_offset() + self._logger.info((f"Magnet data: {x:0.4f},{y:0.4f}. " + f"Offset data: {offset[0]:0.4f},{offset[1]:0.4f}. " + f"Mag with offset: {x+offset[0]:0.4f},{y+offset[1]:0.4f}")) + def wait(self): self._pose.wait() self._magnetometer.wait() - -class RoverError(Exception): - pass - - -class TransportError(RoverError): - pass - - def _create_model( world: str, model: Literal["r1_rover", "ngc_rover"], @@ -291,10 +288,10 @@ def _create_model( logger.debug(f"Reply: {rep}") if not res: - raise TransportError("Failed to send Gazebo message for rover creation") + raise errors.TransportError("Failed to send Gazebo message for rover creation") if not rep.data: - raise RoverError("Could not create rover Gazebo model") + raise errors.RoverError("Could not create rover Gazebo model") return InitializedNode(client) @@ -310,7 +307,7 @@ def _pose_handler( pose_options.msgs_per_sec = 10 if not node.subscribe(Pose_V, f"/world/{world}/pose/info", pose, pose_options): - raise TransportError() + raise errors.TransportError() return pose @@ -326,7 +323,7 @@ def _magnetometer_handler( magnetometer_options.msgs_per_sec = 10 if not node.subscribe(Magnetometer, topic, magnetometer, magnetometer_options): - raise TransportError() + raise errors.TransportError() return magnetometer @@ -344,7 +341,7 @@ def r1(world: str, *, name: str = "r1_rover") -> R1: motors = node.advertise(f"/model/{name}/command/motor_speed", Actuators) if not motors.valid(): - raise TransportError("Could not register publisher for motor control") + raise errors.TransportError("Could not register publisher for motor control") logger.info("Initialized motor topic publisher.") @@ -367,13 +364,13 @@ def ngc(world: str, *, magnet: attacks.Magnet, name: str = "ackermann") -> NGC: motors = node.advertise(f"/model/{name}/command/motor_speed", Actuators) if not motors.valid(): - raise TransportError("Could not register publisher for motor control") + raise errors.TransportError("Could not register publisher for motor control") logger.info("Initialized motor topic publisher.") servos = node.advertise(f"/model/{name}/servo_0", Double) if not servos.valid(): - raise TransportError("Could not register publisher for servo_0 control") + raise errors.TransportError("Could not register publisher for servo_0 control") logger.info("Initialized servo topic publisher.") diff --git a/examples/rover/src/plots.py b/examples/rover/src/plots.py index 3a046b8..ed19220 100644 --- a/examples/rover/src/plots.py +++ b/examples/rover/src/plots.py @@ -41,4 +41,12 @@ def plot(*plots: Plot): plot.color, ) + last_time = times[len(times)-1] + ax.scatter( + [plot.trajectory[last_time][0]], + [plot.trajectory[last_time][1]], + s=None, + c="r", + ) + plt.show(block=True) diff --git a/examples/rover/src/test.py b/examples/rover/src/test.py index 6fb268a..e20eed4 100644 --- a/examples/rover/src/test.py +++ b/examples/rover/src/test.py @@ -11,6 +11,7 @@ import staliro.optimizers import staliro.specifications.rtamt +from controller import errors from controller.attacks import FixedSpeed, GaussianMagnet, SpeedController, Magnet from controller.messages import Start, Result from plots import Plot, plot @@ -40,14 +41,15 @@ def inner(world: str, magnet: Magnet | None, speed: SpeedController | None, freq @click.command("simulation") -@click.option("-f", "--frequency", "freq", type=int, default=2) +@click.option("-f", "--frequency", "freq", type=int, default=10) @click.option("-s", "--speed", type=float, default=5.0) -@click.option("-m", "--magnet", type=float, nargs=2, default=None) +@click.option("-m", "--magnet", type=float, nargs=3, default=None) @click.option("-v", "--verbose", is_flag=True) -def simulation(speed: float, freq: int, magnet: tuple[float, float] | None, *, verbose: bool): +def simulation(speed: float, freq: int, magnet: tuple[float, float, float] | None, *, verbose: bool): + mag_pos = None if magnet: - rng = rand.default_rng() - magnet_ = GaussianMagnet(x=magnet[0], y=magnet[1], rng=rng) + magnet_ = StationaryMagnet(magnitude=magnet[0], x=magnet[1], y=magnet[2]) + mag_pos = (magnet[1],magnet[2]) else: magnet_ = None @@ -55,7 +57,7 @@ def simulation(speed: float, freq: int, magnet: tuple[float, float] | None, *, v firmware_ = firmware(verbose=verbose) result = firmware_.run(gazebo, freq=freq, magnet=magnet_, speed=FixedSpeed(speed)) p = Plot( - magnet=magnet, + magnet=mag_pos, trajectory=staliro.Trace({ step.time: [step.position[0], step.position[1]] for step in result.history }), diff --git a/px4/gazebo.Dockerfile b/px4/gazebo.Dockerfile index f35ebeb..f641381 100644 --- a/px4/gazebo.Dockerfile +++ b/px4/gazebo.Dockerfile @@ -5,7 +5,13 @@ LABEL org.opencontainers.image.source=https://github.com/cpslab-asu/multicosim LABEL org.opencontainers.image.description="MultiCoSim gazebo image with PX4 models and worlds" LABEL org.opencontainers.image.license=BSD-3-Clause -ADD https://github.com/px4/px4-gazebo-models.git#23170a91255d99aea8960d1101541afce0f209d9 px4-gazebo-models/ +RUN apt-get update && apt-get install -y \ + git + +RUN git clone https://github.com/px4/px4-gazebo-models.git px4-gazebo-models/ +WORKDIR "/app/px4-gazebo-models" +RUN git checkout 23170a91255d99aea8960d1101541afce0f209d9 +WORKDIR "/app" RUN mv px4-gazebo-models/models resources/models RUN mv px4-gazebo-models/worlds/* resources/worlds/ RUN rm -r px4-gazebo-models