Skip to content

LH2: send map size with computed locations #98

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
aabadie wants to merge 8 commits into from
Closed

LH2: send map size with computed locations #98

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
a2906f7
tox.ini: run static checks before tests
aabadie May 24, 2023
8e6bcc0
treewide: add new message type to send LH2 location along with map size
aabadie May 24, 2023
3f89879
dotbot/frontend: add UI elements and rest functions for fetching/sett…
aabadie May 24, 2023
55b64c3
dotbot/frontend: adapt test
aabadie May 24, 2023
d722e89
dotbot/tests: test lh2 calibration size in server API
aabadie May 24, 2023
c584472
protocol: use send lh2 location in mm instead of relative [0,1] values
aabadie May 25, 2023
33084f4
dotbot/tests: fix rebase issues
aabadie Sep 15, 2023
236a5c5
dotbot/tests/test_server.py: fix pydantic deprecation warning dict =>…
aabadie Sep 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions dotbot/controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -362,9 +362,15 @@ def handle_received_payload(
header,
PayloadType.LH2_LOCATION,
LH2Location(
int(dotbot.lh2_position.x * 1e6),
int(dotbot.lh2_position.y * 1e6),
int(dotbot.lh2_position.z * 1e6),
int(
dotbot.lh2_position.x
* self.lh2_manager.calibration_data.width
),
int(
dotbot.lh2_position.y
* self.lh2_manager.calibration_data.height
),
int(dotbot.lh2_position.z * 1e3),
),
)
)
Expand Down
3 changes: 3 additions & 0 deletions dotbot/frontend/src/DotBots.test.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,9 @@ const server = setupServer(
rest.get('/controller/lh2/calibration', (req, res, ctx) => {
return res(ctx.json({state: "done"}));
}),
rest.get('/controller/lh2/calibration/size', (req, res, ctx) => {
return res(ctx.json({width: "100", height: "100"}));
}),
rest.put('/controller/dotbots/:address/:application/move_raw', (req, res, ctx) => {
return res();
}),
Expand Down
33 changes: 31 additions & 2 deletions dotbot/frontend/src/DotBotsMap.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,8 @@ import { ApplicationType } from "./constants";

import {
apiFetchLH2CalibrationState, apiApplyLH2Calibration,
apiAddLH2CalibrationPoint, inactiveAddress
apiAddLH2CalibrationPoint, apiFetchLH2MapSize, apiUpdateLH2MapSize,
inactiveAddress
} from "./rest";

const referencePoints = [
Expand Down Expand Up @@ -169,6 +170,9 @@ export const DotBotsMap = (props) => {
const [ displayGrid, setDisplayGrid ] = useState(true);
const [ calibrationFetched, setCalibrationFetched ] = useState(false);
const [ calibrationState, setCalibrationState ] = useState("unknown");
const [ mapSizeFetched, setMapSizeFetched ] = useState(false);
const [ mapWidth, setMapWidth ] = useState(200);
const [ mapHeight, setMapHeight ] = useState(200);
const [ pointsChecked, setPointsChecked ] = useState([false, false, false, false]);

const fetchCalibrationState = useCallback(async () => {
Expand All @@ -178,6 +182,14 @@ export const DotBotsMap = (props) => {
}, [setCalibrationFetched, setCalibrationState]
);

const fetchMapSize = useCallback(async () => {
const size = await apiFetchLH2MapSize().catch((error) => console.error(error));
setMapWidth(size.width);
setMapHeight(size.height);
setMapSizeFetched(true);
}, [setMapSizeFetched, setMapWidth, setMapHeight]
);

const pointClicked = (index) => {
let pointsCheckedTmp = pointsChecked.slice();
pointsCheckedTmp[index] = true;
Expand All @@ -191,6 +203,7 @@ export const DotBotsMap = (props) => {
setCalibrationState("running");
} else if (calibrationState === "ready") {
setCalibrationState("done");
apiUpdateLH2MapSize(mapWidth, mapHeight);
apiApplyLH2Calibration();
}
};
Expand All @@ -215,10 +228,17 @@ export const DotBotsMap = (props) => {
if (!calibrationFetched) {
fetchCalibrationState();
}
if (!mapSizeFetched) {
fetchMapSize();
}
if (pointsChecked.every(v => v === true)) {
setCalibrationState("ready");
}
}, [calibrationFetched, fetchCalibrationState, pointsChecked, setCalibrationState]);
}, [
calibrationFetched, fetchCalibrationState,
mapSizeFetched, fetchMapSize,
pointsChecked, setCalibrationState
]);

let calibrationButtonLabel = "Start calibration";
let calibrationButtonClass = "btn-primary";
Expand Down Expand Up @@ -292,6 +312,15 @@ export const DotBotsMap = (props) => {
<label htmlFor="dotbotHistorySize">Position history size:</label>
<input className="form-control my-1 mr-sm-2" type="number" id="dotbotHistorySize" min="10" max="1000" value={props.historySize} onChange={event => props.setHistorySize(event.target.value)}/>
</form>
<form className="form-inline">
<label htmlFor="dotbotMapWidth">Map width (in mm):</label>
<input className="form-control my-1 mr-sm-2" type="number" id="dotbotMapWidth" min="10" max="65535" value={mapWidth} onChange={event => setMapWidth(event.target.value)}/>
<label htmlFor="dotbotMapHeight">Map height (in mm):</label>
<input className="form-control my-1 mr-sm-2" type="number" id="dotbotMapHeight" min="10" max="65535" value={mapHeight} onChange={event => setMapHeight(event.target.value)}/>
</form>
{/* <div className="d-flex">
<button className="btn btn-sm btn-primary" onClick={updateMapSize}>Update map size</button>
</div> */}
<div className="d-flex">
<button className={`btn btn-sm ${calibrationButtonClass}`} onClick={calibrateClicked}>{calibrationButtonLabel}</button>
</div>
Expand Down
14 changes: 14 additions & 0 deletions dotbot/frontend/src/rest.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,3 +86,17 @@ export const apiAddLH2CalibrationPoint = async (index) => {
`${process.env.REACT_APP_DOTBOTS_BASE_URL}/controller/lh2/calibration/${index}`,
);
}

export const apiFetchLH2MapSize = async (width, height) => {
return await axios.get(
`${process.env.REACT_APP_DOTBOTS_BASE_URL}/controller/lh2/calibration/size`,
).then(res => res.data);
}

export const apiUpdateLH2MapSize = async (width, height) => {
return await axios.put(
`${process.env.REACT_APP_DOTBOTS_BASE_URL}/controller/lh2/calibration/size`,
{width: width, height: height},
{ headers: { 'Content-Type': 'application/json' } }
);
}
18 changes: 14 additions & 4 deletions dotbot/lighthouse2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,11 @@
import numpy as np

from dotbot.logger import LOGGER
from dotbot.models import DotBotLH2Position, DotBotCalibrationStateModel
from dotbot.models import (
DotBotLH2Position,
DotBotCalibrationStateModel,
DotBotCalibrationSizeModel,
)
from dotbot.protocol import Lh2RawData


Expand Down Expand Up @@ -97,6 +101,8 @@ class CalibrationData:
random_rodriguez: np.array
normal: np.array
m: np.array
width: int = 200
height: int = 200


class LighthouseManagerState(Enum):
Expand All @@ -115,6 +121,7 @@ def __init__(self):
self.state = LighthouseManagerState.NotCalibrated
self.reference_points = REFERENCE_POINTS_DEFAULT
Path.mkdir(CALIBRATION_DIR, exist_ok=True)
self.calibration_size = DotBotCalibrationSizeModel()
self.calibration_output_path = CALIBRATION_DIR / "calibration.out"
self.calibration_data = self._load_calibration()
self.calibration_points = np.zeros(
Expand Down Expand Up @@ -245,13 +252,16 @@ def compute_calibration(self): # pylint: disable=too-many-locals
)

self.calibration_data = CalibrationData(zeta, random_rodriguez, n, M)

with open(self.calibration_output_path, "wb") as output_file:
pickle.dump(self.calibration_data, output_file)
self.save_calibration()

self.state = LighthouseManagerState.Calibrated
self.logger.info("Calibration done", data=self.calibration_data)

def save_calibration(self):
"""Store the calibration data in a file."""
with open(self.calibration_output_path, "wb") as output_file:
pickle.dump(self.calibration_data, output_file)

def compute_position(self, raw_data: Lh2RawData) -> Optional[DotBotLH2Position]:
"""Compute the position coordinates from LH2 raw data and available calibration."""
if self.state != LighthouseManagerState.Calibrated:
Expand Down
9 changes: 9 additions & 0 deletions dotbot/models.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,13 @@ class DotBotCalibrationStateModel(BaseModel):
state: str


class DotBotCalibrationSizeModel(BaseModel):
"""Model that holds the width and height of the LH2 calibration whole map."""

width: int = 200 # size in millimeters
height: int = 200


class DotBotMoveRawCommandModel(BaseModel):
"""Model class that defines a move raw command."""

Expand All @@ -46,6 +53,8 @@ class DotBotLH2Position(BaseModel):
x: float
y: float
z: float
width: int = 200
height: int = 200


class DotBotControlModeModel(BaseModel):
Expand Down
16 changes: 8 additions & 8 deletions dotbot/protocol.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
from dataclasses import dataclass


PROTOCOL_VERSION = 8
PROTOCOL_VERSION = 9


class PayloadType(Enum):
Expand Down Expand Up @@ -208,17 +208,17 @@ class LH2Location(ProtocolData):
@property
def fields(self) -> List[ProtocolField]:
return [
ProtocolField(self.pos_x, name="x", length=4),
ProtocolField(self.pos_y, name="y", length=4),
ProtocolField(self.pos_z, name="z", length=4),
ProtocolField(self.pos_x, name="x", length=2),
ProtocolField(self.pos_y, name="y", length=2),
ProtocolField(self.pos_z, name="z", length=2),
]

@staticmethod
def from_bytes(bytes_) -> ProtocolData:
return LH2Location(
int.from_bytes(bytes_[0:4], "little"),
int.from_bytes(bytes_[4:8], "little"),
int.from_bytes(bytes_[8:12], "little"),
pos_x=int.from_bytes(bytes_[0:2], "little"),
pos_y=int.from_bytes(bytes_[2:4], "little"),
pos_z=int.from_bytes(bytes_[4:6], "little"),
)


Expand Down Expand Up @@ -402,7 +402,7 @@ def from_bytes(bytes_: bytes):
elif payload_type == PayloadType.LH2_RAW_DATA:
values = Lh2RawData.from_bytes(bytes_[25:45])
elif payload_type == PayloadType.LH2_LOCATION:
values = LH2Location.from_bytes(bytes_[25:37])
values = LH2Location.from_bytes(bytes_[25:31])
elif payload_type == PayloadType.ADVERTISEMENT:
values = Advertisement.from_bytes(None)
elif payload_type == PayloadType.GPS_POSITION:
Expand Down
26 changes: 26 additions & 0 deletions dotbot/server.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
from dotbot.logger import LOGGER
from dotbot.models import (
DotBotCalibrationStateModel,
DotBotCalibrationSizeModel,
DotBotModel,
DotBotQueryModel,
DotBotAddressModel,
Expand Down Expand Up @@ -312,6 +313,31 @@ async def controller_get_lh2_calibration():
return app.controller.lh2_manager.state_model


@app.put(
path="/controller/lh2/calibration/size",
summary="Set the size of the LH2 map.",
tags=["dotbots"],
)
async def controller_set_lh2_size(size: DotBotCalibrationSizeModel):
"""Set the size of the LH2 map."""
app.controller.lh2_manager.calibration_data.width = size.width
app.controller.lh2_manager.calibration_data.height = size.height


@app.get(
path="/controller/lh2/calibration/size",
response_model=DotBotCalibrationSizeModel,
summary="Get the size of the LH2 map.",
tags=["dotbots"],
)
async def controller_lh2_size():
"""Set the width of the LH2 map."""
return DotBotCalibrationSizeModel(
width=app.controller.lh2_manager.calibration_data.width,
height=app.controller.lh2_manager.calibration_data.height,
)


@app.websocket("/controller/ws/status")
async def websocket_endpoint(websocket: WebSocket):
"""Websocket server endpoint."""
Expand Down
Loading