diff --git a/.github/workflows/pull-request.yml b/.github/workflows/pull-request.yml index aafd4094d8..8624b8deaf 100644 --- a/.github/workflows/pull-request.yml +++ b/.github/workflows/pull-request.yml @@ -107,7 +107,7 @@ jobs: with: lfs: true - name: Test - run: ./pepsi test --locked --all-features + run: ./pepsi test --locked --all-features --workspace --exclude bevyhavior_simulator build: name: Build @@ -116,9 +116,8 @@ jobs: matrix: target: - imagine - - nao - replayer - - webots + - mujoco profile: - dev - release diff --git a/Cargo.lock b/Cargo.lock index c7fe17441f..d6edd7cd69 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -188,28 +188,6 @@ version = "0.2.21" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" -[[package]] -name = "alsa" -version = "0.9.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed7572b7ba83a31e20d1b48970ee402d2e3e0537dcfe0a3ff4d6eb7508617d43" -dependencies = [ - "alsa-sys", - "bitflags 2.9.1", - "cfg-if", - "libc", -] - -[[package]] -name = "alsa-sys" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db8fee663d06c4e303404ef5f40488a53e062f89ba8bfed81f42325aafad1527" -dependencies = [ - "libc", - "pkg-config", -] - [[package]] name = "android-activity" version = "0.6.0" @@ -1835,6 +1813,7 @@ dependencies = [ "ball_filter", "bevy", "bincode", + "booster", "buffered_watch", "chrono", "clap 4.5.40", @@ -1877,6 +1856,7 @@ dependencies = [ "types", "vision", "walking_engine", + "zed", ] [[package]] @@ -1916,29 +1896,6 @@ dependencies = [ "which 4.4.2", ] -[[package]] -name = "bindgen" -version = "0.66.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f2b84e06fc203107bfbad243f4aba2af864eb7db3b1cf46ea0a023b0b433d2a7" -dependencies = [ - "bitflags 2.9.1", - "cexpr", - "clang-sys", - "lazy_static", - "lazycell", - "log", - "peeking_take_while", - "prettyplease", - "proc-macro2", - "quote", - "regex", - "rustc-hash 1.1.0", - "shlex", - "syn 2.0.103", - "which 4.4.2", -] - [[package]] name = "bindgen" version = "0.70.1" @@ -2112,6 +2069,20 @@ dependencies = [ "piper", ] +[[package]] +name = "booster" +version = "0.1.0" +dependencies = [ + "coordinate_systems", + "linear_algebra", + "nalgebra", + "path_serde", + "ros2", + "serde", + "serde_json", + "tokio", +] + [[package]] name = "bstr" version = "1.12.0" @@ -2697,6 +2668,7 @@ dependencies = [ "approx 0.5.1", "ball_filter", "bincode", + "booster", "calibration", "color-eyre", "context_attribute", @@ -4392,9 +4364,12 @@ dependencies = [ name = "hardware" version = "0.1.0" dependencies = [ + "booster", "color-eyre", "hula_types", + "ros2", "types", + "zed", ] [[package]] @@ -4529,17 +4504,18 @@ dependencies = [ ] [[package]] -name = "hulk" +name = "hulk_imagine" version = "0.1.0" dependencies = [ "audio", "ball_filter", "bincode", + "booster", "buffered_watch", "calibration", + "clap 4.5.40", "code_generation", "color-eyre", - "communication", "control", "coordinate_systems", "energy_optimization", @@ -4548,112 +4524,95 @@ dependencies = [ "hardware", "hula_types", "hulk_manifest", + "indicatif", "ittapi", - "libc", "linear_algebra", - "log", + "mcap", "motionfile", "nalgebra", "ndarray 0.16.1", - "object_detection", "parameters", "path_serde", "projection", + "rmp-serde", "serde", "serde_json", "source_analyzer", "spl_network", "spl_network_messages", "step_planning", - "systemd", "tokio", - "tokio-util", "types", "vision", "walking_engine", + "zed", ] [[package]] -name = "hulk_imagine" +name = "hulk_manifest" +version = "0.1.0" +dependencies = [ + "source_analyzer", +] + +[[package]] +name = "hulk_mujoco" version = "0.1.0" dependencies = [ + "approx 0.5.1", "audio", "ball_filter", "bincode", + "blake3", + "booster", "buffered_watch", "calibration", - "clap 4.5.40", + "chrono", "code_generation", "color-eyre", + "communication", "control", "coordinate_systems", + "ctrlc", + "derive_more 2.0.1", + "eframe", "energy_optimization", + "env_logger", + "fern", "framework", + "futures-util", "geometry", "hardware", "hula_types", "hulk_manifest", - "indicatif", + "hulk_widgets", "ittapi", + "libc", "linear_algebra", - "mcap", + "log", "motionfile", "nalgebra", "ndarray 0.16.1", + "object_detection", "parameters", + "parking_lot", "path_serde", "projection", - "rmp-serde", + "ros2", "serde", "serde_json", + "simulation_message", "source_analyzer", "spl_network", "spl_network_messages", - "step_planning", + "systemd", "tokio", + "tokio-tungstenite", + "tokio-util", "types", "vision", "walking_engine", -] - -[[package]] -name = "hulk_manifest" -version = "0.1.0" -dependencies = [ - "source_analyzer", -] - -[[package]] -name = "hulk_nao" -version = "0.1.0" -dependencies = [ - "alsa", - "ball_filter", - "chrono", - "clap 4.5.40", - "color-eyre", - "ctrlc", - "enum-iterator", - "fern", - "framework", - "hardware", - "hula_types", - "hulk", - "libc", - "linear_algebra", - "log", - "nalgebra", - "nao_camera", - "opusfile-ng", - "parking_lot", - "serde", - "serde_json", - "spl_network", - "thiserror 2.0.12", - "tokio", - "tokio-util", - "types", - "watch", + "zed", ] [[package]] @@ -4664,6 +4623,7 @@ dependencies = [ "ball_filter", "bincode", "blake3", + "booster", "buffered_watch", "calibration", "chrono", @@ -4704,34 +4664,7 @@ dependencies = [ "types", "vision", "walking_engine", -] - -[[package]] -name = "hulk_webots" -version = "0.1.0" -dependencies = [ - "ball_filter", - "chrono", - "color-eyre", - "ctrlc", - "fern", - "framework", - "hardware", - "hula_types", - "hulk", - "linear_algebra", - "log", - "nalgebra", - "ndarray 0.16.1", - "parking_lot", - "rand 0.9.1", - "serde", - "serde_json", - "spl_network", - "tokio", - "tokio-util", - "types", - "webots", + "zed", ] [[package]] @@ -5404,11 +5337,12 @@ dependencies = [ [[package]] name = "libsystemd-sys" -version = "0.9.3" +version = "0.9.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed080163caa59cc29b34bce2209b737149a4bac148cd9a8b04e4c12822798119" +checksum = "976306de183e6046819ef6505888d00996214766a3f4660a2ed5761c84a20aed" dependencies = [ "build-env", + "cfg-if", "libc", "pkg-config", ] @@ -6163,9 +6097,9 @@ dependencies = [ [[package]] name = "num-dual" -version = "0.11.1" +version = "0.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c8af2c4f45e74b48064e1023c1dabe753b8ca1d398b1bd80956bc36fb79442d0" +checksum = "ff71f0acbaa027dd8481a79484243ec050255d9174dd955cdd9475a321b48d2d" dependencies = [ "approx 0.5.1", "nalgebra", @@ -6696,19 +6630,6 @@ version = "0.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "04744f49eae99ab78e0d5c0b603ab218f515ea8cfe5a456d7629ad883a3b6e7d" -[[package]] -name = "opusfile-ng" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7f40c412371d87b6055dc8015d3054efe957fd375321bd5b5a045475f6bbc0c5" -dependencies = [ - "bindgen 0.66.1", - "num-derive", - "num-traits", - "pkg-config", - "thiserror 1.0.69", -] - [[package]] name = "orbclient" version = "0.3.48" @@ -6787,6 +6708,7 @@ name = "parameter_tester" version = "0.1.0" dependencies = [ "ball_filter", + "booster", "calibration", "code_generation", "color-eyre", @@ -7843,6 +7765,14 @@ version = "0.0.8" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "082f11ffa03bbef6c2c6ea6bea1acafaade2fd9050ae0234ab44a2153742b058" +[[package]] +name = "ros2" +version = "0.1.0" +dependencies = [ + "nalgebra", + "serde", +] + [[package]] name = "roxmltree" version = "0.20.0" @@ -8311,6 +8241,15 @@ dependencies = [ "quote", ] +[[package]] +name = "simulation_message" +version = "0.1.0" +dependencies = [ + "booster", + "serde", + "zed", +] + [[package]] name = "siphasher" version = "1.0.1" @@ -8726,9 +8665,9 @@ dependencies = [ [[package]] name = "systemd" -version = "0.10.0" +version = "0.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "afec0101d9ae8ab26aedf0840109df689938ea7e538aa03df4369f1854f11562" +checksum = "01e9d1976a15b86245def55d20d52b5818e1a1e81aa030b6a608d3ce57709423" dependencies = [ "cstr-argument", "foreign-types 0.5.0", @@ -9402,6 +9341,7 @@ dependencies = [ "num-traits", "ordered-float 4.6.0", "path_serde", + "ros2", "serde", "spl_network_messages", "splines", @@ -9779,12 +9719,6 @@ dependencies = [ "unicode-ident", ] -[[package]] -name = "watch" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5ed390fa2f6c6036b4dad0a2695711971998ac185f629e4b5d3d5cb3e4ff5c65" - [[package]] name = "wayland-backend" version = "0.3.10" @@ -9930,26 +9864,6 @@ dependencies = [ "web-sys", ] -[[package]] -name = "webots" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f67ba38a7b03bdc1bb8ed6354e8ba5faafaea2135b9ce65b1b2c6d51e7b9a1c7" -dependencies = [ - "thiserror 1.0.69", - "webots-bindings", -] - -[[package]] -name = "webots-bindings" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8da14b8ba2e18594584870331d272b733414674c333265ddcc4ce476597d5c8b" -dependencies = [ - "bindgen 0.63.0", - "walkdir", -] - [[package]] name = "weezl" version = "0.1.10" @@ -11117,6 +11031,14 @@ dependencies = [ "zvariant 4.2.0", ] +[[package]] +name = "zed" +version = "0.1.0" +dependencies = [ + "ros2", + "serde", +] + [[package]] name = "zeno" version = "0.2.3" diff --git a/Cargo.toml b/Cargo.toml index abcafce72f..cbb57740cd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,6 +6,7 @@ members = [ "crates/audio", "crates/ball_filter", "crates/bevyhavior_simulator", + "crates/booster", "crates/buffered_watch", "crates/calibration", "crates/code_generation", @@ -19,12 +20,10 @@ members = [ "crates/geometry", "crates/hardware", "crates/hula_types", - "crates/hulk", "crates/hulk_imagine", "crates/hulk_manifest", - "crates/hulk_nao", + "crates/hulk_mujoco", "crates/hulk_replayer", - "crates/hulk_webots", "crates/hulk_widgets", "crates/hungarian_algorithm", "crates/kinematics", @@ -40,6 +39,8 @@ members = [ "crates/projection", "crates/ransac", "crates/repository", + "crates/ros2", + "crates/simulation_message", "crates/source_analyzer", "crates/spl_network", "crates/spl_network_messages", @@ -48,6 +49,7 @@ members = [ "crates/types", "crates/vision", "crates/walking_engine", + "crates/zed", "tools/annotato", "tools/camera_matrix_extractor", "tools/depp", @@ -97,6 +99,7 @@ bevy_panorbit_camera = { version = "0.23.0", features = ["bevy_egui"] } bincode = "1.3.3" bindgen = "0.71.1" blake3 = { version = "1.8.2", features = ["mmap", "serde"] } +booster = { path = "crates/booster" } buffered_watch = { path = "crates/buffered_watch" } build_script_helpers = { path = "crates/build_script_helpers" } byteorder = "1.5.0" @@ -140,7 +143,6 @@ glob = "0.3.2" hardware = { path = "crates/hardware" } home = "=0.5.9" hula_types = { path = "crates/hula_types" } -hulk = { path = "crates/hulk" } hulk_manifest = { path = "crates/hulk_manifest" } hulk_widgets = { path = "crates/hulk_widgets" } hungarian_algorithm = { path = "crates/hungarian_algorithm" } @@ -201,6 +203,7 @@ regex = "1.11.1" repository = { path = "crates/repository" } reqwest = { version = "0.12.12", features = ["blocking"] } rmp-serde = "1.3.0" +ros2 = { path = "crates/ros2" } rustfft = "6.2.0" scenario = { path = "crates/scenario" } semver = "1.0.25" @@ -213,6 +216,7 @@ serde_json = "1.0.138" serde_test = "1.0.177" sha2 = "0.10.8" simba = "0.9.0" +simulation_message = { path = "crates/simulation_message" } smallvec = "1.14.0" source_analyzer = { path = "crates/source_analyzer" } spl_network = { path = "crates/spl_network" } @@ -244,6 +248,7 @@ watch = "0.2.3" webots = { version = "0.8.0" } xdg = "2.5.2" zbus = "5.5.0" +zed = { path = "crates/zed" } [patch.crates-io] # Pinned to forked serde version since https://github.com/serde-rs/serde/pull/2513 is not merged diff --git a/crates/bevyhavior_simulator/Cargo.toml b/crates/bevyhavior_simulator/Cargo.toml index 04e328ad5a..ec3fbb0a52 100644 --- a/crates/bevyhavior_simulator/Cargo.toml +++ b/crates/bevyhavior_simulator/Cargo.toml @@ -10,6 +10,7 @@ approx = { workspace = true } ball_filter = { workspace = true } bevy = { workspace = true } bincode = { workspace = true } +booster = { workspace = true } buffered_watch = { workspace = true } chrono = { workspace = true } clap = { workspace = true } @@ -48,6 +49,7 @@ tokio-util = { workspace = true } types = { workspace = true } vision = { workspace = true } walking_engine = { workspace = true } +zed = { workspace = true } [build-dependencies] code_generation = { workspace = true } diff --git a/crates/bevyhavior_simulator/src/interfake.rs b/crates/bevyhavior_simulator/src/interfake.rs index ba178854f9..e9a8987f72 100644 --- a/crates/bevyhavior_simulator/src/interfake.rs +++ b/crates/bevyhavior_simulator/src/interfake.rs @@ -9,13 +9,14 @@ use parking_lot::Mutex; use buffered_watch::{Receiver, Sender}; use color_eyre::Result; use hardware::{ - CameraInterface, NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface, - TimeInterface, + CameraInterface, NetworkInterface, PathsInterface, RGBDSensorsInterface, RecordingInterface, + SpeakerInterface, TimeInterface, }; use types::{ audio::SpeakerRequest, messages::{IncomingMessage, OutgoingMessage}, }; +use zed::RGBDSensors; use crate::{cyclers::control::Database, HardwareInterface}; @@ -83,6 +84,12 @@ impl CameraInterface for Interfake { } } +impl RGBDSensorsInterface for Interfake { + fn read_rgbd_sensors(&self) -> Result { + unimplemented!() + } +} + pub trait FakeDataInterface { fn get_last_database_receiver(&self) -> &Mutex>; fn get_last_database_sender(&self) -> &Mutex>; diff --git a/crates/bevyhavior_simulator/src/lib.rs b/crates/bevyhavior_simulator/src/lib.rs index 7f7cf68864..0323bb8d53 100644 --- a/crates/bevyhavior_simulator/src/lib.rs +++ b/crates/bevyhavior_simulator/src/lib.rs @@ -1,8 +1,8 @@ #![allow(unexpected_cfgs)] use hardware::{ - CameraInterface, NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface, - TimeInterface, + CameraInterface, NetworkInterface, PathsInterface, RGBDSensorsInterface, RecordingInterface, + SpeakerInterface, TimeInterface, }; use interfake::FakeDataInterface; @@ -33,5 +33,6 @@ pub trait HardwareInterface: + SpeakerInterface + PathsInterface + CameraInterface + + RGBDSensorsInterface { } diff --git a/crates/booster/Cargo.toml b/crates/booster/Cargo.toml new file mode 100644 index 0000000000..aaea6643f0 --- /dev/null +++ b/crates/booster/Cargo.toml @@ -0,0 +1,16 @@ +[package] +name = "booster" +version.workspace = true +edition.workspace = true +license.workspace = true +homepage.workspace = true + +[dependencies] +coordinate_systems = { workspace = true } +linear_algebra = { workspace = true } +nalgebra = { workspace = true } +path_serde = { workspace = true } +ros2 = { workspace = true } +serde = { workspace = true } +serde_json = { workspace = true } +tokio = { workspace = true } diff --git a/crates/booster/src/lib.rs b/crates/booster/src/lib.rs new file mode 100644 index 0000000000..de1f195817 --- /dev/null +++ b/crates/booster/src/lib.rs @@ -0,0 +1,187 @@ +use coordinate_systems::Robot; +use linear_algebra::Vector3; +use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; +use ros2::geometry_msgs::transform_stamped::TransformStamped; +use serde::{Deserialize, Serialize}; + +#[derive( + Clone, Debug, Default, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect, +)] +pub struct LowState { + /// IMU feedback + pub imu_state: ImuState, + /// Parallel structure joint feedback + pub motor_state_parallel: Vec, + /// Serial structure joint feedback + pub motor_state_serial: Vec, +} + +#[derive( + Clone, Debug, Default, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect, +)] +pub struct ImuState { + #[serde(rename = "rpy")] + /// Euler angle information（x -> roll, y -> pitch, z -> yaw） + pub roll_pitch_yaw: Vector3, + /// Angular velocity information + #[serde(rename = "gyro")] + pub angular_velocity: Vector3, + /// Acceleration information + #[serde(rename = "acc")] + pub linear_acceleration: Vector3, +} + +#[derive(Debug, Copy, Clone, Serialize, Deserialize)] +pub struct MotorState { + #[serde(rename = "q")] + /// Joint angle position (q), unit: rad. + pub position: f32, + #[serde(rename = "dq")] + /// Joint angular velocity (dq), unit: rad/s. + pub velocity: f32, + #[serde(rename = "ddq")] + /// Joint angular acceleration (ddq), unit: rad/s². + pub acceleration: f32, + #[serde(rename = "tau_est")] + /// Joint torque (tau_est), unit: nm + pub torque: f32, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub enum CommandType { + Parallel, + Serial, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct LowCommand { + #[serde(rename = "cmd_type")] + pub command_type: CommandType, + #[serde(rename = "motor_cmd")] + pub motor_commands: Vec, +} + +#[derive(Debug, Default, Clone, Copy, Serialize, Deserialize)] +pub struct MotorCommand { + #[serde(rename = "q")] + /// Joint angle position, unit: rad. + pub position: f32, + #[serde(rename = "dq")] + /// Joint angular velocity, unit: rad/s. + pub velocity: f32, + #[serde(rename = "tau")] + /// Joint torque, unit: nm + pub torque: f32, + /// Proportional coefficient. + pub kp: f32, + /// Gain coefficient. + pub kd: f32, + /// Weight, range [0, 1], specify the proportion of user set motor cmd is mixed with the original cmd sent by the internal controller, which is usually used for gradually move to a user custom motor state from internal controlled motor state. Weight 0 means fully controlled by internal controller, weight 1 means fully controlled by user sent cmds. This parameter is not working if in custom mode, as in custom mode, internal controller will send no motor cmds. + pub weight: f32, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub enum FallDownStateType { + IsReady, + IsFalling, + HasFallen, + IsGettingUp, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct FallDownState { + pub fall_down_state: FallDownStateType, + /// Whether recovery (getting up) action is available + pub is_recovery_available: bool, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub enum ButtonEventType { + PressDown, + PressUp, + SingleClick, + DoubleClick, + TripleClick, + LongPressStart, + LongPressHold, + LongPressEnd, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct ButtonEventMsg { + pub button: i64, + pub event: ButtonEventType, +} + +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct RemoteControllerState { + /** This feature can be used in user programs to implement custom gamepad/controller button functionality. + |type | code | description| + |-|-|-| + |NONE| 0 |no event | + |AXIS | 0x600 | axis motion | + |HAT | 0x602 | hat position change | + |BUTTON_DOWN | 0x603 | button pressed | + |BUTTON_UP | 0x604 | button released | + |REMOVE | 0x606 | device has been removed | + */ + pub event: u64, // refer to remarks + + #[serde(rename = "lx")] + /// left stick horizontal direction, push left to -1, push right to 1 + pub left_joystick_x: f32, + #[serde(rename = "ly")] + /// left stick vertical direction, push front to -1, push back to 1 + pub left_joystick_y: f32, + #[serde(rename = "rx")] + /// right stick horizontal direction, push left to -1, push right to 1 + pub right_joystick_x: f32, + #[serde(rename = "ry")] + /// right stick vertical direction, push front to -1, push back to 1 + pub right_joystick_y: f32, + + pub a: bool, + pub b: bool, + pub x: bool, + pub y: bool, + #[serde(rename = "lb")] + pub left_button: bool, + #[serde(rename = "rb")] + pub right_button: bool, + #[serde(rename = "lt")] + pub left_trigger: bool, + #[serde(rename = "rt")] + pub right_trigger: bool, + #[serde(rename = "ls")] + pub left_joystick: bool, + #[serde(rename = "rs")] + pub right_joystick: bool, + pub back: bool, + pub start: bool, + + #[serde(rename = "hat_c")] + pub dpad_centered: bool, + #[serde(rename = "hat_u")] + pub dpad_up: bool, + #[serde(rename = "hat_d")] + pub dpad_down: bool, + #[serde(rename = "hat_l")] + pub dpad_left: bool, + #[serde(rename = "hat_r")] + pub dpad_right: bool, + #[serde(rename = "hat_lu")] + pub dpad_left_up: bool, + #[serde(rename = "hat_ld")] + pub dpad_left_down: bool, + #[serde(rename = "hat_ru")] + pub dpad_right_up: bool, + #[serde(rename = "hat_rd")] + pub dpad_right_: bool, + pub reserved: u8, +} + +#[derive(Debug, Deserialize, Serialize)] +#[serde(rename = "TFMessage")] +pub struct TransformMessage { + pub transforms: Vec, +} diff --git a/crates/control/Cargo.toml b/crates/control/Cargo.toml index 0a1e99ba4f..43ad5e71b8 100644 --- a/crates/control/Cargo.toml +++ b/crates/control/Cargo.toml @@ -9,6 +9,7 @@ homepage.workspace = true approx = { workspace = true } ball_filter = { workspace = true } bincode = { workspace = true } +booster = { workspace = true } calibration = { workspace = true } color-eyre = { workspace = true } context_attribute = { workspace = true } diff --git a/crates/control/src/motion/command_sender.rs b/crates/control/src/motion/command_sender.rs index 5576bc9ed6..0e20504219 100644 --- a/crates/control/src/motion/command_sender.rs +++ b/crates/control/src/motion/command_sender.rs @@ -1,33 +1,23 @@ +use std::f32::consts::PI; + +use approx::abs_diff_eq; +use booster::{CommandType, LowCommand, MotorCommand}; use color_eyre::{eyre::WrapErr, Result}; use context_attribute::context; -use framework::AdditionalOutput; -use hardware::ActuatorInterface; +use hardware::LowCommandInterface; use serde::{Deserialize, Serialize}; -use types::{ - joints::Joints, led::Leds, motion_selection::MotionSafeExits, motor_commands::MotorCommands, -}; #[derive(Deserialize, Serialize)] -pub struct CommandSender {} +pub struct CommandSender { + time_index: f32, + motor_index: usize, +} #[context] pub struct CreationContext {} #[context] pub struct CycleContext { - optimized_motor_commands: Input>, "motor_commands">, - leds: Input, - joint_calibration_offsets: Parameter, "joint_calibration_offsets">, - motion_safe_exits: CyclerState, - last_actuated_motor_commands: - CyclerState>, "last_actuated_motor_commands">, - - motion_safe_exits_output: AdditionalOutput, - actuated_motor_commands: - AdditionalOutput>, "actuated_motor_commands">, - actuated_motor_commands_difference: - AdditionalOutput, "actuated_motor_commands_difference">, - hardware_interface: HardwareInterface, } @@ -37,40 +27,55 @@ pub struct MainOutputs {} impl CommandSender { pub fn new(_context: CreationContext) -> Result { - Ok(Self {}) + Ok(Self { + time_index: 0.0, + motor_index: 0, + }) } pub fn cycle( &mut self, - mut context: CycleContext, + context: CycleContext, ) -> Result { - let motor_commands = context.optimized_motor_commands; + let motor_commands = + Self::generate_random_motor_commands(self.motor_index, self.time_index); - context - .hardware_interface - .write_to_actuators( - motor_commands.positions + *context.joint_calibration_offsets, - motor_commands.stiffnesses, - *context.leds, - ) - .wrap_err("failed to write to actuators")?; + self.time_index += PI / 100.0; + if abs_diff_eq!(self.time_index % (8.0 * PI), 0.0, epsilon = 0.001) { + self.motor_index = (self.motor_index + 1) % 22; + self.time_index = 0.0; + } - context - .actuated_motor_commands - .fill_if_subscribed(|| *motor_commands); - context - .motion_safe_exits_output - .fill_if_subscribed(|| context.motion_safe_exits.clone()); + let low_command = LowCommand { + command_type: CommandType::Serial, + motor_commands: motor_commands.to_vec(), + }; context - .actuated_motor_commands_difference - .fill_if_subscribed(|| { - motor_commands.positions - context.last_actuated_motor_commands.positions - }); - - context.last_actuated_motor_commands.positions = motor_commands.positions; - context.last_actuated_motor_commands.stiffnesses = motor_commands.stiffnesses; + .hardware_interface + .write_low_command(low_command) + .wrap_err("failed to write to actuators")?; Ok(MainOutputs {}) } + + fn generate_random_motor_commands(motor_index: usize, time_index: f32) -> [MotorCommand; 22] { + let mut motor_commands: [MotorCommand; 22] = [MotorCommand { + position: 0.0, + velocity: 0.0, + torque: 0.0, + kp: 45.0, + kd: 0.2, + weight: 1.0, + }; 22]; + motor_commands[motor_index] = MotorCommand { + position: time_index.sin(), + velocity: time_index.sin(), + torque: 1.0, + kp: 25.0, + kd: 0.3, + weight: 1.0, + }; + motor_commands + } } diff --git a/crates/control/src/sensor_data_receiver.rs b/crates/control/src/sensor_data_receiver.rs index 8a499cc1c8..da0406bfa2 100644 --- a/crates/control/src/sensor_data_receiver.rs +++ b/crates/control/src/sensor_data_receiver.rs @@ -1,15 +1,16 @@ use std::time::{SystemTime, UNIX_EPOCH}; +use booster::LowState; use color_eyre::{eyre::WrapErr, Result}; use context_attribute::context; use coordinate_systems::Robot; use filtering::low_pass_filter::LowPassFilter; -use framework::{AdditionalOutput, MainOutput}; -use hardware::{SensorInterface, TimeInterface}; -use linear_algebra::{Orientation3, Vector2, Vector3}; +use framework::MainOutput; +use hardware::{LowStateInterface, TimeInterface}; +use linear_algebra::Vector3; use nalgebra::UnitQuaternion; use serde::{Deserialize, Serialize}; -use types::{cycle_time::CycleTime, joints::Joints, sensor_data::SensorData}; +use types::cycle_time::CycleTime; #[derive(Default, Serialize, Deserialize)] enum State { @@ -17,7 +18,7 @@ enum State { WaitingForSteady, CalibratingGravity { filtered_gravity: LowPassFilter>, - filtered_roll_pitch: LowPassFilter>, + filtered_roll_pitch_yaw: LowPassFilter>, remaining_cycles: usize, }, Calibrated { @@ -37,26 +38,11 @@ pub struct CreationContext {} #[context] pub struct CycleContext { hardware_interface: HardwareInterface, - joint_calibration_offsets: Parameter, "joint_calibration_offsets">, - - calibration_steady_threshold: - Parameter, - gravity_calibration_smoothing_factor: - Parameter, - roll_pitch_calibration_smoothing_factor: - Parameter, - number_of_calibration_cycles: - Parameter, - - maximum_temperature: AdditionalOutput, - total_current: AdditionalOutput, - - roll_pitch_calibrated: AdditionalOutput, } #[context] pub struct MainOutputs { - pub sensor_data: MainOutput, + pub low_state: MainOutput, pub cycle_time: MainOutput, } @@ -70,88 +56,13 @@ impl SensorDataReceiver { pub fn cycle( &mut self, - mut context: CycleContext, + context: CycleContext, ) -> Result { - let mut sensor_data = context + let low_state = context .hardware_interface - .read_from_sensors() + .read_low_state() .wrap_err("failed to read from sensors")?; - let measured_angular_velocity = sensor_data.inertial_measurement_unit.angular_velocity; - let measured_acceleration = sensor_data.inertial_measurement_unit.linear_acceleration; - let measured_roll_pitch = sensor_data.inertial_measurement_unit.roll_pitch; - let angular_velocity_sum = measured_angular_velocity.abs().inner.sum(); - - let is_steady = angular_velocity_sum < *context.calibration_steady_threshold; - match &mut self.calibration_state { - State::WaitingForSteady => { - if is_steady { - self.calibration_state = State::CalibratingGravity { - filtered_gravity: LowPassFilter::with_smoothing_factor( - measured_acceleration, - *context.gravity_calibration_smoothing_factor, - ), - filtered_roll_pitch: LowPassFilter::with_smoothing_factor( - measured_roll_pitch, - *context.roll_pitch_calibration_smoothing_factor, - ), - remaining_cycles: *context.number_of_calibration_cycles, - } - } - } - State::CalibratingGravity { - filtered_gravity, - filtered_roll_pitch, - remaining_cycles, - } => { - if is_steady { - filtered_gravity.update(measured_acceleration); - filtered_roll_pitch.update(measured_roll_pitch); - *remaining_cycles -= 1; - - if *remaining_cycles == 0 { - let gravity = -filtered_gravity.state().inner; - let up = nalgebra::Vector3::y(); - let orientation = UnitQuaternion::look_at_rh(&gravity, &up); - let roll_pitch_orientation = Orientation3::::from_euler_angles( - -filtered_roll_pitch.state().x(), - -filtered_roll_pitch.state().y(), - 0.0, - ) - .mirror(); - - let roll_pitch_calibration = - roll_pitch_orientation.inner.rotation_to(&orientation); - - self.calibration_state = State::Calibrated { - calibration: roll_pitch_calibration, - }; - } - } else { - self.calibration_state = State::WaitingForSteady; - } - } - State::Calibrated { .. } => {} - } - - if let State::Calibrated { calibration } = self.calibration_state { - let mut roll_pitch_orientation = Orientation3::::from_euler_angles( - -sensor_data.inertial_measurement_unit.roll_pitch.x(), - -sensor_data.inertial_measurement_unit.roll_pitch.y(), - 0.0, - ) - .mirror(); - - roll_pitch_orientation.inner = calibration * roll_pitch_orientation.inner; - - let (roll, pitch, _) = roll_pitch_orientation.euler_angles(); - - sensor_data.inertial_measurement_unit.roll_pitch.inner.x = roll; - sensor_data.inertial_measurement_unit.roll_pitch.inner.y = pitch; - } - - sensor_data.positions = sensor_data.positions - (*context.joint_calibration_offsets); - let now = context.hardware_interface.get_now(); let cycle_time = CycleTime { start_time: now, @@ -160,24 +71,8 @@ impl SensorDataReceiver { .expect("time ran backwards"), }; - context.maximum_temperature.fill_if_subscribed(|| { - sensor_data - .temperature_sensors - .into_iter() - .fold(0.0, f32::max) - }); - - context - .total_current - .fill_if_subscribed(|| sensor_data.currents.into_iter().sum()); - - context - .roll_pitch_calibrated - .fill_if_subscribed(|| matches!(self.calibration_state, State::Calibrated { .. })); - - self.last_cycle_start = now; Ok(MainOutputs { - sensor_data: sensor_data.into(), + low_state: low_state.into(), cycle_time: cycle_time.into(), }) } diff --git a/crates/hardware/Cargo.toml b/crates/hardware/Cargo.toml index b1f3f48ab3..827efa5926 100644 --- a/crates/hardware/Cargo.toml +++ b/crates/hardware/Cargo.toml @@ -6,6 +6,9 @@ license.workspace = true homepage.workspace = true [dependencies] +booster = { workspace = true } color-eyre = { workspace = true } hula_types = { workspace = true } +ros2 = { workspace = true } types = { workspace = true } +zed = { workspace = true } diff --git a/crates/hardware/src/lib.rs b/crates/hardware/src/lib.rs index f35323f5be..13b1a28b66 100644 --- a/crates/hardware/src/lib.rs +++ b/crates/hardware/src/lib.rs @@ -1,5 +1,8 @@ use std::time::SystemTime; +use booster::{ + ButtonEventMsg, FallDownState, LowCommand, LowState, RemoteControllerState, TransformMessage, +}; use color_eyre::eyre::Result; use hula_types::hardware::{Ids, Paths}; @@ -13,6 +16,7 @@ use types::{ sensor_data::SensorData, ycbcr422_image::YCbCr422Image, }; +use zed::RGBDSensors; pub trait ActuatorInterface { fn write_to_actuators( @@ -53,6 +57,31 @@ pub trait SensorInterface { fn read_from_sensors(&self) -> Result; } +pub trait LowStateInterface { + fn read_low_state(&self) -> Result; +} + +pub trait LowCommandInterface { + fn write_low_command(&self, low_command: LowCommand) -> Result<()>; +} + +pub trait FallDownStateInterface { + fn read_fall_down_state(&self) -> Result; +} +pub trait ButtonEventMsgInterface { + fn read_button_event_msg(&self) -> Result; +} +pub trait RemoteControllerStateInterface { + fn read_remote_controller_state(&self) -> Result; +} +pub trait TransformMessageInterface { + fn read_transform_message(&self) -> Result; +} + +pub trait RGBDSensorsInterface { + fn read_rgbd_sensors(&self) -> Result; +} + pub trait SpeakerInterface { fn write_to_speakers(&self, request: SpeakerRequest); } diff --git a/crates/hulk/src/lib.rs b/crates/hulk/src/lib.rs deleted file mode 100644 index 5b6c6ba7e8..0000000000 --- a/crates/hulk/src/lib.rs +++ /dev/null @@ -1,22 +0,0 @@ -#![recursion_limit = "256"] - -use hardware::{ - ActuatorInterface, CameraInterface, IdInterface, MicrophoneInterface, NetworkInterface, - PathsInterface, RecordingInterface, SensorInterface, SpeakerInterface, TimeInterface, -}; - -pub trait HardwareInterface: - ActuatorInterface - + CameraInterface - + IdInterface - + MicrophoneInterface - + NetworkInterface - + PathsInterface - + RecordingInterface - + SensorInterface - + SpeakerInterface - + TimeInterface -{ -} - -include!(concat!(env!("OUT_DIR"), "/generated_code.rs")); diff --git a/crates/hulk_imagine/Cargo.toml b/crates/hulk_imagine/Cargo.toml index 0384f48f75..0126bbe52e 100644 --- a/crates/hulk_imagine/Cargo.toml +++ b/crates/hulk_imagine/Cargo.toml @@ -9,6 +9,7 @@ homepage.workspace = true audio = { workspace = true } ball_filter = { workspace = true } bincode = { workspace = true } +booster = { workspace = true } buffered_watch = { workspace = true } calibration = { workspace = true } clap = { workspace = true } @@ -40,6 +41,7 @@ tokio = { workspace = true } types = { workspace = true } vision = { workspace = true } walking_engine = { workspace = true } +zed = { workspace = true } [build-dependencies] code_generation = { workspace = true } diff --git a/crates/hulk_imagine/src/extractor_hardware_interface.rs b/crates/hulk_imagine/src/extractor_hardware_interface.rs index e69ee2bf3f..54e030b233 100644 --- a/crates/hulk_imagine/src/extractor_hardware_interface.rs +++ b/crates/hulk_imagine/src/extractor_hardware_interface.rs @@ -1,5 +1,6 @@ use hardware::{ - ActuatorInterface, NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface, + ActuatorInterface, LowCommandInterface, LowStateInterface, NetworkInterface, PathsInterface, + RecordingInterface, SpeakerInterface, }; use color_eyre::eyre::Result; @@ -13,7 +14,13 @@ use types::{ }; pub trait HardwareInterface: - ActuatorInterface + NetworkInterface + PathsInterface + RecordingInterface + SpeakerInterface + ActuatorInterface + + LowCommandInterface + + LowStateInterface + + NetworkInterface + + PathsInterface + + RecordingInterface + + SpeakerInterface { } @@ -31,6 +38,18 @@ impl ActuatorInterface for ExtractorHardwareInterface { } } +impl LowCommandInterface for ExtractorHardwareInterface { + fn write_low_command(&self, _low_command: booster::LowCommand) -> Result<()> { + unimplemented!() + } +} + +impl LowStateInterface for ExtractorHardwareInterface { + fn read_low_state(&self) -> Result { + unimplemented!() + } +} + /// `read_from_network` is only executed in setup nodes, which are not executed during replay /// `write_to_network` is a noop during replay impl NetworkInterface for ExtractorHardwareInterface { diff --git a/crates/hulk_imagine/src/main.rs b/crates/hulk_imagine/src/main.rs index f9924f1f4d..a4d0f3482b 100644 --- a/crates/hulk_imagine/src/main.rs +++ b/crates/hulk_imagine/src/main.rs @@ -83,24 +83,12 @@ fn main() -> Result<()> { ) .wrap_err("failed to create image extractor")?; - replayer - .audio_subscriptions_sender - .borrow_mut() - .insert("additional_outputs".to_string()); replayer .control_subscriptions_sender .borrow_mut() .insert("additional_outputs".to_string()); replayer - .spl_network_subscriptions_sender - .borrow_mut() - .insert("additional_outputs".to_string()); - replayer - .vision_top_subscriptions_sender - .borrow_mut() - .insert("additional_outputs".to_string()); - replayer - .vision_bottom_subscriptions_sender + .vision_subscriptions_sender .borrow_mut() .insert("additional_outputs".to_string()); @@ -140,14 +128,9 @@ fn main() -> Result<()> { }; mcap_converter.attach(attachment)?; - let audio_receiver = replayer.audio_receiver(); let control_receiver = replayer.control_receiver(); - let spl_network_receiver = replayer.spl_network_receiver(); - let vision_top_receiver = replayer.vision_top_receiver(); - let vision_bottom_receiver = replayer.vision_bottom_receiver(); + let vision_receiver = replayer.vision_receiver(); - write_to_mcap(&mut replayer, "Audio", &mut mcap_converter, audio_receiver) - .wrap_err("failed to write audio data to mcap")?; write_to_mcap( &mut replayer, "Control", @@ -157,25 +140,11 @@ fn main() -> Result<()> { .wrap_err("failed to write control data to mcap")?; write_to_mcap( &mut replayer, - "VisionBottom", + "Vision", &mut mcap_converter, - vision_bottom_receiver, - ) - .wrap_err("failed to write vision bottom data to mcap")?; - write_to_mcap( - &mut replayer, - "VisionTop", - &mut mcap_converter, - vision_top_receiver, + vision_receiver, ) .wrap_err("failed to write vision top data to mcap")?; - write_to_mcap( - &mut replayer, - "SplNetwork", - &mut mcap_converter, - spl_network_receiver, - ) - .wrap_err("failed to write spl network data to mcap")?; mcap_converter.finish()?; diff --git a/crates/hulk_manifest/src/lib.rs b/crates/hulk_manifest/src/lib.rs index 821b5d9f9c..b08e505978 100644 --- a/crates/hulk_manifest/src/lib.rs +++ b/crates/hulk_manifest/src/lib.rs @@ -12,126 +12,126 @@ pub fn collect_hulk_cyclers(root: impl AsRef) -> Result { CyclerManifest { name: "Vision", kind: CyclerKind::Perception, - instances: vec!["Top", "Bottom"], + instances: vec![""], setup_nodes: vec!["vision::image_receiver"], nodes: vec![ - "vision::ball_detection", - "vision::calibration_measurement_provider", - "vision::camera_matrix_extractor", - "vision::feet_detection", - "vision::field_border_detection", - "vision::image_segmenter", - "vision::limb_projector", - "vision::line_detection", - "vision::perspective_grid_candidates_provider", - "vision::segment_filter", + // "vision::ball_detection", + // "vision::calibration_measurement_provider", + // "vision::camera_matrix_extractor", + // "vision::feet_detection", + // "vision::field_border_detection", + // "vision::image_segmenter", + // "vision::limb_projector", + // "vision::line_detection", + // "vision::perspective_grid_candidates_provider", + // "vision::segment_filter", ], execution_time_warning_threshold: Some(Duration::from_secs_f32(1.0 / 30.0)), }, - CyclerManifest { - name: "ObjectDetection", - kind: CyclerKind::Perception, - instances: vec!["Top"], - setup_nodes: vec!["vision::image_receiver"], - nodes: vec![ - "object_detection::pose_detection", - "object_detection::pose_filter", - "object_detection::pose_interpretation", - ], - execution_time_warning_threshold: Some(Duration::from_secs_f32(1.0)), - }, + // CyclerManifest { + // name: "ObjectDetection", + // kind: CyclerKind::Perception, + // instances: vec!["Top"], + // setup_nodes: vec!["vision::image_receiver"], + // nodes: vec![ + // "object_detection::pose_detection", + // "object_detection::pose_filter", + // "object_detection::pose_interpretation", + // ], + // execution_time_warning_threshold: Some(Duration::from_secs_f32(1.0)), + // }, CyclerManifest { name: "Control", kind: CyclerKind::RealTime, instances: vec![""], setup_nodes: vec!["control::sensor_data_receiver"], nodes: vec![ - "control::active_vision", - "control::ball_filter", - "control::ball_state_composer", - "control::behavior::node", - "control::button_filter", - "control::calibration_controller", - "control::camera_matrix_calculator", - "control::center_of_mass_provider", - "control::dribble_path_planner", - "control::fall_state_estimation", - "control::filtered_game_controller_state_timer", - "control::foot_bumper_filter", - "control::free_kick_signal_filter", - "control::game_controller_filter", - "control::game_controller_state_filter", - "control::ground_contact_detector", - "control::ground_provider", - "control::kick_selector", - "control::kinematics_provider", - "control::led_status", - "control::localization", - "control::motion::animation", - "control::motion::arms_up_squat", - "control::motion::arms_up_stand", - "control::motion::center_jump", + // "control::active_vision", + // "control::ball_filter", + // "control::ball_state_composer", + // "control::behavior::node", + // "control::button_filter", + // "control::calibration_controller", + // "control::camera_matrix_calculator", + // "control::center_of_mass_provider", + // "control::dribble_path_planner", + // "control::fall_state_estimation", + // "control::filtered_game_controller_state_timer", + // "control::foot_bumper_filter", + // "control::free_kick_signal_filter", + // "control::game_controller_filter", + // "control::game_controller_state_filter", + // "control::ground_contact_detector", + // "control::ground_provider", + // "control::kick_selector", + // "control::kinematics_provider", + // "control::led_status", + // "control::localization", + // "control::motion::animation", + // "control::motion::arms_up_squat", + // "control::motion::arms_up_stand", + // "control::motion::center_jump", "control::motion::command_sender", - "control::motion::condition_input_provider", - "control::motion::dispatching_interpolator", - "control::motion::fall_protector", - "control::motion::head_motion", - "control::motion::jump_left", - "control::motion::jump_right", - "control::motion::keeper_jump_left", - "control::motion::keeper_jump_right", - "control::motion::look_around", - "control::motion::look_at", - "control::motion::motion_selector", - "control::motion::motor_commands_collector", - "control::motion::obstacle_avoiding_arms", - "control::motion::sit_down", - "control::motion::stand_up_back", - "control::motion::stand_up_front", - "control::motion::stand_up_sitting", - "control::motion::step_planner", - "control::motion::walk_manager", - "control::motion::walking_engine", - "control::motion::wide_stance", - "control::obstacle_filter", - "control::obstacle_receiver", - "control::odometry", - "control::orientation_filter", - "control::penalty_shot_direction_estimation", - "control::primary_state_filter", - "control::ready_signal_detection_filter", - "control::referee_position_provider", - "control::role_assignment", - "control::rule_obstacle_composer", - "control::sacrificial_lamb", - "control::search_suggestor", - "control::sole_pressure_filter", - "control::sonar_filter", - "control::support_foot_estimation", - "control::team_ball_receiver", - "control::time_to_reach_kick_position", - "control::whistle_filter", - "control::world_state_composer", - "control::zero_moment_point_provider", + // "control::motion::condition_input_provider", + // "control::motion::dispatching_interpolator", + // "control::motion::fall_protector", + // "control::motion::head_motion", + // "control::motion::jump_left", + // "control::motion::jump_right", + // "control::motion::keeper_jump_left", + // "control::motion::keeper_jump_right", + // "control::motion::look_around", + // "control::motion::look_at", + // "control::motion::motion_selector", + // "control::motion::motor_commands_collector", + // "control::motion::obstacle_avoiding_arms", + // "control::motion::sit_down", + // "control::motion::stand_up_back", + // "control::motion::stand_up_front", + // "control::motion::stand_up_sitting", + // "control::motion::step_planner", + // "control::motion::walk_manager", + // "control::motion::walking_engine", + // "control::motion::wide_stance", + // "control::obstacle_filter", + // "control::obstacle_receiver", + // "control::odometry", + // "control::orientation_filter", + // "control::penalty_shot_direction_estimation", + // "control::primary_state_filter", + // "control::ready_signal_detection_filter", + // "control::referee_position_provider", + // "control::role_assignment", + // "control::rule_obstacle_composer", + // "control::sacrificial_lamb", + // "control::search_suggestor", + // "control::sole_pressure_filter", + // "control::sonar_filter", + // "control::support_foot_estimation", + // "control::team_ball_receiver", + // "control::time_to_reach_kick_position", + // "control::whistle_filter", + // "control::world_state_composer", + // "control::zero_moment_point_provider", ], execution_time_warning_threshold: Some(Duration::from_secs_f32(1.0 / 83.0)), }, - CyclerManifest { - name: "SplNetwork", - kind: CyclerKind::Perception, - instances: vec![""], - setup_nodes: vec!["spl_network::message_receiver"], - nodes: vec!["spl_network::message_filter"], - execution_time_warning_threshold: None, - }, - CyclerManifest { - name: "Audio", - kind: CyclerKind::Perception, - instances: vec![""], - setup_nodes: vec!["audio::microphone_recorder"], - nodes: vec!["audio::whistle_detection"], - execution_time_warning_threshold: None, - }, + // CyclerManifest { + // name: "SplNetwork", + // kind: CyclerKind::Perception, + // instances: vec![""], + // setup_nodes: vec!["spl_network::message_receiver"], + // nodes: vec!["spl_network::message_filter"], + // execution_time_warning_threshold: None, + // }, + // CyclerManifest { + // name: "Audio", + // kind: CyclerKind::Perception, + // instances: vec![""], + // setup_nodes: vec!["audio::microphone_recorder"], + // nodes: vec!["audio::whistle_detection"], + // execution_time_warning_threshold: None, + // }, ], }; diff --git a/crates/hulk/Cargo.toml b/crates/hulk_mujoco/Cargo.toml similarity index 66% rename from crates/hulk/Cargo.toml rename to crates/hulk_mujoco/Cargo.toml index be8e8172a4..d16e9185ff 100644 --- a/crates/hulk/Cargo.toml +++ b/crates/hulk_mujoco/Cargo.toml @@ -1,47 +1,63 @@ [package] -name = "hulk" +name = "hulk_mujoco" version.workspace = true edition.workspace = true license.workspace = true homepage.workspace = true [dependencies] +approx.workspace = true audio = { workspace = true } ball_filter = { workspace = true } bincode = { workspace = true } +blake3 = { workspace = true } +booster = { workspace = true } buffered_watch = { workspace = true } calibration = { workspace = true } -color-eyre = { workspace = true } +chrono = { workspace = true } +color-eyre.workspace = true communication = { workspace = true } control = { workspace = true } coordinate_systems = { workspace = true } +ctrlc = { workspace = true } +derive_more = { workspace = true } +eframe = { workspace = true } energy_optimization = { workspace = true } +env_logger = { workspace = true } +fern = { workspace = true } framework = { workspace = true } +futures-util.workspace = true geometry = { workspace = true } hardware = { workspace = true } -hula_types = { workspace = true } +hula_types.workspace = true +hulk_widgets = { workspace = true } ittapi = { workspace = true } libc = { workspace = true, optional = true } linear_algebra = { workspace = true } -log = { workspace = true } +log.workspace = true motionfile = { workspace = true } nalgebra = { workspace = true } ndarray = { workspace = true } object_detection = { workspace = true } parameters = { workspace = true } +parking_lot = { workspace = true } path_serde = { workspace = true } projection = { workspace = true } -serde = { workspace = true } -serde_json = { workspace = true } -spl_network = { workspace = true } +ros2 = { workspace = true } +serde.workspace = true +serde_json.workspace = true +simulation_message = { workspace = true } +spl_network.workspace = true spl_network_messages = { workspace = true } -step_planning = { workspace = true } systemd = { workspace = true, optional = true } tokio = { workspace = true } +tokio-tungstenite = { workspace = true } tokio-util = { workspace = true } types = { workspace = true } vision = { workspace = true } walking_engine = { workspace = true } +zed = { workspace = true } + [build-dependencies] code_generation = { workspace = true } @@ -49,6 +65,7 @@ color-eyre = { workspace = true } hulk_manifest = { workspace = true } source_analyzer = { workspace = true } + [features] realtime = ["libc"] systemd = ["dep:systemd"] diff --git a/crates/hulk/build.rs b/crates/hulk_mujoco/build.rs similarity index 99% rename from crates/hulk/build.rs rename to crates/hulk_mujoco/build.rs index 636e94d4ab..c37376eee5 100644 --- a/crates/hulk/build.rs +++ b/crates/hulk_mujoco/build.rs @@ -1,6 +1,5 @@ -use color_eyre::eyre::{Result, WrapErr}; - use code_generation::{generate, write_to_file::WriteToFile, ExecutionMode}; +use color_eyre::eyre::{Result, WrapErr}; use hulk_manifest::collect_hulk_cyclers; use source_analyzer::{pretty::to_string_pretty, structs::Structs}; diff --git a/crates/hulk_mujoco/src/hardware_interface.rs b/crates/hulk_mujoco/src/hardware_interface.rs new file mode 100644 index 0000000000..c0cc1a0aaf --- /dev/null +++ b/crates/hulk_mujoco/src/hardware_interface.rs @@ -0,0 +1,342 @@ +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; +use std::time::{Duration, SystemTime}; + +use booster::{ + ButtonEventMsg, FallDownState, LowCommand, LowState, RemoteControllerState, TransformMessage, +}; +use color_eyre::eyre::{eyre, Context, OptionExt}; +use color_eyre::Result; +use futures_util::SinkExt; +use futures_util::StreamExt; +use hardware::{ + ButtonEventMsgInterface, IdInterface, MicrophoneInterface, NetworkInterface, PathsInterface, + RGBDSensorsInterface, RecordingInterface, SpeakerInterface, TimeInterface, +}; +use hardware::{ + FallDownStateInterface, LowCommandInterface, LowStateInterface, RemoteControllerStateInterface, + TransformMessageInterface, +}; +use hula_types::hardware::{Ids, Paths}; +use log::{error, warn}; +use parking_lot::Mutex; +use serde::Deserialize; +use simulation_message::{ClientMessageKind, ServerMessageKind, SimulationMessage}; +use tokio::sync::mpsc::{channel, Receiver, Sender}; +use tokio::time::sleep; +use tokio_tungstenite::tungstenite::Message; +use tokio_util::sync::CancellationToken; +use types::audio::SpeakerRequest; +use types::messages::{IncomingMessage, OutgoingMessage}; +use types::samples::Samples; +use zed::RGBDSensors; + +use crate::HardwareInterface; + +const CHANNEL_CAPACITY: usize = 32; + +struct WorkerChannels { + low_state_sender: Sender, + low_command_receiver: Receiver, + fall_down_sender: Sender, + button_event_msg_sender: Sender, + remote_controller_state_sender: Sender, + transform_stamped_sender: Sender, + rgbd_sensors_sender: Sender, +} + +#[derive(Clone, Debug, Deserialize)] +pub struct Parameters { + pub paths: Paths, + pub mujoco_websocket_address: String, +} + +pub struct MujocoHardwareInterface { + paths: Paths, + enable_recording: AtomicBool, + time: Arc>, + + low_state_receiver: Mutex>, + low_command_sender: Sender, + fall_down_receiver: Mutex>, + button_event_msg_receiver: Mutex>, + remote_controller_state_receiver: Mutex>, + transform_stamped_receiver: Mutex>, + rgbd_sensors_receiver: Mutex>, +} + +impl MujocoHardwareInterface { + pub fn new(keep_running: CancellationToken, parameters: Parameters) -> Result { + let (low_state_sender, low_state_receiver) = channel(CHANNEL_CAPACITY); + let (low_command_sender, low_command_receiver) = channel(CHANNEL_CAPACITY); + let (fall_down_sender, fall_down_receiver) = channel(CHANNEL_CAPACITY); + let (button_event_msg_sender, button_event_msg_receiver) = channel(CHANNEL_CAPACITY); + let (remote_controller_state_sender, remote_controller_state_receiver) = + channel(CHANNEL_CAPACITY); + let (transform_stamped_sender, transform_stamped_receiver) = channel(CHANNEL_CAPACITY); + let (rgbd_sensors_sender, rgbd_sensors_receiver) = channel(CHANNEL_CAPACITY); + + let worker_channels = WorkerChannels { + low_state_sender, + low_command_receiver, + fall_down_sender, + button_event_msg_sender, + remote_controller_state_sender, + transform_stamped_sender, + rgbd_sensors_sender, + }; + + let time = Arc::new(Mutex::new(SystemTime::UNIX_EPOCH)); + tokio::spawn(keep_running.clone().run_until_cancelled_owned(worker( + time.clone(), + parameters.mujoco_websocket_address, + keep_running.clone(), + worker_channels, + ))); + + Ok(Self { + paths: parameters.paths, + enable_recording: AtomicBool::new(false), + time, + + low_state_receiver: Mutex::new(low_state_receiver), + low_command_sender, + fall_down_receiver: Mutex::new(fall_down_receiver), + button_event_msg_receiver: Mutex::new(button_event_msg_receiver), + remote_controller_state_receiver: Mutex::new(remote_controller_state_receiver), + transform_stamped_receiver: Mutex::new(transform_stamped_receiver), + rgbd_sensors_receiver: Mutex::new(rgbd_sensors_receiver), + }) + } +} + +async fn worker( + time: Arc>, + address: String, + keep_running: CancellationToken, + mut worker_channels: WorkerChannels, +) -> Result<()> { + let mut websocket = loop { + let websocket = tokio_tungstenite::connect_async(&address).await; + if let Ok((websocket, _)) = websocket { + break websocket; + }; + log::info!("connecting to websocket failed, retrying..."); + sleep(Duration::from_secs_f32(1.0)).await; + }; + + loop { + tokio::select! { + maybe_websocket_event = websocket.next() => { + match maybe_websocket_event { + Some(Ok(message)) => handle_message(time.clone(), message, &worker_channels).await?, + Some(Err(error)) => error!("socket error {error}"), + None => break, + } + }, + maybe_low_command_event = worker_channels.low_command_receiver.recv() => { + match maybe_low_command_event { + Some(low_command) => websocket.send(Message::Text(serde_json::to_string(&ClientMessageKind::LowCommand(low_command))?.into())).await?, + None => break, + }; + }, + _ = keep_running.cancelled() => break + } + } + keep_running.cancel(); + Ok(()) +} + +async fn handle_message( + hardware_interface_time: Arc>, + message: Message, + worker_channels: &WorkerChannels, +) -> Result<()> { + let message = match message { + Message::Text(string) => serde_json::from_str(&string)?, + Message::Close(maybe_frame) => { + warn!("server closed connections: {maybe_frame:#?}"); + return Ok(()); + } + _ => return Ok(()), + }; + match message { + SimulationMessage { + payload: ServerMessageKind::LowState(low_state), + time, + } => { + *hardware_interface_time.lock() = time; + worker_channels.low_state_sender.send(low_state).await? + } + SimulationMessage { + payload: ServerMessageKind::FallDownState(fall_down_state), + time, + } => { + *hardware_interface_time.lock() = time; + worker_channels + .fall_down_sender + .send(fall_down_state) + .await? + } + SimulationMessage { + payload: ServerMessageKind::ButtonEventMsg(button_event_msg), + time, + } => { + *hardware_interface_time.lock() = time; + worker_channels + .button_event_msg_sender + .send(button_event_msg) + .await? + } + SimulationMessage { + payload: ServerMessageKind::RemoteControllerState(remote_controller_state), + time, + } => { + *hardware_interface_time.lock() = time; + worker_channels + .remote_controller_state_sender + .send(remote_controller_state) + .await? + } + SimulationMessage { + payload: ServerMessageKind::TransformMessage(transform_stamped), + time, + } => { + *hardware_interface_time.lock() = time; + worker_channels + .transform_stamped_sender + .send(transform_stamped) + .await? + } + SimulationMessage { + payload: ServerMessageKind::RGBDSensors(rgbd_sensors), + time, + } => { + *hardware_interface_time.lock() = time; + worker_channels + .rgbd_sensors_sender + .send(*rgbd_sensors) + .await? + } + }; + + Ok(()) +} + +impl LowStateInterface for MujocoHardwareInterface { + fn read_low_state(&self) -> Result { + self.low_state_receiver + .lock() + .blocking_recv() + .ok_or_eyre("low state channel closed") + } +} + +impl LowCommandInterface for MujocoHardwareInterface { + fn write_low_command(&self, low_command: LowCommand) -> Result<()> { + self.low_command_sender + .blocking_send(low_command) + .wrap_err("low command send error") + } +} + +impl FallDownStateInterface for MujocoHardwareInterface { + fn read_fall_down_state(&self) -> Result { + self.fall_down_receiver + .lock() + .blocking_recv() + .ok_or_eyre("fall down state channel closed") + } +} + +impl ButtonEventMsgInterface for MujocoHardwareInterface { + fn read_button_event_msg(&self) -> Result { + self.button_event_msg_receiver + .lock() + .blocking_recv() + .ok_or_eyre("button event msg channel closed") + } +} + +impl RemoteControllerStateInterface for MujocoHardwareInterface { + fn read_remote_controller_state(&self) -> Result { + self.remote_controller_state_receiver + .lock() + .blocking_recv() + .ok_or_eyre("channel closed") + } +} + +impl TransformMessageInterface for MujocoHardwareInterface { + fn read_transform_message(&self) -> Result { + self.transform_stamped_receiver + .lock() + .blocking_recv() + .ok_or_eyre("channel closed") + } +} + +impl RGBDSensorsInterface for MujocoHardwareInterface { + fn read_rgbd_sensors(&self) -> Result { + self.rgbd_sensors_receiver + .lock() + .blocking_recv() + .ok_or_eyre("channel closed") + } +} + +impl TimeInterface for MujocoHardwareInterface { + fn get_now(&self) -> SystemTime { + *self.time.lock() + } +} + +impl PathsInterface for MujocoHardwareInterface { + fn get_paths(&self) -> Paths { + self.paths.clone() + } +} + +impl NetworkInterface for MujocoHardwareInterface { + fn read_from_network(&self) -> Result { + todo!() + } + + fn write_to_network(&self, _message: OutgoingMessage) -> Result<()> { + todo!() + } +} + +impl SpeakerInterface for MujocoHardwareInterface { + fn write_to_speakers(&self, _request: SpeakerRequest) { + todo!() + } +} + +impl IdInterface for MujocoHardwareInterface { + fn get_ids(&self) -> Ids { + let name = "Booster K1"; + Ids { + body_id: name.to_string(), + head_id: name.to_string(), + } + } +} + +impl MicrophoneInterface for MujocoHardwareInterface { + fn read_from_microphones(&self) -> Result { + Err(eyre!("microphone interface is not implemented")) + } +} + +impl RecordingInterface for MujocoHardwareInterface { + fn should_record(&self) -> bool { + self.enable_recording.load(Ordering::SeqCst) + } + + fn set_whether_to_record(&self, enable: bool) { + self.enable_recording.store(enable, Ordering::SeqCst) + } +} + +impl HardwareInterface for MujocoHardwareInterface {} diff --git a/crates/hulk_webots/src/main.rs b/crates/hulk_mujoco/src/main.rs similarity index 58% rename from crates/hulk_webots/src/main.rs rename to crates/hulk_mujoco/src/main.rs index f2bed28994..8eadefb0cd 100644 --- a/crates/hulk_webots/src/main.rs +++ b/crates/hulk_mujoco/src/main.rs @@ -7,19 +7,18 @@ use color_eyre::{ }; use ctrlc::set_handler; use framework::Parameters as FrameworkParameters; -use hardware::IdInterface; -use hardware_interface::{HardwareInterface, Parameters as HardwareParameters}; -use hulk::execution::run; +use hardware::{ + IdInterface, LowCommandInterface, LowStateInterface, MicrophoneInterface, NetworkInterface, + PathsInterface, RGBDSensorsInterface, RecordingInterface, SpeakerInterface, TimeInterface, +}; +use hula_types::hardware::Ids; use serde_json::from_reader; use tokio_util::sync::CancellationToken; -mod camera; -mod force_sensitive_resistor_devices; +use crate::execution::run; +use crate::hardware_interface::{MujocoHardwareInterface, Parameters as HardwareParameters}; + mod hardware_interface; -mod intertial_measurement_unit_devices; -mod joint_devices; -mod keyboard_device; -mod sonar_sensor_devices; pub fn setup_logger() -> Result<(), fern::InitError> { fern::Dispatch::new() @@ -38,7 +37,24 @@ pub fn setup_logger() -> Result<(), fern::InitError> { Ok(()) } -fn main() -> Result<()> { +pub trait HardwareInterface: + IdInterface + + LowStateInterface + + LowCommandInterface + + RGBDSensorsInterface + + MicrophoneInterface + + NetworkInterface + + PathsInterface + + RecordingInterface + + SpeakerInterface + + TimeInterface +{ +} + +include!(concat!(env!("OUT_DIR"), "/generated_code.rs")); + +#[tokio::main(flavor = "multi_thread")] +async fn main() -> Result<()> { setup_logger()?; install()?; let framework_parameters_path = args() @@ -54,7 +70,7 @@ fn main() -> Result<()> { let file = File::open(framework_parameters_path).wrap_err("failed to open framework parameters")?; - let framework_parameters: FrameworkParameters = + let mut framework_parameters: FrameworkParameters = from_reader(file).wrap_err("failed to parse framework parameters")?; let file = File::open(framework_parameters.hardware_parameters) @@ -62,17 +78,24 @@ fn main() -> Result<()> { let hardware_parameters: HardwareParameters = from_reader(file).wrap_err("failed to parse hardware parameters")?; - let hardware_interface = HardwareInterface::new(keep_running.clone(), hardware_parameters) - .wrap_err("failed to create hardware interface")?; + if framework_parameters.communication_addresses.is_none() { + let fallback = "127.0.0.1:1337"; + println!("framework.json disabled communication, falling back to {fallback}"); + framework_parameters.communication_addresses = Some(fallback.to_string()); + } - let ids = hardware_interface.get_ids(); + let hardware_interface = + MujocoHardwareInterface::new(keep_running.clone(), hardware_parameters)?; run( Arc::new(hardware_interface), framework_parameters.communication_addresses, framework_parameters.parameters_directory, "logs", - ids, + Ids { + body_id: "K1_BODY".to_string(), + head_id: "K1_HEAD".to_string(), + }, keep_running, framework_parameters.recording_intervals, ) diff --git a/crates/hulk_nao/Cargo.toml b/crates/hulk_nao/Cargo.toml deleted file mode 100644 index 09f5200bd8..0000000000 --- a/crates/hulk_nao/Cargo.toml +++ /dev/null @@ -1,42 +0,0 @@ -[package] -name = "hulk_nao" -version.workspace = true -edition.workspace = true -license.workspace = true -homepage.workspace = true - -[[bin]] -name = "hulk" -path = "src/main.rs" - -[package.metadata.pepsi] -cross-compile = true - -[dependencies] -alsa = { workspace = true } -ball_filter = { workspace = true } -chrono = { workspace = true } -clap = { workspace = true } -color-eyre = { workspace = true } -ctrlc = { workspace = true } -enum-iterator = { workspace = true } -fern = { workspace = true } -framework = { workspace = true } -hardware = { workspace = true } -hula_types = { workspace = true } -hulk = { workspace = true, features = ["realtime", "systemd"] } -libc = { workspace = true } -linear_algebra = { workspace = true } -log = { workspace = true } -nalgebra = { workspace = true } -nao_camera = { workspace = true } -opusfile-ng = { workspace = true } -parking_lot = { workspace = true } -serde = { workspace = true } -serde_json = { workspace = true } -spl_network = { workspace = true } -thiserror = { workspace = true } -tokio = { workspace = true } -tokio-util = { workspace = true } -types = { workspace = true } -watch = { workspace = true } diff --git a/crates/hulk_nao/src/audio_parameter_deserializers.rs b/crates/hulk_nao/src/audio_parameter_deserializers.rs deleted file mode 100644 index 7f59e083ad..0000000000 --- a/crates/hulk_nao/src/audio_parameter_deserializers.rs +++ /dev/null @@ -1,140 +0,0 @@ -use alsa::pcm::{Access, Format}; -use serde::{de::Error, Deserialize, Deserializer}; - -pub fn deserialize_access<'de, D>(deserializer: D) -> Result -where - D: Deserializer<'de>, -{ - let value = String::deserialize(deserializer)?; - Ok(match value.as_str() { - "MMapInterleaved" => Access::MMapInterleaved, - "MMapNonInterleaved" => Access::MMapNonInterleaved, - "MMapComplex" => Access::MMapComplex, - "RWInterleaved" => Access::RWInterleaved, - "RWNonInterleaved" => Access::RWNonInterleaved, - _ => { - return Err(Error::unknown_variant( - value.as_str(), - &[ - "MMapInterleaved", - "MMapNonInterleaved", - "MMapComplex", - "RWInterleaved", - "RWNonInterleaved", - ], - )) - } - }) -} - -pub fn deserialize_format<'de, D>(deserializer: D) -> Result -where - D: Deserializer<'de>, -{ - let value = String::deserialize(deserializer)?; - Ok(match value.as_str() { - "Unknown" => Format::Unknown, - "S8" => Format::S8, - "U8" => Format::U8, - "S16LE" => Format::S16LE, - "S16BE" => Format::S16BE, - "U16LE" => Format::U16LE, - "U16BE" => Format::U16BE, - "S24LE" => Format::S24LE, - "S24BE" => Format::S24BE, - "U24LE" => Format::U24LE, - "U24BE" => Format::U24BE, - "S32LE" => Format::S32LE, - "S32BE" => Format::S32BE, - "U32LE" => Format::U32LE, - "U32BE" => Format::U32BE, - "FloatLE" => Format::FloatLE, - "FloatBE" => Format::FloatBE, - "Float64LE" => Format::Float64LE, - "Float64BE" => Format::Float64BE, - "IEC958SubframeLE" => Format::IEC958SubframeLE, - "IEC958SubframeBE" => Format::IEC958SubframeBE, - "MuLaw" => Format::MuLaw, - "ALaw" => Format::ALaw, - "ImaAdPCM" => Format::ImaAdPCM, - "MPEG" => Format::MPEG, - "GSM" => Format::GSM, - "Special" => Format::Special, - "S243LE" => Format::S243LE, - "S243BE" => Format::S243BE, - "U243LE" => Format::U243LE, - "U243BE" => Format::U243BE, - "S203LE" => Format::S203LE, - "S203BE" => Format::S203BE, - "U203LE" => Format::U203LE, - "U203BE" => Format::U203BE, - "S183LE" => Format::S183LE, - "S183BE" => Format::S183BE, - "U183LE" => Format::U183LE, - "U183BE" => Format::U183BE, - "G72324" => Format::G72324, - "G723241B" => Format::G723241B, - "G72340" => Format::G72340, - "G723401B" => Format::G723401B, - "DSDU8" => Format::DSDU8, - "DSDU16LE" => Format::DSDU16LE, - "DSDU32LE" => Format::DSDU32LE, - "DSDU16BE" => Format::DSDU16BE, - "DSDU32BE" => Format::DSDU32BE, - _ => { - return Err(Error::unknown_variant( - value.as_str(), - &[ - "Unknown", - "S8", - "U8", - "S16LE", - "S16BE", - "U16LE", - "U16BE", - "S24LE", - "S24BE", - "U24LE", - "U24BE", - "S32LE", - "S32BE", - "U32LE", - "U32BE", - "FloatLE", - "FloatBE", - "Float64LE", - "Float64BE", - "IEC958SubframeLE", - "IEC958SubframeBE", - "MuLaw", - "ALaw", - "ImaAdPCM", - "MPEG", - "GSM", - "Special", - "S243LE", - "S243BE", - "U243LE", - "U243BE", - "S203LE", - "S203BE", - "U203LE", - "U203BE", - "S183LE", - "S183BE", - "U183LE", - "U183BE", - "G72324", - "G723241B", - "G72340", - "G723401B", - "DSDU8", - "DSDU16LE", - "DSDU32LE", - "DSDU16BE", - "DSDU32BE", - ], - )) - } - }) -} diff --git a/crates/hulk_nao/src/camera.rs b/crates/hulk_nao/src/camera.rs deleted file mode 100644 index 3adb1133f1..0000000000 --- a/crates/hulk_nao/src/camera.rs +++ /dev/null @@ -1,147 +0,0 @@ -use std::{ - path::{Path, PathBuf}, - sync::Arc, - time::Duration, -}; - -use color_eyre::{ - eyre::{bail, eyre, Context}, - Result, -}; -use nao_camera::{reset_camera_device, Camera as NaoCamera, Parameters, PollingError}; -use parking_lot::Mutex; -use types::{camera_position::CameraPosition, ycbcr422_image::YCbCr422Image}; -use watch::WatchSender as Sender; - -pub struct Camera { - camera: Mutex, - image_sender: Sender>, -} -pub struct CameraHardware { - i2c_head_mutex: Arc>, - camera: Option, - configuration: CameraConfiguration, - camera_position: CameraPosition, -} - -pub struct CameraConfiguration { - path: PathBuf, - parameters: Parameters, -} - -impl Camera { - pub fn new( - path: impl AsRef, - camera_position: CameraPosition, - parameters: Parameters, - i2c_head_mutex: Arc>, - ) -> Result { - let (sender, _) = watch::channel(None); - let camera = Self { - camera: Mutex::new(CameraHardware { - i2c_head_mutex, - configuration: CameraConfiguration { - path: path.as_ref().to_path_buf(), - parameters, - }, - camera: None, - camera_position, - }), - image_sender: sender, - }; - - camera.camera.lock().reset().wrap_err("failed to reset")?; - - Ok(camera) - } - - pub fn read(&self) -> Result { - let mut this_receiver = self.image_sender.subscribe(); - - if let Some(mut camera) = self.camera.try_lock() { - let new_image = camera.get_next_image()?; - self.image_sender.send(Some(new_image.clone())); - return Ok(new_image); - } - - let image = this_receiver.wait(); - Ok(image.unwrap()) - // TODO: read consecutive sequence number checking - } -} - -impl CameraHardware { - fn get_next_image(&mut self) -> Result { - self.wait_for_device() - .wrap_err("failed to wait for device")?; - - let camera = self - .camera - .as_mut() - .ok_or_else(|| eyre!("camera does not exist"))?; - let buffer = camera.dequeue().wrap_err("failed to dequeue buffer")?; - camera - .queue(vec![ - 0; - match self.configuration.parameters.format { - nao_camera::Format::YUVU => - (4 * self.configuration.parameters.width / 2 - * self.configuration.parameters.height) as usize, - } - ]) - .wrap_err("failed to queue buffer")?; - - Ok(YCbCr422Image::from_raw_buffer( - self.configuration.parameters.width / 2, - self.configuration.parameters.height, - buffer, - )) - } - - fn wait_for_device(&mut self) -> Result<()> { - const MAXIMUM_NUMBER_OF_RETRIES: i32 = 10; - for _ in 0..MAXIMUM_NUMBER_OF_RETRIES { - const IMAGE_CAPTURE_TIMEOUT: Duration = Duration::from_secs(1); - match self - .camera - .as_ref() - .unwrap() - .poll(Some(IMAGE_CAPTURE_TIMEOUT)) - { - Ok(_) => {} - Err(PollingError::DevicePollingTimedOut) => { - self.reset().wrap_err("failed to reset")?; - continue; - } - error => error.wrap_err("failed to poll")?, - } - return Ok(()); - } - bail!("too many unsuccessful waiting retries"); - } - - fn reset(&mut self) -> Result<()> { - let _lock = self.i2c_head_mutex.lock(); - - reset_camera_device(&self.configuration.path, self.camera_position) - .wrap_err("failed to reset camera device")?; - let mut camera = NaoCamera::open(&self.configuration.path, &self.configuration.parameters) - .wrap_err("failed to open")?; - camera.start().wrap_err("failed to start")?; - for _ in 0..self.configuration.parameters.amount_of_buffers { - camera - .queue(vec![ - 0; - match self.configuration.parameters.format { - nao_camera::Format::YUVU => - ((4 * self.configuration.parameters.width - * self.configuration.parameters.height) - / 2) as usize, - } - ]) - .wrap_err("failed to queue buffer")?; - } - self.camera = Some(camera); - Ok(()) - } -} diff --git a/crates/hulk_nao/src/double_buffered_reader.rs b/crates/hulk_nao/src/double_buffered_reader.rs deleted file mode 100644 index 09f2c81745..0000000000 --- a/crates/hulk_nao/src/double_buffered_reader.rs +++ /dev/null @@ -1,472 +0,0 @@ -use std::{ - io::{self, ErrorKind, Read}, - mem::{size_of, MaybeUninit}, - os::unix::{io::AsRawFd, prelude::RawFd}, - ptr::null_mut, - slice::from_raw_parts_mut, -}; - -use libc::{fd_set, select, FD_SET, FD_ZERO}; - -const NUMBER_OF_BUFFERS: usize = 2; - -pub struct DoubleBufferedReader { - reader: Reader, - poller: Poller, - buffers: [Item; NUMBER_OF_BUFFERS], - active_buffer_index: usize, - number_of_read_bytes_in_active_buffer: usize, -} - -impl DoubleBufferedReader -where - Item: Copy + Default, - Reader: AsRawFd + Read, - Poller: Poll, -{ - pub fn from_reader_and_poller(reader: Reader, poller: Poller) -> Self { - Self { - reader, - poller, - buffers: [Item::default(); NUMBER_OF_BUFFERS], - active_buffer_index: Default::default(), - number_of_read_bytes_in_active_buffer: Default::default(), - } - } - - fn previous_active_buffer_index(&self) -> usize { - (self.active_buffer_index + NUMBER_OF_BUFFERS - 1) % NUMBER_OF_BUFFERS - } - - fn next_active_buffer_index(&self) -> usize { - (self.active_buffer_index + 1) % NUMBER_OF_BUFFERS - } - - fn activate_next_buffer(&mut self) { - self.active_buffer_index = self.next_active_buffer_index(); - self.number_of_read_bytes_in_active_buffer = 0; - } - - pub fn draining_read(&mut self) -> io::Result<&Item> { - let mut is_at_least_one_buffer_complete = false; - loop { - let buffer = unsafe { - from_raw_parts_mut( - &mut self.buffers[self.active_buffer_index] as *mut Item as *mut u8, - size_of::(), - ) - }; - match self - .reader - .read(&mut buffer[self.number_of_read_bytes_in_active_buffer..]) - { - Ok(number_of_read_bytes) => { - self.number_of_read_bytes_in_active_buffer += number_of_read_bytes; - assert!(self.number_of_read_bytes_in_active_buffer <= size_of::()); - let is_active_buffer_complete = - self.number_of_read_bytes_in_active_buffer == size_of::(); - if is_active_buffer_complete { - self.activate_next_buffer(); - is_at_least_one_buffer_complete = true; - } - } - Err(ref error) if error.kind() == ErrorKind::WouldBlock => { - if is_at_least_one_buffer_complete { - return Ok(&self.buffers[self.previous_active_buffer_index()]); - } - self.poller.poll(self.reader.as_raw_fd())?; - } - Err(error) => return Err(error), - } - } - } -} - -pub trait Poll { - fn poll(&mut self, file_descriptor: RawFd) -> io::Result<()>; -} - -pub struct SelectPoller; - -impl Poll for SelectPoller { - fn poll(&mut self, file_descriptor: RawFd) -> io::Result<()> { - unsafe { - let mut set = MaybeUninit::::uninit(); - FD_ZERO(set.as_mut_ptr()); - let mut set = set.assume_init(); - FD_SET(file_descriptor, &mut set); - if select( - file_descriptor + 1, - &mut set, - null_mut(), - null_mut(), - null_mut(), - ) < 0 - { - return Err(io::Error::last_os_error()); - } - Ok(()) - } - } -} - -#[cfg(test)] -mod tests { - use std::{ - slice::from_raw_parts, - sync::{ - atomic::{AtomicUsize, Ordering}, - Arc, - }, - }; - - use super::*; - - struct PanickingPoller; - - impl Poll for PanickingPoller { - fn poll(&mut self, _file_descriptor: RawFd) -> io::Result<()> { - panic!("should not be called"); - } - } - - struct ErroringPoller; - - impl Poll for ErroringPoller { - fn poll(&mut self, file_descriptor: RawFd) -> io::Result<()> { - assert_eq!(file_descriptor, FIXED_FILE_DESCRIPTOR); - Err(ErrorKind::ConnectionAborted.into()) - } - } - - const FIXED_FILE_DESCRIPTOR: RawFd = 42; - - #[derive(Default)] - struct CountingPoller { - number_of_polls: Arc, - } - - impl Poll for CountingPoller { - fn poll(&mut self, file_descriptor: RawFd) -> io::Result<()> { - assert_eq!(file_descriptor, FIXED_FILE_DESCRIPTOR); - self.number_of_polls.fetch_add(1, Ordering::SeqCst); - Ok(()) - } - } - - #[test] - fn read_error_is_returned() { - struct Reader; - impl AsRawFd for Reader { - fn as_raw_fd(&self) -> RawFd { - panic!("should not be called"); - } - } - impl Read for Reader { - fn read(&mut self, _buffer: &mut [u8]) -> io::Result { - Err(ErrorKind::ConnectionAborted.into()) - } - } - - let mut double_buffered_reader = - DoubleBufferedReader::::from_reader_and_poller(Reader, PanickingPoller); - let result = double_buffered_reader.draining_read(); - assert!(result.is_err()); - let error = result.unwrap_err(); - assert_eq!(error.kind(), ErrorKind::ConnectionAborted); - } - - #[test] - fn poll_error_is_returned() { - struct Reader; - impl AsRawFd for Reader { - fn as_raw_fd(&self) -> RawFd { - FIXED_FILE_DESCRIPTOR - } - } - impl Read for Reader { - fn read(&mut self, _buffer: &mut [u8]) -> io::Result { - Err(ErrorKind::WouldBlock.into()) - } - } - - let mut double_buffered_reader = - DoubleBufferedReader::::from_reader_and_poller(Reader, ErroringPoller); - let result = double_buffered_reader.draining_read(); - assert!(result.is_err()); - let error = result.unwrap_err(); - assert_eq!(error.kind(), ErrorKind::ConnectionAborted); - } - - #[test] - fn complete_read_terminates() { - struct Reader { - data: u16, - returned: bool, - } - impl AsRawFd for Reader { - fn as_raw_fd(&self) -> RawFd { - panic!("should not be called"); - } - } - impl Read for Reader { - fn read(&mut self, buffer: &mut [u8]) -> io::Result { - if self.returned { - return Err(ErrorKind::WouldBlock.into()); - } - assert_eq!(buffer.len(), size_of::()); - let data_slice = unsafe { - from_raw_parts(&self.data as *const u16 as *const u8, size_of::()) - }; - buffer.copy_from_slice(data_slice); - self.returned = true; - Ok(size_of::()) - } - } - - let data = 42; - let mut double_buffered_reader = DoubleBufferedReader::::from_reader_and_poller( - Reader { - data, - returned: false, - }, - PanickingPoller, - ); - let result = double_buffered_reader.draining_read(); - assert!(result.is_ok()); - let read_data = result.unwrap(); - assert_eq!(read_data, &data); - } - - #[test] - fn two_complete_reads_terminate_and_return_latest() { - struct Reader { - reversed_items: Vec, - } - impl AsRawFd for Reader { - fn as_raw_fd(&self) -> RawFd { - panic!("should not be called"); - } - } - impl Read for Reader { - fn read(&mut self, buffer: &mut [u8]) -> io::Result { - match self.reversed_items.pop() { - Some(item) => { - assert_eq!(buffer.len(), size_of::()); - let data_slice = unsafe { - from_raw_parts(&item as *const u16 as *const u8, size_of::()) - }; - buffer.copy_from_slice(data_slice); - Ok(size_of::()) - } - None => Err(ErrorKind::WouldBlock.into()), - } - } - } - - let reversed_items = vec![42, 1337]; - let mut double_buffered_reader = DoubleBufferedReader::::from_reader_and_poller( - Reader { - reversed_items: reversed_items.clone(), - }, - PanickingPoller, - ); - let result = double_buffered_reader.draining_read(); - assert!(result.is_ok()); - let read_data = result.unwrap(); - assert_eq!(read_data, reversed_items.first().unwrap()); - } - - #[test] - fn two_partial_reads_terminate() { - struct Item<'buffer> { - buffer: &'buffer [u8], - expected_buffer_size: usize, - } - struct Reader<'buffer> { - reversed_items: Vec>>, - } - impl AsRawFd for Reader<'_> { - fn as_raw_fd(&self) -> RawFd { - FIXED_FILE_DESCRIPTOR - } - } - impl Read for Reader<'_> { - fn read(&mut self, buffer: &mut [u8]) -> io::Result { - match self.reversed_items.pop() { - Some(Some(item)) => { - assert_eq!(buffer.len(), item.expected_buffer_size); - buffer[..item.buffer.len()].copy_from_slice(item.buffer); - Ok(item.buffer.len()) - } - None | Some(None) => Err(ErrorKind::WouldBlock.into()), - } - } - } - - let data = 42; - let reversed_items = vec![ - Some(Item { - buffer: unsafe { - from_raw_parts( - (&data as *const u16 as *const u8).add(1), - size_of::() - 1, - ) - }, - expected_buffer_size: size_of::() - 1, - }), - None, - Some(Item { - buffer: unsafe { from_raw_parts(&data as *const u16 as *const u8, 1) }, - expected_buffer_size: size_of::(), - }), - ]; - let number_of_polls: Arc = Default::default(); - let mut double_buffered_reader = DoubleBufferedReader::::from_reader_and_poller( - Reader { reversed_items }, - CountingPoller { - number_of_polls: number_of_polls.clone(), - }, - ); - let result = double_buffered_reader.draining_read(); - assert!(result.is_ok()); - assert_eq!(number_of_polls.load(Ordering::SeqCst), 1); - let read_data = result.unwrap(); - assert_eq!(read_data, &data); - } - - #[test] - fn four_partial_reads_terminate_and_return_previous_complete() { - struct Item<'buffer> { - buffer: &'buffer [u8], - expected_buffer_size: usize, - } - struct Reader<'buffer> { - reversed_items: Vec>>, - } - impl AsRawFd for Reader<'_> { - fn as_raw_fd(&self) -> RawFd { - FIXED_FILE_DESCRIPTOR - } - } - impl Read for Reader<'_> { - fn read(&mut self, buffer: &mut [u8]) -> io::Result { - match self.reversed_items.pop() { - Some(Some(item)) => { - assert_eq!(buffer.len(), item.expected_buffer_size); - buffer[..item.buffer.len()].copy_from_slice(item.buffer); - Ok(item.buffer.len()) - } - None | Some(None) => Err(ErrorKind::WouldBlock.into()), - } - } - } - - let returned_data = 42; - let incomplete_data = 1337_u16; - let reversed_items = vec![ - Some(Item { - buffer: unsafe { from_raw_parts(&incomplete_data as *const u16 as *const u8, 1) }, - expected_buffer_size: size_of::(), - }), - Some(Item { - buffer: unsafe { - from_raw_parts( - (&returned_data as *const u16 as *const u8).add(1), - size_of::() - 1, - ) - }, - expected_buffer_size: size_of::() - 1, - }), - None, - Some(Item { - buffer: unsafe { from_raw_parts(&returned_data as *const u16 as *const u8, 1) }, - expected_buffer_size: size_of::(), - }), - ]; - let number_of_polls: Arc = Default::default(); - let mut double_buffered_reader = DoubleBufferedReader::::from_reader_and_poller( - Reader { reversed_items }, - CountingPoller { - number_of_polls: number_of_polls.clone(), - }, - ); - let result = double_buffered_reader.draining_read(); - assert!(result.is_ok()); - assert_eq!(number_of_polls.load(Ordering::SeqCst), 1); - let read_data = result.unwrap(); - assert_eq!(read_data, &returned_data); - } - - #[test] - fn four_partial_reads_terminate_and_return_latest() { - struct Item<'buffer> { - buffer: &'buffer [u8], - expected_buffer_size: usize, - } - struct Reader<'buffer> { - reversed_items: Vec>>, - } - impl AsRawFd for Reader<'_> { - fn as_raw_fd(&self) -> RawFd { - FIXED_FILE_DESCRIPTOR - } - } - impl Read for Reader<'_> { - fn read(&mut self, buffer: &mut [u8]) -> io::Result { - match self.reversed_items.pop() { - Some(Some(item)) => { - assert_eq!(buffer.len(), item.expected_buffer_size); - buffer[..item.buffer.len()].copy_from_slice(item.buffer); - Ok(item.buffer.len()) - } - None | Some(None) => Err(ErrorKind::WouldBlock.into()), - } - } - } - - let returned_data = 42; - let unused_data = 1337_u16; - let reversed_items = vec![ - Some(Item { - buffer: unsafe { - from_raw_parts( - (&returned_data as *const u16 as *const u8).add(1), - size_of::() - 1, - ) - }, - expected_buffer_size: size_of::() - 1, - }), - Some(Item { - buffer: unsafe { from_raw_parts(&returned_data as *const u16 as *const u8, 1) }, - expected_buffer_size: size_of::(), - }), - Some(Item { - buffer: unsafe { - from_raw_parts( - (&unused_data as *const u16 as *const u8).add(1), - size_of::() - 1, - ) - }, - expected_buffer_size: size_of::() - 1, - }), - None, - Some(Item { - buffer: unsafe { from_raw_parts(&unused_data as *const u16 as *const u8, 1) }, - expected_buffer_size: size_of::(), - }), - ]; - let number_of_polls: Arc = Default::default(); - let mut double_buffered_reader = DoubleBufferedReader::::from_reader_and_poller( - Reader { reversed_items }, - CountingPoller { - number_of_polls: number_of_polls.clone(), - }, - ); - let result = double_buffered_reader.draining_read(); - assert!(result.is_ok()); - assert_eq!(number_of_polls.load(Ordering::SeqCst), 1); - let read_data = result.unwrap(); - assert_eq!(read_data, &returned_data); - } -} diff --git a/crates/hulk_nao/src/hardware_interface.rs b/crates/hulk_nao/src/hardware_interface.rs deleted file mode 100644 index 9083b7285a..0000000000 --- a/crates/hulk_nao/src/hardware_interface.rs +++ /dev/null @@ -1,200 +0,0 @@ -use std::{ - sync::{ - atomic::{AtomicBool, Ordering}, - Arc, - }, - time::SystemTime, -}; - -use ::hardware::{ - ActuatorInterface, CameraInterface, IdInterface, MicrophoneInterface, NetworkInterface, - SensorInterface, TimeInterface, -}; -use color_eyre::{ - eyre::{eyre, Error, WrapErr}, - Result, -}; -use parking_lot::Mutex; -use serde::Deserialize; -use tokio::{ - runtime::{Builder, Runtime}, - select, -}; -use tokio_util::sync::CancellationToken; - -use hardware::{PathsInterface, RecordingInterface, SpeakerInterface}; -use hula_types::hardware::{Ids, Paths}; -use spl_network::endpoint::{Endpoint, Ports}; -use types::{ - audio::SpeakerRequest, - camera_position::CameraPosition, - joints::Joints, - led::Leds, - messages::{IncomingMessage, OutgoingMessage}, - samples::Samples, - sensor_data::SensorData, - ycbcr422_image::YCbCr422Image, -}; - -use super::{ - camera::Camera, - hula_wrapper::HulaWrapper, - microphones::{self, Microphones}, - speakers::{self, Speakers}, -}; - -#[derive(Clone, Debug, Deserialize)] -pub struct Parameters { - pub camera_top: nao_camera::Parameters, - pub camera_bottom: nao_camera::Parameters, - pub microphones: microphones::Parameters, - pub paths: Paths, - pub speakers: speakers::Parameters, - pub spl_network_ports: Ports, -} - -pub struct HardwareInterface { - hula_wrapper: HulaWrapper, - microphones: Mutex, - speakers: Speakers, - paths: Paths, - spl_network_endpoint: Endpoint, - async_runtime: Runtime, - camera_top: Camera, - camera_bottom: Camera, - enable_recording: AtomicBool, - keep_running: CancellationToken, -} - -impl HardwareInterface { - pub fn new(keep_running: CancellationToken, parameters: Parameters) -> Result { - let i2c_head_mutex = Arc::new(Mutex::new(())); - let runtime = Builder::new_current_thread() - .enable_all() - .build() - .wrap_err("failed to create tokio runtime")?; - - Ok(Self { - hula_wrapper: HulaWrapper::new().wrap_err("failed to initialize HULA wrapper")?, - microphones: Mutex::new( - Microphones::new(parameters.microphones) - .wrap_err("failed to initialize microphones")?, - ), - speakers: Speakers::new(parameters.speakers, ¶meters.paths) - .wrap_err("failed to initialize speakers")?, - paths: parameters.paths, - spl_network_endpoint: runtime - .block_on(Endpoint::new(parameters.spl_network_ports)) - .wrap_err("failed to initialize SPL network")?, - async_runtime: runtime, - camera_top: Camera::new( - "/dev/video-top", - CameraPosition::Top, - parameters.camera_top, - i2c_head_mutex.clone(), - ) - .wrap_err("failed to initialize top camera")?, - - camera_bottom: Camera::new( - "/dev/video-bottom", - CameraPosition::Bottom, - parameters.camera_bottom, - i2c_head_mutex, - ) - .wrap_err("failed to initialize bottom camera")?, - - enable_recording: AtomicBool::new(false), - keep_running, - }) - } -} - -impl ActuatorInterface for HardwareInterface { - fn write_to_actuators( - &self, - positions: Joints, - stiffnesses: Joints, - leds: Leds, - ) -> Result<()> { - self.hula_wrapper - .write_to_actuators(positions, stiffnesses, leds) - } -} - -impl CameraInterface for HardwareInterface { - fn read_from_camera(&self, camera_position: CameraPosition) -> Result { - match camera_position { - CameraPosition::Top => self.camera_top.read(), - CameraPosition::Bottom => self.camera_bottom.read(), - } - } -} - -impl IdInterface for HardwareInterface { - fn get_ids(&self) -> Ids { - self.hula_wrapper.get_ids() - } -} - -impl MicrophoneInterface for HardwareInterface { - fn read_from_microphones(&self) -> Result { - self.microphones.lock().retrying_read() - } -} - -impl NetworkInterface for HardwareInterface { - fn read_from_network(&self) -> Result { - self.async_runtime.block_on(async { - select! { - result = self.spl_network_endpoint.read() => { - result.map_err(Error::from) - }, - _ = self.keep_running.cancelled() => { - Err(eyre!("termination requested")) - } - } - }) - } - - fn write_to_network(&self, message: OutgoingMessage) -> Result<()> { - self.async_runtime - .block_on(self.spl_network_endpoint.write(message)); - Ok(()) - } -} - -impl PathsInterface for HardwareInterface { - fn get_paths(&self) -> Paths { - self.paths.clone() - } -} - -impl RecordingInterface for HardwareInterface { - fn should_record(&self) -> bool { - self.enable_recording.load(Ordering::SeqCst) - } - - fn set_whether_to_record(&self, enable: bool) { - self.enable_recording.store(enable, Ordering::SeqCst) - } -} - -impl SensorInterface for HardwareInterface { - fn read_from_sensors(&self) -> Result { - self.hula_wrapper.read_from_hula() - } -} - -impl SpeakerInterface for HardwareInterface { - fn write_to_speakers(&self, request: SpeakerRequest) { - self.speakers.write_to_speakers(request); - } -} - -impl TimeInterface for HardwareInterface { - fn get_now(&self) -> SystemTime { - self.hula_wrapper.get_now() - } -} - -impl hulk::HardwareInterface for HardwareInterface {} diff --git a/crates/hulk_nao/src/hula.rs b/crates/hulk_nao/src/hula.rs deleted file mode 100644 index d879ab2b5a..0000000000 --- a/crates/hulk_nao/src/hula.rs +++ /dev/null @@ -1,398 +0,0 @@ -use std::{io::Write, mem::size_of, os::unix::net::UnixStream, slice::from_raw_parts}; - -use color_eyre::{eyre::Context, Result}; -use linear_algebra::IntoFramed; -use nalgebra::{vector, Vector2, Vector3}; -use types::{ - self, - joints::{arm::ArmJoints, head::HeadJoints, leg::LegJoints, Joints}, -}; - -use super::double_buffered_reader::{DoubleBufferedReader, SelectPoller}; - -#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)] -#[repr(C)] -pub struct RobotConfiguration { - pub body_id: [u8; 20], - pub body_version: u8, - pub head_id: [u8; 20], - pub head_version: u8, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct Battery { - pub charge: f32, - pub status: f32, - pub current: f32, - pub temperature: f32, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct Vertex2 { - x: f32, - y: f32, -} - -impl From for Vector2 { - fn from(vertex: Vertex2) -> Self { - vector![vertex.x, vertex.y] - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct Vertex3 { - x: f32, - y: f32, - z: f32, -} - -impl From for Vector3 { - fn from(vertex: Vertex3) -> Self { - vector![vertex.x, vertex.y, vertex.z] - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct InertialMeasurementUnit { - pub accelerometer: Vertex3, - pub angles: Vertex2, - pub gyroscope: Vertex3, -} - -impl From for types::sensor_data::InertialMeasurementUnitData { - fn from(from: InertialMeasurementUnit) -> Self { - types::sensor_data::InertialMeasurementUnitData { - linear_acceleration: -Vector3::from(from.accelerometer).framed(), - angular_velocity: Vector3::from(from.gyroscope).framed(), - roll_pitch: Vector2::from(from.angles).framed(), - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct ForceSensitiveResistors { - left_foot_front_left: f32, - left_foot_front_right: f32, - left_foot_rear_left: f32, - left_foot_rear_right: f32, - right_foot_front_left: f32, - right_foot_front_right: f32, - right_foot_rear_left: f32, - right_foot_rear_right: f32, -} - -impl From for types::sensor_data::ForceSensitiveResistors { - fn from(from: ForceSensitiveResistors) -> Self { - types::sensor_data::ForceSensitiveResistors { - left: types::sensor_data::Foot { - front_left: from.left_foot_front_left, - front_right: from.left_foot_front_right, - rear_left: from.left_foot_rear_left, - rear_right: from.left_foot_rear_right, - }, - right: types::sensor_data::Foot { - front_left: from.right_foot_front_left, - front_right: from.right_foot_front_right, - rear_left: from.right_foot_rear_left, - rear_right: from.right_foot_rear_right, - }, - } - } -} - -#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)] -#[repr(C)] -pub struct TouchSensors { - chest_button: bool, - head_front: bool, - head_middle: bool, - head_rear: bool, - left_foot_left: bool, - left_foot_right: bool, - left_hand_back: bool, - left_hand_left: bool, - left_hand_right: bool, - right_foot_left: bool, - right_foot_right: bool, - right_hand_back: bool, - right_hand_left: bool, - right_hand_right: bool, -} - -impl From for types::sensor_data::TouchSensors { - fn from(from: TouchSensors) -> Self { - types::sensor_data::TouchSensors { - chest_button: from.chest_button, - head_front: from.head_front, - head_middle: from.head_middle, - head_rear: from.head_rear, - left_foot_left: from.left_foot_left, - left_foot_right: from.left_foot_right, - left_hand_back: from.left_hand_back, - left_hand_left: from.left_hand_left, - left_hand_right: from.left_hand_right, - right_foot_left: from.right_foot_left, - right_foot_right: from.right_foot_right, - right_hand_back: from.right_hand_back, - right_hand_left: from.right_hand_left, - right_hand_right: from.right_hand_right, - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct SonarSensors { - pub left: f32, - pub right: f32, -} - -impl From for types::sensor_data::SonarSensors { - fn from(from: SonarSensors) -> Self { - types::sensor_data::SonarSensors { - left: from.left, - right: from.right, - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct JointsArray { - pub head_yaw: f32, - pub head_pitch: f32, - pub left_shoulder_pitch: f32, - pub left_shoulder_roll: f32, - pub left_elbow_yaw: f32, - pub left_elbow_roll: f32, - pub left_wrist_yaw: f32, - pub left_hip_yaw_pitch: f32, - pub left_hip_roll: f32, - pub left_hip_pitch: f32, - pub left_knee_pitch: f32, - pub left_ankle_pitch: f32, - pub left_ankle_roll: f32, - // pub right_hip_yaw_pitch: f32, TODO - pub right_hip_roll: f32, - pub right_hip_pitch: f32, - pub right_knee_pitch: f32, - pub right_ankle_pitch: f32, - pub right_ankle_roll: f32, - pub right_shoulder_pitch: f32, - pub right_shoulder_roll: f32, - pub right_elbow_yaw: f32, - pub right_elbow_roll: f32, - pub right_wrist_yaw: f32, - pub left_hand: f32, - pub right_hand: f32, -} - -impl From> for JointsArray { - fn from(joints: Joints) -> Self { - Self { - head_yaw: joints.head.yaw, - head_pitch: joints.head.pitch, - left_shoulder_pitch: joints.left_arm.shoulder_pitch, - left_shoulder_roll: joints.left_arm.shoulder_roll, - left_elbow_yaw: joints.left_arm.elbow_yaw, - left_elbow_roll: joints.left_arm.elbow_roll, - left_wrist_yaw: joints.left_arm.wrist_yaw, - left_hip_yaw_pitch: joints.left_leg.hip_yaw_pitch, - left_hip_roll: joints.left_leg.hip_roll, - left_hip_pitch: joints.left_leg.hip_pitch, - left_knee_pitch: joints.left_leg.knee_pitch, - left_ankle_pitch: joints.left_leg.ankle_pitch, - left_ankle_roll: joints.left_leg.ankle_roll, - right_hip_roll: joints.right_leg.hip_roll, - right_hip_pitch: joints.right_leg.hip_pitch, - right_knee_pitch: joints.right_leg.knee_pitch, - right_ankle_pitch: joints.right_leg.ankle_pitch, - right_ankle_roll: joints.right_leg.ankle_roll, - right_shoulder_pitch: joints.right_arm.shoulder_pitch, - right_shoulder_roll: joints.right_arm.shoulder_roll, - right_elbow_yaw: joints.right_arm.elbow_yaw, - right_elbow_roll: joints.right_arm.elbow_roll, - right_wrist_yaw: joints.right_arm.wrist_yaw, - left_hand: joints.left_arm.hand, - right_hand: joints.right_arm.hand, - } - } -} - -impl From for Joints { - fn from(joints: JointsArray) -> Self { - Joints { - head: HeadJoints { - yaw: joints.head_yaw, - pitch: joints.head_pitch, - }, - left_arm: ArmJoints { - shoulder_pitch: joints.left_shoulder_pitch, - shoulder_roll: joints.left_shoulder_roll, - elbow_yaw: joints.left_elbow_yaw, - elbow_roll: joints.left_elbow_roll, - wrist_yaw: joints.left_wrist_yaw, - hand: joints.left_hand, - }, - right_arm: ArmJoints { - shoulder_pitch: joints.right_shoulder_pitch, - shoulder_roll: joints.right_shoulder_roll, - elbow_yaw: joints.right_elbow_yaw, - elbow_roll: joints.right_elbow_roll, - wrist_yaw: joints.right_wrist_yaw, - hand: joints.right_hand, - }, - left_leg: LegJoints { - hip_yaw_pitch: joints.left_hip_yaw_pitch, - hip_roll: joints.left_hip_roll, - hip_pitch: joints.left_hip_pitch, - knee_pitch: joints.left_knee_pitch, - ankle_pitch: joints.left_ankle_pitch, - ankle_roll: joints.left_ankle_roll, - }, - right_leg: LegJoints { - hip_yaw_pitch: joints.left_hip_yaw_pitch, //TODO - hip_roll: joints.right_hip_roll, - hip_pitch: joints.right_hip_pitch, - knee_pitch: joints.right_knee_pitch, - ankle_pitch: joints.right_ankle_pitch, - ankle_roll: joints.right_ankle_roll, - }, - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct StateStorage { - /// Seconds since proxy start - pub received_at: f32, - pub robot_configuration: RobotConfiguration, - pub battery: Battery, - pub inertial_measurement_unit: InertialMeasurementUnit, - pub force_sensitive_resistors: ForceSensitiveResistors, - pub touch_sensors: TouchSensors, - pub sonar_sensors: SonarSensors, - pub position: JointsArray, - pub stiffness: JointsArray, - pub currents: JointsArray, - pub temperature: JointsArray, - pub status: JointsArray, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct Color { - pub red: f32, - pub green: f32, - pub blue: f32, -} - -impl From for Color { - fn from(color: types::color::Rgb) -> Self { - Self { - red: color.red as f32 / 255.0, - green: color.green as f32 / 255.0, - blue: color.blue as f32 / 255.0, - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct Eye { - pub color_at_0: Color, - pub color_at_45: Color, - pub color_at_90: Color, - pub color_at_135: Color, - pub color_at_180: Color, - pub color_at_225: Color, - pub color_at_270: Color, - pub color_at_315: Color, -} - -impl From for Eye { - fn from(eye: types::led::Eye) -> Self { - Self { - color_at_0: eye.color_at_0.into(), - color_at_45: eye.color_at_45.into(), - color_at_90: eye.color_at_90.into(), - color_at_135: eye.color_at_135.into(), - color_at_180: eye.color_at_180.into(), - color_at_225: eye.color_at_225.into(), - color_at_270: eye.color_at_270.into(), - color_at_315: eye.color_at_315.into(), - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct Ear { - pub intensity_at_0: f32, - pub intensity_at_36: f32, - pub intensity_at_72: f32, - pub intensity_at_108: f32, - pub intensity_at_144: f32, - pub intensity_at_180: f32, - pub intensity_at_216: f32, - pub intensity_at_252: f32, - pub intensity_at_288: f32, - pub intensity_at_324: f32, -} - -impl From for Ear { - fn from(ear: types::led::Ear) -> Self { - Self { - intensity_at_0: ear.intensity_at_0, - intensity_at_36: ear.intensity_at_36, - intensity_at_72: ear.intensity_at_72, - intensity_at_108: ear.intensity_at_108, - intensity_at_144: ear.intensity_at_144, - intensity_at_180: ear.intensity_at_180, - intensity_at_216: ear.intensity_at_216, - intensity_at_252: ear.intensity_at_252, - intensity_at_288: ear.intensity_at_288, - intensity_at_324: ear.intensity_at_324, - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -#[repr(C)] -pub struct ControlStorage { - pub left_eye: Eye, - pub right_eye: Eye, - pub chest: Color, - pub left_foot: Color, - pub right_foot: Color, - pub left_ear: Ear, - pub right_ear: Ear, - pub position: JointsArray, - pub stiffness: JointsArray, -} - -pub fn read_from_hula( - reader: &mut DoubleBufferedReader, -) -> Result { - Ok(*reader - .draining_read() - .wrap_err("failed to drain from stream")?) -} - -pub fn write_to_hula(stream: &mut UnixStream, control_storage: ControlStorage) -> Result<()> { - let control_storage_buffer = unsafe { - from_raw_parts( - &control_storage as *const ControlStorage as *const u8, - size_of::(), - ) - }; - stream.write_all(control_storage_buffer)?; - stream.flush()?; - Ok(()) -} diff --git a/crates/hulk_nao/src/hula_wrapper.rs b/crates/hulk_nao/src/hula_wrapper.rs deleted file mode 100644 index 4c863a031d..0000000000 --- a/crates/hulk_nao/src/hula_wrapper.rs +++ /dev/null @@ -1,117 +0,0 @@ -use std::{ - os::unix::net::UnixStream, - str::from_utf8, - time::{Duration, SystemTime, UNIX_EPOCH}, -}; - -use color_eyre::{eyre::WrapErr, Result}; -use parking_lot::{Mutex, RwLock}; - -use hula_types::hardware::Ids; -use types::{joints::Joints, led::Leds, sensor_data::SensorData}; - -use super::{ - double_buffered_reader::{DoubleBufferedReader, SelectPoller}, - hula::{read_from_hula, write_to_hula, ControlStorage, StateStorage}, -}; - -pub const HULA_SOCKET_PATH: &str = "/tmp/hula"; - -pub struct HulaWrapper { - now: RwLock, - ids: Ids, - stream: Mutex, - hula_reader: Mutex>, -} - -impl HulaWrapper { - pub fn new() -> Result { - let stream = - UnixStream::connect(HULA_SOCKET_PATH).wrap_err("failed to open HULA socket")?; - stream - .set_nonblocking(true) - .wrap_err("failed to set HULA socket to non-blocking mode")?; - let mut hula_reader = DoubleBufferedReader::from_reader_and_poller( - stream - .try_clone() - .wrap_err("failed to clone HULA socket for reading")?, - SelectPoller, - ); - let state_storage = - read_from_hula(&mut hula_reader).wrap_err("failed to read from HULA")?; - let ids = Ids { - body_id: from_utf8(&state_storage.robot_configuration.body_id) - .wrap_err("failed to convert body ID into UTF-8")? - .to_string(), - head_id: from_utf8(&state_storage.robot_configuration.head_id) - .wrap_err("failed to convert head ID into UTF-8")? - .to_string(), - }; - Ok(Self { - now: RwLock::new(UNIX_EPOCH), - ids, - stream: Mutex::new(stream), - hula_reader: Mutex::new(hula_reader), - }) - } - - pub fn get_now(&self) -> SystemTime { - *self.now.read() - } - - pub fn get_ids(&self) -> Ids { - self.ids.clone() - } - - pub fn read_from_hula(&self) -> Result { - let state_storage = { - let mut hula_reader = self.hula_reader.lock(); - read_from_hula(&mut hula_reader).wrap_err("failed to read from HULA")? - }; - - { - let mut now = self.now.write(); - *now = UNIX_EPOCH + Duration::from_secs_f32(state_storage.received_at); - } - - let positions = state_storage.position.into(); - let inertial_measurement_unit = state_storage.inertial_measurement_unit.into(); - let sonar_sensors = state_storage.sonar_sensors.into(); - let force_sensitive_resistors = state_storage.force_sensitive_resistors.into(); - let touch_sensors = state_storage.touch_sensors.into(); - let temperature_sensors = state_storage.temperature.into(); - let currents = state_storage.currents.into(); - - Ok(SensorData { - positions, - inertial_measurement_unit, - sonar_sensors, - force_sensitive_resistors, - touch_sensors, - temperature_sensors, - currents, - }) - } - - pub fn write_to_actuators( - &self, - positions: Joints, - stiffnesses: Joints, - leds: Leds, - ) -> Result<()> { - let control_storage = ControlStorage { - left_eye: leds.left_eye.into(), - right_eye: leds.right_eye.into(), - chest: leds.chest.into(), - left_foot: leds.left_foot.into(), - right_foot: leds.right_foot.into(), - left_ear: leds.left_ear.into(), - right_ear: leds.right_ear.into(), - position: positions.into(), - stiffness: stiffnesses.into(), - }; - - let mut stream = self.stream.lock(); - write_to_hula(&mut stream, control_storage).wrap_err("failed to write to HULA") - } -} diff --git a/crates/hulk_nao/src/main.rs b/crates/hulk_nao/src/main.rs deleted file mode 100644 index 1c95f882a1..0000000000 --- a/crates/hulk_nao/src/main.rs +++ /dev/null @@ -1,95 +0,0 @@ -#![recursion_limit = "256"] -use std::{ - fs::File, - io::stdout, - path::{Path, PathBuf}, - sync::Arc, -}; - -use clap::Parser; -use color_eyre::{ - eyre::{Result, WrapErr}, - install, -}; -use ctrlc::set_handler; -use framework::Parameters as FrameworkParameters; -use hardware::IdInterface; -use hardware_interface::{HardwareInterface, Parameters as HardwareParameters}; -use hulk::execution::run; -use serde_json::from_reader; -use tokio_util::sync::CancellationToken; - -mod audio_parameter_deserializers; -mod camera; -mod double_buffered_reader; -mod hardware_interface; -mod hula; -mod hula_wrapper; -mod microphones; -mod speakers; - -pub fn setup_logger() -> Result<(), fern::InitError> { - fern::Dispatch::new() - .format(|out, message, record| { - out.finish(format_args!( - "{} {:<18} {:>5} {}", - chrono::Local::now().format("%Y-%m-%d %H:%M:%S"), - record.target(), - record.level(), - message - )) - }) - .level(log::LevelFilter::Debug) - .chain(stdout()) - .apply()?; - Ok(()) -} - -#[derive(Parser)] -struct Arguments { - #[arg(short, long, default_value = "logs")] - log_path: PathBuf, - - #[arg(short, long, default_value = "etc/parameters/framework.json")] - framework_parameters_path: PathBuf, -} - -fn main() -> Result<()> { - setup_logger()?; - install()?; - let keep_running = CancellationToken::new(); - set_handler({ - let keep_running = keep_running.clone(); - move || { - keep_running.cancel(); - } - })?; - - let arguments = Arguments::parse(); - let framework_parameters_path = Path::new(&arguments.framework_parameters_path); - - let file = - File::open(framework_parameters_path).wrap_err("failed to open framework parameters")?; - let framework_parameters: FrameworkParameters = - from_reader(file).wrap_err("failed to parse framework parameters")?; - - let file = File::open(framework_parameters.hardware_parameters) - .wrap_err("failed to open hardware parameters")?; - let hardware_parameters: HardwareParameters = - from_reader(file).wrap_err("failed to parse hardware parameters")?; - - let hardware_interface = HardwareInterface::new(keep_running.clone(), hardware_parameters) - .wrap_err("failed to create hardware interface")?; - - let ids = hardware_interface.get_ids(); - - run( - Arc::new(hardware_interface), - framework_parameters.communication_addresses, - framework_parameters.parameters_directory, - arguments.log_path, - ids, - keep_running, - framework_parameters.recording_intervals, - ) -} diff --git a/crates/hulk_nao/src/microphones.rs b/crates/hulk_nao/src/microphones.rs deleted file mode 100644 index 30c8b13b39..0000000000 --- a/crates/hulk_nao/src/microphones.rs +++ /dev/null @@ -1,117 +0,0 @@ -use std::{sync::Arc, thread::sleep, time::Duration}; - -use alsa::{ - pcm::{Access, Format, HwParams}, - Direction, ValueOr, PCM, -}; -use color_eyre::{ - eyre::{eyre, WrapErr}, - Result, -}; -use log::warn; -use serde::Deserialize; -use types::samples::Samples; - -use crate::audio_parameter_deserializers::{deserialize_access, deserialize_format}; - -pub struct Microphones { - device: PCM, - parameters: Parameters, -} - -impl Microphones { - pub fn new(parameters: Parameters) -> Result { - let device = create_device(¶meters).wrap_err("failed to create device")?; - Ok(Self { device, parameters }) - } - - pub fn retrying_read(&mut self) -> Result { - let number_of_retries = self.parameters.number_of_retries; - for _ in 0..number_of_retries { - match self.read() { - Ok(samples) => return Ok(samples), - Err(error) => { - warn!("failed to read from microphones: {error:#?}"); - sleep(self.parameters.retry_sleep_duration); - self.device = match create_device(&self.parameters) { - Ok(device) => device, - Err(error) => { - warn!("failed to create device: {error:#?}"); - continue; - } - }; - } - } - } - Err(eyre!( - "failed to read from microphones after {number_of_retries}, giving up..." - )) - } - - fn read(&self) -> std::result::Result { - let io_device = self - .device - .io_f32() - .wrap_err("failed to create I/O device")?; - let mut interleaved_buffer = - vec![0.0; self.parameters.number_of_channels * self.parameters.number_of_samples]; - let number_of_frames = io_device - .readi(&mut interleaved_buffer) - .wrap_err("failed to read audio data")?; - let mut non_interleaved_buffer = - vec![Vec::with_capacity(number_of_frames); self.parameters.number_of_channels]; - for (channel_index, non_interleaved_buffer) in non_interleaved_buffer.iter_mut().enumerate() - { - non_interleaved_buffer.extend( - interleaved_buffer - .iter() - .skip(channel_index) - .step_by(self.parameters.number_of_channels), - ); - } - Ok(Samples { - rate: self.parameters.sample_rate, - channels_of_samples: Arc::new(non_interleaved_buffer), - }) - } -} - -fn create_device(parameters: &Parameters) -> Result { - let device = - PCM::new("default", Direction::Capture, false).wrap_err("failed to open audio device")?; - { - let hardware_parameters = - HwParams::any(&device).wrap_err("failed to create hardware parameters")?; - hardware_parameters - .set_access(parameters.access) - .wrap_err("failed to set access")?; - hardware_parameters - .set_format(parameters.format) - .wrap_err("failed to set format")?; - hardware_parameters - .set_rate_near(parameters.sample_rate, ValueOr::Nearest) - .wrap_err("failed to set sample rate")?; - hardware_parameters - .set_channels(parameters.number_of_channels as u32) - .wrap_err("failed to set channel")?; - device - .hw_params(&hardware_parameters) - .wrap_err("failed to set hardware parameters")?; - } - device.prepare().wrap_err("failed to prepare device")?; - Ok(device) -} - -#[derive(Clone, Debug, Deserialize)] -pub struct Parameters { - sample_rate: u32, - number_of_channels: usize, - number_of_samples: usize, - number_of_retries: usize, - retry_sleep_duration: Duration, - - #[serde(deserialize_with = "deserialize_access")] - access: Access, - #[serde(deserialize_with = "deserialize_format")] - format: Format, -} diff --git a/crates/hulk_nao/src/speakers.rs b/crates/hulk_nao/src/speakers.rs deleted file mode 100644 index 030a2c59c5..0000000000 --- a/crates/hulk_nao/src/speakers.rs +++ /dev/null @@ -1,155 +0,0 @@ -use std::{ - collections::HashMap, - sync::mpsc::{sync_channel, Receiver, SyncSender, TrySendError}, - thread::{spawn, JoinHandle}, - time::Duration, -}; - -use alsa::{ - pcm::{Access, Format, HwParams}, - Direction, ValueOr, PCM, -}; -use color_eyre::{eyre::WrapErr, Result}; -use enum_iterator::all; -use log::{error, warn}; -use opusfile_ng::OggOpusFile; -use serde::Deserialize; - -use hula_types::hardware::Paths; -use types::audio::{Sound, SpeakerRequest}; - -use crate::audio_parameter_deserializers::{deserialize_access, deserialize_format}; - -pub struct Speakers { - worker_sender: Option>, - worker: Option>, -} - -impl Speakers { - pub fn new(parameters: Parameters, paths: &Paths) -> Result { - let device = Self::initialize_playback_device(¶meters) - .wrap_err("failed to initialize playback device")?; - let sounds = - Self::load_sounds(paths, parameters.volume).wrap_err("failed to loads sounds")?; - let (sender, receiver) = sync_channel(5); - let worker = Some(spawn(move || worker(device, sounds, receiver))); - Ok(Self { - worker_sender: Some(sender), - worker, - }) - } - - fn initialize_playback_device(parameters: &Parameters) -> Result { - let device = PCM::new("default", Direction::Playback, false) - .wrap_err("failed to open audio device")?; - { - let hardware_parameters = - HwParams::any(&device).wrap_err("failed to create hardware parameters")?; - hardware_parameters - .set_access(parameters.access) - .wrap_err("failed to set access")?; - hardware_parameters - .set_format(parameters.format) - .wrap_err("failed to set format")?; - hardware_parameters - .set_rate_near(parameters.sample_rate, ValueOr::Nearest) - .wrap_err("failed to set sample rate")?; - hardware_parameters - .set_channels(parameters.number_of_channels as u32) - .wrap_err("failed to set channel")?; - hardware_parameters - .set_buffer_time_near( - parameters.buffer_time.as_micros().try_into().unwrap(), - ValueOr::Nearest, - ) - .wrap_err("failed to set buffer time")?; - device - .hw_params(&hardware_parameters) - .wrap_err("failed to set hardware parameters")?; - } - Ok(device) - } - - fn load_sounds(paths: &Paths, volume: f32) -> Result>> { - let mut sounds = HashMap::new(); - for sound in all::() { - let file_name = format!("{sound}.ogg"); - let path = paths.sounds.join(file_name); - let file = OggOpusFile::open_file(&path) - .wrap_err_with(|| format!("failed to open sound file {path:?}"))?; - let number_of_samples = file.pcm_total(-1).wrap_err_with(|| { - format!("failed to get number of samples of sound file {path:?}") - })?; - let mut samples = Vec::with_capacity(number_of_samples); - let mut buffer = [0.0; 2048]; - loop { - let read_bytes = file - .read_float(&mut buffer, None) - .wrap_err_with(|| format!("failed to read sample of sound file {path:?}"))?; - if read_bytes == 0 { - break; - } - for sample in &mut buffer[..read_bytes] { - *sample *= volume; - } - samples.extend(&buffer[..read_bytes]); - } - sounds.insert(sound, samples); - } - Ok(sounds) - } - - pub fn write_to_speakers(&self, request: SpeakerRequest) { - match self.worker_sender.as_ref().unwrap().try_send(request) { - Ok(_) => {} - Err(TrySendError::Full(request)) => { - warn!("speaker queue is full, dropping {request:?}"); - } - Err(TrySendError::Disconnected(_)) => { - panic!("receiver should always wait for all senders"); - } - } - } -} - -impl Drop for Speakers { - fn drop(&mut self) { - drop(self.worker_sender.take()); - if let Some(worker) = self.worker.take() { - worker.join().expect("failed to join worker"); - } - } -} - -#[derive(Clone, Debug, Deserialize)] -pub struct Parameters { - sample_rate: u32, - number_of_channels: usize, - buffer_time: Duration, - volume: f32, - - #[serde(deserialize_with = "deserialize_access")] - access: Access, - #[serde(deserialize_with = "deserialize_format")] - format: Format, -} - -fn worker(device: PCM, sounds: HashMap>, receiver: Receiver) { - while let Ok(SpeakerRequest::PlaySound { sound }) = receiver.recv() { - let samples = sounds - .get(&sound) - .expect("missing sound, recheck Sound::all()"); - let io = device - .io_f32() - .expect("f32 device should always be available"); - if let Err(error) = device.prepare() { - error!("device.prepare(): {error:?}"); - } - if let Err(error) = io.writei(samples) { - error!("device.writei(): {error:?}"); - } - if let Err(error) = device.drain() { - error!("device.drain(): {error:?}"); - } - } -} diff --git a/crates/hulk_replayer/Cargo.toml b/crates/hulk_replayer/Cargo.toml index 1bdf3de0c7..21855aa8e5 100644 --- a/crates/hulk_replayer/Cargo.toml +++ b/crates/hulk_replayer/Cargo.toml @@ -10,6 +10,7 @@ audio = { workspace = true } ball_filter = { workspace = true } bincode = { workspace = true } blake3 = { workspace = true } +booster = { workspace = true } buffered_watch = { workspace = true } calibration = { workspace = true } chrono = { workspace = true } @@ -47,8 +48,10 @@ tokio-util = { workspace = true } types = { workspace = true } vision = { workspace = true } walking_engine = { workspace = true } +zed = { workspace = true } [build-dependencies] +booster = { workspace = true } code_generation = { workspace = true } color-eyre = { workspace = true } hulk_manifest = { workspace = true } diff --git a/crates/hulk_replayer/src/main.rs b/crates/hulk_replayer/src/main.rs index c064285401..55604fbe7f 100644 --- a/crates/hulk_replayer/src/main.rs +++ b/crates/hulk_replayer/src/main.rs @@ -12,8 +12,9 @@ mod worker_thread; use color_eyre::{eyre::Result, install}; use hardware::{ - ActuatorInterface, CameraInterface, IdInterface, MicrophoneInterface, NetworkInterface, - PathsInterface, RecordingInterface, SensorInterface, SpeakerInterface, + ActuatorInterface, CameraInterface, IdInterface, LowCommandInterface, LowStateInterface, + MicrophoneInterface, NetworkInterface, PathsInterface, RGBDSensorsInterface, + RecordingInterface, SensorInterface, SpeakerInterface, }; use hula_types::hardware::{Ids, Paths}; use replayer::replayer; @@ -27,15 +28,19 @@ use types::{ sensor_data::SensorData, ycbcr422_image::YCbCr422Image, }; +use zed::RGBDSensors; pub trait HardwareInterface: ActuatorInterface + CameraInterface + IdInterface + + LowCommandInterface + + LowStateInterface + MicrophoneInterface + NetworkInterface + PathsInterface + RecordingInterface + + RGBDSensorsInterface + SensorInterface + SpeakerInterface { @@ -60,7 +65,7 @@ impl ActuatorInterface for ReplayerHardwareInterface { impl CameraInterface for ReplayerHardwareInterface { fn read_from_camera(&self, _camera_position: CameraPosition) -> Result { - panic!("Replayer cannot produce data from hardware") + unimplemented!("Replayer cannot produce data from hardware") } } @@ -70,15 +75,27 @@ impl IdInterface for ReplayerHardwareInterface { } } +impl LowCommandInterface for ReplayerHardwareInterface { + fn write_low_command(&self, _low_command: booster::LowCommand) -> Result<()> { + unimplemented!() + } +} + +impl LowStateInterface for ReplayerHardwareInterface { + fn read_low_state(&self) -> Result { + unimplemented!() + } +} + impl MicrophoneInterface for ReplayerHardwareInterface { fn read_from_microphones(&self) -> Result { - panic!("Replayer cannot produce data from hardware") + unimplemented!("Replayer cannot produce data from hardware") } } impl NetworkInterface for ReplayerHardwareInterface { fn read_from_network(&self) -> Result { - panic!("Replayer cannot produce data from hardware") + unimplemented!("Replayer cannot produce data from hardware") } fn write_to_network(&self, _message: OutgoingMessage) -> Result<()> { @@ -104,6 +121,12 @@ impl RecordingInterface for ReplayerHardwareInterface { fn set_whether_to_record(&self, _enable: bool) {} } +impl RGBDSensorsInterface for ReplayerHardwareInterface { + fn read_rgbd_sensors(&self) -> Result { + unimplemented!() + } +} + impl SensorInterface for ReplayerHardwareInterface { fn read_from_sensors(&self) -> Result { panic!("Replayer cannot produce data from hardware") diff --git a/crates/hulk_webots/Cargo.toml b/crates/hulk_webots/Cargo.toml deleted file mode 100644 index 2b70b4f63a..0000000000 --- a/crates/hulk_webots/Cargo.toml +++ /dev/null @@ -1,30 +0,0 @@ -[package] -name = "hulk_webots" -version.workspace = true -edition.workspace = true -license.workspace = true -homepage.workspace = true - -[dependencies] -ball_filter = { workspace = true } -chrono = { workspace = true } -color-eyre = { workspace = true } -ctrlc = { workspace = true } -fern = { workspace = true } -framework = { workspace = true } -hardware = { workspace = true } -hula_types = { workspace = true } -hulk = { workspace = true } -linear_algebra = { workspace = true } -log = { workspace = true } -nalgebra = { workspace = true } -ndarray = { workspace = true } -parking_lot = { workspace = true } -rand = { workspace = true } -serde = { workspace = true } -serde_json = { workspace = true } -spl_network = { workspace = true } -tokio = { workspace = true } -tokio-util = { workspace = true } -types = { workspace = true } -webots = { workspace = true } diff --git a/crates/hulk_webots/src/camera.rs b/crates/hulk_webots/src/camera.rs deleted file mode 100644 index 8bf751feea..0000000000 --- a/crates/hulk_webots/src/camera.rs +++ /dev/null @@ -1,318 +0,0 @@ -#[cfg(target_arch = "x86_64")] -use std::arch::x86_64::{ - _mm256_add_epi16, _mm256_castsi256_si128, _mm256_insert_epi8, _mm256_loadu_si256, - _mm256_mullo_epi16, _mm256_permute4x64_epi64, _mm256_setr_epi16, _mm256_setr_epi8, - _mm256_shuffle_epi8, _mm256_sra_epi16, _mm_setr_epi8, _mm_storeu_si128, -}; - -use color_eyre::{ - eyre::{OptionExt, WrapErr}, - Result, -}; -use parking_lot::{Condvar, Mutex}; -use types::{camera_position::CameraPosition, color::YCbCr422, ycbcr422_image::YCbCr422Image}; -use webots::Robot; - -use super::hardware_interface::SIMULATION_TIME_STEP; - -pub struct Camera { - camera: webots::Camera, - buffer: Mutex>>, - buffer_updated: Condvar, -} - -impl Camera { - pub fn new(position: CameraPosition) -> Self { - let camera = Robot::get_camera(match position { - CameraPosition::Top => "CameraTop", - CameraPosition::Bottom => "CameraBottom", - }); - camera.enable(SIMULATION_TIME_STEP); - - Self { - camera, - buffer: Mutex::new(None), - buffer_updated: Condvar::new(), - } - } - - pub fn update_image(&self) -> Result<()> { - let image_data_input = match self - .camera - .get_image() - .wrap_err("failed to get image from camera") - { - Ok(image_data_input) => image_data_input, - Err(error) => { - self.unblock_read(); - return Err(error); - } - }; - - { - let mut bgra_buffer = self.buffer.lock(); - *bgra_buffer = Some(image_data_input.to_vec()); - } - self.buffer_updated.notify_all(); - - Ok(()) - } - - pub fn unblock_read(&self) { - self.buffer_updated.notify_all(); - } - - pub fn read(&self) -> Result { - let bgra_buffer = { - let mut bgra_buffer = self.buffer.lock(); - self.buffer_updated.wait(&mut bgra_buffer); - bgra_buffer.clone().ok_or_eyre("no updated image found")? - }; - assert_eq!(bgra_buffer.len(), 4 * 640 * 480); - let mut ycbcr_buffer = vec![ - YCbCr422 { - y1: 0, - cb: 0, - y2: 0, - cr: 0 - }; - 320 * 480 - ]; - bgra_444_to_ycbcr_422(&bgra_buffer, &mut ycbcr_buffer); - Ok(YCbCr422Image::from_ycbcr_buffer(320, 480, ycbcr_buffer)) - } -} - -fn bgra_444_to_ycbcr_422(bgra_444: &[u8], ycbcr_422: &mut [YCbCr422]) { - assert_eq!(bgra_444.len() % 32, 0); - assert_eq!(8 * ycbcr_422.len(), bgra_444.len()); - - #[cfg(target_arch = "x86_64")] - if is_x86_feature_detected!("avx2") { - // Conversion factors from https://de.wikipedia.org/wiki/YCbCr-Farbmodell#Umrechnung_zwischen_RGB_und_YCbCr - // Consider two 444 BGRA pixels: [ B0 G0 R0 A0 ] [ B1 G1 R1 A1 ] - // Then the single 422 YCbCr pixel [ Y0 Cb Y1 Cr ] is calculated via: - // - // Y0 = 0.299 * R0 + 0.587 * G0 + 0.114 * B0 - // Cb = 128.0 - 0.168736 * R0 - 0.331264 * G0 + 0.5 * B0 - // Y1 = 0.299 * R1 + 0.587 * G1 + 0.114 * B1 - // Cr = 128.0 + 0.5 * R0 - 0.418688 * G0 - 0.081312 * B0 - // - // The vectorized implementation uses two simplifications: - // 1. Integer calculus for performance (all values are multiplied by 128) - // 2. Cb and Cr are calculated from the first BGRA pixel instead of both - // - // This leads to the modified formulas: - // - // Y0 = 38 * R0 + 75 * G0 + 15 * B0 - // Cb = 16384 - 22 * R0 - 42 * G0 + 64 * B0 - // Y1 = 38 * R1 + 75 * G1 + 15 * B1 - // Cr = 16384 + 64 * R0 - 54 * G0 - 10 * B0 - // ^ ^ ^ ^ ^ ^ ^ - // offset | | | | | blue_values - // red_factors | | | blue_factors - // red_values | green_values - // green_factors - // - // The multiplication by 128 transforms the color range from 0 - 255 to 0 - 32640. - // The implementation therefore uses i16 which range from -32768 - 32767. - // - // 256 bit AVX2 vector registers allow to process - // 32 BGRA color components = 8 BGRA 444 pixels = 4 resulting YCbCr 422 pixels - // The vector is therefore split into 4 identical quarters that calculate the same formulas on different input slices (SIMD). - // This means many vectors in the calculation repeat themselves 4 times. - // - // The output will be a 128 bit AVX vector with packed YCbCr 422 color components: - // - // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 - // --------------------------------------------------------------- - // Y0 Cb0 Y1 Cr0 Y2 Cb2 Y3 Cr2 Y4 Cb4 Y5 Cr4 Y6 Cb6 Y7 Cr6 - // - // Starting from the beginning, for calculating a whole 256 bit vector we need 4 identical quarters to form the factor vectors: - // - // 0 1 2 3 4 5 6 7 ... - // ----------------------------------- - // offset: 0 16384 0 16384 ... - // red_factors: 38 -22 38 64 ... - // green_factors: 75 -42 75 -54 ... - // blue_factors: 15 64 15 -10 ... - // - // These are already of type i16. We also need the color components correctly aligned to match the formulas above: - // - // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 ... - // ------------------------------------------------------------------- - // red_values: R0 0 R0 0 R1 0 R0 0 R2 0 R2 0 R3 0 R2 0 ... - // green_values: G0 0 G0 0 G1 0 G0 0 G2 0 G2 0 G3 0 G2 0 ... - // blue_values: B0 0 B0 0 B1 0 B0 0 B2 0 B2 0 B3 0 B2 0 ... - // - // To achieve this we're using some tricks with shuffling. _mm256_shuffle_epi8 allows to permute the i8 items of a 256 bit vector. - // It takes as input the original vector and an index vector specifying which original items to pick for the resulting vector. - // - // Example: Consider an input vector of [7 6 5 4 3 2 1 0] and an index vector of [0 2 4 6 0 2 4 6], - // then the resulting vector will be [7 5 3 1 7 5 3 1]. - // - // The input vector will be the original BGRA 444 pixel data. We are only interested in BGR and not the alpha channel. The alpha - // channel is therefore used as zero provider by overwriting the fourth (index = 3) byte of the input vector to zero. - // The input vector is: - // - // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 ... - // ------------------------------------------------------------------- - // B0 G0 R0 0 B1 G1 R1 A1 B2 G2 R2 A2 B3 G3 R3 A3 ... - // - // We can use the shuffle operation to construct the vectors red_values, green_values, and blue_values by rearranging the items. - // At the zero places the input index 3 is used to create a zero. The resulting vector can be interpreted as i16 vector. - // - // The next instructions are straight-forward multiplications and additions to achieve the resulting sum of weighted color channels. - // Since the calculations used the whole range of i16 because of the multiplication by 128, the last step is to divide by 128 again. - // This is done by shifting the resulting vector 7 bits to the right. - // - // The shuffle operation can only shuffle within both 128 bit halves and cannot shuffle across halves. To convert the sparse i16 to - // packed u8 again, we pack all items within the two halves (with shuffle) and then permute the 64 bit of packed data into 128 bit. - // The 256 bit containing packed 128 bit of data is then truncated and written to the output data slice. - - unsafe { - let offset = _mm256_setr_epi16( - 0, 16384, 0, 16384, 0, 16384, 0, 16384, 0, 16384, 0, 16384, 0, 16384, 0, 16384, - ); - let red_factors = _mm256_setr_epi16( - 38, -22, 38, 64, 38, -22, 38, 64, 38, -22, 38, 64, 38, -22, 38, 64, - ); - let green_factors = _mm256_setr_epi16( - 75, -42, 75, -54, 75, -42, 75, -54, 75, -42, 75, -54, 75, -42, 75, -54, - ); - let blue_factors = _mm256_setr_epi16( - 15, 64, 15, -10, 15, 64, 15, -10, 15, 64, 15, -10, 15, 64, 15, -10, - ); - let red_value_indices = _mm256_setr_epi8( - 2, 3, 2, 3, 6, 3, 2, 3, 10, 3, 10, 3, 14, 3, 10, 3, 18, 3, 18, 3, 22, 3, 18, 3, 26, - 3, 26, 3, 30, 3, 26, 3, - ); - let green_value_indices = _mm256_setr_epi8( - 1, 3, 1, 3, 5, 3, 1, 3, 9, 3, 9, 3, 13, 3, 9, 3, 17, 3, 17, 3, 21, 3, 17, 3, 25, 3, - 25, 3, 29, 3, 25, 3, - ); - let blue_value_indices = _mm256_setr_epi8( - 0, 3, 0, 3, 4, 3, 0, 3, 8, 3, 8, 3, 12, 3, 8, 3, 16, 3, 16, 3, 20, 3, 16, 3, 24, 3, - 24, 3, 28, 3, 24, 3, - ); - let shift_counts = _mm_setr_epi8(7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); - let result_indices = _mm256_setr_epi8( - 0, 2, 4, 6, 8, 10, 12, 14, 0, 0, 0, 0, 0, 0, 0, 0, 16, 18, 20, 22, 24, 26, 28, 30, - 0, 0, 0, 0, 0, 0, 0, 0, - ); - const RESULT_PERMUTATION: i32 = 0b00_00_10_00; - - for (bgra_444, ycbcr_422) in - bgra_444.chunks_exact(32).zip(ycbcr_422.chunks_exact_mut(4)) - { - let bgra_444 = _mm256_loadu_si256(bgra_444.as_ptr() as *const _); - let bgra_444 = _mm256_insert_epi8(bgra_444, 0, 3); - - let red_values = _mm256_shuffle_epi8(bgra_444, red_value_indices); - let green_values = _mm256_shuffle_epi8(bgra_444, green_value_indices); - let blue_values = _mm256_shuffle_epi8(bgra_444, blue_value_indices); - - let red_summand = _mm256_mullo_epi16(red_factors, red_values); - let green_summand = _mm256_mullo_epi16(green_factors, green_values); - let blue_summand = _mm256_mullo_epi16(blue_factors, blue_values); - - let sum = _mm256_add_epi16(offset, red_summand); - let sum = _mm256_add_epi16(sum, green_summand); - let sum = _mm256_add_epi16(sum, blue_summand); - - let result = _mm256_sra_epi16(sum, shift_counts); - - let result = _mm256_shuffle_epi8(result, result_indices); - let result = _mm256_permute4x64_epi64(result, RESULT_PERMUTATION); - let result = _mm256_castsi256_si128(result); - - _mm_storeu_si128(ycbcr_422.as_ptr() as *mut _, result); - } - } - return; - } - - bgra_444_to_ycbcr_422_fallback(bgra_444, ycbcr_422); -} - -fn bgra_444_to_ycbcr_422_fallback(bgra_444: &[u8], ycbcr_422: &mut [YCbCr422]) { - assert_eq!(bgra_444.len() % 32, 0); - assert_eq!(8 * ycbcr_422.len(), bgra_444.len()); - - for (bgra_444, ycbcr_422) in bgra_444.chunks_exact(8).zip(ycbcr_422.iter_mut()) { - let first_blue = bgra_444[0] as i16; - let first_green = bgra_444[1] as i16; - let first_red = bgra_444[2] as i16; - let second_blue = bgra_444[4] as i16; - let second_green = bgra_444[5] as i16; - let second_red = bgra_444[6] as i16; - - let first_luminance = 38 * first_red + 75 * first_green + 15 * first_blue; - let chromaticity_blue = 16384 - 22 * first_red - 42 * first_green + 64 * first_blue; - let second_luminance = 38 * second_red + 75 * second_green + 15 * second_blue; - let chromaticity_red = 16384 + 64 * first_red - 54 * first_green - 10 * first_blue; - - ycbcr_422.y1 = (first_luminance / 128) as u8; - ycbcr_422.cb = (chromaticity_blue / 128) as u8; - ycbcr_422.y2 = (second_luminance / 128) as u8; - ycbcr_422.cr = (chromaticity_red / 128) as u8; - } -} - -#[cfg(test)] -mod tests { - use std::fmt; - - use rand::{prelude::StdRng, Rng, SeedableRng}; - - use super::*; - - const SEED: u64 = 42; - - fn assert_slice_eq(left: &[T], right: &[T]) - where - T: fmt::Debug + PartialEq, - { - assert_eq!(left.len(), right.len()); - for index in 0..left.len() { - assert_eq!(left[index], right[index], "left[{index}] != right[{index}]",); - } - } - - #[test] - fn zero_image() { - let bgra_444 = [0; 32]; - let mut ycbcr_422 = [Default::default(); 4]; - bgra_444_to_ycbcr_422(&bgra_444, &mut ycbcr_422); - let mut manual_ycbcr_422 = [Default::default(); 4]; - bgra_444_to_ycbcr_422_fallback(&bgra_444, &mut manual_ycbcr_422); - assert_slice_eq(&ycbcr_422, &manual_ycbcr_422); - } - - #[test] - fn same_value_images() { - for value in 0..=255 { - let bgra_444 = [value; 32]; - let mut ycbcr_422 = [Default::default(); 4]; - bgra_444_to_ycbcr_422(&bgra_444, &mut ycbcr_422); - let mut manual_ycbcr_422 = [Default::default(); 4]; - bgra_444_to_ycbcr_422_fallback(&bgra_444, &mut manual_ycbcr_422); - assert_slice_eq(&ycbcr_422, &manual_ycbcr_422); - } - } - - #[test] - fn random_value_images() { - let mut random_number_generator = StdRng::seed_from_u64(SEED); - for _ in 0..1000 { - let bgra_444 = (0..32) - .map(|_| random_number_generator.random_range(0..=255)) - .collect::>(); - let mut ycbcr_422 = [Default::default(); 4]; - bgra_444_to_ycbcr_422(&bgra_444, &mut ycbcr_422); - let mut manual_ycbcr_422 = [Default::default(); 4]; - bgra_444_to_ycbcr_422_fallback(&bgra_444, &mut manual_ycbcr_422); - assert_slice_eq(&ycbcr_422, &manual_ycbcr_422); - } - } -} diff --git a/crates/hulk_webots/src/force_sensitive_resistor_devices.rs b/crates/hulk_webots/src/force_sensitive_resistor_devices.rs deleted file mode 100644 index 396bfadba3..0000000000 --- a/crates/hulk_webots/src/force_sensitive_resistor_devices.rs +++ /dev/null @@ -1,106 +0,0 @@ -use color_eyre::{eyre::WrapErr, Result}; -use types::sensor_data::{Foot, ForceSensitiveResistors}; -use webots::{Robot, TouchSensor}; - -use super::hardware_interface::SIMULATION_TIME_STEP; - -pub struct ForceSensitiveResistorDevices { - left_foot_front_left: TouchSensor, - left_foot_front_right: TouchSensor, - left_foot_rear_left: TouchSensor, - left_foot_rear_right: TouchSensor, - right_foot_front_left: TouchSensor, - right_foot_front_right: TouchSensor, - right_foot_rear_left: TouchSensor, - right_foot_rear_right: TouchSensor, -} - -impl Default for ForceSensitiveResistorDevices { - fn default() -> Self { - let left_foot_front_left = Robot::get_touch_sensor("LFoot/FSR/FrontLeft"); - left_foot_front_left.enable(SIMULATION_TIME_STEP); - - let left_foot_front_right = Robot::get_touch_sensor("LFoot/FSR/RearLeft"); - left_foot_front_right.enable(SIMULATION_TIME_STEP); - - let left_foot_rear_left = Robot::get_touch_sensor("LFoot/FSR/FrontRight"); - left_foot_rear_left.enable(SIMULATION_TIME_STEP); - - let left_foot_rear_right = Robot::get_touch_sensor("LFoot/FSR/RearRight"); - left_foot_rear_right.enable(SIMULATION_TIME_STEP); - - let right_foot_front_left = Robot::get_touch_sensor("RFoot/FSR/FrontLeft"); - right_foot_front_left.enable(SIMULATION_TIME_STEP); - - let right_foot_front_right = Robot::get_touch_sensor("RFoot/FSR/RearLeft"); - right_foot_front_right.enable(SIMULATION_TIME_STEP); - - let right_foot_rear_left = Robot::get_touch_sensor("RFoot/FSR/FrontRight"); - right_foot_rear_left.enable(SIMULATION_TIME_STEP); - - let right_foot_rear_right = Robot::get_touch_sensor("RFoot/FSR/RearRight"); - right_foot_rear_right.enable(SIMULATION_TIME_STEP); - - Self { - left_foot_front_left, - left_foot_front_right, - left_foot_rear_left, - left_foot_rear_right, - right_foot_front_left, - right_foot_front_right, - right_foot_rear_left, - right_foot_rear_right, - } - } -} - -impl ForceSensitiveResistorDevices { - pub fn get_values(&self) -> Result { - let left_foot_front_left_values = self - .left_foot_front_left - .get_values() - .wrap_err("failed to get front left force sensitive resistor of left foot")?; - let left_foot_front_right_values = self - .left_foot_front_right - .get_values() - .wrap_err("failed to get front right force sensitive resistor of left foot")?; - let left_foot_rear_left_values = self - .left_foot_rear_left - .get_values() - .wrap_err("failed to get rear left force sensitive resistor of left foot")?; - let left_foot_rear_right_values = self - .left_foot_rear_right - .get_values() - .wrap_err("failed to get rear right force sensitive resistor of left foot")?; - let right_foot_front_left_values = self - .right_foot_front_left - .get_values() - .wrap_err("failed to get front left force sensitive resistor of right foot")?; - let right_foot_front_right_values = self - .right_foot_front_right - .get_values() - .wrap_err("failed to get front right force sensitive resistor of right foot")?; - let right_foot_rear_left_values = self - .right_foot_rear_left - .get_values() - .wrap_err("failed to get rear left force sensitive resistor of right foot")?; - let right_foot_rear_right_values = self - .right_foot_rear_right - .get_values() - .wrap_err("failed to get rear right force sensitive resistor of right foot")?; - Ok(ForceSensitiveResistors { - left: Foot { - front_left: left_foot_front_left_values[2] as f32, - front_right: left_foot_front_right_values[2] as f32, - rear_left: left_foot_rear_left_values[2] as f32, - rear_right: left_foot_rear_right_values[2] as f32, - }, - right: Foot { - front_left: right_foot_front_left_values[2] as f32, - front_right: right_foot_front_right_values[2] as f32, - rear_left: right_foot_rear_left_values[2] as f32, - rear_right: right_foot_rear_right_values[2] as f32, - }, - }) - } -} diff --git a/crates/hulk_webots/src/hardware_interface.rs b/crates/hulk_webots/src/hardware_interface.rs deleted file mode 100644 index e8c8e6ac70..0000000000 --- a/crates/hulk_webots/src/hardware_interface.rs +++ /dev/null @@ -1,420 +0,0 @@ -use std::{ - str::from_utf8, - sync::{ - atomic::{AtomicBool, Ordering}, - Arc, Barrier, - }, - time::{Duration, SystemTime, UNIX_EPOCH}, -}; - -use color_eyre::{ - eyre::{bail, eyre, Error, WrapErr}, - Result, -}; -use serde::Deserialize; -use tokio_util::sync::CancellationToken; -use webots::Robot; - -use hardware::{ - ActuatorInterface, CameraInterface, IdInterface, MicrophoneInterface, NetworkInterface, - PathsInterface, RecordingInterface, SensorInterface, SpeakerInterface, TimeInterface, -}; -use hula_types::hardware::{Ids, Paths}; -use spl_network::endpoint::{Endpoint, Ports}; -use tokio::{ - runtime::{Builder, Runtime}, - select, -}; -use types::{ - audio::SpeakerRequest, - camera_position::CameraPosition, - joints::Joints, - led::Leds, - messages::{IncomingMessage, OutgoingMessage}, - samples::Samples, - sensor_data::SensorData, - ycbcr422_image::YCbCr422Image, -}; - -use super::{ - camera::Camera, force_sensitive_resistor_devices::ForceSensitiveResistorDevices, - intertial_measurement_unit_devices::InertialMeasurementUnitDevices, - joint_devices::JointDevices, keyboard_device::KeyboardDevice, - sonar_sensor_devices::SonarSensorDevices, -}; - -pub const SIMULATION_TIME_STEP: i32 = 10; - -#[derive(Clone, Debug, Deserialize)] -pub struct Parameters { - pub paths: Paths, - pub spl_network_ports: Ports, -} - -pub struct HardwareInterface { - _robot: Robot, - - inertial_measurement_unit: InertialMeasurementUnitDevices, - sonar_sensors: SonarSensorDevices, - force_sensitive_resistors: ForceSensitiveResistorDevices, - joints: JointDevices, - keyboard: KeyboardDevice, - top_camera: Camera, - bottom_camera: Camera, - top_camera_requested: AtomicBool, - bottom_camera_requested: AtomicBool, - paths: Paths, - spl_network_endpoint: Endpoint, - async_runtime: Runtime, - enable_recording: AtomicBool, - keep_running: CancellationToken, - simulator_audio_synchronization: Barrier, -} - -impl HardwareInterface { - pub fn new(keep_running: CancellationToken, parameters: Parameters) -> Result { - let robot = Default::default(); - let runtime = Builder::new_current_thread() - .enable_all() - .build() - .wrap_err("failed to create tokio runtime")?; - - Ok(Self { - _robot: robot, - inertial_measurement_unit: Default::default(), - sonar_sensors: Default::default(), - force_sensitive_resistors: Default::default(), - joints: Default::default(), - keyboard: Default::default(), - top_camera: Camera::new(CameraPosition::Top), - bottom_camera: Camera::new(CameraPosition::Bottom), - top_camera_requested: AtomicBool::new(false), - bottom_camera_requested: AtomicBool::new(false), - paths: parameters.paths, - spl_network_endpoint: runtime - .block_on(Endpoint::new(parameters.spl_network_ports)) - .wrap_err("failed to initialize SPL network")?, - async_runtime: runtime, - enable_recording: AtomicBool::new(false), - keep_running, - simulator_audio_synchronization: Barrier::new(2), - }) - } - - fn step_simulation(&self) -> Result<()> { - if Robot::step(SIMULATION_TIME_STEP) == -1 { - // initiate tear down very early - self.keep_running.cancel(); - bail!("termination requested"); - } - Ok(()) - } - - fn update_cameras(&self) -> Result<()> { - if self - .top_camera_requested - .compare_exchange_weak(true, false, Ordering::SeqCst, Ordering::SeqCst) - .is_ok() - { - self.top_camera - .update_image() - .wrap_err("failed to update top camera image")?; - } - - if self - .bottom_camera_requested - .compare_exchange_weak(true, false, Ordering::SeqCst, Ordering::SeqCst) - .is_ok() - { - self.bottom_camera - .update_image() - .wrap_err("failed to update bottom camera image")?; - } - - Ok(()) - } -} - -impl ActuatorInterface for HardwareInterface { - fn write_to_actuators( - &self, - positions: Joints, - _stiffnesses: Joints, - _leds: Leds, - ) -> Result<()> { - self.joints - .head - .yaw - .motor - .set_position(positions.head.yaw.into()); - self.joints - .head - .pitch - .motor - .set_position(positions.head.pitch.into()); - self.joints - .left_arm - .shoulder_pitch - .motor - .set_position(positions.left_arm.shoulder_pitch.into()); - self.joints - .left_arm - .shoulder_roll - .motor - .set_position(positions.left_arm.shoulder_roll.into()); - self.joints - .left_arm - .elbow_yaw - .motor - .set_position(positions.left_arm.elbow_yaw.into()); - self.joints - .left_arm - .elbow_roll - .motor - .set_position(positions.left_arm.elbow_roll.into()); - self.joints - .left_arm - .wrist_yaw - .motor - .set_position(positions.left_arm.wrist_yaw.into()); - self.joints - .left_leg - .hip_yaw_pitch - .motor - .set_position(positions.left_leg.hip_yaw_pitch.into()); - self.joints - .left_leg - .hip_roll - .motor - .set_position(positions.left_leg.hip_roll.into()); - self.joints - .left_leg - .hip_pitch - .motor - .set_position(positions.left_leg.hip_pitch.into()); - self.joints - .left_leg - .knee_pitch - .motor - .set_position(positions.left_leg.knee_pitch.into()); - self.joints - .left_leg - .ankle_pitch - .motor - .set_position(positions.left_leg.ankle_pitch.into()); - self.joints - .left_leg - .ankle_roll - .motor - .set_position(positions.left_leg.ankle_roll.into()); - self.joints - .right_leg - .hip_yaw_pitch - .motor - .set_position(positions.right_leg.hip_yaw_pitch.into()); - self.joints - .right_leg - .hip_roll - .motor - .set_position(positions.right_leg.hip_roll.into()); - self.joints - .right_leg - .hip_pitch - .motor - .set_position(positions.right_leg.hip_pitch.into()); - self.joints - .right_leg - .knee_pitch - .motor - .set_position(positions.right_leg.knee_pitch.into()); - self.joints - .right_leg - .ankle_pitch - .motor - .set_position(positions.right_leg.ankle_pitch.into()); - self.joints - .right_leg - .ankle_roll - .motor - .set_position(positions.right_leg.ankle_roll.into()); - self.joints - .right_arm - .shoulder_pitch - .motor - .set_position(positions.right_arm.shoulder_pitch.into()); - self.joints - .right_arm - .shoulder_roll - .motor - .set_position(positions.right_arm.shoulder_roll.into()); - self.joints - .right_arm - .elbow_yaw - .motor - .set_position(positions.right_arm.elbow_yaw.into()); - self.joints - .right_arm - .elbow_roll - .motor - .set_position(positions.right_arm.elbow_roll.into()); - self.joints - .right_arm - .wrist_yaw - .motor - .set_position(positions.right_arm.wrist_yaw.into()); - self.joints - .left_arm - .hand - .motor - .set_position(positions.left_arm.hand.into()); - self.joints - .right_arm - .hand - .motor - .set_position(positions.right_arm.hand.into()); - // Webots robot model does not have stiffnesses - // Webots robot model does not have LEDs - Ok(()) - } -} - -impl CameraInterface for HardwareInterface { - fn read_from_camera(&self, camera_position: CameraPosition) -> Result { - let result = match camera_position { - CameraPosition::Top => { - self.top_camera_requested.store(true, Ordering::SeqCst); - self.top_camera - .read() - .wrap_err("failed to read from top camera") - } - CameraPosition::Bottom => { - self.bottom_camera_requested.store(true, Ordering::SeqCst); - self.bottom_camera - .read() - .wrap_err("failed to read from bottom camera") - } - }; - if self.keep_running.is_cancelled() { - bail!("termination requested"); - } - result - } -} - -impl IdInterface for HardwareInterface { - fn get_ids(&self) -> Ids { - let name = from_utf8(Robot::get_name()).expect("robot name must be valid UTF-8"); - Ids { - body_id: name.to_string(), - head_id: name.to_string(), - } - } -} - -impl MicrophoneInterface for HardwareInterface { - fn read_from_microphones(&self) -> Result { - self.simulator_audio_synchronization.wait(); - if self.keep_running.is_cancelled() { - bail!("termination requested"); - } - Ok(Samples { - rate: 0, - channels_of_samples: Arc::new(vec![]), - }) - } -} - -impl NetworkInterface for HardwareInterface { - fn read_from_network(&self) -> Result { - self.async_runtime.block_on(async { - select! { - result = self.spl_network_endpoint.read() => { - result.map_err(Error::from) - }, - _ = self.keep_running.cancelled() => { - Err(eyre!("termination requested")) - } - } - }) - } - - fn write_to_network(&self, message: OutgoingMessage) -> Result<()> { - self.async_runtime - .block_on(self.spl_network_endpoint.write(message)); - Ok(()) - } -} - -impl PathsInterface for HardwareInterface { - fn get_paths(&self) -> Paths { - self.paths.clone() - } -} - -impl RecordingInterface for HardwareInterface { - fn should_record(&self) -> bool { - self.enable_recording.load(Ordering::SeqCst) - } - - fn set_whether_to_record(&self, enable: bool) { - self.enable_recording.store(enable, Ordering::SeqCst) - } -} - -impl SensorInterface for HardwareInterface { - fn read_from_sensors(&self) -> Result { - match self.step_simulation().wrap_err("failed to step simulation") { - Ok(_) => { - self.simulator_audio_synchronization.wait(); - } - Err(error) => { - self.simulator_audio_synchronization.wait(); - self.top_camera.unblock_read(); - self.bottom_camera.unblock_read(); - return Err(error); - } - }; - if self.keep_running.is_cancelled() { - bail!("termination requested"); - } - let positions = self.joints.get_positions(); - let inertial_measurement_unit = self - .inertial_measurement_unit - .get_values() - .wrap_err("failed to get inertial measurement unit values")?; - let sonar_sensors = self.sonar_sensors.get_values(); - let force_sensitive_resistors = self - .force_sensitive_resistors - .get_values() - .wrap_err("failed to get force sensitive resistor values")?; - let touch_sensors = self.keyboard.get_touch_sensors(); - let temperature_sensors = Joints::default(); - let currents = Joints::default(); - - self.update_cameras().wrap_err("failed to update cameras")?; - - Ok(SensorData { - positions, - inertial_measurement_unit, - sonar_sensors, - force_sensitive_resistors, - touch_sensors, - temperature_sensors, - currents, - }) - } -} - -impl SpeakerInterface for HardwareInterface { - fn write_to_speakers(&self, _request: SpeakerRequest) { - // not implemented - } -} - -impl TimeInterface for HardwareInterface { - fn get_now(&self) -> SystemTime { - UNIX_EPOCH + Duration::from_secs_f64(Robot::get_time()) - } -} - -impl hulk::HardwareInterface for HardwareInterface {} diff --git a/crates/hulk_webots/src/intertial_measurement_unit_devices.rs b/crates/hulk_webots/src/intertial_measurement_unit_devices.rs deleted file mode 100644 index fb078b0324..0000000000 --- a/crates/hulk_webots/src/intertial_measurement_unit_devices.rs +++ /dev/null @@ -1,62 +0,0 @@ -use color_eyre::{eyre::WrapErr, Result}; -use linear_algebra::vector; -use types::sensor_data::InertialMeasurementUnitData; -use webots::{Accelerometer, Gyro, InertialUnit, Robot}; - -use super::hardware_interface::SIMULATION_TIME_STEP; - -pub struct InertialMeasurementUnitDevices { - accelerometer: Accelerometer, - gyroscope: Gyro, - inertial_unit: InertialUnit, -} - -impl Default for InertialMeasurementUnitDevices { - fn default() -> Self { - let accelerometer = Robot::get_accelerometer("IMU accelerometer"); - accelerometer.enable(SIMULATION_TIME_STEP); - - let gyroscope = Robot::get_gyro("IMU gyro"); - gyroscope.enable(SIMULATION_TIME_STEP); - - let inertial_unit = Robot::get_inertial_unit("IMU inertial"); - inertial_unit.enable(SIMULATION_TIME_STEP); - - Self { - accelerometer, - gyroscope, - inertial_unit, - } - } -} - -impl InertialMeasurementUnitDevices { - pub fn get_values(&self) -> Result { - let accelerometer = self - .accelerometer - .get_values() - .wrap_err("failed to get accelerometer values")?; - let gyroscope = self - .gyroscope - .get_values() - .wrap_err("failed to get gyroscope values")?; - let inertial_unit = self - .inertial_unit - .get_roll_pitch_yaw() - .wrap_err("failed to get inertial measurement unit values")?; - - Ok(InertialMeasurementUnitData { - linear_acceleration: vector![ - accelerometer[0] as f32, - accelerometer[1] as f32, - accelerometer[2] as f32 - ], - angular_velocity: vector![ - gyroscope[0] as f32, - gyroscope[1] as f32, - gyroscope[2] as f32 - ], - roll_pitch: vector![inertial_unit[0] as f32, inertial_unit[1] as f32], - }) - } -} diff --git a/crates/hulk_webots/src/joint_devices.rs b/crates/hulk_webots/src/joint_devices.rs deleted file mode 100644 index 2dbffbffd2..0000000000 --- a/crates/hulk_webots/src/joint_devices.rs +++ /dev/null @@ -1,148 +0,0 @@ -use types::joints::{arm::ArmJoints, head::HeadJoints, leg::LegJoints, Joints}; -use webots::{Motor, PositionSensor, Robot}; - -use super::hardware_interface::SIMULATION_TIME_STEP; - -pub struct JointDevice { - pub motor: Motor, - pub position_sensor: PositionSensor, -} - -impl JointDevice { - fn new(motor_device_name: &str, position_sensor_device_name: &str) -> Self { - let motor = Robot::get_motor(motor_device_name); - - let position_sensor = Robot::get_position_sensor(position_sensor_device_name); - position_sensor.enable(SIMULATION_TIME_STEP); - - Self { - motor, - position_sensor, - } - } - - pub fn get_position(&self) -> f64 { - self.position_sensor.get_value() - } -} - -pub struct Head { - pub yaw: JointDevice, - pub pitch: JointDevice, -} - -impl Head { - pub fn get_positions(&self) -> HeadJoints { - HeadJoints { - yaw: self.yaw.get_position() as f32, - pitch: self.pitch.get_position() as f32, - } - } -} - -pub struct Arm { - pub shoulder_pitch: JointDevice, - pub shoulder_roll: JointDevice, - pub elbow_yaw: JointDevice, - pub elbow_roll: JointDevice, - pub wrist_yaw: JointDevice, - pub hand: JointDevice, -} - -impl Arm { - pub fn get_positions(&self) -> ArmJoints { - ArmJoints { - shoulder_pitch: self.shoulder_pitch.get_position() as f32, - shoulder_roll: self.shoulder_roll.get_position() as f32, - elbow_yaw: self.elbow_yaw.get_position() as f32, - elbow_roll: self.elbow_roll.get_position() as f32, - wrist_yaw: self.wrist_yaw.get_position() as f32, - hand: self.hand.get_position() as f32, - } - } -} - -pub struct Leg { - pub hip_yaw_pitch: JointDevice, - pub hip_roll: JointDevice, - pub hip_pitch: JointDevice, - pub knee_pitch: JointDevice, - pub ankle_pitch: JointDevice, - pub ankle_roll: JointDevice, -} - -impl Leg { - pub fn get_positions(&self) -> LegJoints { - LegJoints { - hip_yaw_pitch: self.hip_yaw_pitch.get_position() as f32, - hip_roll: self.hip_roll.get_position() as f32, - hip_pitch: self.hip_pitch.get_position() as f32, - knee_pitch: self.knee_pitch.get_position() as f32, - ankle_pitch: self.ankle_pitch.get_position() as f32, - ankle_roll: self.ankle_roll.get_position() as f32, - } - } -} - -pub struct JointDevices { - pub head: Head, - pub left_arm: Arm, - pub right_arm: Arm, - pub left_leg: Leg, - pub right_leg: Leg, -} - -impl JointDevices { - pub fn get_positions(&self) -> Joints { - Joints { - head: self.head.get_positions(), - left_arm: self.left_arm.get_positions(), - right_arm: self.right_arm.get_positions(), - left_leg: self.left_leg.get_positions(), - right_leg: self.right_leg.get_positions(), - } - } -} - -impl Default for JointDevices { - fn default() -> Self { - Self { - head: Head { - yaw: JointDevice::new("HeadYaw", "HeadYaw_sensor"), - pitch: JointDevice::new("HeadPitch", "HeadPitch_sensor"), - }, - left_arm: Arm { - shoulder_pitch: JointDevice::new("LShoulderPitch", "LShoulderPitch_sensor"), - shoulder_roll: JointDevice::new("LShoulderRoll", "LShoulderRoll_sensor"), - elbow_yaw: JointDevice::new("LElbowYaw", "LElbowYaw_sensor"), - elbow_roll: JointDevice::new("LElbowRoll", "LElbowRoll_sensor"), - wrist_yaw: JointDevice::new("LWristYaw", "LWristYaw_sensor"), - hand: JointDevice::new("LHand", "LHand_sensor"), - }, - right_arm: Arm { - shoulder_pitch: JointDevice::new("RShoulderPitch", "RShoulderPitch_sensor"), - shoulder_roll: JointDevice::new("RShoulderRoll", "RShoulderRoll_sensor"), - elbow_yaw: JointDevice::new("RElbowYaw", "RElbowYaw_sensor"), - elbow_roll: JointDevice::new("RElbowRoll", "RElbowRoll_sensor"), - wrist_yaw: JointDevice::new("RWristYaw", "RWristYaw_sensor"), - hand: JointDevice::new("RHand", "RHand_sensor"), - }, - left_leg: Leg { - hip_yaw_pitch: JointDevice::new("LHipYawPitch", "LHipYawPitch_sensor"), - hip_roll: JointDevice::new("LHipRoll", "LHipRoll_sensor"), - hip_pitch: JointDevice::new("LHipPitch", "LHipPitch_sensor"), - knee_pitch: JointDevice::new("LKneePitch", "LKneePitch_sensor"), - ankle_pitch: JointDevice::new("LAnklePitch", "LAnklePitch_sensor"), - ankle_roll: JointDevice::new("LAnkleRoll", "LAnkleRoll_sensor"), - }, - right_leg: Leg { - hip_yaw_pitch: JointDevice::new("RHipYawPitch", "RHipYawPitch_sensor"), - hip_roll: JointDevice::new("RHipRoll", "RHipRoll_sensor"), - hip_pitch: JointDevice::new("RHipPitch", "RHipPitch_sensor"), - knee_pitch: JointDevice::new("RKneePitch", "RKneePitch_sensor"), - ankle_pitch: JointDevice::new("RAnklePitch", "RAnklePitch_sensor"), - ankle_roll: JointDevice::new("RAnkleRoll", "RAnkleRoll_sensor"), - }, - } - } -} diff --git a/crates/hulk_webots/src/keyboard_device.rs b/crates/hulk_webots/src/keyboard_device.rs deleted file mode 100644 index 77ae69d205..0000000000 --- a/crates/hulk_webots/src/keyboard_device.rs +++ /dev/null @@ -1,62 +0,0 @@ -use types::sensor_data::TouchSensors; -use webots::{Keyboard, Receiver, Robot}; - -use super::hardware_interface::SIMULATION_TIME_STEP; - -pub struct KeyboardDevice { - keyboard: Keyboard, - receiver: Receiver, -} - -impl Default for KeyboardDevice { - fn default() -> Self { - let keyboard = Robot::get_keyboard(); - keyboard.enable(SIMULATION_TIME_STEP); - let receiver = Robot::get_receiver("ChestButton Channel"); - receiver.enable(SIMULATION_TIME_STEP); - Self { keyboard, receiver } - } -} - -impl KeyboardDevice { - pub fn get_touch_sensors(&self) -> TouchSensors { - let received_message = match self.receiver.get_next_packet() { - Ok(message) => message, - Err(error) => { - println!("error in received message: {error:?}"); - None - } - }; - - let (control_shift_c_pressed, control_shift_x_pressed, control_shift_u_pressed) = - if let Some(key) = self.keyboard.get_key() { - const CONTROL_SHIFT_MASK: u32 = Keyboard::CONTROL | Keyboard::SHIFT; - ( - key == CONTROL_SHIFT_MASK | 'C' as u32, - key == CONTROL_SHIFT_MASK | 'X' as u32, - key == CONTROL_SHIFT_MASK | 'U' as u32, - ) - } else { - (false, false, false) - }; - - TouchSensors { - chest_button: received_message.is_some() - || control_shift_c_pressed - || control_shift_x_pressed, - head_front: control_shift_u_pressed || control_shift_x_pressed, - head_middle: control_shift_u_pressed, - head_rear: control_shift_u_pressed, - left_foot_left: false, - left_foot_right: false, - left_hand_back: false, - left_hand_left: false, - left_hand_right: false, - right_foot_left: false, - right_foot_right: false, - right_hand_back: false, - right_hand_left: false, - right_hand_right: false, - } - } -} diff --git a/crates/hulk_webots/src/sonar_sensor_devices.rs b/crates/hulk_webots/src/sonar_sensor_devices.rs deleted file mode 100644 index e6b33decff..0000000000 --- a/crates/hulk_webots/src/sonar_sensor_devices.rs +++ /dev/null @@ -1,30 +0,0 @@ -use types::sensor_data::SonarSensors; -use webots::{DistanceSensor, Robot}; - -use super::hardware_interface::SIMULATION_TIME_STEP; - -pub struct SonarSensorDevices { - left: DistanceSensor, - right: DistanceSensor, -} - -impl Default for SonarSensorDevices { - fn default() -> Self { - let left = Robot::get_distance_sensor("Sonar/Left"); - left.enable(SIMULATION_TIME_STEP); - - let right = Robot::get_distance_sensor("Sonar/Right"); - right.enable(SIMULATION_TIME_STEP); - - Self { left, right } - } -} - -impl SonarSensorDevices { - pub fn get_values(&self) -> SonarSensors { - SonarSensors { - left: self.left.get_value() as f32, - right: self.right.get_value() as f32, - } - } -} diff --git a/crates/ros2/Cargo.toml b/crates/ros2/Cargo.toml new file mode 100644 index 0000000000..cc98946166 --- /dev/null +++ b/crates/ros2/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "ros2" +version.workspace = true +edition.workspace = true +license.workspace = true +homepage.workspace = true + +[dependencies] +nalgebra = { workspace = true } +serde = { workspace = true } diff --git a/crates/ros2/src/builtin_interfaces/duration.rs b/crates/ros2/src/builtin_interfaces/duration.rs new file mode 100644 index 0000000000..7cb29818b4 --- /dev/null +++ b/crates/ros2/src/builtin_interfaces/duration.rs @@ -0,0 +1,16 @@ +/// Duration defines a period between two time points. +/// Messages of this datatype are of ROS Time following this design: +/// https://design.ros2.org/articles/clock_and_time.html +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Duration { + /// The seconds component, valid over all int32 values. + pub sec: i32, + + /// The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. + /// e.g. + /// The duration -1.7 seconds is represented as {sec: -2, nanosec: 3e8} + /// The duration 1.7 seconds is represented as {sec: 1, nanosec: 7e8} + pub nanosec: u32, +} diff --git a/crates/ros2/src/builtin_interfaces/mod.rs b/crates/ros2/src/builtin_interfaces/mod.rs new file mode 100644 index 0000000000..adce43c1aa --- /dev/null +++ b/crates/ros2/src/builtin_interfaces/mod.rs @@ -0,0 +1,2 @@ +pub mod duration; +pub mod time; diff --git a/crates/ros2/src/builtin_interfaces/time.rs b/crates/ros2/src/builtin_interfaces/time.rs new file mode 100644 index 0000000000..80648fea7e --- /dev/null +++ b/crates/ros2/src/builtin_interfaces/time.rs @@ -0,0 +1,15 @@ +/// This message communicates ROS Time defined here: +/// https://design.ros2.org/articles/clock_and_time.html +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Time { + /// The seconds component, valid over all int32 values. + pub sec: i32, + + /// The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. + /// e.g. + /// The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8} + /// The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8} + pub nanosec: u32, +} diff --git a/crates/ros2/src/geometry_msgs/mod.rs b/crates/ros2/src/geometry_msgs/mod.rs new file mode 100644 index 0000000000..86fe6d6497 --- /dev/null +++ b/crates/ros2/src/geometry_msgs/mod.rs @@ -0,0 +1,5 @@ +pub mod quaternion; +pub mod transform; +pub mod transform_stamped; +pub mod vector3; +pub mod vector3_stamped; diff --git a/crates/ros2/src/geometry_msgs/quaternion.rs b/crates/ros2/src/geometry_msgs/quaternion.rs new file mode 100644 index 0000000000..feb6512263 --- /dev/null +++ b/crates/ros2/src/geometry_msgs/quaternion.rs @@ -0,0 +1,10 @@ +/// This represents an orientation in free space in quaternion form. +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Quaternion { + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, +} diff --git a/crates/ros2/src/geometry_msgs/transform.rs b/crates/ros2/src/geometry_msgs/transform.rs new file mode 100644 index 0000000000..5061c8e0cd --- /dev/null +++ b/crates/ros2/src/geometry_msgs/transform.rs @@ -0,0 +1,10 @@ +/// This represents the transform between two coordinate frames in free space. +use serde::{Deserialize, Serialize}; + +use crate::geometry_msgs::{quaternion::Quaternion, vector3::Vector3}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Transform { + pub translation: Vector3, + pub rotation: Quaternion, +} diff --git a/crates/ros2/src/geometry_msgs/transform_stamped.rs b/crates/ros2/src/geometry_msgs/transform_stamped.rs new file mode 100644 index 0000000000..96fceae3ea --- /dev/null +++ b/crates/ros2/src/geometry_msgs/transform_stamped.rs @@ -0,0 +1,25 @@ +/// This expresses a transform from coordinate frame header.frame_id +/// to the coordinate frame child_frame_id at the time of header.stamp +/// +/// This message is mostly used by the +/// tf2 package. +/// See its documentation for more information. +/// +/// The child_frame_id is necessary in addition to the frame_id +/// in the Header to communicate the full reference for the transform +/// in a self contained message. +use serde::{Deserialize, Serialize}; + +use crate::{geometry_msgs::transform::Transform, std_msgs::header::Header}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct TransformStamped { + /// The frame id in the header is used as the reference frame of this transform. + pub header: Header, + + /// The frame id of the child frame to which this transform points. + pub child_frame_id: String, + + /// Translation and rotation in 3-dimensions of child_frame_id from header.frame_id. + pub transform: Transform, +} diff --git a/crates/ros2/src/geometry_msgs/vector3.rs b/crates/ros2/src/geometry_msgs/vector3.rs new file mode 100644 index 0000000000..923e706477 --- /dev/null +++ b/crates/ros2/src/geometry_msgs/vector3.rs @@ -0,0 +1,13 @@ +/// This represents a vector in free space. +/// +/// This is semantically different than a point. +/// A vector is always anchored at the origin. +/// When a transform is applied to a vector, only the rotational component is applied. +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Vector3 { + pub x: f64, + pub y: f64, + pub z: f64, +} diff --git a/crates/ros2/src/geometry_msgs/vector3_stamped.rs b/crates/ros2/src/geometry_msgs/vector3_stamped.rs new file mode 100644 index 0000000000..dd2886eb36 --- /dev/null +++ b/crates/ros2/src/geometry_msgs/vector3_stamped.rs @@ -0,0 +1,13 @@ +/// This represents a Vector3 with reference coordinate frame and timestamp +/// +/// Note that this follows vector semantics with it always anchored at the origin, +/// so the rotational elements of a transform are the only parts applied when transforming. +use serde::{Deserialize, Serialize}; + +use crate::{geometry_msgs::vector3::Vector3, std_msgs::header::Header}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Vector3Stamped { + pub header: Header, + pub vector: Vector3, +} diff --git a/crates/ros2/src/lib.rs b/crates/ros2/src/lib.rs new file mode 100644 index 0000000000..c19502daed --- /dev/null +++ b/crates/ros2/src/lib.rs @@ -0,0 +1,4 @@ +pub mod builtin_interfaces; +pub mod geometry_msgs; +pub mod sensor_msgs; +pub mod std_msgs; diff --git a/crates/ros2/src/sensor_msgs/camera_info.rs b/crates/ros2/src/sensor_msgs/camera_info.rs new file mode 100644 index 0000000000..05b956e69c --- /dev/null +++ b/crates/ros2/src/sensor_msgs/camera_info.rs @@ -0,0 +1,141 @@ +/// This message defines meta information for a camera. It should be in a +/// camera namespace on topic "camera_info" and accompanied by up to five +/// image topics named: +/// +/// image_raw - raw data from the camera driver, possibly Bayer encoded +/// image - monochrome, distorted +/// image_color - color, distorted +/// image_rect - monochrome, rectified +/// image_rect_color - color, rectified +/// +/// The image_pipeline contains packages (image_proc, stereo_image_proc) +/// for producing the four processed image topics from image_raw and +/// camera_info. The meaning of the camera parameters are described in +/// detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. +/// +/// The image_geometry package provides a user-friendly interface to +/// common operations using this meta information. If you want to, e.g., +/// project a 3d point into image coordinates, we strongly recommend +/// using image_geometry. +/// +/// If the camera is uncalibrated, the matrices D, K, R, P should be left +/// zeroed out. In particular, clients may assume that K[0] == 0.0 +/// indicates an uncalibrated camera. +/// +/// # Calibration Parameters +/// +/// These are fixed during camera calibration. Their values will be the +/// same in all messages until the camera is recalibrated. Note that +/// self-calibrating systems may "recalibrate" frequently. +/// +/// The internal parameters can be used to warp a raw (distorted) image +/// to: +/// 1. An undistorted image (requires D and K) +/// 2. A rectified image (requires D, K, R) +/// The projection matrix P projects 3D points into the rectified image. +/// +/// The image dimensions with which the camera was calibrated. +/// Normally this will be the full camera resolution in pixels. +use serde::{Deserialize, Serialize}; + +use crate::{sensor_msgs::region_of_interest::RegionOfInterest, std_msgs::header::Header}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct CameraInfo { + /// Time of image acquisition, camera coordinate frame ID + /// Header timestamp should be acquisition time of image + /// Header frame_id should be optical frame of camera + pub header: Header, + + /// origin of frame should be optical center of camera + /// +x should point to the right in the image + /// +y should point down in the image + /// +z should point into the plane of the image + pub height: u32, + pub width: u32, + + /// The distortion model used. Supported models are listed in + /// sensor_msgs/distortion_models.hpp. For most cameras, "plumb_bob" - a + /// simple model of radial and tangential distortion - is sufficent. + pub distortion_model: String, + + /// The distortion parameters, size depending on the distortion model. + /// For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). + pub d: Vec, + + /// Intrinsic camera matrix for the raw (distorted) images. + /// [fx 0 cx] + /// K = [ 0 fy cy] + /// [ 0 0 1] + /// Projects 3D points in the camera coordinate frame to 2D pixel + /// coordinates using the focal lengths (fx, fy) and principal point + /// (cx, cy). + /// + /// 3x3 row-major matrix + pub k: [f64; 9], + + /// Rectification matrix (stereo cameras only) + /// A rotation matrix aligning the camera coordinate system to the ideal + /// stereo image plane so that epipolar lines in both stereo images are + /// parallel. + /// + /// 3x3 row-major matrix + pub r: [f64; 9], + + /// Projection/camera matrix + /// [fx' 0 cx' Tx] + /// P = [ 0 fy' cy' Ty] + /// [ 0 0 1 0] + /// By convention, this matrix specifies the intrinsic (camera) matrix + /// of the processed (rectified) image. That is, the left 3x3 portion + /// is the normal camera intrinsic matrix for the rectified image. + /// + /// It projects 3D points in the camera coordinate frame to 2D pixel + /// coordinates using the focal lengths (fx', fy') and principal point + /// (cx', cy') - these may differ from the values in K. + /// + /// For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will + /// also have R = the identity and P[1:3,1:3] = K. + /// + /// For a stereo pair, the fourth column [Tx Ty 0]' is related to the + /// position of the optical center of the second camera in the first + /// camera's frame. We assume Tz = 0 so both cameras are in the same + /// stereo image plane. The first camera always has Tx = Ty = 0. For + /// the right (second) camera of a horizontal stereo pair, Ty = 0 and + /// Tx = -fx' * B, where B is the baseline between the cameras. + /// + /// Given a 3D point [X Y Z]', the projection (x, y) of the point onto + /// the rectified image is given by: + /// [u v w]' = P * [X Y Z 1]' + /// x = u / w + /// y = v / w + /// This holds for both images of a stereo pair. + /// + /// 3x4 row-major matrix + pub p: [f64; 12], + + /// # Operational Parameters + /// + /// These define the image region actually captured by the camera + /// driver. Although they affect the geometry of the output image, they + /// may be changed freely without recalibrating the camera. + /// + /// Binning refers here to any camera setting which combines rectangular + /// neighborhoods of pixels into larger "crate-pixels." It reduces the + /// resolution of the output image to + /// (width / binning_x) x (height / binning_y). + /// + /// The default values binning_x = binning_y = 0 is considered the same + /// as binning_x = binning_y = 1 (no subsampling). + pub binning_x: u32, + pub binning_y: u32, + + /// Region of interest (subwindow of full camera resolution), given in + /// full resolution (unbinned) image coordinates. A particular ROI + /// always denotes the same window of pixels on the camera sensor, + /// regardless of binning settings. + /// + /// The default setting of roi (all values 0) is considered the same as + /// full resolution (roi.width = width, roi.height = height). + pub roi: RegionOfInterest, +} diff --git a/crates/ros2/src/sensor_msgs/image.rs b/crates/ros2/src/sensor_msgs/image.rs new file mode 100644 index 0000000000..7f4e7393dd --- /dev/null +++ b/crates/ros2/src/sensor_msgs/image.rs @@ -0,0 +1,39 @@ +/// This message contains an uncompressed image +/// (0, 0) is at top-left corner of image +use serde::{Deserialize, Serialize}; + +use crate::std_msgs::header::Header; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Image { + /// Header timestamp should be acquisition time of image + /// Header frame_id should be optical frame of camera + /// If the frame_id here and the frame_id of the CameraInfo + /// message associated with the image conflict + /// the behavior is undefined + pub header: Header, + + /// origin of frame should be optical center of cameara + /// +x should point to the right in the image + /// +y should point down in the image + /// +z should point into to plane of the image + /// + /// image height, that is, number of rows + pub height: u32, + /// image width, that is, number of columns + pub width: u32, + + /// The legal values for encoding are in file src/image_encodings.cpp + /// If you want to standardize a new string format, join + /// ros-users@lists.ros.org and send an email proposing a new encoding. + /// Encoding of pixels -- channel meaning, ordering, size + /// taken from the list of strings in include/sensor_msgs/image_encodings.hpp + pub encoding: String, + + /// is this data bigendian? + pub is_bigendian: u8, + /// Full row length in bytes + pub step: u32, + /// actual matrix data, size is (step * rows) + pub data: Vec, +} diff --git a/crates/ros2/src/sensor_msgs/imu.rs b/crates/ros2/src/sensor_msgs/imu.rs new file mode 100644 index 0000000000..7b889c3cb7 --- /dev/null +++ b/crates/ros2/src/sensor_msgs/imu.rs @@ -0,0 +1,36 @@ +/// This is a message to hold data from an IMU (Inertial Measurement Unit) +/// +/// Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec +/// +/// If the covariance of the measurement is known, it should be filled in (if all you know is the +/// variance of each measurement, e.g. from the datasheet, just put those along the diagonal) +/// A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the +/// data a covariance will have to be assumed or gotten from some other source +/// +/// If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an +/// orientation estimate), please set element 0 of the associated covariance matrix to -1 +/// If you are interpreting this message, please check for a value of -1 in the first element of each +/// covariance matrix, and disregard the associated estimate. +use serde::{Deserialize, Serialize}; + +use crate::{ + geometry_msgs::{quaternion::Quaternion, vector3::Vector3}, + std_msgs::header::Header, +}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Imu { + pub header: Header, + + pub orientation: Quaternion, + /// Row major about x, y, z axes + pub orientation_covariance: [f64; 9], + + pub angular_velocity: Vector3, + /// Row major about x, y, z axes + pub angular_velocity_covariance: [f64; 9], + + pub linear_acceleration: Vector3, + /// Row major x, y z + pub linear_acceleration_covariance: [f64; 9], +} diff --git a/crates/ros2/src/sensor_msgs/magnetic_field.rs b/crates/ros2/src/sensor_msgs/magnetic_field.rs new file mode 100644 index 0000000000..b6f02cba36 --- /dev/null +++ b/crates/ros2/src/sensor_msgs/magnetic_field.rs @@ -0,0 +1,27 @@ +/// Measurement of the Magnetic Field vector at a specific location. +/// +/// If the covariance of the measurement is known, it should be filled in. +/// If all you know is the variance of each measurement, e.g. from the datasheet, +/// just put those along the diagonal. +/// A covariance matrix of all zeros will be interpreted as "covariance unknown", +/// and to use the data a covariance will have to be assumed or gotten from some +/// other source. +use serde::{Deserialize, Serialize}; + +use crate::{geometry_msgs::vector3::Vector3, std_msgs::header::Header}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct MagneticField { + /// timestamp is the time the + /// frame_id is the location and orientation + /// of the field measurement + pub header: Header, + /// field was measured + /// x, y, and z components of the + pub magnetic_field: Vector3, + /// field vector in Tesla + /// If your sensor does not output 3 axes, + /// put NaNs in the components not reported. + /// Row major about x, y, z axes + pub magnetic_field_covariance: [f64; 9], +} diff --git a/crates/ros2/src/sensor_msgs/mod.rs b/crates/ros2/src/sensor_msgs/mod.rs new file mode 100644 index 0000000000..696dd06cfd --- /dev/null +++ b/crates/ros2/src/sensor_msgs/mod.rs @@ -0,0 +1,6 @@ +pub mod camera_info; +pub mod image; +pub mod imu; +pub mod magnetic_field; +pub mod region_of_interest; +pub mod time_reference; diff --git a/crates/ros2/src/sensor_msgs/region_of_interest.rs b/crates/ros2/src/sensor_msgs/region_of_interest.rs new file mode 100644 index 0000000000..868c042530 --- /dev/null +++ b/crates/ros2/src/sensor_msgs/region_of_interest.rs @@ -0,0 +1,27 @@ +/// This message is used to specify a region of interest within an image. +/// +/// When used to specify the ROI setting of the camera when the image was +/// taken, the height and width fields should either match the height and +/// width fields for the associated image; or height = width = 0 +/// indicates that the full resolution image was captured. +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct RegionOfInterest { + /// Leftmost pixel of the ROI + /// (0 if the ROI includes the left edge of the image) + pub x_offset: u32, + /// Topmost pixel of the ROI + pub y_offset: u32, + /// (0 if the ROI includes the top edge of the image) + /// Height of ROI + pub height: u32, + /// Width of ROI + pub width: u32, + + /// True if a distinct rectified ROI should be calculated from the "raw" + /// ROI in this message. Typically this should be False if the full image + /// is captured (ROI not used), and True if a subwindow is captured (ROI + /// used). + pub do_rectify: bool, +} diff --git a/crates/ros2/src/sensor_msgs/time_reference.rs b/crates/ros2/src/sensor_msgs/time_reference.rs new file mode 100644 index 0000000000..0f79a1b6c4 --- /dev/null +++ b/crates/ros2/src/sensor_msgs/time_reference.rs @@ -0,0 +1,15 @@ +/// Measurement from an external time source not actively synchronized with the system clock. +use serde::{Deserialize, Serialize}; + +use crate::{builtin_interfaces::time::Time, std_msgs::header::Header}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct TimeReference { + /// stamp is system time for which measurement was valid + /// frame_id is not used + pub header: Header, + /// corresponding time from this external source + pub time_ref: Time, + /// (optional) name of time source + pub source: String, +} diff --git a/crates/ros2/src/std_msgs/header.rs b/crates/ros2/src/std_msgs/header.rs new file mode 100644 index 0000000000..4fe6dc29bf --- /dev/null +++ b/crates/ros2/src/std_msgs/header.rs @@ -0,0 +1,15 @@ +/// Standard metadata for higher-level stamped data types. +/// This is generally used to communicate timestamped data +/// in a particular coordinate frame. +use serde::{Deserialize, Serialize}; + +use crate::builtin_interfaces::time::Time; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Header { + /// Two-integer timestamp that is expressed as seconds and nanoseconds. + pub stamp: Time, + + /// Transform frame with which this data is associated. + pub frame_id: String, +} diff --git a/crates/ros2/src/std_msgs/mod.rs b/crates/ros2/src/std_msgs/mod.rs new file mode 100644 index 0000000000..f505d688b4 --- /dev/null +++ b/crates/ros2/src/std_msgs/mod.rs @@ -0,0 +1 @@ +pub mod header; diff --git a/crates/simulation_message/Cargo.toml b/crates/simulation_message/Cargo.toml new file mode 100644 index 0000000000..dcc33888d0 --- /dev/null +++ b/crates/simulation_message/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "simulation_message" +version.workspace = true +edition.workspace = true +license.workspace = true +homepage.workspace = true + +[dependencies] +booster = { workspace = true } +serde = { workspace = true } +zed = { workspace = true } diff --git a/crates/simulation_message/src/lib.rs b/crates/simulation_message/src/lib.rs new file mode 100644 index 0000000000..aa026aad16 --- /dev/null +++ b/crates/simulation_message/src/lib.rs @@ -0,0 +1,28 @@ +use std::time::SystemTime; + +use booster::{ + ButtonEventMsg, FallDownState, LowCommand, LowState, RemoteControllerState, TransformMessage, +}; +use serde::{Deserialize, Serialize}; +use zed::RGBDSensors; + +#[derive(Clone, Debug, Serialize, Deserialize)] +pub struct SimulationMessage { + pub time: SystemTime, + pub payload: T, +} + +#[derive(Debug, Serialize, Deserialize)] +pub enum ServerMessageKind { + LowState(LowState), + FallDownState(FallDownState), + ButtonEventMsg(ButtonEventMsg), + RemoteControllerState(RemoteControllerState), + TransformMessage(TransformMessage), + RGBDSensors(Box), +} + +#[derive(Debug, Serialize, Deserialize)] +pub enum ClientMessageKind { + LowCommand(LowCommand), +} diff --git a/crates/types/Cargo.toml b/crates/types/Cargo.toml index fee9d4662e..0e9b27e32d 100644 --- a/crates/types/Cargo.toml +++ b/crates/types/Cargo.toml @@ -18,6 +18,7 @@ nalgebra = { workspace = true } num-traits = { workspace = true } ordered-float = { workspace = true } path_serde = { workspace = true } +ros2 = { workspace = true } serde = { workspace = true } spl_network_messages = { workspace = true } splines = { workspace = true } diff --git a/crates/types/src/ycbcr422_image.rs b/crates/types/src/ycbcr422_image.rs index a7b1feac21..fe5f226542 100644 --- a/crates/types/src/ycbcr422_image.rs +++ b/crates/types/src/ycbcr422_image.rs @@ -18,6 +18,7 @@ use crate::{ color::{Rgb, YCbCr422, YCbCr444}, jpeg::JpegImage, }; +use ros2::sensor_msgs::image::Image as Ros2Image; pub const SAMPLE_SIZE: usize = 32; @@ -109,6 +110,42 @@ impl From for RgbImage { } } +impl From<&Ros2Image> for YCbCr422Image { + fn from(ros2_image: &Ros2Image) -> Self { + let width_422 = ros2_image.width / 2; + let height = ros2_image.height; + + let data = match ros2_image.encoding.as_str() { + "rgb8" => ros2_image + .data + .chunks(6) + .map(|pixel| { + let left_color: YCbCr444 = Rgb { + red: pixel[0], + green: pixel[1], + blue: pixel[2], + } + .into(); + let right_color: YCbCr444 = Rgb { + red: pixel[3], + green: pixel[4], + blue: pixel[5], + } + .into(); + [left_color, right_color].into() + }) + .collect(), + _ => unimplemented!("image encoding not supported"), + }; + + Self { + width_422, + height, + buffer: Arc::new(data), + } + } +} + impl YCbCr422Image { pub fn zero(width: u32, height: u32) -> Self { assert!( diff --git a/crates/vision/src/image_receiver.rs b/crates/vision/src/image_receiver.rs index 4eff4af064..bf3aee8ee6 100644 --- a/crates/vision/src/image_receiver.rs +++ b/crates/vision/src/image_receiver.rs @@ -3,11 +3,9 @@ use std::time::SystemTime; use color_eyre::Result; use context_attribute::context; use framework::MainOutput; -use hardware::{CameraInterface, TimeInterface}; +use hardware::{RGBDSensorsInterface, TimeInterface}; use serde::{Deserialize, Serialize}; -use types::{ - camera_position::CameraPosition, cycle_time::CycleTime, ycbcr422_image::YCbCr422Image, -}; +use types::{cycle_time::CycleTime, ycbcr422_image::YCbCr422Image}; #[derive(Deserialize, Serialize)] pub struct ImageReceiver { @@ -20,7 +18,6 @@ pub struct CreationContext {} #[context] pub struct CycleContext { hardware_interface: HardwareInterface, - camera_position: Parameter, } #[context] @@ -38,11 +35,10 @@ impl ImageReceiver { pub fn cycle( &mut self, - context: CycleContext, + context: CycleContext, ) -> Result { - let image = context - .hardware_interface - .read_from_camera(*context.camera_position)?; + let rgbd_image = context.hardware_interface.read_rgbd_sensors()?; + let ycbcr422_image: YCbCr422Image = rgbd_image.rgb.as_ref().into(); let now = context.hardware_interface.get_now(); let cycle_time = CycleTime { @@ -54,7 +50,7 @@ impl ImageReceiver { self.last_cycle_start = now; Ok(MainOutputs { - image: image.into(), + image: ycbcr422_image.into(), cycle_time: cycle_time.into(), }) } diff --git a/crates/zed/Cargo.toml b/crates/zed/Cargo.toml new file mode 100644 index 0000000000..be36bc1bb9 --- /dev/null +++ b/crates/zed/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "zed" +version.workspace = true +edition.workspace = true +license.workspace = true +homepage.workspace = true + +[dependencies] +ros2 = { workspace = true } +serde = { workspace = true } diff --git a/crates/zed/src/lib.rs b/crates/zed/src/lib.rs new file mode 100644 index 0000000000..a565e76268 --- /dev/null +++ b/crates/zed/src/lib.rs @@ -0,0 +1,22 @@ +use ros2::{ + sensor_msgs::{camera_info::CameraInfo, image::Image, imu::Imu, magnetic_field::MagneticField}, + std_msgs::header::Header, +}; +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Deserialize, Serialize)] +pub struct RGBDSensors { + pub header: Header, + + #[serde(rename = "rgbCameraInfo")] + pub rgb_camera_info: CameraInfo, + #[serde(rename = "depthCameraInfo")] + pub depth_camera_info: CameraInfo, + + pub rgb: Box, + pub depth: Box, + + pub imu: Imu, + + pub mag: MagneticField, +} diff --git a/etc/parameters/default.json b/etc/parameters/default.json index aa916f1c75..9e26dfeeb6 100644 --- a/etc/parameters/default.json +++ b/etc/parameters/default.json @@ -1,1622 +1 @@ -{ - "sensor_data_receiver": { - "calibration_steady_threshold": 0.02, - "gravity_calibration_smoothing_factor": 0.1, - "roll_pitch_calibration_smoothing_factor": 0.1, - "number_of_calibration_cycles": 15 - }, - "pose_detection": { - "enable": true, - "maximum_intersection_over_union": 0.45, - "maximum_distance_to_referee_position": 2.0, - "minimum_bounding_box_confidence": 0.8, - "minimum_overall_keypoint_confidence": 0.5, - "minimum_visual_referee_keypoint_confidence": 0.8, - "minimum_shoulder_angle": 0.2, - "free_kick_signal_angle_range": [1.2, 2.3], - "foot_z_offset": 0.025, - "override_pose_detection": false - }, - "feet_detection": { - "vision_top": { - "enable": true, - "maximum_cluster_distance": 0.25, - "minimum_consecutive_segments": 5, - "minimum_luminance_standard_deviation": 10, - "minimum_samples_per_cluster": 5, - "minimum_feet_width": 0.0, - "minimum_segment_height": 0.08 - }, - "vision_bottom": { - "enable": true, - "maximum_cluster_distance": 0.2, - "minimum_consecutive_segments": 3, - "minimum_luminance_standard_deviation": 7, - "minimum_samples_per_cluster": 4, - "minimum_feet_width": 0.05, - "minimum_segment_height": 0.1 - } - }, - "whistle_detection": { - "detection_band": { - "start": 2000, - "end": 4000 - }, - "background_noise_scaling": 1.6, - "whistle_scaling": 3.8, - "number_of_chunks": 16 - }, - "ball_detection": { - "vision_top": { - "minimal_radius": 42.0, - "preclassifier_neural_network": "preclassifier.hdf5", - "classifier_neural_network": "classifier.hdf5", - "positioner_neural_network": "positioner.hdf5", - "maximum_number_of_candidate_evaluations": 75, - "preclassifier_confidence_threshold": 0.9, - "classifier_confidence_threshold": 0.9, - "confidence_merge_factor": 1.0, - "correction_proximity_merge_factor": 1.0, - "image_containment_merge_factor": 1.0, - "cluster_merge_radius_factor": 1.5, - "ball_radius_enlargement_factor": 1.8, - "detection_noise": [8.0, 8.0], - "noise_increase_slope": 8.0, - "noise_increase_distance_threshold": 3.0 - }, - "vision_bottom": { - "minimal_radius": 42.0, - "preclassifier_neural_network": "preclassifier.hdf5", - "classifier_neural_network": "classifier.hdf5", - "positioner_neural_network": "positioner.hdf5", - "maximum_number_of_candidate_evaluations": 75, - "preclassifier_confidence_threshold": 0.9, - "classifier_confidence_threshold": 0.9, - "confidence_merge_factor": 1.0, - "correction_proximity_merge_factor": 1.0, - "image_containment_merge_factor": 1.0, - "cluster_merge_radius_factor": 1.5, - "ball_radius_enlargement_factor": 1.7, - "detection_noise": [4.0, 4.0], - "noise_increase_slope": 0.0, - "noise_increase_distance_threshold": 0.0 - } - }, - "camera_matrix_parameters": { - "vision_top": { - "camera_pitch": -1.2, - "focal_lengths": [0.96666, 1.28574], - "cc_optical_center": [0.5, 0.5] - }, - "vision_bottom": { - "camera_pitch": -39.7, - "focal_lengths": [0.96666, 1.28574], - "cc_optical_center": [0.5, 0.5] - }, - "calibration": { - "correction_in_robot": [0.0, 0.0, 0.0], - "correction_in_camera_top": [0.0, 0.0, 0.0], - "correction_in_camera_bottom": [0.0, 0.0, 0.0] - } - }, - "foot_bumper_filter": { - "activations_needed": 2, - "acceptance_duration": { "nanos": 0, "secs": 1 }, - "buffer_size": 25, - "number_of_detections_in_buffer_for_defective_declaration": 22, - "number_of_detections_in_buffer_to_reset_in_use": 2, - "obstacle_distance": 0.18, - "sensor_angle": 0.3 - }, - "image_receiver": { - "vision_top": { - "camera_position": "Top" - }, - "object_detection_top": { - "camera_position": "Top" - }, - "vision_bottom": { - "camera_position": "Bottom" - } - }, - "image_segmenter": { - "vision_top": { - "horizontal_edge_threshold": 15, - "horizontal_median_mode": "FivePixels", - "horizontal_stride": 8, - "vertical_edge_threshold": 15, - "vertical_median_mode": "FivePixels", - "vertical_stride": 2, - "vertical_stride_in_ground": 0.02 - }, - "vision_bottom": { - "horizontal_edge_threshold": 32, - "horizontal_median_mode": "FivePixels", - "horizontal_stride": 16, - "vertical_edge_threshold": 32, - "vertical_median_mode": "FivePixels", - "vertical_stride": 8, - "vertical_stride_in_ground": 0.02 - } - }, - "line_detection": { - "vision_top": { - "use_horizontal_segments": true, - "use_vertical_segments": true, - "allowed_line_length_in_field": { - "start": 0.3, - "end": 4.0 - }, - "check_edge_types": true, - "check_edge_gradient": false, - "check_line_distance": true, - "check_line_length": true, - "check_line_segments_projection": true, - "gradient_alignment": -0.9, - "gradient_sobel_stride": 3, - "margin_for_point_inclusion": 0.025, - "maximum_distance_to_robot": 6.0, - "maximum_fit_distance_in_ground": 0.05, - "maximum_gap_on_line": 0.25, - "maximum_merge_gap_in_pixels": 5, - "maximum_number_of_lines": 10, - "allowed_projected_segment_length": { - "start": 0.02, - "end": 0.095 - }, - "minimum_number_of_points_on_line": 15, - "ransac_iterations": 20 - }, - "vision_bottom": { - "use_horizontal_segments": true, - "use_vertical_segments": true, - "allowed_line_length_in_field": { - "start": 0.15, - "end": 4.0 - }, - "check_edge_types": true, - "check_edge_gradient": true, - "check_line_distance": true, - "check_line_length": true, - "check_line_segments_projection": true, - "gradient_alignment": -0.95, - "gradient_sobel_stride": 5, - "margin_for_point_inclusion": 0.025, - "maximum_distance_to_robot": 6.0, - "maximum_fit_distance_in_ground": 0.05, - "maximum_gap_on_line": 0.1, - "maximum_merge_gap_in_pixels": 30, - "maximum_number_of_lines": 10, - "allowed_projected_segment_length": { - "start": 0.02, - "end": 0.1 - }, - "minimum_number_of_points_on_line": 10, - "ransac_iterations": 20 - } - }, - "field_border_detection": { - "vision_top": { - "enable": true, - "min_points_per_line": 10, - "angle_threshold": 0.35, - "first_line_association_distance": 2.0, - "second_line_association_distance": 2.0 - }, - "vision_bottom": { - "enable": false, - "min_points_per_line": 10, - "angle_threshold": 0.35, - "first_line_association_distance": 2.0, - "second_line_association_distance": 2.0 - } - }, - "perspective_grid_candidates_provider": { - "vision_top": { - "minimum_radius": 3.0, - "minimum_number_of_segments_per_circle": 2 - }, - "vision_bottom": { - "minimum_radius": 3.0, - "minimum_number_of_segments_per_circle": 2 - } - }, - "current_minimizer_parameters": { - "allowed_current": 0.09, - "allowed_current_upper_threshold": 0.12, - "optimization_speed": 0.000005 - }, - "ball_filter": { - "hypothesis_timeout": { - "nanos": 0, - "secs": 20 - }, - "maximum_matching_cost": 0.25, - "maximum_matching_cost_validity_penalty_factor": 0.14, - "hypothesis_merge_distance": 0.1, - "log_likelihood_of_zero_velocity_threshold": 0.5, - "maximum_number_of_hypotheses": 10, - "visible_validity_exponential_decay_factor": 0.9, - "hidden_validity_exponential_decay_factor": 0.997, - "validity_output_threshold": 1.5, - "validity_discard_threshold": 0.5, - "velocity_decay_factor": 0.998, - "noise": { - "process_noise_moving": [0.005, 0.005, 0.02, 0.02], - "process_noise_resting": [0.001, 0.001], - "initial_covariance": [0.5, 0.5, 40.0, 40.0] - } - }, - "button_filter": { - "head_buttons_timeout": { - "nanos": 100000000, - "secs": 0 - }, - "calibration_buttons_timeout": { - "nanos": 0, - "secs": 1 - }, - "animation_button_timeout": { - "nanos": 0, - "secs": 2 - } - }, - "center_head_position": { - "yaw": 0.0, - "pitch": 0.4 - }, - "obstacle_filter": { - "hypothesis_timeout": { - "nanos": 0, - "secs": 2 - }, - "network_robot_measurement_matching_distance": 0.2, - "sonar_goal_post_matching_distance": 0.2, - "feet_detection_measurement_matching_distance": 0.2, - "goal_post_measurement_matching_distance": 0.35, - "hypothesis_merge_distance": 0.3, - "process_noise": [0.005, 0.005], - "feet_measurement_noise": [500.0, 500.0], - "robot_measurement_noise": [1000.0, 1000.0], - "sonar_measurement_noise": [1000.0, 1000.0], - "network_robot_measurement_noise": [3.0, 5.0], - "initial_covariance": [0.25, 0.25], - "measurement_count_threshold": 10, - "use_feet_detection_measurements": true, - "use_sonar_measurements": true, - "use_foot_bumper_measurements": true, - "robot_obstacle_radius_at_hip_height": 0.2, - "robot_obstacle_radius_at_foot_height": 0.2, - "unknown_obstacle_radius": 0.125, - "goal_post_obstacle_radius": 0.2 - }, - "sonar_filter": { - "low_pass_filter_coefficient": 0.05, - "maximal_reliable_distance": 0.6, - "minimal_reliable_distance": 0.03, - "maximal_detectable_distance": 5.0, - "middle_merge_threshold": 0.3 - }, - "sonar_obstacle": { - "sensor_angle": 0.35 - }, - "step_planner": { - "mode": "Mpc", - "injected_step": null, - "walk_volume_delta_slow": { - "forward": 0.0, - "backward": 0.0, - "outward": 0.0, - "inward": 0.0, - "outward_rotation": 0.0, - "inward_rotation": 0.0 - }, - "walk_volume_delta_fast": { - "forward": 0.01, - "backward": 0.0, - "outward": 0.02, - "inward": 0.0, - "outward_rotation": 0.2, - "inward_rotation": 0.2 - }, - "request_scale": { - "forward": 1.0, - "left": 1.4, - "turn": 1.5 - }, - "walk_volume_extents": { - "forward": 0.045, - "backward": 0.03, - "outward": 0.1, - "inward": 0.01, - "outward_rotation": 0.5, - "inward_rotation": 0.5 - }, - "optimization_parameters": { - "optimizer_steps": 10, - "cost_factors": { - "path_progress": 0.5, - "path_distance": 10.0, - "target_orientation": 10.0, - "walk_orientation": 0.5 - }, - "path_alignment_tolerance": 0.5, - "path_progress_smoothness": 0.05, - "target_orientation_ahead_tolerance": 0.5, - "target_orientation_side_alignment_tolerance": 1.4, - "hybrid_align_distance": 0.4, - "warm_start": true - } - }, - "zero_moment_point": { - "linear_acceleration_low_pass_factor": 0.005 - }, - "whistle_filter": { - "buffer_length": 20, - "minimum_detections": 2 - }, - "walking_engine": { - "anatomic_constraints": { - "valid_x": [-0.05, 0.05], - "valid_y": [-0.1, 0.1] - }, - "base": { - "foot_lift_apex": 0.008, - "foot_lift_apex_increase": { - "forward": 0.003, - "left": 0.003, - "turn": 0.005 - }, - "foot_offset_left": [0.0, 0.052, 0.0], - "foot_offset_right": [0.0, -0.052, 0.0], - "step_duration": { "nanos": 235000000, "secs": 0 }, - "step_duration_increase": { - "forward": 0.005, - "left": 0.013, - "turn": 0.02 - }, - "step_midpoint": 0.5, - "torso_offset": 0.0, - "torso_tilt_base": 0.025, - "torso_tilt": { - "forward": 0.005, - "left": 0.005, - "turn": 0.05 - }, - "walk_height": 0.23 - }, - "catching_steps": { - "enabled": true, - "target_x_scale_backward": 0.7, - "target_x_scale_forward": 0.6, - "target_y_scale": 0.6, - "max_target_distance": [0.1, 0.15], - "over_estimation_factor": [0.4, 1.0], - "balance_region_x": 0.04 - }, - "gyro_balancing": { - "noise_scale": [0.6, 0.6], - "balance_factors": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": 0.01, - "knee_pitch": 0.01, - "ankle_pitch": 0.05, - "ankle_roll": 0.0 - }, - "low_pass_factor": 0.4, - "max_delta": { - "hip_yaw_pitch": 0.05, - "hip_roll": 0.05, - "hip_pitch": 0.05, - "knee_pitch": 0.05, - "ankle_pitch": 0.05, - "ankle_roll": 0.05 - } - }, - "dynamic_interpolation_speed": { - "active_range": [-0.1445, -0.0573], - "max_reduction": 0.7, - "slip_reduction": 1.0, - "xy_offset_stop_weight": 0.15 - }, - "foot_leveling": { - "leaning_backwards_factor": 0.5, - "leaning_forward_factor": 0.75, - "max_level_delta": 0.025, - "max_pitch": 0.12217, - "max_roll": 0.08727, - "noise_scale": [0.087, 0.087], - "roll_factor": 0.5, - "start_reduce_to_zero": 0.75 - }, - "forward_turn_reduction": 0.7, - "forward_turn_threshold": 0.45, - "foot_support": { - "min": [-0.02, -0.02], - "max": [0.04, 0.02] - }, - "max_rotation_speed": 50000.0, - "max_forward_acceleration": 0.01, - "max_base_inside_turn": 0.1, - "max_inside_turn_increase": 4, - "max_foot_speed": 0.6, - "max_step_duration": { "nanos": 0, "secs": 2 }, - "max_support_foot_lift_speed": 0.025, - "max_turn_acceleration": 0.7, - "min_step_duration": { "nanos": 150000000, "secs": 0 }, - "min_sole_pressure": 0.15, - "sole_pressure_threshold": 1.0, - "step_midpoint": { - "forward": 0.45, - "left": 0.8, - "turn": 0.5 - }, - "stiffnesses": { - "arm_stiffness": 0.5, - "leg_stiffness_stand": 0.6, - "leg_stiffness_walk": 1.0 - }, - "stiffness_loss_compensation": { - "ankle_pitch": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": 0.0, - "knee_pitch": 0.1, - "ankle_pitch": 0.0, - "ankle_roll": 0.0 - } - }, - "swinging_arms": { - "default_roll": 0.13, - "pitch_factor": 8.0, - "pull_back_joints": { - "elbow_roll": -1.6, - "elbow_yaw": 1.5, - "hand": 0.0, - "shoulder_pitch": 1.7, - "shoulder_roll": 0.1, - "wrist_yaw": -1.6 - }, - "pull_tight_joints": { - "elbow_roll": -1.6, - "elbow_yaw": 1.1, - "hand": 0.0, - "shoulder_pitch": 1.35, - "shoulder_roll": -0.3, - "wrist_yaw": -1.0 - }, - "pulling_back_duration": { "nanos": 800000000, "secs": 0 }, - "pulling_tight_duration": { "nanos": 300000000, "secs": 0 }, - "roll_factor": 0.4, - "torso_tilt_compensation_factor": 0.05 - } - }, - "stand_up_back": { - "gyro_low_pass_factor": 0.3, - "leg_balancing_factor": [0, 0.05] - }, - "stand_up_front": { - "gyro_low_pass_factor": 0.3, - "leg_balancing_factor": [0, 0.05], - "slow_speed_factor": 0.55 - }, - "stand_up_sitting": { - "gyro_low_pass_factor": 0.3, - "leg_balancing_factor": [0, 0.04], - "slow_speed_factor": 0.5 - }, - "keeper_motion": { - "maximum_ball_distance": 1.5, - "minimum_ball_velocity": 0.6, - "action_radius_center": 0.25, - "minimum_velocity": -0.5, - "action_radius_left": 0.5 - }, - "kick_steps": { - "forward": [ - { - "base_step": { "forward": 0.04, "left": 0.0, "turn": 0.0 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.015, - "midpoint": 0.5, - "support_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - }, - "swing_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - } - }, - { - "base_step": { "forward": 0.08, "left": 0.0, "turn": 0.0 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.02, - "midpoint": 0.5, - "support_overrides": { - "hip_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": -0.02, "timepoint": { "nanos": 100000000, "secs": 0 } }, - { "value": -0.04, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": -0.02, "timepoint": { "nanos": 240000000, "secs": 0 } } - ], - "knee_pitch": null, - "ankle_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 50000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 200000000, "secs": 0 } } - ] - }, - "swing_overrides": { - "hip_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 100000000, "secs": 0 } }, - { "value": -0.4, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 240000000, "secs": 0 } } - ], - "knee_pitch": null, - "ankle_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 50000000, "secs": 0 } }, - { "value": 0.1, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 200000000, "secs": 0 } } - ] - } - }, - { - "base_step": { "forward": 0.0, "left": 0.0, "turn": 0.0 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.015, - "midpoint": 0.5, - "support_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - }, - "swing_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - } - }, - { - "base_step": { "forward": -0.02, "left": 0.0, "turn": 0.0 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.01, - "midpoint": 0.5, - "support_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - }, - "swing_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - } - } - ], - "turn": [ - { - "base_step": { "forward": 0.0, "left": 0.005, "turn": -0.8 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.01, - "midpoint": 0.5, - "support_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - }, - "swing_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - } - }, - { - "base_step": { "forward": 0.06, "left": 0.0, "turn": -0.25 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.02, - "midpoint": 0.6, - "support_overrides": { - "hip_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": -0.02, "timepoint": { "nanos": 100000000, "secs": 0 } }, - { "value": -0.04, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": -0.02, "timepoint": { "nanos": 240000000, "secs": 0 } } - ], - "knee_pitch": null, - "ankle_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 50000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 200000000, "secs": 0 } } - ] - }, - "swing_overrides": { - "hip_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 100000000, "secs": 0 } }, - { "value": -0.4, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 240000000, "secs": 0 } } - ], - "knee_pitch": null, - "ankle_pitch": [ - { "value": 0.0, "timepoint": { "nanos": 0, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 50000000, "secs": 0 } }, - { "value": 0.2, "timepoint": { "nanos": 150000000, "secs": 0 } }, - { "value": 0.0, "timepoint": { "nanos": 200000000, "secs": 0 } } - ] - } - }, - { - "base_step": { "forward": -0.02, "left": 0.0, "turn": 0.0 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.01, - "midpoint": 0.5, - "support_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - }, - "swing_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - } - } - ], - "side": [ - { - "base_step": { "forward": 0.0, "left": 0.0, "turn": 0.0 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.01, - "midpoint": 0.5, - "support_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - }, - "swing_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - } - }, - { - "base_step": { "forward": 0.0, "left": 0.12, "turn": 0.0 }, - "step_duration": { "nanos": 260000000, "secs": 0 }, - "foot_lift_apex": 0.02, - "midpoint": 0.4, - "support_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - }, - "swing_overrides": { - "hip_pitch": null, - "knee_pitch": null, - "ankle_pitch": null - } - } - ] - }, - "localization": { - "circle_measurement_noise": [600.0, 600.0], - "additional_moving_noise_circle": [600.0, 600.0], - "gradient_convergence_threshold": 1e-2, - "gradient_descent_step_size": 0.01, - "hypothesis_prediction_score_reduction_factor": 0.9, - "hypothesis_retain_factor": 0.05, - "initial_hypothesis_covariance": [ - 0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001 - ], - "initial_hypothesis_score": 10.0, - "initial_poses": { - "one": { - "center_line_offset_x": -3.0, - "side": "Left" - }, - "two": { - "center_line_offset_x": -2.0, - "side": "Left" - }, - "three": { - "center_line_offset_x": -3.5, - "side": "Right" - }, - "four": { - "center_line_offset_x": -2.0, - "side": "Right" - }, - "five": { - "center_line_offset_x": -3.0, - "side": "Right" - }, - "six": { - "center_line_offset_x": -1.0, - "side": "Left" - }, - "seven": { - "center_line_offset_x": -1.0, - "side": "Right" - } - }, - "line_length_acceptance_factor": 1.5, - "line_measurement_noise": [600.0, 100.0], - "additional_moving_noise_line": [600.0, 200.0], - "maximum_amount_of_gradient_descent_iterations": 20, - "maximum_amount_of_outer_iterations": 10, - "minimum_fit_error": 0.001, - "odometry_noise": [0.05, 0.01, 0.008], - "use_line_measurements": true, - "penalized_distance": 0.5, - "penalized_hypothesis_covariance": [ - 0.01, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.001 - ], - "good_matching_threshold": 0.5, - "score_per_good_match": 2.0, - "tentative_penalized_duration": { - "nanos": 0, - "secs": 12 - }, - "hypothesis_score_base_increase": 0.1 - }, - "odometry": { - "odometry_scale_factor": [1.1, 1.2] - }, - "orientation_filter": { - "filter_gain": 0.01, - "calibration_steady_threshold": 0.02, - "calibration_smoothing_factor": 0.1, - "number_of_calibration_cycles": 25 - }, - "fall_state_estimation": { - "linear_acceleration_low_pass_factor": 0.05, - "angular_velocity_low_pass_factor": 0.2, - "roll_pitch_low_pass_factor": 0.9, - "gravitational_acceleration_threshold": 7.0, - "gravitational_force_sitting": [3.6, 0.0, 8.8], - "falling_angle_threshold_forward": [-0.4, 0.6], - "falling_angle_threshold_forward_with_catching_steps": [-0.45, 0.6], - "falling_timeout": { - "nanos": 0, - "secs": 1 - }, - "difference_to_sitting_threshold": 1.5, - "sitting_pose": { - "head": { - "pitch": 0.4693620204925537, - "yaw": -0.013848066329956056 - }, - "left_arm": { - "elbow_roll": -1.1366519927978516, - "elbow_yaw": 0.1518239974975586, - "hand": 0.04400002956390381, - "shoulder_pitch": 2.0739259719848633, - "shoulder_roll": 0.2438640594482422, - "wrist_yaw": -1.6015381813049316 - }, - "left_leg": { - "ankle_pitch": -0.7992560863494873, - "ankle_roll": 0.000041961669921875, - "hip_pitch": -1.5508320331573486, - "hip_roll": 0.007711887359619141, - "hip_yaw_pitch": 0.0614018440246582, - "knee_pitch": 0.7761621475219727 - }, - "right_arm": { - "elbow_roll": 1.095317840576172, - "elbow_yaw": -0.16418004035949707, - "hand": 0.05200004577636719, - "shoulder_pitch": 2.0816798210144043, - "shoulder_roll": -0.23321008682250977, - "wrist_yaw": 1.579978108406067 - }, - "right_leg": { - "ankle_pitch": -0.7899680137634277, - "ankle_roll": 0.007711887359619141, - "hip_pitch": -1.5447797775268557, - "hip_roll": 0.0061779022216796875, - "hip_yaw_pitch": 0.0614018440246582, - "knee_pitch": 0.8207318782806396 - } - } - }, - "fall_protection": { - "early_protection_timeout": { - "nanos": 400000000, - "secs": 0 - }, - "reached_threshold": 0.1, - "head_stiffness": { - "start": 0.5, - "end": 0.2 - }, - "arm_stiffness": { - "start": 0.3, - "end": 0.05 - }, - "leg_stiffness": { - "start": 0.3, - "end": 0.2 - }, - "hip_yaw_pitch_stiffness": 0.8, - "front_early": { - "head": { - "yaw": 0.0, - "pitch": -0.5759586531581288 - }, - "left_arm": { - "shoulder_pitch": 1.0471975511965976, - "shoulder_roll": 0.0, - "elbow_yaw": 2.0943951023931953, - "elbow_roll": 0.0, - "wrist_yaw": -1.5707963267948966, - "hand": 0.0 - }, - "right_arm": { - "shoulder_pitch": 0.6981317007977318, - "shoulder_roll": 0.17453292519943295, - "elbow_yaw": -1.3962634015954636, - "elbow_roll": 0.8726646259971648, - "wrist_yaw": 1.5707963267948966, - "hand": 0.0 - }, - "left_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": -0.3490658503988659, - "knee_pitch": 1.9198621771937625, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - }, - "right_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": 0.3490658503988659, - "knee_pitch": 1.9198621771937625, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - } - }, - "front_late": { - "head": { - "yaw": 0.0, - "pitch": -0.5759586531581288 - }, - "left_arm": { - "shoulder_pitch": 1.0471975511965976, - "shoulder_roll": 0.0, - "elbow_yaw": 2.0943951023931953, - "elbow_roll": 0.0, - "wrist_yaw": -1.5707963267948966, - "hand": 0.0 - }, - "right_arm": { - "shoulder_pitch": 0.6981317007977318, - "shoulder_roll": 0.17453292519943295, - "elbow_yaw": -1.3962634015954636, - "elbow_roll": 0.8726646259971648, - "wrist_yaw": 1.5707963267948966, - "hand": 0.0 - }, - "left_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": 0.3490658503988659, - "knee_pitch": 0.2617993877991494, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - }, - "right_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": 0.3490658503988659, - "knee_pitch": 0.2617993877991494, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - } - }, - "back_early": { - "head": { - "yaw": 0.0, - "pitch": 0.4363323129985824 - }, - "left_arm": { - "shoulder_pitch": 2.0943951023931953, - "shoulder_roll": 0.2617993877991494, - "elbow_yaw": 0.17453292519943295, - "elbow_roll": -1.2217304763960306, - "wrist_yaw": -1.5707963267948966, - "hand": 0.0 - }, - "right_arm": { - "shoulder_pitch": 2.0943951023931953, - "shoulder_roll": -0.2617993877991494, - "elbow_yaw": -0.17453292519943295, - "elbow_roll": 1.2217304763960306, - "wrist_yaw": 1.5707963267948966, - "hand": 0.0 - }, - "left_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": -1.5707963267948966, - "knee_pitch": 1.9198621771937625, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - }, - "right_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": -1.5707963267948966, - "knee_pitch": 1.9198621771937625, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - } - }, - "back_late": { - "head": { - "yaw": 0.0, - "pitch": 0.4363323129985824 - }, - "left_arm": { - "shoulder_pitch": 2.0943951023931953, - "shoulder_roll": 0.2617993877991494, - "elbow_yaw": 0.17453292519943295, - "elbow_roll": -1.2217304763960306, - "wrist_yaw": -1.5707963267948966, - "hand": 0.0 - }, - "right_arm": { - "shoulder_pitch": 2.0943951023931953, - "shoulder_roll": -0.2617993877991494, - "elbow_yaw": -0.17453292519943295, - "elbow_roll": 1.2217304763960306, - "wrist_yaw": 1.5707963267948966, - "hand": 0.0 - }, - "left_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": -1.5707963267948966, - "knee_pitch": 0.8377580409572781, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - }, - "right_leg": { - "hip_yaw_pitch": 0.0, - "hip_roll": 0.0, - "hip_pitch": -1.5707963267948966, - "knee_pitch": 0.8377580409572781, - "ankle_pitch": -0.7853981633974483, - "ankle_roll": 0.0 - } - } - }, - "default_motion_stiffness_upper_body": 0.5, - "initial_pose": { - "head": { - "pitch": 0.4, - "yaw": 0.0 - }, - "left_arm": { - "elbow_roll": 0.0, - "elbow_yaw": -1.57, - "hand": 0.0, - "shoulder_pitch": 1.57, - "shoulder_roll": 0.1, - "wrist_yaw": 0.0 - }, - "left_leg": { - "ankle_pitch": 0.01, - "ankle_roll": -0.002, - "hip_pitch": 0.09, - "hip_roll": 0.0, - "hip_yaw_pitch": 0.0, - "knee_pitch": -0.06 - }, - "right_arm": { - "elbow_roll": 0.0, - "elbow_yaw": 1.57, - "hand": 0.0, - "shoulder_pitch": 1.57, - "shoulder_roll": -0.1, - "wrist_yaw": 0.0 - }, - "right_leg": { - "ankle_pitch": 0.01, - "ankle_roll": 0.002, - "hip_pitch": 0.09, - "hip_roll": 0.0, - "hip_yaw_pitch": 0.0, - "knee_pitch": -0.06 - } - }, - "penalized_pose": { - "head": { - "pitch": 0.4, - "yaw": 0.0 - }, - "left_arm": { - "elbow_roll": 0.0, - "elbow_yaw": -1.57, - "hand": 0.0, - "shoulder_pitch": 1.57, - "shoulder_roll": 0.1, - "wrist_yaw": 0.0 - }, - "left_leg": { - "ankle_pitch": 0.01, - "ankle_roll": -0.002, - "hip_pitch": 0.09, - "hip_roll": 0.0, - "hip_yaw_pitch": 0.0, - "knee_pitch": -0.06 - }, - "right_arm": { - "elbow_roll": 0.0, - "elbow_yaw": 1.57, - "hand": 0.0, - "shoulder_pitch": 1.57, - "shoulder_roll": -0.1, - "wrist_yaw": 0.0 - }, - "right_leg": { - "ankle_pitch": 0.01, - "ankle_roll": 0.002, - "hip_pitch": 0.09, - "hip_roll": 0.0, - "hip_yaw_pitch": 0.0, - "knee_pitch": -0.06 - } - }, - "penalty_shot_direction_estimation": { - "moving_distance_threshold": 0.2, - "minimum_velocity": -0.5, - "center_jump_trigger_radius": 0.15 - }, - "head_motion": { - "maximum_velocity": { - "yaw": 3.0, - "pitch": 0.8 - }, - "maximum_defender_velocity": { - "yaw": 2.0, - "pitch": 0.8 - }, - "outer_maximum_pitch": 0.0, - "inner_maximum_pitch": 0.61, - "outer_minimum_pitch": 0.0, - "inner_minimum_pitch": -0.43, - "outer_yaw": 1.3, - "injected_head_joints": null - }, - "look_at": { - "glance_angle": 0.25, - "glance_direction_toggle_interval": { - "nanos": 0, - "secs": 1 - }, - "minimum_bottom_focus_pitch": 0.2, - "image_regions": { - "bottom": [0.5, 0.85], - "center": [0.5, 0.5] - } - }, - "look_around": { - "look_around_timeout": { - "nanos": 0, - "secs": 2 - }, - "quick_search_timeout": { - "nanos": 300000000, - "secs": 0 - }, - "middle_positions": { - "yaw": 0.0, - "pitch": 0.4 - }, - "left_positions": { - "yaw": 1.3, - "pitch": 0.0 - }, - "right_positions": { - "yaw": -1.3, - "pitch": 0.0 - }, - "halfway_left_positions": { - "yaw": 0.9, - "pitch": 0.0 - }, - "halfway_right_positions": { - "yaw": -0.9, - "pitch": 0.0 - }, - "initial_left_positions": { - "yaw": 0.6, - "pitch": 0.4 - }, - "initial_right_positions": { - "yaw": -0.6, - "pitch": 0.4 - } - }, - "in_walk_kicks": { - "forward": { - "position": [-0.23, -0.05], - "position_offset": [0.0, 0.0], - "orientation": 0.0, - "reached_x": { - "start": -0.12, - "end": 0.06 - }, - "reached_y": { - "start": -0.02, - "end": 0.02 - }, - "reached_turn": { - "start": -0.1, - "end": 0.1 - }, - "shot_distance": 4.0, - "enabled": true - }, - "turn": { - "position": [-0.17, -0.11], - "position_offset": [0.0, 0.0], - "orientation": 0.9, - "reached_x": { - "start": -0.02, - "end": 0.02 - }, - "reached_y": { - "start": -0.02, - "end": 0.02 - }, - "reached_turn": { - "start": -0.1, - "end": 0.1 - }, - "shot_distance": 3.5, - "enabled": true - }, - "side": { - "position": [-0.15, 0.03], - "position_offset": [0.0, 0.0], - "orientation": -1.57, - "reached_x": { - "start": -0.05, - "end": 0.05 - }, - "reached_y": { - "start": -0.06, - "end": 0.06 - }, - "reached_turn": { - "start": -0.1, - "end": 0.1 - }, - "shot_distance": 0.8, - "enabled": true - } - }, - "kick_selector": { - "distance_to_corner": 1.5, - "corner_kick_target_distance_to_goal": 1.3, - "min_obstacle_distance": 1.0, - "kick_pose_robot_radius": 0.1, - "kick_off_angle": 0.9, - "default_kick_variants": ["Forward", "Side", "Turn"], - "corner_kick_variants": ["Forward", "Side", "Turn"], - "kick_off_kick_variants": ["Forward"], - "penalty_shot_kick_variants": ["Forward"], - "default_kick_strength": 1.0, - "corner_kick_strength": 0.25, - "kick_off_kick_strength": 1.0, - "penalty_shot_kick_strength": 1.0, - "angle_distance_weight": 0.02, - "closer_to_goal_threshold": 1.0, - "goal_accuracy_margin": 0.25 - }, - "role_assignment": { - "forced_role": null, - "keeper_replacementkeeper_switch_time": { "nanos": 0, "secs": 12 }, - "claim_striker_from_team_ball": false, - "loser_timeout": { "nanos": 0, "secs": 5 }, - "claim_striker_from_team_ball": false, - "maximum_trusted_team_ball_age": { "nanos": 0, "secs": 1 }, - "maximum_trusted_team_ball_distance": 0.5 - }, - "walk_speed": { - "defend": "Normal", - "dribble": "Fast", - "intercept_ball": "Fast", - "lost_ball": "Normal", - "search": "Normal", - "support": "Normal", - "walk_to_kickoff": "Normal", - "walk_to_penalty_kick": "Normal" - }, - "behavior": { - "optional_roles": [ - "DefenderLeft", - "DefenderRight", - "StrikerSupporter", - "MidfielderRight", - "MidfielderLeft" - ], - "injected_motion_command": null, - "role_positions": { - "defender_aggressive_ring_radius": 2.0, - "defender_passive_ring_radius": 1.7, - "defender_y_offset": 0.7, - "defender_passive_distance": 3.5, - "defender_passive_hysteresis": 1.0, - "left_midfielder_distance_to_ball": 2.5, - "left_midfielder_maximum_x_in_ready_and_when_ball_is_not_free": -1.5, - "left_midfielder_minimum_x": -1.0, - "right_midfielder_distance_to_ball": 2.5, - "right_midfielder_maximum_x_in_ready_and_when_ball_is_not_free": -1.5, - "right_midfielder_minimum_x": 0.0, - "striker_supporter_distance_to_ball": 1.2, - "striker_supporter_maximum_x_in_ready_and_when_ball_is_not_free": -1.0, - "striker_supporter_minimum_x": -2.0, - "keeper_x_offset": 0.1, - "keeper_passive_distance": 4.5, - "striker_distance_to_non_free_center_circle": 0.4, - "striker_kickoff_position": [-0.21, 0.03] - }, - "dribbling": { - "hybrid_align_distance": 0.5, - "distance_to_be_aligned": 0.1, - "angle_to_approach_ball_from_threshold": 0.78, - "ignore_robot_when_near_ball_radius": 0.6, - "distance_to_look_directly_at_the_ball": 1.0 - }, - "walk_and_stand": { - "hysteresis": [0.1, 0.1], - "target_reached_thresholds": [0.02, 0.05], - "hybrid_align_distance": 0.95, - "normal_distance_to_be_aligned": 0.05, - "defender_distance_to_be_aligned": 1.05, - "defender_hysteresis": [0.5, 0.5], - "supporter_hysteresis": [0.5, 0.5] - }, - "lost_ball": { - "offset_to_last_ball_location": [1.0, 0.0] - }, - "path_planning": { - "robot_radius_at_hip_height": 0.15, - "robot_radius_at_foot_height": 0.2, - "minimum_robot_radius_at_foot_height": 0.11, - "ball_obstacle_radius": 0.05, - "field_border_weight": 0.15, - "line_walking_speed": 0.25, - "arc_walking_speed": 0.2, - "rotation_penalty_factor": 0.4, - "half_rotation": { - "nanos": 0, - "secs": 3 - } - }, - "search": { - "position_reached_distance": 0.4, - "rotation_per_step": 0.4, - "turn_secs": 3.0, - "stand_secs": 1.0, - "estimated_ball_speed": 0.75 - }, - "look_action": { - "angle_threshold": 0.95, - "distance_threshold": 3.0, - "look_forward_position": [1.0, 0.0], - "position_of_interest_switch_interval": { - "nanos": 0, - "secs": 1 - } - }, - "intercept_ball": { - "maximum_ball_distance": 3.0, - "minimum_ball_velocity": 0.4, - "minimum_ball_velocity_towards_robot": 0.2, - "minimum_ball_velocity_towards_own_half": 0.05, - "maximum_intercept_distance": 0.5 - }, - "maximum_lookaround_duration": { - "nanos": 0, - "secs": 6 - }, - "time_to_reach_delay_when_fallen": { - "nanos": 0, - "secs": 5 - }, - "maximum_standup_attempts": 3 - }, - "game_controller_filter": { - "time_since_last_message_to_consider_ip_active": { - "nanos": 0, - "secs": 5 - }, - "collision_alert_cooldown": { - "nanos": 0, - "secs": 10 - } - }, - "game_state_filter": { - "game_controller_controller_delay": { - "nanos": 0, - "secs": 3 - }, - "kick_off_grace_period": { - "nanos": 0, - "secs": 10 - }, - "playing_message_delay": { - "nanos": 0, - "secs": 15 - }, - "ready_message_delay": { - "nanos": 0, - "secs": 15 - }, - "tentative_finish_duration": { - "nanos": 0, - "secs": 4 - }, - "distance_to_consider_ball_moved_in_kick_off": 0.3, - "whistle_acceptance_goal_distance": [0.5, 0.5], - "duration_to_keep_observed_ball": { "nanos": 0, "secs": 14 }, - "duration_to_keep_new_penalties": { "nanos": 0, "secs": 30 } - }, - "ready_signal_detection_filter": { - "minimum_ready_signal_detections": 1, - "pose_queue_length": 10, - "minimum_number_poses_before_message": 2, - "message_grace_period": { - "nanos": 0, - "secs": 3 - }, - "message_interval": { - "nanos": 0, - "secs": 1 - } - }, - "free_kick_signal_detection_filter": { - "minimum_free_kick_signal_detections": 1, - "pose_queue_length": 10, - "minimum_number_poses_before_message": 2, - "message_grace_period": { - "nanos": 0, - "secs": 3 - }, - "message_interval": { - "nanos": 0, - "secs": 1 - } - }, - "sacrificial_lamb": { - "wait_for_opponent_penalties_period": { - "nanos": 0, - "secs": 0 - }, - "wait_for_own_penalties_period": { - "nanos": 0, - "secs": 0 - }, - "sacrificial_nao_playernumber": "Three" - }, - "ground_contact_detector": { - "pressure_threshold": 0.6, - "hysteresis": 0.6, - "timeout": { - "nanos": 50000000, - "secs": 0 - } - }, - "support_foot_estimation": { - "hysteresis": 0.4 - }, - "projected_limbs": { - "vision_top": { - "enable": false - }, - "vision_bottom": { - "enable": true - }, - "torso_bounding_polyline": [ - [0, -0.09799999743700027, 0.23499999940395355], - [-0.03500000014901161, -0.09799999743700027, 0.2199999988079071], - [-0.05999999865889549, -0.09799999743700027, 0.17000000178813934], - [-0.05000000074505806, -0.07000000029802322, 0.17000000178813934], - [-0.05000000074505806, -0.05000000074505806, 0.17000000178813934], - [-0.05000000074505806, -0.029999999329447743, 0.17000000178813934], - [-0.05000000074505806, 0, 0.17000000178813934], - [-0.05000000074505806, 0.029999999329447743, 0.17000000178813934], - [-0.05000000074505806, 0.05000000074505806, 0.17000000178813934], - [-0.05000000074505806, 0.07000000029802322, 0.17000000178813934], - [-0.05999999865889549, 0.09799999743700027, 0.17000000178813934], - [-0.03500000014901161, 0.09799999743700027, 0.2199999988079071], - [0, 0.09799999743700027, 0.23499999940395355], - [0.03500000014901161, 0.09799999743700027, 0.2199999988079071], - [0.05999999865889549, 0.09799999743700027, 0.17000000178813934], - [0.03650000074505806, 0.06000000029802322, 0.1850000059604645], - [0.06000000029802322, 0.03500000014901161, 0.15000000596046448], - [0.06300000029802322, 0, 0.15000000596046448], - [0.06000000029802322, -0.03500000014901161, 0.15000000596046448], - [0.05000000074505806, -0.07000000029802322, 0.15000000596046448], - [0.05999999865889549, -0.09799999743700027, 0.17000000178813934], - [0.03500000014901161, -0.09799999743700027, 0.2199999988079071], - [0, -0.09799999743700027, 0.23499999940395355] - ], - "lower_arm_bounding_polyline": [ - [-0.06, 0.03, 0], - [-0.06, 0.0225, 0.0225], - [-0.06, 0, 0.03], - [-0.06, -0.0225, 0.0225], - [-0.06, -0.03, 0], - [-0.06, -0.0225, -0.0225], - [-0.06, 0, -0.03], - [-0.06, 0.0225, -0.0225], - [-0.06, 0.03, 0], - [0.085, 0.045, 0], - [0.085, 0.0325, 0.0325], - [0.085, 0, 0.045], - [0.085, -0.0325, 0.0325], - [0.085, -0.045, 0], - [0.085, -0.0325, -0.0325], - [0.085, 0, -0.045], - [0.085, 0.0325, -0.0325], - [0.085, 0.045, 0] - ], - "upper_arm_bounding_polyline": [ - [0, 0.029999999329447746, 0], - [0, 0.02250000089406967, 0.02250000089406967], - [0, 0, 0.029999999329447746], - [0, -0.02250000089406967, 0.02250000089406967], - [0, -0.029999999329447746, 0], - [0, -0.02250000089406967, -0.02250000089406967], - [0, 0, -0.029999999329447746], - [0, 0.02250000089406967, -0.02250000089406967], - [0, 0.029999999329447746, 0], - [-0.12, 0.029999999329447746, 0], - [-0.12, 0.02250000089406967, 0.02250000089406967], - [-0.12, 0, 0.029999999329447746], - [-0.12, -0.02250000089406967, 0.02250000089406967], - [-0.12, -0.029999999329447746, 0], - [-0.12, -0.02250000089406967, -0.02250000089406967], - [-0.12, 0, -0.029999999329447746], - [-0.12, 0.02250000089406967, -0.02250000089406967], - [-0.12, 0.029999999329447746, 0] - ], - "knee_bounding_polyline": [ - [0.05, 0.05, -0.1], - [0.08, 0, -0.1], - [0.05, -0.05, -0.1] - ], - "foot_bounding_polyline": [ - [0.1, 0.06, 0.02], - [0.12, 0, 0.02], - [0.1, -0.06, 0.02] - ] - }, - "field_dimensions": { - "ball_radius": 0.05, - "length": 9.0, - "width": 6.0, - "line_width": 0.05, - "penalty_marker_size": 0.1, - "goal_box_area_length": 0.6, - "goal_box_area_width": 2.2, - "penalty_area_length": 1.65, - "penalty_area_width": 4.0, - "penalty_marker_distance": 1.3, - "center_circle_diameter": 1.5, - "border_strip_width": 2.0, - "goal_inner_width": 1.5, - "goal_post_diameter": 0.1, - "goal_depth": 0.5 - }, - "player_number": "Seven", - "recorded_primary_states": ["Standby", "Ready", "Set", "Playing"], - "spl_network": { - "game_controller_return_message_interval": { - "nanos": 0, - "secs": 1 - }, - "remaining_amount_of_messages_to_stop_sending": 20, - "silence_interval_between_messages": { - "nanos": 0, - "secs": 1 - }, - "spl_striker_message_receive_timeout": { - "nanos": 0, - "secs": 3 - }, - "spl_striker_message_send_interval": { - "nanos": 0, - "secs": 2 - } - }, - "team_ball": { - "maximum_age": { - "nanos": 500000000, - "secs": 4 - } - }, - "joint_calibration_offsets": { - "head": { - "pitch": 0.0, - "yaw": 0.0 - }, - "left_arm": { - "elbow_roll": 0.0, - "elbow_yaw": 0.0, - "hand": 0.0, - "shoulder_pitch": 0.0, - "shoulder_roll": 0.0, - "wrist_yaw": 0.0 - }, - "left_leg": { - "ankle_pitch": 0.0, - "ankle_roll": 0.0, - "hip_pitch": 0.0, - "hip_roll": 0.0, - "hip_yaw_pitch": 0.0, - "knee_pitch": 0.0 - }, - "right_arm": { - "elbow_roll": 0.0, - "elbow_yaw": 0.0, - "hand": 0.0, - "shoulder_pitch": 0.0, - "shoulder_roll": 0.0, - "wrist_yaw": 0.0 - }, - "right_leg": { - "ankle_pitch": 0.0, - "ankle_roll": 0.0, - "hip_pitch": 0.0, - "hip_roll": 0.0, - "hip_yaw_pitch": 0.0, - "knee_pitch": 0.0 - } - }, - "condition_input_provider": { - "angular_velocity_smoothing_factor": 0.1 - }, - "rule_obstacles": { - "center_circle_obstacle_radius_increase": 0.2, - "center_circle_ballspace_free_obstacle_radius": 0.2, - "free_kick_obstacle_radius": 0.75, - "penaltykick_box_extension": 0.2 - }, - "search_suggestor": { - "cells_per_meter": 2.0, - "heatmap_convolution_kernel_weight": 0.001007049, - "minimum_validity": 0.01, - "own_ball_weight": 1.0, - "team_ball_weight": 1.0, - "rule_ball_weight": 1.0 - }, - "physical_constants": { - "gravity_acceleration": 9.81 - }, - "calibration_controller": { - "max_retries_per_capture": 10, - "stabilization_delay": { - "nanos": 0, - "secs": 5 - }, - "look_at_dispatch_delay": { - "nanos": 0, - "secs": 1 - }, - "use_stand_head_unstiff_calibration": true - } -} +{} \ No newline at end of file diff --git a/etc/parameters/hardware.json b/etc/parameters/hardware.json index 1420363deb..77bc8391da 100644 --- a/etc/parameters/hardware.json +++ b/etc/parameters/hardware.json @@ -86,6 +86,7 @@ "width": 640 }, "communication_addresses": "[::]:1337", + "mujoco_websocket_address": "ws://127.0.0.1:8000/simulation/subscribe", "microphones": { "access": "RWInterleaved", "format": "FloatLE", diff --git a/tools/parameter_tester/Cargo.toml b/tools/parameter_tester/Cargo.toml index 791ae2eea0..031540fd5b 100644 --- a/tools/parameter_tester/Cargo.toml +++ b/tools/parameter_tester/Cargo.toml @@ -7,6 +7,7 @@ homepage.workspace = true [dependencies] ball_filter = { workspace = true } +booster = { workspace = true } calibration = { workspace = true } color-eyre = { workspace = true } coordinate_systems = { workspace = true } diff --git a/tools/pepsi/src/cargo.rs b/tools/pepsi/src/cargo.rs index e789609489..751ffd3911 100644 --- a/tools/pepsi/src/cargo.rs +++ b/tools/pepsi/src/cargo.rs @@ -39,9 +39,8 @@ lazy_static! { pub static ref MANIFEST_PATHS: HashMap<&'static str, &'static str> = { HashMap::from([ ("imagine", "crates/hulk_imagine"), - ("nao", "crates/hulk_nao"), ("replayer", "crates/hulk_replayer"), - ("webots", "crates/hulk_webots"), + ("mujoco", "crates/hulk_mujoco"), ("aliveness", "services/aliveness"), ("breeze", "services/breeze"), ("hula", "services/hula"),