feat: Add Chongyoucar Control in node-hub

Added a control node for chongyoucar to receive text commands, 'forward' 'left' 'right' 'backward' and 'stop' to control the car's forward turning and backward stopping etc.

Author: Leon <[email protected]>

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",

node-hub/dora-chongyoucar/.env_example

@@ -0,0 +1,2 @@
+# Defining the serial port number
+SERIAL_PORT = "/dev/ttyUSB0"

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"] }

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|

node-hub/dora-chongyoucar/src/command.rs

@@ -0,0 +1,35 @@
+// 差速小车
+pub fn send_speed_to_x4chassis(x: f64, y: f64, w: f64) -> Vec<u8> {
+    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
+}

node-hub/dora-chongyoucar/src/config.rs

@@ -0,0 +1,2 @@
+/// serial port
+pub const SERIAL_PORT: &str = "SERIAL_PORT";

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
+}

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,
+}

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,
+}

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::<f64>()
+        .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(())
+}