even better command stuff
This commit is contained in:
parent
fc141339f9
commit
f5c520816e
376
src/command.rs
376
src/command.rs
@ -11,154 +11,246 @@ impl CommandId for u64 {
|
||||
}
|
||||
}
|
||||
|
||||
pub trait WithCommandData {
|
||||
fn command_data(&self) -> &CommandData;
|
||||
pub trait WithCommandInfo {
|
||||
fn command_info(&self) -> &CommandInfo;
|
||||
}
|
||||
|
||||
pub trait CommandInfo: CommandId {
|
||||
pub trait GetCommandInfo: CommandId {
|
||||
fn name(&self) -> &str;
|
||||
}
|
||||
|
||||
pub struct CommandData {
|
||||
pub struct CommandInfo {
|
||||
command_id: u64,
|
||||
name: String,
|
||||
}
|
||||
|
||||
impl CommandData {
|
||||
impl CommandInfo {
|
||||
pub fn new<TName: Into<String>>(command_id: u64, name: TName) -> Self {
|
||||
CommandData {
|
||||
Self {
|
||||
command_id,
|
||||
name: name.into(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl CommandId for CommandData {
|
||||
impl CommandId for CommandInfo {
|
||||
fn command_id(&self) -> u64 {
|
||||
self.command_id
|
||||
}
|
||||
}
|
||||
|
||||
impl CommandInfo for CommandData {
|
||||
impl GetCommandInfo for CommandInfo {
|
||||
fn name(&self) -> &str {
|
||||
&self.name
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: WithCommandData> CommandId for T {
|
||||
impl<T: WithCommandInfo> CommandId for T {
|
||||
fn command_id(&self) -> u64 {
|
||||
self.command_data().command_id
|
||||
self.command_info().command_id
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: WithCommandData> CommandInfo for T {
|
||||
impl<T: WithCommandInfo> GetCommandInfo for T {
|
||||
fn name(&self) -> &str {
|
||||
&self.command_data().name
|
||||
&self.command_info().name
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, PartialEq, Clone, Copy)]
|
||||
#[derive(Debug, PartialEq, Eq, Clone, Copy)]
|
||||
pub enum CommandResult {
|
||||
Continue,
|
||||
Done,
|
||||
Cancel,
|
||||
}
|
||||
|
||||
pub trait Command: WithCommandData {
|
||||
fn init(&mut self);
|
||||
fn step(&mut self) -> CommandResult;
|
||||
fn finish(&mut self, result: CommandResult);
|
||||
#[derive(Debug, PartialEq, Eq, Clone, Copy)]
|
||||
pub enum CommandStatus {
|
||||
Stopped,
|
||||
Running,
|
||||
Ended,
|
||||
Cancelling,
|
||||
Cancelled,
|
||||
}
|
||||
|
||||
pub struct GenericCommand<TConfig, TState> {
|
||||
command_data: CommandData,
|
||||
pub config: TConfig,
|
||||
pub state: TState,
|
||||
}
|
||||
|
||||
impl<TConfig, TState> GenericCommand<TConfig, TState>
|
||||
pub struct CommandContext<T>
|
||||
where
|
||||
TConfig: 'static,
|
||||
GenericCommand<TConfig, TState>: Command,
|
||||
TState: Default + 'static,
|
||||
T: CommandImp,
|
||||
{
|
||||
pub fn new(command_data: CommandData, config: TConfig) -> Self {
|
||||
info: CommandInfo,
|
||||
status: CommandStatus,
|
||||
state: <T as CommandImp>::State,
|
||||
}
|
||||
|
||||
impl<T: CommandImp> CommandContext<T> {
|
||||
pub fn status(&self) -> CommandStatus {
|
||||
self.status
|
||||
}
|
||||
|
||||
pub fn state(&mut self) -> &mut T::State {
|
||||
&mut self.state
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: CommandImp> WithCommandInfo for CommandContext<T> {
|
||||
fn command_info(&self) -> &CommandInfo {
|
||||
&self.info
|
||||
}
|
||||
}
|
||||
|
||||
pub trait CommandImp {
|
||||
type State: Default;
|
||||
|
||||
fn init(&self, ctx: &mut CommandContext<Self>)
|
||||
where
|
||||
Self: Sized;
|
||||
fn step(&self, ctx: &mut CommandContext<Self>) -> CommandResult
|
||||
where
|
||||
Self: Sized;
|
||||
fn finish(&self, ctx: &mut CommandContext<Self>)
|
||||
where
|
||||
Self: Sized;
|
||||
}
|
||||
|
||||
pub trait Command: WithCommandInfo {
|
||||
fn reset(&mut self);
|
||||
fn status(&self) -> CommandStatus;
|
||||
fn execute(&mut self) -> CommandStatus;
|
||||
fn cancel(&mut self);
|
||||
}
|
||||
|
||||
pub struct GenericCommand<T: CommandImp> {
|
||||
context: CommandContext<T>,
|
||||
imp: T,
|
||||
}
|
||||
|
||||
impl<T: CommandImp + 'static> GenericCommand<T> {
|
||||
pub fn new(info: CommandInfo, imp: T) -> Self {
|
||||
Self {
|
||||
command_data,
|
||||
config,
|
||||
state: TState::default(),
|
||||
context: CommandContext {
|
||||
info,
|
||||
status: CommandStatus::Stopped,
|
||||
state: T::State::default(),
|
||||
},
|
||||
imp,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn new_ref(command_data: CommandData, config: TConfig) -> CommandRef {
|
||||
Rc::new(RefCell::new(Self::new(command_data, config)))
|
||||
pub fn new_ref(info: CommandInfo, imp: T) -> CommandRef {
|
||||
Rc::new(RefCell::new(Self::new(info, imp)))
|
||||
}
|
||||
}
|
||||
|
||||
impl<TConfig, TState> WithCommandData for GenericCommand<TConfig, TState> {
|
||||
fn command_data(&self) -> &CommandData {
|
||||
&self.command_data
|
||||
impl<T: CommandImp> WithCommandInfo for GenericCommand<T> {
|
||||
fn command_info(&self) -> &CommandInfo {
|
||||
&self.context.info
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: CommandImp> Command for GenericCommand<T> {
|
||||
fn status(&self) -> CommandStatus {
|
||||
self.context.status
|
||||
}
|
||||
|
||||
fn execute(&mut self) -> CommandStatus {
|
||||
use self::CommandResult::*;
|
||||
use self::CommandStatus::*;
|
||||
let do_cancel = |me: &mut Self| {
|
||||
me.context.status = Cancelling;
|
||||
me.imp.finish(&mut me.context);
|
||||
Cancelled
|
||||
};
|
||||
let do_step = |me: &mut Self| {
|
||||
let result = me.imp.step(&mut me.context);
|
||||
match result {
|
||||
Continue => Running,
|
||||
Done => {
|
||||
me.imp.finish(&mut me.context);
|
||||
Ended
|
||||
}
|
||||
Cancel => do_cancel(me),
|
||||
}
|
||||
};
|
||||
let new_status = match self.status() {
|
||||
Stopped => {
|
||||
self.imp.init(&mut self.context);
|
||||
do_step(self)
|
||||
}
|
||||
Running => do_step(self),
|
||||
Ended => Ended,
|
||||
Cancelling => do_cancel(self),
|
||||
Cancelled => Cancelled,
|
||||
};
|
||||
self.context.status = new_status;
|
||||
new_status
|
||||
}
|
||||
|
||||
fn reset(&mut self) {
|
||||
self.context.status = CommandStatus::Stopped;
|
||||
}
|
||||
fn cancel(&mut self) {
|
||||
self.context.status = CommandStatus::Cancelling;
|
||||
}
|
||||
}
|
||||
|
||||
macro_rules! command_impl {
|
||||
(
|
||||
$command:ident<$tconfig:ty,$tstate:ty>;
|
||||
$init_self:ident => $init:block;
|
||||
$step_self:ident => $step:block;
|
||||
$finish_self:ident, $finish_result:ident => $finish:block
|
||||
$slf:ident, $ctx:ident => $init:block;
|
||||
$step:block;
|
||||
$finish:block
|
||||
) => {
|
||||
pub type $command = GenericCommand<$tconfig, $tstate>;
|
||||
impl $crate::command::CommandImp for $tconfig {
|
||||
type State = $tstate;
|
||||
|
||||
impl $crate::command::Command for $command {
|
||||
fn init(&mut $init_self) {
|
||||
use $crate::command::CommandInfo;
|
||||
fn init(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) {
|
||||
use $crate::command::GetCommandInfo;
|
||||
debug!(
|
||||
"{}({}) init",
|
||||
stringify!($command_st),
|
||||
$init_self.name()
|
||||
stringify!($command),
|
||||
$ctx.name()
|
||||
);
|
||||
$init;
|
||||
}
|
||||
|
||||
fn step(&mut $step_self) -> $crate::command::CommandResult {
|
||||
use $crate::command::CommandInfo;
|
||||
fn step(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) -> $crate::command::CommandResult {
|
||||
use $crate::command::GetCommandInfo;
|
||||
debug!(
|
||||
"{}({}) step",
|
||||
stringify!($command_st),
|
||||
$step_self.name()
|
||||
stringify!($command),
|
||||
$ctx.name()
|
||||
);
|
||||
$step
|
||||
}
|
||||
|
||||
fn finish(&mut $finish_self, $finish_result: $crate::command::CommandResult) {
|
||||
use $crate::command::CommandInfo;
|
||||
fn finish(&$slf, $ctx: &mut $crate::command::CommandContext<Self>) {
|
||||
use $crate::command::GetCommandInfo;
|
||||
debug!(
|
||||
"{}({}) finish({:?}",
|
||||
stringify!($command_st),
|
||||
$finish_self.name(),
|
||||
$finish_result,
|
||||
"{}({}) finish({:?})",
|
||||
stringify!($command),
|
||||
$ctx.name(),
|
||||
$ctx.status(),
|
||||
);
|
||||
$finish;
|
||||
}
|
||||
|
||||
}
|
||||
pub type $command = $crate::command::GenericCommand<$tconfig>;
|
||||
};
|
||||
(
|
||||
$command:ident<$tconfig:ty,$tstate:ty>;
|
||||
$init_self:ident => $init:block;
|
||||
$step_self:ident => $step:block;
|
||||
$finish_self:ident => $finish:block
|
||||
$slf:ident => $init:block;
|
||||
$step:block;
|
||||
$finish:block
|
||||
) => {
|
||||
command_impl! { $command<$tconfig,$tstate>;
|
||||
$init_self => $init; $step_self => $step; $finish_self, res => {} }
|
||||
$init_self, cts => $init; $step; $finish }
|
||||
};
|
||||
(
|
||||
$command:ident<$tconfig:ty>;
|
||||
$init_self:ident => $init:block
|
||||
$slf:ident => $init:block
|
||||
) => {
|
||||
command_impl! { $command<$tconfig,()>;
|
||||
$init_self => $init; self => { $crate::command::CommandResult::Done }; self => {} }
|
||||
$slf, ctx => $init; { $crate::command::CommandResult::Done }; {} }
|
||||
};
|
||||
}
|
||||
|
||||
@ -175,134 +267,84 @@ impl CommandGroupConfig {
|
||||
}
|
||||
|
||||
pub struct CommandGroupState {
|
||||
current_idx: Option<usize>,
|
||||
has_initialized: bool,
|
||||
current_idx: usize,
|
||||
has_cancelled: bool,
|
||||
}
|
||||
|
||||
impl Default for CommandGroupState {
|
||||
fn default() -> Self {
|
||||
CommandGroupState {
|
||||
current_idx: None,
|
||||
has_initialized: false,
|
||||
current_idx: 0,
|
||||
has_cancelled: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub type CommandGroup = GenericCommand<CommandGroupConfig, CommandGroupState>;
|
||||
pub type CommandGroup = GenericCommand<CommandGroupConfig>;
|
||||
|
||||
impl Command for CommandGroup {
|
||||
fn init(&mut self) {
|
||||
if self.state.current_idx.is_some() {
|
||||
error!("CommandGroup init called twice");
|
||||
}
|
||||
self.state.current_idx = Some(0);
|
||||
self.state.has_initialized = false;
|
||||
self.state.has_cancelled = false;
|
||||
debug!("CommandGroup {} init", self.command_data().name());
|
||||
impl CommandImp for CommandGroupConfig {
|
||||
type State = CommandGroupState;
|
||||
|
||||
fn init(&self, ctx: &mut CommandContext<Self>) {
|
||||
*ctx.state() = Default::default();
|
||||
debug!("CommandGroup {} init", ctx.name());
|
||||
}
|
||||
|
||||
fn step(&mut self) -> CommandResult {
|
||||
fn step(&self, ctx: &mut CommandContext<Self>) -> CommandResult {
|
||||
use self::CommandResult::*;
|
||||
let state = &mut self.state;
|
||||
let commands = &self.config.commands;
|
||||
use self::CommandStatus::*;
|
||||
let state = ctx.state();
|
||||
let commands = &self.commands;
|
||||
if state.has_cancelled {
|
||||
debug!("CommandGroup step called after cancelled");
|
||||
return CommandResult::Cancel;
|
||||
}
|
||||
let mut current_idx = if let Some(c) = state.current_idx {
|
||||
c
|
||||
} else {
|
||||
error!("CommandGroup step called before init");
|
||||
state.has_cancelled = true;
|
||||
return CommandResult::Cancel;
|
||||
};
|
||||
let mut result: CommandResult;
|
||||
let current_idx = &mut state.current_idx;
|
||||
loop {
|
||||
if current_idx >= commands.len() {
|
||||
if *current_idx >= commands.len() {
|
||||
debug!("CommandGroup out of commands");
|
||||
return CommandResult::Done;
|
||||
}
|
||||
let mut command = commands[current_idx].borrow_mut();
|
||||
if !state.has_initialized {
|
||||
debug!(
|
||||
"CommandGroup init {0} ({1})",
|
||||
current_idx,
|
||||
command.command_data().name()
|
||||
);
|
||||
command.init();
|
||||
state.has_initialized = true;
|
||||
}
|
||||
result = command.step();
|
||||
if result != Continue {
|
||||
debug!(
|
||||
"CommandGroup {2:?} {0} ({1}) ",
|
||||
current_idx,
|
||||
command.command_data().name(),
|
||||
result
|
||||
);
|
||||
command.finish(result)
|
||||
}
|
||||
match result {
|
||||
Done => {
|
||||
current_idx += 1;
|
||||
state.has_initialized = false;
|
||||
let mut command = commands[*current_idx].borrow_mut();
|
||||
let status = command.execute();
|
||||
match status {
|
||||
Ended => {
|
||||
command.reset();
|
||||
*current_idx += 1;
|
||||
}
|
||||
Cancel => {
|
||||
Cancelled => {
|
||||
command.reset();
|
||||
state.has_cancelled = true;
|
||||
break;
|
||||
return Cancel;
|
||||
}
|
||||
Running => return Continue,
|
||||
other => {
|
||||
warn!("CommandGroup unhandled status {:?}", other);
|
||||
return Continue;
|
||||
}
|
||||
_ => break,
|
||||
}
|
||||
}
|
||||
state.current_idx = Some(current_idx);
|
||||
CommandResult::Continue
|
||||
}
|
||||
|
||||
fn finish(&mut self, result: CommandResult) {
|
||||
let state = &mut self.state;
|
||||
let commands = &mut self.config.commands;
|
||||
if result == CommandResult::Cancel {
|
||||
if let Some(command) = state
|
||||
.current_idx
|
||||
.and_then(|current_idx| commands.get(current_idx))
|
||||
{
|
||||
command.borrow_mut().finish(result);
|
||||
fn finish(&self, ctx: &mut CommandContext<Self>) {
|
||||
let commands = &self.commands;
|
||||
if ctx.status() == CommandStatus::Cancelling {
|
||||
if let Some(command) = commands.get(ctx.state().current_idx) {
|
||||
let mut command = command.borrow_mut();
|
||||
command.cancel();
|
||||
command.execute();
|
||||
}
|
||||
}
|
||||
state.current_idx = None;
|
||||
}
|
||||
}
|
||||
|
||||
pub struct CommandRun {
|
||||
initialized: bool,
|
||||
cancelled: bool,
|
||||
command: CommandRef,
|
||||
}
|
||||
|
||||
impl CommandId for CommandRun {
|
||||
fn command_id(&self) -> u64 {
|
||||
self.command.borrow().command_data().command_id()
|
||||
}
|
||||
}
|
||||
|
||||
impl CommandRun {
|
||||
pub fn initialized(&self) -> bool {
|
||||
self.initialized
|
||||
}
|
||||
|
||||
pub fn cancelled(&self) -> bool {
|
||||
self.cancelled
|
||||
}
|
||||
|
||||
pub fn cancel(&mut self) -> bool {
|
||||
if self.cancelled {
|
||||
false
|
||||
} else {
|
||||
self.cancelled = true;
|
||||
true
|
||||
}
|
||||
self.command.borrow().command_info().command_id()
|
||||
}
|
||||
}
|
||||
|
||||
@ -346,50 +388,28 @@ impl CommandScheduler {
|
||||
* 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_data().command_id()) {
|
||||
if self.is_running(command.borrow().command_info().command_id()) {
|
||||
false
|
||||
} else {
|
||||
self.running_commands.push(CommandRun {
|
||||
initialized: false,
|
||||
cancelled: false,
|
||||
command,
|
||||
});
|
||||
self.running_commands.push(CommandRun { command });
|
||||
true
|
||||
}
|
||||
}
|
||||
|
||||
pub fn cancel<T: CommandId>(&mut self, id: T) -> bool {
|
||||
self.get_command_run_mut(id).map_or(false, |command_run| {
|
||||
command_run.cancelled = true;
|
||||
command_run.command.borrow_mut().cancel();
|
||||
true
|
||||
})
|
||||
}
|
||||
|
||||
pub fn execute(&mut self) {
|
||||
use self::CommandStatus::*;
|
||||
self.running_commands.drain_filter(|command_run| {
|
||||
let mut command = command_run.command.borrow_mut();
|
||||
if !command_run.initialized && !command_run.cancelled {
|
||||
{
|
||||
let command_data = command.command_data();
|
||||
debug!(
|
||||
"initialzing command {} (id {})",
|
||||
command_data.name(),
|
||||
command_data.command_id()
|
||||
);
|
||||
}
|
||||
command.init();
|
||||
command_run.initialized = true;
|
||||
}
|
||||
let result = if command_run.initialized {
|
||||
command.step()
|
||||
} else {
|
||||
CommandResult::Cancel
|
||||
};
|
||||
if result != CommandResult::Continue {
|
||||
command.finish(result);
|
||||
true
|
||||
} else {
|
||||
false
|
||||
match command.execute() {
|
||||
Ended | Cancelled => true,
|
||||
Stopped | Running | Cancelling => false,
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
43
src/robot.rs
43
src/robot.rs
@ -3,13 +3,13 @@ use std::rc::Rc;
|
||||
use std::thread;
|
||||
use std::time::Duration;
|
||||
|
||||
use command::{CommandData, CommandGroup, CommandGroupConfig, CommandRef, CommandScheduler};
|
||||
use command::{CommandGroup, CommandGroupConfig, CommandInfo, CommandRef, CommandScheduler};
|
||||
use drive::{Drive, DriveRef};
|
||||
|
||||
mod cmds {
|
||||
use command::{Command, CommandResult, GenericCommand};
|
||||
use drive::DriveRef;
|
||||
use self::CommandResult::*;
|
||||
use command::CommandResult;
|
||||
use drive::DriveRef;
|
||||
|
||||
pub struct DriveCommandConfig {
|
||||
pub drive: DriveRef,
|
||||
@ -25,28 +25,28 @@ mod cmds {
|
||||
}
|
||||
}
|
||||
|
||||
pub type DriveCommand = GenericCommand<DriveCommandConfig, DriveCommandState>;
|
||||
|
||||
impl Command for DriveCommand {
|
||||
fn init(&mut self) {
|
||||
self.state = DriveCommandState::default();
|
||||
}
|
||||
fn step(&mut self) -> CommandResult {
|
||||
self.config.drive.borrow_mut().drive_powers(1.0, 1.0);
|
||||
self.state.iterations += 1;
|
||||
if self.state.iterations >= 10 {
|
||||
command_impl! (
|
||||
DriveCommand<DriveCommandConfig, DriveCommandState>;
|
||||
self, ctx => {
|
||||
*ctx.state() = DriveCommandState::default();
|
||||
}; {
|
||||
let state = ctx.state();
|
||||
self.drive.borrow_mut().drive_powers(1.0, 1.0);
|
||||
state.iterations += 1;
|
||||
if state.iterations >= 10 {
|
||||
Done
|
||||
} else {
|
||||
Continue
|
||||
}
|
||||
}; {
|
||||
self.drive.borrow_mut().drive_powers(0.0, 0.0);
|
||||
}
|
||||
fn finish(&mut self, res: CommandResult) {
|
||||
self.config.drive.borrow_mut().drive_powers(0.0, 0.0);
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
pub struct LogCommandConfig;
|
||||
|
||||
command_impl! {
|
||||
LogCmd<()>; self => { info!("{}", "LogCmd") }
|
||||
LogCmd<LogCommandConfig>; self => { info!("{}", "LogCmd") }
|
||||
}
|
||||
}
|
||||
|
||||
@ -69,14 +69,15 @@ impl Robot {
|
||||
warn!("Starting RobotRS");
|
||||
|
||||
let command = cmds::DriveCommand::new_ref(
|
||||
CommandData::new(0, "DriveCommand"),
|
||||
CommandInfo::new(0, "DriveCommand"),
|
||||
cmds::DriveCommandConfig {
|
||||
drive: self.drive.clone(),
|
||||
},
|
||||
);
|
||||
let log_command = cmds::LogCmd::new_ref(CommandData::new(2, "LogCommand"), ());
|
||||
let log_command =
|
||||
cmds::LogCmd::new_ref(CommandInfo::new(2, "LogCommand"), cmds::LogCommandConfig);
|
||||
let command_group = CommandGroup::new(
|
||||
CommandData::new(1, "Group"),
|
||||
CommandInfo::new(1, "Group"),
|
||||
CommandGroupConfig::new(vec![command, log_command]),
|
||||
);
|
||||
let command_ref: CommandRef = Rc::new(RefCell::new(command_group));
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user