lots of command improvements

src/command.rs

@@ -73,12 +73,17 @@ pub enum CommandStatus {
     Cancelled,
 }
 
+pub trait CommandParent {
+    fn start_command(&mut self, cmd: CommandRef) -> bool;
+}
+
 pub struct CommandContext<T>
 where
     T: CommandImp,
 {
     info: CommandInfo,
     status: CommandStatus,
+    parent: Option<*mut CommandParent>,
     state: <T as CommandImp>::State,
 }
 
@@ -90,6 +95,16 @@ impl<T: CommandImp> CommandContext<T> {
     pub fn state(&mut self) -> &mut T::State {
         &mut self.state
     }
+
+    pub fn parent(&self) -> &mut CommandParent {
+        self.try_parent()
+            .expect("CommandContext parent called but no parent exists")
+    }
+
+    pub fn try_parent<'a>(&'a self) -> Option<&'a mut CommandParent> {
+        self.parent
+            .map(|ptr| unsafe { &mut *ptr } as &'a mut CommandParent)
+    }
 }
 
 impl<T: CommandImp> WithCommandInfo for CommandContext<T> {
@@ -115,13 +130,13 @@ pub trait CommandImp {
 pub trait Command: WithCommandInfo {
     fn reset(&mut self);
     fn status(&self) -> CommandStatus;
-    fn execute(&mut self) -> CommandStatus;
+    fn execute(&mut self, &mut CommandParent) -> CommandStatus;
     fn cancel(&mut self);
 }
 
 pub struct GenericCommand<T: CommandImp> {
     context: CommandContext<T>,
-    imp: T,
+    pub imp: T,
 }
 
 impl<T: CommandImp + 'static> GenericCommand<T> {
@@ -130,13 +145,14 @@ impl<T: CommandImp + 'static> GenericCommand<T> {
             context: CommandContext {
                 info,
                 status: CommandStatus::Stopped,
+                parent: None,
                 state: T::State::default(),
             },
             imp,
         }
     }
 
-    pub fn new_ref(info: CommandInfo, imp: T) -> CommandRef {
+    pub fn new_ref(info: CommandInfo, imp: T) -> CommandRef<Self> {
         Rc::new(RefCell::new(Self::new(info, imp)))
     }
 }
@@ -152,9 +168,11 @@ impl<T: CommandImp> Command for GenericCommand<T> {
         self.context.status
     }
 
-    fn execute(&mut self) -> CommandStatus {
+    fn execute(&mut self, parent: &mut CommandParent) -> CommandStatus {
         use self::CommandResult::*;
         use self::CommandStatus::*;
+        let parent_ptr: *mut CommandParent = unsafe { std::mem::transmute(parent) };
+        self.context.parent = Some(parent_ptr);
         let do_cancel = |me: &mut Self| {
             me.context.status = Cancelling;
             me.imp.finish(&mut me.context);
@@ -182,14 +200,23 @@ impl<T: CommandImp> Command for GenericCommand<T> {
             Cancelled => Cancelled,
         };
         self.context.status = new_status;
+        self.context.parent = None;
         new_status
     }
 
     fn reset(&mut self) {
         use self::CommandStatus::*;
         self.context.status = match self.context.status {
-            Ended | Cancelled => Stopped,
+            Stopped | Ended | Cancelled => Stopped,
-            other => other,
+            other => {
+                warn!(
+                    "GenericCommand({}) reset called in non-resetable status {:?}",
+                    self.name(),
+                    other
+                );
+                self.imp.finish(&mut self.context);
+                Stopped
+            }
         };
     }
 
@@ -203,6 +230,20 @@ impl<T: CommandImp> Command for GenericCommand<T> {
 }
 
 macro_rules! command_impl {
+    (
+        $command:ident<$tconfig:ty>;
+        $slf:ident => $init:block
+    ) => {
+        command_impl! { $command<$tconfig>;
+        $slf, ctx => $init }
+    };
+    (
+        $command:ident<$tconfig:ty>;
+        $slf:ident, $ctx:ident => $init:block
+    ) => {
+        command_impl! { $command<$tconfig,()>;
+        $slf, $ctx => $init; { $crate::command::CommandResult::Done }; {} }
+    };
     (
         $command:ident<$tconfig:ty,$tstate:ty>;
         $slf:ident, $ctx:ident => $init:block;
@@ -254,16 +295,10 @@ macro_rules! command_impl {
         command_impl! { $command<$tconfig,$tstate>;
         $init_self, cts => $init; $step; $finish }
     };
-    (
+
-        $command:ident<$tconfig:ty>;
-        $slf:ident => $init:block
-    ) => {
-        command_impl! { $command<$tconfig,()>;
-        $slf, ctx => $init; { $crate::command::CommandResult::Done }; {} }
-    };
 }
 
-pub type CommandRef = Rc<RefCell<dyn Command>>;
+pub type CommandRef<T = dyn Command> = Rc<RefCell<T>>;
 
 pub struct CommandGroupConfig {
     pub commands: Vec<CommandRef>,
@@ -302,28 +337,26 @@ impl CommandImp for CommandGroupConfig {
     fn step(&self, ctx: &mut CommandContext<Self>) -> CommandResult {
         use self::CommandResult::*;
         use self::CommandStatus::*;
-        let state = ctx.state();
         let commands = &self.commands;
-        if state.has_cancelled {
+        if ctx.state().has_cancelled {
             debug!("CommandGroup step called after cancelled");
             return CommandResult::Cancel;
         }
-        let current_idx = &mut state.current_idx;
         loop {
-            if *current_idx >= commands.len() {
+            if ctx.state().current_idx >= commands.len() {
                 debug!("CommandGroup out of commands");
                 return CommandResult::Done;
             }
-            let mut command = commands[*current_idx].borrow_mut();
+            let mut command = commands[ctx.state().current_idx].borrow_mut();
-            let status = command.execute();
+            let status = command.execute(ctx.parent());
             match status {
                 Ended => {
                     command.reset();
-                    *current_idx += 1;
+                    ctx.state().current_idx += 1;
                 }
                 Cancelled => {
                     command.reset();
-                    state.has_cancelled = true;
+                    ctx.state().has_cancelled = true;
                     return Cancel;
                 }
                 Running => return Continue,
@@ -341,89 +374,116 @@ impl CommandImp for CommandGroupConfig {
             if let Some(command) = commands.get(ctx.state().current_idx) {
                 let mut command = command.borrow_mut();
                 command.cancel();
-                command.execute();
+                command.execute(ctx.parent());
                 command.reset();
             }
         }
     }
 }
 
-pub struct CommandRun {
+struct SchedulerCommandParent {
-    command: CommandRef,
+    start_queue: Vec<CommandRef>,
 }
 
-impl CommandId for CommandRun {
+impl SchedulerCommandParent {
-    fn command_id(&self) -> u64 {
+    fn new() -> Self {
-        self.command.borrow().command_info().command_id()
+        Self {
+            start_queue: Vec::new(),
+        }
     }
 }
 
-pub struct CommandScheduler {
+impl CommandParent for SchedulerCommandParent {
-    running_commands: Vec<CommandRun>,
+    fn start_command(&mut self, command: CommandRef) -> bool {
+        self.start_queue.push(command);
+        true
+    }
 }
 
-impl CommandScheduler {
+pub struct Scheduler {
+    running_commands: Vec<CommandRef>,
+}
+
+impl Scheduler {
     pub fn new() -> Self {
-        CommandScheduler {
+        Self {
             running_commands: Vec::new(),
         }
     }
 
-    pub fn get_command_run<T: CommandId>(&self, id: T) -> Option<&CommandRun> {
+    fn get_command<T: CommandId>(&self, id: T) -> Option<&CommandRef> {
         let id = id.command_id();
-        for command_run in &self.running_commands {
+        for command in &self.running_commands {
-            if command_run.command_id() == id {
+            if command.borrow().command_info().command_id() == id {
-                return Some(command_run);
+                return Some(command);
             }
         }
         None
     }
 
-    fn get_command_run_mut<T: CommandId>(&mut self, id: T) -> Option<&mut CommandRun> {
+    fn get_command_mut<T: CommandId>(&mut self, id: T) -> Option<&mut CommandRef> {
         let id = id.command_id();
-        for command_run in &mut self.running_commands {
+        for command in &mut self.running_commands {
-            if command_run.command_id() == id {
+            if command.borrow().command_info().command_id() == id {
-                return Some(command_run);
+                return Some(command);
             }
         }
         None
     }
 
-    pub fn is_running<T: CommandId>(&self, id: T) -> bool {
+    pub fn start_command(&mut self, command: CommandRef) -> bool {
-        self.get_command_run(id).is_some()
+        let command_id = command.borrow().command_info().command_id();
-    }
+        if self.is_running(command_id) {
-
-    /**
-     * Starts running a command.
-     * Returns true if the command was started, false if it was not started because it was already running
-     */
-    pub fn start(&mut self, command: CommandRef) -> bool {
-        if self.is_running(command.borrow().command_info().command_id()) {
             false
         } else {
-            self.running_commands.push(CommandRun { command });
+            self.running_commands.push(command);
             true
         }
     }
 
-    pub fn cancel<T: CommandId>(&mut self, id: T) -> bool {
+    pub fn is_running<T: CommandId>(&self, id: T) -> bool {
-        self.get_command_run_mut(id).map_or(false, |command_run| {
+        self.get_command(id).is_some()
-            command_run.command.borrow_mut().cancel();
+    }
+
+    pub fn cancel_command<T: CommandId>(&mut self, id: T) -> bool {
+        self.get_command_mut(id).map_or(false, |command| {
+            command.borrow_mut().cancel();
             true
         })
     }
 
     pub fn execute(&mut self) {
         use self::CommandStatus::*;
-        self.running_commands.drain_filter(|command_run| {
+        let mut parent = SchedulerCommandParent::new();
-            let mut command = command_run.command.borrow_mut();
+        let exec_command =
-            match command.execute() {
+            |command: &mut Command, parent: &mut CommandParent| match command.execute(parent) {
                 Ended | Cancelled => {
                     command.reset();
                     true
-                },
+                }
                 Stopped | Running | Cancelling => false,
-            }
+            };
+        self.running_commands.drain_filter(|command| {
+            let mut command = command.borrow_mut();
+            exec_command(&mut *command, &mut parent)
         });
+        while !parent.start_queue.is_empty() {
+            let mut new_parent = SchedulerCommandParent::new();
+            {
+                let more_commands = parent.start_queue.into_iter().filter_map(|command_ref| {
+                    let done = {
+                        let mut command = command_ref.borrow_mut();
+                        exec_command(&mut *command, &mut new_parent)
+                    };
+                    if !done {
+                        Some(command_ref)
+                    } else {
+                        None
+                    }
+                });
+                self.running_commands.extend(more_commands);
+            }
+            parent = new_parent;
+        }
     }
 }

src/robot.rs

@@ -3,7 +3,9 @@ use std::rc::Rc;
 use std::thread;
 use std::time::Duration;
 
-use command::{CommandGroup, CommandGroupConfig, CommandInfo, CommandRef, CommandScheduler};
+use command::{
+    CommandGroup, CommandGroupConfig, CommandInfo, CommandRef, Scheduler as CommandScheduler,
+};
 use drive::{Drive, DriveRef};
 
 mod cmds {
@@ -48,6 +50,21 @@ mod cmds {
     command_impl! {
         LogCmd<LogCommandConfig>; self => { info!("{}", "LogCmd") }
     }
+
+    use command::Command;
+    use std::cell::RefCell;
+    use std::rc::Weak;
+    pub struct StartCommandConfig(pub Weak<RefCell<Command>>);
+
+    command_impl! {
+        StartCommand<StartCommandConfig>; self, ctx => {
+            let command = if let Some(command) = self.0.upgrade() {command} else {
+                warn!("StartCommand({}) has undefined weak reference to command", ctx.name());
+                return;
+            };
+            ctx.parent().start_command(command);
+        }
+    }
 }
 
 pub struct Robot {
@@ -76,13 +93,17 @@ impl Robot {
         );
         let log_command =
             cmds::LogCmd::new_ref(CommandInfo::new(2, "LogCommand"), cmds::LogCommandConfig);
-        let command_group = CommandGroup::new(
+        let start_command: Rc<_> = cmds::StartCommand::new_ref(
+            CommandInfo::new(3, "StartCommand"),
+            cmds::StartCommandConfig(std::rc::Weak::<RefCell<CommandGroup>>::new()),
+        );
+        let command_group = CommandGroup::new_ref(
             CommandInfo::new(1, "Group"),
-            CommandGroupConfig::new(vec![command, log_command]),
+            CommandGroupConfig::new(vec![command, log_command, start_command.clone()]),
         );
-        let command_ref: CommandRef = Rc::new(RefCell::new(command_group));
+        start_command.borrow_mut().imp.0 = Rc::downgrade(&command_group) as std::rc::Weak<RefCell<dyn crate::command::Command>>;
 
-        self.command_scheduler.start(command_ref);
+        self.command_scheduler.start_command(command_group);
 
         loop {
             self.execute();