Automatic calibration controller (Der dessen name nicht genannt werden darf II)

[tuxbotix]

Introduced Changes

The current work in #695 mainly provides the calibration line detection and extracting measurements (although the PR originally meant to implement the integration too).

• This PR will provide the behaviour and coordination components
  • Extracting measurements or running actual calibration is out of the scope
• Automatic Callibration (Der dessen name nicht genannt werden darf) #695 can be cleaned for calibration line detection and measurement extraction. -> Replaced by Center circle detection for camera calibration #1057

Whichever PR gets merged last, will connect the final integration (writing calibration results, validation, etc) or as a 3rd PR.

Depends on #1076 (for datatypes & workflow testing) - It can be decoupled if nessesary.
Fixes #131

State changes:

• Controller's commands (blue thin stripe is "capture"
• other two are vision cyclers, Yellow is "captured value" -> fake measurement generator randomly deciding to send or not.
  

Workflow

The controller node tracks the state of the primary state and issues the calibration commands when the primary state is set to Calibrate.

Commands & State

• Inactive
  • Do nothing when the primary state is anything other than Calibrate
  • Transition to Initialize when the primary state goes to Calibrate and also set the cycle time.
• Initialize { started time }
  • Active during initialization, waits for initial_stabilization_delay until the robot finishes movement and stabilizes.
  • Once the timeout is reached, transition to LookAt with the stored LookAt command list. If nothing is found, jump to Processing phase.
• LookAt { target, camera, dispatch_time }
  • Move the head to the given target.
  • Waits look_at_dispatch_waiting from dispatch_time to ensure the head movement is complete.
  • Once the timeout is complete, switch to 'Capture'
• Capture { dispatch_time }
  • The vision node extracts measurements using the camera images. Shall do necessary filtering in this phase as well.
  • Waits until a measurement arrives with this information & stores it.
    • This may need improvements - See TODOs
  • Transition to the next LookAt once capture information arrives or jump to Processing if all look-at commands are executed.
• Processing
  • Runs the solver, gets the results and persists to storage if satisfactory.
  • Transition to Finish once above is complete.
• Finish
  • End state. Does nothing until exiting calibration primary phase.
  • Auto-change primary state??

ToDo / Known Issues

• Verify that the use of PerceptionInput is correct
• Handle when a measurement cannot be obtained from a given position (retry?, move to next look-at?
  • vision retries and says RetriesExceeded if it can't find any measurement.
  • If both cameras fail to find a measurement, it'll goto the next one.
• Consider where to run the actual calibration processing -> on the node, on a future or a separate cycler
  • Done on the controller as of now, seems to be fast enough*
• CalibrationCommand (also tracks state) could be improved.
  • Further actions added for Next Iterations
• LookAt commands use a timeout to decide the end of the movement. Consider alternatives
  • Won't consider anything else for this PR
• The initial phase (after activating calibration) uses a timeout (instead of checking if the pose is reached and stable). Consider alternatives
  • Won't consider anything else for this PR
• Remove logging and replace with AdditionalOutputs
• Nested match and if blocks could be made more elegant/ flatter
  • Done as much as I can. Reviewer could help!

Ideas for Next Iterations (Not This PR)

• Make look-at commands configurable (got a fixed list for now)
• Finalize the integration and include tests for the entire workflow.
• Should consider adding camera to the capture command. (already done)
• Decide on keeping the timeout for look-at or check for reaching the look-at status (joint value?)
• Decide on keeping the timeout for initial to calibration start - might want to look at IMU, etc values for stability?
• Should we automatically switch to Initial state after reaching Finish state in calibration?
  • How should the calibration phase end? #1092

How to Test

Webots or a NAO:

• On Twix, subscribe to:
  • Control.main_outputs.calibration_command to look at the calibration phase
  • Control.main_outputs.primary_state to verify the transition from initial to calibration (useful for Webots)
• Enter the calibration mode from Initial, (press ctrl + shift + x on webots for 1 second of "simulated time")
• If on webots, pepsi run will print the state changes and measurement collection on the terminal

[schmidma]

This looks extremely wip, but resides in "Request for Review"... Do you expect of getting a review on your code in the current state? If not, please move to "In Progress"

[tuxbotix]

Hi, Yes it is quite WIP and as written on Telegram when I opened it, this was 1.5 days work. What I wanted by putting for request for review is to get first remarks and feedback on the architecture and the implementation approach before investing any more time. If it is better, I can put this back to in development and discuss with you and/or whoever else interested to decide if this is a good approach to continue. I simply wanted to avoid a repeat of HULKs/nao#2951 where the devised approach wasn't all that great. At the same time, trying the implementation on this PR was much faster to answer the questions I had about this approach regarding the framework.

…

On Sun, 5 May 2024, 20:38 Maximilian Schmidt, ***@***.***> wrote: This looks extremely wip, but resides in "Request for Review"... Do you expect of getting a review on your code in the current state? If not, please move to "In Progress" — Reply to this email directly, view it on GitHub <#798 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABTOBX3WITSKJEM2I6KYDRDZAZ4EHAVCNFSM6AAAAABF3KK3OCVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDAOJUHEYDOMZTGU> . You are receiving this because you authored the thread.Message ID: ***@***.***>

[tuxbotix]

Fixed majority of the TODO/ WIP stuff. Works on Webots (Will test on a NAO today)

It would be great if a review can be done on this status to confirm the design choices and finalize this PR.

[knoellle]

Apart from walking to positions on the field, this PR also seems to largely fix #127.

[knoellle]

So, nu aber 😄
LGTM