diff --git a/Cargo.lock b/Cargo.lock index ced71c737..912ec5ebb 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2341,6 +2341,20 @@ dependencies = [ "eyre", ] +[[package]] +name = "dora-chongyoucar" +version = "0.3.7" +dependencies = [ + "dora-node-api", + "dotenv", + "eyre", + "serde", + "serde_json", + "serial", + "thiserror", + "tokio", +] + [[package]] name = "dora-cli" version = "0.3.7" @@ -2778,6 +2792,12 @@ dependencies = [ "tracing-subscriber", ] +[[package]] +name = "dotenv" +version = "0.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77c90badedccf4105eca100756a0b1289e191f6fcbdadd3cee1d2f614f97da8f" + [[package]] name = "downcast-rs" version = "1.2.1" @@ -4502,6 +4522,15 @@ version = "2.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5a611371471e98973dbcab4e0ec66c31a10bc356eeb4d54a0e05eac8158fe38c" +[[package]] +name = "ioctl-rs" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f7970510895cee30b3e9128319f2cefd4bde883a39f38baa279567ba3a7eb97d" +dependencies = [ + "libc", +] + [[package]] name = "iovec" version = "0.1.4" @@ -5684,7 +5713,7 @@ version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "681030a937600a36906c185595136d26abfebb4aa9c65701cefcaf8578bb982b" dependencies = [ - "proc-macro-crate 1.3.1", + "proc-macro-crate 3.2.0", "proc-macro2", "quote", "syn 2.0.86", @@ -9441,6 +9470,48 @@ dependencies = [ "unsafe-libyaml", ] +[[package]] +name = "serial" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1237a96570fc377c13baa1b88c7589ab66edced652e43ffb17088f003db3e86" +dependencies = [ + "serial-core", + "serial-unix", + "serial-windows", +] + +[[package]] +name = "serial-core" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f46209b345401737ae2125fe5b19a77acce90cd53e1658cda928e4fe9a64581" +dependencies = [ + "libc", +] + +[[package]] +name = "serial-unix" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f03fbca4c9d866e24a459cbca71283f545a37f8e3e002ad8c70593871453cab7" +dependencies = [ + "ioctl-rs", + "libc", + "serial-core", + "termios", +] + +[[package]] +name = "serial-windows" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "15c6d3b776267a75d31bbdfd5d36c0ca051251caafc285827052bc53bcdc8162" +dependencies = [ + "libc", + "serial-core", +] + [[package]] name = "sha1" version = "0.10.6" @@ -9837,7 +9908,7 @@ version = "36.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "43cf89a0cc9131279235baf8599b0e073fbcb096419204de0cc5d1a48ae73f74" dependencies = [ - "proc-macro-crate 1.3.1", + "proc-macro-crate 3.2.0", "proc-macro2", "quote", "rand", @@ -10073,6 +10144,15 @@ dependencies = [ "windows-sys 0.59.0", ] +[[package]] +name = "termios" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d5d9cf598a6d7ce700a4e6a9199da127e6819a61e64b68609683cc9a01b5683a" +dependencies = [ + "libc", +] + [[package]] name = "textwrap" version = "0.16.1" diff --git a/Cargo.toml b/Cargo.toml index e6d406e01..bbfac7ad5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -34,6 +34,7 @@ members = [ "node-hub/dora-rerun", "node-hub/terminal-print", "node-hub/openai-proxy-server", + "node-hub/dora-chongyoucar", "libraries/extensions/ros2-bridge", "libraries/extensions/ros2-bridge/msg-gen", "libraries/extensions/ros2-bridge/python", diff --git a/node-hub/dora-chongyoucar/.env_example b/node-hub/dora-chongyoucar/.env_example new file mode 100644 index 000000000..11638412b --- /dev/null +++ b/node-hub/dora-chongyoucar/.env_example @@ -0,0 +1,2 @@ +# Defining the serial port number +SERIAL_PORT = "/dev/ttyUSB0" diff --git a/node-hub/dora-chongyoucar/Cargo.toml b/node-hub/dora-chongyoucar/Cargo.toml new file mode 100644 index 000000000..9f8d66330 --- /dev/null +++ b/node-hub/dora-chongyoucar/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "dora-chongyoucar" +edition = "2021" +version.workspace = true +description.workspace = true +documentation.workspace = true +license.workspace = true +repository.workspace = true + +[dependencies] +eyre = "0.6.8" +dora-node-api = { workspace = true, features = ["tracing"] } +dotenv = "0.15.0" +serde = { version = "1.0.204", features = ["derive"] } +serde_json = "1.0.120" +serial = "0.4.0" +thiserror = "1.0.63" +tokio = { version = "1.24.2", features = ["rt", "macros", "rt-multi-thread"] } diff --git a/node-hub/dora-chongyoucar/README.md b/node-hub/dora-chongyoucar/README.md new file mode 100644 index 000000000..959661a65 --- /dev/null +++ b/node-hub/dora-chongyoucar/README.md @@ -0,0 +1,15 @@ +# Chongyou Car Control + +## Introduce + +Control of the movement of the trolley by receiving texts + +## Text Command Description + +|`text`|`description`| +|---|---| +|`forward`|Control the trolley to move forward| +|`left`|Control the trolley to move left| +|`right`|Control the trolley to move right| +|`backward`|Control the trolley to move backward| +|`stop`|Control the trolley to move stop| diff --git a/node-hub/dora-chongyoucar/src/command.rs b/node-hub/dora-chongyoucar/src/command.rs new file mode 100644 index 000000000..d5c5c930c --- /dev/null +++ b/node-hub/dora-chongyoucar/src/command.rs @@ -0,0 +1,35 @@ +// 差速小车 +pub fn send_speed_to_x4chassis(x: f64, y: f64, w: f64) -> Vec { + let mut data = vec![]; + + let speed_offset = 10.0; // 速度偏移值 10m/s,把速度转换成正数发送 + + data.push(0xAE_u8); + data.push(0xEA); + data.push(0x0B); + data.push(0xF3); + let x = ((x + speed_offset) * 100.0) as u16; + data.push((x >> 8) as u8); + data.push(x as u8); + data.push(0x00); + data.push(0x00); + let w = ((w + speed_offset) * 100.0) as u16; + data.push((w >> 8) as u8); + data.push(w as u8); + data.push(0x00); + data.push(0x00); + let len = data.len(); + data[2] = len as u8 - 1; + + let mut count = 0; + for &d in data.iter().take(len).skip(2) { + count += d as u16; + } + // 数据校验位 + data.push(count as u8); + + data.push(0xEF); + data.push(0xFE); + + data +} diff --git a/node-hub/dora-chongyoucar/src/config.rs b/node-hub/dora-chongyoucar/src/config.rs new file mode 100644 index 000000000..f09a81d59 --- /dev/null +++ b/node-hub/dora-chongyoucar/src/config.rs @@ -0,0 +1,2 @@ +/// serial port +pub const SERIAL_PORT: &str = "SERIAL_PORT"; diff --git a/node-hub/dora-chongyoucar/src/enums.rs b/node-hub/dora-chongyoucar/src/enums.rs new file mode 100644 index 000000000..fa1b5f67f --- /dev/null +++ b/node-hub/dora-chongyoucar/src/enums.rs @@ -0,0 +1,8 @@ +use serde::{Deserialize, Serialize}; + +// command type +#[derive(Debug, Serialize, Deserialize)] +#[serde(tag = "command_type")] +pub enum CommandType { + DifferSpeed { x: f64, y: f64, w: f64 }, // Differential Speed +} diff --git a/node-hub/dora-chongyoucar/src/error.rs b/node-hub/dora-chongyoucar/src/error.rs new file mode 100644 index 000000000..44564534a --- /dev/null +++ b/node-hub/dora-chongyoucar/src/error.rs @@ -0,0 +1,11 @@ +use thiserror::Error; + +#[derive(Debug, Error)] +pub enum Error { + #[error("serial connect fail")] + SerialConnect, + #[error("serial settings set fail")] + SerialSettingsSet, + #[error("serial set timeout fail")] + SerialSetTimeout, +} diff --git a/node-hub/dora-chongyoucar/src/json_data.rs b/node-hub/dora-chongyoucar/src/json_data.rs new file mode 100644 index 000000000..917aba4d2 --- /dev/null +++ b/node-hub/dora-chongyoucar/src/json_data.rs @@ -0,0 +1,9 @@ +use serde::{Deserialize, Serialize}; + +use crate::enums::CommandType; + +#[derive(Debug, Serialize, Deserialize)] +pub struct JsonData { + pub sleep_second: u64, + pub command: CommandType, +} diff --git a/node-hub/dora-chongyoucar/src/main.rs b/node-hub/dora-chongyoucar/src/main.rs new file mode 100644 index 000000000..3f239d9d9 --- /dev/null +++ b/node-hub/dora-chongyoucar/src/main.rs @@ -0,0 +1,88 @@ +// Chongyou Car Control +// Author:Leon(李扬) + +mod command; +mod config; +mod enums; +mod error; +mod json_data; + +use std::{io::Write, time::Duration}; + +use dora_node_api::{DoraNode, Event}; +use error::Error; +use serial::SerialPort; +use tokio::sync::mpsc; + +#[tokio::main] +async fn main() -> eyre::Result<()> { + dotenv::dotenv().ok(); + + // serial port + let serial_port = std::env::var(config::SERIAL_PORT).unwrap_or("/dev/ttyUSB0".to_string()); + let speed = std::env::var("SPEED") + .unwrap_or("0.2".to_string()) + .parse::() + .unwrap_or(0.2_f64); + + // connect serial + const COM_SETTINGS: serial::PortSettings = serial::PortSettings { + baud_rate: serial::Baud115200, + char_size: serial::Bits8, + parity: serial::ParityNone, + stop_bits: serial::Stop1, + flow_control: serial::FlowNone, + }; + + let mut com = serial::open(&serial_port).map_err(|_| Error::SerialConnect)?; + com.configure(&COM_SETTINGS) + .map_err(|_| Error::SerialSettingsSet)?; + com.set_timeout(Duration::from_millis(1000)) + .map_err(|_| Error::SerialSetTimeout)?; + + // msg channel + let (tx_key, mut rx_key) = mpsc::channel::<(f64, f64)>(100); + + tokio::spawn(async move { + while let Some((x, w)) = rx_key.recv().await { + // println!("{:?}", (x, w)); + let data = command::send_speed_to_x4chassis(x, 0.0, w); + com.write_all(&data).ok(); + } + }); + + let r = 1.0; + + let (_, mut events) = DoraNode::init_from_env()?; + + while let Some(event) = events.recv() { + if let Event::Input { + id: _, + metadata: _, + data, + } = event + { + let received_string: &str = TryFrom::try_from(&data).unwrap(); + match received_string { + "forward" => { + tx_key.send((speed * r, 0.0)).await.ok(); + } + "left" => { + tx_key.send((0.0, speed * r)).await.ok(); + } + "right" => { + tx_key.send((0.0, -speed * r)).await.ok(); + } + "backward" => { + tx_key.send((-speed * r, 0.0)).await.ok(); + } + "stop" => { + tx_key.send((0.0, 0.0)).await.ok(); + } + _ => {} + } + } + } + + Ok(()) +}