+#include "QuinticWalk.h"++#include <fmt/format.h>++#include "extension/Configuration.h"++#include "message/behaviour/FixedWalkCommand.h"+#include "message/motion/GetupCommand.h"+#include "message/motion/KinematicsModel.h"+#include "message/motion/ServoTarget.h"+#include "message/motion/WalkCommand.h"+#include "message/support/SaveConfiguration.h"++#include "utility/math/comparison.h"+#include "utility/math/euler.h"+#include "utility/math/matrix/Transform3D.h"+#include "utility/motion/InverseKinematics.h"+#include "utility/support/eigen_armadillo.h"+#include "utility/support/yaml_expression.h"++namespace module {+namespace motion {++    using extension::Configuration;++    using message::behaviour::ServoCommand;+    using message::input::Sensors;+    using message::motion::DisableWalkEngineCommand;+    using message::motion::EnableWalkEngineCommand;+    using message::motion::ExecuteGetup;+    using message::motion::KillGetup;+    using message::motion::KinematicsModel;+    using message::motion::ServoTarget;+    using message::motion::StopCommand;+    using message::motion::WalkCommand;+    using utility::support::Expression;++    using utility::input::ServoID;+    using utility::math::matrix::Transform3D;+    using utility::motion::kinematics::calculateLegJoints;++    QuinticWalk::QuinticWalk(std::unique_ptr<NUClear::Environment> environment) : Reactor(std::move(environment)) {++        on<Configuration>("QuinticWalk.yaml").then([this](const Configuration& cfg) {+            // Use configuration here from file QuinticWalk.yaml+            params.freq                          = cfg["walk"]["freq"].as<float>();+            params.double_support_ratio          = cfg["walk"]["double_support_ratio"].as<float>();+            params.first_step_swing_factor       = cfg["walk"]["first_step_swing_factor"].as<float>();+            params.foot_distance                 = cfg["walk"]["foot"]["distance"].as<float>();+            params.foot_rise                     = cfg["walk"]["foot"]["rise"].as<float>();+            params.foot_z_pause                  = cfg["walk"]["foot"]["z_pause"].as<float>();+            params.foot_put_down_z_offset        = cfg["walk"]["foot"]["put_down"]["z_offset"].as<float>();+            params.foot_put_down_phase           = cfg["walk"]["foot"]["put_down"]["phase"].as<float>();+            params.foot_put_down_roll_offset     = cfg["walk"]["foot"]["put_down"]["roll_offset"].as<float>();+            params.foot_apex_phase               = cfg["walk"]["foot"]["apex_phase"].as<float>();+            params.foot_overshoot_ratio          = cfg["walk"]["foot"]["overshoot"]["ratio"].as<float>();+            params.foot_overshoot_phase          = cfg["walk"]["foot"]["overshoot"]["phase"].as<float>();+            params.trunk_height                  = cfg["walk"]["trunk"]["height"].as<float>();+            params.trunk_pitch                   = 1.0f + cfg["walk"]["trunk"]["pitch"].as<Expression>();+            params.trunk_phase                   = cfg["walk"]["trunk"]["phase"].as<float>();+            params.trunk_x_offset                = cfg["walk"]["trunk"]["x_offset"].as<float>();+            params.trunk_y_offset                = cfg["walk"]["trunk"]["y_offset"].as<float>();+            params.trunk_swing                   = cfg["walk"]["trunk"]["swing"].as<float>();+            params.trunk_pause                   = cfg["walk"]["trunk"]["pause"].as<float>();+            params.trunk_x_offset_p_coef_forward = cfg["walk"]["trunk"]["x_offset_p_coef"]["forward"].as<float>();+            params.trunk_x_offset_p_coef_turn    = cfg["walk"]["trunk"]["x_offset_p_coef"]["turn"].as<float>();+            params.trunk_pitch_p_coef_forward = 1.0f + cfg["walk"]["trunk"]["pitch_p_coef"]["forward"].as<Expression>();+            params.trunk_pitch_p_coef_turn    = 1.0f + cfg["walk"]["trunk"]["pitch_p_coef"]["turn"].as<Expression>();+            params.kick_length                = cfg["walk"]["kick"]["length"].as<float>();+            params.kick_phase                 = cfg["walk"]["kick"]["phase"].as<float>();+            params.kick_vel                   = cfg["walk"]["kick"]["vel"].as<float>();+            params.pause_duration             = cfg["walk"]["pause"]["duration"].as<float>();++            // Send these parameters to the walk engine+            walk_engine.setParameters(params);++            config.max_step[0] = cfg["max_step"]["x"].as<float>();+            config.max_step[1] = cfg["max_step"]["y"].as<float>();+            config.max_step[2] = cfg["max_step"]["z"].as<float>();+            config.max_step_xy = cfg["max_step"]["xy"].as<float>();++            config.imu_active          = cfg["imu"]["active"].as<bool>();+            config.imu_pitch_threshold = cfg["imu"]["pitch"]["threshold"].as<float>();+            config.imu_roll_threshold  = cfg["imu"]["roll"]["threshold"].as<float>();++            for (int i = 0; i < ServoID::NUMBER_OF_SERVOS; ++i) {+                if ((i >= 6) && (i < 18)) {+                    jointGains[i] = cfg["gains"]["legs"].as<float>();+                }+            }+        });++        on<Startup, Trigger<KinematicsModel>>().then("Update Kinematics Model", [this](const KinematicsModel& model) {+            kinematicsModel = model;+            first_run       = true;+            current_orders.setZero();+            is_left_support  = true;+            falling          = false;+            last_update_time = NUClear::clock::now();+            walk_engine.reset();+        });++        on<Trigger<ExecuteGetup>>().then([this]() { falling = true; });++        on<Trigger<KillGetup>>().then([this]() { falling = false; });++        on<Trigger<StopCommand>>().then([this] { current_orders.setZero(); });++        on<Trigger<WalkCommand>>().then([this](const WalkCommand& walkCommand) {+            // the engine expects orders in [m] not [m/s]. We have to compute by dividing by step frequency which is a+            // double step factor 2 since the order distance is only for a single step, not double step+            const float factor             = (1.0 / (params.freq)) / 2.0;+            const Eigen::Vector3f& command = walkCommand.command.cast<float>() * factor;++            // Clamp velocity command+            Eigen::Vector3f orders =+                command.array().max(-config.max_step.array()).min(config.max_step.array()).matrix();++            // translational orders (x+y) should not exceed combined limit. scale if necessary+            if (config.max_step_xy != 0) {+                float scaling_factor = 1.0f / std::max(1.0f, (orders.x() + orders.y()) / config.max_step_xy);+                orders.cwiseProduct(Eigen::Vector3f(scaling_factor, scaling_factor, 1.0f));+            }++            // warn user that speed was limited+            if (command.x() != orders.x() || command.y() != orders.y() || command.z() != orders.z()) {+                log<NUClear::WARN>(+                    fmt::format("Speed command was x: {} y: {} z: {} xy: {} but maximum is x: {} y: {} z: {} xy: {}",+                                command.x(),+                                command.y(),+                                command.z(),+                                command.x() + command.y(),+                                config.max_step[0] / factor,+                                config.max_step[1] / factor,+                                config.max_step[2] / factor,+                                config.max_step_xy / factor));+            }++            // Update orders+            current_orders = orders;+        });++        on<Trigger<Sensors>>().then([this](const Sensors& sensors) {+            if (config.imu_active) {+                Eigen::Vector3f RPY =+                    utility::math::euler::MatrixToEulerIntrinsic(sensors.Htw.topLeftCorner<3, 3>().cast<float>());++                // compute the pitch offset to the currently wanted pitch of the engine+                float wanted_pitch =+                    params.trunk_pitch + params.trunk_pitch_p_coef_forward * walk_engine.getFootstep().getNext().x()+                    + params.trunk_pitch_p_coef_turn * std::abs(walk_engine.getFootstep().getNext().z());+                RPY.y() += wanted_pitch;++                // threshold pitch and roll+                if (std::abs(RPY[0]) > config.imu_roll_threshold) {+                    log<NUClear::WARN>(fmt::format("Robot roll exceeds threshold - {} > {}",+                                                   std::abs(RPY.x()),+                                                   config.imu_roll_threshold));+                    walk_engine.requestPause();+                }+                else if (std::abs(RPY[1]) > config.imu_pitch_threshold) {+                    log<NUClear::WARN>(fmt::format("Robot pitch exceeds threshold - {} > {}",+                                                   std::abs(RPY.y()),+                                                   config.imu_pitch_threshold));+                    walk_engine.requestPause();+                }+            }+        });++        on<Trigger<EnableWalkEngineCommand>>().then([this](const EnableWalkEngineCommand& command) {+            subsumptionId = command.subsumptionId;+            walk_engine.reset();+            update_handle.enable();+        });++        on<Trigger<DisableWalkEngineCommand>>().then([this] { update_handle.disable(); });++        update_handle = on<Every<UPDATE_FREQUENCY, Per<std::chrono::seconds>>, Single>().then([this]() {+            const float dt = getTimeDelta();++            if (falling) {+                // We are falling, reset walk engine+                walk_engine.reset();+            }+            else {+                // we don't want to walk, even if we have orders, if we are not in the right state+                bool walkableState = true;  // TODO: what is our equivalent here?++                // see if the walk engine has new goals for us+                walkableState = walk_engine.updateState(dt, current_orders, walkableState);++                if ((walk_engine.getState() != engine::WalkEngineState::IDLE) && walkableState) {  // todo+                    calculateJointGoals();+                }+            }+        });+    }  // namespace motion++    float QuinticWalk::getTimeDelta() {+        // compute time delta depended if we are currently in simulation or reality+        float dt;+        auto current_time = NUClear::clock::now();+        // only take real time difference if walking was not stopped before+        // TODO Is this really necessary?+        auto time_diff_ms = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_update_time);+        dt                = time_diff_ms.count() / 1000.0f;+        if (dt == 0.0f) {+            // log<NUClear::WARN>(fmt::format("dt was 0 ({})", time_diff_ms.count()));+            dt = 0.001f;+        }+        last_update_time = current_time;+        // time is wrong when we run it for the first time+        if (first_run) {+            first_run = false;+            dt        = 0.0001f;+        }+        return dt;+    }++    void QuinticWalk::calculateJointGoals() {+        /*+        This method computes the next motor goals and publishes them.+        */+        auto setRPY = [&](const float& roll, const float& pitch, const float& yaw) {+            const float halfYaw   = yaw * 0.5f;+            const float halfPitch = pitch * 0.5f;+            const float halfRoll  = roll * 0.5f;+            const float cosYaw    = std::cos(halfYaw);+            const float sinYaw    = std::sin(halfYaw);+            const float cosPitch  = std::cos(halfPitch);+            const float sinPitch  = std::sin(halfPitch);+            const float cosRoll   = std::cos(halfRoll);+            const float sinRoll   = std::sin(halfRoll);+            return Eigen::Quaternionf(cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw,  // formerly yzx+                                      sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,  // x+                                      cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,  // y+                                      cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw   // z+                                      )+                .normalized()+                .toRotationMatrix();+        };++        // Read the cartesian positions and orientations for trunk and fly foot+        walk_engine.computeCartesianPosition(trunk_pos, trunk_axis, foot_pos, foot_axis, is_left_support);++        // Change goals from support foot based coordinate system to trunk based coordinate system+        Eigen::Affine3f Hst;  // trunk_to_support_foot_goal

+#include "QuinticWalk.h"++#include <fmt/format.h>++#include "extension/Configuration.h"++#include "message/behaviour/FixedWalkCommand.h"+#include "message/motion/GetupCommand.h"+#include "message/motion/KinematicsModel.h"+#include "message/motion/ServoTarget.h"+#include "message/motion/WalkCommand.h"+#include "message/support/SaveConfiguration.h"++#include "utility/math/comparison.h"+#include "utility/math/euler.h"+#include "utility/math/matrix/Transform3D.h"+#include "utility/motion/InverseKinematics.h"+#include "utility/support/eigen_armadillo.h"+#include "utility/support/yaml_expression.h"++namespace module {+namespace motion {++    using extension::Configuration;++    using message::behaviour::ServoCommand;+    using message::input::Sensors;+    using message::motion::DisableWalkEngineCommand;+    using message::motion::EnableWalkEngineCommand;+    using message::motion::ExecuteGetup;+    using message::motion::KillGetup;+    using message::motion::KinematicsModel;+    using message::motion::ServoTarget;+    using message::motion::StopCommand;+    using message::motion::WalkCommand;+    using utility::support::Expression;++    using utility::input::ServoID;+    using utility::math::matrix::Transform3D;+    using utility::motion::kinematics::calculateLegJoints;++    QuinticWalk::QuinticWalk(std::unique_ptr<NUClear::Environment> environment) : Reactor(std::move(environment)) {++        on<Configuration>("QuinticWalk.yaml").then([this](const Configuration& cfg) {+            // Use configuration here from file QuinticWalk.yaml+            params.freq                          = cfg["walk"]["freq"].as<float>();+            params.double_support_ratio          = cfg["walk"]["double_support_ratio"].as<float>();+            params.first_step_swing_factor       = cfg["walk"]["first_step_swing_factor"].as<float>();+            params.foot_distance                 = cfg["walk"]["foot"]["distance"].as<float>();+            params.foot_rise                     = cfg["walk"]["foot"]["rise"].as<float>();+            params.foot_z_pause                  = cfg["walk"]["foot"]["z_pause"].as<float>();+            params.foot_put_down_z_offset        = cfg["walk"]["foot"]["put_down"]["z_offset"].as<float>();+            params.foot_put_down_phase           = cfg["walk"]["foot"]["put_down"]["phase"].as<float>();+            params.foot_put_down_roll_offset     = cfg["walk"]["foot"]["put_down"]["roll_offset"].as<float>();+            params.foot_apex_phase               = cfg["walk"]["foot"]["apex_phase"].as<float>();+            params.foot_overshoot_ratio          = cfg["walk"]["foot"]["overshoot"]["ratio"].as<float>();+            params.foot_overshoot_phase          = cfg["walk"]["foot"]["overshoot"]["phase"].as<float>();+            params.trunk_height                  = cfg["walk"]["trunk"]["height"].as<float>();+            params.trunk_pitch                   = 1.0f + cfg["walk"]["trunk"]["pitch"].as<Expression>();+            params.trunk_phase                   = cfg["walk"]["trunk"]["phase"].as<float>();+            params.trunk_x_offset                = cfg["walk"]["trunk"]["x_offset"].as<float>();+            params.trunk_y_offset                = cfg["walk"]["trunk"]["y_offset"].as<float>();+            params.trunk_swing                   = cfg["walk"]["trunk"]["swing"].as<float>();+            params.trunk_pause                   = cfg["walk"]["trunk"]["pause"].as<float>();+            params.trunk_x_offset_p_coef_forward = cfg["walk"]["trunk"]["x_offset_p_coef"]["forward"].as<float>();+            params.trunk_x_offset_p_coef_turn    = cfg["walk"]["trunk"]["x_offset_p_coef"]["turn"].as<float>();+            params.trunk_pitch_p_coef_forward = 1.0f + cfg["walk"]["trunk"]["pitch_p_coef"]["forward"].as<Expression>();+            params.trunk_pitch_p_coef_turn    = 1.0f + cfg["walk"]["trunk"]["pitch_p_coef"]["turn"].as<Expression>();+            params.kick_length                = cfg["walk"]["kick"]["length"].as<float>();+            params.kick_phase                 = cfg["walk"]["kick"]["phase"].as<float>();+            params.kick_vel                   = cfg["walk"]["kick"]["vel"].as<float>();+            params.pause_duration             = cfg["walk"]["pause"]["duration"].as<float>();++            // Send these parameters to the walk engine+            walk_engine.setParameters(params);++            config.max_step[0] = cfg["max_step"]["x"].as<float>();+            config.max_step[1] = cfg["max_step"]["y"].as<float>();+            config.max_step[2] = cfg["max_step"]["z"].as<float>();+            config.max_step_xy = cfg["max_step"]["xy"].as<float>();++            config.imu_active          = cfg["imu"]["active"].as<bool>();+            config.imu_pitch_threshold = cfg["imu"]["pitch"]["threshold"].as<float>();+            config.imu_roll_threshold  = cfg["imu"]["roll"]["threshold"].as<float>();++            for (int i = 0; i < ServoID::NUMBER_OF_SERVOS; ++i) {+                if ((i >= 6) && (i < 18)) {+                    jointGains[i] = cfg["gains"]["legs"].as<float>();+                }+            }+        });++        on<Startup, Trigger<KinematicsModel>>().then("Update Kinematics Model", [this](const KinematicsModel& model) {+            kinematicsModel = model;+            first_run       = true;+            current_orders.setZero();+            is_left_support  = true;+            falling          = false;+            last_update_time = NUClear::clock::now();+            walk_engine.reset();+        });++        on<Trigger<ExecuteGetup>>().then([this]() { falling = true; });++        on<Trigger<KillGetup>>().then([this]() { falling = false; });++        on<Trigger<StopCommand>>().then([this] { current_orders.setZero(); });++        on<Trigger<WalkCommand>>().then([this](const WalkCommand& walkCommand) {+            // the engine expects orders in [m] not [m/s]. We have to compute by dividing by step frequency which is a+            // double step factor 2 since the order distance is only for a single step, not double step+            const float factor             = (1.0 / (params.freq)) / 2.0;+            const Eigen::Vector3f& command = walkCommand.command.cast<float>() * factor;++            // Clamp velocity command+            Eigen::Vector3f orders =+                command.array().max(-config.max_step.array()).min(config.max_step.array()).matrix();++            // translational orders (x+y) should not exceed combined limit. scale if necessary+            if (config.max_step_xy != 0) {+                float scaling_factor = 1.0f / std::max(1.0f, (orders.x() + orders.y()) / config.max_step_xy);+                orders.cwiseProduct(Eigen::Vector3f(scaling_factor, scaling_factor, 1.0f));+            }++            // warn user that speed was limited+            if (command.x() != orders.x() || command.y() != orders.y() || command.z() != orders.z()) {+                log<NUClear::WARN>(+                    fmt::format("Speed command was x: {} y: {} z: {} xy: {} but maximum is x: {} y: {} z: {} xy: {}",+                                command.x(),+                                command.y(),+                                command.z(),+                                command.x() + command.y(),+                                config.max_step[0] / factor,+                                config.max_step[1] / factor,+                                config.max_step[2] / factor,+                                config.max_step_xy / factor));+            }++            // Update orders+            current_orders = orders;+        });++        on<Trigger<Sensors>>().then([this](const Sensors& sensors) {+            if (config.imu_active) {+                Eigen::Vector3f RPY =+                    utility::math::euler::MatrixToEulerIntrinsic(sensors.Htw.topLeftCorner<3, 3>().cast<float>());++                // compute the pitch offset to the currently wanted pitch of the engine+                float wanted_pitch =+                    params.trunk_pitch + params.trunk_pitch_p_coef_forward * walk_engine.getFootstep().getNext().x()+                    + params.trunk_pitch_p_coef_turn * std::abs(walk_engine.getFootstep().getNext().z());+                RPY.y() += wanted_pitch;++                // threshold pitch and roll+                if (std::abs(RPY[0]) > config.imu_roll_threshold) {+                    log<NUClear::WARN>(fmt::format("Robot roll exceeds threshold - {} > {}",+                                                   std::abs(RPY.x()),+                                                   config.imu_roll_threshold));+                    walk_engine.requestPause();+                }+                else if (std::abs(RPY[1]) > config.imu_pitch_threshold) {+                    log<NUClear::WARN>(fmt::format("Robot pitch exceeds threshold - {} > {}",+                                                   std::abs(RPY.y()),+                                                   config.imu_pitch_threshold));+                    walk_engine.requestPause();+                }+            }+        });++        on<Trigger<EnableWalkEngineCommand>>().then([this](const EnableWalkEngineCommand& command) {+            subsumptionId = command.subsumptionId;+            walk_engine.reset();+            update_handle.enable();+        });++        on<Trigger<DisableWalkEngineCommand>>().then([this] { update_handle.disable(); });++        update_handle = on<Every<UPDATE_FREQUENCY, Per<std::chrono::seconds>>, Single>().then([this]() {+            const float dt = getTimeDelta();++            if (falling) {+                // We are falling, reset walk engine+                walk_engine.reset();+            }+            else {+                // we don't want to walk, even if we have orders, if we are not in the right state+                bool walkableState = true;  // TODO: what is our equivalent here?++                // see if the walk engine has new goals for us+                walkableState = walk_engine.updateState(dt, current_orders, walkableState);++                if ((walk_engine.getState() != engine::WalkEngineState::IDLE) && walkableState) {  // todo+                    calculateJointGoals();+                }+            }+        });+    }  // namespace motion++    float QuinticWalk::getTimeDelta() {+        // compute time delta depended if we are currently in simulation or reality+        float dt;+        auto current_time = NUClear::clock::now();+        // only take real time difference if walking was not stopped before+        // TODO Is this really necessary?+        auto time_diff_ms = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_update_time);+        dt                = time_diff_ms.count() / 1000.0f;+        if (dt == 0.0f) {+            // log<NUClear::WARN>(fmt::format("dt was 0 ({})", time_diff_ms.count()));+            dt = 0.001f;+        }+        last_update_time = current_time;+        // time is wrong when we run it for the first time+        if (first_run) {+            first_run = false;+            dt        = 0.0001f;+        }+        return dt;+    }++    void QuinticWalk::calculateJointGoals() {+        /*+        This method computes the next motor goals and publishes them.+        */+        auto setRPY = [&](const float& roll, const float& pitch, const float& yaw) {+            const float halfYaw   = yaw * 0.5f;+            const float halfPitch = pitch * 0.5f;+            const float halfRoll  = roll * 0.5f;+            const float cosYaw    = std::cos(halfYaw);+            const float sinYaw    = std::sin(halfYaw);+            const float cosPitch  = std::cos(halfPitch);+            const float sinPitch  = std::sin(halfPitch);+            const float cosRoll   = std::cos(halfRoll);+            const float sinRoll   = std::sin(halfRoll);+            return Eigen::Quaternionf(cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw,  // formerly yzx+                                      sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,  // x+                                      cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,  // y+                                      cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw   // z+                                      )+                .normalized()+                .toRotationMatrix();+        };++        // Read the cartesian positions and orientations for trunk and fly foot+        walk_engine.computeCartesianPosition(trunk_pos, trunk_axis, foot_pos, foot_axis, is_left_support);++        // Change goals from support foot based coordinate system to trunk based coordinate system+        Eigen::Affine3f Hst;  // trunk_to_support_foot_goal

+#ifndef MODULE_INPUT_IMAGEDECOMPRESSOR_H+#define MODULE_INPUT_IMAGEDECOMPRESSOR_H++#include <atomic>+#include <cstdint>+#include <memory>+#include <mutex>+#include <nuclear>++#include "decompressor/DecompressorFactory.h"++namespace module {+namespace input {++    class ImageDecompressor : public NUClear::Reactor {+    private:+        struct DecompressorContext {+            struct Decompressor {+                /// This atomic bool gets turned on/off depending on if it is in use+                std::unique_ptr<std::atomic<bool>> active;

 message KinematicsModel {         mat3 foot        = 9;     } -    Leg         leg                       = 1;-    Head        head                      = 2;-    Arm         arm                       = 3;-    MassModel   massModel                 = 4;-    TensorModel tensorModel               = 5;-    float       TEAMDARWINCHEST_TO_ORIGIN = 6;+    // Instantiations of the config containers.+    Leg         leg         = 1;+    Head        head        = 2;+    Arm         arm         = 3;+    MassModel   massModel   = 4;+    TensorModel tensorModel = 5;++    float TEAMDARWINCHEST_TO_ORIGIN = 6;

+---+section: System+chapter: Subsystems+title: Logging+description: Logging and how it works in the NUbots codebase.+slug: /system/subsystems/logging+---++# Data Playback++The DataPlayback module will allow you to play back `.nbs` files as if they were happening in realtime. This can be very useful when you are trying to diagnose a problem for which you have a recording since you can continuously play back the same data.++The module will buffer up packets from the future and emit them at the same rate that they were emitted originally.++## Usage++Add `support::logging::DataPlayback` to your role file and comment out the roles that you want the `nbs` files to replace. For example, if you are playing back `message.input.Sensors` you need to comment out `platform::darwin::SensorFilter` from your role otherwise you will have two different modules emitting conflicting `Sensors` messages. You must recompile your role for these changes to take effect.++In order to play back the messages you need to enable the message types that are the input to the modules that you wish to test by setting them to true in the configuration (e.g. `message.input.sensors: true`). Make sure the `.nbs` file contains the messages you are trying to emit (you can check with `./b nbs stats <nbs-file>`).

+---+section: System+chapter: Subsystems+title: Logging+description: Logging and how it works in the NUbots codebase.+slug: /system/subsystems/logging+---++# Data Playback++The DataPlayback module will allow you to play back `.nbs` files as if they were happening in realtime. This can be very useful when you are trying to diagnose a problem for which you have a recording since you can continuously play back the same data.++The module will buffer up packets from the future and emit them at the same rate that they were emitted originally.++## Usage++Add `support::logging::DataPlayback` to your role file and comment out the roles that you want the `nbs` files to replace. For example, if you are playing back `message.input.Sensors` you need to comment out `platform::darwin::SensorFilter` from your role otherwise you will have two different modules emitting conflicting `Sensors` messages. You must recompile your role for these changes to take effect.++In order to play back the messages you need to enable the message types that are the input to the modules that you wish to test by setting them to true in the configuration (e.g. `message.input.sensors: true`). Make sure the `.nbs` file contains the messages you are trying to emit (you can check with `./b nbs stats <nbs-file>`).++`nbs` files can be provided to the module via the configuration file or via the command line. If the configuration file has arguments those are used first. If there are no files provided in the configuration file, the arguments from the command line are used instead. This is so that if you have another module in your role that relies on command line arguments you have an alternate way to provide these files.

 function(ToolchainLibraryFinder)   include(FindPackageHandleStandardArgs)   find_package_handle_standard_args(     ${PACKAGE_NAME}-    FOUND_VAR-    ${PACKAGE_NAME}_FOUND-    REQUIRED_VARS-    ${required_vars}-    VERSION_VAR-    ${PACKAGE_NAME}_VERSION-    # VERSION_VAR "${MAJOR}.${MINOR}.${PATCH}")+    FOUND_VAR ${PACKAGE_NAME}_FOUND+    REQUIRED_VARS ${required_vars}+    VERSION_VAR ${PACKAGE_NAME}_VERSION # VERSION_VAR "${MAJOR}.${MINOR}.${PATCH}")

   "image": "nubots:generic",   "workspaceMount": "type=bind,consistency=delegated,src=${localWorkspaceFolder},dst=/home/nubots/NUbots",   "workspaceFolder": "/home/nubots/NUbots",-  "runArgs": [-    "--mount",-    "type=volume,src=nubots_generic_build,dst=/home/nubots/build,consistency=delegated"+  "mounts": [+    "type=volume,src=horus_generic_build,dst=/home/fourtel/build,consistency=delegated"

     "files.trimFinalNewlines": true,     "files.trimTrailingWhitespace": true,     "files.eol": "\n",+    "cSpell.language": "en-GB",     "editor.rulers": [         120     ],+    "protoc": {+        "options": [+            "-I${workspaceFolder}/nuclear/message/proto"+        ]+    },     "python.formatting.provider": "black",+    "yaml.format.printWidth": 120,

 namespace vision {                         }                          // Ball cam space info-                        b.cone.axis     = horizon.Hcw.topLeftCorner<3, 3>().cast<float>() * axis;-                        float proj      = 1.0f / radius;-                        b.cone.gradient = std::sqrt(proj * proj - 1.0f);-                        b.cone.radius   = radius;+                        b.cone.axis   = horizon.Hcw.topLeftCorner<3, 3>().cast<float>() * axis;+                        float proj    = 1.0f / radius;

 message Frustum { }  message Cone {-    fvec3 axis     = 1;-    float gradient = 2;-    float radius   = 3;+    fvec3 axis = 1;+    // Cos(theta) of the cone angle+    float radius = 3;

 title: Working with Robots description: How to safely handle and work with robots. slug: /guides/hardware/working-with-robots ---++## Aligning an Ankle++An alignment of a ankle may need to be done if the foot and the leg of the robot aren't perpendicular when standing. +An ankle alignment may also be necessary when the foot has come loose. ++To align an ankle correctly perform the following instructions:++1. On the inner part of the robots leg, unscrew the screws holding the plastic casing to the the ankle and knee servo. +Remove the plastic casing, as seen in the following image:+![leg cover](./images/ankle_leg/leg_cover.jpg 'leg cover')++2. There should be a silver disk, the following photo is a close up:+![servo close up](./images/ankle_leg/servo_close.jpg 'servo close up')+There are three small indents on the silver disk, two on one side and one on the other. The single dot should be pointing towards the ground when the robot is standing and the other two dots towards the head.+These three dots form an arrow, this arrow should be pointing downwards when the robot is standing.++3. Manually move the disk on the servo so it is in the correct orientation ++4. Place the plastic cover back on and replace all the screws. When replacing the screws, one hole must be left without a screw to allow for the servo to move. The hole left without a screw is the largest hole, which can be seen on the right of the following image:

 import minimist from 'minimist'; import favicon from 'serve-favicon'; import sio from 'socket.io'; -import { WebSocketProxyUDPServer } from './network/network';-import { WebSocketServer } from './network/web_socket_server';+import { UDPServers } from './network/network'; +// Process command line arguments const args = minimist(process.argv.slice(2));-const teamAAddress = args.team_a || '239.226.152.162';-const teamBAddress = args.team_b || '239.226.152.163';++// Get all of the teams from the command line+// Fall back to our defaults if none were specified+let teams = args._;+if (teams.length === 0) {+  teams = ['Red Team:239.226.152.162:9917', 'Blue Team:239.226.152.163:9917'];+}++// Convert command line data into a more usable format+const teamData = teams.map((team: string) => {+  const nameSplit = team.indexOf(':');

 import minimist from 'minimist'; import favicon from 'serve-favicon'; import sio from 'socket.io'; -import { WebSocketProxyUDPServer } from './network/network';-import { WebSocketServer } from './network/web_socket_server';+import { UDPServers } from './network/network'; +// Process command line arguments const args = minimist(process.argv.slice(2));-const teamAAddress = args.team_a || '239.226.152.162';-const teamBAddress = args.team_b || '239.226.152.163';++// Get all of the teams from the command line+// Fall back to our defaults if none were specified+let teams = args._;+if (teams.length === 0) {+  teams = ['Red Team:239.226.152.162:9917', 'Blue Team:239.226.152.163:9917'];+}++// Convert command line data into a more usable format+const teamData = teams.map((team: string) => {+  const nameSplit = team.indexOf(':');

 import minimist from 'minimist'; import favicon from 'serve-favicon'; import sio from 'socket.io'; -import { WebSocketProxyUDPServer } from './network/network';-import { WebSocketServer } from './network/web_socket_server';+import { UDPServers } from './network/network'; +// Process command line arguments const args = minimist(process.argv.slice(2));-const teamAAddress = args.team_a || '239.226.152.162';-const teamBAddress = args.team_b || '239.226.152.163';++// Get all of the teams from the command line+// Fall back to our defaults if none were specified+let teams = args._;+if (teams.length === 0) {+  teams = ['Red Team:239.226.152.162:9917', 'Blue Team:239.226.152.163:9917'];+}++// Convert command line data into a more usable format+const teamData = teams.map((team: string) => {+  const nameSplit = team.indexOf(':');

 import compression from 'compression'; import history from 'connect-history-api-fallback'; import express from 'express'; import http from 'http';+import minimist from 'minimist'; import favicon from 'serve-favicon';+import sio from 'socket.io';++import { WebSocketProxyUDPServer } from './network/network';+import { WebSocketServer } from './network/web_socket_server';++const args = minimist(process.argv.slice(2));+const teamAAddress = args.team_a || '239.226.152.162';+const teamBAddress = args.team_b || '239.226.152.163';  const app = express(); const server = http.createServer(app);+const sioNetwork = sio(server);++// Initialize socket.io namespace immediately to catch reconnections.+WebSocketProxyUDPServer.of(WebSocketServer.of(sioNetwork.of('/dashboard')), {

 export class UDPServer {     return () => this.clients.delete(client);   } -  // Set up callbacks for a given team and then bind the team to its multicast address and port-  private initialiseListener = (listener: Listener) => {-    listener.server.on('listening', () => {+  // One of the UDP servers received a new message, send it on to all clients+  private onMessage = (+    team: TeamData,+    packet: Buffer,+    rinfo: dgram.RemoteInfo,+  ) => {+    for (let emit_cb of this.clients.values()) {+      emit_cb('udp_packet', {+        payload: packet,+        team: {+          name: team.name,+          address: team.address,+          port: team.port,+        },+        rinfo: {+          family: rinfo.family,+          address: rinfo.address,+          port: rinfo.port,+        },+      });+    }+  };+}++class UDPServer {+  private socket: dgram.Socket;++  constructor(+    private team: TeamData,+    private msg_cb: MessageCallback,+    ipv6: boolean,+  ) {+    if (ipv6) {+      this.socket = dgram.createSocket({ type: 'udp6', reuseAddr: true });+    } else {+      this.socket = dgram.createSocket({ type: 'udp4', reuseAddr: true });+    }++    this.socket.on('listening', () => {       // Add multicast membership if we are using a multicast address-      if (listener.ipv6) {+      if (ipv6) {

 export class UDPServer {     return () => this.clients.delete(client);   } -  // Set up callbacks for a given team and then bind the team to its multicast address and port-  private initialiseListener = (listener: Listener) => {-    listener.server.on('listening', () => {+  // One of the UDP servers received a new message, send it on to all clients+  private onMessage = (+    team: TeamData,+    packet: Buffer,+    rinfo: dgram.RemoteInfo,+  ) => {+    for (let emit_cb of this.clients.values()) {+      emit_cb('udp_packet', {+        payload: packet,+        team: {+          name: team.name,+          address: team.address,+          port: team.port,+        },+        rinfo: {+          family: rinfo.family,+          address: rinfo.address,+          port: rinfo.port,+        },+      });+    }+  };+}++class UDPServer {+  private socket: dgram.Socket;++  constructor(+    private team: TeamData,+    private msg_cb: MessageCallback,+    ipv6: boolean,+  ) {+    if (ipv6) {+      this.socket = dgram.createSocket({ type: 'udp6', reuseAddr: true });+    } else {+      this.socket = dgram.createSocket({ type: 'udp4', reuseAddr: true });+    }++    this.socket.on('listening', () => {       // Add multicast membership if we are using a multicast address-      if (listener.ipv6) {+      if (ipv6) {         // IPv6 multicast addresses start with 0xFF00-        if (listener.team.address.split(':')[0].toUpperCase() === 'FF00') {-          listener.server.addMembership(listener.team.address);+        if (team.address.split(':')[0].toUpperCase() === 'FF00') {+          this.socket.addMembership(team.address);         }       } else {         // IPv4 multicast addresses start with 0xE-        const octet = Number(listener.team.address.split('.')[0])+        const octet = Number(team.address.split('.')[0])           .toString(16)           .toUpperCase();         if (octet.startsWith('E')) {

 export class UDPServer {     return () => this.clients.delete(client);   } -  // Set up callbacks for a given team and then bind the team to its multicast address and port-  private initialiseListener = (listener: Listener) => {-    listener.server.on('listening', () => {+  // One of the UDP servers received a new message, send it on to all clients+  private onMessage = (+    team: TeamData,+    packet: Buffer,+    rinfo: dgram.RemoteInfo,+  ) => {+    for (let emit_cb of this.clients.values()) {+      emit_cb('udp_packet', {+        payload: packet,+        team: {+          name: team.name,+          address: team.address,+          port: team.port,+        },+        rinfo: {+          family: rinfo.family,+          address: rinfo.address,+          port: rinfo.port,+        },+      });+    }+  };+}++class UDPServer {+  private socket: dgram.Socket;++  constructor(+    private team: TeamData,+    private msg_cb: MessageCallback,+    ipv6: boolean,+  ) {+    if (ipv6) {+      this.socket = dgram.createSocket({ type: 'udp6', reuseAddr: true });+    } else {+      this.socket = dgram.createSocket({ type: 'udp4', reuseAddr: true });+    }++    this.socket.on('listening', () => {       // Add multicast membership if we are using a multicast address-      if (listener.ipv6) {+      if (ipv6) {         // IPv6 multicast addresses start with 0xFF00-        if (listener.team.address.split(':')[0].toUpperCase() === 'FF00') {-          listener.server.addMembership(listener.team.address);+        if (team.address.split(':')[0].toUpperCase() === 'FF00') {

 import dgram = require('dgram'); import dns = require('dns');  export type ClientListener = (event: string, ...args: any[]) => void;+export type MessageCallback = (+  team: TeamData,+  payload: Buffer,+  rinfo: dgram.RemoteInfo,+) => void;  export type TeamData = {   name: string;   address: string;   port: number; }; -type Listener = {-  server: dgram.Socket;-  ipv6: boolean;-  team: TeamData;-};--export class UDPServer {-  private listeners: Array<Listener>;+export class UDPServers {+  private servers: Array<UDPServer>;   private clients: Map<string, ClientListener>;    constructor(teams: Array<TeamData>) {     this.clients = new Map<string, ClientListener>();-    this.listeners = new Array<Listener>();+    this.servers = new Array<UDPServer>();      teams.forEach((team: TeamData) => {-      // Determine if we are listening on a IPv4 or IPv6 address and resolve any DNS names+      // Determine if we are listening on a IPv4 or IPv6 address and resolve any DNS address+      // If the specified address can be resolved to an IPv4 address prefer that over an IPv6 address       dns.lookup(         team.address,-        { family: 0, all: false },+        { family: 0, all: true },         (           error: NodeJS.ErrnoException | null,-          address: string,-          family: number,+          addresses: dns.LookupAddress[],         ) => {-          if (family === 6) {-            const listener = {-              server: dgram.createSocket({ type: 'udp6', reuseAddr: true }),-              ipv6: true,-              team: {-                name: team.name,-                address: address,-                port: team.port,-              },-            };-            this.initialiseListener(listener);-            this.listeners.push(listener);-          } else if (family === 4) {-            const listener = {-              server: dgram.createSocket({ type: 'udp4', reuseAddr: true }),-              ipv6: false,-              team: {-                name: team.name,-                address: address,-                port: team.port,-              },-            };-            this.initialiseListener(listener);-            this.listeners.push(listener);+          // Find the first IPv4 and IPv6 addresses that were returned+          let ipv4 = '';+          let ipv6 = '';+          addresses.forEach((address: dns.LookupAddress) => {+            if (address.family === 6 && ipv6 === '') {+              ipv6 = address.address;+            }+            if (address.family === 4 && ipv4 === '') {+              ipv4 = address.address;+            }+          });

 import dgram = require('dgram'); import dns = require('dns');  export type ClientListener = (event: string, ...args: any[]) => void;+export type MessageCallback = (+  team: TeamData,+  payload: Buffer,+  rinfo: dgram.RemoteInfo,+) => void;  export type TeamData = {   name: string;   address: string;   port: number; }; -type Listener = {-  server: dgram.Socket;-  ipv6: boolean;-  team: TeamData;-};--export class UDPServer {-  private listeners: Array<Listener>;+export class UDPServers {+  private servers: Array<UDPServer>;   private clients: Map<string, ClientListener>;    constructor(teams: Array<TeamData>) {     this.clients = new Map<string, ClientListener>();-    this.listeners = new Array<Listener>();+    this.servers = new Array<UDPServer>();      teams.forEach((team: TeamData) => {-      // Determine if we are listening on a IPv4 or IPv6 address and resolve any DNS names+      // Determine if we are listening on a IPv4 or IPv6 address and resolve any DNS address+      // If the specified address can be resolved to an IPv4 address prefer that over an IPv6 address       dns.lookup(         team.address,-        { family: 0, all: false },+        { family: 0, all: true },         (           error: NodeJS.ErrnoException | null,-          address: string,-          family: number,+          addresses: dns.LookupAddress[],         ) => {-          if (family === 6) {-            const listener = {-              server: dgram.createSocket({ type: 'udp6', reuseAddr: true }),-              ipv6: true,-              team: {-                name: team.name,-                address: address,-                port: team.port,-              },-            };-            this.initialiseListener(listener);-            this.listeners.push(listener);-          } else if (family === 4) {-            const listener = {-              server: dgram.createSocket({ type: 'udp4', reuseAddr: true }),-              ipv6: false,-              team: {-                name: team.name,-                address: address,-                port: team.port,-              },-            };-            this.initialiseListener(listener);-            this.listeners.push(listener);+          // Find the first IPv4 and IPv6 addresses that were returned+          let ipv4 = '';+          let ipv6 = '';+          addresses.forEach((address: dns.LookupAddress) => {+            if (address.family === 6 && ipv6 === '') {+              ipv6 = address.address;+            }+            if (address.family === 4 && ipv4 === '') {+              ipv4 = address.address;+            }+          });++          // Prefer IPv4 addresses+          if (ipv4 !== '') {+            this.servers.push(+              UDPServer.of(+                { name: team.name, address: ipv4, port: team.port },+                (team: TeamData, payload: Buffer, rinfo: dgram.RemoteInfo) => {+                  this.onMessage(team, payload, rinfo);+                },+                false,+              ),+            );+          } else if (ipv6 !== '') {+            this.servers.push(+              UDPServer.of(+                { name: team.name, address: ipv6, port: team.port },+                (team: TeamData, payload: Buffer, rinfo: dgram.RemoteInfo) => {+                  this.onMessage(team, payload, rinfo);+                },+                true,+              ),+            );

 export class UDPServer {     return () => this.clients.delete(client);   } -  // Set up callbacks for a given team and then bind the team to its multicast address and port-  private initialiseListener = (listener: Listener) => {-    listener.server.on('listening', () => {+  // One of the UDP servers received a new message, send it on to all clients+  private onMessage = (+    team: TeamData,+    packet: Buffer,+    rinfo: dgram.RemoteInfo,+  ) => {

 if (teams.length === 0) { // Convert command line data into a more usable format var teamData = new Array<TeamData>(); teams.forEach((team: string) => {-  const data = team.split(':');-  teamData.push({ name: data[0], address: data[1], port: Number(data[2]) });+  const nameSplit = team.indexOf(':');+  const portSplit = team.lastIndexOf(':');+  const teamName = team.substring(0, nameSplit);+  const teamAddress = team.substring(nameSplit + 1, portSplit);+  const teamPort = Number(team.substring(portSplit + 1));+  teamData.push({ name: teamName, address: teamAddress, port: teamPort }); });

 export class UDPServer {     this.listeners = new Array<Listener>();      teams.forEach((team: TeamData) => {-      this.listeners.push({-        server: dgram.createSocket({ type: 'udp4', reuseAddr: true }),-        team: team,-      });-    });--    this.listeners.forEach((listener: Listener) => {-      this.initialiseListener(listener);+      // Determine if we are listening on a IPv4 or IPv6 address and resolve any DNS names+      dns.lookup(+        team.address,+        { family: 0, all: false },+        (+          error: NodeJS.ErrnoException | null,+          address: string,+          family: number,+        ) => {+          if (family === 6) {+            const listener = {+              server: dgram.createSocket({ type: 'udp6', reuseAddr: true }),+              ipv6: true,+              team: {+                name: team.name,+                address: address,+                port: team.port,+              },+            };+            this.initialiseListener(listener);

 export type TeamData = {  type Listener = {   server: dgram.Socket;+  ipv6: boolean;

 import compression from 'compression'; import history from 'connect-history-api-fallback'; import express from 'express'; import http from 'http';+import minimist from 'minimist'; import favicon from 'serve-favicon';+import sio from 'socket.io'; import webpack from 'webpack'; import webpackDevMiddleware from 'webpack-dev-middleware';  import webpackConfig from '../../webpack.config'; +import { UDPServer } from './network/network';++const args = minimist(process.argv.slice(2));+const redTeamAddress = args.team_a || '239.226.152.162';+const blueTeamAddress = args.team_b || '239.226.152.163';+ const compiler = webpack(webpackConfig); const app = express(); const server = http.createServer(app);+const sioNetwork = sio(server);++// Start listening to the multicast groups+const udpServer = UDPServer.of(redTeamAddress, blueTeamAddress);++// Whenever we get a new client connection let the UDP server know about it+sioNetwork.on('connection', (socket: sio.Socket) => {+  const off_cb = udpServer.on(+    socket.client.id,+    (event: string, ...args: any[]) => {

 import compression from 'compression'; import history from 'connect-history-api-fallback'; import express from 'express'; import http from 'http';+import minimist from 'minimist'; import favicon from 'serve-favicon';+import sio from 'socket.io';++import { WebSocketProxyUDPServer } from './network/network';+import { WebSocketServer } from './network/web_socket_server';++const args = minimist(process.argv.slice(2));