$$\newcommand{\vmath}[1]{\mathsf{#1}} \newcommand{\mapsfrom}{\leftarrow\!\shortmid} \newcommand{\maps}{\mapsto} \newcommand{\exc}{\vmath{\colF{exec}}} \newcommand{\eval}{\vmath{\colR{eval}}} \newcommand{\ufloor}{{\vmath{\colL floor}}} \newcommand{\uceil}{{\vmath{\colU ceil}}} \newcommand{\triv}{\text{Triv}} \newcommand{\idFunc}{\mathrm{Id}} \newcommand{\reals}{\mathbb{R}} \newcommand{\One}{\mathbf{1}} \renewcommand{\mathbbm}[1]{\mathbb{#1}} \renewcommand{\mathscr}[1]{\mathcal{#1}} \newcommand{\varocircle}{⦾} \newcommand{\varotimes}{⊗} \newcommand{\varovee}{(\vee)} \newcommand{\colR}{\color{darkred}} \newcommand{\colF}{\color{darkgreen}} \newcommand{\colH}{\color{blue}} \newcommand{\colI}{\color{orange}} \newcommand{\rtof}{{\colH{\varphi}}} \newcommand{\ftor}{{\colH{h}}} \newcommand{\ftoR}{{\colH{H}}} \newcommand{\Rcomp}{{\mathbb{R}}^{*}_{\small+}} \newcommand{\nonNegRealsComp}{\Rcomp} \newcommand{\nonNegReals}{\mathbb{R}_+} \newcommand{\Rcpu}[1]{\Rcomp{}[\textrm{#1}]} \newcommand{\funsp}{{\colF{\mathscr{F}}}} \newcommand{\impsp}{{\colI{\mathscr{I}}}} \newcommand{\ressp}{{\colR{\mathscr{R}}}} \newcommand{\funleq}{\posleq_{\funsp}} \newcommand{\fun}{\vmath{\colF{f}}} \newcommand{\res}{\vmath{\colR{r}}} \newcommand{\funtop}{\top_\funsp} \newcommand{\funbot}{\bot_\funsp} \newcommand{\imp}{\vmath{i}} \newcommand{\paramsp}{\mathscr{P}} \newcommand{\resleq}{\posleq_{\ressp}} \newcommand{\restop}{\top_\ressp} \newcommand{\resbot}{\bot_\ressp} \newcommand{\resspleq}{\resleq} \newcommand{\tressp}{\trof(\ressp)} \newcommand{\trof}{\mathscr{T}} \newcommand{\tres}{T} \newcommand{\tresleq}{\leq_{\trof}} \newcommand{\trleq}{\leq_{\trof}} \newcommand{\dpisp}{\ensuremath{\vmath{DPI}}} \newcommand{\cdpisp}{\ensuremath{\vmath{CDPI}}} \newcommand{\dprobsp}{\ensuremath{\vmath{DP}}} \newcommand{\dprob}{\vmath{dp}} \newcommand{\dpseries}{\vmath{series}} \newcommand{\dppar}{\vmath{par}} \newcommand{\dploop}{\vmath{loop}} \newcommand{\dploopb}{\vmath{loopb}} \newcommand{\cdprobsp}{\ensuremath{\vmath{CDP}}} \newcommand{\cdprob}{\vmath{cdp}} \newcommand{\dpatoms}{\vmath{atoms}} \newcommand{\resMin}{{\Min_{\resleq}}} \newcommand{\unconnectedfun}{\mathsf{UF}} \newcommand{\unconnectedres}{\mathsf{UR}} \newcommand{\Aressp}{{\mathsf{\colR A}\colR\ressp}} \newcommand{\Afunsp}{{\mathsf{\colF A}\colF\funsp}} \newcommand{\udpa}{\boldsymbol{u}_a} \newcommand{\udpb}{\boldsymbol{u}_b} \newcommand{\udpL}{\boldsymbol{\mathsf{L}}} \newcommand{\udpU}{\boldsymbol{\mathsf{U}}} \newcommand{\udpsp}{\vmath{UDP}} \newcommand{\udpleq}{\posleq_\udpsp} \newcommand{\dpsp}{\vmath{DP}} \newcommand{\dpleq}{\posleq_\dpsp} \newcommand{\terms}{\vmath{Terms}} \newcommand{\udpsem}{\Phi} \newcommand{\dpsem}{\varphi} \newcommand{\atoms}{\mathcal{A}} \newcommand{\atree}{\boldsymbol{\vmath{T}}} \newcommand{\val}{\boldsymbol{v}} \newcommand{\ops}{\vmath{ops}} \newcommand{\ftorL}{\ftor_L} \newcommand{\ftorU}{\ftor_U} \newcommand{\acprod}{\mathbin{\boldsymbol{\times}}} \newcommand{\oploop}{\dagger} \newcommand{\opseries}{\mathbin{\varocircle}} \newcommand{\oppar}{\mathbin{\varotimes}} \newcommand{\opcoprod}{\mathbin{\varovee}} \newcommand{\UId}{\vmath{UId}} \newcommand{\vdc}{\vmath{vdc}} \newcommand{\makedp}{\Gamma} \newcommand{\colU}{\color{purple}} \newcommand{\colL}{\color{orange}} \newcommand{\R}[1]{{\colR{#1}}} \newcommand{\F}[1]{{\colF{#1}}} \newcommand{\I}[1]{{\colI{#1}}} \newcommand{\cdpiN}{\mathcal{V}} \newcommand{\cdpin}{v} \newcommand{\cdpinA}{v_1} \newcommand{\cdpinB}{v_2} \newcommand{\cdpiresind}{i} \newcommand{\cdpifunind}{j} \newcommand{\cdpiresindA}{i_1} \newcommand{\cdpifunindB}{j_2} \newcommand{\dpinumf}{\vmath{nf}} \newcommand{\dpinumr}{\vmath{nr}} \newcommand{\cdpinnumf}{\dpinumf_\cdpin} \newcommand{\cdpinnumr}{\dpinumr_\cdpin} \newcommand{\cdpiE}{\mathcal{E}} \newcommand{\subto}{\text{s.t.}} \newcommand{\with}{\text{using}} \newcommand{\pset}{\mathscr{P}} \DeclareMathOperator*{\Min}{Min} \DeclareMathOperator*{\Inf}{Inf} \DeclareMathOperator*{\Sup}{Sup} \DeclareMathOperator*{\Max}{Max} \newcommand{\lowerbounds}{\vmath{lowerbounds}} \newcommand{\upperbounds}{\vmath{upperbounds}} \newcommand{\posMin}{\Min} \newcommand{\posleq}{\preceq} \newcommand{\poslt}{\prec} \newcommand{\posgeq}{\succeq} \newcommand{\posA}{\mathcal{P}} \newcommand{\posAleq}{\mathrel{{\posleq_\posA}}} \newcommand{\posAMin}{\mathop{{\posMin_{\posAleq}}}} \newcommand{\posAmin}{\mathop{{\min_{\posAleq}}}} \newcommand{\posAmax}{\mathop{{\max_{\posAleq}}}} \newcommand{\posB}{\mathcal{Q}} \newcommand{\posBleq}{\mathrel{{\posleq_\posB}}} \newcommand{\posC}{\mathcal{R}} \newcommand{\lfp}{\vmath{lfp}} \newcommand{\prefixed}{\vmath{prefixed}} \newcommand{\CPOs}{\textsc{CPO}s\xspace} \newcommand{\CPO}{\textsc{CPO}\xspace} \newcommand{\DCPOs}{\textsc{DCPO}s\xspace} \newcommand{\DCPO}{\textsc{DCPO}\xspace} \newcommand{\antichains}{\vmath{A}} \newcommand{\upsets}{\vmath{U}} \newcommand{\downsets}{\vmath{D}} \newcommand{\upresleq}{\posleq_{\upressp}} \newcommand{\upressp}{\upsets\ressp} \newcommand{\allupsets}{\vmath{Up}} \newcommand{\upit}{{\uparrow\,}} \newcommand{\stupit}{\dot{\upit}} \newcommand{\posetwidth}{\vmath{width}} \newcommand{\posetheight}{\vmath{height}} \newcommand{\posdef}[1]{\mathcal{P}_{#1}} \newcommand{\MR}{\M{R}} \newcommand{\myacronym}[1]{\textsc{#1}\xspace} \newcommand{\T}[1]{\boldsymbol{{\mathsf{#1}}}} \newcommand{\Tel}[1]{{\mathsf{#1}}} \newcommand{\Te}[1]{\Tel{#1}} \newcommand{\M}[1]{\mathbf{#1}} \newcommand{\Mel}[1]{\mathrm{#1}} \newcommand{\aset}[1]{\mathscr{#1}} \newcommand{\agroup}[1]{\mathrm{#1}} \newcommand{\aseq}[1]{\boldsymbol{#1}} \newcommand{\aseqe}[1]{#1} \newcommand{\dummyIndices}{} \newcommand{\aword}[1]{\mathsf{#1}} \newcommand{\vmath}[1]{\aword{#1}} \newcommand{\codefunc}[1]{\texttt{#1}\xspace} \newcommand{\swpackage}[1]{\textsc{#1}\xspace} \newcommand{\MA}{\M{A}} \newcommand{\MB}{\M{B}} \newcommand{\MC}{\M{C}} \newcommand{\MG}{\M{G}} \newcommand{\MH}{\M{H}} \newcommand{\ML}{\M{L}} \newcommand{\MQ}{\M{Q}} \newcommand{\MP}{\M{P}} \newcommand{\MS}{\M{S}} \newcommand{\MSigma}{\M{\Sigma}} \newcommand{\MV}{\M{V}} \newcommand{\MW}{\M{W}} \newcommand{\SP}{P_{\text{s}}} \newcommand{\AP}{P_{\text{a}}} \newcommand{\SE}{E} \newcommand{\ER}{r} \newcommand{\HP}{\Theta} \newcommand{\np}{n} \newcommand{\ones}{\boldsymbol{1}} \newcommand{\idMat}{\M{I}} \newcommand{\matTrace}{\vmath{Tr}} \newcommand{\angleFun}{\angle} \newcommand{\flatten}{\mathsf{vec}} \newcommand{\batterymass}{{\colR{m}}} \newcommand{\batterycapacity}{{\colF{C}}} \newcommand{\batterycost}{{\colR{c}}} \newcommand{\specificenergy}{{\colR{\rho}}} \newcommand{\specificcost}{{\colR{\alpha}}} \newcommand{\D}{\,\textrm{d}} \newcommand{\ex}{\mathbb{E}} \newcommand{\aset}[1]{\mathcal{#1}} \newcommand{\amat}[1]{\mathbf{#1}} \newcommand{\avec}[1]{\mathbf{#1}} \newcommand{\rv}[1]{\boldsymbol{#1}} \newcommand{\definedas}{\triangleq} \newcommand{\nats}{\mathbb{N}} \newcommand{\ints}{\mathbb{Z}} \newcommand{\rats}{\mathbb{Q}} \newcommand{\reals}{\mathbb{R}} \newcommand{\comp}{\mathbb{C}} \newcommand{\Time}{\mathbb{T}} \newcommand{\SEthree}{\text{SE}(3)} \newcommand{\SEtwo}{\text{SE}(2)} \newcommand{\sethree}{\text{se}(3)} \newcommand{\setwo}{\text{se}(2)} \newcommand{\SOthree}{\text{SO}(3)} \newcommand{\pose}{\boldsymbol{q}} \newcommand{\state}{\boldsymbol{x}} \newcommand{\statesp}{\mathcal{X}} \newcommand{\bmu}{\boldsymbol{\mu}} \newcommand{\bSigma}{\boldsymbol{\Sigma}} \newcommand{\tup}[1]{\langle#1\rangle}$$

General Overview

✎

Modified 2019-09-23 by AmaurX

In this part it is explained how the system for module 1 and 2 works.

Responsibilities of overall managing the chargers

✎

Modified 2020-07-18 by Andrea Censi

There are 3 main tasks that have to be accomplished :

TASK 1- Evaluating the April tag’s position in order to understand where the reference tags are located and in which direction the duckiebot went
TASK 2- Gathering information on charger sizes and finding the next free charger a duckiebot newly arriving the maintenance area should go
TASK 3- Selecting the frequency with which the LED should blink

In Modul 1, all the responsibilities are done by the charging manager. The output of tasks above feeds information to other tasks, for example, in task 1, it will be found out, which duckiebot entered or left the charger and this will be saved in a data structure that can be accessed by the charging manager. In task 2, charging manager will iterate over the data structure in order to discover how many duckiebots occupy the chargers. Through linear search, the most unoccupied charger will be determined and the frequency of LEDs corresponding charger id will be changed accordingly. In task 3 LEDs will blink with the mentioned frequency.

In Modul 2, the tasks are completed in separate devices. Task 1 is accomplished by doorkeepers and the rest will be done by the charging manager. The output of task 1 will be shared with the charging manager through a communication server. For this objective, charging manager serves a python-dictionary of duckiebot Apriltags as keys and charger indices as values (initially all charger indices are set to zero to indicate that the duckiebot does not occupy any charger.). The clients, in this case the doorkeepers, will update this dictionary according to their evaluation of April tag positions, for example, if duckiebot with April tag 400 enters charger 3 then the doorkeeper will change the value of 400 from 0 to 3.

Workflow in detail

✎

Modified 2019-07-18 by alifahriander

Here you can find step-by-step workflow of module 1 and module 2

Preliminary information:

✎

Modified 2020-07-18 by Andrea Censi

Before starting to read,note that every frequency with which the LED blinks, implies a charger the duckiebot should go. Also, CSLAM Acquisiton node provides many information. Only the data about the position of the center on the image will be used by charging manager(for module 1) or doorkeeper(for module 2).

Module 1:

✎

Modified 2019-07-18 by alifahriander

Charging Manager

✎

Charging Manager’s LED blinks with the initial frequency
(TASK 1) CSLAM Acquisition node provides information on the Apriltags in the scope of camera
In the scope of the camera, lies the reference tags and duckiebots’ Apriltags if there are any duckiebots arrived to the intersection.
The positions of duckiebots’ Apriltags and reference tags will be saved and updated, as the node publishes the topic.
Upon receiving these, the closest neighbor reference tag to duckiebot’s apriltag and the timestamp will be computed and saved (Here minimal distance is defined by the minimal euclidean distance of two points on the image.)
(The first computed closest neighbor reference tag will be saved under the variable “first_neighbor” and the other results will be updated and saved under “last_neighbor”.)
(TASK 1) Apriltag Evalutaion:
After not seeing an Apriltag of a duckiebot for more than threshold time, the “first_neighbor” and “last_neighbor” will be evaluated. That means, if the first_neighbor is the entrance tag and the last_neighbor is the direction2_tag, then Duckiebot went in direction2. Since direction2 corresponds to , for example, charger 2, the Duckiebot went in charger 2. According to this outcome the chargers dictionary will be updated and the tag of duckiebot which has entered/exited the charger i and the timestamp of entrance/exit will be saved/deleted under/from charger i’s list.
(TASK 2) Charger sizes will be updated according to step 2
If the most unoccupied charger is changed, it will be noticed and new frequency corresponding the new charger with the most free spaces will be determined.
(TASK 3) LED blinks with the determined frequency in step 3

These steps will be repeated upon receiving the messages from acquisition node’s topic

Module 2:

✎

Modified 2019-07-18 by alifahriander

Preliminary information:

✎

In this module a dictionary where keys are the duckiebots Apriltags and values are the charger indices or zero(indicating that the duckiebot is currently not occupying any charger) is served by the charging manager. The clients,namely the doorkeepers, will update this dictionary with charger indices if a Duckiebot enters a charger or with zero if a Duckiebot leaves a charger.

Doorkeeper

✎

(TASK 1) CSLAM Acquisition node provides information on the Apriltags in the scope of camera
In the scope of the camera, lies the reference tags and duckiebots’ Apriltags if there are any duckiebots arrived to the intersection.
The positions of duckiebots’ Apriltags and reference tags will be saved and updated, as the node publishes the topic.
Upon receiving these, the closest neighbor reference tag to duckiebot’s apriltag and the timestamp will be computed and saved (Here minimal distance is defined by the minimal euclidean distance of two points on the image.)
(The first computed closest neighbor reference tag will be saved under the variable “first_neighbor” and the other results will be updated and saved under “last_neighbor”.)
(TASK 1) Apriltag Evalutaion: After not seeing an Apriltag of a duckiebot for more than threshold time, the “first_neighbor” and “last_neighbor” will be evaluated. That means, if the first_neighbor is the entrance tag and the last_neighbor is the direction2_tag, then Duckiebot went in direction2. Since direction2 corresponds to , for example, charger 2, the Duckiebot went in charger 2. According to this outcome the chargers dictionary will be updated and the tag of duckiebot which has entered/exited the charger i and the timestamp of entrance/exit will be saved/deleted under/from charger i’s list.
(Communication Server) Sharing the information where duckiebot went After finding out where a duckiebot went, its value in the served dictionary will be either set to the charger index if it entered a charger or to zero, if it left a charger.

Charging Manager

✎

Charging Manager’s LED blinks with the initial frequency
(Communication Server) Charging Manager checks the dictionary updated by the doorkeepers periodically If the charger index of an Apriltag in the dictionary is changed, then the tag of duckiebot which has entered/exited the charger i and the timestamp of entrance will be saved/deleted under/from charger i’s list.
(TASK 2) Charger sizes will be updated according to step 2 If the most unoccupied charger is changed, it will be noticed and new frequency corresponding the new charger with the most free spaces will be determined
(TASK 3) LED blinks with the determined frequency in step 3

These steps will be repeated upon receiving the messages from acquisition node’s topic within doorkeepers