use crate::Error as OpenrrCommandError;
use arci::{BaseVelocity, MoveBase, Navigation, Speaker};
use async_recursion::async_recursion;
use k::nalgebra::{Isometry2, Vector2};
use log::info;
use openrr_client::{isometry, BoxRobotClient};
use std::{
error::Error,
fs::File,
io::{BufRead, BufReader},
path::PathBuf,
process::Command,
thread::sleep,
time::{Duration, Instant},
};
use structopt::StructOpt;
fn parse_joints<T, U>(s: &str) -> Result<(T, U), Box<dyn Error>>
where
T: std::str::FromStr,
T::Err: Error + 'static,
U: std::str::FromStr,
U::Err: Error + 'static,
{
let pos = s
.find('=')
.ok_or_else(|| format!("invalid KEY=value: no `=` found in `{}`", s))?;
Ok((s[..pos].parse()?, s[pos + 1..].parse()?))
}
#[derive(StructOpt, Debug)]
#[structopt(rename_all = "snake_case")]
pub enum RobotCommand {
SendJoints {
name: String,
#[structopt(short, long, default_value = "3.0")]
duration: f64,
#[structopt(name = "interpolate", short, long)]
use_interpolation: bool,
#[structopt(short, parse(try_from_str=parse_joints))]
joint: Vec<(usize, f64)>,
},
SendJointsPose {
name: String,
pose_name: String,
#[structopt(short, long, default_value = "3.0")]
duration: f64,
},
MoveIk {
name: String,
#[structopt(short, long)]
x: Option<f64>,
#[structopt(short, long)]
y: Option<f64>,
#[structopt(short, long)]
z: Option<f64>,
#[structopt(long)]
yaw: Option<f64>,
#[structopt(short, long)]
pitch: Option<f64>,
#[structopt(short, long)]
roll: Option<f64>,
#[structopt(short, long, default_value = "3.0")]
duration: f64,
#[structopt(name = "interpolate", short, long)]
use_interpolation: bool,
#[structopt(name = "local", short, long)]
is_local: bool,
},
GetState { name: String },
LoadCommands {
#[structopt(parse(from_os_str))]
command_file_path: PathBuf,
},
List,
Speak { message: Vec<String> },
ExecuteCommand { command: Vec<String> },
GetNavigationCurrentPose,
SendNavigationGoal {
x: f64,
y: f64,
yaw: f64,
#[structopt(short, long, default_value = "map")]
frame_id: String,
#[structopt(short, long, default_value = "100.0")]
timeout_secs: f64,
},
CancelNavigationGoal,
SendBaseVelocity {
x: f64,
y: f64,
theta: f64,
#[structopt(short, long, default_value = "1.0")]
duration_secs: f64,
},
}
pub struct RobotCommandExecutor {}
impl RobotCommandExecutor {
#[async_recursion]
pub async fn execute(
&self,
client: &BoxRobotClient,
command: &RobotCommand,
) -> Result<(), OpenrrCommandError> {
match &command {
RobotCommand::SendJoints {
name,
duration,
use_interpolation,
joint,
} => {
let mut positions = client.current_joint_positions(name).await?;
let mut should_send = false;
for (index, position) in joint {
if *index < positions.len() {
should_send = true;
positions[*index] = *position;
}
}
if !should_send {
return Ok(());
}
if *use_interpolation {
client
.send_joint_positions_with_pose_interpolation(name, &positions, *duration)
.await?;
} else {
client
.send_joint_positions(name, &positions, *duration)
.await?;
}
}
RobotCommand::SendJointsPose {
name,
pose_name,
duration,
} => {
client.send_joints_pose(name, pose_name, *duration).await?;
}
RobotCommand::MoveIk {
name,
x,
y,
z,
yaw,
pitch,
roll,
duration,
use_interpolation,
is_local,
} => {
if !client.is_ik_client(name) {
return Err(OpenrrCommandError::NoIkClient(name.clone()));
}
let mut should_send = false;
let current_pose = client.current_end_transform(name).await?;
let target_pose = [
if let Some(x) = x {
should_send = true;
*x
} else if *is_local {
0.0
} else {
current_pose.translation.x
},
if let Some(y) = y {
should_send = true;
*y
} else if *is_local {
0.0
} else {
current_pose.translation.y
},
if let Some(z) = z {
should_send = true;
*z
} else if *is_local {
0.0
} else {
current_pose.translation.z
},
if let Some(roll) = roll {
should_send = true;
*roll
} else if *is_local {
0.0
} else {
current_pose.rotation.euler_angles().0
},
if let Some(pitch) = pitch {
should_send = true;
*pitch
} else if *is_local {
0.0
} else {
current_pose.rotation.euler_angles().1
},
if let Some(yaw) = yaw {
should_send = true;
*yaw
} else if *is_local {
0.0
} else {
current_pose.rotation.euler_angles().2
},
];
if !should_send {
return Ok(());
}
let target_pose = isometry(
target_pose[0],
target_pose[1],
target_pose[2],
target_pose[3],
target_pose[4],
target_pose[5],
);
let target_pose = if *is_local {
client.transform(name, &target_pose).await?
} else {
target_pose
};
if *use_interpolation {
client
.move_ik_with_interpolation(name, &target_pose, *duration)
.await?
} else {
client.move_ik(name, &target_pose, *duration).await?
}
}
RobotCommand::GetState { name } => {
println!(
"Joint positions : {:?}",
client.current_joint_positions(name).await?
);
if client.is_ik_client(name) {
let pose = client.current_end_transform(name).await?;
println!("End pose");
println!(" translation = {:?}", pose.translation.vector.data);
println!(" rotation = {:?}", pose.rotation.euler_angles());
}
}
RobotCommand::LoadCommands { command_file_path } => {
for command in load_command_file_and_filter(command_file_path.clone())? {
let command_parsed_iter = command.split_whitespace();
let read_opt = RobotCommand::from_iter(command_parsed_iter);
info!("Executing {}", command);
self.execute(client, &read_opt).await?;
}
}
RobotCommand::List => {
println!("Raw joint trajectory clients");
for name in client.raw_joint_trajectory_clients_names() {
println!(" {}", name);
}
println!("Joint trajectory clients");
for name in client.joint_trajectory_clients_names() {
println!(" {}", name);
}
println!("Collision check clients");
for name in client.collision_check_clients_names() {
println!(" {}", name);
}
println!("Ik clients");
for name in client.ik_clients_names() {
println!(" {}", name);
}
}
RobotCommand::Speak { message } => {
client.speak(&message.join(" "));
}
RobotCommand::ExecuteCommand { command } => {
let mut iter = command.iter();
let cmd_str = iter
.next()
.ok_or_else(|| OpenrrCommandError::NoCommand(command.to_owned()))?;
let output = Command::new(cmd_str).args(iter).output().map_err(|e| {
OpenrrCommandError::CommandExecutionFailure(command.to_owned(), e)
})?;
info!("{}", String::from_utf8_lossy(&output.stdout));
}
RobotCommand::GetNavigationCurrentPose => {
println!("Base Pose {}", client.current_pose()?);
}
RobotCommand::SendNavigationGoal {
x,
y,
yaw,
frame_id,
timeout_secs,
} => {
client
.send_pose(
Isometry2::new(Vector2::new(*x, *y), *yaw),
frame_id,
Duration::from_secs_f64(*timeout_secs),
)
.await?;
}
RobotCommand::CancelNavigationGoal => {
client.cancel()?;
}
RobotCommand::SendBaseVelocity {
x,
y,
theta,
duration_secs,
} => {
let start = Instant::now();
let duration = Duration::from_secs_f64(*duration_secs);
let sleep_duration = Duration::from_secs_f64(0.01);
while start.elapsed() < duration {
client.send_velocity(&BaseVelocity::new(*x, *y, *theta))?;
sleep(sleep_duration);
}
}
}
Ok(())
}
}
pub fn load_command_file_and_filter(file_path: PathBuf) -> Result<Vec<String>, OpenrrCommandError> {
let file = File::open(&file_path)
.map_err(|e| OpenrrCommandError::CommandFileOpenFailure(file_path, e.to_string()))?;
let buf = BufReader::new(file);
Ok(buf
.lines()
.map(|line| line.expect("Could not parse line"))
.filter(|command| {
let command_parsed_iter = command.split_whitespace();
command_parsed_iter.clone().count() > 0
&& command_parsed_iter.clone().next().unwrap().find('#') == None
})
.collect())
}