pflex_module_rs/
tcs_client.rs

1use log::{debug, info};
2use std::fmt;
3use std::io::{Read, Write};
4use std::net::{Shutdown, TcpStream};
5use std::time::Duration;
6
7use crate::error_codes::{ResponseCodes, RobotError};
8
9/// Commands enumerator for the robot API
10#[derive(Debug, PartialEq, Clone)]
11pub enum TCSCommand {
12    Mode,
13    Exit,
14    Power,
15    Select,
16    Attach,
17    Home,
18    Halt,
19    Loc,
20    LocXyz,
21    Profile,
22    Move,
23    MoveToCart,
24    MoveToJoints,
25    MotionState,
26    MoveOneAxis,
27    MoveRail,
28    GetParam,
29    GetLocJoints,
30    GetLocCart,
31    FreeMode,
32    NoOp,
33    SystemSpeed,
34    Payload,
35    WaitForEOM,
36}
37
38impl fmt::Display for TCSCommand {
39    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
40        let s = match self {
41            TCSCommand::Mode => "mode",
42            TCSCommand::Exit => "exit",
43            TCSCommand::Power => "hp",
44            TCSCommand::Select => "selectRobot",
45            TCSCommand::Attach => "attach",
46            TCSCommand::Home => "home",
47            TCSCommand::Halt => "halt",
48            TCSCommand::Loc => "loc",
49            TCSCommand::LocXyz => "locXYZ",
50            TCSCommand::Profile => "profile",
51            TCSCommand::Move => "move",
52            TCSCommand::MoveToCart => "movec",
53            TCSCommand::MoveToJoints => "movej",
54            TCSCommand::MotionState => "state",
55            TCSCommand::MoveOneAxis => "moveoneaxis",
56            TCSCommand::MoveRail => "moveRail",
57            TCSCommand::GetParam => "pd",
58            TCSCommand::GetLocJoints => "wherej",
59            TCSCommand::GetLocCart => "wherec",
60            TCSCommand::FreeMode => "freemode",
61            TCSCommand::NoOp => "nop",
62            TCSCommand::SystemSpeed => "mspeed",
63            TCSCommand::Payload => "payload",
64            TCSCommand::WaitForEOM => "waitForEOM",
65        };
66        write!(f, "{}", s)
67    }
68}
69
70#[derive(Debug)]
71pub struct TCSClient {
72    pub socket: Option<TcpStream>,
73}
74
75/// Creates a new TCSClient instance without an active socket
76impl TCSClient {
77    pub(crate) const DEFAULT_TIMEOUT: f64 = 5.0;
78    const REQUEST_SEPARATOR: &'static str = "\n";
79    const RESPONSE_SEPARATOR: &'static str = "\r\n";
80    const SPACEBAR_SEPERATOR: &'static str = " ";
81    const TCS_SERVER_PORT: u16 = 10100;
82
83    pub fn new() -> TCSClient {
84        TCSClient { socket: None }
85    }
86
87    /// Attempts to connect to the specified robot
88    /// # Arguments
89    /// * `ip` - IP address of the robot
90    /// * `timeout` - Optional timeout setting for all socket read/write attempts
91    pub fn connect(&mut self, ip: &str, timeout: Option<f64>) -> Result<(), std::io::Error> {
92        let timeout = timeout.unwrap_or(TCSClient::DEFAULT_TIMEOUT);
93        let addr = format!("{}:{}", ip, Self::TCS_SERVER_PORT);
94        info!("tcs_client::connect called");
95        let conn_attempt = TcpStream::connect(addr.clone());
96        match conn_attempt {
97            Ok(stream) => {
98                stream.set_read_timeout(Some(Duration::from_secs_f64(timeout)))?;
99                stream.set_write_timeout(Some(Duration::from_secs_f64(timeout)))?;
100                self.socket = Some(stream);
101                debug!("connected to client");
102                Ok(())
103            }
104            Err(e) => {
105                debug!("failed to connect to client");
106                Err(e)
107            }
108        }
109    }
110
111    /// Generates and sends the command payload to the robot
112    /// # Arguments
113    /// * `command` - Selected command to run from the TCSCommand enum
114    /// * `command_args` - Optional command arguments
115    /// * `wait_for_response` - A boolean option should any commands not require waiting for a response
116    /// * `read_timeout` - Optional argument to set the read timeout on the socket
117    pub fn send_command<'a>(
118        &mut self,
119        command: TCSCommand,
120        command_args: Option<Vec<&str>>,
121        wait_for_response: bool,
122        read_timeout: Option<f64>,
123    ) -> Result<Vec<String>, RobotError> {
124        info!("tcs_client::send_command called");
125        if read_timeout.is_some() {
126            self.socket
127                .as_ref()
128                .unwrap()
129                .set_read_timeout(Some(Duration::from_secs_f64(read_timeout.unwrap())))
130                .expect("Failed to set read timeout");
131        }
132        let payload: String;
133
134        // build the additional arguments if they exist
135        if command_args.is_some() {
136            let payload_slice = command_args.unwrap().join(TCSClient::SPACEBAR_SEPERATOR);
137            payload = format!(
138                "{}{}{}{}",
139                command,
140                TCSClient::SPACEBAR_SEPERATOR,
141                payload_slice,
142                TCSClient::REQUEST_SEPARATOR
143            );
144        // build the command without additional arguments
145        } else {
146            payload = format!("{}{}", command, TCSClient::REQUEST_SEPARATOR);
147        }
148        debug!("tcs_client::send_command payload: {}", payload);
149        // send the command
150        self.socket
151            .as_ref()
152            .unwrap()
153            .write_all(payload.as_bytes())
154            .expect("Failed to write message");
155
156        // read the response (if needed)
157        if wait_for_response {
158            let response = self.get_response();
159            match response {
160                Ok(r) => {
161                    let if_error_code = ResponseCodes::check_code(r[0].to_owned());
162                    match if_error_code {
163                        Ok(_) => Ok(r[1..].to_vec()),
164                        Err(e) => Err(e),
165                    }
166                }
167                Err(e) => Err(e.to_string()),
168            }
169        } else {
170            Ok(vec![])
171        }
172    }
173
174    fn get_response<'a>(&mut self) -> Result<Vec<String>, std::io::Error> {
175        info!("tcs_client::get_response called");
176        let read_buffer = &mut [0; 1024];
177        let mut response: Vec<u8> = Vec::new();
178        // todo: remove unwrap
179        let read_attempt = self.socket.as_ref().unwrap().read(read_buffer);
180        match read_attempt {
181            Ok(bytes_read) => {
182                response.extend_from_slice(&read_buffer[..bytes_read]);
183            }
184            Err(e) => {
185                return Err(e);
186            }
187        }
188        debug!("tcs_client::get_response payload: {:#?}", read_attempt);
189        // todo: make this nicer
190        let response_str = std::str::from_utf8(&response).unwrap();
191        let response_parts = response_str
192            .split(TCSClient::SPACEBAR_SEPERATOR)
193            .collect::<Vec<&str>>();
194        let response_parts = response_parts
195            .iter()
196            .map(|part| part.trim_end_matches(TCSClient::RESPONSE_SEPARATOR))
197            .collect::<Vec<&str>>();
198        let collated_response = response_parts
199            .iter()
200            .map(|part| part.to_string())
201            .collect::<Vec<String>>();
202        debug!(
203            "tcs_client::get_response return value: {:#?}",
204            collated_response
205        );
206        Ok(collated_response)
207    }
208
209    /// Closes the socket only
210    pub fn disconnect(&mut self) -> Result<(), std::io::Error> {
211        // this only closes the socket, it doesn't tell the robot that you're disconnecting
212        // that'll need to be done by calling the exit command
213        info!("tcs_client::disconnect called");
214        if self.socket.is_some() {
215            self.socket
216                .as_ref()
217                .unwrap()
218                .shutdown(Shutdown::Both)
219                .expect("Shutdown failed so I'm failing I guess(?)");
220            self.socket = None;
221            Ok(())
222        } else {
223            Err(std::io::Error::new(
224                std::io::ErrorKind::NotConnected,
225                "Not connected to a TCS",
226            ))
227        }
228    }
229}
pflex_module_rs/tcs_client.rs

pflex_module_rs/
tcs_client.rs
