Refactor SimulationMessage

Author: alexschmander

Cargo.lock

@@ -43,7 +43,6 @@ members = [
   "crates/ransac",
   "crates/repository", 
   "crates/ros2", 
-  "crates/simulation-message",
   "crates/source_analyzer",
   "crates/spl_network",
   "crates/spl_network_messages",
@@ -216,7 +215,6 @@ 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

@@ -36,7 +36,6 @@ proptest = { workspace = true }
 rand = { workspace = true }
 rand_chacha = { workspace = true }
 serde = { workspace = true }
-simulation-message = { workspace = true }
 smallvec = { workspace = true }
 spl_network_messages = { workspace = true }
 splines = { workspace = true }

crates/control/Cargo.toml

@@ -9,7 +9,6 @@ use hardware::{LowStateInterface, TimeInterface};
 use linear_algebra::Vector3;
 use nalgebra::UnitQuaternion;
 use serde::{Deserialize, Serialize};
-use simulation_message::SimulationMessage;
 use types::{
     cycle_time::CycleTime,
     joints::Joints,
@@ -77,13 +76,11 @@ impl SensorDataReceiver {
         &mut self,
         mut context: CycleContext<impl LowStateInterface + TimeInterface>,
     ) -> Result<MainOutputs> {
-        let SimulationMessage { time, payload } = context
+        let low_state = context
             .hardware_interface
             .read_low_state()
             .wrap_err("failed to read from sensors")?;
 
-        let low_state = payload;
-
         // let measured_angular_velocity = low_state.imu_state.angular_velocity;
         // let measured_acceleration = low_state.imu_state.linear_acceleration;
         // let measured_roll_pitch_yaw = low_state.imu_state.roll_pitch_yaw;

crates/control/src/sensor_data_receiver.rs

@@ -10,6 +10,5 @@ booster.workspace = true
 color-eyre = { workspace = true }
 hula_types = { workspace = true }
 ros2 = { workspace = true }
-simulation-message = { workspace = true }
 types = { workspace = true }
 zed = { workspace = true }

crates/hardware/Cargo.toml

@@ -5,7 +5,6 @@ use color_eyre::eyre::Result;
 
 use hula_types::hardware::{Ids, Paths};
 use ros2::geometry_msgs::transform_stamped::TransformStamped;
-use simulation_message::SimulationMessage;
 use types::{
     audio::SpeakerRequest,
     camera_position::CameraPosition,
@@ -58,28 +57,28 @@ pub trait SensorInterface {
 }
 
 pub trait LowStateInterface {
-    fn read_low_state(&self) -> Result<SimulationMessage<LowState>>;
+    fn read_low_state(&self) -> Result<LowState>;
 }
 
 pub trait LowCommandInterface {
     fn write_low_command(&self, low_command: LowCommand) -> Result<()>;
 }
 
 pub trait FallDownStateInterface {
-    fn read_fall_down_state(&self) -> Result<SimulationMessage<FallDownState>>;
+    fn read_fall_down_state(&self) -> Result<FallDownState>;
 }
 pub trait ButtonEventMsgInterface {
-    fn read_button_event_msg(&self) -> Result<SimulationMessage<ButtonEventMsg>>;
+    fn read_button_event_msg(&self) -> Result<ButtonEventMsg>;
 }
 pub trait RemoteControllerStateInterface {
-    fn read_remote_controller_state(&self) -> Result<SimulationMessage<RemoteControllerState>>;
+    fn read_remote_controller_state(&self) -> Result<RemoteControllerState>;
 }
 pub trait TransformStampedInterface {
-    fn read_transform_stamped(&self) -> Result<SimulationMessage<TransformStamped>>;
+    fn read_transform_stamped(&self) -> Result<TransformStamped>;
 }
 
 pub trait RGBDSensorsInterface {
-    fn read_rgbd_sensors(&self) -> Result<SimulationMessage<RGBDSensors>>;
+    fn read_rgbd_sensors(&self) -> Result<RGBDSensors>;
 }
 
 pub trait SpeakerInterface {

crates/hardware/src/lib.rs

@@ -47,7 +47,6 @@ serde_json.workspace = true
 serde.workspace = true
 spl_network_messages = { workspace = true }
 spl_network.workspace = true
-simulation-message = { workspace = true }
 tokio = { workspace = true }
 tokio-tungstenite.workspace = true
 tokio-util = { workspace = true }

crates/hulk_mujoco/Cargo.toml

@@ -20,7 +20,6 @@ use log::{error, warn};
 use parking_lot::Mutex;
 use ros2::geometry_msgs::transform_stamped::TransformStamped;
 use serde::Deserialize;
-use simulation_message::{ClientMessageKind, ServerMessageKind, SimulationMessage};
 use spl_network::endpoint::{Endpoint, Ports};
 use tokio::{
     runtime::{Builder, Runtime},
@@ -33,18 +32,19 @@ use types::messages::{IncomingMessage, OutgoingMessage};
 use types::samples::Samples;
 use zed::RGBDSensors;
 
+use crate::simulation_message::{ClientMessageKind, ServerMessageKind, SimulationMessage};
 use crate::HardwareInterface;
 
 const CHANNEL_CAPACITY: usize = 32;
 
 struct WorkerChannels {
-    low_state_sender: Sender<SimulationMessage<LowState>>,
+    low_state_sender: Sender<LowState>,
     low_command_receiver: Receiver<LowCommand>,
-    fall_down_sender: Sender<SimulationMessage<FallDownState>>,
-    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>>,
+    fall_down_sender: Sender<FallDownState>,
+    button_event_msg_sender: Sender<ButtonEventMsg>,
+    remote_controller_state_sender: Sender<RemoteControllerState>,
+    transform_stamped_sender: Sender<TransformStamped>,
+    rgbd_sensors_sender: Sender<RGBDSensors>,
 }
 
 #[derive(Clone, Debug, Deserialize)]
@@ -62,13 +62,13 @@ pub struct MujocoHardwareInterface {
     enable_recording: AtomicBool,
     time: Arc<Mutex<SystemTime>>,
 
-    low_state_receiver: Mutex<Receiver<SimulationMessage<LowState>>>,
+    low_state_receiver: Mutex<Receiver<LowState>>,
     low_command_sender: Sender<LowCommand>,
-    fall_down_receiver: Mutex<Receiver<SimulationMessage<FallDownState>>>,
-    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>>>,
+    fall_down_receiver: Mutex<Receiver<FallDownState>>,
+    button_event_msg_receiver: Mutex<Receiver<ButtonEventMsg>>,
+    remote_controller_state_receiver: Mutex<Receiver<RemoteControllerState>>,
+    transform_stamped_receiver: Mutex<Receiver<TransformStamped>>,
+    rgbd_sensors_receiver: Mutex<Receiver<RGBDSensors>>,
 }
 
 impl MujocoHardwareInterface {
@@ -180,10 +180,7 @@ async fn handle_message(
             time,
         } => {
             *hardware_interface_time.lock() = time;
-            worker_channels
-                .low_state_sender
-                .send(SimulationMessage::new(time, low_state))
-                .await?
+            worker_channels.low_state_sender.send(low_state).await?
         }
         SimulationMessage {
             payload: ServerMessageKind::FallDownState(fall_down_state),
@@ -192,16 +189,17 @@ async fn handle_message(
             *hardware_interface_time.lock() = time;
             worker_channels
                 .fall_down_sender
-                .send(SimulationMessage::new(time, fall_down_state))
+                .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(SimulationMessage::new(time, button_event_msg))
+                .send(button_event_msg)
                 .await?
         }
         SimulationMessage {
@@ -211,7 +209,7 @@ async fn handle_message(
             *hardware_interface_time.lock() = time;
             worker_channels
                 .remote_controller_state_sender
-                .send(SimulationMessage::new(time, remote_controller_state))
+                .send(remote_controller_state)
                 .await?
         }
         SimulationMessage {
@@ -221,7 +219,7 @@ async fn handle_message(
             *hardware_interface_time.lock() = time;
             worker_channels
                 .transform_stamped_sender
-                .send(SimulationMessage::new(time, transform_stamped))
+                .send(transform_stamped)
                 .await?
         }
         SimulationMessage {
@@ -231,7 +229,7 @@ async fn handle_message(
             *hardware_interface_time.lock() = time;
             worker_channels
                 .rgbd_sensors_sender
-                .send(SimulationMessage::new(time, rgbd_sensors))
+                .send(rgbd_sensors)
                 .await?
         }
     };
@@ -240,7 +238,7 @@ async fn handle_message(
 }
 
 impl LowStateInterface for MujocoHardwareInterface {
-    fn read_low_state(&self) -> Result<SimulationMessage<LowState>> {
+    fn read_low_state(&self) -> Result<LowState> {
         self.low_state_receiver
             .lock()
             .blocking_recv()
@@ -257,7 +255,7 @@ impl LowCommandInterface for MujocoHardwareInterface {
 }
 
 impl FallDownStateInterface for MujocoHardwareInterface {
-    fn read_fall_down_state(&self) -> Result<SimulationMessage<FallDownState>> {
+    fn read_fall_down_state(&self) -> Result<FallDownState> {
         self.fall_down_receiver
             .lock()
             .blocking_recv()
@@ -266,7 +264,7 @@ impl FallDownStateInterface for MujocoHardwareInterface {
 }
 
 impl ButtonEventMsgInterface for MujocoHardwareInterface {
-    fn read_button_event_msg(&self) -> Result<SimulationMessage<ButtonEventMsg>> {
+    fn read_button_event_msg(&self) -> Result<ButtonEventMsg> {
         self.button_event_msg_receiver
             .lock()
             .blocking_recv()
@@ -275,7 +273,7 @@ impl ButtonEventMsgInterface for MujocoHardwareInterface {
 }
 
 impl RemoteControllerStateInterface for MujocoHardwareInterface {
-    fn read_remote_controller_state(&self) -> Result<SimulationMessage<RemoteControllerState>> {
+    fn read_remote_controller_state(&self) -> Result<RemoteControllerState> {
         self.remote_controller_state_receiver
             .lock()
             .blocking_recv()
@@ -284,7 +282,7 @@ impl RemoteControllerStateInterface for MujocoHardwareInterface {
 }
 
 impl TransformStampedInterface for MujocoHardwareInterface {
-    fn read_transform_stamped(&self) -> Result<SimulationMessage<TransformStamped>> {
+    fn read_transform_stamped(&self) -> Result<TransformStamped> {
         self.transform_stamped_receiver
             .lock()
             .blocking_recv()
@@ -293,7 +291,7 @@ impl TransformStampedInterface for MujocoHardwareInterface {
 }
 
 impl RGBDSensorsInterface for MujocoHardwareInterface {
-    fn read_rgbd_sensors(&self) -> Result<SimulationMessage<RGBDSensors>> {
+    fn read_rgbd_sensors(&self) -> Result<RGBDSensors> {
         self.rgbd_sensors_receiver
             .lock()
             .blocking_recv()

crates/hulk_mujoco/src/hardware_interface.rs

@@ -19,6 +19,7 @@ use crate::execution::run;
 use crate::hardware_interface::{MujocoHardwareInterface, Parameters as HardwareParameters};
 
 mod hardware_interface;
+mod simulation_message;
 
 pub fn setup_logger() -> Result<(), fern::InitError> {
     fern::Dispatch::new()