diff --git a/docs.sh b/docs.sh
new file mode 100755
index 0000000..37563d2
--- /dev/null
+++ b/docs.sh
@@ -0,0 +1,3 @@
+#!/bin/sh
+export RUSTDOCFLAGS="--cfg docsrs --generate-link-to-definition -Z unstable-options"
+cargo +nightly doc --all-features --open
diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs
index e3ab71f..0665e23 100644
--- a/satrs-example/src/acs/mgm.rs
+++ b/satrs-example/src/acs/mgm.rs
@@ -1,7 +1,7 @@
 use derive_new::new;
 use satrs::hk::{HkRequest, HkRequestVariant};
+use satrs::mode_tree::{ModeChild, ModeNode};
 use satrs::power::{PowerSwitchInfo, PowerSwitcherCommandSender};
-use satrs::queue::{GenericSendError, GenericTargetedMessagingError};
 use satrs_example::{DeviceMode, TimestampHelper};
 use satrs_minisim::acs::lis3mdl::{
     MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR,
@@ -16,6 +16,7 @@ use std::time::Duration;
 
 use satrs::mode::{
     ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler,
+    ModeRequestHandlerMpscBounded,
 };
 use satrs::pus::{EcssTmSender, PusTmVariant};
 use satrs::request::{GenericMessage, MessageMetadata, UniqueApidTargetId};
@@ -171,7 +172,7 @@ pub struct MgmHandlerLis3Mdl<
 > {
     id: UniqueApidTargetId,
     dev_str: &'static str,
-    mode_interface: MpscModeLeafInterface,
+    mode_node: ModeRequestHandlerMpscBounded,
     composite_request_rx: mpsc::Receiver<GenericMessage<CompositeRequest>>,
     hk_reply_tx: mpsc::Sender<GenericMessage<HkReply>>,
     switch_helper: SwitchHelper,
@@ -275,25 +276,26 @@ impl<
     pub fn handle_mode_requests(&mut self) {
         loop {
             // TODO: Only allow one set mode request per cycle?
-            match self.mode_interface.request_rx.try_recv() {
-                Ok(msg) => {
-                    let result = self.handle_mode_request(msg);
-                    // TODO: Trigger event?
-                    if result.is_err() {
-                        log::warn!(
-                            "{}: mode request failed with error {:?}",
-                            self.dev_str,
-                            result.err().unwrap()
-                        );
+            match self.mode_node.try_recv_mode_request() {
+                Ok(opt_msg) => {
+                    if let Some(msg) = opt_msg {
+                        let result = self.handle_mode_request(msg);
+                        // TODO: Trigger event?
+                        if result.is_err() {
+                            log::warn!(
+                                "{}: mode request failed with error {:?}",
+                                self.dev_str,
+                                result.err().unwrap()
+                            );
+                        }
                     }
                 }
-                Err(e) => {
-                    if e != mpsc::TryRecvError::Empty {
+                Err(e) => match e {
+                    satrs::queue::GenericReceiveError::Empty => (),
+                    satrs::queue::GenericReceiveError::TxDisconnected(e) => {
                         log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e);
-                    } else {
-                        break;
                     }
-                }
+                },
             }
         }
     }
@@ -386,6 +388,7 @@ impl<
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        _forced: bool,
     ) -> Result<(), satrs::mode::ModeError> {
         log::info!(
             "{}: transitioning to mode {:?}",
@@ -448,10 +451,9 @@ impl<
                 requestor.sender_id()
             );
         }
-        self.mode_interface
-            .reply_to_pus_tx
-            .send(GenericMessage::new(requestor, reply))
-            .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?;
+        self.mode_node
+            .send_mode_reply(requestor, reply)
+            .map_err(ModeError::Send)?;
         Ok(())
     }
 
@@ -464,17 +466,46 @@ impl<
     }
 }
 
+impl<
+        ComInterface: SpiInterface,
+        TmSender: EcssTmSender,
+        SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
+    > ModeNode for MgmHandlerLis3Mdl<ComInterface, TmSender, SwitchHelper>
+{
+    fn id(&self) -> satrs::ComponentId {
+        self.id.into()
+    }
+}
+
+impl<
+        ComInterface: SpiInterface,
+        TmSender: EcssTmSender,
+        SwitchHelper: PowerSwitchInfo<PcduSwitch> + PowerSwitcherCommandSender<PcduSwitch>,
+    > ModeChild for MgmHandlerLis3Mdl<ComInterface, TmSender, SwitchHelper>
+{
+    type Sender = mpsc::SyncSender<GenericMessage<ModeReply>>;
+
+    fn add_mode_parent(&mut self, id: satrs::ComponentId, reply_sender: Self::Sender) {
+        self.mode_node.add_message_target(id, reply_sender);
+    }
+}
+
 #[cfg(test)]
 mod tests {
-    use std::sync::{mpsc, Arc};
+    use std::{
+        collections::HashMap,
+        sync::{mpsc, Arc},
+    };
 
     use satrs::{
         mode::{ModeReply, ModeRequest},
+        mode_tree::ModeParent,
         power::SwitchStateBinary,
         request::{GenericMessage, UniqueApidTargetId},
         tmtc::PacketAsVec,
+        ComponentId,
     };
-    use satrs_example::config::components::Apid;
+    use satrs_example::config::components::{Apid, MGM_ASSEMBLY};
     use satrs_minisim::acs::lis3mdl::MgmLis3RawValues;
 
     use crate::{eps::TestSwitchHelper, pus::hk::HkReply, requests::CompositeRequest};
@@ -503,7 +534,7 @@ mod tests {
     }
 
     pub struct MgmTestbench {
-        pub mode_request_tx: mpsc::Sender<GenericMessage<ModeRequest>>,
+        pub mode_request_tx: mpsc::SyncSender<GenericMessage<ModeRequest>>,
         pub mode_reply_rx_to_pus: mpsc::Receiver<GenericMessage<ModeReply>>,
         pub mode_reply_rx_to_parent: mpsc::Receiver<GenericMessage<ModeReply>>,
         pub composite_request_tx: mpsc::Sender<GenericMessage<CompositeRequest>>,
@@ -513,38 +544,76 @@ mod tests {
             MgmHandlerLis3Mdl<TestSpiInterface, mpsc::Sender<PacketAsVec>, TestSwitchHelper>,
     }
 
+    #[derive(Default)]
+    pub struct MgmAssemblyMock(
+        pub HashMap<ComponentId, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
+    );
+
+    impl ModeNode for MgmAssemblyMock {
+        fn id(&self) -> satrs::ComponentId {
+            PUS_MODE_SERVICE.into()
+        }
+    }
+
+    impl ModeParent for MgmAssemblyMock {
+        type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;
+
+        fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) {
+            self.0.insert(id, request_sender);
+        }
+    }
+
+    #[derive(Default)]
+    pub struct PusMock {
+        pub request_sender_map: HashMap<ComponentId, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
+    }
+
+    impl ModeNode for PusMock {
+        fn id(&self) -> satrs::ComponentId {
+            PUS_MODE_SERVICE.into()
+        }
+    }
+
+    impl ModeParent for PusMock {
+        type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;
+
+        fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) {
+            self.request_sender_map.insert(id, request_sender);
+        }
+    }
+
     impl MgmTestbench {
         pub fn new() -> Self {
-            let (request_tx, request_rx) = mpsc::channel();
-            let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel();
+            let (request_tx, request_rx) = mpsc::sync_channel(5);
+            let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::sync_channel(5);
             let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::sync_channel(5);
-            let mode_interface = MpscModeLeafInterface {
-                request_rx,
-                reply_to_pus_tx: reply_tx_to_pus,
-                reply_to_parent_tx: reply_tx_to_parent,
-            };
+            let id = UniqueApidTargetId::new(Apid::Acs as u16, 1);
+            let mode_node = ModeRequestHandlerMpscBounded::new(id.into(), request_rx);
             let (composite_request_tx, composite_request_rx) = mpsc::channel();
             let (hk_reply_tx, hk_reply_rx) = mpsc::channel();
             let (tm_tx, tm_rx) = mpsc::channel::<PacketAsVec>();
             let shared_mgm_set = Arc::default();
+            let mut handler = MgmHandlerLis3Mdl::new(
+                id,
+                "TEST_MGM",
+                mode_node,
+                composite_request_rx,
+                hk_reply_tx,
+                TestSwitchHelper::default(),
+                tm_tx,
+                TestSpiInterface::default(),
+                shared_mgm_set,
+            );
+            handler.add_mode_parent(PUS_MODE_SERVICE.into(), reply_tx_to_pus);
+            handler.add_mode_parent(MGM_ASSEMBLY.into(), reply_tx_to_parent);
             Self {
                 mode_request_tx: request_tx,
                 mode_reply_rx_to_pus: reply_rx_to_pus,
                 mode_reply_rx_to_parent: reply_rx_to_parent,
                 composite_request_tx,
+                handler,
                 tm_rx,
                 hk_reply_rx,
-                handler: MgmHandlerLis3Mdl::new(
-                    UniqueApidTargetId::new(Apid::Acs as u16, 1),
-                    "TEST_MGM",
-                    mode_interface,
-                    composite_request_rx,
-                    hk_reply_tx,
-                    TestSwitchHelper::default(),
-                    tm_tx,
-                    TestSpiInterface::default(),
-                    shared_mgm_set,
-                ),
             }
         }
     }
@@ -575,7 +644,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         testbench.handler.periodic_operation();
@@ -633,7 +705,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         testbench.handler.periodic_operation();
diff --git a/satrs-example/src/config.rs b/satrs-example/src/config.rs
index 5608bf3..ff1e2c8 100644
--- a/satrs-example/src/config.rs
+++ b/satrs-example/src/config.rs
@@ -149,6 +149,7 @@ pub mod components {
     #[derive(Copy, Clone, PartialEq, Eq)]
     pub enum AcsId {
         Mgm0 = 0,
+        Assembly = 1,
     }
 
     #[derive(Copy, Clone, PartialEq, Eq)]
@@ -176,6 +177,8 @@ pub mod components {
         UniqueApidTargetId::new(Apid::GenericPus as u16, PusId::PusHk as u32);
     pub const PUS_SCHED_SERVICE: UniqueApidTargetId =
         UniqueApidTargetId::new(Apid::Sched as u16, 0);
+    pub const MGM_ASSEMBLY: UniqueApidTargetId =
+        UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Assembly as u32);
     pub const MGM_HANDLER_0: UniqueApidTargetId =
         UniqueApidTargetId::new(Apid::Acs as u16, AcsId::Mgm0 as u32);
     pub const PCDU_HANDLER: UniqueApidTargetId =
diff --git a/satrs-example/src/eps/pcdu.rs b/satrs-example/src/eps/pcdu.rs
index 908bfb2..efcb55b 100644
--- a/satrs-example/src/eps/pcdu.rs
+++ b/satrs-example/src/eps/pcdu.rs
@@ -11,7 +11,7 @@ use satrs::{
     mode::{ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler},
     power::SwitchRequest,
     pus::{EcssTmSender, PusTmVariant},
-    queue::{GenericSendError, GenericTargetedMessagingError},
+    queue::GenericSendError,
     request::{GenericMessage, MessageMetadata, UniqueApidTargetId},
     spacepackets::ByteConversionError,
 };
@@ -412,6 +412,7 @@ impl<ComInterface: SerialInterface, TmSender: EcssTmSender> ModeRequestHandler
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        _forced: bool,
     ) -> Result<(), satrs::mode::ModeError> {
         log::info!(
             "{}: transitioning to mode {:?}",
@@ -469,7 +470,7 @@ impl<ComInterface: SerialInterface, TmSender: EcssTmSender> ModeRequestHandler
         self.mode_interface
             .reply_to_pus_tx
             .send(GenericMessage::new(requestor, reply))
-            .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?;
+            .map_err(|_| GenericSendError::RxDisconnected)?;
         Ok(())
     }
 
@@ -660,7 +661,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         let switch_map_shared = testbench.handler.shared_switch_map.lock().unwrap();
@@ -692,7 +696,10 @@ mod tests {
             .mode_request_tx
             .send(GenericMessage::new(
                 MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
-                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+                ModeRequest::SetMode {
+                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
+                    forced: false,
+                },
             ))
             .expect("failed to send mode request");
         testbench
diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs
index 317e3f0..5d7fcbf 100644
--- a/satrs-example/src/main.rs
+++ b/satrs-example/src/main.rs
@@ -21,6 +21,7 @@ use log::info;
 use pus::test::create_test_service_dynamic;
 use satrs::hal::std::tcp_server::ServerConfig;
 use satrs::hal::std::udp_server::UdpTcServer;
+use satrs::mode_tree::connect_mode_nodes;
 use satrs::pus::HandlingStatus;
 use satrs::request::{GenericMessage, MessageMetadata};
 use satrs::tmtc::{PacketSenderWithSharedPool, SharedPacketPool};
@@ -47,7 +48,7 @@ use crate::pus::scheduler::{create_scheduler_service_dynamic, create_scheduler_s
 use crate::pus::test::create_test_service_static;
 use crate::pus::{PusTcDistributor, PusTcMpscRouter};
 use crate::requests::{CompositeRequest, GenericRequestRouter};
-use satrs::mode::{Mode, ModeAndSubmode, ModeRequest};
+use satrs::mode::{Mode, ModeAndSubmode, ModeRequest, ModeRequestHandlerMpscBounded};
 use satrs::pus::event_man::EventRequestWithToken;
 use satrs::spacepackets::{time::cds::CdsTime, time::TimeWriter};
 use satrs_example::config::components::{
@@ -93,9 +94,6 @@ fn static_tmtc_pool_main() {
     request_map
         .composite_router_map
         .insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx);
-    request_map
-        .mode_router_map
-        .insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx);
     request_map
         .composite_router_map
         .insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx);
@@ -232,12 +230,8 @@ fn static_tmtc_pool_main() {
     let switch_helper = PowerSwitchHelper::new(switch_request_tx, shared_switch_set.clone());
 
     let shared_mgm_set = Arc::default();
-    let mgm_mode_leaf_interface = MpscModeLeafInterface {
-        request_rx: mgm_handler_mode_rx,
-        reply_to_pus_tx: pus_mode_reply_tx.clone(),
-        reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
-    };
-
+    let mgm_mode_node =
+        ModeRequestHandlerMpscBounded::new(MGM_HANDLER_0.into(), mgm_handler_mode_rx);
     let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
         sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx);
         SpiSimInterfaceWrapper::Sim(SpiSimInterface {
@@ -250,7 +244,7 @@ fn static_tmtc_pool_main() {
     let mut mgm_handler = MgmHandlerLis3Mdl::new(
         MGM_HANDLER_0,
         "MGM_0",
-        mgm_mode_leaf_interface,
+        mgm_mode_node,
         mgm_handler_composite_rx,
         pus_hk_reply_tx.clone(),
         switch_helper.clone(),
@@ -258,6 +252,13 @@ fn static_tmtc_pool_main() {
         mgm_spi_interface,
         shared_mgm_set,
     );
+    // Connect PUS mode service to device handler.
+    connect_mode_nodes(
+        &mut pus_stack.mode_srv,
+        mgm_handler_mode_tx,
+        &mut mgm_handler,
+        mgm_handler_mode_reply_to_parent_tx,
+    );
 
     let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) =
         mpsc::sync_channel(10);
@@ -291,7 +292,10 @@ fn static_tmtc_pool_main() {
     pcdu_handler_mode_tx
         .send(GenericMessage::new(
             MessageMetadata::new(0, NO_SENDER),
-            ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)),
+            ModeRequest::SetMode {
+                mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0),
+                forced: false,
+            },
         ))
         .expect("sending initial mode request failed");
 
@@ -423,9 +427,9 @@ fn dyn_tmtc_pool_main() {
     request_map
         .composite_router_map
         .insert(MGM_HANDLER_0.id(), mgm_handler_composite_tx);
-    request_map
-        .mode_router_map
-        .insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx);
+    //request_map
+    //.mode_router_map
+    //.insert(MGM_HANDLER_0.id(), mgm_handler_mode_tx);
     request_map
         .composite_router_map
         .insert(PCDU_HANDLER.id(), pcdu_handler_composite_tx);
@@ -540,11 +544,8 @@ fn dyn_tmtc_pool_main() {
     let (mgm_handler_mode_reply_to_parent_tx, _mgm_handler_mode_reply_to_parent_rx) =
         mpsc::sync_channel(5);
     let shared_mgm_set = Arc::default();
-    let mode_leaf_interface = MpscModeLeafInterface {
-        request_rx: mgm_handler_mode_rx,
-        reply_to_pus_tx: pus_mode_reply_tx.clone(),
-        reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
-    };
+    let mgm_mode_node =
+        ModeRequestHandlerMpscBounded::new(MGM_HANDLER_0.into(), mgm_handler_mode_rx);
 
     let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
         sim_client.add_reply_recipient(satrs_minisim::SimComponent::MgmLis3Mdl, mgm_sim_reply_tx);
@@ -558,7 +559,7 @@ fn dyn_tmtc_pool_main() {
     let mut mgm_handler = MgmHandlerLis3Mdl::new(
         MGM_HANDLER_0,
         "MGM_0",
-        mode_leaf_interface,
+        mgm_mode_node,
         mgm_handler_composite_rx,
         pus_hk_reply_tx.clone(),
         switch_helper.clone(),
@@ -566,6 +567,12 @@ fn dyn_tmtc_pool_main() {
         mgm_spi_interface,
         shared_mgm_set,
     );
+    connect_mode_nodes(
+        &mut pus_stack.mode_srv,
+        mgm_handler_mode_tx,
+        &mut mgm_handler,
+        mgm_handler_mode_reply_to_parent_tx,
+    );
 
     let (pcdu_handler_mode_reply_to_parent_tx, _pcdu_handler_mode_reply_to_parent_rx) =
         mpsc::sync_channel(10);
@@ -598,7 +605,10 @@ fn dyn_tmtc_pool_main() {
     pcdu_handler_mode_tx
         .send(GenericMessage::new(
             MessageMetadata::new(0, NO_SENDER),
-            ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as Mode, 0)),
+            ModeRequest::SetMode {
+                mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as Mode, 0),
+                forced: false,
+            },
         ))
         .expect("sending initial mode request failed");
 
diff --git a/satrs-example/src/pus/mode.rs b/satrs-example/src/pus/mode.rs
index 56ae9d4..181680c 100644
--- a/satrs-example/src/pus/mode.rs
+++ b/satrs-example/src/pus/mode.rs
@@ -1,4 +1,5 @@
 use derive_new::new;
+use satrs::mode_tree::{ModeNode, ModeParent};
 use satrs::tmtc::{PacketAsVec, PacketSenderWithSharedPool};
 use std::sync::mpsc;
 use std::time::Duration;
@@ -110,6 +111,7 @@ impl PusReplyHandler<ActivePusRequestStd, ModeReply> for ModeReplyHandler {
                     ),
                 )?;
             }
+            ModeReply::ModeInfo(_mode_and_submode) => (),
         };
         Ok(true)
     }
@@ -190,7 +192,13 @@ impl PusTcToRequestConverter<ActivePusRequestStd, ModeRequest> for ModeRequestCo
                 }
                 let mode_and_submode = ModeAndSubmode::from_be_bytes(&tc.user_data()[4..])
                     .expect("mode and submode extraction failed");
-                Ok((active_request, ModeRequest::SetMode(mode_and_submode)))
+                Ok((
+                    active_request,
+                    ModeRequest::SetMode {
+                        mode_and_submode,
+                        forced: false,
+                    },
+                ))
             }
             Subservice::TcReadMode => Ok((active_request, ModeRequest::ReadMode)),
             Subservice::TcAnnounceMode => Ok((active_request, ModeRequest::AnnounceMode)),
@@ -268,6 +276,27 @@ pub struct ModeServiceWrapper<TmSender: EcssTmSender, TcInMemConverter: EcssTcIn
     >,
 }
 
+impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> ModeNode
+    for ModeServiceWrapper<TmSender, TcInMemConverter>
+{
+    fn id(&self) -> ComponentId {
+        self.service.service_helper.id()
+    }
+}
+
+impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> ModeParent
+    for ModeServiceWrapper<TmSender, TcInMemConverter>
+{
+    type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender) {
+        self.service
+            .request_router
+            .mode_router_map
+            .insert(id, request_sender);
+    }
+}
+
 impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> TargetedPusService
     for ModeServiceWrapper<TmSender, TcInMemConverter>
 {
@@ -346,7 +375,13 @@ mod tests {
         let (_active_req, req) = testbench
             .convert(token, &[], TEST_APID, TEST_UNIQUE_ID_0)
             .expect("conversion has failed");
-        assert_eq!(req, ModeRequest::SetMode(mode_and_submode));
+        assert_eq!(
+            req,
+            ModeRequest::SetMode {
+                mode_and_submode,
+                forced: false
+            }
+        );
     }
 
     #[test]
diff --git a/satrs-example/src/pus/stack.rs b/satrs-example/src/pus/stack.rs
index 3594bd6..67c3b57 100644
--- a/satrs-example/src/pus/stack.rs
+++ b/satrs-example/src/pus/stack.rs
@@ -15,12 +15,12 @@ use super::{
 // for targeted services..
 #[derive(new)]
 pub struct PusStack<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter> {
-    test_srv: TestCustomServiceWrapper<TmSender, TcInMemConverter>,
-    hk_srv_wrapper: HkServiceWrapper<TmSender, TcInMemConverter>,
-    event_srv: EventServiceWrapper<TmSender, TcInMemConverter>,
-    action_srv_wrapper: ActionServiceWrapper<TmSender, TcInMemConverter>,
-    schedule_srv: SchedulingServiceWrapper<TmSender, TcInMemConverter>,
-    mode_srv: ModeServiceWrapper<TmSender, TcInMemConverter>,
+    pub test_srv: TestCustomServiceWrapper<TmSender, TcInMemConverter>,
+    pub hk_srv_wrapper: HkServiceWrapper<TmSender, TcInMemConverter>,
+    pub event_srv: EventServiceWrapper<TmSender, TcInMemConverter>,
+    pub action_srv_wrapper: ActionServiceWrapper<TmSender, TcInMemConverter>,
+    pub schedule_srv: SchedulingServiceWrapper<TmSender, TcInMemConverter>,
+    pub mode_srv: ModeServiceWrapper<TmSender, TcInMemConverter>,
 }
 
 impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter>
diff --git a/satrs/CHANGELOG.md b/satrs/CHANGELOG.md
index bb92cb7..b50049f 100644
--- a/satrs/CHANGELOG.md
+++ b/satrs/CHANGELOG.md
@@ -20,6 +20,17 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 - `StaticHeaplessMemoryPool` which can be grown with user-provided static buffers.
 - Scheduling table for systems with a standard runtime
+- Mode Tree Feature which allows building a network of mode components which can send mode
+  messages to each other.
+- Added first helper features like the `SubsystemExecutionHelper` and the
+  `SubsystemCommandingHelper` which allows to build subsystem components. Subsystem components
+  are able to execute mode sequences and perform target keeping based on a declarative table
+  format.
+- Added `DevManagerCommandingHelper` which performs some of the boilerplate logik required
+  by Assembly and Device Management components. This includes forwarding mode requests and
+  handling mode replies.
+- First basic health module with `HealthState`s and the `HealthTableProvider` trait. These
+  components are important for any FDIR components which get added in the future.
 
 # [v0.2.1] 2024-05-19
 
diff --git a/satrs/Cargo.toml b/satrs/Cargo.toml
index c333c71..88ee316 100644
--- a/satrs/Cargo.toml
+++ b/satrs/Cargo.toml
@@ -2,7 +2,7 @@
 name = "satrs"
 version = "0.2.1"
 edition = "2021"
-rust-version = "1.71.1"
+rust-version = "1.82.0"
 authors = ["Robin Mueller <muellerr@irs.uni-stuttgart.de>"]
 description = "A framework to build software for remote systems"
 homepage = "https://absatsw.irs.uni-stuttgart.de/projects/sat-rs/"
diff --git a/satrs/src/dev_mgmt.rs b/satrs/src/dev_mgmt.rs
new file mode 100644
index 0000000..b9eca0a
--- /dev/null
+++ b/satrs/src/dev_mgmt.rs
@@ -0,0 +1,448 @@
+use crate::{
+    mode::{ModeAndSubmode, ModeReply, ModeRequest, ModeRequestSender},
+    mode_tree::{ModeStoreProvider, ModeStoreVec},
+    queue::{GenericSendError, GenericTargetedMessagingError},
+    request::{GenericMessage, RequestId},
+    ComponentId,
+};
+use core::fmt::Debug;
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct ActiveModeCommandContext {
+    pub target_mode: ModeAndSubmode,
+    pub active_request_id: RequestId,
+}
+
+#[derive(Debug, Default, PartialEq, Eq)]
+pub enum DevManagerHelperResult {
+    #[default]
+    Idle,
+    Busy,
+    ModeCommandingDone(ActiveModeCommandContext),
+}
+
+#[derive(Debug)]
+pub enum DevManagerHelperError {
+    ChildNotInStore,
+}
+
+pub trait DevManagerUserHook: Debug {
+    fn send_mode_cmd_to_child(
+        &self,
+        request_id: RequestId,
+        target_id: ComponentId,
+        mode: ModeAndSubmode,
+        forced: bool,
+        children_mode_store: &mut ModeStoreVec,
+        mode_req_sender: &impl ModeRequestSender,
+    ) -> Result<(), GenericSendError>;
+
+    fn send_mode_cmds_to_children(
+        &self,
+        request_id: RequestId,
+        commanded_parent_mode: ModeAndSubmode,
+        forced: bool,
+        children_mode_store: &mut ModeStoreVec,
+        mode_req_sender: &impl ModeRequestSender,
+    ) -> Result<(), GenericSendError>;
+}
+
+#[derive(Debug, Default)]
+pub struct TransparentDevManagerHook {}
+
+impl DevManagerUserHook for TransparentDevManagerHook {
+    fn send_mode_cmds_to_children(
+        &self,
+        request_id: RequestId,
+        commanded_parent_mode: ModeAndSubmode,
+        forced: bool,
+        children_mode_store: &mut ModeStoreVec,
+        mode_req_sender: &impl ModeRequestSender,
+    ) -> Result<(), GenericSendError> {
+        for child in children_mode_store {
+            mode_req_sender.send_mode_request(
+                request_id,
+                child.id(),
+                ModeRequest::SetMode {
+                    mode_and_submode: commanded_parent_mode,
+                    forced,
+                },
+            )?;
+            child.awaiting_reply = true;
+        }
+        Ok(())
+    }
+
+    fn send_mode_cmd_to_child(
+        &self,
+        request_id: RequestId,
+        target_id: ComponentId,
+        mode: ModeAndSubmode,
+        forced: bool,
+        children_mode_store: &mut ModeStoreVec,
+        mode_req_sender: &impl ModeRequestSender,
+    ) -> Result<(), GenericSendError> {
+        let mut_val = children_mode_store
+            .get_mut(target_id)
+            .ok_or(GenericSendError::TargetDoesNotExist(target_id))?;
+        mut_val.awaiting_reply = true;
+        mode_req_sender.send_mode_request(
+            request_id,
+            target_id,
+            ModeRequest::SetMode {
+                mode_and_submode: mode,
+                forced,
+            },
+        )?;
+        Ok(())
+    }
+}
+
+#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
+pub enum DevManagerCommandingState {
+    #[default]
+    Idle,
+    AwaitingReplies(ActiveModeCommandContext),
+}
+
+impl DevManagerCommandingState {
+    fn new_active_cmd(mode_and_submode: ModeAndSubmode, active_request_id: RequestId) -> Self {
+        DevManagerCommandingState::AwaitingReplies(ActiveModeCommandContext {
+            target_mode: mode_and_submode,
+            active_request_id,
+        })
+    }
+}
+
+/// A generic helper for manager components which manage child components in a mode tree.
+///
+/// Mode commands are usually forwarded to all children components transparently.
+/// For example, this could be used in an Assembly component which manages multiple redundant
+/// child components. It can also be used inside a manager component which only manages one device.
+#[derive(Debug, Default)]
+pub struct DevManagerCommandingHelper<UserHook: DevManagerUserHook> {
+    /// The IDs, modes and reply awaition status of all children are tracked in this data
+    /// structure.
+    pub children_mode_store: ModeStoreVec,
+    pub user_hook: UserHook,
+    pub state: DevManagerCommandingState,
+}
+
+impl<UserHook: DevManagerUserHook> DevManagerCommandingHelper<UserHook> {
+    pub fn new(user_hook: UserHook) -> Self {
+        Self {
+            children_mode_store: Default::default(),
+            user_hook,
+            state: Default::default(),
+        }
+    }
+
+    pub fn send_mode_cmd_to_one_child(
+        &mut self,
+        request_id: RequestId,
+        target_id: ComponentId,
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+        mode_req_sender: &impl ModeRequestSender,
+    ) -> Result<(), GenericSendError> {
+        self.state = DevManagerCommandingState::new_active_cmd(mode_and_submode, request_id);
+        self.user_hook.send_mode_cmd_to_child(
+            request_id,
+            target_id,
+            mode_and_submode,
+            forced,
+            &mut self.children_mode_store,
+            mode_req_sender,
+        )?;
+        Ok(())
+    }
+
+    pub fn send_mode_cmd_to_all_children(
+        &mut self,
+        request_id: RequestId,
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+        mode_req_sender: &impl ModeRequestSender,
+    ) -> Result<(), GenericSendError> {
+        self.state = DevManagerCommandingState::new_active_cmd(mode_and_submode, request_id);
+        self.user_hook.send_mode_cmds_to_children(
+            request_id,
+            mode_and_submode,
+            forced,
+            &mut self.children_mode_store,
+            mode_req_sender,
+        )?;
+        Ok(())
+    }
+
+    pub fn target_mode(&self) -> Option<ModeAndSubmode> {
+        match self.state {
+            DevManagerCommandingState::Idle => None,
+            DevManagerCommandingState::AwaitingReplies(context) => Some(context.target_mode),
+        }
+    }
+
+    pub fn state(&self) -> DevManagerCommandingState {
+        self.state
+    }
+
+    pub fn send_announce_mode_cmd_to_children(
+        &self,
+        request_id: RequestId,
+        mode_req_sender: &impl ModeRequestSender,
+        recursive: bool,
+    ) -> Result<(), GenericTargetedMessagingError> {
+        let mut request = ModeRequest::AnnounceMode;
+        if recursive {
+            request = ModeRequest::AnnounceModeRecursive;
+        }
+        for child in self.children_mode_store.0.iter() {
+            mode_req_sender.send_mode_request(request_id, child.id(), request)?;
+        }
+        Ok(())
+    }
+
+    pub fn add_mode_child(&mut self, target_id: ComponentId, mode: ModeAndSubmode) {
+        self.children_mode_store.add_component(target_id, mode);
+    }
+
+    /// Helper method which counts the number of children which have a certain mode.
+    pub fn count_number_of_children_with_mode(&self, mode_and_submode: ModeAndSubmode) -> usize {
+        let mut children_in_target_mode = 0;
+        for child in &self.children_mode_store {
+            if child.mode_and_submode() == mode_and_submode {
+                children_in_target_mode += 1;
+            }
+        }
+        children_in_target_mode
+    }
+
+    pub fn handle_mode_reply(
+        &mut self,
+        mode_reply: &GenericMessage<ModeReply>,
+    ) -> Result<DevManagerHelperResult, DevManagerHelperError> {
+        let context = match self.state {
+            DevManagerCommandingState::Idle => return Ok(DevManagerHelperResult::Idle),
+            DevManagerCommandingState::AwaitingReplies(active_mode_command_context) => {
+                Some(active_mode_command_context)
+            }
+        };
+        if !self
+            .children_mode_store
+            .has_component(mode_reply.sender_id())
+        {
+            return Err(DevManagerHelperError::ChildNotInStore);
+        }
+        let mut generic_mode_reply_handler = |mode_and_submode: Option<ModeAndSubmode>| {
+            // Tying the reply awaition to the request ID ensures that something like replies
+            // belonging to older requests do not interfere with the completion handling of
+            // the mode commanding. This is important for forced mode commands.
+            let mut handle_awaition = false;
+            if let DevManagerCommandingState::AwaitingReplies { .. } = self.state {
+                handle_awaition = true;
+            }
+            let still_awating_replies = self.children_mode_store.mode_reply_handler(
+                mode_reply.sender_id(),
+                mode_and_submode,
+                handle_awaition,
+            );
+            // It is okay to unwrap: If awaition should be handled, the returned value should
+            // always be some valid value.
+            if handle_awaition && !still_awating_replies.unwrap() {
+                self.state = DevManagerCommandingState::Idle;
+                return Ok(DevManagerHelperResult::ModeCommandingDone(context.unwrap()));
+            }
+            Ok(DevManagerHelperResult::Busy)
+        };
+        match mode_reply.message {
+            ModeReply::ModeInfo(mode_and_submode) | ModeReply::ModeReply(mode_and_submode) => {
+                generic_mode_reply_handler(Some(mode_and_submode))
+            }
+            ModeReply::CantReachMode(_result_u16) => generic_mode_reply_handler(None),
+            ModeReply::WrongMode {
+                expected: _,
+                reached,
+            } => generic_mode_reply_handler(Some(reached)),
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::{
+        mode::{tests::ModeReqSenderMock, UNKNOWN_MODE},
+        request::MessageMetadata,
+    };
+
+    use super::*;
+
+    pub enum ExampleId {
+        Id1 = 1,
+        Id2 = 2,
+    }
+
+    pub enum ExampleMode {
+        Mode1 = 1,
+        Mode2 = 2,
+    }
+
+    #[test]
+    fn test_basic() {
+        let assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default());
+        assert_eq!(assy_helper.state(), DevManagerCommandingState::Idle);
+    }
+
+    #[test]
+    fn test_mode_announce() {
+        let mut assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default());
+        let mode_req_sender = ModeReqSenderMock::default();
+        assy_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE);
+        assy_helper.add_mode_child(ExampleId::Id2 as u64, UNKNOWN_MODE);
+        assy_helper
+            .send_announce_mode_cmd_to_children(1, &mode_req_sender, false)
+            .unwrap();
+        assert_eq!(mode_req_sender.requests.borrow().len(), 2);
+        let mut req = mode_req_sender.requests.borrow_mut().pop_front().unwrap();
+        assert_eq!(req.target_id, ExampleId::Id1 as u64);
+        assert_eq!(req.request_id, 1);
+        assert_eq!(req.request, ModeRequest::AnnounceMode);
+        req = mode_req_sender.requests.borrow_mut().pop_front().unwrap();
+        assert_eq!(req.target_id, ExampleId::Id2 as u64);
+        assert_eq!(req.request_id, 1);
+        assert_eq!(req.request, ModeRequest::AnnounceMode);
+    }
+
+    #[test]
+    fn test_mode_announce_recursive() {
+        let mut assy_helper = DevManagerCommandingHelper::new(TransparentDevManagerHook::default());
+        let mode_req_sender = ModeReqSenderMock::default();
+        assy_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE);
+        assy_helper.add_mode_child(ExampleId::Id2 as u64, UNKNOWN_MODE);
+        assy_helper
+            .send_announce_mode_cmd_to_children(1, &mode_req_sender, true)
+            .unwrap();
+        assert_eq!(mode_req_sender.requests.borrow().len(), 2);
+        let mut req = mode_req_sender.requests.borrow_mut().pop_front().unwrap();
+        assert_eq!(req.target_id, ExampleId::Id1 as u64);
+        assert_eq!(req.request_id, 1);
+        assert_eq!(req.request, ModeRequest::AnnounceModeRecursive);
+        req = mode_req_sender.requests.borrow_mut().pop_front().unwrap();
+        assert_eq!(req.target_id, ExampleId::Id2 as u64);
+        assert_eq!(req.request_id, 1);
+        assert_eq!(req.request, ModeRequest::AnnounceModeRecursive);
+    }
+
+    #[test]
+    fn test_mode_commanding_one_child() {
+        let mut dev_mgmt_helper =
+            DevManagerCommandingHelper::new(TransparentDevManagerHook::default());
+        let mode_req_sender = ModeReqSenderMock::default();
+        dev_mgmt_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE);
+        let expected_mode = ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0);
+        dev_mgmt_helper
+            .send_mode_cmd_to_one_child(
+                1,
+                ExampleId::Id1 as u64,
+                expected_mode,
+                false,
+                &mode_req_sender,
+            )
+            .unwrap();
+        assert_eq!(mode_req_sender.requests.borrow().len(), 1);
+        let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap();
+        assert_eq!(req.target_id, ExampleId::Id1 as u64);
+        assert_eq!(req.request_id, 1);
+        assert_eq!(
+            req.request,
+            ModeRequest::SetMode {
+                mode_and_submode: expected_mode,
+                forced: false
+            }
+        );
+        matches!(
+            dev_mgmt_helper.state(),
+            DevManagerCommandingState::AwaitingReplies { .. }
+        );
+        if let DevManagerCommandingState::AwaitingReplies(ctx) = dev_mgmt_helper.state() {
+            assert_eq!(ctx.target_mode, expected_mode);
+            assert_eq!(ctx.active_request_id, 1);
+        }
+        let reply = GenericMessage::new(
+            MessageMetadata::new(1, ExampleId::Id1 as u64),
+            ModeReply::ModeReply(expected_mode),
+        );
+        if let DevManagerHelperResult::ModeCommandingDone(ActiveModeCommandContext {
+            target_mode,
+            active_request_id,
+        }) = dev_mgmt_helper.handle_mode_reply(&reply).unwrap()
+        {
+            assert_eq!(target_mode, expected_mode);
+            assert_eq!(active_request_id, 1);
+        }
+        matches!(dev_mgmt_helper.state(), DevManagerCommandingState::Idle);
+    }
+
+    #[test]
+    fn test_mode_commanding_multi_child() {
+        let mut dev_mgmt_helper =
+            DevManagerCommandingHelper::new(TransparentDevManagerHook::default());
+        let mode_req_sender = ModeReqSenderMock::default();
+        dev_mgmt_helper.add_mode_child(ExampleId::Id1 as u64, UNKNOWN_MODE);
+        dev_mgmt_helper.add_mode_child(ExampleId::Id2 as u64, UNKNOWN_MODE);
+        let expected_mode = ModeAndSubmode::new(ExampleMode::Mode2 as u32, 0);
+        dev_mgmt_helper
+            .send_mode_cmd_to_all_children(1, expected_mode, false, &mode_req_sender)
+            .unwrap();
+        assert_eq!(mode_req_sender.requests.borrow().len(), 2);
+        let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap();
+        assert_eq!(req.target_id, ExampleId::Id1 as u64);
+        assert_eq!(req.request_id, 1);
+        assert_eq!(
+            req.request,
+            ModeRequest::SetMode {
+                mode_and_submode: expected_mode,
+                forced: false
+            }
+        );
+        let req = mode_req_sender.requests.borrow_mut().pop_front().unwrap();
+        assert_eq!(req.target_id, ExampleId::Id2 as u64);
+        assert_eq!(req.request_id, 1);
+        assert_eq!(
+            req.request,
+            ModeRequest::SetMode {
+                mode_and_submode: expected_mode,
+                forced: false
+            }
+        );
+        matches!(
+            dev_mgmt_helper.state(),
+            DevManagerCommandingState::AwaitingReplies { .. }
+        );
+        if let DevManagerCommandingState::AwaitingReplies(ctx) = dev_mgmt_helper.state() {
+            assert_eq!(ctx.target_mode, expected_mode);
+            assert_eq!(ctx.active_request_id, 1);
+        }
+
+        let reply = GenericMessage::new(
+            MessageMetadata::new(1, ExampleId::Id1 as u64),
+            ModeReply::ModeReply(expected_mode),
+        );
+        assert_eq!(
+            dev_mgmt_helper.handle_mode_reply(&reply).unwrap(),
+            DevManagerHelperResult::Busy
+        );
+        let reply = GenericMessage::new(
+            MessageMetadata::new(1, ExampleId::Id2 as u64),
+            ModeReply::ModeReply(expected_mode),
+        );
+        if let DevManagerHelperResult::ModeCommandingDone(ActiveModeCommandContext {
+            target_mode,
+            active_request_id,
+        }) = dev_mgmt_helper.handle_mode_reply(&reply).unwrap()
+        {
+            assert_eq!(target_mode, expected_mode);
+            assert_eq!(active_request_id, 1);
+        }
+        matches!(dev_mgmt_helper.state(), DevManagerCommandingState::Idle);
+    }
+}
diff --git a/satrs/src/health.rs b/satrs/src/health.rs
new file mode 100644
index 0000000..9d32457
--- /dev/null
+++ b/satrs/src/health.rs
@@ -0,0 +1,39 @@
+use crate::ComponentId;
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub enum HealthState {
+    Healthy = 1,
+    Faulty = 2,
+    PermanentFaulty = 3,
+    ExternalControl = 4,
+    NeedsRecovery = 5,
+}
+
+pub trait HealthTableProvider {
+    fn health(&self, id: ComponentId) -> Option<HealthState>;
+    fn set_health(&mut self, id: ComponentId, health: HealthState);
+}
+
+#[cfg(feature = "std")]
+#[derive(Debug, Clone)]
+pub struct HealthTableMapSync(
+    std::sync::Arc<std::sync::Mutex<hashbrown::HashMap<ComponentId, HealthState>>>,
+);
+
+#[cfg(feature = "std")]
+impl HealthTableMapSync {
+    pub fn new(health_table: hashbrown::HashMap<ComponentId, HealthState>) -> Self {
+        Self(std::sync::Arc::new(std::sync::Mutex::new(health_table)))
+    }
+}
+
+#[cfg(feature = "std")]
+impl HealthTableProvider for HealthTableMapSync {
+    fn health(&self, id: ComponentId) -> Option<HealthState> {
+        self.0.lock().unwrap().get(&id).copied()
+    }
+
+    fn set_health(&mut self, id: ComponentId, health: HealthState) {
+        self.0.lock().unwrap().insert(id, health);
+    }
+}
diff --git a/satrs/src/lib.rs b/satrs/src/lib.rs
index 1801be5..5b81fe2 100644
--- a/satrs/src/lib.rs
+++ b/satrs/src/lib.rs
@@ -22,14 +22,21 @@ extern crate downcast_rs;
 #[cfg(any(feature = "std", test))]
 extern crate std;
 
+pub mod action;
+#[cfg(feature = "alloc")]
+pub mod dev_mgmt;
 pub mod encoding;
 pub mod event_man;
 pub mod events;
 #[cfg(feature = "std")]
 pub mod executable;
 pub mod hal;
+pub mod health;
+pub mod hk;
+pub mod mode;
 #[cfg(feature = "std")]
 pub mod mode_tree;
+pub mod params;
 pub mod pool;
 pub mod power;
 pub mod pus;
@@ -38,14 +45,11 @@ pub mod request;
 pub mod res_code;
 #[cfg(feature = "alloc")]
 pub mod scheduling;
+#[cfg(feature = "alloc")]
+pub mod subsystem;
 pub mod time;
 pub mod tmtc;
 
-pub mod action;
-pub mod hk;
-pub mod mode;
-pub mod params;
-
 pub use spacepackets;
 
 use spacepackets::PacketId;
diff --git a/satrs/src/mode.rs b/satrs/src/mode.rs
index 01beac9..b9bfa84 100644
--- a/satrs/src/mode.rs
+++ b/satrs/src/mode.rs
@@ -11,8 +11,10 @@ pub use alloc_mod::*;
 pub use std_mod::*;
 
 use crate::{
-    queue::GenericTargetedMessagingError,
-    request::{GenericMessage, MessageMetadata, MessageReceiver, MessageReceiverWithId, RequestId},
+    queue::{GenericReceiveError, GenericSendError},
+    request::{
+        GenericMessage, MessageMetadata, MessageReceiverProvider, MessageReceiverWithId, RequestId,
+    },
     ComponentId,
 };
 
@@ -26,6 +28,11 @@ pub struct ModeAndSubmode {
     submode: Submode,
 }
 
+pub const INVALID_MODE_VAL: Mode = Mode::MAX;
+pub const UNKNOWN_MODE_VAL: Mode = Mode::MAX - 1;
+pub const INVALID_MODE: ModeAndSubmode = ModeAndSubmode::new(INVALID_MODE_VAL, 0);
+pub const UNKNOWN_MODE: ModeAndSubmode = ModeAndSubmode::new(UNKNOWN_MODE_VAL, 0);
+
 impl ModeAndSubmode {
     pub const RAW_LEN: usize = size_of::<Mode>() + size_of::<Submode>();
 
@@ -111,7 +118,10 @@ impl TargetedModeCommand {
 pub enum ModeRequest {
     /// Mode information. Can be used to notify other components of changed modes.
     ModeInfo(ModeAndSubmode),
-    SetMode(ModeAndSubmode),
+    SetMode {
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+    },
     ReadMode,
     AnnounceMode,
     AnnounceModeRecursive,
@@ -127,6 +137,8 @@ pub struct TargetedModeRequest {
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
 pub enum ModeReply {
+    /// Mode information. Can be used to notify other components of changed modes.
+    ModeInfo(ModeAndSubmode),
     /// Reply to a mode request to confirm the commanded mode was reached.
     ModeReply(ModeAndSubmode),
     // Can not reach the commanded mode. Contains a reason as a [ResultU16].
@@ -147,34 +159,33 @@ pub trait ModeRequestSender {
         request_id: RequestId,
         target_id: ComponentId,
         request: ModeRequest,
-    ) -> Result<(), GenericTargetedMessagingError>;
+    ) -> Result<(), GenericSendError>;
 }
 
 pub trait ModeRequestReceiver {
     fn try_recv_mode_request(
         &self,
-    ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError>;
+    ) -> Result<Option<GenericMessage<ModeRequest>>, GenericReceiveError>;
 }
 
-impl<R: MessageReceiver<ModeRequest>> ModeRequestReceiver
+impl<R: MessageReceiverProvider<ModeRequest>> ModeRequestReceiver
     for MessageReceiverWithId<ModeRequest, R>
 {
     fn try_recv_mode_request(
         &self,
-    ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+    ) -> Result<Option<GenericMessage<ModeRequest>>, GenericReceiveError> {
         self.try_recv_message()
     }
 }
 
-#[derive(Debug, Clone)]
+#[derive(Debug, Clone, thiserror::Error)]
 pub enum ModeError {
-    Messaging(GenericTargetedMessagingError),
-}
-
-impl From<GenericTargetedMessagingError> for ModeError {
-    fn from(value: GenericTargetedMessagingError) -> Self {
-        Self::Messaging(value)
-    }
+    #[error("Messaging send error: {0}")]
+    Send(#[from] GenericSendError),
+    #[error("Messaging receive error: {0}")]
+    Receive(#[from] GenericReceiveError),
+    #[error("busy with other mode request")]
+    Busy,
 }
 
 pub trait ModeProvider {
@@ -196,6 +207,7 @@ pub trait ModeRequestHandler: ModeProvider {
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), Self::Error>;
 
     fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool);
@@ -222,9 +234,10 @@ pub trait ModeRequestHandler: ModeProvider {
         request: GenericMessage<ModeRequest>,
     ) -> Result<(), Self::Error> {
         match request.message {
-            ModeRequest::SetMode(mode_and_submode) => {
-                self.start_transition(request.requestor_info, mode_and_submode)
-            }
+            ModeRequest::SetMode {
+                mode_and_submode,
+                forced,
+            } => self.start_transition(request.requestor_info, mode_and_submode, forced),
             ModeRequest::ReadMode => self.send_mode_reply(
                 request.requestor_info,
                 ModeReply::ModeReply(self.mode_and_submode()),
@@ -243,15 +256,16 @@ pub trait ModeRequestHandler: ModeProvider {
 }
 
 pub trait ModeReplyReceiver {
-    fn try_recv_mode_reply(
-        &self,
-    ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError>;
+    fn try_recv_mode_reply(&self)
+        -> Result<Option<GenericMessage<ModeReply>>, GenericReceiveError>;
 }
 
-impl<R: MessageReceiver<ModeReply>> ModeReplyReceiver for MessageReceiverWithId<ModeReply, R> {
+impl<R: MessageReceiverProvider<ModeReply>> ModeReplyReceiver
+    for MessageReceiverWithId<ModeReply, R>
+{
     fn try_recv_mode_reply(
         &self,
-    ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+    ) -> Result<Option<GenericMessage<ModeReply>>, GenericReceiveError> {
         self.try_recv_message()
     }
 }
@@ -264,24 +278,28 @@ pub trait ModeReplySender {
         &self,
         requestor_info: MessageMetadata,
         reply: ModeReply,
-    ) -> Result<(), GenericTargetedMessagingError>;
+    ) -> Result<(), GenericSendError>;
 }
 
 #[cfg(feature = "alloc")]
 pub mod alloc_mod {
-    use crate::request::{
-        MessageSender, MessageSenderAndReceiver, MessageSenderMap, RequestAndReplySenderAndReceiver,
+    use crate::{
+        queue::{GenericReceiveError, GenericSendError},
+        request::{
+            MessageSenderAndReceiver, MessageSenderMap, MessageSenderProvider,
+            MessageSenderStoreProvider, RequestAndReplySenderAndReceiver,
+        },
     };
 
     use super::*;
 
-    impl<S: MessageSender<ModeReply>> MessageSenderMap<ModeReply, S> {
+    impl<S: MessageSenderProvider<ModeReply>> MessageSenderMap<ModeReply, S> {
         pub fn send_mode_reply(
             &self,
             requestor_info: MessageMetadata,
             target_id: ComponentId,
             request: ModeReply,
-        ) -> Result<(), GenericTargetedMessagingError> {
+        ) -> Result<(), GenericSendError> {
             self.send_message(requestor_info, target_id, request)
         }
 
@@ -290,8 +308,13 @@ pub mod alloc_mod {
         }
     }
 
-    impl<FROM, S: MessageSender<ModeReply>, R: MessageReceiver<FROM>> ModeReplySender
-        for MessageSenderAndReceiver<ModeReply, FROM, S, R>
+    impl<
+            From,
+            Sender: MessageSenderProvider<ModeReply>,
+            Receiver: MessageReceiverProvider<From>,
+            SenderStore: MessageSenderStoreProvider<ModeReply, Sender>,
+        > ModeReplySender
+        for MessageSenderAndReceiver<ModeReply, From, Sender, Receiver, SenderStore>
     {
         fn local_channel_id(&self) -> ComponentId {
             self.local_channel_id_generic()
@@ -301,8 +324,8 @@ pub mod alloc_mod {
             &self,
             requestor_info: MessageMetadata,
             request: ModeReply,
-        ) -> Result<(), GenericTargetedMessagingError> {
-            self.message_sender_map.send_mode_reply(
+        ) -> Result<(), GenericSendError> {
+            self.message_sender_store.send_message(
                 MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()),
                 requestor_info.sender_id(),
                 request,
@@ -310,37 +333,67 @@ pub mod alloc_mod {
         }
     }
 
-    impl<TO, S: MessageSender<TO>, R: MessageReceiver<ModeReply>> ModeReplyReceiver
-        for MessageSenderAndReceiver<TO, ModeReply, S, R>
+    impl<
+            To,
+            Sender: MessageSenderProvider<To>,
+            Receiver: MessageReceiverProvider<ModeReply>,
+            SenderStore: MessageSenderStoreProvider<To, Sender>,
+        > ModeReplyReceiver
+        for MessageSenderAndReceiver<To, ModeReply, Sender, Receiver, SenderStore>
     {
         fn try_recv_mode_reply(
             &self,
-        ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ModeReply>>, GenericReceiveError> {
             self.message_receiver.try_recv_message()
         }
     }
 
     impl<
-            REQUEST,
-            S0: MessageSender<REQUEST>,
-            R0: MessageReceiver<ModeReply>,
-            S1: MessageSender<ModeReply>,
-            R1: MessageReceiver<REQUEST>,
-        > RequestAndReplySenderAndReceiver<REQUEST, ModeReply, S0, R0, S1, R1>
+            Request,
+            ReqSender: MessageSenderProvider<Request>,
+            ReqReceiver: MessageReceiverProvider<Request>,
+            ReqSenderStore: MessageSenderStoreProvider<Request, ReqSender>,
+            Reply,
+            ReplySender: MessageSenderProvider<Reply>,
+            ReplyReceiver: MessageReceiverProvider<Reply>,
+            ReplySenderStore: MessageSenderStoreProvider<Reply, ReplySender>,
+        >
+        RequestAndReplySenderAndReceiver<
+            Request,
+            ReqSender,
+            ReqReceiver,
+            ReqSenderStore,
+            Reply,
+            ReplySender,
+            ReplyReceiver,
+            ReplySenderStore,
+        >
     {
-        pub fn add_reply_target(&mut self, target_id: ComponentId, reply_sender: S1) {
-            self.reply_sender_map
+        pub fn add_reply_target(&mut self, target_id: ComponentId, reply_sender: ReplySender) {
+            self.reply_sender_store
                 .add_message_target(target_id, reply_sender)
         }
     }
 
     impl<
-            REQUEST,
-            S0: MessageSender<REQUEST>,
-            R0: MessageReceiver<ModeReply>,
-            S1: MessageSender<ModeReply>,
-            R1: MessageReceiver<REQUEST>,
-        > ModeReplySender for RequestAndReplySenderAndReceiver<REQUEST, ModeReply, S0, R0, S1, R1>
+            Request,
+            ReqSender: MessageSenderProvider<Request>,
+            ReqReceiver: MessageReceiverProvider<Request>,
+            ReqSenderStore: MessageSenderStoreProvider<Request, ReqSender>,
+            ReplySender: MessageSenderProvider<ModeReply>,
+            ReplyReceiver: MessageReceiverProvider<ModeReply>,
+            ReplySenderStore: MessageSenderStoreProvider<ModeReply, ReplySender>,
+        > ModeReplySender
+        for RequestAndReplySenderAndReceiver<
+            Request,
+            ReqSender,
+            ReqReceiver,
+            ReqSenderStore,
+            ModeReply,
+            ReplySender,
+            ReplyReceiver,
+            ReplySenderStore,
+        >
     {
         fn local_channel_id(&self) -> ComponentId {
             self.local_channel_id_generic()
@@ -349,42 +402,56 @@ pub mod alloc_mod {
         fn send_mode_reply(
             &self,
             requestor_info: MessageMetadata,
-            request: ModeReply,
-        ) -> Result<(), GenericTargetedMessagingError> {
-            self.reply_sender_map.send_mode_reply(
+            reply: ModeReply,
+        ) -> Result<(), GenericSendError> {
+            self.reply_sender_store.send_message(
                 MessageMetadata::new(requestor_info.request_id(), self.local_channel_id()),
                 requestor_info.sender_id(),
-                request,
+                reply,
             )
         }
     }
 
     impl<
-            REQUEST,
-            S0: MessageSender<REQUEST>,
-            R0: MessageReceiver<ModeReply>,
-            S1: MessageSender<ModeReply>,
-            R1: MessageReceiver<REQUEST>,
+            Request,
+            ReqSender: MessageSenderProvider<Request>,
+            ReqReceiver: MessageReceiverProvider<Request>,
+            ReqSenderStore: MessageSenderStoreProvider<Request, ReqSender>,
+            ReplySender: MessageSenderProvider<ModeReply>,
+            ReplyReceiver: MessageReceiverProvider<ModeReply>,
+            ReplySenderStore: MessageSenderStoreProvider<ModeReply, ReplySender>,
         > ModeReplyReceiver
-        for RequestAndReplySenderAndReceiver<REQUEST, ModeReply, S0, R0, S1, R1>
+        for RequestAndReplySenderAndReceiver<
+            Request,
+            ReqSender,
+            ReqReceiver,
+            ReqSenderStore,
+            ModeReply,
+            ReplySender,
+            ReplyReceiver,
+            ReplySenderStore,
+        >
     {
         fn try_recv_mode_reply(
             &self,
-        ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ModeReply>>, GenericReceiveError> {
             self.reply_receiver.try_recv_message()
         }
     }
 
     /// Helper type definition for a mode handler which can handle mode requests.
-    pub type ModeRequestHandlerInterface<S, R> =
-        MessageSenderAndReceiver<ModeReply, ModeRequest, S, R>;
+    pub type ModeRequestHandlerInterface<Sender, Receiver, ReplySenderStore> =
+        MessageSenderAndReceiver<ModeReply, ModeRequest, Sender, Receiver, ReplySenderStore>;
 
-    impl<S: MessageSender<ModeReply>, R: MessageReceiver<ModeRequest>>
-        ModeRequestHandlerInterface<S, R>
+    impl<
+            Sender: MessageSenderProvider<ModeReply>,
+            Receiver: MessageReceiverProvider<ModeRequest>,
+            ReplySenderStore: MessageSenderStoreProvider<ModeReply, Sender>,
+        > ModeRequestHandlerInterface<Sender, Receiver, ReplySenderStore>
     {
         pub fn try_recv_mode_request(
             &self,
-        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericReceiveError> {
             self.try_recv_message()
         }
 
@@ -392,7 +459,7 @@ pub mod alloc_mod {
             &self,
             requestor_info: MessageMetadata,
             reply: ModeReply,
-        ) -> Result<(), GenericTargetedMessagingError> {
+        ) -> Result<(), GenericSendError> {
             self.send_message(
                 requestor_info.request_id(),
                 requestor_info.sender_id(),
@@ -403,12 +470,18 @@ pub mod alloc_mod {
 
     /// Helper type defintion for a mode handler object which can send mode requests and receive
     /// mode replies.
-    pub type ModeRequestorInterface<S, R> = MessageSenderAndReceiver<ModeRequest, ModeReply, S, R>;
+    pub type ModeRequestorInterface<Sender, Receiver, RequestSenderStore> =
+        MessageSenderAndReceiver<ModeRequest, ModeReply, Sender, Receiver, RequestSenderStore>;
 
-    impl<S: MessageSender<ModeRequest>, R: MessageReceiver<ModeReply>> ModeRequestorInterface<S, R> {
+    impl<
+            Sender: MessageSenderProvider<ModeRequest>,
+            Receiver: MessageReceiverProvider<ModeReply>,
+            RequestSenderStore: MessageSenderStoreProvider<ModeRequest, Sender>,
+        > ModeRequestorInterface<Sender, Receiver, RequestSenderStore>
+    {
         pub fn try_recv_mode_reply(
             &self,
-        ) -> Result<Option<GenericMessage<ModeReply>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ModeReply>>, GenericReceiveError> {
             self.try_recv_message()
         }
 
@@ -417,23 +490,38 @@ pub mod alloc_mod {
             request_id: RequestId,
             target_id: ComponentId,
             reply: ModeRequest,
-        ) -> Result<(), GenericTargetedMessagingError> {
+        ) -> Result<(), GenericSendError> {
             self.send_message(request_id, target_id, reply)
         }
     }
 
     /// Helper type defintion for a mode handler object which can both send mode requests and
     /// process mode requests.
-    pub type ModeInterface<S0, R0, S1, R1> =
-        RequestAndReplySenderAndReceiver<ModeRequest, ModeReply, S0, R0, S1, R1>;
+    pub type ModeInterface<
+        ReqSender,
+        ReqReceiver,
+        ReqSenderStore,
+        ReplySender,
+        ReplyReceiver,
+        ReplySenderStore,
+    > = RequestAndReplySenderAndReceiver<
+        ModeRequest,
+        ReqSender,
+        ReqReceiver,
+        ReqSenderStore,
+        ModeReply,
+        ReplySender,
+        ReplyReceiver,
+        ReplySenderStore,
+    >;
 
-    impl<S: MessageSender<ModeRequest>> MessageSenderMap<ModeRequest, S> {
+    impl<S: MessageSenderProvider<ModeRequest>> MessageSenderMap<ModeRequest, S> {
         pub fn send_mode_request(
             &self,
             requestor_info: MessageMetadata,
             target_id: ComponentId,
             request: ModeRequest,
-        ) -> Result<(), GenericTargetedMessagingError> {
+        ) -> Result<(), GenericSendError> {
             self.send_message(requestor_info, target_id, request)
         }
 
@@ -442,35 +530,28 @@ pub mod alloc_mod {
         }
     }
 
-    /*
-    impl<S: MessageSender<ModeRequest>> ModeRequestSender for MessageSenderMapWithId<ModeRequest, S> {
-        fn local_channel_id(&self) -> ComponentId {
-            self.local_channel_id
-        }
-
-        fn send_mode_request(
-            &self,
-            request_id: RequestId,
-            target_id: ComponentId,
-            request: ModeRequest,
-        ) -> Result<(), GenericTargetedMessagingError> {
-            self.send_message(request_id, target_id, request)
-        }
-    }
-    */
-
-    impl<TO, S: MessageSender<TO>, R: MessageReceiver<ModeRequest>> ModeRequestReceiver
-        for MessageSenderAndReceiver<TO, ModeRequest, S, R>
+    impl<
+            To,
+            Sender: MessageSenderProvider<To>,
+            Receiver: MessageReceiverProvider<ModeRequest>,
+            SenderStore: MessageSenderStoreProvider<To, Sender>,
+        > ModeRequestReceiver
+        for MessageSenderAndReceiver<To, ModeRequest, Sender, Receiver, SenderStore>
     {
         fn try_recv_mode_request(
             &self,
-        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericReceiveError> {
             self.message_receiver.try_recv_message()
         }
     }
 
-    impl<FROM, S: MessageSender<ModeRequest>, R: MessageReceiver<FROM>> ModeRequestSender
-        for MessageSenderAndReceiver<ModeRequest, FROM, S, R>
+    impl<
+            From,
+            Sender: MessageSenderProvider<ModeRequest>,
+            Receiver: MessageReceiverProvider<From>,
+            SenderStore: MessageSenderStoreProvider<ModeRequest, Sender>,
+        > ModeRequestSender
+        for MessageSenderAndReceiver<ModeRequest, From, Sender, Receiver, SenderStore>
     {
         fn local_channel_id(&self) -> ComponentId {
             self.local_channel_id_generic()
@@ -481,8 +562,8 @@ pub mod alloc_mod {
             request_id: RequestId,
             target_id: ComponentId,
             request: ModeRequest,
-        ) -> Result<(), GenericTargetedMessagingError> {
-            self.message_sender_map.send_mode_request(
+        ) -> Result<(), GenericSendError> {
+            self.message_sender_store.send_message(
                 MessageMetadata::new(request_id, self.local_channel_id()),
                 target_id,
                 request,
@@ -491,27 +572,50 @@ pub mod alloc_mod {
     }
 
     impl<
-            REPLY,
-            S0: MessageSender<ModeRequest>,
-            R0: MessageReceiver<REPLY>,
-            S1: MessageSender<REPLY>,
-            R1: MessageReceiver<ModeRequest>,
-        > RequestAndReplySenderAndReceiver<ModeRequest, REPLY, S0, R0, S1, R1>
+            ReqSender: MessageSenderProvider<ModeRequest>,
+            ReqReceiver: MessageReceiverProvider<ModeRequest>,
+            ReqSenderStore: MessageSenderStoreProvider<ModeRequest, ReqSender>,
+            Reply,
+            ReplySender: MessageSenderProvider<Reply>,
+            ReplyReceiver: MessageReceiverProvider<Reply>,
+            ReplySenderStore: MessageSenderStoreProvider<Reply, ReplySender>,
+        >
+        RequestAndReplySenderAndReceiver<
+            ModeRequest,
+            ReqSender,
+            ReqReceiver,
+            ReqSenderStore,
+            Reply,
+            ReplySender,
+            ReplyReceiver,
+            ReplySenderStore,
+        >
     {
-        pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: S0) {
-            self.request_sender_map
+        pub fn add_request_target(&mut self, target_id: ComponentId, request_sender: ReqSender) {
+            self.request_sender_store
                 .add_message_target(target_id, request_sender)
         }
     }
 
     impl<
-            REPLY,
-            S0: MessageSender<ModeRequest>,
-            R0: MessageReceiver<REPLY>,
-            S1: MessageSender<REPLY>,
-            R1: MessageReceiver<ModeRequest>,
+            ReqSender: MessageSenderProvider<ModeRequest>,
+            ReqReceiver: MessageReceiverProvider<ModeRequest>,
+            ReqSenderStore: MessageSenderStoreProvider<ModeRequest, ReqSender>,
+            Reply,
+            ReplySender: MessageSenderProvider<Reply>,
+            ReplyReceiver: MessageReceiverProvider<Reply>,
+            ReplySenderStore: MessageSenderStoreProvider<Reply, ReplySender>,
         > ModeRequestSender
-        for RequestAndReplySenderAndReceiver<ModeRequest, REPLY, S0, R0, S1, R1>
+        for RequestAndReplySenderAndReceiver<
+            ModeRequest,
+            ReqSender,
+            ReqReceiver,
+            ReqSenderStore,
+            Reply,
+            ReplySender,
+            ReplyReceiver,
+            ReplySenderStore,
+        >
     {
         fn local_channel_id(&self) -> ComponentId {
             self.local_channel_id_generic()
@@ -522,8 +626,8 @@ pub mod alloc_mod {
             request_id: RequestId,
             target_id: ComponentId,
             request: ModeRequest,
-        ) -> Result<(), GenericTargetedMessagingError> {
-            self.request_sender_map.send_mode_request(
+        ) -> Result<(), GenericSendError> {
+            self.request_sender_store.send_message(
                 MessageMetadata::new(request_id, self.local_channel_id()),
                 target_id,
                 request,
@@ -532,17 +636,28 @@ pub mod alloc_mod {
     }
 
     impl<
-            REPLY,
-            S0: MessageSender<ModeRequest>,
-            R0: MessageReceiver<REPLY>,
-            S1: MessageSender<REPLY>,
-            R1: MessageReceiver<ModeRequest>,
+            ReqSender: MessageSenderProvider<ModeRequest>,
+            ReqReceiver: MessageReceiverProvider<ModeRequest>,
+            ReqSenderStore: MessageSenderStoreProvider<ModeRequest, ReqSender>,
+            Reply,
+            ReplySender: MessageSenderProvider<Reply>,
+            ReplyReceiver: MessageReceiverProvider<Reply>,
+            ReplySenderStore: MessageSenderStoreProvider<Reply, ReplySender>,
         > ModeRequestReceiver
-        for RequestAndReplySenderAndReceiver<ModeRequest, REPLY, S0, R0, S1, R1>
+        for RequestAndReplySenderAndReceiver<
+            ModeRequest,
+            ReqSender,
+            ReqReceiver,
+            ReqSenderStore,
+            Reply,
+            ReplySender,
+            ReplyReceiver,
+            ReplySenderStore,
+        >
     {
         fn try_recv_mode_request(
             &self,
-        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ModeRequest>>, GenericReceiveError> {
             self.request_receiver.try_recv_message()
         }
     }
@@ -552,39 +667,97 @@ pub mod alloc_mod {
 pub mod std_mod {
     use std::sync::mpsc;
 
+    use crate::request::{MessageSenderList, OneMessageSender};
+
     use super::*;
 
     pub type ModeRequestHandlerMpsc = ModeRequestHandlerInterface<
         mpsc::Sender<GenericMessage<ModeReply>>,
         mpsc::Receiver<GenericMessage<ModeRequest>>,
+        MessageSenderList<ModeReply, mpsc::Sender<GenericMessage<ModeReply>>>,
     >;
     pub type ModeRequestHandlerMpscBounded = ModeRequestHandlerInterface<
         mpsc::SyncSender<GenericMessage<ModeReply>>,
         mpsc::Receiver<GenericMessage<ModeRequest>>,
+        MessageSenderList<ModeReply, mpsc::SyncSender<GenericMessage<ModeReply>>>,
     >;
 
-    pub type ModeRequestorMpsc = ModeRequestorInterface<
+    pub type ModeRequestorOneChildMpsc = ModeRequestorInterface<
         mpsc::Sender<GenericMessage<ModeRequest>>,
         mpsc::Receiver<GenericMessage<ModeReply>>,
+        OneMessageSender<ModeRequest, mpsc::Sender<GenericMessage<ModeRequest>>>,
     >;
-    pub type ModeRequestorBoundedMpsc = ModeRequestorInterface<
+    pub type ModeRequestorOneChildBoundedMpsc = ModeRequestorInterface<
         mpsc::SyncSender<GenericMessage<ModeRequest>>,
         mpsc::Receiver<GenericMessage<ModeReply>>,
+        OneMessageSender<ModeRequest, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
+    >;
+    pub type ModeRequestorChildListMpsc = ModeRequestorInterface<
+        mpsc::Sender<GenericMessage<ModeRequest>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+        MessageSenderList<ModeRequest, mpsc::Sender<GenericMessage<ModeRequest>>>,
+    >;
+    pub type ModeRequestorChildListBoundedMpsc = ModeRequestorInterface<
+        mpsc::SyncSender<GenericMessage<ModeRequest>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+        MessageSenderList<ModeRequest, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
     >;
 
     pub type ModeRequestorAndHandlerMpsc = ModeInterface<
         mpsc::Sender<GenericMessage<ModeRequest>>,
-        mpsc::Receiver<GenericMessage<ModeReply>>,
-        mpsc::Sender<GenericMessage<ModeReply>>,
         mpsc::Receiver<GenericMessage<ModeRequest>>,
+        MessageSenderList<ModeRequest, mpsc::Sender<GenericMessage<ModeRequest>>>,
+        mpsc::Sender<GenericMessage<ModeReply>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+        MessageSenderList<ModeReply, mpsc::Sender<GenericMessage<ModeReply>>>,
     >;
     pub type ModeRequestorAndHandlerMpscBounded = ModeInterface<
         mpsc::SyncSender<GenericMessage<ModeRequest>>,
-        mpsc::Receiver<GenericMessage<ModeReply>>,
-        mpsc::SyncSender<GenericMessage<ModeReply>>,
         mpsc::Receiver<GenericMessage<ModeRequest>>,
+        MessageSenderList<ModeRequest, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
+        mpsc::SyncSender<GenericMessage<ModeReply>>,
+        mpsc::Receiver<GenericMessage<ModeReply>>,
+        MessageSenderList<ModeReply, mpsc::SyncSender<GenericMessage<ModeReply>>>,
     >;
 }
 
 #[cfg(test)]
-mod tests {}
+pub(crate) mod tests {
+    use core::cell::RefCell;
+    use std::collections::VecDeque;
+
+    use crate::{request::RequestId, ComponentId};
+
+    use super::*;
+
+    pub struct ModeReqWrapper {
+        pub request_id: RequestId,
+        pub target_id: ComponentId,
+        pub request: ModeRequest,
+    }
+
+    #[derive(Default)]
+    pub struct ModeReqSenderMock {
+        pub requests: RefCell<VecDeque<ModeReqWrapper>>,
+    }
+
+    impl ModeRequestSender for ModeReqSenderMock {
+        fn local_channel_id(&self) -> crate::ComponentId {
+            0
+        }
+
+        fn send_mode_request(
+            &self,
+            request_id: RequestId,
+            target_id: ComponentId,
+            request: ModeRequest,
+        ) -> Result<(), GenericSendError> {
+            self.requests.borrow_mut().push_back(ModeReqWrapper {
+                request_id,
+                target_id,
+                request,
+            });
+            Ok(())
+        }
+    }
+}
diff --git a/satrs/src/mode_tree.rs b/satrs/src/mode_tree.rs
index 1cddd32..e058378 100644
--- a/satrs/src/mode_tree.rs
+++ b/satrs/src/mode_tree.rs
@@ -2,10 +2,57 @@ use alloc::vec::Vec;
 use hashbrown::HashMap;
 
 use crate::{
-    mode::{Mode, ModeAndSubmode, Submode},
+    mode::{Mode, ModeAndSubmode, ModeReply, ModeRequest, Submode},
+    request::MessageSenderProvider,
     ComponentId,
 };
 
+#[cfg(feature = "alloc")]
+pub use alloc_mod::*;
+
+/// Common trait for node modes which can have mode parents or mode children.
+pub trait ModeNode {
+    fn id(&self) -> ComponentId;
+}
+/// Trait which denotes that an object is a parent in a mode tree.
+///
+/// A mode parent is capable of sending mode requests to child objects and has a unique component
+/// ID.
+pub trait ModeParent: ModeNode {
+    type Sender: MessageSenderProvider<ModeRequest>;
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender);
+}
+
+/// Trait which denotes that an object is a child in a mode tree.
+///
+/// A child is capable of sending mode replies to parent objects and has a unique component ID.
+pub trait ModeChild: ModeNode {
+    type Sender: MessageSenderProvider<ModeReply>;
+
+    fn add_mode_parent(&mut self, id: ComponentId, reply_sender: Self::Sender);
+}
+
+/// Utility method which connects a mode tree parent object to a child object by calling
+/// [ModeParent::add_mode_child] on the [parent][ModeParent] and calling
+/// [ModeChild::add_mode_parent] on the [child][ModeChild].
+///
+/// # Arguments
+///
+/// * `parent` - The parent object which implements [ModeParent].
+/// * `request_sender` - Sender object to send mode requests to the child.
+/// * `child` - The child object which implements [ModeChild].
+/// * `reply_sender` - Sender object to send mode replies to the parent.
+pub fn connect_mode_nodes<ReqSender, ReplySender>(
+    parent: &mut impl ModeParent<Sender = ReqSender>,
+    request_sender: ReqSender,
+    child: &mut impl ModeChild<Sender = ReplySender>,
+    reply_sender: ReplySender,
+) {
+    parent.add_mode_child(child.id(), request_sender);
+    child.add_mode_parent(parent.id(), reply_sender);
+}
+
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 pub enum TableEntryType {
     /// Target table containing information of the expected children modes for  given mode.
@@ -15,23 +62,553 @@ pub enum TableEntryType {
     Sequence,
 }
 
-pub struct ModeTableEntry {
+/// Common fields required for both target and sequence table entries.
+///
+/// The most important parameters here are the target ID which this entry belongs to, and the mode
+/// and submode the entry either will be commanded to for sequence table entries or which will be
+/// monitored for target table entries.
+#[derive(Debug, Copy, Clone)]
+pub struct ModeTableEntryCommon {
     /// Name of respective table entry.
     pub name: &'static str,
-    /// Target channel ID.
-    pub channel_id: ComponentId,
+    /// Target component ID.
+    pub target_id: ComponentId,
+    /// Has a different meaning depending on whether this is a sequence table or a target table.
+    ///
+    ///  - For sequence tables, this denotes the mode which will be commanded
+    ///  - For target tables, this is the mode which the target children should have and which
+    ///    might be monitored depending on configuration.
     pub mode_submode: ModeAndSubmode,
+    /// This mask allows to specify multiple allowed submodes for a given mode.
     pub allowed_submode_mask: Option<Submode>,
+}
+
+impl ModeTableEntryCommon {
+    pub fn set_allowed_submode_mask(&mut self, mask: Submode) {
+        self.allowed_submode_mask = Some(mask);
+    }
+
+    pub fn allowed_submode_mask(&self) -> Option<Submode> {
+        self.allowed_submode_mask
+    }
+}
+
+/// An entry for the target tables.
+#[derive(Debug)]
+pub struct TargetTableEntry {
+    pub common: ModeTableEntryCommon,
+    pub monitor_state: bool,
+}
+
+impl TargetTableEntry {
+    pub fn new(
+        name: &'static str,
+        target_id: ComponentId,
+        mode_submode: ModeAndSubmode,
+        allowed_submode_mask: Option<Submode>,
+    ) -> Self {
+        Self {
+            common: ModeTableEntryCommon {
+                name,
+                target_id,
+                mode_submode,
+                allowed_submode_mask,
+            },
+            monitor_state: true,
+        }
+    }
+
+    pub fn new_with_precise_submode(
+        name: &'static str,
+        target_id: ComponentId,
+        mode_submode: ModeAndSubmode,
+    ) -> Self {
+        Self {
+            common: ModeTableEntryCommon {
+                name,
+                target_id,
+                mode_submode,
+                allowed_submode_mask: None,
+            },
+            monitor_state: true,
+        }
+    }
+
+    delegate::delegate! {
+        to self.common {
+            pub fn set_allowed_submode_mask(&mut self, mask: Submode);
+            pub fn allowed_submode_mask(&self) -> Option<Submode>;
+        }
+    }
+}
+
+/// An entry for the sequence tables.
+///
+/// The [Self::check_success] field specifies that a mode sequence executor should check that the
+/// target mode was actually reached before executing the next sequence.
+#[derive(Debug)]
+pub struct SequenceTableEntry {
+    pub common: ModeTableEntryCommon,
     pub check_success: bool,
 }
 
-pub struct ModeTableMapValue {
-    /// Name for a given mode table entry.
-    pub name: &'static str,
-    pub entries: Vec<ModeTableEntry>,
+impl SequenceTableEntry {
+    pub fn new(
+        name: &'static str,
+        target_id: ComponentId,
+        mode_submode: ModeAndSubmode,
+        check_success: bool,
+    ) -> Self {
+        Self {
+            common: ModeTableEntryCommon {
+                name,
+                target_id,
+                mode_submode,
+                allowed_submode_mask: None,
+            },
+            check_success,
+        }
+    }
+
+    delegate::delegate! {
+        to self.common {
+            pub fn set_allowed_submode_mask(&mut self, mask: Submode);
+            pub fn allowed_submode_mask(&self) -> Option<Submode>;
+        }
+    }
 }
 
-pub type ModeTable = HashMap<Mode, ModeTableMapValue>;
+#[derive(Debug, thiserror::Error)]
+#[error("target {0} not in mode store")]
+pub struct TargetNotInModeStoreError(pub ComponentId);
+
+/// Mode store value type.
+#[derive(Debug, Copy, Clone)]
+pub struct ModeStoreValue {
+    /// ID of the mode component.
+    id: ComponentId,
+    /// Current mode and submode of the component.
+    pub mode_and_submode: ModeAndSubmode,
+    /// State information to track whether a reply should be awaited for the mode component.
+    pub awaiting_reply: bool,
+}
+
+impl ModeStoreValue {
+    pub fn new(id: ComponentId, mode_and_submode: ModeAndSubmode) -> Self {
+        Self {
+            id,
+            mode_and_submode,
+            awaiting_reply: false,
+        }
+    }
+
+    pub fn id(&self) -> ComponentId {
+        self.id
+    }
+
+    pub fn mode_and_submode(&self) -> ModeAndSubmode {
+        self.mode_and_submode
+    }
+}
+
+pub trait ModeStoreProvider {
+    fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode);
+
+    fn has_component(&self, target_id: ComponentId) -> bool;
+
+    fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue>;
+
+    fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue>;
+
+    /// Generic handler for mode replies received from child components.
+    ///
+    /// Implementation should clear the awaition flag if the `handle_reply_awaition` argument is
+    /// true and returns whether any children are still awaiting replies. If the flag is not set
+    fn mode_reply_handler_with_reply_awaition(
+        &mut self,
+        sender_id: ComponentId,
+        reported_mode_and_submode: Option<ModeAndSubmode>,
+    ) -> bool {
+        self.mode_reply_handler(sender_id, reported_mode_and_submode, true)
+            .unwrap_or(false)
+    }
+
+    fn mode_reply_handler_without_reply_awaition(
+        &mut self,
+        sender_id: ComponentId,
+        reported_mode_and_submode: Option<ModeAndSubmode>,
+    ) {
+        self.mode_reply_handler(sender_id, reported_mode_and_submode, false);
+    }
+
+    fn mode_reply_handler(
+        &mut self,
+        sender_id: ComponentId,
+        reported_mode_and_submode: Option<ModeAndSubmode>,
+        with_reply_awaition: bool,
+    ) -> Option<bool>;
+}
+
+#[cfg(feature = "alloc")]
+pub mod alloc_mod {
+    use super::*;
+
+    #[derive(Debug)]
+    pub struct TargetTablesMapValue {
+        /// Name for a given mode table entry.
+        pub name: &'static str,
+        /// Optional fallback mode if the target mode can not be kept.
+        pub fallback_mode: Option<Mode>,
+        /// These are the rows of the a target table.
+        pub entries: Vec<TargetTableEntry>,
+    }
+
+    impl TargetTablesMapValue {
+        pub fn new(name: &'static str, fallback_mode: Option<Mode>) -> Self {
+            Self {
+                name,
+                fallback_mode,
+                entries: Default::default(),
+            }
+        }
+
+        pub fn add_entry(&mut self, entry: TargetTableEntry) {
+            self.entries.push(entry);
+        }
+    }
+
+    /// One sequence of a [SequenceTablesMapValue] in a [SequenceModeTables].
+    ///
+    /// It contains all mode requests which need to be executed for a sequence step and it also
+    /// associates a [Self::name] with the sequence.
+    #[derive(Debug)]
+    pub struct SequenceTableMapTable {
+        /// Name for a given mode sequence.
+        pub name: &'static str,
+        /// These are the rows of the a sequence table.
+        pub entries: Vec<SequenceTableEntry>,
+    }
+
+    impl SequenceTableMapTable {
+        pub fn new(name: &'static str) -> Self {
+            Self {
+                name,
+                entries: Default::default(),
+            }
+        }
+
+        pub fn add_entry(&mut self, entry: SequenceTableEntry) {
+            self.entries.push(entry);
+        }
+    }
+
+    /// A sequence table entry.
+    ///
+    /// This is simply a list of [SequenceTableMapTable]s which also associates a [Self::name]
+    /// with the sequence. The order of sub-tables in the list also specifies the execution order
+    /// in the mode sequence.
+    #[derive(Debug)]
+    pub struct SequenceTablesMapValue {
+        /// Name for a given mode sequence.
+        pub name: &'static str,
+        /// Each sequence can consists of multiple sequences that are executed consecutively.
+        pub entries: Vec<SequenceTableMapTable>,
+    }
+
+    impl SequenceTablesMapValue {
+        pub fn new(name: &'static str) -> Self {
+            Self {
+                name,
+                entries: Default::default(),
+            }
+        }
+
+        pub fn add_sequence_table(&mut self, entry: SequenceTableMapTable) {
+            self.entries.push(entry);
+        }
+    }
+
+    #[derive(Debug, Default)]
+    pub struct TargetModeTables(pub HashMap<Mode, TargetTablesMapValue>);
+
+    impl TargetModeTables {
+        pub fn name(&self, mode: Mode) -> Option<&'static str> {
+            self.0.get(&mode).map(|value| value.name)
+        }
+    }
+
+    impl SequenceModeTables {
+        pub fn name(&self, mode: Mode) -> Option<&'static str> {
+            self.0.get(&mode).map(|value| value.name)
+        }
+
+        pub fn name_of_sequence(&self, mode: Mode, seq_idx: usize) -> Option<&'static str> {
+            self.0
+                .get(&mode)
+                .map(|value| value.entries.get(seq_idx).map(|v| v.name))?
+        }
+    }
+
+    /// This is the core data structure used to store mode sequence tables.
+    ///
+    /// A mode sequence table specifies which commands have to be sent in which order
+    /// to reach a certain [Mode]. Therefore, it simply maps a [Mode] to a [SequenceTablesMapValue].
+    #[derive(Debug, Default)]
+    pub struct SequenceModeTables(pub HashMap<Mode, SequenceTablesMapValue>);
+
+    /// Mode store which tracks the [mode information][ModeStoreValue] inside a [Vec]
+    #[derive(Debug, Default)]
+    pub struct ModeStoreVec(pub alloc::vec::Vec<ModeStoreValue>);
+
+    impl<'a> IntoIterator for &'a ModeStoreVec {
+        type Item = &'a ModeStoreValue;
+        type IntoIter = std::slice::Iter<'a, ModeStoreValue>;
+
+        fn into_iter(self) -> Self::IntoIter {
+            self.0.iter()
+        }
+    }
+
+    impl<'a> IntoIterator for &'a mut ModeStoreVec {
+        type Item = &'a mut ModeStoreValue;
+        type IntoIter = std::slice::IterMut<'a, ModeStoreValue>;
+
+        fn into_iter(self) -> Self::IntoIter {
+            self.0.iter_mut()
+        }
+    }
+
+    /// Mode store which tracks the mode information inside a [hashbrown::HashMap]
+    #[derive(Debug, Default)]
+    pub struct ModeStoreMap(pub hashbrown::HashMap<ComponentId, ModeStoreValue>);
+
+    impl<'a> IntoIterator for &'a ModeStoreMap {
+        type Item = (&'a ComponentId, &'a ModeStoreValue);
+        type IntoIter = hashbrown::hash_map::Iter<'a, ComponentId, ModeStoreValue>;
+
+        fn into_iter(self) -> Self::IntoIter {
+            self.0.iter()
+        }
+    }
+
+    impl ModeStoreProvider for ModeStoreVec {
+        fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) {
+            self.0.push(ModeStoreValue::new(target_id, mode));
+        }
+
+        fn has_component(&self, target_id: ComponentId) -> bool {
+            self.0.iter().any(|val| val.id == target_id)
+        }
+
+        fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> {
+            self.0.iter().find(|val| val.id == target_id)
+        }
+
+        fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> {
+            self.0.iter_mut().find(|val| val.id == target_id)
+        }
+
+        fn mode_reply_handler(
+            &mut self,
+            sender_id: ComponentId,
+            reported_mode_and_submode: Option<ModeAndSubmode>,
+            handle_reply_awaition: bool,
+        ) -> Option<bool> {
+            let mut still_awating_replies = None;
+            if handle_reply_awaition {
+                still_awating_replies = Some(false);
+            }
+            self.0.iter_mut().for_each(|val| {
+                if val.id() == sender_id {
+                    if let Some(mode_and_submode) = reported_mode_and_submode {
+                        val.mode_and_submode = mode_and_submode;
+                    }
+                    if handle_reply_awaition {
+                        val.awaiting_reply = false;
+                    }
+                }
+                if handle_reply_awaition && val.awaiting_reply {
+                    still_awating_replies = Some(true);
+                }
+            });
+            still_awating_replies
+        }
+    }
+
+    impl ModeStoreProvider for ModeStoreMap {
+        fn add_component(&mut self, target_id: ComponentId, mode: ModeAndSubmode) {
+            self.0
+                .insert(target_id, ModeStoreValue::new(target_id, mode));
+        }
+
+        fn has_component(&self, target_id: ComponentId) -> bool {
+            self.0.contains_key(&target_id)
+        }
+
+        fn get(&self, target_id: ComponentId) -> Option<&ModeStoreValue> {
+            self.0.get(&target_id)
+        }
+
+        fn get_mut(&mut self, target_id: ComponentId) -> Option<&mut ModeStoreValue> {
+            self.0.get_mut(&target_id)
+        }
+
+        fn mode_reply_handler(
+            &mut self,
+            sender_id: ComponentId,
+            reported_mode_and_submode: Option<ModeAndSubmode>,
+            handle_reply_awaition: bool,
+        ) -> Option<bool> {
+            let mut still_awating_replies = None;
+            if handle_reply_awaition {
+                still_awating_replies = Some(false);
+            }
+            for val in self.0.values_mut() {
+                if val.id() == sender_id {
+                    if let Some(mode_and_submode) = reported_mode_and_submode {
+                        val.mode_and_submode = mode_and_submode;
+                    }
+                    if handle_reply_awaition {
+                        val.awaiting_reply = false;
+                    }
+                }
+                if handle_reply_awaition && val.awaiting_reply {
+                    still_awating_replies = Some(true);
+                }
+            }
+            still_awating_replies
+        }
+    }
+}
 
 #[cfg(test)]
-mod tests {}
+mod tests {
+    use super::*;
+
+    fn generic_test(mode_store: &mut impl ModeStoreProvider) {
+        mode_store.add_component(1, ModeAndSubmode::new(0, 0));
+        mode_store.add_component(2, ModeAndSubmode::new(1, 0));
+        assert!(mode_store.has_component(1));
+        assert!(mode_store.has_component(2));
+        assert_eq!(
+            mode_store.get(1).unwrap().mode_and_submode(),
+            ModeAndSubmode::new(0, 0)
+        );
+        assert!(!mode_store.get(1).unwrap().awaiting_reply);
+        assert!(!mode_store.get(2).unwrap().awaiting_reply);
+        assert_eq!(mode_store.get(1).unwrap().id, 1);
+        assert_eq!(mode_store.get(2).unwrap().id, 2);
+        assert!(mode_store.get(3).is_none());
+        assert!(mode_store.get_mut(3).is_none());
+    }
+
+    fn generic_reply_handling_with_reply_awaition(mode_store: &mut impl ModeStoreProvider) {
+        mode_store.add_component(1, ModeAndSubmode::new(0, 0));
+        mode_store.add_component(2, ModeAndSubmode::new(1, 0));
+        mode_store.get_mut(1).unwrap().awaiting_reply = true;
+        mode_store.get_mut(2).unwrap().awaiting_reply = true;
+        let mut reply_awation_pending =
+            mode_store.mode_reply_handler_with_reply_awaition(1, Some(ModeAndSubmode::new(2, 0)));
+        assert!(reply_awation_pending);
+        reply_awation_pending = mode_store.mode_reply_handler_with_reply_awaition(2, None);
+        assert!(!reply_awation_pending);
+        assert!(!mode_store.get(1).unwrap().awaiting_reply);
+        assert!(!mode_store.get(2).unwrap().awaiting_reply);
+        assert_eq!(
+            mode_store.get(1).unwrap().mode_and_submode(),
+            ModeAndSubmode::new(2, 0)
+        );
+        assert_eq!(
+            mode_store.get(2).unwrap().mode_and_submode(),
+            ModeAndSubmode::new(1, 0)
+        );
+    }
+
+    fn generic_reply_handling_test_no_reply_awaition(mode_store: &mut impl ModeStoreProvider) {
+        mode_store.add_component(1, ModeAndSubmode::new(0, 0));
+        mode_store.add_component(2, ModeAndSubmode::new(1, 0));
+        mode_store.get_mut(1).unwrap().awaiting_reply = true;
+        mode_store.get_mut(2).unwrap().awaiting_reply = true;
+        mode_store.mode_reply_handler_without_reply_awaition(1, Some(ModeAndSubmode::new(2, 0)));
+        mode_store.mode_reply_handler_without_reply_awaition(2, None);
+        assert!(mode_store.get(1).unwrap().awaiting_reply);
+        assert!(mode_store.get(2).unwrap().awaiting_reply);
+        assert_eq!(
+            mode_store.get(1).unwrap().mode_and_submode(),
+            ModeAndSubmode::new(2, 0)
+        );
+        assert_eq!(
+            mode_store.get(2).unwrap().mode_and_submode(),
+            ModeAndSubmode::new(1, 0)
+        );
+    }
+
+    fn generic_reply_handling_with_reply_awaition_2(mode_store: &mut impl ModeStoreProvider) {
+        mode_store.add_component(1, ModeAndSubmode::new(0, 0));
+        mode_store.add_component(2, ModeAndSubmode::new(1, 0));
+        mode_store.get_mut(1).unwrap().awaiting_reply = true;
+        mode_store.get_mut(2).unwrap().awaiting_reply = true;
+        let mut reply_awation_pending =
+            mode_store.mode_reply_handler(1, Some(ModeAndSubmode::new(2, 0)), true);
+        assert!(reply_awation_pending.unwrap());
+        reply_awation_pending = mode_store.mode_reply_handler(2, None, true);
+        assert!(!reply_awation_pending.unwrap());
+        assert!(!mode_store.get(1).unwrap().awaiting_reply);
+        assert!(!mode_store.get(2).unwrap().awaiting_reply);
+        assert_eq!(
+            mode_store.get(1).unwrap().mode_and_submode(),
+            ModeAndSubmode::new(2, 0)
+        );
+        assert_eq!(
+            mode_store.get(2).unwrap().mode_and_submode(),
+            ModeAndSubmode::new(1, 0)
+        );
+    }
+
+    #[test]
+    fn test_vec_mode_store() {
+        let mut mode_store = ModeStoreVec::default();
+        generic_test(&mut mode_store);
+    }
+
+    #[test]
+    fn test_map_mode_store() {
+        let mut mode_store = ModeStoreMap::default();
+        generic_test(&mut mode_store);
+    }
+
+    #[test]
+    fn test_generic_reply_handler_vec_with_reply_awaition() {
+        let mut mode_store = ModeStoreVec::default();
+        generic_reply_handling_with_reply_awaition(&mut mode_store);
+    }
+
+    #[test]
+    fn test_generic_reply_handler_vec_with_reply_awaition_2() {
+        let mut mode_store = ModeStoreVec::default();
+        generic_reply_handling_with_reply_awaition_2(&mut mode_store);
+    }
+
+    #[test]
+    fn test_generic_reply_handler_map_with_reply_awaition() {
+        let mut mode_store = ModeStoreMap::default();
+        generic_reply_handling_with_reply_awaition(&mut mode_store);
+    }
+
+    #[test]
+    fn test_generic_reply_handler_map_with_reply_awaition_2() {
+        let mut mode_store = ModeStoreMap::default();
+        generic_reply_handling_with_reply_awaition_2(&mut mode_store);
+    }
+
+    #[test]
+    fn test_generic_reply_handler_vec_no_reply_awaition() {
+        let mut mode_store = ModeStoreVec::default();
+        generic_reply_handling_test_no_reply_awaition(&mut mode_store);
+    }
+    #[test]
+    fn test_generic_reply_handler_map_no_reply_awaition() {
+        let mut mode_store = ModeStoreMap::default();
+        generic_reply_handling_test_no_reply_awaition(&mut mode_store);
+    }
+}
diff --git a/satrs/src/pus/action.rs b/satrs/src/pus/action.rs
index 6bcd270..7a048b1 100644
--- a/satrs/src/pus/action.rs
+++ b/satrs/src/pus/action.rs
@@ -66,9 +66,10 @@ impl GenericActionReplyPus {
 pub mod alloc_mod {
     use crate::{
         action::ActionRequest,
-        queue::GenericTargetedMessagingError,
+        queue::{GenericReceiveError, GenericSendError},
         request::{
-            GenericMessage, MessageReceiver, MessageSender, MessageSenderAndReceiver, RequestId,
+            GenericMessage, MessageReceiverProvider, MessageSenderAndReceiver,
+            MessageSenderProvider, MessageSenderStoreProvider, RequestId,
         },
         ComponentId,
     };
@@ -76,15 +77,18 @@ pub mod alloc_mod {
     use super::ActionReplyPus;
 
     /// Helper type definition for a mode handler which can handle mode requests.
-    pub type ActionRequestHandlerInterface<S, R> =
-        MessageSenderAndReceiver<ActionReplyPus, ActionRequest, S, R>;
+    pub type ActionRequestHandlerInterface<Sender, Receiver, ReplySenderStore> =
+        MessageSenderAndReceiver<ActionReplyPus, ActionRequest, Sender, Receiver, ReplySenderStore>;
 
-    impl<S: MessageSender<ActionReplyPus>, R: MessageReceiver<ActionRequest>>
-        ActionRequestHandlerInterface<S, R>
+    impl<
+            Sender: MessageSenderProvider<ActionReplyPus>,
+            Receiver: MessageReceiverProvider<ActionRequest>,
+            ReplySender: MessageSenderStoreProvider<ActionReplyPus, Sender>,
+        > ActionRequestHandlerInterface<Sender, Receiver, ReplySender>
     {
         pub fn try_recv_action_request(
             &self,
-        ) -> Result<Option<GenericMessage<ActionRequest>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ActionRequest>>, GenericReceiveError> {
             self.try_recv_message()
         }
 
@@ -93,22 +97,31 @@ pub mod alloc_mod {
             request_id: RequestId,
             target_id: ComponentId,
             reply: ActionReplyPus,
-        ) -> Result<(), GenericTargetedMessagingError> {
+        ) -> Result<(), GenericSendError> {
             self.send_message(request_id, target_id, reply)
         }
     }
 
     /// Helper type defintion for a mode handler object which can send mode requests and receive
     /// mode replies.
-    pub type ActionRequestorInterface<S, R> =
-        MessageSenderAndReceiver<ActionRequest, ActionReplyPus, S, R>;
+    pub type ActionRequestorInterface<Sender, Receiver, RequestSenderStore> =
+        MessageSenderAndReceiver<
+            ActionRequest,
+            ActionReplyPus,
+            Sender,
+            Receiver,
+            RequestSenderStore,
+        >;
 
-    impl<S: MessageSender<ActionRequest>, R: MessageReceiver<ActionReplyPus>>
-        ActionRequestorInterface<S, R>
+    impl<
+            Sender: MessageSenderProvider<ActionRequest>,
+            Receiver: MessageReceiverProvider<ActionReplyPus>,
+            RequestSenderStore: MessageSenderStoreProvider<ActionRequest, Sender>,
+        > ActionRequestorInterface<Sender, Receiver, RequestSenderStore>
     {
         pub fn try_recv_action_reply(
             &self,
-        ) -> Result<Option<GenericMessage<ActionReplyPus>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<ActionReplyPus>>, GenericReceiveError> {
             self.try_recv_message()
         }
 
@@ -117,7 +130,7 @@ pub mod alloc_mod {
             request_id: RequestId,
             target_id: ComponentId,
             request: ActionRequest,
-        ) -> Result<(), GenericTargetedMessagingError> {
+        ) -> Result<(), GenericSendError> {
             self.send_message(request_id, target_id, request)
         }
     }
@@ -132,6 +145,7 @@ pub mod std_mod {
             verification::{self, TcStateToken},
             ActivePusRequestStd, ActiveRequestProvider, DefaultActiveRequestMap,
         },
+        request::{MessageSenderMap, OneMessageSender},
         ComponentId,
     };
 
@@ -174,22 +188,38 @@ pub mod std_mod {
     }
     pub type DefaultActiveActionRequestMap = DefaultActiveRequestMap<ActivePusActionRequestStd>;
 
-    pub type ActionRequestHandlerMpsc = ActionRequestHandlerInterface<
+    pub type ActionRequestHandlerOneSenderMpsc = ActionRequestHandlerInterface<
         mpsc::Sender<GenericMessage<ActionReplyPus>>,
         mpsc::Receiver<GenericMessage<ActionRequest>>,
+        OneMessageSender<
+            GenericMessage<ActionReplyPus>,
+            mpsc::Sender<GenericMessage<ActionReplyPus>>,
+        >,
     >;
-    pub type ActionRequestHandlerMpscBounded = ActionRequestHandlerInterface<
+    pub type ActionRequestHandlerOneSenderMpscBounded = ActionRequestHandlerInterface<
         mpsc::SyncSender<GenericMessage<ActionReplyPus>>,
         mpsc::Receiver<GenericMessage<ActionRequest>>,
+        OneMessageSender<
+            GenericMessage<ActionReplyPus>,
+            mpsc::SyncSender<GenericMessage<ActionReplyPus>>,
+        >,
     >;
 
-    pub type ActionRequestorMpsc = ActionRequestorInterface<
+    pub type ActionRequestorWithSenderMapMpsc = ActionRequestorInterface<
         mpsc::Sender<GenericMessage<ActionRequest>>,
         mpsc::Receiver<GenericMessage<ActionReplyPus>>,
+        MessageSenderMap<
+            GenericMessage<ActionRequest>,
+            mpsc::Sender<GenericMessage<ActionRequest>>,
+        >,
     >;
-    pub type ActionRequestorBoundedMpsc = ActionRequestorInterface<
+    pub type ActionRequestorWithSenderMapBoundedMpsc = ActionRequestorInterface<
         mpsc::SyncSender<GenericMessage<ActionRequest>>,
         mpsc::Receiver<GenericMessage<ActionReplyPus>>,
+        MessageSenderMap<
+            GenericMessage<ActionRequest>,
+            mpsc::SyncSender<GenericMessage<ActionRequest>>,
+        >,
     >;
 }
 
diff --git a/satrs/src/pus/mod.rs b/satrs/src/pus/mod.rs
index d4c5ff6..8a63454 100644
--- a/satrs/src/pus/mod.rs
+++ b/satrs/src/pus/mod.rs
@@ -1221,9 +1221,10 @@ pub(crate) fn source_buffer_large_enough(
 
 #[cfg(any(feature = "test_util", test))]
 pub mod test_util {
-    use crate::request::UniqueApidTargetId;
     use spacepackets::ecss::{tc::PusTcCreator, tm::PusTmReader};
 
+    use crate::request::UniqueApidTargetId;
+
     use super::{
         verification::{self, TcStateAccepted, VerificationToken},
         DirectPusPacketHandlerResult, PusPacketHandlingError,
@@ -1232,6 +1233,7 @@ pub mod test_util {
     pub const TEST_APID: u16 = 0x101;
     pub const TEST_UNIQUE_ID_0: u32 = 0x05;
     pub const TEST_UNIQUE_ID_1: u32 = 0x06;
+
     pub const TEST_COMPONENT_ID_0: UniqueApidTargetId =
         UniqueApidTargetId::new(TEST_APID, TEST_UNIQUE_ID_0);
     pub const TEST_COMPONENT_ID_1: UniqueApidTargetId =
@@ -1268,14 +1270,13 @@ pub mod tests {
     use spacepackets::ecss::tm::{GenericPusTmSecondaryHeader, PusTmCreator, PusTmReader};
     use spacepackets::ecss::{PusPacket, WritablePusPacket};
     use spacepackets::CcsdsPacket;
+    use test_util::{TEST_APID, TEST_COMPONENT_ID_0};
 
     use crate::pool::{PoolProvider, SharedStaticMemoryPool, StaticMemoryPool, StaticPoolConfig};
     use crate::pus::verification::{RequestId, VerificationReporter};
     use crate::tmtc::{PacketAsVec, PacketInPool, PacketSenderWithSharedPool, SharedPacketPool};
     use crate::ComponentId;
 
-    use super::test_util::{TEST_APID, TEST_COMPONENT_ID_0};
-
     use super::verification::test_util::TestVerificationReporter;
     use super::verification::{
         TcStateAccepted, VerificationReporterCfg, VerificationReportingProvider, VerificationToken,
diff --git a/satrs/src/pus/mode.rs b/satrs/src/pus/mode.rs
index 5ff78bf..773ad90 100644
--- a/satrs/src/pus/mode.rs
+++ b/satrs/src/pus/mode.rs
@@ -39,7 +39,7 @@ mod tests {
     use crate::{
         mode::{
             ModeAndSubmode, ModeReply, ModeReplySender, ModeRequest, ModeRequestSender,
-            ModeRequestorAndHandlerMpsc, ModeRequestorMpsc,
+            ModeRequestorAndHandlerMpsc, ModeRequestorOneChildMpsc,
         },
         request::{GenericMessage, MessageMetadata},
     };
@@ -52,7 +52,8 @@ mod tests {
     fn test_simple_mode_requestor() {
         let (reply_sender, reply_receiver) = mpsc::channel();
         let (request_sender, request_receiver) = mpsc::channel();
-        let mut mode_requestor = ModeRequestorMpsc::new(TEST_COMPONENT_ID_0, reply_receiver);
+        let mut mode_requestor =
+            ModeRequestorOneChildMpsc::new(TEST_COMPONENT_ID_0, reply_receiver);
         mode_requestor.add_message_target(TEST_COMPONENT_ID_1, request_sender);
 
         // Send a request and verify it arrives at the receiver.
diff --git a/satrs/src/queue.rs b/satrs/src/queue.rs
index 93c8ec8..c167114 100644
--- a/satrs/src/queue.rs
+++ b/satrs/src/queue.rs
@@ -1,6 +1,3 @@
-use core::fmt::{Display, Formatter};
-#[cfg(feature = "std")]
-use std::error::Error;
 #[cfg(feature = "std")]
 use std::sync::mpsc;
 
@@ -10,89 +7,31 @@ use crate::ComponentId;
 pub type ChannelId = u32;
 
 /// Generic error type for sending something via a message queue.
-#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+#[derive(Debug, Copy, Clone, PartialEq, Eq, thiserror::Error)]
 pub enum GenericSendError {
+    #[error("rx side has disconnected")]
     RxDisconnected,
+    #[error("queue with max capacity of {0:?} is full")]
     QueueFull(Option<u32>),
+    #[error("target queue with ID {0} does not exist")]
     TargetDoesNotExist(ComponentId),
 }
 
-impl Display for GenericSendError {
-    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
-        match self {
-            GenericSendError::RxDisconnected => {
-                write!(f, "rx side has disconnected")
-            }
-            GenericSendError::QueueFull(max_cap) => {
-                write!(f, "queue with max capacity of {max_cap:?} is full")
-            }
-            GenericSendError::TargetDoesNotExist(target) => {
-                write!(f, "target queue with ID {target} does not exist")
-            }
-        }
-    }
-}
-
-#[cfg(feature = "std")]
-impl Error for GenericSendError {}
-
 /// Generic error type for sending something via a message queue.
-#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+#[derive(Debug, Copy, Clone, PartialEq, Eq, thiserror::Error)]
 pub enum GenericReceiveError {
+    #[error("nothing to receive")]
     Empty,
+    #[error("tx side with id {0:?} has disconnected")]
     TxDisconnected(Option<ComponentId>),
 }
 
-impl Display for GenericReceiveError {
-    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
-        match self {
-            Self::TxDisconnected(channel_id) => {
-                write!(f, "tx side with id {channel_id:?} has disconnected")
-            }
-            Self::Empty => {
-                write!(f, "nothing to receive")
-            }
-        }
-    }
-}
-
-#[cfg(feature = "std")]
-impl Error for GenericReceiveError {}
-
-#[derive(Debug, Clone)]
+#[derive(Debug, Clone, thiserror::Error)]
 pub enum GenericTargetedMessagingError {
-    Send(GenericSendError),
-    Receive(GenericReceiveError),
-}
-impl From<GenericSendError> for GenericTargetedMessagingError {
-    fn from(value: GenericSendError) -> Self {
-        Self::Send(value)
-    }
-}
-
-impl From<GenericReceiveError> for GenericTargetedMessagingError {
-    fn from(value: GenericReceiveError) -> Self {
-        Self::Receive(value)
-    }
-}
-
-impl Display for GenericTargetedMessagingError {
-    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
-        match self {
-            Self::Send(err) => write!(f, "generic targeted messaging error: {}", err),
-            Self::Receive(err) => write!(f, "generic targeted messaging error: {}", err),
-        }
-    }
-}
-
-#[cfg(feature = "std")]
-impl Error for GenericTargetedMessagingError {
-    fn source(&self) -> Option<&(dyn Error + 'static)> {
-        match self {
-            GenericTargetedMessagingError::Send(send) => Some(send),
-            GenericTargetedMessagingError::Receive(receive) => Some(receive),
-        }
-    }
+    #[error("generic targeted messaging send error: {0}")]
+    Send(#[from] GenericSendError),
+    #[error("generic targeted messaging receive error: {0}")]
+    Receive(#[from] GenericReceiveError),
 }
 
 #[cfg(feature = "std")]
diff --git a/satrs/src/request.rs b/satrs/src/request.rs
index f188798..180ce00 100644
--- a/satrs/src/request.rs
+++ b/satrs/src/request.rs
@@ -13,7 +13,10 @@ use spacepackets::{
     ByteConversionError,
 };
 
-use crate::{queue::GenericTargetedMessagingError, ComponentId};
+use crate::{
+    queue::{GenericReceiveError, GenericSendError},
+    ComponentId,
+};
 
 /// Generic request ID type. Requests can be associated with an ID to have a unique identifier
 /// for them. This can be useful for tasks like tracking their progress.
@@ -140,37 +143,38 @@ impl<Message> GenericMessage<Message> {
 }
 
 /// Generic trait for objects which can send targeted messages.
-pub trait MessageSender<MSG>: Send {
-    fn send(&self, message: GenericMessage<MSG>) -> Result<(), GenericTargetedMessagingError>;
+pub trait MessageSenderProvider<MSG>: Send {
+    fn send(&self, message: GenericMessage<MSG>) -> Result<(), GenericSendError>;
 }
 
 // Generic trait for objects which can receive targeted messages.
-pub trait MessageReceiver<MSG> {
-    fn try_recv(&self) -> Result<Option<GenericMessage<MSG>>, GenericTargetedMessagingError>;
+pub trait MessageReceiverProvider<MSG> {
+    fn try_recv(&self) -> Result<Option<GenericMessage<MSG>>, GenericReceiveError>;
 }
 
-pub struct MessageWithSenderIdReceiver<MSG, R: MessageReceiver<MSG>>(pub R, PhantomData<MSG>);
+pub struct MessageWithSenderIdReceiver<Msg, Receiver: MessageReceiverProvider<Msg>>(
+    pub Receiver,
+    PhantomData<Msg>,
+);
 
-impl<MSG, R: MessageReceiver<MSG>> From<R> for MessageWithSenderIdReceiver<MSG, R> {
+impl<MSG, R: MessageReceiverProvider<MSG>> From<R> for MessageWithSenderIdReceiver<MSG, R> {
     fn from(receiver: R) -> Self {
         MessageWithSenderIdReceiver(receiver, PhantomData)
     }
 }
 
-impl<MSG, R: MessageReceiver<MSG>> MessageWithSenderIdReceiver<MSG, R> {
-    pub fn try_recv_message(
-        &self,
-    ) -> Result<Option<GenericMessage<MSG>>, GenericTargetedMessagingError> {
+impl<MSG, R: MessageReceiverProvider<MSG>> MessageWithSenderIdReceiver<MSG, R> {
+    pub fn try_recv_message(&self) -> Result<Option<GenericMessage<MSG>>, GenericReceiveError> {
         self.0.try_recv()
     }
 }
 
-pub struct MessageReceiverWithId<MSG, R: MessageReceiver<MSG>> {
+pub struct MessageReceiverWithId<MSG, R: MessageReceiverProvider<MSG>> {
     local_channel_id: ComponentId,
     reply_receiver: MessageWithSenderIdReceiver<MSG, R>,
 }
 
-impl<MSG, R: MessageReceiver<MSG>> MessageReceiverWithId<MSG, R> {
+impl<MSG, R: MessageReceiverProvider<MSG>> MessageReceiverWithId<MSG, R> {
     pub fn new(local_channel_id: ComponentId, reply_receiver: R) -> Self {
         Self {
             local_channel_id,
@@ -183,43 +187,129 @@ impl<MSG, R: MessageReceiver<MSG>> MessageReceiverWithId<MSG, R> {
     }
 }
 
-impl<MSG, R: MessageReceiver<MSG>> MessageReceiverWithId<MSG, R> {
-    pub fn try_recv_message(
-        &self,
-    ) -> Result<Option<GenericMessage<MSG>>, GenericTargetedMessagingError> {
+impl<MSG, R: MessageReceiverProvider<MSG>> MessageReceiverWithId<MSG, R> {
+    pub fn try_recv_message(&self) -> Result<Option<GenericMessage<MSG>>, GenericReceiveError> {
         self.reply_receiver.0.try_recv()
     }
 }
 
+pub trait MessageSenderStoreProvider<Message, Sender>: Default {
+    fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender);
+
+    fn send_message(
+        &self,
+        requestor_info: MessageMetadata,
+        target_channel_id: ComponentId,
+        message: Message,
+    ) -> Result<(), GenericSendError>;
+}
+
 #[cfg(feature = "alloc")]
 pub mod alloc_mod {
     use crate::queue::GenericSendError;
+    use std::convert::From;
 
     use super::*;
     use hashbrown::HashMap;
 
-    pub struct MessageSenderMap<MSG, S: MessageSender<MSG>>(
-        pub HashMap<ComponentId, S>,
+    pub struct OneMessageSender<Msg, S: MessageSenderProvider<Msg>> {
+        pub id_and_sender: Option<(ComponentId, S)>,
+        pub(crate) phantom: PhantomData<Msg>,
+    }
+
+    impl<Msg, S: MessageSenderProvider<Msg>> Default for OneMessageSender<Msg, S> {
+        fn default() -> Self {
+            Self {
+                id_and_sender: Default::default(),
+                phantom: Default::default(),
+            }
+        }
+    }
+
+    impl<Msg, Sender: MessageSenderProvider<Msg>> MessageSenderStoreProvider<Msg, Sender>
+        for OneMessageSender<Msg, Sender>
+    {
+        fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) {
+            if self.id_and_sender.is_some() {
+                return;
+            }
+            self.id_and_sender = Some((target_id, message_sender));
+        }
+
+        fn send_message(
+            &self,
+            requestor_info: MessageMetadata,
+            target_channel_id: ComponentId,
+            message: Msg,
+        ) -> Result<(), GenericSendError> {
+            if let Some((current_id, sender)) = &self.id_and_sender {
+                if *current_id == target_channel_id {
+                    sender.send(GenericMessage::new(requestor_info, message))?;
+                    return Ok(());
+                }
+            }
+            Err(GenericSendError::TargetDoesNotExist(target_channel_id))
+        }
+    }
+
+    pub struct MessageSenderList<MSG, S: MessageSenderProvider<MSG>>(
+        pub alloc::vec::Vec<(ComponentId, S)>,
         pub(crate) PhantomData<MSG>,
     );
 
-    impl<MSG, S: MessageSender<MSG>> Default for MessageSenderMap<MSG, S> {
+    impl<MSG, S: MessageSenderProvider<MSG>> Default for MessageSenderList<MSG, S> {
         fn default() -> Self {
             Self(Default::default(), PhantomData)
         }
     }
 
-    impl<MSG, S: MessageSender<MSG>> MessageSenderMap<MSG, S> {
-        pub fn add_message_target(&mut self, target_id: ComponentId, message_sender: S) {
-            self.0.insert(target_id, message_sender);
+    impl<Msg, Sender: MessageSenderProvider<Msg>> MessageSenderStoreProvider<Msg, Sender>
+        for MessageSenderList<Msg, Sender>
+    {
+        fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) {
+            self.0.push((target_id, message_sender));
         }
 
-        pub fn send_message(
+        fn send_message(
             &self,
             requestor_info: MessageMetadata,
             target_channel_id: ComponentId,
-            message: MSG,
-        ) -> Result<(), GenericTargetedMessagingError> {
+            message: Msg,
+        ) -> Result<(), GenericSendError> {
+            for (current_id, sender) in &self.0 {
+                if *current_id == target_channel_id {
+                    sender.send(GenericMessage::new(requestor_info, message))?;
+                    return Ok(());
+                }
+            }
+            Err(GenericSendError::TargetDoesNotExist(target_channel_id))
+        }
+    }
+
+    pub struct MessageSenderMap<MSG, S: MessageSenderProvider<MSG>>(
+        pub HashMap<ComponentId, S>,
+        pub(crate) PhantomData<MSG>,
+    );
+
+    impl<MSG, S: MessageSenderProvider<MSG>> Default for MessageSenderMap<MSG, S> {
+        fn default() -> Self {
+            Self(Default::default(), PhantomData)
+        }
+    }
+
+    impl<Msg, Sender: MessageSenderProvider<Msg>> MessageSenderStoreProvider<Msg, Sender>
+        for MessageSenderMap<Msg, Sender>
+    {
+        fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) {
+            self.0.insert(target_id, message_sender);
+        }
+
+        fn send_message(
+            &self,
+            requestor_info: MessageMetadata,
+            target_channel_id: ComponentId,
+            message: Msg,
+        ) -> Result<(), GenericSendError> {
             if self.0.contains_key(&target_channel_id) {
                 return self
                     .0
@@ -227,29 +317,42 @@ pub mod alloc_mod {
                     .unwrap()
                     .send(GenericMessage::new(requestor_info, message));
             }
-            Err(GenericSendError::TargetDoesNotExist(target_channel_id).into())
+            Err(GenericSendError::TargetDoesNotExist(target_channel_id))
         }
     }
 
-    pub struct MessageSenderAndReceiver<TO, FROM, S: MessageSender<TO>, R: MessageReceiver<FROM>> {
+    pub struct MessageSenderAndReceiver<
+        To,
+        From,
+        Sender: MessageSenderProvider<To>,
+        Receiver: MessageReceiverProvider<From>,
+        SenderStore: MessageSenderStoreProvider<To, Sender>,
+    > {
         pub local_channel_id: ComponentId,
-        pub message_sender_map: MessageSenderMap<TO, S>,
-        pub message_receiver: MessageWithSenderIdReceiver<FROM, R>,
+        pub message_sender_store: SenderStore,
+        pub message_receiver: MessageWithSenderIdReceiver<From, Receiver>,
+        pub(crate) phantom: PhantomData<(To, Sender)>,
     }
 
-    impl<TO, FROM, S: MessageSender<TO>, R: MessageReceiver<FROM>>
-        MessageSenderAndReceiver<TO, FROM, S, R>
+    impl<
+            To,
+            From,
+            Sender: MessageSenderProvider<To>,
+            Receiver: MessageReceiverProvider<From>,
+            SenderStore: MessageSenderStoreProvider<To, Sender>,
+        > MessageSenderAndReceiver<To, From, Sender, Receiver, SenderStore>
     {
-        pub fn new(local_channel_id: ComponentId, message_receiver: R) -> Self {
+        pub fn new(local_channel_id: ComponentId, message_receiver: Receiver) -> Self {
             Self {
                 local_channel_id,
-                message_sender_map: Default::default(),
+                message_sender_store: Default::default(),
                 message_receiver: MessageWithSenderIdReceiver::from(message_receiver),
+                phantom: PhantomData,
             }
         }
 
-        pub fn add_message_target(&mut self, target_id: ComponentId, message_sender: S) {
-            self.message_sender_map
+        pub fn add_message_target(&mut self, target_id: ComponentId, message_sender: Sender) {
+            self.message_sender_store
                 .add_message_target(target_id, message_sender)
         }
 
@@ -262,9 +365,9 @@ pub mod alloc_mod {
             &self,
             request_id: RequestId,
             target_id: ComponentId,
-            message: TO,
-        ) -> Result<(), GenericTargetedMessagingError> {
-            self.message_sender_map.send_message(
+            message: To,
+        ) -> Result<(), GenericSendError> {
+            self.message_sender_store.send_message(
                 MessageMetadata::new(request_id, self.local_channel_id_generic()),
                 target_id,
                 message,
@@ -274,48 +377,64 @@ pub mod alloc_mod {
         /// Try to receive a message, which can be a reply or a request, depending on the generics.
         pub fn try_recv_message(
             &self,
-        ) -> Result<Option<GenericMessage<FROM>>, GenericTargetedMessagingError> {
+        ) -> Result<Option<GenericMessage<From>>, GenericReceiveError> {
             self.message_receiver.try_recv_message()
         }
     }
 
     pub struct RequestAndReplySenderAndReceiver<
-        REQUEST,
-        REPLY,
-        S0: MessageSender<REQUEST>,
-        R0: MessageReceiver<REPLY>,
-        S1: MessageSender<REPLY>,
-        R1: MessageReceiver<REQUEST>,
+        Request,
+        ReqSender: MessageSenderProvider<Request>,
+        ReqReceiver: MessageReceiverProvider<Request>,
+        ReqSenderStore: MessageSenderStoreProvider<Request, ReqSender>,
+        Reply,
+        ReplySender: MessageSenderProvider<Reply>,
+        ReplyReceiver: MessageReceiverProvider<Reply>,
+        ReplySenderStore: MessageSenderStoreProvider<Reply, ReplySender>,
     > {
         pub local_channel_id: ComponentId,
         // These 2 are a functional group.
-        pub request_sender_map: MessageSenderMap<REQUEST, S0>,
-        pub reply_receiver: MessageWithSenderIdReceiver<REPLY, R0>,
+        pub request_sender_store: ReqSenderStore,
+        pub reply_receiver: MessageWithSenderIdReceiver<Reply, ReplyReceiver>,
         // These 2 are a functional group.
-        pub request_receiver: MessageWithSenderIdReceiver<REQUEST, R1>,
-        pub reply_sender_map: MessageSenderMap<REPLY, S1>,
+        pub request_receiver: MessageWithSenderIdReceiver<Request, ReqReceiver>,
+        pub reply_sender_store: ReplySenderStore,
+        phantom: PhantomData<(ReqSender, ReplySender)>,
     }
 
     impl<
-            REQUEST,
-            REPLY,
-            S0: MessageSender<REQUEST>,
-            R0: MessageReceiver<REPLY>,
-            S1: MessageSender<REPLY>,
-            R1: MessageReceiver<REQUEST>,
-        > RequestAndReplySenderAndReceiver<REQUEST, REPLY, S0, R0, S1, R1>
+            Request,
+            ReqSender: MessageSenderProvider<Request>,
+            ReqReceiver: MessageReceiverProvider<Request>,
+            ReqSenderStore: MessageSenderStoreProvider<Request, ReqSender>,
+            Reply,
+            ReplySender: MessageSenderProvider<Reply>,
+            ReplyReceiver: MessageReceiverProvider<Reply>,
+            ReplySenderStore: MessageSenderStoreProvider<Reply, ReplySender>,
+        >
+        RequestAndReplySenderAndReceiver<
+            Request,
+            ReqSender,
+            ReqReceiver,
+            ReqSenderStore,
+            Reply,
+            ReplySender,
+            ReplyReceiver,
+            ReplySenderStore,
+        >
     {
         pub fn new(
             local_channel_id: ComponentId,
-            request_receiver: R1,
-            reply_receiver: R0,
+            request_receiver: ReqReceiver,
+            reply_receiver: ReplyReceiver,
         ) -> Self {
             Self {
                 local_channel_id,
                 request_receiver: request_receiver.into(),
                 reply_receiver: reply_receiver.into(),
-                request_sender_map: Default::default(),
-                reply_sender_map: Default::default(),
+                request_sender_store: Default::default(),
+                reply_sender_store: Default::default(),
+                phantom: PhantomData,
             }
         }
 
@@ -333,21 +452,19 @@ pub mod std_mod {
 
     use crate::queue::{GenericReceiveError, GenericSendError};
 
-    impl<MSG: Send> MessageSender<MSG> for mpsc::Sender<GenericMessage<MSG>> {
-        fn send(&self, message: GenericMessage<MSG>) -> Result<(), GenericTargetedMessagingError> {
+    impl<MSG: Send> MessageSenderProvider<MSG> for mpsc::Sender<GenericMessage<MSG>> {
+        fn send(&self, message: GenericMessage<MSG>) -> Result<(), GenericSendError> {
             self.send(message)
                 .map_err(|_| GenericSendError::RxDisconnected)?;
             Ok(())
         }
     }
-    impl<MSG: Send> MessageSender<MSG> for mpsc::SyncSender<GenericMessage<MSG>> {
-        fn send(&self, message: GenericMessage<MSG>) -> Result<(), GenericTargetedMessagingError> {
+    impl<MSG: Send> MessageSenderProvider<MSG> for mpsc::SyncSender<GenericMessage<MSG>> {
+        fn send(&self, message: GenericMessage<MSG>) -> Result<(), GenericSendError> {
             if let Err(e) = self.try_send(message) {
                 return match e {
-                    mpsc::TrySendError::Full(_) => Err(GenericSendError::QueueFull(None).into()),
-                    mpsc::TrySendError::Disconnected(_) => {
-                        Err(GenericSendError::RxDisconnected.into())
-                    }
+                    mpsc::TrySendError::Full(_) => Err(GenericSendError::QueueFull(None)),
+                    mpsc::TrySendError::Disconnected(_) => Err(GenericSendError::RxDisconnected),
                 };
             }
             Ok(())
@@ -357,14 +474,14 @@ pub mod std_mod {
     pub type MessageSenderMapMpsc<MSG> = MessageReceiverWithId<MSG, mpsc::Sender<MSG>>;
     pub type MessageSenderMapBoundedMpsc<MSG> = MessageReceiverWithId<MSG, mpsc::SyncSender<MSG>>;
 
-    impl<MSG> MessageReceiver<MSG> for mpsc::Receiver<GenericMessage<MSG>> {
-        fn try_recv(&self) -> Result<Option<GenericMessage<MSG>>, GenericTargetedMessagingError> {
+    impl<MSG> MessageReceiverProvider<MSG> for mpsc::Receiver<GenericMessage<MSG>> {
+        fn try_recv(&self) -> Result<Option<GenericMessage<MSG>>, GenericReceiveError> {
             match self.try_recv() {
                 Ok(msg) => Ok(Some(msg)),
                 Err(e) => match e {
                     mpsc::TryRecvError::Empty => Ok(None),
                     mpsc::TryRecvError::Disconnected => {
-                        Err(GenericReceiveError::TxDisconnected(None).into())
+                        Err(GenericReceiveError::TxDisconnected(None))
                     }
                 },
             }
@@ -385,8 +502,8 @@ mod tests {
     };
 
     use crate::{
-        queue::{GenericReceiveError, GenericSendError, GenericTargetedMessagingError},
-        request::{MessageMetadata, MessageSenderMap},
+        queue::{GenericReceiveError, GenericSendError},
+        request::{MessageMetadata, MessageSenderMap, MessageSenderStoreProvider},
     };
 
     use super::{GenericMessage, MessageReceiverWithId, UniqueApidTargetId};
@@ -478,9 +595,7 @@ mod tests {
         let reply = receiver.try_recv_message();
         assert!(reply.is_err());
         let error = reply.unwrap_err();
-        if let GenericTargetedMessagingError::Receive(GenericReceiveError::TxDisconnected(None)) =
-            error
-        {
+        if let GenericReceiveError::TxDisconnected(None) = error {
         } else {
             panic!("unexpected error type");
         }
@@ -529,9 +644,7 @@ mod tests {
         );
         assert!(result.is_err());
         let error = result.unwrap_err();
-        if let GenericTargetedMessagingError::Send(GenericSendError::TargetDoesNotExist(target)) =
-            error
-        {
+        if let GenericSendError::TargetDoesNotExist(target) = error {
             assert_eq!(target, TEST_CHANNEL_ID_2);
         } else {
             panic!("Unexpected error type");
@@ -556,7 +669,7 @@ mod tests {
         );
         assert!(result.is_err());
         let error = result.unwrap_err();
-        if let GenericTargetedMessagingError::Send(GenericSendError::QueueFull(capacity)) = error {
+        if let GenericSendError::QueueFull(capacity) = error {
             assert!(capacity.is_none());
         } else {
             panic!("Unexpected error type {}", error);
@@ -576,7 +689,7 @@ mod tests {
         );
         assert!(result.is_err());
         let error = result.unwrap_err();
-        if let GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected) = error {
+        if let GenericSendError::RxDisconnected = error {
         } else {
             panic!("Unexpected error type {}", error);
         }
diff --git a/satrs/src/scheduling.rs b/satrs/src/scheduling.rs
index 5df7033..ff2c0a1 100644
--- a/satrs/src/scheduling.rs
+++ b/satrs/src/scheduling.rs
@@ -154,7 +154,7 @@ pub mod std_mod {
         }
 
         /// Can be used to set the start of the slot to the current time. This is useful if a custom
-        /// runner implementation is used instead of the [Self::start] method.
+        /// runner implementation is used instead of the [Self::run_one_task_cycle] method.
         pub fn init_start_of_slot(&mut self) {
             self.start_of_slot = Instant::now();
         }
diff --git a/satrs/src/subsystem.rs b/satrs/src/subsystem.rs
new file mode 100644
index 0000000..0212f76
--- /dev/null
+++ b/satrs/src/subsystem.rs
@@ -0,0 +1,1610 @@
+use crate::{
+    health::{HealthState, HealthTableProvider},
+    mode::{Mode, ModeAndSubmode, ModeReply, ModeRequest, ModeRequestSender, UNKNOWN_MODE_VAL},
+    mode_tree::{
+        ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableMapTable,
+        SequenceTablesMapValue, TargetModeTables, TargetNotInModeStoreError, TargetTablesMapValue,
+    },
+    queue::GenericTargetedMessagingError,
+    request::{GenericMessage, RequestId},
+    ComponentId,
+};
+
+#[derive(Debug, PartialEq, Eq, Copy, Clone)]
+pub enum SequenceExecutionHelperState {
+    /// The sequence execution is IDLE, no command is loaded or the sequence exection has
+    /// finished
+    Idle,
+    /// The sequence helper is executing a sequence and no replies need to be awaited.
+    Busy,
+    /// The sequence helper is still awaiting a reply from a mode children. The reply awaition
+    /// is a property of a mode commanding sequence
+    AwaitingSuccessCheck,
+}
+
+#[derive(Debug, PartialEq, Eq)]
+pub enum ModeCommandingResult {
+    /// The commanding of all children is finished
+    Done,
+    /// One step of a commanding chain is finished
+    StepDone,
+    /// Reply awaition is required for some children
+    AwaitingSuccessCheck,
+}
+
+#[derive(Debug, thiserror::Error)]
+#[error("mode {0} does not exist")]
+pub struct ModeDoesNotExistError(Mode);
+
+#[derive(Debug, thiserror::Error)]
+pub enum StartSequenceError {
+    #[error("mode {0} does not exist")]
+    ModeDoesNotExist(#[from] ModeDoesNotExistError),
+    #[error("invalid request ID")]
+    InvalidRequestId(RequestId),
+}
+
+/// This sequence execution helper includes some boilerplate logic to
+/// execute [SequenceModeTables].
+///
+/// It takes care of commanding the [ModeRequest]s specified in those tables and also includes the
+/// states required to track the current progress of a sequence execution and take care of
+/// reply and success awaition.
+#[derive(Debug)]
+pub struct SequenceExecutionHelper {
+    target_mode: Option<Mode>,
+    state: SequenceExecutionHelperState,
+    request_id: Option<RequestId>,
+    current_sequence_index: Option<usize>,
+    last_sequence_index: Option<usize>,
+}
+
+impl Default for SequenceExecutionHelper {
+    fn default() -> Self {
+        Self {
+            target_mode: None,
+            state: SequenceExecutionHelperState::Idle,
+            request_id: None,
+            current_sequence_index: None,
+            last_sequence_index: None,
+        }
+    }
+}
+
+pub trait IsChildCommandable {
+    fn is_commandable(&self, id: ComponentId) -> bool;
+}
+
+impl<T> IsChildCommandable for T
+where
+    T: HealthTableProvider,
+{
+    fn is_commandable(&self, id: ComponentId) -> bool {
+        self.health(id)
+            .is_none_or(|h| h != HealthState::ExternalControl)
+    }
+}
+
+impl SequenceExecutionHelper {
+    pub fn new() -> Self {
+        Default::default()
+    }
+
+    /// Load a new mode sequence to be executed
+    pub fn load(
+        &mut self,
+        mode: Mode,
+        request_id: RequestId,
+        sequence_tables: &SequenceModeTables,
+    ) -> Result<(), ModeDoesNotExistError> {
+        if !sequence_tables.0.contains_key(&mode) {
+            return Err(ModeDoesNotExistError(mode));
+        }
+        self.target_mode = Some(mode);
+        self.request_id = Some(request_id);
+        self.state = SequenceExecutionHelperState::Busy;
+        self.current_sequence_index = None;
+        Ok(())
+    }
+
+    /// Run the sequence execution helper.
+    ///
+    /// This function will execute the sequence in the given [SequenceModeTables] based on the
+    /// mode loaded in [Self::load]. It calls [Self::execute_sequence_and_map_to_result] and
+    /// automatically takes care of state management, including increments of the sequence table
+    /// index.
+    ///
+    /// The returnvalues of the helper have the following meaning.
+    ///
+    /// * [ModeCommandingResult::AwaitingSuccessCheck] - The sequence is still awaiting a success.
+    ///   The user should check whether all children have reached the commanded target mode, for
+    ///   example by checking [mode replies][ModeReply] received by the children components, and
+    ///   then calling [Self::confirm_sequence_done] to advance to the sequence or complete the
+    ///   sequence.
+    /// * [ModeCommandingResult::CommandingDone] - The sequence is done. The user can load a new
+    ///   sequence now without overwriting the last one. The sequence executor is in
+    ///   [SequenceExecutionHelperState::Idle] again.
+    /// * [ModeCommandingResult::CommandingStepDone] - The sequence has advanced one step. The user
+    ///   can now call [Self::run] again to immediately execute the next step in the sequence.
+    ///
+    /// Generally, periodic execution of the [Self::run] method should be performed while
+    /// [Self::state] is not [SequenceExecutionHelperState::Idle].
+    ///
+    /// # Arguments
+    ///
+    /// * `table` - This table contains the sequence tables to reach the mode previously loaded
+    ///   with [Self::load]
+    /// * `sender` - The sender to send mode requests to the components
+    /// * `children_mode_store` - The mode store vector to keep track of the mode states of
+    ///    children components
+    pub fn run(
+        &mut self,
+        table: &SequenceModeTables,
+        sender: &impl ModeRequestSender,
+        children_mode_store: &mut ModeStoreVec,
+        is_commandable: &impl IsChildCommandable,
+    ) -> Result<ModeCommandingResult, GenericTargetedMessagingError> {
+        if self.state == SequenceExecutionHelperState::Idle {
+            return Ok(ModeCommandingResult::Done);
+        }
+        if self.state == SequenceExecutionHelperState::AwaitingSuccessCheck {
+            return Ok(ModeCommandingResult::AwaitingSuccessCheck);
+        }
+        if self.target_mode.is_none() {
+            return Ok(ModeCommandingResult::Done);
+        }
+        match self.current_sequence_index {
+            Some(idx) => {
+                // Execute the sequence.
+                let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap();
+                self.execute_sequence_and_map_to_result(
+                    seq_table_value,
+                    idx,
+                    sender,
+                    children_mode_store,
+                    is_commandable,
+                )
+            }
+            None => {
+                // Find the first sequence
+                let seq_table_value = table.0.get(&self.target_mode.unwrap()).unwrap();
+                self.last_sequence_index = Some(seq_table_value.entries.len() - 1);
+                if seq_table_value.entries.is_empty() {
+                    Ok(ModeCommandingResult::Done)
+                } else {
+                    self.current_sequence_index = Some(0);
+                    self.execute_sequence_and_map_to_result(
+                        seq_table_value,
+                        0,
+                        sender,
+                        children_mode_store,
+                        is_commandable,
+                    )
+                }
+            }
+        }
+    }
+
+    /// Retrieve the currently loaded target mode
+    pub fn target_mode(&self) -> Option<Mode> {
+        self.target_mode
+    }
+
+    /// Confirm that a sequence which is awaiting a success check is done
+    pub fn confirm_sequence_done(&mut self) {
+        if let SequenceExecutionHelperState::AwaitingSuccessCheck = self.state {
+            self.state = SequenceExecutionHelperState::Busy;
+            if let (Some(last_sequence_index), Some(current_sequence_index)) =
+                (self.last_sequence_index, self.current_sequence_index)
+            {
+                if current_sequence_index == last_sequence_index {
+                    self.state = SequenceExecutionHelperState::Idle;
+                }
+            }
+            self.current_sequence_index = Some(self.current_sequence_index.unwrap() + 1);
+        }
+    }
+
+    /// Internal state of the execution helper.
+    pub fn state(&self) -> SequenceExecutionHelperState {
+        self.state
+    }
+
+    pub fn request_id(&self) -> Option<RequestId> {
+        self.request_id
+    }
+
+    pub fn set_request_id(&mut self, request_id: RequestId) {
+        self.request_id = Some(request_id);
+    }
+
+    pub fn awaiting_success_check(&self) -> bool {
+        self.state == SequenceExecutionHelperState::AwaitingSuccessCheck
+    }
+
+    pub fn current_sequence_index(&self) -> Option<usize> {
+        self.current_sequence_index
+    }
+
+    /// Execute a sequence at the given sequence index for a given [SequenceTablesMapValue].
+    ///
+    /// This method calls [Self::execute_sequence] and maps the result to a [ModeCommandingResult].
+    /// It is also called by the [Self::run] method of this helper.
+    pub fn execute_sequence_and_map_to_result(
+        &mut self,
+        seq_table_value: &SequenceTablesMapValue,
+        sequence_idx: usize,
+        sender: &impl ModeRequestSender,
+        mode_store_vec: &mut ModeStoreVec,
+        is_commandable: &impl IsChildCommandable,
+    ) -> Result<ModeCommandingResult, GenericTargetedMessagingError> {
+        if self.state() == SequenceExecutionHelperState::Idle || self.request_id.is_none() {
+            return Ok(ModeCommandingResult::Done);
+        }
+        if Self::execute_sequence(
+            self.request_id.unwrap(),
+            &seq_table_value.entries[sequence_idx],
+            sender,
+            mode_store_vec,
+            is_commandable,
+        )? {
+            self.state = SequenceExecutionHelperState::AwaitingSuccessCheck;
+            Ok(ModeCommandingResult::AwaitingSuccessCheck)
+        } else if seq_table_value.entries.len() - 1 == sequence_idx {
+            self.state = SequenceExecutionHelperState::Idle;
+            return Ok(ModeCommandingResult::Done);
+        } else {
+            self.current_sequence_index = Some(sequence_idx + 1);
+            return Ok(ModeCommandingResult::StepDone);
+        }
+    }
+
+    /// Generic stateless execution helper method.
+    ///
+    /// The [RequestId] and the [SequenceTableMapTable] to be executed are passed explicitely
+    /// here. This method is called by [Self::execute_sequence_and_map_to_result].
+    ///
+    /// This method itereates through the entries of the given sequence table and sends out
+    /// [ModeRequest]s to set the modes of the children according to the table entries.
+    /// It also sets the reply awaition field in the children mode store where a success
+    /// check is required to true.
+    ///
+    /// It returns whether any commanding success check is required by any entry in the table.
+    pub fn execute_sequence(
+        request_id: RequestId,
+        map_table: &SequenceTableMapTable,
+        sender: &impl ModeRequestSender,
+        children_mode_store: &mut ModeStoreVec,
+        commandable: &impl IsChildCommandable,
+    ) -> Result<bool, GenericTargetedMessagingError> {
+        let mut some_succes_check_required = false;
+        for entry in &map_table.entries {
+            if !commandable.is_commandable(entry.common.target_id) {
+                continue;
+            }
+            sender.send_mode_request(
+                request_id,
+                entry.common.target_id,
+                ModeRequest::SetMode {
+                    mode_and_submode: entry.common.mode_submode,
+                    forced: false,
+                },
+            )?;
+            if entry.check_success {
+                children_mode_store.0.iter_mut().for_each(|val| {
+                    if val.id() == entry.common.target_id {
+                        val.awaiting_reply = true;
+                    }
+                });
+                some_succes_check_required = true;
+            }
+        }
+        Ok(some_succes_check_required)
+    }
+}
+
+#[derive(Debug, Default, PartialEq, Eq, Clone, Copy)]
+pub enum ModeTreeHelperState {
+    #[default]
+    Idle,
+    /// The helper is currently trying to keep a target mode.
+    TargetKeeping,
+    /// The helper is currently busy to command a mode.
+    ModeCommanding,
+}
+
+#[derive(Debug, Default, PartialEq, Eq)]
+pub enum SubsystemHelperResult {
+    #[default]
+    Idle,
+    /// Busy with target keeping.
+    TargetKeeping,
+    /// Result of a mode commanding operation
+    ModeCommanding(ModeCommandingResult),
+}
+
+impl From<ModeCommandingResult> for SubsystemHelperResult {
+    fn from(value: ModeCommandingResult) -> Self {
+        Self::ModeCommanding(value)
+    }
+}
+
+#[derive(Debug, thiserror::Error)]
+pub enum ModeTreeHelperError {
+    #[error("generic targeted messaging error: {0}")]
+    Message(#[from] GenericTargetedMessagingError),
+    #[error("current mode {0} is not contained in target table")]
+    CurrentModeNotInTargetTable(Mode),
+    /// Mode command has failed, for example while executing a mode table.
+    #[error("mode command failed")]
+    ModeCommmandFailure {
+        /// Table index of the sequence table entry which failed.
+        seq_table_index: Option<usize>,
+    },
+    /// Target mode keeping violation.
+    #[error("target keeping violation")]
+    TargetKeepingViolation {
+        /// Table index of the sequence table entry which failed.
+        fallback_mode: Option<Mode>,
+    },
+}
+
+/// This is a helper object which can be used by a subsystem component to execute mode sequences
+/// and perform target keeping.
+///
+/// This helper object tries to compose as much data and state information as possible which is
+/// required for this process.
+pub struct SubsystemCommandingHelper {
+    /// State of the helper.
+    state: ModeTreeHelperState,
+    /// Current mode of the owner subsystem.
+    current_mode: Mode,
+    /// This data structure is used to track all mode children.
+    pub children_mode_store: ModeStoreVec,
+    /// This field is set when a mode sequence is executed. It is used to determine whether mode
+    /// replies are relevant for reply awaition logic.
+    active_internal_request_id: Option<RequestId>,
+    /// The primary data structure to keep the target state information for subsystem
+    /// [modes][Mode]. it specifies the mode each child should have for a certain subsystem mode
+    /// and is relevant for target keeping.
+    pub target_tables: TargetModeTables,
+    /// The primary data structure to keep the sequence commanding information for commanded
+    /// subsystem [modes][Mode]. It specifies the actual commands and the order they should be
+    /// sent in to reach a certain [mode][Mode].
+    pub sequence_tables: SequenceModeTables,
+    /// The sequence execution helper is used to execute sequences in the [Self::sequence_tables].
+    pub seq_exec_helper: SequenceExecutionHelper,
+}
+
+impl Default for SubsystemCommandingHelper {
+    fn default() -> Self {
+        Self {
+            current_mode: UNKNOWN_MODE_VAL,
+            state: Default::default(),
+            children_mode_store: Default::default(),
+            active_internal_request_id: None,
+            target_tables: Default::default(),
+            sequence_tables: Default::default(),
+            seq_exec_helper: Default::default(),
+        }
+    }
+}
+
+impl SubsystemCommandingHelper {
+    /// Create a new substem commanding helper with an intial [ModeTreeHelperState::Idle] state,
+    /// an empty mode children store and empty target and sequence mode tables.
+    pub fn new(
+        children_mode_store: ModeStoreVec,
+        target_tables: TargetModeTables,
+        sequence_tables: SequenceModeTables,
+    ) -> Self {
+        Self {
+            current_mode: UNKNOWN_MODE_VAL,
+            state: ModeTreeHelperState::Idle,
+            children_mode_store,
+            active_internal_request_id: None,
+            target_tables,
+            sequence_tables,
+            seq_exec_helper: Default::default(),
+        }
+    }
+
+    pub fn state(&self) -> ModeTreeHelperState {
+        self.state
+    }
+
+    pub fn mode(&self) -> Mode {
+        self.current_mode
+    }
+
+    pub fn request_id(&self) -> Option<RequestId> {
+        self.active_internal_request_id.map(|v| v >> 8)
+    }
+
+    /// This returns the internal request ID, which is the regular [Self::request_id] specified
+    /// by the user shifter 8 to the right and then increment with the current sequence commanding
+    /// step. The value can still be retrieved because it might be required for reply verification.
+    ///
+    /// The state machine specifies this request ID for all mode commands related to the
+    /// current step of sequence commanding.
+    pub fn internal_request_id(&self) -> Option<RequestId> {
+        self.active_internal_request_id
+    }
+
+    /// Retrieve the fallback mode for the current mode of the subsystem by trying to retrieve
+    /// it from the target table.
+    ///
+    /// If the current mode does not have a fallback mode, returns [None].
+    /// If the current mode is not inside the target table, returns a [ModeDoesNotExistError].
+    /// The fallback mode can and should be commanded when a target keeping violation was detected
+    /// or after self-commanding to the current mode has failed, which can happen after a failed
+    /// mode table execution.
+    pub fn fallback_mode(&self) -> Result<Option<Mode>, ModeDoesNotExistError> {
+        self.target_tables
+            .0
+            .get(&self.current_mode)
+            .ok_or(ModeDoesNotExistError(self.current_mode))
+            .map(|v| v.fallback_mode)
+    }
+
+    /// Add a mode child to the internal [Self::children_mode_store].
+    pub fn add_mode_child(&mut self, child: ComponentId, mode: ModeAndSubmode) {
+        self.children_mode_store.add_component(child, mode);
+    }
+
+    /// Add a target mode table and an associated sequence mode table.
+    pub fn add_target_and_sequence_table(
+        &mut self,
+        mode: Mode,
+        target_table_val: TargetTablesMapValue,
+        sequence_table_val: SequenceTablesMapValue,
+    ) {
+        self.target_tables.0.insert(mode, target_table_val);
+        self.sequence_tables.0.insert(mode, sequence_table_val);
+    }
+
+    /// Starts a command sequence for a given [mode][Mode].
+    ///
+    /// # Arguments
+    ///
+    /// - `mode` - The mode to command
+    /// - `request_id` - Request ID associated with the command sequence. The value of this value
+    ///   should not be larger than the maximum possible value for 24 bits: (2 ^ 24) - 1 = 16777215
+    ///   because 8 bits are reserved for internal sequence index tracking.
+    pub fn start_command_sequence(
+        &mut self,
+        mode: Mode,
+        request_id: RequestId,
+    ) -> Result<(), StartSequenceError> {
+        if request_id > 2_u32.pow(24) - 1 {
+            return Err(StartSequenceError::InvalidRequestId(request_id));
+        }
+        self.active_internal_request_id = Some(request_id << 8);
+        self.seq_exec_helper.load(
+            mode,
+            self.active_internal_request_id.unwrap(),
+            &self.sequence_tables,
+        )?;
+        self.state = ModeTreeHelperState::ModeCommanding;
+        Ok(())
+    }
+
+    pub fn send_announce_mode_cmd_to_children(
+        &self,
+        request_id: RequestId,
+        req_sender: &impl ModeRequestSender,
+        recursive: bool,
+    ) -> Result<(), GenericTargetedMessagingError> {
+        let mut request = ModeRequest::AnnounceMode;
+        if recursive {
+            request = ModeRequest::AnnounceModeRecursive;
+        }
+        for child in &self.children_mode_store.0 {
+            req_sender.send_mode_request(request_id, child.id(), request)?;
+        }
+        Ok(())
+    }
+
+    pub fn state_machine(
+        &mut self,
+        opt_reply: Option<GenericMessage<ModeReply>>,
+        req_sender: &impl ModeRequestSender,
+        is_commandable: &impl IsChildCommandable,
+    ) -> Result<SubsystemHelperResult, ModeTreeHelperError> {
+        if let Some(reply) = opt_reply {
+            if self.handle_mode_reply(&reply)? {
+                if self.seq_exec_helper.state() == SequenceExecutionHelperState::Idle {
+                    self.transition_to_target_keeping();
+                    return Ok(SubsystemHelperResult::ModeCommanding(
+                        ModeCommandingResult::Done,
+                    ));
+                }
+                return Ok(SubsystemHelperResult::ModeCommanding(
+                    ModeCommandingResult::StepDone,
+                ));
+            }
+        }
+        match self.state {
+            ModeTreeHelperState::Idle => Ok(SubsystemHelperResult::Idle),
+            ModeTreeHelperState::TargetKeeping => {
+                // We check whether the current mode is modelled by a target table first.
+                if let Some(target_table) = self.target_tables.0.get(&self.current_mode) {
+                    self.perform_target_keeping(target_table)?;
+                }
+                Ok(SubsystemHelperResult::TargetKeeping)
+            }
+            ModeTreeHelperState::ModeCommanding => {
+                let result = self.seq_exec_helper.run(
+                    &self.sequence_tables,
+                    req_sender,
+                    &mut self.children_mode_store,
+                    is_commandable,
+                )?;
+                match result {
+                    ModeCommandingResult::Done => {
+                        // By default, the helper will automatically transition into the target keeping
+                        // mode after an executed sequence.
+                        self.transition_to_target_keeping();
+                    }
+                    ModeCommandingResult::StepDone => {
+                        // Normally, this step is done after all replies were received, but if no
+                        // reply checking is required for a command sequence, the step would never
+                        // be performed, so this function needs to be called here as well.
+                        self.update_internal_req_id();
+                    }
+                    ModeCommandingResult::AwaitingSuccessCheck => (),
+                }
+                Ok(result.into())
+            }
+        }
+    }
+
+    fn transition_to_target_keeping(&mut self) {
+        self.state = ModeTreeHelperState::TargetKeeping;
+        self.current_mode = self.seq_exec_helper.target_mode().unwrap();
+    }
+
+    fn perform_target_keeping(
+        &self,
+        target_table: &TargetTablesMapValue,
+    ) -> Result<(), ModeTreeHelperError> {
+        for entry in &target_table.entries {
+            if !entry.monitor_state {
+                continue;
+            }
+            let mut target_mode_violated = false;
+            self.children_mode_store.0.iter().for_each(|val| {
+                if val.id() == entry.common.target_id {
+                    target_mode_violated =
+                        if let Some(allowed_submode_mask) = entry.allowed_submode_mask() {
+                            let fixed_bits = !allowed_submode_mask;
+                            (val.mode_and_submode().mode() != entry.common.mode_submode.mode())
+                                && (val.mode_and_submode().submode() & fixed_bits
+                                    != entry.common.mode_submode.submode() & fixed_bits)
+                        } else {
+                            val.mode_and_submode() != entry.common.mode_submode
+                        };
+                }
+            });
+            if target_mode_violated {
+                // Target keeping violated. Report violation and fallback mode to user.
+                return Err(ModeTreeHelperError::TargetKeepingViolation {
+                    fallback_mode: target_table.fallback_mode,
+                });
+            }
+        }
+        Ok(())
+    }
+
+    fn update_internal_req_id(&mut self) {
+        let new_internal_req_id = self.request_id().unwrap() << 8
+            | self.seq_exec_helper.current_sequence_index().unwrap() as u32;
+        self.seq_exec_helper.set_request_id(new_internal_req_id);
+        self.active_internal_request_id = Some(new_internal_req_id);
+    }
+
+    // Handles a mode reply message and returns whether the reply completes a step of sequence
+    // commanding.
+    fn handle_mode_reply(
+        &mut self,
+        reply: &GenericMessage<ModeReply>,
+    ) -> Result<bool, ModeTreeHelperError> {
+        if !self.children_mode_store.has_component(reply.sender_id()) {
+            return Ok(false);
+        }
+        let mut generic_mode_reply_handler =
+            |sender_id, mode_and_submode: Option<ModeAndSubmode>, success: bool| {
+                let mut partial_step_done = false;
+                // Tying the reply awaition to the request ID ensures that something like replies
+                // belonging to older requests do not interfere with the completion handling of
+                // the mode commanding. This is important for forced mode commands.
+                let mut handle_awaition = false;
+                if self.state == ModeTreeHelperState::ModeCommanding
+                    && self.active_internal_request_id.is_some()
+                    && reply.request_id() == self.active_internal_request_id.unwrap()
+                {
+                    handle_awaition = true;
+                }
+                let still_awating_replies = self.children_mode_store.mode_reply_handler(
+                    sender_id,
+                    mode_and_submode,
+                    handle_awaition,
+                );
+                if self.state == ModeTreeHelperState::ModeCommanding
+                    && handle_awaition
+                    && !still_awating_replies.unwrap_or(false)
+                {
+                    self.seq_exec_helper.confirm_sequence_done();
+                    self.update_internal_req_id();
+                    partial_step_done = true;
+                }
+                if !success && self.state == ModeTreeHelperState::ModeCommanding {
+                    // The user has to decide how to proceed.
+                    self.state = ModeTreeHelperState::Idle;
+                    return Err(ModeTreeHelperError::ModeCommmandFailure {
+                        seq_table_index: self.seq_exec_helper.current_sequence_index(),
+                    });
+                }
+                Ok(partial_step_done)
+            };
+        match reply.message {
+            ModeReply::ModeInfo(mode_and_submode) => {
+                generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true)
+            }
+            ModeReply::ModeReply(mode_and_submode) => {
+                generic_mode_reply_handler(reply.sender_id(), Some(mode_and_submode), true)
+            }
+            ModeReply::CantReachMode(_) => {
+                generic_mode_reply_handler(reply.sender_id(), None, false)
+            }
+            ModeReply::WrongMode { reached, .. } => {
+                generic_mode_reply_handler(reply.sender_id(), Some(reached), true)
+            }
+        }
+    }
+
+    pub fn update_child_mode(
+        &mut self,
+        child: ComponentId,
+        mode: ModeAndSubmode,
+    ) -> Result<(), TargetNotInModeStoreError> {
+        let val_mut = self
+            .children_mode_store
+            .get_mut(child)
+            .ok_or(TargetNotInModeStoreError(child))?;
+        val_mut.mode_and_submode = mode;
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    use crate::{
+        mode::{
+            tests::{ModeReqSenderMock, ModeReqWrapper},
+            Mode, ModeAndSubmode, ModeReply, ModeRequest, UNKNOWN_MODE,
+        },
+        mode_tree::{
+            ModeStoreProvider, ModeStoreVec, SequenceModeTables, SequenceTableEntry,
+            SequenceTableMapTable, SequenceTablesMapValue, TargetModeTables,
+        },
+        queue::GenericTargetedMessagingError,
+        request::{GenericMessage, MessageMetadata, RequestId},
+        subsystem::{ModeCommandingResult, ModeTreeHelperState, SequenceExecutionHelperState},
+        ComponentId,
+    };
+
+    #[derive(Debug)]
+    pub enum ExampleTargetId {
+        Target0 = 1,
+        Target1 = 2,
+        Target2 = 3,
+    }
+
+    #[derive(Debug)]
+    pub enum ExampleMode {
+        Mode0 = 1,
+        Mode1 = 2,
+        Mode2 = 3,
+    }
+
+    #[derive(Debug, Default)]
+    pub struct IsCommandableMock {
+        pub commandable_map: std::collections::HashMap<ComponentId, bool>,
+    }
+
+    impl IsChildCommandable for IsCommandableMock {
+        fn is_commandable(&self, id: ComponentId) -> bool {
+            self.commandable_map.get(&id).copied().unwrap_or(true)
+        }
+    }
+
+    pub struct SequenceExecutorTestbench {
+        pub sender: ModeReqSenderMock,
+        pub mode_store: ModeStoreVec,
+        pub seq_tables: SequenceModeTables,
+        pub execution_helper: SequenceExecutionHelper,
+        pub is_commandable_mock: IsCommandableMock,
+    }
+
+    impl SequenceExecutorTestbench {
+        pub fn new() -> Self {
+            let mode_store = create_default_mode_store();
+            let (seq_tables, _) = create_simple_sample_seq_tables();
+            Self {
+                sender: ModeReqSenderMock::default(),
+                mode_store,
+                seq_tables,
+                execution_helper: SequenceExecutionHelper::new(),
+                is_commandable_mock: IsCommandableMock::default(),
+            }
+        }
+
+        pub fn get_mode_table(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue {
+            self.seq_tables.0.get_mut(&(mode as Mode)).unwrap()
+        }
+
+        pub fn run(&mut self) -> Result<ModeCommandingResult, GenericTargetedMessagingError> {
+            self.execution_helper.run(
+                &self.seq_tables,
+                &self.sender,
+                &mut self.mode_store,
+                &self.is_commandable_mock,
+            )
+        }
+
+        fn check_run_is_no_op(&mut self) {
+            // Assure that no unexpected behaviour occurs.
+            assert_eq!(
+                self.execution_helper
+                    .run(
+                        &self.seq_tables,
+                        &self.sender,
+                        &mut self.mode_store,
+                        &self.is_commandable_mock
+                    )
+                    .unwrap(),
+                ModeCommandingResult::Done
+            );
+            assert_eq!(
+                self.execution_helper.state(),
+                SequenceExecutionHelperState::Idle
+            );
+            assert!(self.sender.requests.borrow().is_empty());
+        }
+
+        fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) {
+            assert_eq!(
+                self.execution_helper.target_mode().unwrap(),
+                ExampleMode::Mode1 as Mode
+            );
+            assert_eq!(self.sender.requests.borrow().len(), 2);
+            let req_0 = self.sender.requests.get_mut().pop_front().unwrap();
+            assert_eq!(req_0.target_id, ExampleTargetId::Target0 as u64);
+            assert_eq!(req_0.request_id, expected_req_id);
+            assert_eq!(
+                req_0.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE,
+                    forced: false
+                }
+            );
+            let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap();
+            assert_eq!(req_1.target_id, ExampleTargetId::Target1 as u64);
+            assert_eq!(
+                req_1.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE,
+                    forced: false
+                }
+            );
+        }
+        fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) {
+            assert_eq!(
+                self.execution_helper.target_mode().unwrap(),
+                ExampleMode::Mode1 as Mode
+            );
+            assert_eq!(self.sender.requests.borrow().len(), 1);
+            let req_0 = self.sender.requests.get_mut().pop_front().unwrap();
+            assert_eq!(req_0.target_id, ExampleTargetId::Target2 as u64);
+            assert_eq!(req_0.request_id, expected_req_id);
+            assert_eq!(
+                req_0.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE,
+                    forced: false
+                }
+            );
+        }
+
+        fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) {
+            assert_eq!(
+                self.execution_helper.target_mode().unwrap(),
+                ExampleMode::Mode0 as Mode
+            );
+            assert_eq!(self.execution_helper.current_sequence_index().unwrap(), 0);
+            assert_eq!(self.sender.requests.borrow().len(), 2);
+            let req_0 = self.sender.requests.get_mut().pop_front().unwrap();
+            assert_eq!(req_0.target_id, ExampleTargetId::Target0 as u64);
+            assert_eq!(req_0.request_id, expected_req_id);
+            assert_eq!(
+                req_0.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE,
+                    forced: false
+                }
+            );
+            let req_1 = self.sender.requests.borrow_mut().pop_front().unwrap();
+            assert_eq!(req_1.target_id, ExampleTargetId::Target1 as u64);
+            assert_eq!(
+                req_1.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE,
+                    forced: false
+                }
+            );
+        }
+    }
+
+    fn create_default_mode_store() -> ModeStoreVec {
+        let mut mode_store = ModeStoreVec::default();
+        mode_store.add_component(ExampleTargetId::Target0 as u64, UNKNOWN_MODE);
+        mode_store.add_component(ExampleTargetId::Target1 as u64, UNKNOWN_MODE);
+        mode_store.add_component(ExampleTargetId::Target2 as u64, UNKNOWN_MODE);
+        mode_store
+    }
+
+    fn create_simple_sample_seq_tables() -> (SequenceModeTables, TargetModeTables) {
+        let mut seq_tables = SequenceModeTables::default();
+        // Mode 0 - One step command
+        let mut table_val = SequenceTablesMapValue::new("MODE_0");
+        let mut table_seq_0 = SequenceTableMapTable::new("MODE_0_SEQ_0");
+        table_seq_0.add_entry(SequenceTableEntry::new(
+            "TARGET_0",
+            ExampleTargetId::Target0 as u64,
+            SUBSYSTEM_MD0_TGT0_MODE,
+            false,
+        ));
+        table_seq_0.add_entry(SequenceTableEntry::new(
+            "TARGET_1",
+            ExampleTargetId::Target1 as u64,
+            SUBSYSTEM_MD0_TGT1_MODE,
+            false,
+        ));
+        table_val.add_sequence_table(table_seq_0);
+        seq_tables.0.insert(ExampleMode::Mode0 as u32, table_val);
+
+        // Mode 1 - Multi Step command
+        let mut table_val = SequenceTablesMapValue::new("MODE_1");
+        let mut table_seq_0 = SequenceTableMapTable::new("MODE_1_SEQ_0");
+        table_seq_0.add_entry(SequenceTableEntry::new(
+            "MD1_SEQ0_TGT0",
+            ExampleTargetId::Target0 as u64,
+            SUBSYSTEM_MD1_ST0_TGT0_MODE,
+            false,
+        ));
+        table_seq_0.add_entry(SequenceTableEntry::new(
+            "MD1_SEQ0_TGT1",
+            ExampleTargetId::Target1 as u64,
+            SUBSYSTEM_MD1_ST0_TGT1_MODE,
+            false,
+        ));
+        table_val.add_sequence_table(table_seq_0);
+        let mut table_seq_1 = SequenceTableMapTable::new("MODE_1_SEQ_1");
+        table_seq_1.add_entry(SequenceTableEntry::new(
+            "MD1_SEQ1_TGT2",
+            ExampleTargetId::Target2 as u64,
+            SUBSYSTEM_MD1_ST1_TGT2_MODE,
+            false,
+        ));
+        table_val.add_sequence_table(table_seq_1);
+        seq_tables.0.insert(ExampleMode::Mode1 as u32, table_val);
+
+        let mode_tables = TargetModeTables::default();
+        // TODO: Write mode tables.
+        (seq_tables, mode_tables)
+    }
+
+    pub struct SubsystemHelperTestbench {
+        pub sender: ModeReqSenderMock,
+        pub helper: SubsystemCommandingHelper,
+        pub is_commandable_mock: IsCommandableMock,
+    }
+
+    impl SubsystemHelperTestbench {
+        pub fn new() -> Self {
+            let (sequence_tables, target_tables) = create_simple_sample_seq_tables();
+            Self {
+                sender: ModeReqSenderMock::default(),
+                helper: SubsystemCommandingHelper::new(
+                    create_default_mode_store(),
+                    target_tables,
+                    sequence_tables,
+                ),
+                is_commandable_mock: IsCommandableMock::default(),
+            }
+        }
+
+        pub fn start_command_sequence(
+            &mut self,
+            mode: ExampleMode,
+            request_id: RequestId,
+        ) -> Result<(), StartSequenceError> {
+            self.helper.start_command_sequence(mode as Mode, request_id)
+        }
+
+        pub fn send_announce_mode_cmd_to_children(
+            &mut self,
+            request_id: RequestId,
+            recursive: bool,
+        ) -> Result<(), GenericTargetedMessagingError> {
+            self.helper
+                .send_announce_mode_cmd_to_children(request_id, &self.sender, recursive)
+        }
+
+        pub fn get_sequence_tables(&mut self, mode: ExampleMode) -> &mut SequenceTablesMapValue {
+            self.helper
+                .sequence_tables
+                .0
+                .get_mut(&(mode as Mode))
+                .unwrap()
+        }
+
+        pub fn state_machine(
+            &mut self,
+            opt_reply: Option<GenericMessage<ModeReply>>,
+        ) -> Result<SubsystemHelperResult, ModeTreeHelperError> {
+            self.helper
+                .state_machine(opt_reply, &self.sender, &self.is_commandable_mock)
+        }
+
+        pub fn generic_checks_subsystem_md0(&mut self, expected_req_id: RequestId) {
+            assert_eq!(self.sender.requests.borrow().len(), 2);
+            let req0 = self.sender.requests.borrow_mut().pop_front().unwrap();
+            assert_eq!(req0.request_id, expected_req_id);
+            assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId);
+            assert_eq!(
+                req0.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD0_TGT0_MODE,
+                    forced: false
+                }
+            );
+
+            let req1 = self.sender.requests.borrow_mut().pop_front().unwrap();
+            assert_eq!(req1.request_id, expected_req_id);
+            assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId);
+            assert_eq!(
+                req1.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD0_TGT1_MODE,
+                    forced: false
+                }
+            );
+        }
+
+        pub fn generic_checks_subsystem_md1_step0(&mut self, expected_req_id: RequestId) {
+            assert_eq!(self.sender.requests.borrow().len(), 2);
+            let req0 = self.sender.requests.borrow_mut().pop_front().unwrap();
+            assert_eq!(req0.request_id, expected_req_id);
+            assert_eq!(req0.target_id, ExampleTargetId::Target0 as ComponentId);
+            assert_eq!(
+                req0.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD1_ST0_TGT0_MODE,
+                    forced: false
+                }
+            );
+
+            let req1 = self.sender.requests.borrow_mut().pop_front().unwrap();
+            assert_eq!(req1.request_id, expected_req_id);
+            assert_eq!(req1.target_id, ExampleTargetId::Target1 as ComponentId);
+            assert_eq!(
+                req1.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD1_ST0_TGT1_MODE,
+                    forced: false
+                }
+            );
+        }
+
+        pub fn generic_checks_subsystem_md1_step1(&mut self, expected_req_id: RequestId) {
+            assert_eq!(self.sender.requests.borrow().len(), 1);
+            let req0 = self.sender.requests.borrow_mut().pop_front().unwrap();
+            assert_eq!(req0.request_id, expected_req_id);
+            assert_eq!(req0.target_id, ExampleTargetId::Target2 as ComponentId);
+            assert_eq!(
+                req0.request,
+                ModeRequest::SetMode {
+                    mode_and_submode: SUBSYSTEM_MD1_ST1_TGT2_MODE,
+                    forced: false
+                }
+            );
+        }
+    }
+
+    const SUBSYSTEM_MD0_TGT0_MODE: ModeAndSubmode =
+        ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0);
+    const SUBSYSTEM_MD0_TGT1_MODE: ModeAndSubmode =
+        ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0);
+
+    const SUBSYSTEM_MD1_ST0_TGT0_MODE: ModeAndSubmode =
+        ModeAndSubmode::new(ExampleMode::Mode2 as u32, 0);
+    const SUBSYSTEM_MD1_ST0_TGT1_MODE: ModeAndSubmode =
+        ModeAndSubmode::new(ExampleMode::Mode0 as u32, 0);
+    const SUBSYSTEM_MD1_ST1_TGT2_MODE: ModeAndSubmode =
+        ModeAndSubmode::new(ExampleMode::Mode1 as u32, 0);
+
+    #[test]
+    fn test_init_state() {
+        let execution_helper = SequenceExecutionHelper::new();
+        assert_eq!(execution_helper.state(), SequenceExecutionHelperState::Idle);
+        assert!(!execution_helper.awaiting_success_check());
+        assert!(execution_helper.target_mode().is_none());
+        assert!(execution_helper.current_sequence_index().is_none());
+    }
+
+    #[test]
+    fn test_sequence_execution_helper_no_success_check() {
+        let mut tb = SequenceExecutorTestbench::new();
+        let expected_req_id = 1;
+        tb.execution_helper
+            .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables)
+            .unwrap();
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Busy
+        );
+        assert!(!tb.execution_helper.awaiting_success_check());
+        assert_eq!(
+            tb.execution_helper.target_mode().unwrap(),
+            ExampleMode::Mode0 as Mode
+        );
+        assert_eq!(
+            tb.run().expect("sequence exeecution helper run failure"),
+            ModeCommandingResult::Done
+        );
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Idle
+        );
+        assert!(!tb.execution_helper.awaiting_success_check());
+        tb.generic_checks_subsystem_md0(expected_req_id);
+        tb.check_run_is_no_op();
+    }
+
+    #[test]
+    fn test_sequence_execution_helper_with_success_check() {
+        let mut tb = SequenceExecutorTestbench::new();
+        let mode0_table = tb.get_mode_table(ExampleMode::Mode0);
+        mode0_table.entries[0].entries[0].check_success = true;
+        mode0_table.entries[0].entries[1].check_success = true;
+        let expected_req_id = 1;
+        tb.execution_helper
+            .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables)
+            .unwrap();
+
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Busy
+        );
+        assert!(!tb.execution_helper.awaiting_success_check());
+        assert_eq!(
+            tb.execution_helper.target_mode().unwrap(),
+            ExampleMode::Mode0 as Mode
+        );
+        assert_eq!(
+            tb.run().expect("sequence exeecution helper run failure"),
+            ModeCommandingResult::AwaitingSuccessCheck
+        );
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::AwaitingSuccessCheck
+        );
+        // These are not cleared, even if the execution helper is already IDLE. This is okay for
+        // now.
+        assert!(tb.execution_helper.awaiting_success_check());
+        tb.generic_checks_subsystem_md0(expected_req_id);
+        tb.execution_helper.confirm_sequence_done();
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Idle
+        );
+
+        tb.check_run_is_no_op();
+    }
+
+    #[test]
+    fn test_sequence_execution_helper_with_partial_check() {
+        let mut tb = SequenceExecutorTestbench::new();
+        let mode0_table = tb.get_mode_table(ExampleMode::Mode0);
+        mode0_table.entries[0].entries[0].check_success = true;
+        let expected_req_id = 1;
+        tb.execution_helper
+            .load(ExampleMode::Mode0 as u32, expected_req_id, &tb.seq_tables)
+            .unwrap();
+
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Busy
+        );
+        assert!(!tb.execution_helper.awaiting_success_check());
+        assert_eq!(
+            tb.run().expect("sequence execution helper run failure"),
+            ModeCommandingResult::AwaitingSuccessCheck
+        );
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::AwaitingSuccessCheck
+        );
+        // These are not cleared, even if the execution helper is already IDLE. This is okay for
+        // now.
+        assert!(tb.execution_helper.awaiting_success_check());
+        tb.generic_checks_subsystem_md0(expected_req_id);
+        tb.execution_helper.confirm_sequence_done();
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Idle
+        );
+        tb.check_run_is_no_op();
+    }
+
+    #[test]
+    fn test_sequence_execution_helper_multi_step_no_success_check() {
+        let mut tb = SequenceExecutorTestbench::new();
+        let expected_req_id = 1;
+        tb.execution_helper
+            .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables)
+            .unwrap();
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Busy
+        );
+        assert!(!tb.execution_helper.awaiting_success_check());
+        assert_eq!(
+            tb.execution_helper.target_mode().unwrap(),
+            ExampleMode::Mode1 as Mode
+        );
+        assert_eq!(
+            tb.run().expect("sequence execution helper run failure"),
+            ModeCommandingResult::StepDone
+        );
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Busy
+        );
+        assert!(!tb.execution_helper.awaiting_success_check());
+        tb.generic_checks_subsystem_md1_step0(expected_req_id);
+        assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1);
+
+        assert_eq!(
+            tb.run().expect("sequence execution helper run failure"),
+            ModeCommandingResult::Done
+        );
+        tb.generic_checks_subsystem_md1_step1(expected_req_id);
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Idle
+        );
+        tb.check_run_is_no_op();
+    }
+
+    #[test]
+    fn test_sequence_execution_helper_multi_step_full_success_check() {
+        let mut tb = SequenceExecutorTestbench::new();
+        let expected_req_id = 1;
+        tb.execution_helper
+            .load(ExampleMode::Mode1 as u32, expected_req_id, &tb.seq_tables)
+            .unwrap();
+        let mode1_table = tb.get_mode_table(ExampleMode::Mode1);
+        mode1_table.entries[0].entries[0].check_success = true;
+        mode1_table.entries[0].entries[1].check_success = true;
+        mode1_table.entries[1].entries[0].check_success = true;
+
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::Busy
+        );
+        assert!(!tb.execution_helper.awaiting_success_check());
+        assert_eq!(
+            tb.execution_helper.target_mode().unwrap(),
+            ExampleMode::Mode1 as Mode
+        );
+        assert_eq!(
+            tb.run().expect("sequence execution helper run failure"),
+            ModeCommandingResult::AwaitingSuccessCheck
+        );
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::AwaitingSuccessCheck
+        );
+        assert!(tb.execution_helper.awaiting_success_check());
+        tb.generic_checks_subsystem_md1_step0(expected_req_id);
+        assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 0);
+        tb.execution_helper.confirm_sequence_done();
+
+        assert_eq!(
+            tb.run().expect("sequence execution helper run failure"),
+            ModeCommandingResult::AwaitingSuccessCheck
+        );
+        assert_eq!(
+            tb.execution_helper.state(),
+            SequenceExecutionHelperState::AwaitingSuccessCheck
+        );
+        assert!(tb.execution_helper.awaiting_success_check());
+        assert_eq!(tb.execution_helper.current_sequence_index().unwrap(), 1);
+        tb.generic_checks_subsystem_md1_step1(expected_req_id);
+        tb.execution_helper.confirm_sequence_done();
+        tb.check_run_is_no_op();
+    }
+
+    // TODO: Test subsystem commanding helper
+    #[test]
+    fn test_subsystem_helper_basic_state() {
+        let tb = SubsystemHelperTestbench::new();
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::Idle);
+        assert!(tb.helper.active_internal_request_id.is_none());
+        assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL);
+        assert!(tb.helper.request_id().is_none());
+    }
+
+    #[test]
+    fn test_subsystem_helper_announce_recursive() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        tb.send_announce_mode_cmd_to_children(expected_req_id, true)
+            .unwrap();
+        assert_eq!(tb.sender.requests.borrow().len(), 3);
+        let check_req = |req: ModeReqWrapper, target_id: ComponentId| {
+            assert_eq!(req.target_id, target_id);
+            assert_eq!(req.request_id, expected_req_id);
+            assert_eq!(req.request, ModeRequest::AnnounceModeRecursive);
+        };
+        let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap();
+        check_req(req0, ExampleTargetId::Target0 as u64);
+        let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap();
+        check_req(req1, ExampleTargetId::Target1 as u64);
+        let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap();
+        check_req(req2, ExampleTargetId::Target2 as u64);
+    }
+
+    #[test]
+    fn test_subsystem_helper_announce() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        tb.send_announce_mode_cmd_to_children(expected_req_id, false)
+            .unwrap();
+        assert_eq!(tb.sender.requests.borrow().len(), 3);
+        let check_req = |req: ModeReqWrapper, target_id: ComponentId| {
+            assert_eq!(req.target_id, target_id);
+            assert_eq!(req.request_id, expected_req_id);
+            assert_eq!(req.request, ModeRequest::AnnounceMode);
+        };
+        let req0 = tb.sender.requests.borrow_mut().pop_front().unwrap();
+        check_req(req0, ExampleTargetId::Target0 as u64);
+        let req1 = tb.sender.requests.borrow_mut().pop_front().unwrap();
+        check_req(req1, ExampleTargetId::Target1 as u64);
+        let req2 = tb.sender.requests.borrow_mut().pop_front().unwrap();
+        check_req(req2, ExampleTargetId::Target2 as u64);
+    }
+
+    #[test]
+    fn test_subsystem_helper_cmd_mode0_no_success_checks() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        tb.start_command_sequence(ExampleMode::Mode0, expected_req_id)
+            .unwrap();
+        assert_eq!(tb.helper.request_id().unwrap(), 1);
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.sender.requests.borrow().len(), 0);
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done)
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode);
+        tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap());
+        // FSM call should be a no-op.
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::TargetKeeping
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode);
+    }
+
+    #[test]
+    fn test_subsystem_helper_cmd_mode1_no_success_checks() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        tb.start_command_sequence(ExampleMode::Mode1, expected_req_id)
+            .unwrap();
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.sender.requests.borrow().len(), 0);
+        // Need to cache this before it is incremented, because it is incremented
+        // immediately in the state machine (no reply checking)
+        let expected_req_id = tb.helper.internal_request_id().unwrap();
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone)
+        );
+        // Assert that this was already incremented because no reply checking is necessary.
+        assert_eq!(
+            tb.helper.internal_request_id().unwrap(),
+            expected_req_id + 1
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL);
+        tb.generic_checks_subsystem_md1_step0(expected_req_id);
+        // Second commanding step.
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done)
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode);
+        tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap());
+
+        // FSM call should be a no-op.
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::TargetKeeping
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode);
+    }
+
+    #[test]
+    fn test_subsystem_helper_cmd_mode0_with_success_checks() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        let seq_tables = tb.get_sequence_tables(ExampleMode::Mode0);
+        seq_tables.entries[0].entries[0].check_success = true;
+        seq_tables.entries[0].entries[1].check_success = true;
+        tb.start_command_sequence(ExampleMode::Mode0, expected_req_id)
+            .unwrap();
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.sender.requests.borrow().len(), 0);
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL);
+        tb.generic_checks_subsystem_md0(tb.helper.internal_request_id().unwrap());
+        let mode_reply_ok_0 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target0 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE),
+        );
+        let mode_reply_ok_1 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target1 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE),
+        );
+        // One success reply still expected.
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_0)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_1)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done)
+        );
+
+        // FSM call should be a no-op.
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::TargetKeeping
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode0 as Mode);
+    }
+
+    #[test]
+    fn test_subsystem_helper_cmd_mode1_with_success_checks() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1);
+        seq_tables.entries[0].entries[0].check_success = true;
+        seq_tables.entries[0].entries[1].check_success = true;
+        seq_tables.entries[1].entries[0].check_success = true;
+        tb.start_command_sequence(ExampleMode::Mode1, expected_req_id)
+            .unwrap();
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.sender.requests.borrow().len(), 0);
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL);
+        tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap());
+        let mode_reply_ok_0 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target0 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE),
+        );
+        let mode_reply_ok_1 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target1 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE),
+        );
+        // One success reply still expected.
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_0)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_1)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone)
+        );
+
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        let mode_reply_ok = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target2 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE),
+        );
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done)
+        );
+
+        // FSM call should be a no-op.
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::TargetKeeping
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode);
+    }
+
+    #[test]
+    fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_0() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1);
+        seq_tables.entries[0].entries[0].check_success = true;
+        seq_tables.entries[0].entries[1].check_success = false;
+        seq_tables.entries[1].entries[0].check_success = false;
+        tb.start_command_sequence(ExampleMode::Mode1, expected_req_id)
+            .unwrap();
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.sender.requests.borrow().len(), 0);
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL);
+        tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap());
+        let mode_reply_ok_0 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target0 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE),
+        );
+        let mode_reply_ok_1 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target1 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE),
+        );
+        // One success reply still expected.
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_1)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_0)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone)
+        );
+
+        // Inserting the reply makes no difference: This call completes the sequence commanding.
+        let mode_reply_ok = GenericMessage::new(
+            MessageMetadata::new(expected_req_id, ExampleTargetId::Target2 as ComponentId),
+            ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE),
+        );
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done)
+        );
+        // The internal request ID is still cached.
+        tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap());
+
+        // FSM call should be a no-op.
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::TargetKeeping
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode);
+    }
+
+    #[test]
+    fn test_subsystem_helper_cmd_mode1_with_partial_success_checks_1() {
+        let mut tb = SubsystemHelperTestbench::new();
+        let expected_req_id = 1;
+        let seq_tables = tb.get_sequence_tables(ExampleMode::Mode1);
+        seq_tables.entries[0].entries[0].check_success = true;
+        seq_tables.entries[0].entries[1].check_success = false;
+        seq_tables.entries[1].entries[0].check_success = false;
+        tb.start_command_sequence(ExampleMode::Mode1, expected_req_id)
+            .unwrap();
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.sender.requests.borrow().len(), 0);
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::AwaitingSuccessCheck)
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::ModeCommanding);
+        assert_eq!(tb.helper.mode(), UNKNOWN_MODE_VAL);
+        tb.generic_checks_subsystem_md1_step0(tb.helper.internal_request_id().unwrap());
+        let mode_reply_ok_0 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target0 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT0_MODE),
+        );
+        let mode_reply_ok_1 = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target1 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD0_TGT1_MODE),
+        );
+        // This completes the step, so the next FSM call will perform the next step
+        // in sequence commanding.
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_0)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::StepDone)
+        );
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok_1)).unwrap(),
+            SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done)
+        );
+
+        // Inserting the reply makes no difference: Sequence command is done and target keeping
+        // is performed.
+        let mode_reply_ok = GenericMessage::new(
+            MessageMetadata::new(
+                tb.helper.internal_request_id().unwrap(),
+                ExampleTargetId::Target2 as ComponentId,
+            ),
+            ModeReply::ModeInfo(SUBSYSTEM_MD1_ST1_TGT2_MODE),
+        );
+        assert_eq!(
+            tb.state_machine(Some(mode_reply_ok)).unwrap(),
+            SubsystemHelperResult::TargetKeeping
+        );
+        // The internal request ID is still cached.
+        tb.generic_checks_subsystem_md1_step1(tb.helper.internal_request_id().unwrap());
+
+        // FSM call should be a no-op.
+        assert_eq!(
+            tb.state_machine(None).unwrap(),
+            SubsystemHelperResult::TargetKeeping
+        );
+        assert_eq!(tb.helper.state(), ModeTreeHelperState::TargetKeeping);
+        assert_eq!(tb.helper.mode(), ExampleMode::Mode1 as Mode);
+    }
+}
diff --git a/satrs/tests/mode_tree.rs b/satrs/tests/mode_tree.rs
index 17f9836..06c1b2f 100644
--- a/satrs/tests/mode_tree.rs
+++ b/satrs/tests/mode_tree.rs
@@ -1,103 +1,826 @@
 use core::cell::Cell;
-use std::{println, sync::mpsc};
-
-use satrs::mode::{
-    ModeError, ModeProvider, ModeReplyReceiver, ModeReplySender, ModeRequestHandler,
-    ModeRequestHandlerMpscBounded, ModeRequestReceiver, ModeRequestorAndHandlerMpscBounded,
-    ModeRequestorBoundedMpsc,
+use num_enum::TryFromPrimitive;
+use satrs::dev_mgmt::{
+    DevManagerCommandingHelper, DevManagerHelperResult, TransparentDevManagerHook,
+};
+use satrs::mode::{
+    Mode, ModeError, ModeProvider, ModeReplyReceiver, ModeReplySender, ModeRequestHandler,
+    ModeRequestHandlerMpscBounded, ModeRequestReceiver, ModeRequestorAndHandlerMpscBounded,
+    ModeRequestorOneChildBoundedMpsc, INVALID_MODE, UNKNOWN_MODE,
+};
+use satrs::mode_tree::{
+    connect_mode_nodes, ModeChild, ModeNode, ModeParent, ModeStoreProvider, SequenceTableEntry,
+    SequenceTableMapTable, TargetTableEntry,
+};
+use satrs::mode_tree::{SequenceTablesMapValue, TargetTablesMapValue};
+use satrs::request::{MessageMetadata, RequestId};
+use satrs::res_code::ResultU16;
+use satrs::subsystem::{
+    IsChildCommandable, ModeCommandingResult, ModeTreeHelperError, ModeTreeHelperState,
+    StartSequenceError, SubsystemCommandingHelper, SubsystemHelperResult,
 };
-use satrs::request::MessageMetadata;
 use satrs::{
     mode::{ModeAndSubmode, ModeReply, ModeRequest},
     queue::GenericTargetedMessagingError,
     request::GenericMessage,
     ComponentId,
 };
-use std::string::{String, ToString};
+use std::borrow::{Borrow, BorrowMut};
+use std::cell::RefCell;
+use std::collections::{HashMap, VecDeque};
+use std::convert::Infallible;
+use std::{println, sync::mpsc};
 
+pub enum DefaultMode {
+    OFF = 0,
+    ON = 1,
+    NORMAL = 2,
+}
+
+#[derive(Debug)]
+pub enum AcsMode {
+    OFF = 0,
+    SAFE = 1,
+    IDLE = 2,
+}
+
+#[derive(Debug, TryFromPrimitive)]
+#[repr(u64)]
 pub enum TestComponentId {
-    Device1 = 1,
-    Device2 = 2,
-    Assembly = 3,
-    PusModeService = 4,
+    MagnetometerDevice0 = 1,
+    MagnetometerDevice1 = 2,
+    MagnetorquerDevice = 5,
+    ReactionWheelDevice = 6,
+    StartrackerDevice = 7,
+    MgtDevManager = 8,
+    ReactionWheelAssembly = 10,
+    MagnetometerAssembly = 11,
+    AcsController = 14,
+    AcsSubsystem = 15,
+    PusModeService = 16,
+}
+
+pub type RequestSenderType = mpsc::SyncSender<GenericMessage<ModeRequest>>;
+pub type ReplySenderType = mpsc::SyncSender<GenericMessage<ModeReply>>;
+
+#[derive(Default, Debug)]
+pub struct ModeRequestHandlerMock {
+    pub id: ComponentId,
+    get_mode_calls: RefCell<usize>,
+    start_transition_calls: VecDeque<(MessageMetadata, ModeAndSubmode)>,
+    announce_mode_calls: RefCell<VecDeque<AnnounceModeInfo>>,
+    handle_mode_info_calls: VecDeque<(MessageMetadata, ModeAndSubmode)>,
+    handle_mode_reached_calls: RefCell<VecDeque<Option<MessageMetadata>>>,
+    send_mode_reply_calls: RefCell<VecDeque<(MessageMetadata, ModeReply)>>,
+}
+
+impl ModeRequestHandlerMock {
+    pub fn new(id: ComponentId) -> Self {
+        Self {
+            id,
+            ..Default::default()
+        }
+    }
+}
+
+impl ModeRequestHandlerMock {
+    pub fn clear(&mut self) {
+        self.get_mode_calls.replace(0);
+        self.start_transition_calls.clear();
+        self.announce_mode_calls.borrow_mut().clear();
+        self.handle_mode_reached_calls.borrow_mut().clear();
+        self.handle_mode_info_calls.clear();
+        self.send_mode_reply_calls.borrow_mut().clear();
+    }
+    pub fn mode_messages_received(&self) -> usize {
+        *self.get_mode_calls.borrow()
+            + self.start_transition_calls.borrow().len()
+            + self.announce_mode_calls.borrow().len()
+            + self.handle_mode_info_calls.borrow().len()
+            + self.handle_mode_reached_calls.borrow().len()
+            + self.send_mode_reply_calls.borrow().len()
+    }
+}
+
+impl ModeProvider for ModeRequestHandlerMock {
+    fn mode_and_submode(&self) -> ModeAndSubmode {
+        *self.get_mode_calls.borrow_mut() += 1;
+        INVALID_MODE
+    }
+}
+
+impl ModeRequestHandler for ModeRequestHandlerMock {
+    type Error = Infallible;
+
+    fn start_transition(
+        &mut self,
+        requestor: MessageMetadata,
+        mode_and_submode: ModeAndSubmode,
+        _forced: bool,
+    ) -> Result<(), Self::Error> {
+        self.start_transition_calls
+            .push_back((requestor, mode_and_submode));
+        Ok(())
+    }
+
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
+        self.announce_mode_calls
+            .borrow_mut()
+            .push_back(AnnounceModeInfo {
+                requestor: requestor_info,
+                recursive,
+            });
+    }
+
+    fn handle_mode_reached(
+        &mut self,
+        requestor_info: Option<MessageMetadata>,
+    ) -> Result<(), Self::Error> {
+        self.handle_mode_reached_calls
+            .borrow_mut()
+            .push_back(requestor_info);
+        Ok(())
+    }
+
+    fn handle_mode_info(
+        &mut self,
+        requestor_info: MessageMetadata,
+        info: ModeAndSubmode,
+    ) -> Result<(), Self::Error> {
+        self.handle_mode_info_calls
+            .push_back((requestor_info, info));
+        todo!()
+    }
+
+    fn send_mode_reply(
+        &self,
+        requestor_info: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), Self::Error> {
+        self.send_mode_reply_calls
+            .borrow_mut()
+            .push_back((requestor_info, reply));
+        Ok(())
+    }
+}
+
+#[derive(Debug)]
+pub struct ModeReplyHandlerMock {
+    pub id: ComponentId,
+    mode_info_messages: VecDeque<(MessageMetadata, ModeAndSubmode)>,
+    mode_reply_messages: VecDeque<(MessageMetadata, ModeAndSubmode)>,
+    cant_reach_mode_messages: VecDeque<(MessageMetadata, ResultU16)>,
+    wrong_mode_messages: VecDeque<(MessageMetadata, ModeAndSubmode, ModeAndSubmode)>,
+}
+
+impl ModeReplyHandlerMock {
+    pub fn new(id: ComponentId) -> Self {
+        Self {
+            id,
+            mode_info_messages: Default::default(),
+            mode_reply_messages: Default::default(),
+            cant_reach_mode_messages: Default::default(),
+            wrong_mode_messages: Default::default(),
+        }
+    }
+
+    pub fn num_of_received_mode_replies(&self) -> usize {
+        self.mode_info_messages.len()
+            + self.mode_reply_messages.len()
+            + self.cant_reach_mode_messages.len()
+            + self.wrong_mode_messages.len()
+    }
+
+    pub fn handle_mode_reply(&mut self, request: &GenericMessage<ModeReply>) {
+        match request.message {
+            ModeReply::ModeInfo(mode_and_submode) => {
+                self.mode_info_messages
+                    .push_back((request.requestor_info, mode_and_submode));
+            }
+            ModeReply::ModeReply(mode_and_submode) => {
+                self.mode_reply_messages
+                    .push_back((request.requestor_info, mode_and_submode));
+            }
+            ModeReply::CantReachMode(result_u16) => {
+                self.cant_reach_mode_messages
+                    .push_back((request.requestor_info, result_u16));
+            }
+            ModeReply::WrongMode { expected, reached } => {
+                self.wrong_mode_messages
+                    .push_back((request.requestor_info, expected, reached));
+            }
+        }
+    }
 }
 
 struct PusModeService {
     pub request_id_counter: Cell<u32>,
-    pub mode_node: ModeRequestorBoundedMpsc,
+    pub mode_reply_mock: ModeReplyHandlerMock,
+    mode_node: ModeRequestorOneChildBoundedMpsc,
 }
 
 impl PusModeService {
-    pub fn send_announce_mode_cmd_to_assy(&self) {
+    pub fn new(init_req_count: u32, mode_node: ModeRequestorOneChildBoundedMpsc) -> Self {
+        Self {
+            request_id_counter: Cell::new(init_req_count),
+            mode_reply_mock: ModeReplyHandlerMock::new(
+                TestComponentId::PusModeService as ComponentId,
+            ),
+            mode_node,
+        }
+    }
+    pub fn run(&mut self) {
+        while let Some(reply) = self.mode_node.try_recv_mode_reply().unwrap() {
+            self.mode_reply_mock.handle_mode_reply(&reply);
+        }
+    }
+
+    pub fn announce_modes_recursively(&self) {
         self.mode_node
             .send_mode_request(
                 self.request_id_counter.get(),
-                TestComponentId::Assembly as ComponentId,
+                TestComponentId::AcsSubsystem as ComponentId,
                 ModeRequest::AnnounceModeRecursive,
             )
             .unwrap();
         self.request_id_counter
             .replace(self.request_id_counter.get() + 1);
     }
+
+    pub fn send_mode_cmd(&self, mode: ModeAndSubmode) -> RequestId {
+        let request_id = self.request_id_counter.get();
+        self.mode_node
+            .send_mode_request(
+                request_id,
+                TestComponentId::AcsSubsystem as ComponentId,
+                ModeRequest::SetMode {
+                    mode_and_submode: mode,
+                    forced: false,
+                },
+            )
+            .unwrap();
+        self.request_id_counter.replace(request_id + 1);
+        request_id
+    }
 }
 
-struct TestDevice {
-    pub name: String,
-    pub mode_node: ModeRequestHandlerMpscBounded,
-    pub mode_and_submode: ModeAndSubmode,
+impl ModeNode for PusModeService {
+    fn id(&self) -> ComponentId {
+        TestComponentId::PusModeService as ComponentId
+    }
 }
 
-impl TestDevice {
-    pub fn run(&mut self) {
-        self.check_mode_requests().expect("mode messaging error");
+impl ModeParent for PusModeService {
+    type Sender = RequestSenderType;
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender) {
+        self.mode_node.add_message_target(id, request_sender);
+    }
+}
+
+#[derive(Debug, Default)]
+struct IsCommandableDummy {}
+
+impl IsChildCommandable for IsCommandableDummy {
+    fn is_commandable(&self, _id: ComponentId) -> bool {
+        true
+    }
+}
+
+struct AcsSubsystem {
+    pub mode_node: ModeRequestorAndHandlerMpscBounded,
+    pub mode_requestor_info: Option<MessageMetadata>,
+    pub target_mode_and_submode: Option<ModeAndSubmode>,
+    pub subsystem_helper: SubsystemCommandingHelper,
+    is_commandable_dummy: IsCommandableDummy,
+    pub mode_req_mock: ModeRequestHandlerMock,
+    pub mode_reply_mock: ModeReplyHandlerMock,
+}
+
+impl AcsSubsystem {
+    pub fn id() -> ComponentId {
+        TestComponentId::AcsSubsystem as u64
     }
 
-    pub fn check_mode_requests(&mut self) -> Result<(), ModeError> {
-        if let Some(request) = self.mode_node.try_recv_mode_request()? {
-            self.handle_mode_request(request)?
+    pub fn new(mode_node: ModeRequestorAndHandlerMpscBounded) -> Self {
+        Self {
+            mode_node,
+            mode_requestor_info: None,
+            target_mode_and_submode: None,
+            is_commandable_dummy: IsCommandableDummy::default(),
+            subsystem_helper: SubsystemCommandingHelper::default(),
+            mode_req_mock: ModeRequestHandlerMock::new(Self::id()),
+            mode_reply_mock: ModeReplyHandlerMock::new(Self::id()),
+        }
+    }
+
+    pub fn get_num_mode_requests(&mut self) -> usize {
+        self.mode_req_mock.mode_messages_received()
+    }
+
+    pub fn handle_subsystem_helper_result(
+        &mut self,
+        result: Result<SubsystemHelperResult, ModeTreeHelperError>,
+    ) {
+        match result {
+            Ok(result) => {
+                if let SubsystemHelperResult::ModeCommanding(ModeCommandingResult::Done) = result {
+                    self.handle_mode_reached(self.mode_requestor_info)
+                        .expect("mode reply handling failed");
+                }
+            }
+            Err(error) => match error {
+                ModeTreeHelperError::Message(_generic_targeted_messaging_error) => {
+                    panic!("messaging error")
+                }
+                ModeTreeHelperError::CurrentModeNotInTargetTable(_) => panic!("mode not found"),
+                ModeTreeHelperError::ModeCommmandFailure { seq_table_index: _ } => {
+                    // TODO: Cache the command failure.
+                }
+                ModeTreeHelperError::TargetKeepingViolation { fallback_mode } => {
+                    if let Some(fallback_mode) = fallback_mode {
+                        self.subsystem_helper
+                            .start_command_sequence(fallback_mode, 0)
+                            .unwrap();
+                    }
+                }
+            },
+        }
+    }
+
+    pub fn run(&mut self) {
+        while let Some(request) = self.mode_node.try_recv_mode_request().unwrap() {
+            self.handle_mode_request(request)
+                .expect("mode messaging error");
+        }
+
+        let mut received_reply = false;
+        while let Some(mode_reply) = self.mode_node.try_recv_mode_reply().unwrap() {
+            received_reply = true;
+            self.mode_reply_mock.handle_mode_reply(&mode_reply);
+            let result = self.subsystem_helper.state_machine(
+                Some(mode_reply),
+                &self.mode_node,
+                &self.is_commandable_dummy,
+            );
+            self.handle_subsystem_helper_result(result);
+        }
+        if !received_reply {
+            let result = self.subsystem_helper.state_machine(
+                None,
+                &self.mode_node,
+                &self.is_commandable_dummy,
+            );
+            self.handle_subsystem_helper_result(result);
+        }
+    }
+
+    pub fn add_target_and_sequence_table(
+        &mut self,
+        mode: Mode,
+        target_table_val: TargetTablesMapValue,
+        sequence_table_val: SequenceTablesMapValue,
+    ) {
+        self.subsystem_helper.add_target_and_sequence_table(
+            mode,
+            target_table_val,
+            sequence_table_val,
+        );
+    }
+}
+
+impl ModeNode for AcsSubsystem {
+    fn id(&self) -> ComponentId {
+        Self::id()
+    }
+}
+
+impl ModeParent for AcsSubsystem {
+    type Sender = RequestSenderType;
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) {
+        self.subsystem_helper.add_mode_child(id, UNKNOWN_MODE);
+        self.mode_node.add_request_target(id, request_sender);
+    }
+}
+
+impl ModeChild for AcsSubsystem {
+    type Sender = ReplySenderType;
+
+    fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) {
+        self.mode_node.add_reply_target(id, reply_sender);
+    }
+}
+
+impl ModeProvider for AcsSubsystem {
+    fn mode_and_submode(&self) -> ModeAndSubmode {
+        ModeAndSubmode::new(self.subsystem_helper.mode(), 0)
+    }
+}
+
+#[derive(Debug, thiserror::Error)]
+pub enum SubsystemModeError {
+    #[error("messaging error: {0:?}")]
+    Mode(#[from] ModeError),
+    #[error("start sequence error: {0}")]
+    StartError(#[from] StartSequenceError),
+}
+
+impl ModeRequestHandler for AcsSubsystem {
+    type Error = SubsystemModeError;
+
+    fn start_transition(
+        &mut self,
+        requestor: MessageMetadata,
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+    ) -> Result<(), Self::Error> {
+        if !forced && self.subsystem_helper.state() == ModeTreeHelperState::ModeCommanding {
+            return Err(ModeError::Busy.into());
+        }
+        self.mode_requestor_info = Some(requestor);
+        self.target_mode_and_submode = Some(mode_and_submode);
+        self.mode_req_mock
+            .start_transition(requestor, mode_and_submode, forced)
+            .unwrap();
+        println!("subsystem: mode req with ID {}", requestor.request_id());
+        self.subsystem_helper
+            .start_command_sequence(mode_and_submode.mode(), requestor.request_id())?;
+        Ok(())
+    }
+
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
+        println!(
+            "TestAssembly: Announcing mode (recursively: {}): {:?}",
+            recursive,
+            self.subsystem_helper.mode()
+        );
+        let request_id = requestor_info.map_or(0, |info| info.request_id());
+        self.subsystem_helper
+            .send_announce_mode_cmd_to_children(request_id, &self.mode_node, recursive)
+            .expect("sending mode request failed");
+        self.mode_req_mock.announce_mode(requestor_info, recursive);
+    }
+
+    fn handle_mode_reached(
+        &mut self,
+        requestor_info: Option<MessageMetadata>,
+    ) -> Result<(), Self::Error> {
+        self.mode_req_mock
+            .handle_mode_reached(requestor_info)
+            .unwrap();
+        if let Some(requestor) = requestor_info {
+            self.send_mode_reply(
+                requestor,
+                ModeReply::ModeReply(ModeAndSubmode::new(self.subsystem_helper.mode(), 0)),
+            )?;
+        }
+        Ok(())
+    }
+
+    fn handle_mode_info(
+        &mut self,
+        requestor_info: MessageMetadata,
+        info: ModeAndSubmode,
+    ) -> Result<(), Self::Error> {
+        self.mode_req_mock
+            .handle_mode_info(requestor_info, info)
+            .unwrap();
+        // TODO: Need to check whether mode table execution is finished.
+        // This works by checking the children modes received through replies against the
+        // mode table after all transition tables were executed.
+        Ok(())
+    }
+
+    fn send_mode_reply(
+        &self,
+        requestor_info: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), Self::Error> {
+        self.mode_req_mock
+            .send_mode_reply(requestor_info, reply)
+            .unwrap();
+        self.mode_node
+            .send_mode_reply(requestor_info, reply)
+            .map_err(ModeError::Send)?;
+        Ok(())
+    }
+}
+
+struct MgmAssembly {
+    pub mode_node: ModeRequestorAndHandlerMpscBounded,
+    pub mode_requestor_info: Option<MessageMetadata>,
+    pub mode_and_submode: ModeAndSubmode,
+    pub commanding_helper: DevManagerCommandingHelper<TransparentDevManagerHook>,
+    pub mode_req_mock: ModeRequestHandlerMock,
+    pub mode_reply_mock: ModeReplyHandlerMock,
+}
+
+impl MgmAssembly {
+    pub fn id() -> ComponentId {
+        TestComponentId::MagnetometerAssembly as u64
+    }
+    pub fn new(mode_node: ModeRequestorAndHandlerMpscBounded) -> Self {
+        Self {
+            mode_node,
+            mode_requestor_info: None,
+            mode_and_submode: UNKNOWN_MODE,
+            commanding_helper: DevManagerCommandingHelper::new(TransparentDevManagerHook::default()),
+            mode_req_mock: ModeRequestHandlerMock::new(Self::id()),
+            mode_reply_mock: ModeReplyHandlerMock::new(Self::id()),
+        }
+    }
+
+    pub fn run(&mut self) {
+        self.check_mode_requests().expect("mode messaging error");
+        self.check_mode_replies().expect("mode messaging error");
+    }
+
+    pub fn get_num_mode_requests(&mut self) -> usize {
+        self.mode_req_mock.mode_messages_received()
+    }
+
+    pub fn check_mode_requests(&mut self) -> Result<(), GenericTargetedMessagingError> {
+        while let Some(request) = self.mode_node.try_recv_mode_request()? {
+            self.handle_mode_request(request).unwrap();
+        }
+        Ok(())
+    }
+
+    pub fn check_mode_replies(&mut self) -> Result<(), ModeError> {
+        while let Some(reply_and_id) = self.mode_node.try_recv_mode_reply()? {
+            self.mode_reply_mock.handle_mode_reply(&reply_and_id);
+            match self.commanding_helper.handle_mode_reply(&reply_and_id) {
+                Ok(result) => {
+                    if let DevManagerHelperResult::ModeCommandingDone(context) = result {
+                        // Complete the mode command.
+                        self.mode_and_submode = context.target_mode;
+                        self.handle_mode_reached(self.mode_requestor_info)?;
+                    }
+                }
+                Err(err) => match err {
+                    satrs::dev_mgmt::DevManagerHelperError::ChildNotInStore => todo!(),
+                },
+            }
         }
         Ok(())
     }
 }
 
-impl ModeProvider for TestDevice {
+impl ModeNode for MgmAssembly {
+    fn id(&self) -> ComponentId {
+        Self::id()
+    }
+}
+impl ModeParent for MgmAssembly {
+    type Sender = RequestSenderType;
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: RequestSenderType) {
+        self.mode_node.add_request_target(id, request_sender);
+        self.commanding_helper.add_mode_child(id, UNKNOWN_MODE);
+    }
+}
+
+impl ModeChild for MgmAssembly {
+    type Sender = ReplySenderType;
+
+    fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) {
+        self.mode_node.add_reply_target(id, reply_sender);
+    }
+}
+
+impl ModeProvider for MgmAssembly {
     fn mode_and_submode(&self) -> ModeAndSubmode {
         self.mode_and_submode
     }
 }
 
-impl ModeRequestHandler for TestDevice {
+impl ModeRequestHandler for MgmAssembly {
+    type Error = ModeError;
+    fn start_transition(
+        &mut self,
+        requestor: MessageMetadata,
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+    ) -> Result<(), Self::Error> {
+        // Always accept forced commands and commands to mode OFF.
+        if self.commanding_helper.target_mode().is_some()
+            && !forced
+            && mode_and_submode.mode() != DefaultMode::OFF as u32
+        {
+            return Err(ModeError::Busy);
+        }
+        self.mode_requestor_info = Some(requestor);
+        self.mode_req_mock
+            .start_transition(requestor, mode_and_submode, forced)
+            .unwrap();
+        self.commanding_helper.send_mode_cmd_to_all_children(
+            requestor.request_id(),
+            mode_and_submode,
+            forced,
+            &self.mode_node,
+        )?;
+        Ok(())
+    }
+
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
+        println!(
+            "TestAssembly: Announcing mode (recursively: {}): {:?}",
+            recursive, self.mode_and_submode
+        );
+        let request_id = requestor_info.map_or(0, |info| info.request_id());
+        self.commanding_helper
+            .send_announce_mode_cmd_to_children(request_id, &self.mode_node, recursive)
+            .expect("sending mode request failed");
+        self.mode_req_mock.announce_mode(requestor_info, recursive);
+    }
+
+    fn handle_mode_reached(
+        &mut self,
+        mode_requestor: Option<MessageMetadata>,
+    ) -> Result<(), Self::Error> {
+        if let Some(requestor) = mode_requestor {
+            self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?;
+        }
+        self.mode_req_mock
+            .handle_mode_reached(mode_requestor)
+            .unwrap();
+        Ok(())
+    }
+
+    fn handle_mode_info(
+        &mut self,
+        requestor_info: MessageMetadata,
+        info: ModeAndSubmode,
+    ) -> Result<(), Self::Error> {
+        self.mode_req_mock
+            .handle_mode_info(requestor_info, info)
+            .unwrap();
+        // TODO: A proper assembly must reach to mode changes of its children..
+        Ok(())
+    }
+
+    fn send_mode_reply(
+        &self,
+        requestor: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), Self::Error> {
+        self.mode_node.send_mode_reply(requestor, reply)?;
+        self.mode_req_mock
+            .send_mode_reply(requestor, reply)
+            .unwrap();
+        Ok(())
+    }
+}
+
+struct DeviceManager {
+    name: &'static str,
+    pub id: ComponentId,
+    pub commanding_helper: DevManagerCommandingHelper<TransparentDevManagerHook>,
+    pub mode_node: ModeRequestorAndHandlerMpscBounded,
+    pub mode_requestor_info: Option<MessageMetadata>,
+    pub mode_and_submode: ModeAndSubmode,
+    pub mode_req_mock: ModeRequestHandlerMock,
+    pub mode_reply_mock: ModeReplyHandlerMock,
+}
+
+impl DeviceManager {
+    pub fn new(
+        name: &'static str,
+        id: ComponentId,
+        mode_node: ModeRequestorAndHandlerMpscBounded,
+    ) -> Self {
+        Self {
+            name,
+            id,
+            mode_node,
+            mode_requestor_info: None,
+            commanding_helper: DevManagerCommandingHelper::new(TransparentDevManagerHook::default()),
+            mode_and_submode: UNKNOWN_MODE,
+            mode_req_mock: ModeRequestHandlerMock::new(id),
+            mode_reply_mock: ModeReplyHandlerMock::new(id),
+        }
+    }
+
+    pub fn get_num_mode_requests(&mut self) -> usize {
+        self.mode_req_mock.mode_messages_received()
+    }
+
+    pub fn run(&mut self) {
+        self.check_mode_requests().expect("mode messaging error");
+        self.check_mode_replies().expect("mode reply error");
+    }
+
+    pub fn check_mode_requests(&mut self) -> Result<(), ModeError> {
+        while let Some(request) = self.mode_node.try_recv_mode_request()? {
+            self.handle_mode_request(request)?
+        }
+        Ok(())
+    }
+
+    pub fn check_mode_replies(&mut self) -> Result<(), ModeError> {
+        while let Some(reply) = self.mode_node.try_recv_mode_reply()? {
+            self.handle_mode_reply(&reply)?;
+        }
+        Ok(())
+    }
+
+    pub fn handle_mode_reply(
+        &mut self,
+        mode_reply: &GenericMessage<ModeReply>,
+    ) -> Result<(), ModeError> {
+        self.mode_reply_mock.handle_mode_reply(mode_reply);
+        match self.commanding_helper.handle_mode_reply(mode_reply) {
+            Ok(result) => {
+                match result {
+                    DevManagerHelperResult::Idle => todo!(),
+                    DevManagerHelperResult::Busy => todo!(),
+                    DevManagerHelperResult::ModeCommandingDone(context) => {
+                        // Complete the mode command.
+                        self.handle_mode_reached(self.mode_requestor_info)?;
+                        self.mode_and_submode = context.target_mode;
+                    }
+                }
+            }
+            Err(e) => match e {
+                satrs::dev_mgmt::DevManagerHelperError::ChildNotInStore => todo!(),
+            },
+        }
+        Ok(())
+    }
+}
+
+impl ModeNode for DeviceManager {
+    fn id(&self) -> ComponentId {
+        self.id
+    }
+}
+
+impl ModeChild for DeviceManager {
+    type Sender = ReplySenderType;
+
+    fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) {
+        self.mode_node.add_reply_target(id, reply_sender);
+    }
+}
+
+impl ModeParent for DeviceManager {
+    type Sender = RequestSenderType;
+
+    fn add_mode_child(&mut self, id: ComponentId, request_sender: Self::Sender) {
+        self.commanding_helper
+            .children_mode_store
+            .add_component(id, UNKNOWN_MODE);
+        self.mode_node.add_request_target(id, request_sender);
+    }
+}
+
+impl ModeProvider for DeviceManager {
+    fn mode_and_submode(&self) -> ModeAndSubmode {
+        self.mode_and_submode
+    }
+}
+
+impl ModeRequestHandler for DeviceManager {
     type Error = ModeError;
 
     fn start_transition(
         &mut self,
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
+        forced: bool,
     ) -> Result<(), ModeError> {
         self.mode_and_submode = mode_and_submode;
-        self.handle_mode_reached(Some(requestor))?;
+        self.mode_requestor_info = Some(requestor);
+        self.mode_req_mock
+            .start_transition(requestor, mode_and_submode, forced)
+            .unwrap();
+        self.commanding_helper.send_mode_cmd_to_all_children(
+            requestor.request_id(),
+            mode_and_submode,
+            forced,
+            &self.mode_node,
+        )?;
         Ok(())
     }
 
-    fn announce_mode(&self, _requestor_info: Option<MessageMetadata>, _recursive: bool) {
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
         println!(
             "{}: announcing mode: {:?}",
             self.name, self.mode_and_submode
         );
+        let request_id = requestor_info.map_or(0, |info| info.request_id());
+        self.commanding_helper
+            .send_announce_mode_cmd_to_children(request_id, &self.mode_node, recursive)
+            .expect("sending mode announce request failed");
+        self.mode_req_mock.announce_mode(requestor_info, recursive);
     }
 
     fn handle_mode_reached(&mut self, requestor: Option<MessageMetadata>) -> Result<(), ModeError> {
         if let Some(requestor) = requestor {
             self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?;
         }
-        Ok(())
-    }
-    fn send_mode_reply(
-        &self,
-        requestor_info: MessageMetadata,
-        reply: ModeReply,
-    ) -> Result<(), ModeError> {
-        self.mode_node.send_mode_reply(requestor_info, reply)?;
+        self.mode_req_mock.handle_mode_reached(requestor).unwrap();
         Ok(())
     }
 
@@ -113,246 +836,775 @@ impl ModeRequestHandler for TestDevice {
             requestor_info.sender_id(),
             info
         );
-        Ok(())
-    }
-}
-
-struct TestAssembly {
-    pub mode_node: ModeRequestorAndHandlerMpscBounded,
-    pub mode_requestor_info: Option<MessageMetadata>,
-    pub mode_and_submode: ModeAndSubmode,
-    pub target_mode_and_submode: Option<ModeAndSubmode>,
-}
-
-impl ModeProvider for TestAssembly {
-    fn mode_and_submode(&self) -> ModeAndSubmode {
-        self.mode_and_submode
-    }
-}
-
-impl TestAssembly {
-    pub fn run(&mut self) {
-        self.check_mode_requests().expect("mode messaging error");
-        self.check_mode_replies().expect("mode messaging error");
-    }
-
-    pub fn check_mode_requests(&mut self) -> Result<(), GenericTargetedMessagingError> {
-        if let Some(request) = self.mode_node.try_recv_mode_request()? {
-            match request.message {
-                ModeRequest::SetMode(mode_and_submode) => {
-                    self.start_transition(request.requestor_info, mode_and_submode)
-                        .unwrap();
-                }
-                ModeRequest::ReadMode => self
-                    .mode_node
-                    .send_mode_reply(
-                        request.requestor_info,
-                        ModeReply::ModeReply(self.mode_and_submode),
-                    )
-                    .unwrap(),
-                ModeRequest::AnnounceMode => {
-                    self.announce_mode(Some(request.requestor_info), false)
-                }
-                ModeRequest::AnnounceModeRecursive => {
-                    self.announce_mode(Some(request.requestor_info), true)
-                }
-                ModeRequest::ModeInfo(_) => todo!(),
-            }
-        }
-        Ok(())
-    }
-
-    pub fn check_mode_replies(&mut self) -> Result<(), GenericTargetedMessagingError> {
-        if let Some(reply_and_id) = self.mode_node.try_recv_mode_reply()? {
-            match reply_and_id.message {
-                ModeReply::ModeReply(reply) => {
-                    println!(
-                        "TestAssembly: Received mode reply from {:?}, reached: {:?}",
-                        reply_and_id.sender_id(),
-                        reply
-                    );
-                }
-                ModeReply::CantReachMode(_) => todo!(),
-                ModeReply::WrongMode { expected, reached } => {
-                    println!(
-                        "TestAssembly: Wrong mode reply from {:?}, reached {:?}, expected {:?}",
-                        reply_and_id.sender_id(),
-                        reached,
-                        expected
-                    );
-                }
-            }
-        }
-        Ok(())
-    }
-}
-
-impl ModeRequestHandler for TestAssembly {
-    type Error = ModeError;
-    fn start_transition(
-        &mut self,
-        requestor: MessageMetadata,
-        mode_and_submode: ModeAndSubmode,
-    ) -> Result<(), Self::Error> {
-        self.mode_requestor_info = Some(requestor);
-        self.target_mode_and_submode = Some(mode_and_submode);
-        Ok(())
-    }
-
-    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
-        println!(
-            "TestAssembly: Announcing mode (recursively: {}): {:?}",
-            recursive, self.mode_and_submode
-        );
-        // self.mode_requestor_info = Some((request_id, sender_id));
-        let mut mode_request = ModeRequest::AnnounceMode;
-        if recursive {
-            mode_request = ModeRequest::AnnounceModeRecursive;
-        }
-        let request_id = requestor_info.map_or(0, |info| info.request_id());
-        self.mode_node
-            .request_sender_map
-            .0
-            .iter()
-            .for_each(|(_, sender)| {
-                sender
-                    .send(GenericMessage::new(
-                        MessageMetadata::new(request_id, self.mode_node.local_channel_id_generic()),
-                        mode_request,
-                    ))
-                    .expect("sending mode request failed");
-            });
-    }
-
-    fn handle_mode_reached(
-        &mut self,
-        mode_requestor: Option<MessageMetadata>,
-    ) -> Result<(), Self::Error> {
-        if let Some(requestor) = mode_requestor {
-            self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?;
-        }
+        self.mode_req_mock
+            .handle_mode_info(requestor_info, info)
+            .unwrap();
         Ok(())
     }
 
     fn send_mode_reply(
         &self,
-        requestor: MessageMetadata,
+        requestor_info: MessageMetadata,
         reply: ModeReply,
-    ) -> Result<(), Self::Error> {
-        self.mode_node.send_mode_reply(requestor, reply)?;
+    ) -> Result<(), ModeError> {
+        self.mode_node.send_mode_reply(requestor_info, reply)?;
+        self.mode_req_mock
+            .send_mode_reply(requestor_info, reply)
+            .unwrap();
+        Ok(())
+    }
+}
+
+struct CommonDevice {
+    name: &'static str,
+    pub id: ComponentId,
+    pub mode_node: ModeRequestHandlerMpscBounded,
+    pub mode_and_submode: ModeAndSubmode,
+    pub mode_req_mock: ModeRequestHandlerMock,
+}
+
+impl CommonDevice {
+    pub fn new(
+        name: &'static str,
+        id: ComponentId,
+        mode_node: ModeRequestHandlerMpscBounded,
+    ) -> Self {
+        Self {
+            name,
+            id,
+            mode_node,
+            mode_and_submode: UNKNOWN_MODE,
+            mode_req_mock: ModeRequestHandlerMock::new(id),
+        }
+    }
+
+    pub fn run(&mut self) {
+        self.check_mode_requests().expect("mode messaging error");
+    }
+
+    pub fn check_mode_requests(&mut self) -> Result<(), ModeError> {
+        if let Some(request) = self.mode_node.try_recv_mode_request()? {
+            self.handle_mode_request(request)?
+        }
+        Ok(())
+    }
+
+    pub fn get_and_clear_num_mode_msgs(&mut self) -> usize {
+        self.mode_req_mock.mode_messages_received()
+    }
+}
+
+impl ModeNode for CommonDevice {
+    fn id(&self) -> ComponentId {
+        self.id
+    }
+}
+
+impl ModeChild for CommonDevice {
+    type Sender = ReplySenderType;
+
+    fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) {
+        self.mode_node.add_message_target(id, reply_sender);
+    }
+}
+
+impl ModeProvider for CommonDevice {
+    fn mode_and_submode(&self) -> ModeAndSubmode {
+        self.mode_and_submode
+    }
+}
+
+impl ModeRequestHandler for CommonDevice {
+    type Error = ModeError;
+
+    fn start_transition(
+        &mut self,
+        requestor: MessageMetadata,
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+    ) -> Result<(), ModeError> {
+        if self.id() == TestComponentId::MagnetorquerDevice as u64 {
+            println!("test");
+        }
+        self.mode_and_submode = mode_and_submode;
+        self.handle_mode_reached(Some(requestor))?;
+        self.mode_req_mock
+            .start_transition(requestor, mode_and_submode, forced)
+            .unwrap();
+        Ok(())
+    }
+
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
+        println!(
+            "{}: announcing mode: {:?}",
+            self.name, self.mode_and_submode
+        );
+        self.mode_req_mock.announce_mode(requestor_info, recursive);
+    }
+
+    fn handle_mode_reached(&mut self, requestor: Option<MessageMetadata>) -> Result<(), ModeError> {
+        if let Some(requestor) = requestor {
+            self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?;
+        }
+        self.mode_req_mock.handle_mode_reached(requestor).unwrap();
         Ok(())
     }
 
     fn handle_mode_info(
         &mut self,
-        _requestor_info: MessageMetadata,
-        _info: ModeAndSubmode,
-    ) -> Result<(), Self::Error> {
-        // TODO: A proper assembly must reach to mode changes of its children..
+        requestor_info: MessageMetadata,
+        info: ModeAndSubmode,
+    ) -> Result<(), ModeError> {
+        // A device is a leaf in the tree.. so this really should not happen
+        println!(
+            "{}: unexpected mode info from {:?} with mode: {:?}",
+            self.name,
+            requestor_info.sender_id(),
+            info
+        );
+        self.mode_req_mock
+            .handle_mode_info(requestor_info, info)
+            .unwrap();
+        Ok(())
+    }
+
+    fn send_mode_reply(
+        &self,
+        requestor_info: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), ModeError> {
+        self.mode_node.send_mode_reply(requestor_info, reply)?;
+        self.mode_req_mock
+            .send_mode_reply(requestor_info, reply)
+            .unwrap();
         Ok(())
     }
 }
 
-fn main() {
-    // All request channel handles.
-    let (request_sender_to_dev1, request_receiver_dev1) = mpsc::sync_channel(10);
-    let (request_sender_to_dev2, request_receiver_dev2) = mpsc::sync_channel(10);
-    let (request_sender_to_assy, request_receiver_assy) = mpsc::sync_channel(10);
-
-    // All reply channel handles.
-    let (reply_sender_to_assy, reply_receiver_assy) = mpsc::sync_channel(10);
-    let (reply_sender_to_pus, reply_receiver_pus) = mpsc::sync_channel(10);
-
-    // Mode requestors and handlers.
-    let mut mode_node_assy = ModeRequestorAndHandlerMpscBounded::new(
-        TestComponentId::Assembly as ComponentId,
-        request_receiver_assy,
-        reply_receiver_assy,
-    );
-    // Mode requestors only.
-    let mut mode_node_pus = ModeRequestorBoundedMpsc::new(
-        TestComponentId::PusModeService as ComponentId,
-        reply_receiver_pus,
-    );
-
-    // Request handlers only.
-    let mut mode_node_dev1 = ModeRequestHandlerMpscBounded::new(
-        TestComponentId::Device1 as ComponentId,
-        request_receiver_dev1,
-    );
-    let mut mode_node_dev2 = ModeRequestHandlerMpscBounded::new(
-        TestComponentId::Device2 as ComponentId,
-        request_receiver_dev2,
-    );
-
-    // Set up mode request senders first.
-    mode_node_pus.add_message_target(
-        TestComponentId::Assembly as ComponentId,
-        request_sender_to_assy,
-    );
-    mode_node_pus.add_message_target(
-        TestComponentId::Device1 as ComponentId,
-        request_sender_to_dev1.clone(),
-    );
-    mode_node_pus.add_message_target(
-        TestComponentId::Device2 as ComponentId,
-        request_sender_to_dev2.clone(),
-    );
-    mode_node_assy.add_request_target(
-        TestComponentId::Device1 as ComponentId,
-        request_sender_to_dev1,
-    );
-    mode_node_assy.add_request_target(
-        TestComponentId::Device2 as ComponentId,
-        request_sender_to_dev2,
-    );
-
-    // Set up mode reply senders.
-    mode_node_dev1.add_message_target(
-        TestComponentId::Assembly as ComponentId,
-        reply_sender_to_assy.clone(),
-    );
-    mode_node_dev1.add_message_target(
-        TestComponentId::PusModeService as ComponentId,
-        reply_sender_to_pus.clone(),
-    );
-    mode_node_dev2.add_message_target(
-        TestComponentId::Assembly as ComponentId,
-        reply_sender_to_assy,
-    );
-    mode_node_dev2.add_message_target(
-        TestComponentId::PusModeService as ComponentId,
-        reply_sender_to_pus.clone(),
-    );
-    mode_node_assy.add_reply_target(
-        TestComponentId::PusModeService as ComponentId,
-        reply_sender_to_pus,
-    );
-
-    let mut device1 = TestDevice {
-        name: "Test Device 1".to_string(),
-        mode_node: mode_node_dev1,
-        mode_and_submode: ModeAndSubmode::new(0, 0),
-    };
-    let mut device2 = TestDevice {
-        name: "Test Device 2".to_string(),
-        mode_node: mode_node_dev2,
-        mode_and_submode: ModeAndSubmode::new(0, 0),
-    };
-    let mut assy = TestAssembly {
-        mode_node: mode_node_assy,
-        mode_requestor_info: None,
-        mode_and_submode: ModeAndSubmode::new(0, 0),
-        target_mode_and_submode: None,
-    };
-    let pus_service = PusModeService {
-        request_id_counter: Cell::new(0),
-        mode_node: mode_node_pus,
-    };
-
-    pus_service.send_announce_mode_cmd_to_assy();
-    assy.run();
-    device1.run();
-    device2.run();
-    assy.run();
+#[derive(Debug, Default)]
+pub struct AnnounceModeInfo {
+    pub requestor: Option<MessageMetadata>,
+    pub recursive: bool,
+}
+
+pub struct AcsController {
+    pub mode_node: ModeRequestHandlerMpscBounded,
+    pub mode_and_submode: ModeAndSubmode,
+    pub announce_mode_queue: RefCell<VecDeque<AnnounceModeInfo>>,
+    pub mode_req_mock: ModeRequestHandlerMock,
+}
+
+impl AcsController {
+    pub fn id() -> ComponentId {
+        TestComponentId::AcsController as u64
+    }
+    pub fn new(mode_node: ModeRequestHandlerMpscBounded) -> Self {
+        Self {
+            mode_node,
+            mode_and_submode: UNKNOWN_MODE,
+            announce_mode_queue: Default::default(),
+            mode_req_mock: ModeRequestHandlerMock::new(Self::id()),
+        }
+    }
+    pub fn run(&mut self) {
+        self.check_mode_requests().expect("mode messaging error");
+    }
+
+    pub fn check_mode_requests(&mut self) -> Result<(), ModeError> {
+        if let Some(request) = self.mode_node.try_recv_mode_request()? {
+            self.handle_mode_request(request)?
+        }
+        Ok(())
+    }
+}
+
+impl ModeNode for AcsController {
+    fn id(&self) -> ComponentId {
+        TestComponentId::AcsController as u64
+    }
+}
+
+impl ModeChild for AcsController {
+    type Sender = ReplySenderType;
+
+    fn add_mode_parent(&mut self, id: ComponentId, reply_sender: ReplySenderType) {
+        self.mode_node.add_message_target(id, reply_sender);
+    }
+}
+
+impl ModeProvider for AcsController {
+    fn mode_and_submode(&self) -> ModeAndSubmode {
+        self.mode_and_submode
+    }
+}
+
+impl ModeRequestHandler for AcsController {
+    type Error = ModeError;
+
+    fn start_transition(
+        &mut self,
+        requestor: MessageMetadata,
+        mode_and_submode: ModeAndSubmode,
+        forced: bool,
+    ) -> Result<(), Self::Error> {
+        self.mode_and_submode = mode_and_submode;
+        self.handle_mode_reached(Some(requestor))?;
+        self.mode_req_mock
+            .start_transition(requestor, mode_and_submode, forced)
+            .unwrap();
+        Ok(())
+    }
+
+    fn announce_mode(&self, requestor_info: Option<MessageMetadata>, recursive: bool) {
+        self.announce_mode_queue
+            .borrow_mut()
+            .push_back(AnnounceModeInfo {
+                requestor: requestor_info,
+                recursive,
+            });
+        self.mode_req_mock.announce_mode(requestor_info, recursive);
+    }
+
+    fn handle_mode_reached(
+        &mut self,
+        requestor_info: Option<MessageMetadata>,
+    ) -> Result<(), Self::Error> {
+        if let Some(requestor) = requestor_info {
+            self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode))?;
+        }
+        self.mode_req_mock
+            .handle_mode_reached(requestor_info)
+            .unwrap();
+        Ok(())
+    }
+
+    fn handle_mode_info(
+        &mut self,
+        requestor_info: MessageMetadata,
+        info: ModeAndSubmode,
+    ) -> Result<(), Self::Error> {
+        // The controller is a leaf in the tree.. so this really should not happen
+        println!(
+            "ACS Controller: unexpected mode info from {:?} with mode: {:?}",
+            requestor_info.sender_id(),
+            info
+        );
+        self.mode_req_mock
+            .handle_mode_info(requestor_info, info)
+            .unwrap();
+        Ok(())
+    }
+
+    fn send_mode_reply(
+        &self,
+        requestor_info: MessageMetadata,
+        reply: ModeReply,
+    ) -> Result<(), Self::Error> {
+        self.mode_node.send_mode_reply(requestor_info, reply)?;
+        self.mode_req_mock
+            .send_mode_reply(requestor_info, reply)
+            .unwrap();
+        Ok(())
+    }
+}
+
+pub struct TreeTestbench {
+    pus: PusModeService,
+    subsystem: AcsSubsystem,
+    ctrl: AcsController,
+    mgt_manager: DeviceManager,
+    mgm_assy: MgmAssembly,
+    mgm_devs: [CommonDevice; 2],
+    mgt_dev: CommonDevice,
+}
+
+impl Default for TreeTestbench {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl TreeTestbench {
+    pub fn new() -> Self {
+        // All request channel handles.
+        let (request_sender_to_mgm_dev_0, request_receiver_mgm_dev_0) = mpsc::sync_channel(10);
+        let (request_sender_to_mgm_dev_1, request_receiver_mgm_dev_1) = mpsc::sync_channel(10);
+        let (request_sender_to_mgt_dev, request_receiver_mgt_dev) = mpsc::sync_channel(10);
+        let (request_sender_to_mgt_man, request_receiver_mgt_man) = mpsc::sync_channel(10);
+        let (request_sender_to_mgm_assy, request_receiver_mgm_assy) = mpsc::sync_channel(10);
+        let (request_sender_to_acs_subsystem, request_receiver_acs_subsystem) =
+            mpsc::sync_channel(10);
+        let (request_sender_to_acs_ctrl, request_receiver_acs_ctrl) = mpsc::sync_channel(10);
+
+        // All reply channel handles.
+        let (reply_sender_to_mgm_assy, reply_receiver_mgm_assy) = mpsc::sync_channel(10);
+        let (reply_sender_to_acs_subsystem, reply_receiver_acs_subsystem) = mpsc::sync_channel(10);
+        let (reply_sender_to_pus, reply_receiver_pus) = mpsc::sync_channel(10);
+        let (reply_sender_to_mgt_man, reply_receiver_mgt_man) = mpsc::sync_channel(10);
+
+        // Mode requestors only.
+        let mode_node_pus = ModeRequestorOneChildBoundedMpsc::new(
+            TestComponentId::PusModeService as ComponentId,
+            reply_receiver_pus,
+        );
+
+        // Mode requestors and handlers.
+        let mgm_assy_node = ModeRequestorAndHandlerMpscBounded::new(
+            TestComponentId::MagnetometerAssembly as ComponentId,
+            request_receiver_mgm_assy,
+            reply_receiver_mgm_assy,
+        );
+        let mgt_dev_mgmt_node = ModeRequestorAndHandlerMpscBounded::new(
+            TestComponentId::MgtDevManager as ComponentId,
+            request_receiver_mgt_man,
+            reply_receiver_mgt_man,
+        );
+        let acs_subsystem_node = ModeRequestorAndHandlerMpscBounded::new(
+            TestComponentId::AcsSubsystem as ComponentId,
+            request_receiver_acs_subsystem,
+            reply_receiver_acs_subsystem,
+        );
+
+        // Request handlers only.
+        let mgm_dev_node_0 = ModeRequestHandlerMpscBounded::new(
+            TestComponentId::MagnetometerDevice0 as ComponentId,
+            request_receiver_mgm_dev_0,
+        );
+        let mgm_dev_node_1 = ModeRequestHandlerMpscBounded::new(
+            TestComponentId::MagnetometerDevice1 as ComponentId,
+            request_receiver_mgm_dev_1,
+        );
+        let mgt_dev_node = ModeRequestHandlerMpscBounded::new(
+            TestComponentId::MagnetorquerDevice as ComponentId,
+            request_receiver_mgt_dev,
+        );
+        let acs_ctrl_node = ModeRequestHandlerMpscBounded::new(
+            TestComponentId::AcsController as ComponentId,
+            request_receiver_acs_ctrl,
+        );
+
+        let mut mgm_dev_0 = CommonDevice::new(
+            "MGM_0",
+            TestComponentId::MagnetometerDevice0 as u64,
+            mgm_dev_node_0,
+        );
+        let mut mgm_dev_1 = CommonDevice::new(
+            "MGM_1",
+            TestComponentId::MagnetometerDevice1 as u64,
+            mgm_dev_node_1,
+        );
+        let mut mgt_dev = CommonDevice::new(
+            "MGT",
+            TestComponentId::MagnetorquerDevice as u64,
+            mgt_dev_node,
+        );
+        let mut mgt_manager = DeviceManager::new(
+            "MGT_MANAGER",
+            TestComponentId::MgtDevManager as u64,
+            mgt_dev_mgmt_node,
+        );
+        let mut mgm_assy = MgmAssembly::new(mgm_assy_node);
+        let mut acs_subsystem = AcsSubsystem::new(acs_subsystem_node);
+        let mut acs_ctrl = AcsController::new(acs_ctrl_node);
+        let mut pus_service = PusModeService::new(1, mode_node_pus);
+
+        // ACS subsystem tables
+        let mut target_table_safe = TargetTablesMapValue::new("SAFE_TARGET_TBL", None);
+        target_table_safe.add_entry(TargetTableEntry::new(
+            "CTRL_SAFE",
+            TestComponentId::AcsController as u64,
+            ModeAndSubmode::new(AcsMode::SAFE as u32, 0),
+            // All submodes allowed.
+            Some(0xffff),
+        ));
+        target_table_safe.add_entry(TargetTableEntry::new_with_precise_submode(
+            "MGM_A_NML",
+            TestComponentId::MagnetometerAssembly as u64,
+            ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+        ));
+        target_table_safe.add_entry(TargetTableEntry::new_with_precise_submode(
+            "MGT_MAN_NML",
+            TestComponentId::MgtDevManager as u64,
+            ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+        ));
+        let mut sequence_tbl_safe_0 = SequenceTableMapTable::new("SAFE_SEQ_0_TBL");
+        sequence_tbl_safe_0.add_entry(SequenceTableEntry::new(
+            "SAFE_SEQ_0_MGM_A",
+            TestComponentId::MagnetometerAssembly as u64,
+            ModeAndSubmode::new(DefaultMode::NORMAL as Mode, 0),
+            false,
+        ));
+        sequence_tbl_safe_0.add_entry(SequenceTableEntry::new(
+            "SAFE_SEQ_0_MGT_MAN",
+            TestComponentId::MgtDevManager as u64,
+            ModeAndSubmode::new(DefaultMode::NORMAL as Mode, 0),
+            false,
+        ));
+        let mut sequence_tbl_safe_1 = SequenceTableMapTable::new("SAFE_SEQ_1_TBL");
+        sequence_tbl_safe_1.add_entry(SequenceTableEntry::new(
+            "SAFE_SEQ_1_ACS_CTRL",
+            TestComponentId::AcsController as u64,
+            ModeAndSubmode::new(AcsMode::SAFE as Mode, 0),
+            false,
+        ));
+        let mut sequence_tbl_safe = SequenceTablesMapValue::new("SAFE_SEQ_TBL");
+        sequence_tbl_safe.add_sequence_table(sequence_tbl_safe_0);
+        sequence_tbl_safe.add_sequence_table(sequence_tbl_safe_1);
+        acs_subsystem.add_target_and_sequence_table(
+            AcsMode::SAFE as u32,
+            target_table_safe,
+            sequence_tbl_safe,
+        );
+
+        // Connect the PUS mode service to all mode objects.
+        connect_mode_nodes(
+            &mut pus_service,
+            request_sender_to_acs_subsystem,
+            &mut acs_subsystem,
+            reply_sender_to_pus.clone(),
+        );
+        connect_mode_nodes(
+            &mut pus_service,
+            request_sender_to_acs_ctrl.clone(),
+            &mut acs_ctrl,
+            reply_sender_to_pus.clone(),
+        );
+        connect_mode_nodes(
+            &mut pus_service,
+            request_sender_to_mgm_dev_0.clone(),
+            &mut mgm_dev_0,
+            reply_sender_to_pus.clone(),
+        );
+        connect_mode_nodes(
+            &mut pus_service,
+            request_sender_to_mgm_dev_1.clone(),
+            &mut mgm_dev_1,
+            reply_sender_to_pus.clone(),
+        );
+        connect_mode_nodes(
+            &mut pus_service,
+            request_sender_to_mgm_assy.clone(),
+            &mut mgm_assy,
+            reply_sender_to_pus.clone(),
+        );
+        connect_mode_nodes(
+            &mut pus_service,
+            request_sender_to_mgt_man.clone(),
+            &mut mgt_manager,
+            reply_sender_to_pus.clone(),
+        );
+        connect_mode_nodes(
+            &mut pus_service,
+            request_sender_to_mgt_dev.clone(),
+            &mut mgt_dev,
+            reply_sender_to_pus.clone(),
+        );
+
+        // Connect the ACS subsystem to all children.
+        connect_mode_nodes(
+            &mut acs_subsystem,
+            request_sender_to_mgm_assy,
+            &mut mgm_assy,
+            reply_sender_to_acs_subsystem.clone(),
+        );
+        connect_mode_nodes(
+            &mut acs_subsystem,
+            request_sender_to_acs_ctrl,
+            &mut acs_ctrl,
+            reply_sender_to_acs_subsystem.clone(),
+        );
+        connect_mode_nodes(
+            &mut acs_subsystem,
+            request_sender_to_mgt_man.clone(),
+            &mut mgt_manager,
+            reply_sender_to_acs_subsystem.clone(),
+        );
+
+        connect_mode_nodes(
+            &mut mgm_assy,
+            request_sender_to_mgm_dev_0,
+            &mut mgm_dev_0,
+            reply_sender_to_mgm_assy.clone(),
+        );
+        connect_mode_nodes(
+            &mut mgm_assy,
+            request_sender_to_mgm_dev_1,
+            &mut mgm_dev_1,
+            reply_sender_to_mgm_assy,
+        );
+
+        connect_mode_nodes(
+            &mut mgt_manager,
+            request_sender_to_mgt_dev,
+            &mut mgt_dev,
+            reply_sender_to_mgt_man,
+        );
+        Self {
+            pus: pus_service,
+            subsystem: acs_subsystem,
+            ctrl: acs_ctrl,
+            mgm_assy,
+            mgt_dev,
+            mgt_manager,
+            mgm_devs: [mgm_dev_0, mgm_dev_1],
+        }
+    }
+
+    pub fn run(&mut self) {
+        self.pus.run();
+        self.subsystem.run();
+        self.ctrl.run();
+        self.mgt_manager.run();
+        self.mgm_assy.run();
+        self.mgm_devs[0].run();
+        self.mgm_devs[1].run();
+        self.mgt_dev.run();
+    }
+}
+
+#[test]
+fn announce_recursively() {
+    let mut tb = TreeTestbench::new();
+    tb.pus.announce_modes_recursively();
+    // Run everything twice so the order does not matter.
+    tb.run();
+    tb.run();
+    assert_eq!(tb.subsystem.get_num_mode_requests(), 1);
+    let mut announces = tb.subsystem.mode_req_mock.announce_mode_calls.borrow_mut();
+    assert_eq!(announces.len(), 1);
+    announces = tb.ctrl.mode_req_mock.announce_mode_calls.borrow_mut();
+    assert_eq!(tb.ctrl.mode_req_mock.start_transition_calls.len(), 0);
+    assert_eq!(tb.ctrl.mode_and_submode(), UNKNOWN_MODE);
+    assert_eq!(announces.len(), 1);
+    assert_eq!(tb.mgm_assy.get_num_mode_requests(), 1);
+    announces = tb.mgm_assy.mode_req_mock.announce_mode_calls.borrow_mut();
+    assert_eq!(tb.mgm_assy.mode_req_mock.start_transition_calls.len(), 0);
+    assert_eq!(tb.mgm_assy.mode_and_submode(), UNKNOWN_MODE);
+    assert_eq!(announces.len(), 1);
+    for mgm_dev in &mut tb.mgm_devs {
+        assert_eq!(mgm_dev.get_and_clear_num_mode_msgs(), 1);
+        announces = mgm_dev.mode_req_mock.announce_mode_calls.borrow_mut();
+        assert_eq!(mgm_dev.mode_req_mock.start_transition_calls.len(), 0);
+        assert_eq!(mgm_dev.mode_and_submode(), UNKNOWN_MODE);
+        assert_eq!(announces.len(), 1);
+    }
+    assert_eq!(announces.len(), 1);
+    assert_eq!(tb.mgt_dev.get_and_clear_num_mode_msgs(), 1);
+    announces = tb.mgt_dev.mode_req_mock.announce_mode_calls.borrow_mut();
+    assert_eq!(tb.mgt_dev.mode_req_mock.start_transition_calls.len(), 0);
+    assert_eq!(tb.mgt_dev.mode_and_submode(), UNKNOWN_MODE);
+    assert_eq!(announces.len(), 1);
+    assert_eq!(tb.mgt_manager.get_num_mode_requests(), 1);
+    announces = tb
+        .mgt_manager
+        .mode_req_mock
+        .announce_mode_calls
+        .borrow_mut();
+    assert_eq!(tb.mgt_manager.mode_req_mock.start_transition_calls.len(), 0);
+    assert_eq!(tb.mgt_manager.mode_and_submode(), UNKNOWN_MODE);
+    assert_eq!(announces.len(), 1);
+}
+
+fn generic_mode_reply_checker(
+    reply_meta: MessageMetadata,
+    mode_and_submode: ModeAndSubmode,
+    expected_modes: &mut HashMap<u64, ModeAndSubmode>,
+) {
+    let id = TestComponentId::try_from(reply_meta.sender_id()).expect("invalid sender id");
+    if !expected_modes.contains_key(&reply_meta.sender_id()) {
+        panic!("received unexpected mode reply from component {:?}", id);
+    }
+    let expected_mode = expected_modes.get(&reply_meta.sender_id()).unwrap();
+    assert_eq!(
+        expected_mode, &mode_and_submode,
+        "mode mismatch for component {:?}, expected {:?}, got {:?}",
+        id, expected_mode, mode_and_submode
+    );
+    expected_modes.remove(&reply_meta.sender_id());
+}
+
+#[test]
+fn command_safe_mode() {
+    let mut tb = TreeTestbench::new();
+    assert_eq!(tb.ctrl.mode_and_submode(), UNKNOWN_MODE);
+    assert_eq!(tb.mgm_devs[0].mode_and_submode(), UNKNOWN_MODE);
+    assert_eq!(tb.mgm_devs[1].mode_and_submode(), UNKNOWN_MODE);
+    let request_id = tb
+        .pus
+        .send_mode_cmd(ModeAndSubmode::new(AcsMode::SAFE as u32, 0));
+    tb.run();
+    tb.run();
+    tb.run();
+    let expected_req_id_not_ctrl = request_id << 8;
+    let expected_req_id_ctrl = (request_id << 8) + 1;
+    assert_eq!(
+        tb.ctrl.mode_and_submode(),
+        ModeAndSubmode::new(AcsMode::SAFE as u32, 0)
+    );
+    assert_eq!(
+        tb.mgm_devs[0].mode_and_submode(),
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0)
+    );
+    assert_eq!(
+        tb.mgm_devs[1].mode_and_submode(),
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0)
+    );
+    assert_eq!(
+        tb.mgm_assy.mode_and_submode(),
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0)
+    );
+    assert_eq!(
+        tb.mgt_manager.mode_and_submode(),
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0)
+    );
+    assert_eq!(
+        tb.mgt_dev.mode_and_submode(),
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0)
+    );
+    assert_eq!(
+        tb.subsystem.mode_and_submode(),
+        ModeAndSubmode::new(AcsMode::SAFE as u32, 0)
+    );
+
+    // Check function calls for subsystem
+    let generic_mock_check = |ctx: &str,
+                              mock: &mut ModeRequestHandlerMock,
+                              expected_mode_for_transition: ModeAndSubmode,
+                              expected_req_id: RequestId| {
+        assert_eq!(mock.start_transition_calls.borrow().len(), 1);
+        let start_transition_call = mock.start_transition_calls.borrow_mut().front().unwrap();
+        assert_eq!(
+            start_transition_call.0.request_id(),
+            expected_req_id,
+            "unexpected req ID for component {}",
+            ctx
+        );
+        assert_eq!(start_transition_call.1, expected_mode_for_transition);
+        assert_eq!(mock.handle_mode_reached_calls.borrow().len(), 1);
+
+        let handle_mode_reached_ref = mock.handle_mode_reached_calls.borrow();
+        let handle_mode_reached_call = handle_mode_reached_ref.front().unwrap();
+        assert_eq!(
+            handle_mode_reached_call.as_ref().unwrap().request_id(),
+            expected_req_id
+        );
+        drop(handle_mode_reached_ref);
+
+        assert_eq!(mock.send_mode_reply_calls.borrow().len(), 1);
+        let mode_reply_call = *mock.send_mode_reply_calls.borrow_mut().front().unwrap();
+        assert_eq!(mode_reply_call.0.request_id(), expected_req_id);
+        if let ModeReply::ModeReply(mode_and_submode) = mode_reply_call.1 {
+            assert_eq!(
+                mode_and_submode, expected_mode_for_transition,
+                "unexpected mode for component {}",
+                mock.id
+            );
+        } else {
+            panic!("Unexpected mode reply for component {}", mock.id);
+        }
+        // TODO: Check all mode replies
+        assert_eq!(mock.mode_messages_received(), 3);
+        mock.clear();
+    };
+
+    generic_mock_check(
+        "subsystem",
+        &mut tb.subsystem.mode_req_mock,
+        ModeAndSubmode::new(AcsMode::SAFE as u32, 0),
+        request_id,
+    );
+    assert_eq!(
+        tb.subsystem.subsystem_helper.request_id().unwrap(),
+        request_id
+    );
+    assert_eq!(
+        tb.subsystem.subsystem_helper.state(),
+        ModeTreeHelperState::TargetKeeping
+    );
+    assert_eq!(tb.subsystem.subsystem_helper.mode(), AcsMode::SAFE as Mode);
+    assert_eq!(
+        tb.subsystem.mode_reply_mock.num_of_received_mode_replies(),
+        3
+    );
+    let mut expected_modes = HashMap::new();
+    expected_modes.insert(
+        TestComponentId::AcsController as u64,
+        ModeAndSubmode::new(AcsMode::SAFE as u32, 0),
+    );
+    expected_modes.insert(
+        TestComponentId::MagnetometerAssembly as u64,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+    );
+    expected_modes.insert(
+        TestComponentId::MgtDevManager as u64,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+    );
+    while let Some(reply) = tb.subsystem.mode_reply_mock.mode_reply_messages.pop_front() {
+        generic_mode_reply_checker(reply.0, reply.1, &mut expected_modes);
+    }
+    assert!(expected_modes.is_empty());
+
+    generic_mock_check(
+        "ctrl",
+        &mut tb.ctrl.mode_req_mock,
+        ModeAndSubmode::new(AcsMode::SAFE as u32, 0),
+        expected_req_id_ctrl,
+    );
+    generic_mock_check(
+        "mgm assy",
+        &mut tb.mgm_assy.mode_req_mock,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+        expected_req_id_not_ctrl,
+    );
+    let mut expected_modes = HashMap::new();
+    expected_modes.insert(
+        TestComponentId::MagnetometerDevice0 as u64,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+    );
+    expected_modes.insert(
+        TestComponentId::MagnetometerDevice1 as u64,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+    );
+    while let Some(reply) = tb.mgm_assy.mode_reply_mock.mode_reply_messages.pop_front() {
+        generic_mode_reply_checker(reply.0, reply.1, &mut expected_modes);
+    }
+    assert!(expected_modes.is_empty());
+
+    generic_mock_check(
+        "mgt mgmt",
+        &mut tb.mgt_manager.mode_req_mock,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+        expected_req_id_not_ctrl,
+    );
+    let mut expected_modes = HashMap::new();
+    expected_modes.insert(
+        TestComponentId::MagnetorquerDevice as u64,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+    );
+    let reply = tb
+        .mgt_manager
+        .mode_reply_mock
+        .mode_reply_messages
+        .pop_front()
+        .unwrap();
+    generic_mode_reply_checker(reply.0, reply.1, &mut expected_modes);
+
+    generic_mock_check(
+        "mgt dev",
+        &mut tb.mgt_dev.mode_req_mock,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+        expected_req_id_not_ctrl,
+    );
+    generic_mock_check(
+        "mgm dev 0",
+        &mut tb.mgm_devs[0].mode_req_mock,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+        expected_req_id_not_ctrl,
+    );
+    generic_mock_check(
+        "mgm dev 1",
+        &mut tb.mgm_devs[1].mode_req_mock,
+        ModeAndSubmode::new(DefaultMode::NORMAL as u32, 0),
+        expected_req_id_not_ctrl,
+    );
 }
diff --git a/satrs/tests/pus_events.rs b/satrs/tests/pus_events.rs
index d9d572e..81c7b70 100644
--- a/satrs/tests/pus_events.rs
+++ b/satrs/tests/pus_events.rs
@@ -6,7 +6,6 @@ use satrs::events::{EventU32, EventU32TypedSev, Severity, SeverityInfo};
 use satrs::params::U32Pair;
 use satrs::params::{Params, ParamsHeapless, WritableToBeBytes};
 use satrs::pus::event_man::{DefaultPusEventReportingMap, EventReporter, PusEventTmCreatorWithMap};
-use satrs::pus::test_util::TEST_COMPONENT_ID_0;
 use satrs::request::UniqueApidTargetId;
 use satrs::tmtc::PacketAsVec;
 use spacepackets::ecss::tm::PusTmReader;
@@ -100,10 +99,7 @@ fn test_threaded_usage() {
     // Event sender and TM checker thread
     let jh1 = thread::spawn(move || {
         event_tx
-            .send(EventMessage::new(
-                TEST_COMPONENT_ID_0.id(),
-                INFO_EVENT.into(),
-            ))
+            .send(EventMessage::new(TEST_ID.id(), INFO_EVENT.into()))
             .expect("Sending info event failed");
         loop {
             match event_packet_rx.try_recv() {
@@ -130,7 +126,7 @@ fn test_threaded_usage() {
         }
         event_tx
             .send(EventMessage::new_with_params(
-                TEST_COMPONENT_ID_0.id(),
+                TEST_ID.id(),
                 LOW_SEV_EVENT,
                 &Params::Heapless((2_u32, 3_u32).into()),
             ))