diff --git a/Cargo.lock b/Cargo.lock index ee4d3ff2fb..ae279c0b01 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2040,6 +2040,7 @@ dependencies = [ name = "booster" version = "0.1.0" dependencies = [ + "color-eyre", "coordinate_systems", "linear_algebra", "nalgebra", @@ -2207,9 +2208,9 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.2.53" +version = "1.2.54" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "755d2fce177175ffca841e9a06afdb2c4ab0f593d53b4dee48147dfaade85932" +checksum = "6354c81bbfd62d9cfa9cb3c773c2b7b2a3a482d569de977fd0e961f6e7c00583" dependencies = [ "find-msvc-tools", "jobserver", @@ -2218,18 +2219,13 @@ dependencies = [ ] [[package]] -name = "cdr-encoding" -version = "0.10.2" +name = "cdr" +version = "0.2.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73f0cdb643d85bb03b1b786ab8d332747f386f0a25b19367d559b437f44f1e46" +checksum = "9617422bf43fde9280707a7e90f8f7494389c182f5c70b0f67592d0f06d41dfa" dependencies = [ "byteorder", - "log", - "paste", "serde", - "serde_repr", - "static_assertions", - "thiserror 1.0.69", ] [[package]] @@ -4742,25 +4738,67 @@ dependencies = [ name = "hulk_booster" version = "0.1.0" dependencies = [ + "approx 0.5.1", + "audio", + "ball_filter", + "bincode", + "blake3", "booster", + "buffered_watch", "byteorder", "bytes", - "cdr-encoding", + "calibration", + "cdr", "chrono", + "clap", "code_generation", "color-eyre", + "communication", "control", + "coordinate_systems", + "ctrlc", + "derive_more", + "eframe", + "energy_optimization", "env_logger", "fern", - "flume", + "framework", + "futures-util", + "geometry", + "hardware", + "hsl_network", + "hsl_network_messages", + "hula_types", "hulk_manifest", + "hulk_widgets", + "ittapi", + "libc", + "linear_algebra", "log", + "motionfile", + "nalgebra", + "ndarray 0.16.1", + "object_detection", + "parameters", + "parking_lot", + "path_serde", + "projection", "ros2", + "sensor_receiver", "serde", + "serde_bytes", + "serde_json", + "simulation_message", "source_analyzer", + "systemd", "tokio", + "tokio-tungstenite 0.26.2", + "tokio-util", + "types", "vision", "walking_inference", + "world_state", + "yuv", "zenoh", ] @@ -4799,6 +4837,7 @@ dependencies = [ "projection", "rmp-serde", "ros2", + "sensor_receiver", "serde", "serde_json", "source_analyzer", @@ -4863,6 +4902,7 @@ dependencies = [ "path_serde", "projection", "ros2", + "sensor_receiver", "serde", "serde_json", "simulation_message", @@ -4918,6 +4958,7 @@ dependencies = [ "path_serde", "projection", "ros2", + "sensor_receiver", "serde", "serde_json", "source_analyzer", @@ -5026,7 +5067,7 @@ dependencies = [ "libc", "percent-encoding", "pin-project-lite", - "socket2 0.6.1", + "socket2 0.6.2", "system-configuration", "tokio", "tower-service", @@ -5569,9 +5610,9 @@ dependencies = [ [[package]] name = "libm" -version = "0.2.15" +version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" +checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981" [[package]] name = "libredox" @@ -6282,9 +6323,9 @@ dependencies = [ [[package]] name = "num-conv" -version = "0.1.0" +version = "0.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "51d515d32fb182ee37cda2ccdcb92950d6a3c2893aa280e540671c2cd0f3b1d9" +checksum = "cf97ec579c3c42f953ef76dbf8d55ac91fb219dde70e49aa4a6b7d74e9919050" [[package]] name = "num-derive" @@ -6705,6 +6746,7 @@ dependencies = [ "image", "itertools 0.14.0", "linear_algebra", + "log", "nalgebra", "ndarray 0.16.1", "openvino", @@ -6790,9 +6832,9 @@ checksum = "d05e27ee213611ffe7d6348b942e8f942b37114c00cc03cec254295a4a17852e" [[package]] name = "openssl-probe" -version = "0.2.0" +version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9f50d9b3dabb09ecd771ad0aa242ca6894994c130308ca3d7684634df8037391" +checksum = "7c87def4c32ab89d880effc9e097653c8da5d6ef28e6b539d313baaacfbafcbe" [[package]] name = "openssl-sys" @@ -7588,9 +7630,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.105" +version = "1.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "535d180e0ecab6268a3e718bb9fd44db66bbbc256257165fc699dadf70d16fe7" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" dependencies = [ "unicode-ident", ] @@ -7776,7 +7818,7 @@ dependencies = [ "quinn-udp", "rustc-hash 2.1.1", "rustls", - "socket2 0.6.1", + "socket2 0.6.2", "thiserror 2.0.18", "tokio", "tracing", @@ -7815,16 +7857,16 @@ dependencies = [ "cfg_aliases", "libc", "once_cell", - "socket2 0.6.1", + "socket2 0.6.2", "tracing", "windows-sys 0.60.2", ] [[package]] name = "quote" -version = "1.0.43" +version = "1.0.44" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc74d9a594b72ae6656596548f56f667211f8a97b3d4c3d467150794690dc40a" +checksum = "21b2ebcf727b7760c461f091f9f0f539b77b8e87f2fd88131e7f1b433b3cece4" dependencies = [ "proc-macro2", ] @@ -8294,6 +8336,7 @@ dependencies = [ "path_serde", "pyo3", "serde", + "yuv", ] [[package]] @@ -8419,7 +8462,7 @@ version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "612460d5f7bea540c490b2b6395d8e34a953e52b491accd6c86c8164c5932a63" dependencies = [ - "openssl-probe 0.2.0", + "openssl-probe 0.2.1", "rustls-pki-types", "schannel", "security-framework 3.5.1", @@ -8669,6 +8712,39 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cd0b0ec5f1c1ca621c432a25813d8d60c88abe6d3e08a3eb9cf37d97a0fe3d73" +[[package]] +name = "sensor_receiver" +version = "0.1.0" +dependencies = [ + "approx 0.5.1", + "booster", + "calibration", + "color-eyre", + "context_attribute", + "control", + "coordinate_systems", + "framework", + "geometry", + "hardware", + "hsl_network_messages", + "image", + "itertools 0.14.0", + "linear_algebra", + "log", + "nalgebra", + "ndarray 0.16.1", + "openvino", + "ordered-float 4.6.0", + "ort", + "parameters", + "path_serde", + "projection", + "rand 0.9.2", + "ros2", + "serde", + "types", +] + [[package]] name = "serde" version = "1.0.228" @@ -8688,6 +8764,16 @@ dependencies = [ "serde", ] +[[package]] +name = "serde_bytes" +version = "0.11.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a5d440709e79d88e51ac01c4b72fc6cb7314017bb7da9eeff678aa94c10e3ea8" +dependencies = [ + "serde", + "serde_core", +] + [[package]] name = "serde_core" version = "1.0.228" @@ -9070,9 +9156,9 @@ dependencies = [ [[package]] name = "socket2" -version = "0.6.1" +version = "0.6.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "17129e116933cf371d018bb80ae557e889637989d8638274fb25622827b03881" +checksum = "86f4aa3ad99f2088c990dfa82d367e19cb29268ed67c574d10d0a4bfe71f07e0" dependencies = [ "libc", "windows-sys 0.60.2", @@ -9526,9 +9612,9 @@ dependencies = [ [[package]] name = "time" -version = "0.3.45" +version = "0.3.46" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9e442fc33d7fdb45aa9bfeb312c095964abdf596f7567261062b2a7107aaabd" +checksum = "9da98b7d9b7dad93488a84b8248efc35352b0b2657397d4167e7ad67e5d535e5" dependencies = [ "deranged", "itoa", @@ -9541,15 +9627,15 @@ dependencies = [ [[package]] name = "time-core" -version = "0.1.7" +version = "0.1.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8b36ee98fd31ec7426d599183e8fe26932a8dc1fb76ddb6214d05493377d34ca" +checksum = "7694e1cfe791f8d31026952abf09c69ca6f6fa4e1a1229e18988f06a04a12dca" [[package]] name = "time-macros" -version = "0.2.25" +version = "0.2.26" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "71e552d1249bf61ac2a52db88179fd0673def1e1ad8243a00d9ec9ed71fee3dd" +checksum = "78cc610bac2dcee56805c99642447d4c5dbde4d01f752ffea0199aee1f601dc4" dependencies = [ "num-conv", "time-core", @@ -9651,7 +9737,7 @@ dependencies = [ "parking_lot", "pin-project-lite", "signal-hook-registry", - "socket2 0.6.1", + "socket2 0.6.2", "tokio-macros", "windows-sys 0.61.2", ] @@ -10316,9 +10402,9 @@ checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" [[package]] name = "uuid" -version = "1.19.0" +version = "1.20.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2e054861b4bd027cd373e18e8d8d8e6548085000e41290d95ce0c373a654b4a" +checksum = "ee48d38b119b0cd71fe4141b30f5ba9c7c5d9f4e7a3a8b4a674e4b6ef789976f" dependencies = [ "getrandom 0.3.4", "js-sys", @@ -11748,6 +11834,15 @@ dependencies = [ "synstructure", ] +[[package]] +name = "yuv" +version = "0.8.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "28f1bad143caadcfcaec93039dc9c40a30fc86f23d9e7cc03764a39fe51d9d43" +dependencies = [ + "num-traits", +] + [[package]] name = "zbus" version = "5.13.2" diff --git a/Cargo.toml b/Cargo.toml index 49bf8affa0..262394451e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -44,6 +44,7 @@ members = [ "crates/repository", "crates/robot", "crates/ros2", + "crates/sensor_receiver", "crates/simulation_message", "crates/source_analyzer", "crates/step_planning", @@ -105,7 +106,9 @@ booster = { path = "crates/booster" } buffered_watch = { path = "crates/buffered_watch" } build_script_helpers = { path = "crates/build_script_helpers" } byteorder = "1.5.0" +bytes = { version = "1.11.0", features = ["serde"] } calibration = { path = "crates/calibration" } +cdr = "0.2.4" chrono = "0.4.39" clap = { version = "4.5.29", features = ["derive", "env"] } clap_complete = "4.5.44" @@ -217,9 +220,11 @@ reqwest = { version = "0.12.12", features = ["blocking"] } rmp-serde = "1.3.0" robot = { path = "crates/robot" } ros2 = { path = "crates/ros2" } +rustdds = "0.11.5" rustfft = "6.2.0" scenario = { path = "crates/scenario" } semver = "1.0.25" +sensor_receiver = { path = "crates/sensor_receiver" } serde = { version = "1.0.228", features = ["derive", "rc"] } serde-transcode = "1.1.1" serde_bytes = "0.11.15" @@ -260,8 +265,10 @@ webots = { version = "0.8.0" } wgpu = "27.0.0" world_state = { path = "crates/world_state" } xdg = "2.5.2" +yuv = "0.8.9" zbus = "5.5.0" zed = { path = "crates/zed" } +zenoh = "1.6.2" [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/src/interfake.rs b/crates/bevyhavior_simulator/src/interfake.rs index 8246e14f37..9270ab0a25 100644 --- a/crates/bevyhavior_simulator/src/interfake.rs +++ b/crates/bevyhavior_simulator/src/interfake.rs @@ -76,10 +76,35 @@ impl PathsInterface for Interfake { } impl CameraInterface for Interfake { - fn read_image(&self) -> Result { + fn read_rectified_image(&self) -> Result { unimplemented!() } - fn read_camera_info(&self) -> Result { + + fn read_rectified_right_image(&self) -> Result { + unimplemented!() + } + + fn read_stereonet_depth_image(&self) -> Result { + unimplemented!() + } + + fn read_stereonet_visual_image(&self) -> Result { + unimplemented!() + } + + fn read_image_left_raw(&self) -> Result { + unimplemented!() + } + + fn read_image_left_raw_camera_info(&self) -> Result { + unimplemented!() + } + + fn read_image_right_raw(&self) -> Result { + unimplemented!() + } + + fn read_image_right_raw_camera_info(&self) -> Result { unimplemented!() } } diff --git a/crates/booster/Cargo.toml b/crates/booster/Cargo.toml index 94a2df54d5..1c5be76905 100644 --- a/crates/booster/Cargo.toml +++ b/crates/booster/Cargo.toml @@ -6,6 +6,7 @@ license.workspace = true homepage.workspace = true [dependencies] +color-eyre = { workspace = true } coordinate_systems = { workspace = true } linear_algebra = { workspace = true } nalgebra = { workspace = true } diff --git a/crates/booster/src/lib.rs b/crates/booster/src/lib.rs index 812c8f722b..eb56664afa 100644 --- a/crates/booster/src/lib.rs +++ b/crates/booster/src/lib.rs @@ -1,3 +1,4 @@ +use color_eyre::eyre::{bail, Result}; use coordinate_systems::Robot; use linear_algebra::Vector3; use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; @@ -41,6 +42,32 @@ impl LowState { } } +impl LowState { + pub fn serial_motor_states(&self) -> Result> { + if self.motor_state_serial.len() == 22 { + Ok(self + .motor_state_serial + .iter() + .copied() + .collect::>()) + } else { + bail!("failed to construct motor states") + } + } + + pub fn parallel_motor_states(&self) -> Result> { + if self.motor_state_parallel.len() == 22 { + Ok(self + .motor_state_parallel + .iter() + .copied() + .collect::>()) + } else { + bail!("failed to construct motor states") + } + } +} + #[repr(C)] #[cfg_attr(feature = "pyo3", pyclass(frozen))] #[derive( @@ -105,6 +132,7 @@ impl ImuState { PathIntrospect, )] pub struct MotorState { + pub mode: i8, #[serde(rename = "q")] /// Joint angle position (q), unit: rad. pub position: f32, @@ -117,18 +145,35 @@ pub struct MotorState { #[serde(rename = "tau_est")] /// Joint torque (tau_est), unit: nm pub torque: f32, + pub temperature: i8, + pub lost: u32, + pub reserve: [u32; 2], } +#[allow(clippy::too_many_arguments)] #[cfg(feature = "pyo3")] #[pymethods] impl MotorState { #[new] - pub fn new(position: f32, velocity: f32, acceleration: f32, torque: f32) -> Self { + pub fn new( + mode: i8, + position: f32, + velocity: f32, + acceleration: f32, + torque: f32, + temperature: i8, + lost: u32, + reserve: [u32; 2], + ) -> Self { Self { + mode, position, velocity, acceleration, torque, + temperature, + lost, + reserve, } } } @@ -202,9 +247,10 @@ impl LowCommand { pub fn new( joint_positions: &Joints, motor_command_parameters: &MotorCommandParameters, + command_type: CommandType, ) -> Self { LowCommand { - command_type: CommandType::Serial, + command_type, motor_commands: joint_positions .into_iter() .zip(motor_command_parameters.proportional_coefficients) @@ -224,7 +270,17 @@ impl LowCommand { #[repr(C)] #[cfg_attr(feature = "pyo3", pyclass(frozen, get_all))] -#[derive(Debug, Default, Clone, Copy, Serialize, Deserialize)] +#[derive( + Debug, + Default, + Clone, + Copy, + Serialize, + Deserialize, + PathSerialize, + PathDeserialize, + PathIntrospect, +)] pub struct MotorCommand { #[serde(rename = "q")] /// Joint angle position, unit: rad. @@ -261,8 +317,11 @@ impl MotorCommand { #[repr(C)] #[cfg_attr(feature = "pyo3", pyclass(frozen, get_all))] -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive( + Debug, Clone, Default, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect, +)] pub enum FallDownStateType { + #[default] IsReady, IsFalling, HasFallen, @@ -271,7 +330,9 @@ pub enum FallDownStateType { #[repr(C)] #[cfg_attr(feature = "pyo3", pyclass(frozen, get_all))] -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive( + Debug, Clone, Default, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect, +)] pub struct FallDownState { pub fall_down_state: FallDownStateType, /// Whether recovery (getting up) action is available @@ -323,7 +384,9 @@ impl ButtonEventMsg { #[repr(C)] #[cfg_attr(feature = "pyo3", pyclass(frozen, get_all))] -#[derive(Debug, Clone, Serialize, Deserialize)] +#[derive( + Debug, Clone, Default, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect, +)] pub struct RemoteControllerState { /** This feature can be used in user programs to implement custom gamepad/controller button functionality. |type | code | description| @@ -391,7 +454,9 @@ pub struct RemoteControllerState { } #[repr(C)] -#[derive(Clone, Debug, Deserialize, Serialize)] +#[derive( + Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, +)] #[serde(rename = "TFMessage")] pub struct TransformMessage { pub transforms: Vec, diff --git a/crates/code_generation/src/cyclers.rs b/crates/code_generation/src/cyclers.rs index b123d2a015..3df4d7a82c 100644 --- a/crates/code_generation/src/cyclers.rs +++ b/crates/code_generation/src/cyclers.rs @@ -77,6 +77,7 @@ fn generate_cycler_instance(cycler: &Cycler) -> TokenStream { .iter() .map(|instance| format_ident!("{}", instance)); quote! { + #[allow(clippy::enum_variant_names)] #[derive(Clone, Copy, Debug)] pub(crate) enum CyclerInstance { #(#instances,)* diff --git a/crates/control/src/motion/command_sender.rs b/crates/control/src/motion/command_sender.rs index d869e6c1d5..ed6f3a1e2d 100644 --- a/crates/control/src/motion/command_sender.rs +++ b/crates/control/src/motion/command_sender.rs @@ -1,4 +1,4 @@ -use booster::LowCommand; +use booster::{CommandType, LowCommand}; use color_eyre::{eyre::WrapErr, Result}; use context_attribute::context; use framework::AdditionalOutput; @@ -27,13 +27,13 @@ pub struct CycleContext { low_command: AdditionalOutput, target_joint_positions: Input, + look_at: Input, "look_at">, + motion_command: Input, walk_motor_command_parameters: Parameter, _prepare_motor_command_parameters: Parameter, hardware_interface: HardwareInterface, - look_at: Input, "look_at">, - motion_command: Input, } #[context] @@ -70,6 +70,7 @@ impl CommandSender { let walk_low_command = LowCommand::new( &target_joint_positions, context.walk_motor_command_parameters, + CommandType::Serial, ); context diff --git a/crates/control/src/motion/look_at.rs b/crates/control/src/motion/look_at.rs index 71d0aa56e3..085d7a979a 100644 --- a/crates/control/src/motion/look_at.rs +++ b/crates/control/src/motion/look_at.rs @@ -1,5 +1,6 @@ use std::{time::Duration, time::SystemTime}; +use booster::{JointsMotorState, MotorState}; use color_eyre::Result; use kinematics::forward::{head_to_neck, neck_to_robot}; use projection::camera_matrix::CameraMatrix; @@ -11,11 +12,9 @@ use framework::MainOutput; use linear_algebra::{distance, point, vector, Isometry3, Point2}; use types::{ cycle_time::CycleTime, - joints::head::HeadJoints, + joints::{head::HeadJoints, Joints}, motion_command::{GlanceDirection, HeadMotion, ImageRegion, MotionCommand}, parameters::ImageRegionParameters, - sensor_data::SensorData, - // world_state::WorldState, }; #[derive(Deserialize, Serialize)] @@ -33,7 +32,7 @@ pub struct CycleContext { cycle_time: Input, ground_to_robot: Input>, "WorldState", "ground_to_robot?">, motion_command: Input, - sensor_data: Input, + serial_motor_states: Input, "serial_motor_states">, // expected_referee_position: Input>, "expected_referee_position?">, // world_state: Input, glance_angle: Parameter, @@ -58,7 +57,7 @@ impl LookAt { pub fn cycle(&mut self, context: CycleContext) -> Result { let cycle_start_time = context.cycle_time.start_time; - let measured_head_angles = context.sensor_data.positions.head; + let measured_head_angles = context.serial_motor_states.positions().head; let default_output = Ok(MainOutputs { look_at: measured_head_angles.into(), }); diff --git a/crates/control/src/sensor_data_receiver.rs b/crates/control/src/sensor_data_receiver.rs index 95db0bb93a..9ce3d5cce8 100644 --- a/crates/control/src/sensor_data_receiver.rs +++ b/crates/control/src/sensor_data_receiver.rs @@ -10,11 +10,7 @@ use hardware::{LowStateInterface, TimeInterface}; use linear_algebra::Vector3; use nalgebra::UnitQuaternion; use serde::{Deserialize, Serialize}; -use types::{ - cycle_time::CycleTime, - joints::{arm::ArmJoints, head::HeadJoints, leg::LegJoints, Joints}, - sensor_data::SensorData, -}; +use types::{cycle_time::CycleTime, joints::Joints}; #[derive(Default, Serialize, Deserialize)] enum State { @@ -48,8 +44,8 @@ pub struct CycleContext { pub struct MainOutputs { pub imu_state: MainOutput, pub serial_motor_states: MainOutput>, + pub parallel_motor_states: MainOutput>>, pub cycle_time: MainOutput, - pub sensor_data: MainOutput, } impl SensorDataReceiver { @@ -78,55 +74,11 @@ impl SensorDataReceiver { }; self.last_cycle_start = now; - let positions = Joints { - head: HeadJoints { - yaw: low_state.motor_state_serial[0].position, - pitch: low_state.motor_state_serial[1].position, - }, - left_arm: ArmJoints { - shoulder_pitch: low_state.motor_state_serial[2].position, - shoulder_roll: low_state.motor_state_serial[3].position, - shoulder_yaw: low_state.motor_state_serial[4].position, - elbow: low_state.motor_state_serial[5].position, - }, - right_arm: ArmJoints { - shoulder_pitch: low_state.motor_state_serial[6].position, - shoulder_roll: low_state.motor_state_serial[7].position, - shoulder_yaw: low_state.motor_state_serial[8].position, - elbow: low_state.motor_state_serial[9].position, - }, - left_leg: LegJoints { - hip_pitch: low_state.motor_state_serial[10].position, - hip_roll: low_state.motor_state_serial[11].position, - hip_yaw: low_state.motor_state_serial[12].position, - knee: low_state.motor_state_serial[13].position, - ankle_up: low_state.motor_state_serial[14].position, - ankle_down: low_state.motor_state_serial[15].position, - }, - right_leg: LegJoints { - hip_pitch: low_state.motor_state_serial[16].position, - hip_roll: low_state.motor_state_serial[17].position, - hip_yaw: low_state.motor_state_serial[18].position, - knee: low_state.motor_state_serial[19].position, - ankle_up: low_state.motor_state_serial[20].position, - ankle_down: low_state.motor_state_serial[21].position, - }, - }; - - let sensor_data = SensorData { - positions, - ..SensorData::default() - }; - Ok(MainOutputs { imu_state: low_state.imu_state.into(), - serial_motor_states: low_state - .motor_state_serial - .into_iter() - .collect::>() - .into(), + serial_motor_states: low_state.serial_motor_states()?.into(), + parallel_motor_states: low_state.parallel_motor_states().ok().into(), cycle_time: cycle_time.into(), - sensor_data: sensor_data.into(), }) } } diff --git a/crates/hardware/src/lib.rs b/crates/hardware/src/lib.rs index de73d9257f..15db63134f 100644 --- a/crates/hardware/src/lib.rs +++ b/crates/hardware/src/lib.rs @@ -26,8 +26,18 @@ pub trait ActuatorInterface { } pub trait CameraInterface { - fn read_image(&self) -> Result; - fn read_camera_info(&self) -> Result; + fn read_rectified_image(&self) -> Result; + fn read_rectified_right_image(&self) -> Result; + fn read_origin_left_image(&self) -> Result; + fn read_origin_right_image(&self) -> Result; + fn read_stereonet_depth_image(&self) -> Result; + fn read_stereonet_depth_camera_info(&self) -> Result; + fn read_stereonet_visual_image(&self) -> Result; + fn read_image_combine_raw(&self) -> Result; + fn read_image_left_raw(&self) -> Result; + fn read_image_left_raw_camera_info(&self) -> Result; + fn read_image_right_raw(&self) -> Result; + fn read_image_right_raw_camera_info(&self) -> Result; } pub trait IdInterface { @@ -73,6 +83,7 @@ pub trait ButtonEventMsgInterface { pub trait RemoteControllerStateInterface { fn read_remote_controller_state(&self) -> Result; } + pub trait TransformMessageInterface { fn read_transform_message(&self) -> Result; } diff --git a/crates/hulk_booster/Cargo.toml b/crates/hulk_booster/Cargo.toml index 83766e1e44..6d820c2bd4 100644 --- a/crates/hulk_booster/Cargo.toml +++ b/crates/hulk_booster/Cargo.toml @@ -9,26 +9,74 @@ homepage.workspace = true cross-compile = 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 } byteorder = { workspace = true } -bytes = { version = "1.11.0", features = ["serde"] } -cdr-encoding = "0.10.2" +bytes = { workspace = true } +calibration = { workspace = true } +cdr = { workspace = true } chrono = { workspace = true } +clap = { 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 } -flume = "0.11.1" +framework = { workspace = true } +futures-util.workspace = true +geometry = { workspace = true } +hardware = { workspace = true } +hsl_network = { workspace = true } +hsl_network_messages = { 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 } +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 } ros2 = { workspace = true } +sensor_receiver = { workspace = true } serde = { workspace = true } +serde_bytes = { workspace = true } +serde_json = { workspace = true } +simulation_message = { 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_inference = { workspace = true } -zenoh = "1.6.2" +world_state = { workspace = true } +yuv = { workspace = true } +zenoh = { workspace = true } [build-dependencies] code_generation = { workspace = true } color-eyre = { workspace = true } hulk_manifest = { workspace = true } source_analyzer = { workspace = true } + +[features] +realtime = ["libc"] +systemd = ["dep:systemd"] +is_webots = [] diff --git a/crates/hulk_booster/src/hardware_interface.rs b/crates/hulk_booster/src/hardware_interface.rs new file mode 100644 index 0000000000..c6150c6f92 --- /dev/null +++ b/crates/hulk_booster/src/hardware_interface.rs @@ -0,0 +1,443 @@ +use std::future::{Future, IntoFuture}; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::time::SystemTime; + +use booster::{ + ButtonEventMsg, FallDownState, LowCommand, LowState, RemoteControllerState, TransformMessage, +}; +use cdr::{CdrLe, Infinite}; +use color_eyre::eyre::{bail, eyre, Context}; +use color_eyre::Result; +use hardware::{ + ButtonEventMsgInterface, CameraInterface, IdInterface, MicrophoneInterface, NetworkInterface, + PathsInterface, RecordingInterface, SpeakerInterface, TimeInterface, TransformMessageInterface, +}; +use hardware::{ + FallDownStateInterface, LowCommandInterface, LowStateInterface, RemoteControllerStateInterface, +}; +use hsl_network::endpoint::{Endpoint, Ports}; +use hula_types::hardware::{Ids, Paths}; +use ros2::sensor_msgs::camera_info::CameraInfo; +use ros2::sensor_msgs::image::Image; +use serde::{Deserialize, Serialize}; +use tokio::runtime::Handle; +use tokio_util::sync::CancellationToken; +use types::audio::SpeakerRequest; +use types::messages::{IncomingMessage, OutgoingMessage}; +use types::samples::Samples; +use zenoh::bytes::ZBytes; +use zenoh::handlers::{RingChannel, RingChannelHandler}; +use zenoh::pubsub::{Publisher, Subscriber}; +use zenoh::sample::Sample; +use zenoh::Session; + +use crate::HardwareInterface; + +struct TopicInfos { + low_state: TopicInfo, + joint_ctrl: TopicInfo, + fall_down: TopicInfo, + button_event: TopicInfo, + remote_controller_state: TopicInfo, + transform: TopicInfo, + origin_left_raw: TopicInfo, + origin_right_raw: TopicInfo, + rectified_image: TopicInfo, + rectified_right_image: TopicInfo, + stereonet_depth: TopicInfo, + stereonet_depth_camera_info: TopicInfo, + stereonet_visual: TopicInfo, + image_combine_raw: TopicInfo, + image_left_raw: TopicInfo, + image_left_raw_camera_info: TopicInfo, + image_right_raw: TopicInfo, + image_right_raw_camera_info: TopicInfo, +} + +impl Default for TopicInfos { + fn default() -> Self { + Self { + low_state: TopicInfo::new("booster/low_state"), + joint_ctrl: TopicInfo::new("booster/joint_ctrl"), + fall_down: TopicInfo::new("booster/fall_down_state"), + button_event: TopicInfo::new("booster/button_event"), + remote_controller_state: TopicInfo::new("booster/remote_controller_state"), + transform: TopicInfo::new("booster/tf"), + origin_left_raw: TopicInfo::new("booster/origin_left_image"), + origin_right_raw: TopicInfo::new("booster/origin_right_image"), + rectified_image: TopicInfo::new("booster/rectified_image"), + rectified_right_image: TopicInfo::new("booster/rectified_right_image"), + stereonet_depth: TopicInfo::new("booster/stereonet_depth"), + stereonet_depth_camera_info: TopicInfo::new("booster/stereonet_depth/camera_info"), + stereonet_visual: TopicInfo::new("booster/stereonet_visual"), + image_combine_raw: TopicInfo::new("booster/image_combine_raw"), + image_left_raw: TopicInfo::new("booster/image_left_raw"), + image_left_raw_camera_info: TopicInfo::new("booster/image_left_raw/camera_info"), + image_right_raw: TopicInfo::new("booster/image_right_raw"), + image_right_raw_camera_info: TopicInfo::new("booster/image_right_raw/camera_info"), + } + } +} + +struct TopicInfo { + pub name: &'static str, +} + +impl TopicInfo { + const fn new(name: &'static str) -> Self { + TopicInfo { name } + } +} + +#[derive(Clone, Debug, Deserialize)] +pub struct Parameters { + pub dds_domain_id: u16, + pub hsl_network_ports: Ports, + pub paths: Paths, +} + +pub struct BoosterHardwareInterface { + paths: Paths, + enable_recording: AtomicBool, + + low_state_subscriber: Subscriber>, + joint_control_publisher: Publisher<'static>, + fall_down_state_subscriber: Subscriber>, + button_event_msg_subscriber: Subscriber>, + remote_controller_state_subscriber: Subscriber>, + transform_subscriber: Subscriber>, + rectified_image_subscriber: Subscriber>, + rectified_right_image_subscriber: Subscriber>, + origin_left_raw_subscriber: Subscriber>, + origin_right_raw_subscriber: Subscriber>, + stereonet_depth_subscriber: Subscriber>, + stereonet_depth_camera_info_subscriber: Subscriber>, + stereonet_visual_subscriber: Subscriber>, + image_combine_raw_subscriber: Subscriber>, + image_left_raw_subscriber: Subscriber>, + image_left_raw_camera_info_subscriber: Subscriber>, + image_right_raw_subscriber: Subscriber>, + image_right_raw_camera_info_subscriber: Subscriber>, + + _session: Session, + runtime_handle: Handle, + hsl_network_endpoint: Endpoint, + keep_running: CancellationToken, +} + +impl BoosterHardwareInterface { + pub async fn new( + runtime_handle: Handle, + keep_running: CancellationToken, + parameters: Parameters, + ) -> Result { + let session = zenoh::open(zenoh::Config::default()) + .await + .expect("failed to open zenoh session"); + + let topic_infos = TopicInfos::default(); + + Ok(Self { + paths: parameters.paths, + enable_recording: AtomicBool::new(false), + + low_state_subscriber: declare_subscriber(&session, &topic_infos.low_state).await?, + joint_control_publisher: declare_publisher(&session, &topic_infos.joint_ctrl).await?, + fall_down_state_subscriber: declare_subscriber(&session, &topic_infos.fall_down) + .await?, + button_event_msg_subscriber: declare_subscriber(&session, &topic_infos.button_event) + .await?, + remote_controller_state_subscriber: declare_subscriber( + &session, + &topic_infos.remote_controller_state, + ) + .await?, + transform_subscriber: declare_subscriber(&session, &topic_infos.transform).await?, + rectified_image_subscriber: declare_subscriber(&session, &topic_infos.rectified_image) + .await?, + rectified_right_image_subscriber: declare_subscriber( + &session, + &topic_infos.rectified_right_image, + ) + .await?, + origin_left_raw_subscriber: declare_subscriber(&session, &topic_infos.origin_left_raw) + .await?, + origin_right_raw_subscriber: declare_subscriber( + &session, + &topic_infos.origin_right_raw, + ) + .await?, + stereonet_depth_subscriber: declare_subscriber(&session, &topic_infos.stereonet_depth) + .await?, + stereonet_depth_camera_info_subscriber: declare_subscriber( + &session, + &topic_infos.stereonet_depth_camera_info, + ) + .await?, + stereonet_visual_subscriber: declare_subscriber( + &session, + &topic_infos.stereonet_visual, + ) + .await?, + image_combine_raw_subscriber: declare_subscriber( + &session, + &topic_infos.image_combine_raw, + ) + .await?, + image_left_raw_subscriber: declare_subscriber(&session, &topic_infos.image_left_raw) + .await?, + image_left_raw_camera_info_subscriber: declare_subscriber( + &session, + &topic_infos.image_left_raw_camera_info, + ) + .await?, + image_right_raw_subscriber: declare_subscriber(&session, &topic_infos.image_right_raw) + .await?, + image_right_raw_camera_info_subscriber: declare_subscriber( + &session, + &topic_infos.image_right_raw_camera_info, + ) + .await?, + + _session: session, + hsl_network_endpoint: keep_running + .clone() + .run_until_cancelled(Endpoint::new(parameters.hsl_network_ports)) + .await + .ok_or(eyre!("termination requested"))? + .wrap_err("failed to initialize HSL network")?, + runtime_handle, + keep_running, + }) + } + + fn run_until_cancelled(&self, fut: impl Future) -> Result { + self.runtime_handle + .clone() + .block_on(self.keep_running.run_until_cancelled(fut)) + .ok_or(eyre!("termination_requested")) + } +} + +async fn declare_subscriber( + session: &Session, + topic_info: &TopicInfo, +) -> Result>> { + session + .declare_subscriber(topic_info.name) + .with(RingChannel::new(10)) + .await + .map_err(|err| eyre!(err)) +} + +async fn declare_publisher( + session: &Session, + topic_info: &TopicInfo, +) -> Result> { + let key_expression = session + .declare_keyexpr(topic_info.name) + .await + .map_err(|err| eyre!(err))?; + + session + .declare_publisher(key_expression) + .await + .map_err(|err| eyre!(err)) +} + +fn deserialize_sample(sample: Sample) -> Result +where + for<'de> T: Deserialize<'de>, +{ + let deserialized_message = cdr::deserialize(&sample.payload().to_bytes())?; + Ok(deserialized_message) +} + +fn serialize_sample(payload: T) -> Result +where + T: Serialize, +{ + let message = cdr::serialize::<_, _, CdrLe>(&payload, Infinite)?; + + Ok(ZBytes::from(message)) +} + +impl LowStateInterface for BoosterHardwareInterface { + fn read_low_state(&self) -> Result { + self.run_until_cancelled(self.low_state_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } +} + +impl LowCommandInterface for BoosterHardwareInterface { + fn write_low_command(&self, low_command: LowCommand) -> Result<()> { + let payload = serialize_sample(low_command)?; + + self.run_until_cancelled(self.joint_control_publisher.put(payload).into_future())? + .map_err(|error| eyre!(error)) + } +} + +impl FallDownStateInterface for BoosterHardwareInterface { + fn read_fall_down_state(&self) -> Result { + self.run_until_cancelled(self.fall_down_state_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } +} + +impl ButtonEventMsgInterface for BoosterHardwareInterface { + fn read_button_event_msg(&self) -> Result { + self.run_until_cancelled(self.button_event_msg_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } +} + +impl TransformMessageInterface for BoosterHardwareInterface { + fn read_transform_message(&self) -> Result { + self.run_until_cancelled(self.transform_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } +} + +impl RemoteControllerStateInterface for BoosterHardwareInterface { + fn read_remote_controller_state(&self) -> Result { + self.run_until_cancelled(self.remote_controller_state_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } +} + +impl CameraInterface for BoosterHardwareInterface { + fn read_rectified_image(&self) -> Result { + self.run_until_cancelled(self.rectified_image_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_rectified_right_image(&self) -> Result { + self.run_until_cancelled(self.rectified_right_image_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_origin_left_image(&self) -> Result { + self.run_until_cancelled(self.origin_left_raw_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_origin_right_image(&self) -> Result { + self.run_until_cancelled(self.origin_right_raw_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_stereonet_depth_image(&self) -> Result { + self.run_until_cancelled(self.stereonet_depth_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_stereonet_depth_camera_info(&self) -> Result { + self.run_until_cancelled(self.stereonet_depth_camera_info_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_stereonet_visual_image(&self) -> Result { + self.run_until_cancelled(self.stereonet_visual_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_image_combine_raw(&self) -> Result { + self.run_until_cancelled(self.image_combine_raw_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_image_left_raw(&self) -> Result { + self.run_until_cancelled(self.image_left_raw_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_image_left_raw_camera_info(&self) -> Result { + self.run_until_cancelled(self.image_left_raw_camera_info_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_image_right_raw(&self) -> Result { + self.run_until_cancelled(self.image_right_raw_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } + + fn read_image_right_raw_camera_info(&self) -> Result { + self.run_until_cancelled(self.image_right_raw_camera_info_subscriber.recv_async())? + .map_err(|error| eyre!(error)) + .and_then(deserialize_sample) + } +} + +impl TimeInterface for BoosterHardwareInterface { + fn get_now(&self) -> SystemTime { + SystemTime::now() + } +} + +impl PathsInterface for BoosterHardwareInterface { + fn get_paths(&self) -> Paths { + self.paths.clone() + } +} + +impl NetworkInterface for BoosterHardwareInterface { + fn read_from_network(&self) -> Result { + self.run_until_cancelled(self.hsl_network_endpoint.read())? + .wrap_err("failed to read from network") + } + + fn write_to_network(&self, message: OutgoingMessage) -> Result<()> { + self.run_until_cancelled(self.hsl_network_endpoint.write(message)) + } +} + +impl SpeakerInterface for BoosterHardwareInterface { + fn write_to_speakers(&self, _request: SpeakerRequest) { + log::warn!("tried to play audio request, not implemented!") + } +} + +impl IdInterface for BoosterHardwareInterface { + fn get_ids(&self) -> Ids { + let name = "Booster K1"; + Ids { + body_id: name.to_string(), + head_id: name.to_string(), + } + } +} + +impl MicrophoneInterface for BoosterHardwareInterface { + fn read_from_microphones(&self) -> Result { + bail!("microphone interface is not implemented") + } +} + +impl RecordingInterface for BoosterHardwareInterface { + 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 BoosterHardwareInterface {} diff --git a/crates/hulk_booster/src/main.rs b/crates/hulk_booster/src/main.rs index 2983f4a82f..285c853438 100644 --- a/crates/hulk_booster/src/main.rs +++ b/crates/hulk_booster/src/main.rs @@ -1,70 +1,124 @@ -use byteorder::{BigEndian, LittleEndian}; -use bytes::Bytes; -use cdr_encoding::CdrDeserializer; -use color_eyre::eyre::{eyre, Context, Result}; -use flume::bounded; -use ros2::geometry_msgs::transform::Transform; -use serde::Deserialize; +#![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::{ + ButtonEventMsgInterface, CameraInterface, FallDownStateInterface, IdInterface, + LowCommandInterface, LowStateInterface, MicrophoneInterface, NetworkInterface, PathsInterface, + RecordingInterface, SpeakerInterface, TimeInterface, TransformMessageInterface, +}; +use hula_types::hardware::Ids; +use serde_json::from_reader; +use tokio_util::sync::CancellationToken; + +use crate::execution::run; +use crate::hardware_interface::{BoosterHardwareInterface, Parameters as HardwareParameters}; + +mod hardware_interface; pub fn setup_logger() -> Result<(), fern::InitError> { - env_logger::init(); + 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(Deserialize)] -struct DDSDataWrapper { - representation_identifier: [u8; 2], - representation_options: [u8; 2], - bytes: Bytes, +pub trait HardwareInterface: + IdInterface + + LowStateInterface + + LowCommandInterface + + CameraInterface + + FallDownStateInterface + + ButtonEventMsgInterface + + TransformMessageInterface + + MicrophoneInterface + + NetworkInterface + + PathsInterface + + RecordingInterface + + SpeakerInterface + + TimeInterface +{ } -impl DDSDataWrapper { - fn new(bytes: &[u8]) -> Self { - Self { - representation_identifier: bytes[0..2].try_into().unwrap(), - representation_options: bytes[2..4].try_into().unwrap(), - bytes: bytes[4..].to_owned().into(), - } - } +include!(concat!(env!("OUT_DIR"), "/generated_code.rs")); + +#[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, } #[tokio::main(flavor = "multi_thread")] async fn main() -> Result<()> { setup_logger()?; - let session = zenoh::open(zenoh::Config::default()).await.unwrap(); - - println!("{}", zenoh::Config::default()); - - log::info!("Creating subscriber..."); - let subscriber = session - .declare_subscriber("**") - .with(bounded(1000)) - .await - .unwrap(); - log::info!("Created subscriber..."); - loop { - let sample = subscriber - .recv_async() - .await - .map_err(|err| eyre!(err.to_string())) - .wrap_err("recv failed")?; - log::info!("{}", sample.key_expr().as_str()); - if sample.key_expr().contains("tf") { - let ddsdata_wrapper = DDSDataWrapper::new(&sample.payload().to_bytes()); - let message: Option = match ddsdata_wrapper.representation_identifier { - [0x00, 0x01] => { - let mut deserializer = - CdrDeserializer::::new(&ddsdata_wrapper.bytes); - Some(serde::de::Deserialize::deserialize(&mut deserializer).unwrap()) - } - [0x00, 0x00] => { - let mut deserializer = - CdrDeserializer::::new(&ddsdata_wrapper.bytes); - Some(serde::de::Deserialize::deserialize(&mut deserializer).unwrap()) - } - _ => None, - }; - println!("{:?}", message); - }; + install()?; + + let arguments = Arguments::parse(); + let framework_parameters_path = Path::new(&arguments.framework_parameters_path); + + let keep_running = CancellationToken::new(); + set_handler({ + let keep_running = keep_running.clone(); + move || { + keep_running.cancel(); + } + })?; + + let file = + File::open(framework_parameters_path).wrap_err("failed to open framework parameters")?; + let mut 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")?; + + if framework_parameters.communication_addresses.is_none() { + let fallback = "127.0.0.1:1337"; + log::warn!("framework.json disabled communication, falling back to {fallback}"); + framework_parameters.communication_addresses = Some(fallback.to_string()); } + + let runtime_handle = tokio::runtime::Handle::current(); + let hardware_interface = + BoosterHardwareInterface::new(runtime_handle, keep_running.clone(), hardware_parameters) + .await?; + + run( + Arc::new(hardware_interface), + framework_parameters.communication_addresses, + framework_parameters.parameters_directory, + "logs", + Ids { + body_id: "K1_BODY".to_string(), + head_id: "K1_HEAD".to_string(), + }, + keep_running, + framework_parameters.recording_intervals, + ) } diff --git a/crates/hulk_imagine/Cargo.toml b/crates/hulk_imagine/Cargo.toml index b3a74edcb4..be4c7cd7e4 100644 --- a/crates/hulk_imagine/Cargo.toml +++ b/crates/hulk_imagine/Cargo.toml @@ -35,6 +35,7 @@ path_serde = { workspace = true } projection = { workspace = true } rmp-serde = { workspace = true } ros2 = { workspace = true } +sensor_receiver = { workspace = true } serde = { workspace = true } serde_json = { workspace = true } step_planning = { workspace = true } diff --git a/crates/hulk_manifest/src/lib.rs b/crates/hulk_manifest/src/lib.rs index 0e93bfef9c..d471e4cb58 100644 --- a/crates/hulk_manifest/src/lib.rs +++ b/crates/hulk_manifest/src/lib.rs @@ -126,6 +126,22 @@ pub fn collect_hulk_cyclers(root: impl AsRef) -> Result { // nodes: vec!["audio::whistle_detection"], // execution_time_warning_threshold: None, // }, + CyclerManifest { + name: "Image", + kind: CyclerKind::Perception, + instances: vec!["Rectified", "StereonetDepth"], + setup_nodes: vec!["sensor_receiver::image_receiver"], + nodes: vec![], + execution_time_warning_threshold: None, + }, + CyclerManifest { + name: "FallDownState", + kind: CyclerKind::Perception, + instances: vec![""], + setup_nodes: vec!["sensor_receiver::fall_down_state_receiver"], + nodes: vec![], + execution_time_warning_threshold: None, + }, ], }; diff --git a/crates/hulk_mujoco/Cargo.toml b/crates/hulk_mujoco/Cargo.toml index 49298ff400..f6958b4060 100644 --- a/crates/hulk_mujoco/Cargo.toml +++ b/crates/hulk_mujoco/Cargo.toml @@ -46,6 +46,7 @@ parking_lot = { workspace = true } path_serde = { workspace = true } projection = { workspace = true } ros2 = { workspace = true } +sensor_receiver = { workspace = true } serde = { workspace = true } serde_json = { workspace = true } simulation_message = { workspace = true } diff --git a/crates/hulk_mujoco/src/hardware_interface.rs b/crates/hulk_mujoco/src/hardware_interface.rs index 71f34a31bb..22543f8d8e 100644 --- a/crates/hulk_mujoco/src/hardware_interface.rs +++ b/crates/hulk_mujoco/src/hardware_interface.rs @@ -11,11 +11,10 @@ use futures_util::SinkExt; use futures_util::StreamExt; use hardware::{ ButtonEventMsgInterface, CameraInterface, IdInterface, MicrophoneInterface, NetworkInterface, - PathsInterface, RecordingInterface, SpeakerInterface, TimeInterface, + PathsInterface, RecordingInterface, SpeakerInterface, TimeInterface, TransformMessageInterface, }; use hardware::{ FallDownStateInterface, LowCommandInterface, LowStateInterface, RemoteControllerStateInterface, - TransformMessageInterface, }; use hsl_network::endpoint::{Endpoint, Ports}; use hula_types::hardware::{Ids, Paths}; @@ -62,7 +61,7 @@ pub struct MujocoHardwareInterface { low_state_receiver: Mutex>, low_command_sender: Sender, - fall_down_receiver: Mutex>, + _fall_down_receiver: Mutex>, button_event_msg_receiver: Mutex>, remote_controller_state_receiver: Mutex>, transform_stamped_receiver: Mutex>, @@ -114,7 +113,7 @@ impl MujocoHardwareInterface { low_state_receiver: Mutex::new(low_state_receiver), low_command_sender, - fall_down_receiver: Mutex::new(fall_down_receiver), + _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), @@ -193,7 +192,7 @@ async fn handle_message( time, } => { *hardware_interface_time.lock() = time; - worker_channels.low_state_sender.send(low_state).await? + worker_channels.low_state_sender.send(*low_state).await? } SimulatorMessage { payload: ServerMessageKind::FallDownState(fall_down_state), @@ -269,6 +268,15 @@ impl LowStateInterface for MujocoHardwareInterface { } } +impl TransformMessageInterface for MujocoHardwareInterface { + fn read_transform_message(&self) -> Result { + self.transform_stamped_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 @@ -279,10 +287,7 @@ impl LowCommandInterface for MujocoHardwareInterface { 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") + Ok(Default::default()) } } @@ -304,29 +309,60 @@ impl RemoteControllerStateInterface for MujocoHardwareInterface { } } -impl TransformMessageInterface for MujocoHardwareInterface { - fn read_transform_message(&self) -> Result { - self.transform_stamped_receiver - .lock() - .blocking_recv() - .ok_or_eyre("channel closed") - } -} - impl CameraInterface for MujocoHardwareInterface { - fn read_image(&self) -> Result { + fn read_image_left_raw(&self) -> Result { self.image_receiver .lock() .blocking_recv() .ok_or_eyre("channel closed") } - fn read_camera_info(&self) -> Result { + fn read_image_left_raw_camera_info(&self) -> Result { self.camera_info_receiver .lock() .blocking_recv() .ok_or_eyre("channel closed") } + + fn read_rectified_image(&self) -> Result { + Ok(Default::default()) + } + + fn read_rectified_right_image(&self) -> Result { + Ok(Default::default()) + } + + fn read_stereonet_depth_image(&self) -> Result { + Ok(Default::default()) + } + + fn read_stereonet_visual_image(&self) -> Result { + Ok(Default::default()) + } + + fn read_image_right_raw(&self) -> Result { + Ok(Default::default()) + } + + fn read_image_right_raw_camera_info(&self) -> Result { + Ok(Default::default()) + } + + fn read_origin_left_image(&self) -> Result { + Ok(Default::default()) + } + + fn read_origin_right_image(&self) -> Result { + Ok(Default::default()) + } + + fn read_stereonet_depth_camera_info(&self) -> Result { + Ok(Default::default()) + } + + fn read_image_combine_raw(&self) -> Result { + Ok(Default::default()) + } } impl TimeInterface for MujocoHardwareInterface { diff --git a/crates/hulk_mujoco/src/main.rs b/crates/hulk_mujoco/src/main.rs index 52ec6a4912..7f7f9c0176 100644 --- a/crates/hulk_mujoco/src/main.rs +++ b/crates/hulk_mujoco/src/main.rs @@ -8,8 +8,9 @@ use color_eyre::{ use ctrlc::set_handler; use framework::Parameters as FrameworkParameters; use hardware::{ - CameraInterface, IdInterface, LowCommandInterface, LowStateInterface, MicrophoneInterface, - NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface, TimeInterface, + CameraInterface, FallDownStateInterface, IdInterface, LowCommandInterface, LowStateInterface, + MicrophoneInterface, NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface, + TimeInterface, TransformMessageInterface, }; use hula_types::hardware::Ids; use serde_json::from_reader; @@ -42,6 +43,8 @@ pub trait HardwareInterface: IdInterface + LowStateInterface + LowCommandInterface + + FallDownStateInterface + + TransformMessageInterface + CameraInterface + MicrophoneInterface + NetworkInterface diff --git a/crates/hulk_replayer/Cargo.toml b/crates/hulk_replayer/Cargo.toml index 0357abc4e3..20261a1d9f 100644 --- a/crates/hulk_replayer/Cargo.toml +++ b/crates/hulk_replayer/Cargo.toml @@ -41,6 +41,7 @@ parameters = { workspace = true } path_serde = { workspace = true } projection = { workspace = true } ros2 = { workspace = true } +sensor_receiver = { workspace = true } serde = { workspace = true } serde_json = { workspace = true } step_planning = { workspace = true } diff --git a/crates/hulk_replayer/src/main.rs b/crates/hulk_replayer/src/main.rs index 3a727e5a51..eb2344e366 100644 --- a/crates/hulk_replayer/src/main.rs +++ b/crates/hulk_replayer/src/main.rs @@ -62,10 +62,51 @@ impl ActuatorInterface for ReplayerHardwareInterface { } impl CameraInterface for ReplayerHardwareInterface { - fn read_image(&self) -> Result { + fn read_rectified_image(&self) -> Result { unimplemented!("Replayer cannot produce data from hardware") } - fn read_camera_info(&self) -> Result { + + fn read_rectified_right_image(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_stereonet_depth_image(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_stereonet_visual_image(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_image_left_raw(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_image_left_raw_camera_info(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_image_right_raw(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_image_right_raw_camera_info(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_origin_left_image(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_origin_right_image(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_stereonet_depth_camera_info(&self) -> Result { + unimplemented!("Replayer cannot produce data from hardware") + } + + fn read_image_combine_raw(&self) -> Result { unimplemented!("Replayer cannot produce data from hardware") } } diff --git a/crates/object_detection/Cargo.toml b/crates/object_detection/Cargo.toml index 6fc044fe0f..7b79bc1997 100644 --- a/crates/object_detection/Cargo.toml +++ b/crates/object_detection/Cargo.toml @@ -19,6 +19,7 @@ hsl_network_messages = { workspace = true } image = { workspace = true } itertools = { workspace = true } linear_algebra = { workspace = true } +log = { workspace = true } nalgebra = { workspace = true } ndarray = { workspace = true } openvino = { workspace = true } diff --git a/crates/object_detection/src/image_receiver.rs b/crates/object_detection/src/image_receiver.rs index 8bfb8f3ffc..840388a5cd 100644 --- a/crates/object_detection/src/image_receiver.rs +++ b/crates/object_detection/src/image_receiver.rs @@ -23,8 +23,8 @@ pub struct CycleContext { #[context] pub struct MainOutputs { - pub image: MainOutput, - pub camera_info: MainOutput, + pub image_left_raw: MainOutput, + pub image_left_raw_camera_info: MainOutput, pub cycle_time: MainOutput, } @@ -39,8 +39,10 @@ impl ImageReceiver { &mut self, context: CycleContext, ) -> Result { - let image = context.hardware_interface.read_image()?; - let camera_info = context.hardware_interface.read_camera_info()?; + let image_left_raw = context.hardware_interface.read_image_left_raw()?; + let image_left_raw_camera_info = context + .hardware_interface + .read_image_left_raw_camera_info()?; let now = context.hardware_interface.get_now(); let cycle_time = CycleTime { @@ -52,8 +54,8 @@ impl ImageReceiver { self.last_cycle_start = now; Ok(MainOutputs { - image: image.into(), - camera_info: camera_info.into(), + image_left_raw: image_left_raw.into(), + image_left_raw_camera_info: image_left_raw_camera_info.into(), cycle_time: cycle_time.into(), }) } diff --git a/crates/object_detection/src/object_detection.rs b/crates/object_detection/src/object_detection.rs index 20b5b61c89..02688e2268 100644 --- a/crates/object_detection/src/object_detection.rs +++ b/crates/object_detection/src/object_detection.rs @@ -1,7 +1,4 @@ -use color_eyre::{ - eyre::{eyre, Ok}, - Result, -}; +use color_eyre::Result; use context_attribute::context; use framework::{deserialize_not_implemented, MainOutput}; use geometry::rectangle::Rectangle; @@ -36,8 +33,8 @@ pub struct CreationContext { #[context] pub struct CycleContext { - image: Input, - camera_info: Input, + image_left_raw: Input, + image_left_raw_camera_info: Input, parameters: Parameter, } @@ -55,10 +52,15 @@ impl ObjectDetection { let session = Session::builder()? .with_execution_providers([ - TensorRTExecutionProvider::default().build(), + TensorRTExecutionProvider::default() + .with_engine_cache(true) + .with_engine_cache_path( + neural_network_folder.join("./trt_engine_cache").display(), + ) + .build(), CUDAExecutionProvider::default().build(), ])? - .commit_from_file(neural_network_folder.join("yolo11n.onnx"))?; + .commit_from_file(neural_network_folder.join("yolo11n-544x448.onnx"))?; Ok(Self { session }) } @@ -68,12 +70,11 @@ impl ObjectDetection { return Ok(MainOutputs::default()); } - let height = context.camera_info.height; - let width = context.camera_info.width; - let rgb_image = if context.image.encoding == "rgb8" { - RgbImage::from_raw(width, height, context.image.data.clone()).unwrap() - } else { - return Err(eyre!("unsupported image encoding")); + let height = context.image_left_raw_camera_info.height; + let width = context.image_left_raw_camera_info.width; + + let Ok(rgb_image): Result = context.image_left_raw.clone().try_into() else { + return Ok(MainOutputs::default()); }; let mut input = Array::zeros((1, 3, height as usize, width as usize)); diff --git a/crates/object_detection/src/pose_detection.rs b/crates/object_detection/src/pose_detection.rs index 6b32db82e0..784d0a6598 100644 --- a/crates/object_detection/src/pose_detection.rs +++ b/crates/object_detection/src/pose_detection.rs @@ -57,7 +57,7 @@ pub struct CycleContext { inference_duration: AdditionalOutput, postprocess_duration: AdditionalOutput, - image: Input, + image_left_raw: Input, motion_command: Input, maximum_intersection_over_union: @@ -161,7 +161,7 @@ impl PoseDetection { return Ok(MainOutputs::default()); }; - let image = context.image; + let image = context.image_left_raw; let mut infer_request = match image_size_used_for_detection { DetectionRegion::Narrow => { diff --git a/crates/path_serde/src/not_supported.rs b/crates/path_serde/src/not_supported.rs index 1432598448..a6802a6da0 100644 --- a/crates/path_serde/src/not_supported.rs +++ b/crates/path_serde/src/not_supported.rs @@ -124,6 +124,7 @@ implement_as_not_supported!(bool); implement_as_not_supported!(f32); implement_as_not_supported!(i16); implement_as_not_supported!(i32); +implement_as_not_supported!(i8); implement_as_not_supported!(u8); implement_as_not_supported!(u16); implement_as_not_supported!(u32); diff --git a/crates/ros2/Cargo.toml b/crates/ros2/Cargo.toml index 540918da57..770f883721 100644 --- a/crates/ros2/Cargo.toml +++ b/crates/ros2/Cargo.toml @@ -12,6 +12,7 @@ nalgebra = { workspace = true } path_serde = { workspace = true } pyo3 = { workspace = true, optional = true } serde = { workspace = true } +yuv = { workspace = true } [features] pyo3 = ["dep:pyo3"] diff --git a/crates/ros2/src/builtin_interfaces/time.rs b/crates/ros2/src/builtin_interfaces/time.rs index 6297f32263..127355c481 100644 --- a/crates/ros2/src/builtin_interfaces/time.rs +++ b/crates/ros2/src/builtin_interfaces/time.rs @@ -1,8 +1,11 @@ -/// This message communicates ROS Time defined here: -/// https://design.ros2.org/articles/clock_and_time.html +use color_eyre::Result; +use std::time::{Duration, SystemTime}; + use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; use serde::{Deserialize, Serialize}; +/// This message communicates ROS Time defined here: +/// https://design.ros2.org/articles/clock_and_time.html #[repr(C)] #[derive( Clone, Debug, Default, Serialize, Deserialize, PathIntrospect, PathSerialize, PathDeserialize, @@ -17,3 +20,23 @@ pub struct Time { /// The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8} pub nanosec: u32, } + +impl From