use std::mem::size_of;
use crate::exception::{create_command_exception, FrankaException, FrankaResult};
use crate::gripper::gripper_state::GripperState;
use crate::gripper::types::{
kCommandPort, kVersion, Command, ConnectRequest, ConnectRequestWithHeader, ConnectResponse,
GraspRequest, GraspRequestWithHeader, GraspResponse, GripperStateIntern,
HomingRequestWithHeader, HomingResponse, MoveRequest, MoveRequestWithHeader, MoveResponse,
Status, StopRequestWithHeader, StopResponse,
};
use crate::network::{Network, NetworkType};
pub mod gripper_state;
pub(crate) mod types;
pub struct Gripper {
network: Network,
ri_version: Option<u16>,
}
impl Gripper {
pub fn new(franka_address: &str) -> FrankaResult<Gripper> {
let mut gripper = Gripper {
network: Network::new(NetworkType::Gripper, franka_address, kCommandPort).map_err(
|e| FrankaException::NetworkException {
message: e.to_string(),
},
)?,
ri_version: None,
};
gripper.connect_gripper(&kVersion)?;
Ok(gripper)
}
fn connect_gripper(&mut self, ri_version: &u16) -> Result<(), FrankaException> {
let connect_command = ConnectRequestWithHeader {
header: self.network.create_header_for_gripper(
Command::kConnect,
size_of::<ConnectRequestWithHeader>(),
),
request: ConnectRequest::new(self.network.get_udp_port()),
};
let command_id: u32 = self.network.tcp_send_request(connect_command);
let connect_response: ConnectResponse =
self.network.tcp_blocking_receive_response(command_id);
match connect_response.status {
Status::kSuccess => {
self.ri_version = Some(connect_response.version);
Ok(())
}
_ => Err(FrankaException::IncompatibleLibraryVersionError {
server_version: connect_response.version,
library_version: *ri_version,
}),
}
}
pub fn move_gripper(&mut self, width: f64, speed: f64) -> FrankaResult<bool> {
let command = MoveRequestWithHeader {
header: self
.network
.create_header_for_gripper(Command::kMove, size_of::<MoveRequestWithHeader>()),
request: MoveRequest::new(width, speed),
};
let command_id: u32 = self.network.tcp_send_request(command);
let response: MoveResponse = self.network.tcp_blocking_receive_response(command_id);
handle_response_status(&response.status)
}
pub fn homing(&mut self) -> FrankaResult<bool> {
let command: HomingRequestWithHeader = self
.network
.create_header_for_gripper(Command::kHoming, size_of::<HomingRequestWithHeader>());
let command_id: u32 = self.network.tcp_send_request(command);
let response: HomingResponse = self.network.tcp_blocking_receive_response(command_id);
handle_response_status(&response.status)
}
pub fn stop(&mut self) -> FrankaResult<bool> {
let command: StopRequestWithHeader = self
.network
.create_header_for_gripper(Command::kStop, size_of::<StopRequestWithHeader>());
let command_id: u32 = self.network.tcp_send_request(command);
let response: StopResponse = self.network.tcp_blocking_receive_response(command_id);
handle_response_status(&response.status)
}
pub fn grasp(
&mut self,
width: f64,
speed: f64,
force: f64,
epsilon_inner: Option<f64>,
epsilon_outer: Option<f64>,
) -> FrankaResult<bool> {
let epsilon_inner = epsilon_inner.unwrap_or(0.005);
let epsilon_outer = epsilon_outer.unwrap_or(0.005);
let command = GraspRequestWithHeader {
header: self
.network
.create_header_for_gripper(Command::kGrasp, size_of::<GraspRequestWithHeader>()),
request: GraspRequest::new(width, speed, force, epsilon_inner, epsilon_outer),
};
let command_id: u32 = self.network.tcp_send_request(command);
let response: GraspResponse = self.network.tcp_blocking_receive_response(command_id);
handle_response_status(&response.status)
}
pub fn read_once(&mut self) -> FrankaResult<GripperState> {
while self.network.udp_receive::<GripperStateIntern>().is_some() {}
Ok(self
.network
.udp_blocking_receive::<GripperStateIntern>()?
.into())
}
pub fn server_version(&self) -> u16 {
self.ri_version.unwrap()
}
}
fn handle_response_status(status: &Status) -> FrankaResult<bool> {
match status {
Status::kSuccess => Ok(true),
Status::kFail => Err(create_command_exception(
"libfranka-rs gripper: Command failed!",
)),
Status::kUnsuccessful => Ok(false),
Status::kAborted => Err(create_command_exception(
"libfranka-rs gripper: Command aborted!",
)),
}
}
#[cfg(test)]
mod tests {
use crate::gripper::types::{
kCommandPort, kVersion, Command, CommandHeader, ConnectRequestWithHeader, ConnectResponse,
GraspRequest, GraspRequestWithHeader, GraspResponse, GripperStateIntern,
HomingRequestWithHeader, HomingResponse, MoveRequest, MoveRequestWithHeader, MoveResponse,
Status, StopRequestWithHeader, StopResponse,
};
use crate::gripper::Gripper;
use crate::FrankaResult;
use bincode::{deserialize, serialize, serialized_size};
use mio::net::UdpSocket;
use mockall::{automock, predicate::*};
use std::io::{Read, Write};
use std::net::TcpListener;
use std::net::ToSocketAddrs;
use std::rc::Rc;
use std::sync::{Arc, Mutex};
use std::time::{Duration, Instant};
use crate::exception::FrankaException;
use crate::gripper::types::Status::{kFail, kSuccess};
use crate::network::MessageCommand;
use std::iter::FromIterator;
use std::mem::size_of;
struct Socket<F: Fn(&Vec<u8>), G: Fn(&mut Vec<u8>)> {
pub send_bytes: F,
pub receive_bytes: G,
}
struct GripperMockServer {
server_version: u16,
}
pub struct ServerReaction {}
#[automock]
#[allow(unused)]
impl ServerReaction {
fn process_received_bytes(&self, bytes: &mut Vec<u8>) -> Vec<u8> {
Vec::new()
}
fn number_of_reactions(&self) -> usize {
0
}
}
impl GripperMockServer {
pub fn new(server_version: u16) -> Self {
GripperMockServer { server_version }
}
pub fn server_thread(&mut self, reaction: &mut MockServerReaction) {
let hostname: &str = "127.0.0.1";
let address = format!("{}:{}", hostname, kCommandPort)
.to_socket_addrs()
.unwrap()
.next()
.unwrap();
let srv_sock = TcpListener::bind(address).unwrap();
let (tcp_socket, _remote_address) = srv_sock.accept().unwrap();
tcp_socket.set_nonblocking(false).unwrap();
tcp_socket.set_nodelay(true).unwrap();
let tcp_socket = Rc::new(Mutex::new(tcp_socket));
let mut tcp_socket_wrapper = Socket {
send_bytes: |bytes| {
let mut soc = tcp_socket.lock().unwrap();
soc.write(bytes.as_slice()).unwrap();
},
receive_bytes: |bytes| {
let mut soc = tcp_socket.lock().unwrap();
let mut buffer = vec![0 as u8; 100];
let num_bytes = soc.read(&mut buffer).unwrap();
buffer.resize(num_bytes, 0);
assert_eq!(buffer.len(), num_bytes);
*bytes = buffer;
},
};
let request = self.receive_gripper_connect_request(&mut tcp_socket_wrapper);
let udp_port = request.request.udp_port;
self.send_gripper_connect_response(request, &mut tcp_socket_wrapper);
let udp_socket = UdpSocket::bind(
format!("{}:1833", hostname)
.to_socket_addrs()
.unwrap()
.next()
.unwrap(),
)
.unwrap();
udp_socket
.connect(
format!("{}:{}", hostname, udp_port)
.to_socket_addrs()
.unwrap()
.next()
.unwrap(),
)
.unwrap();
let udp_socket_wrapper = Socket {
send_bytes: move |bytes| {
let res = udp_socket.send(bytes.as_slice());
if res.is_err() {
println!("could not send upd bytes");
return;
}
let num_bytes = res.unwrap();
assert_eq!(num_bytes, bytes.len());
},
receive_bytes: |_bytes| unimplemented!(),
};
let udp_thread = std::thread::spawn(move || {
let mut counter = 1;
let start = Instant::now();
while (Instant::now() - start).as_secs_f64() < 0.01 {
let bytes = serialize(&GripperStateIntern {
message_id: counter,
width: 0.0,
max_width: 0.0,
is_grasped: false,
temperature: 0,
})
.unwrap();
counter += 1;
(udp_socket_wrapper.send_bytes)(&bytes);
std::thread::sleep(Duration::from_millis(10));
}
});
let mut wrapper = Box::new(tcp_socket_wrapper);
for _ in 0..reaction.number_of_reactions() {
self.handle_receive(&mut wrapper, reaction);
std::thread::sleep(Duration::from_millis(10));
}
udp_thread.join().unwrap();
}
fn handle_receive<F, G>(
&self,
tcp_socket: &mut Box<Socket<F, G>>,
reaction: &mut MockServerReaction,
) where
F: Fn(&Vec<u8>),
G: Fn(&mut Vec<u8>),
{
let mut bytes = vec![0 as u8; 100];
(tcp_socket.receive_bytes)(&mut bytes);
let response = reaction.process_received_bytes(&mut bytes);
(tcp_socket.send_bytes)(&response);
}
fn receive_gripper_connect_request<F: Fn(&Vec<u8>), G: Fn(&mut Vec<u8>)>(
&self,
tcp_socket: &mut Socket<F, G>,
) -> ConnectRequestWithHeader {
let mut bytes = vec![0 as u8; 100];
(tcp_socket.receive_bytes)(&mut bytes);
let request: ConnectRequestWithHeader = deserialize(bytes.as_slice()).unwrap();
return request;
}
fn send_gripper_connect_response<F: Fn(&Vec<u8>), G: Fn(&mut Vec<u8>)>(
&self,
request: ConnectRequestWithHeader,
tcp_socket: &mut Socket<F, G>,
) {
let mut response = ConnectResponse {
header: CommandHeader {
command: Command::kConnect,
command_id: request.get_command_message_id(),
size: 0,
},
status: match self.server_version == request.request.version {
true => kSuccess,
false => kFail,
},
version: self.server_version,
};
let response_size = serialized_size(&response).unwrap();
response.header.size = response_size as u32;
let serialized_response = serialize(&response).unwrap();
(tcp_socket.send_bytes)(&serialized_response);
}
}
#[test]
fn gripper_move_test() -> FrankaResult<()> {
let mut counter = 0;
let move_request_values = [(0.1, 0.5), (0.3, 0.1)];
let mut generate_move_request = move |width: f64, speed: f64| -> MoveRequestWithHeader {
counter += 1;
MoveRequestWithHeader {
header: CommandHeader::new(
Command::kMove,
counter,
size_of::<MoveRequestWithHeader>() as u32,
),
request: MoveRequest::new(width, speed),
}
};
let requests = Arc::new(Vec::from_iter(
move_request_values
.iter()
.map(|(x, y)| generate_move_request(*x, *y)),
));
let requests_server = requests.clone();
let thread = std::thread::spawn(|| {
let mut mock_gripper = GripperMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
let num_requests = requests_server.len();
let mut counter = 0;
mock.expect_process_received_bytes()
.returning(move |bytes: &mut Vec<u8>| -> Vec<u8> {
let expected_request = requests_server.get(counter).unwrap();
let serialized_expected_request = serialize(expected_request).unwrap();
assert_eq!(bytes.len(), serialized_expected_request.len());
bytes
.iter()
.zip(serialized_expected_request.iter())
.for_each(|(x, y)| assert_eq!(x, y));
let req: MoveRequestWithHeader = deserialize(&bytes).unwrap();
counter += 1;
let mut response = MoveResponse {
header: CommandHeader::new(Command::kMove, req.header.command_id, 0),
status: Status::kSuccess,
};
response.header.size = serialized_size(&response).unwrap() as u32;
serialize(&response).unwrap()
})
.times(num_requests);
mock.expect_number_of_reactions().return_const(num_requests);
mock_gripper.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure");
assert_eq!(gripper.server_version(), kVersion);
for (width, speed) in move_request_values.iter() {
gripper.move_gripper(*width, *speed).unwrap();
}
}
thread.join().unwrap();
Ok(())
}
#[test]
fn gripper_stop_test() -> FrankaResult<()> {
let thread = std::thread::spawn(|| {
let mut mock_gripper = GripperMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
mock.expect_process_received_bytes()
.returning(move |bytes: &mut Vec<u8>| -> Vec<u8> {
let req: StopRequestWithHeader = deserialize(&bytes).unwrap();
match req.command {
Command::kStop => {}
_ => {
assert!(false)
}
}
let mut response = StopResponse {
header: CommandHeader::new(Command::kStop, req.command_id, 0),
status: Status::kSuccess,
};
response.header.size = serialized_size(&response).unwrap() as u32;
serialize(&response).unwrap()
})
.times(1);
mock.expect_number_of_reactions().return_const(1 as usize);
mock_gripper.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure");
gripper.stop().unwrap();
}
thread.join().unwrap();
Ok(())
}
#[test]
fn gripper_homing_test() -> FrankaResult<()> {
let thread = std::thread::spawn(|| {
let mut mock_gripper = GripperMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
mock.expect_process_received_bytes()
.returning(move |bytes: &mut Vec<u8>| -> Vec<u8> {
let req: HomingRequestWithHeader = deserialize(&bytes).unwrap();
match req.command {
Command::kHoming => {}
_ => {
assert!(false)
}
}
let mut response = HomingResponse {
header: CommandHeader::new(Command::kHoming, req.command_id, 0),
status: Status::kSuccess,
};
response.header.size = serialized_size(&response).unwrap() as u32;
serialize(&response).unwrap()
})
.times(1);
mock.expect_number_of_reactions().return_const(1 as usize);
mock_gripper.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure");
gripper.homing().unwrap();
}
thread.join().unwrap();
Ok(())
}
#[test]
fn gripper_grasp_test() -> FrankaResult<()> {
let mut counter = 0;
let grasp_request_values = [(0.05, 0.1, 400., 0.004, 0.005)];
let mut generate_grasp_request = move |width: f64,
speed: f64,
force: f64,
epsilon_inner: f64,
epsilon_outer: f64|
-> GraspRequestWithHeader {
counter += 1;
GraspRequestWithHeader {
header: CommandHeader::new(
Command::kGrasp,
counter,
size_of::<GraspRequestWithHeader>() as u32,
),
request: GraspRequest::new(width, speed, force, epsilon_inner, epsilon_outer),
}
};
let requests =
Arc::new(Vec::from_iter(grasp_request_values.iter().map(
|(a, b, c, d, e)| generate_grasp_request(*a, *b, *c, *d, *e),
)));
let requests_server = requests.clone();
let thread = std::thread::spawn(|| {
let mut mock_gripper = GripperMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
let num_requests = requests_server.len();
let mut counter = 0;
mock.expect_process_received_bytes()
.returning(move |bytes: &mut Vec<u8>| -> Vec<u8> {
let expected_request = requests_server.get(counter).unwrap();
let serialized_expected_request = serialize(expected_request).unwrap();
assert_eq!(bytes.len(), serialized_expected_request.len());
bytes
.iter()
.zip(serialized_expected_request.iter())
.for_each(|(x, y)| assert_eq!(x, y));
let req: MoveRequestWithHeader = deserialize(&bytes).unwrap();
counter += 1;
let mut response = GraspResponse {
header: CommandHeader::new(Command::kGrasp, req.header.command_id, 0),
status: Status::kSuccess,
};
response.header.size = serialized_size(&response).unwrap() as u32;
serialize(&response).unwrap()
})
.times(num_requests);
mock.expect_number_of_reactions().return_const(num_requests);
mock_gripper.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure");
for (width, speed, force, epsilon_inner, epsilon_outer) in grasp_request_values.iter() {
gripper
.grasp(
*width,
*speed,
*force,
Some(*epsilon_inner),
Some(*epsilon_outer),
)
.unwrap();
}
}
thread.join().unwrap();
Ok(())
}
#[test]
fn incompatible_library() {
let thread = std::thread::spawn(|| {
let mut mock_gripper = GripperMockServer::new(kVersion + 1);
let mut mock = MockServerReaction::default();
mock.expect_process_received_bytes()
.returning(|_bytes| Vec::<u8>::new());
mock.expect_number_of_reactions().return_const(0 as usize);
mock_gripper.server_thread(&mut mock);
});
let gripper_result;
{
std::thread::sleep(Duration::from_secs_f64(0.01));
gripper_result = Gripper::new("127.0.0.1")
}
thread.join().unwrap();
match gripper_result {
Ok(_) => {
panic!("Expected incompatible library version")
}
Err(error) => match error {
FrankaException::IncompatibleLibraryVersionError { .. } => {}
e => {
panic!("Expected incompatible library version but found {:?}", e)
}
},
};
}
#[test]
fn gripper_read_once() {
let thread = std::thread::spawn(|| {
let mut server = GripperMockServer::new(kVersion);
let mut mock = MockServerReaction::default();
mock.expect_process_received_bytes()
.returning(|_bytes| Vec::<u8>::new());
mock.expect_number_of_reactions().return_const(0 as usize);
server.server_thread(&mut mock);
});
{
std::thread::sleep(Duration::from_secs_f64(0.01));
let mut gripper = Gripper::new("127.0.0.1").expect("gripper failure");
let _state = gripper.read_once().expect("could not read gripper state");
}
thread.join().unwrap();
}
}