hulk_mujoco: impl RGBDSensorsInterface

Author: alexschmander

Cargo.lock

@@ -42,7 +42,8 @@ members = [
   "crates/projection",
   "crates/ransac",
   "crates/repository", 
-  "crates/ros2",
+  "crates/ros2", 
+  "crates/simulation-message",
   "crates/source_analyzer",
   "crates/spl_network",
   "crates/spl_network_messages",
@@ -215,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" }

Cargo.toml

@@ -11,4 +11,5 @@ color-eyre = { workspace = true }
 hula_types = { workspace = true }
 ros2 = { workspace = true }
 simulation-message = { workspace = true }
-types = { workspace = true }
+types = { workspace = true }
+zed = { workspace = true }

crates/hardware/Cargo.toml

@@ -16,6 +16,7 @@ use types::{
     sensor_data::SensorData,
     ycbcr422_image::YCbCr422Image,
 };
+use zed::RGBDSensors;
 
 pub trait ActuatorInterface {
     fn write_to_actuators(
@@ -77,6 +78,10 @@ pub trait TransformStampedInterface {
     fn read_transform_stamped(&self) -> Result<SimulationMessage<TransformStamped>>;
 }
 
+pub trait RGBDSensorsInterface {
+    fn read_rgbd_sensors(&self) -> Result<SimulationMessage<RGBDSensors>>;
+}
+
 pub trait SpeakerInterface {
     fn write_to_speakers(&self, request: SpeakerRequest);
 }

crates/hardware/src/lib.rs

@@ -8,8 +8,8 @@ use color_eyre::Result;
 use futures_util::SinkExt;
 use futures_util::StreamExt;
 use hardware::{
-    ButtonEventMsgInterface, CameraInterface, IdInterface, MicrophoneInterface, NetworkInterface,
-    PathsInterface, RecordingInterface, SpeakerInterface, TimeInterface,
+    ButtonEventMsgInterface, IdInterface, MicrophoneInterface, NetworkInterface, PathsInterface,
+    RGBDSensorsInterface, RecordingInterface, SpeakerInterface, TimeInterface,
 };
 use hardware::{
     FallDownStateInterface, LowCommandInterface, LowStateInterface, RemoteControllerStateInterface,
@@ -29,10 +29,9 @@ use tokio::{
 use tokio_tungstenite::tungstenite::Message;
 use tokio_util::sync::CancellationToken;
 use types::audio::SpeakerRequest;
-use types::camera_position::CameraPosition;
 use types::messages::{IncomingMessage, OutgoingMessage};
 use types::samples::Samples;
-use types::ycbcr422_image::YCbCr422Image;
+use zed::RGBDSensors;
 
 use crate::HardwareInterface;
 
@@ -45,6 +44,7 @@ struct WorkerChannels {
     button_event_msg_sender: Sender<SimulationMessage<ButtonEventMsg>>,
     remote_controller_state_sender: Sender<SimulationMessage<RemoteControllerState>>,
     transform_stamped_sender: Sender<SimulationMessage<TransformStamped>>,
+    rgbd_sensors_sender: Sender<SimulationMessage<RGBDSensors>>,
 }
 
 #[derive(Clone, Debug, Deserialize)]
@@ -68,6 +68,7 @@ pub struct MujocoHardwareInterface {
     button_event_msg_receiver: Mutex<Receiver<SimulationMessage<ButtonEventMsg>>>,
     remote_controller_state_receiver: Mutex<Receiver<SimulationMessage<RemoteControllerState>>>,
     transform_stamped_receiver: Mutex<Receiver<SimulationMessage<TransformStamped>>>,
+    rgbd_sensors_receiver: Mutex<Receiver<SimulationMessage<RGBDSensors>>>,
 }
 
 impl MujocoHardwareInterface {
@@ -84,6 +85,7 @@ impl MujocoHardwareInterface {
         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,
@@ -92,6 +94,7 @@ impl MujocoHardwareInterface {
             button_event_msg_sender,
             remote_controller_state_sender,
             transform_stamped_sender,
+            rgbd_sensors_sender,
         };
 
         let time = Arc::new(Mutex::new(SystemTime::UNIX_EPOCH));
@@ -118,6 +121,7 @@ impl MujocoHardwareInterface {
             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),
         })
     }
 }
@@ -221,9 +225,15 @@ async fn handle_message(
                 .await?
         }
         SimulationMessage {
-            payload: ServerMessageKind::RGBDSensors(_),
-            ..
-        } => todo!(),
+            payload: ServerMessageKind::RGBDSensors(rgbd_sensors),
+            time,
+        } => {
+            *hardware_interface_time.lock() = time;
+            worker_channels
+                .rgbd_sensors_sender
+                .send(SimulationMessage::new(time, rgbd_sensors))
+                .await?
+        }
     };
 
     Ok(())
@@ -234,15 +244,15 @@ impl LowStateInterface for MujocoHardwareInterface {
         self.low_state_receiver
             .lock()
             .blocking_recv()
-            .ok_or_eyre("channel closed")
+            .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("send error")
+            .wrap_err("low command send error")
     }
 }
 
@@ -251,7 +261,7 @@ impl FallDownStateInterface for MujocoHardwareInterface {
         self.fall_down_receiver
             .lock()
             .blocking_recv()
-            .ok_or_eyre("channel closed")
+            .ok_or_eyre("fall down state channel closed")
     }
 }
 
@@ -260,7 +270,7 @@ impl ButtonEventMsgInterface for MujocoHardwareInterface {
         self.button_event_msg_receiver
             .lock()
             .blocking_recv()
-            .ok_or_eyre("channel closed")
+            .ok_or_eyre("button event msg channel closed")
     }
 }
 
@@ -282,6 +292,15 @@ impl TransformStampedInterface for MujocoHardwareInterface {
     }
 }
 
+impl RGBDSensorsInterface for MujocoHardwareInterface {
+    fn read_rgbd_sensors(&self) -> Result<SimulationMessage<RGBDSensors>> {
+        self.rgbd_sensors_receiver
+            .lock()
+            .blocking_recv()
+            .ok_or_eyre("channel closed")
+    }
+}
+
 impl TimeInterface for MujocoHardwareInterface {
     fn get_now(&self) -> SystemTime {
         *self.time.lock()
@@ -348,10 +367,4 @@ impl RecordingInterface for MujocoHardwareInterface {
     }
 }
 
-impl CameraInterface for MujocoHardwareInterface {
-    fn read_from_camera(&self, camera_position: CameraPosition) -> Result<YCbCr422Image> {
-        Ok(YCbCr422Image::default())
-    }
-}
-
 impl HardwareInterface for MujocoHardwareInterface {}

crates/hulk_mujoco/src/hardware_interface.rs

@@ -8,8 +8,8 @@ use color_eyre::{
 use ctrlc::set_handler;
 use framework::Parameters as FrameworkParameters;
 use hardware::{
-    CameraInterface, IdInterface, LowCommandInterface, LowStateInterface, MicrophoneInterface,
-    NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface, TimeInterface,
+    IdInterface, LowCommandInterface, LowStateInterface, MicrophoneInterface, NetworkInterface,
+    PathsInterface, RGBDSensorsInterface, RecordingInterface, SpeakerInterface, TimeInterface,
 };
 use hula_types::hardware::Ids;
 use serde_json::from_reader;
@@ -38,10 +38,10 @@ pub fn setup_logger() -> Result<(), fern::InitError> {
 }
 
 pub trait HardwareInterface:
-    CameraInterface
-    + IdInterface
+    IdInterface
     + LowStateInterface
     + LowCommandInterface
+    + RGBDSensorsInterface
     + MicrophoneInterface
     + NetworkInterface
     + PathsInterface

crates/hulk_mujoco/src/main.rs

@@ -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 }
+splines = { workspace = true }

crates/types/Cargo.toml

@@ -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<YCbCr422Image> 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!(

crates/types/src/ycbcr422_image.rs

@@ -26,4 +26,5 @@ rand = { workspace = true }
 rand_chacha = { workspace = true }
 ransac = { workspace = true }
 serde = { workspace = true }
+simulation-message = { workspace = true }
 types = { workspace = true }

crates/vision/Cargo.toml

@@ -3,8 +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 simulation_message::SimulationMessage;
 use types::{
     camera_position::CameraPosition, cycle_time::CycleTime, ycbcr422_image::YCbCr422Image,
 };
@@ -38,11 +39,10 @@ impl ImageReceiver {
 
     pub fn cycle(
         &mut self,
-        context: CycleContext<impl CameraInterface + TimeInterface>,
+        context: CycleContext<impl RGBDSensorsInterface + TimeInterface>,
     ) -> Result<MainOutputs> {
-        let image = context
-            .hardware_interface
-            .read_from_camera(*context.camera_position)?;
+        let SimulationMessage { payload, .. } = context.hardware_interface.read_rgbd_sensors()?;
+        let ycbcr422_image: YCbCr422Image = (&payload.rgb).into();
 
         let now = context.hardware_interface.get_now();
         let cycle_time = CycleTime {
@@ -54,7 +54,7 @@ impl ImageReceiver {
         self.last_cycle_start = now;
 
         Ok(MainOutputs {
-            image: image.into(),
+            image: ycbcr422_image.into(),
             cycle_time: cycle_time.into(),
         })
     }