feat: Add Dora-kit car Control in node-hub (#715)

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-kit-car",
     "libraries/extensions/ros2-bridge",
     "libraries/extensions/ros2-bridge/msg-gen",
     "libraries/extensions/ros2-bridge/python",

node-hub/dora-kit-car/.env_example

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

node-hub/dora-kit-car/Cargo.toml

@@ -0,0 +1,30 @@
+[package]
+name = "dora-kit-car"
+edition = "2021"
+version.workspace = true
+description.workspace = true
+documentation.workspace = true
+license.workspace = true
+repository.workspace = true
+
+[lib]
+name = "dora_kit_car"
+path = "src/lib.rs"
+crate-type = ["lib", "cdylib"]
+
+[features]
+default = []
+python = ["pyo3"]
+
+[dependencies]
+dora-node-api = { workspace = true, features = ["tracing"] }
+dotenv = "0.15.0"
+eyre = "0.6.8"
+pyo3 = { workspace = true, features = [
+    "extension-module",
+    "abi3",
+], optional = true }
+serde = { version = "1.0.204", features = ["derive"] }
+serde_json = "1.0.120"
+serial = "0.4.0"
+thiserror = "1.0.63"

node-hub/dora-kit-car/README.md

@@ -0,0 +1,19 @@
+# Chongyou Car Control
+
+## Introduce
+
+Control of the movement of the trolley by receiving texts
+
+## Usage
+
+Accepts an array of six f64's
+
+- six f64 array [x, y, z, rx, ry, rz] only used x, rz
+
+see [https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html)
+
+## Environment
+
+The default serial port is `/dev/ttyUSB0`
+
+Added definition of default serial port number. Can additionally define the environment variable `SERIAL_PORT`

node-hub/dora-kit-car/pyproject.toml

@@ -0,0 +1,13 @@
+[build-system]
+requires = ["maturin>=0.13.2"]
+build-backend = "maturin"
+
+[project]
+name = "dora-kit-car"
+version = "0.3.7"
+authors = [{ name = "Leon", email = "[email protected]" }]
+description = "Dora Node for dora kit car"
+readme = "README.md"
+
+[tool.maturin]
+features = ["python", "pyo3/extension-module"]

node-hub/dora-kit-car/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-kit-car/src/config.rs

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

node-hub/dora-kit-car/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-kit-car/src/error.rs

@@ -0,0 +1,11 @@
+use thiserror::Error;
+
+#[derive(Debug, Error)]
+pub enum Error {
+    #[error("serial: {0} connect fail")]
+    Connect(String),
+    #[error("serial settings: {0} set fail")]
+    SettingsSet(String),
+    #[error("serial set timeout: {0} fail")]
+    SetTimeout(String),
+}

node-hub/dora-kit-car/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-kit-car/src/lib.rs

@@ -0,0 +1,94 @@
+// Chongyou Car Control
+// Author：Leon <[email protected]>
+
+mod command;
+mod config;
+mod enums;
+mod error;
+mod json_data;
+
+use std::{env, io::Write, time::Duration};
+
+use dora_node_api::{arrow::array::Float64Array, DoraNode, Event};
+use error::Error;
+use eyre::Context;
+use serial::SerialPort;
+use std::sync::mpsc;
+
+pub fn lib_main() -> eyre::Result<()> {
+    dotenv::dotenv().ok();
+
+    // serial port
+    let serial_port = env::var(config::SERIAL_PORT).unwrap_or("/dev/ttyUSB0".to_string());
+
+    // 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).wrap_err(Error::Connect(serial_port))?;
+    com.configure(&COM_SETTINGS)
+        .wrap_err(Error::SettingsSet(format!("{:?}", COM_SETTINGS)))?;
+    com.set_timeout(Duration::from_millis(1000))
+        .wrap_err(Error::SetTimeout("1000ms".to_string()))?;
+
+    // msg channel
+    let (tx_key, rx_key) = mpsc::channel::<(f64, f64)>();
+
+    std::thread::spawn(move || {
+        while let Ok((x, w)) = rx_key.recv() {
+            let data = command::send_speed_to_x4chassis(x, 0.0, w);
+            com.write_all(&data).ok();
+        }
+    });
+
+    let (_, mut events) = DoraNode::init_from_env()?;
+
+    while let Some(event) = events.recv() {
+        if let Event::Input {
+            id: _,
+            metadata: _,
+            data,
+        } = event
+        {
+            if let Some(cmd) = data.as_any().downcast_ref::<Float64Array>() {
+                let data = cmd.values();
+                if data.len() == 6 {
+                    // https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Twist.html
+                    // [x, y, z, rx, ry, rz]
+                    let x = data[0];
+                    let w = data[5];
+                    tx_key.send((x, w)).ok();
+                }
+            }
+        }
+    }
+
+    Ok(())
+}
+
+#[cfg(feature = "python")]
+use pyo3::{
+    pyfunction, pymodule,
+    types::{PyModule, PyModuleMethods},
+    wrap_pyfunction, Bound, PyResult, Python,
+};
+
+#[cfg(feature = "python")]
+#[pyfunction]
+fn py_main(_py: Python) -> PyResult<()> {
+    pyo3::prepare_freethreaded_python();
+
+    lib_main().map_err(|err| pyo3::exceptions::PyException::new_err(err.to_string()))
+}
+
+#[cfg(feature = "python")]
+#[pymodule]
+fn dora_kit_car(_py: Python, m: Bound<'_, PyModule>) -> PyResult<()> {
+    m.add_function(wrap_pyfunction!(py_main, &m)?)?;
+    Ok(())
+}

node-hub/dora-kit-car/src/main.rs

@@ -0,0 +1,6 @@
+// Chongyou Car Control
+// Author：Leon <[email protected]>
+
+fn main() -> eyre::Result<()> {
+    dora_kit_car::lib_main()
+}