Rust for Robotics in 2025: Libraries, Tools, and Best Practices

// Using embedded-hal for hardware abstraction
// Cargo.toml:
// [dependencies]
// embedded-hal = "1.0"
// stm32f4xx-hal = { version = "0.15", features = ["stm32f411"] }
// cortex-m = "0.7"
// cortex-m-rt = "0.7"
// panic-halt = "0.2"

#![no_std]
#![no_main]

use cortex_m_rt::entry;
use embedded_hal::digital::v2::{OutputPin, InputPin};
use embedded_hal::PwmPin;
use panic_halt as _;
use stm32f4xx_hal::{
    gpio::{Edge, Input, Output, Pin, PullUp},
    pac,
    prelude::*,
    pwm::{Channel, Pwm},
    timer::Timer,
};

// Motor control structure
struct Motor<PWM, DIR1, DIR2> {
    pwm: PWM,
    dir1: DIR1,
    dir2: DIR2,
}

impl<PWM, DIR1, DIR2> Motor<PWM, DIR1, DIR2>
where
    PWM: PwmPin,
    DIR1: OutputPin,
    DIR2: OutputPin,
{
    fn new(mut pwm: PWM, dir1: DIR1, dir2: DIR2) -> Self {
        // Initialize with motor stopped
        pwm.disable();
        Motor { pwm, dir1, dir2 }
    }
    
    fn set_speed(&mut self, speed: i16) -> Result<(), ()> {
        let max_duty = self.pwm.get_max_duty();
        
        if speed > 0 {
            // Forward direction
            self.dir1.set_high().map_err(|_| ())?;
            self.dir2.set_low().map_err(|_| ())?;
            
            let duty = ((speed as u32) * max_duty as u32 / 100) as u16;
            self.pwm.set_duty(duty.min(max_duty));
            self.pwm.enable();
        } else if speed < 0 {
            // Reverse direction
            self.dir1.set_low().map_err(|_| ())?;
            self.dir2.set_high().map_err(|_| ())?;
            
            let duty = ((-speed as u32) * max_duty as u32 / 100) as u16;
            self.pwm.set_duty(duty.min(max_duty));
            self.pwm.enable();
        } else {
            // Stop
            self.dir1.set_low().map_err(|_| ())?;
            self.dir2.set_low().map_err(|_| ())?;
            self.pwm.set_duty(0);
            self.pwm.disable();
        }
        
        Ok(())
    }
    
    fn brake(&mut self) -> Result<(), ()> {
        // Brake by setting both direction pins high
        self.dir1.set_high().map_err(|_| ())?;
        self.dir2.set_high().map_err(|_| ())?;
        self.pwm.set_duty(self.pwm.get_max_duty());
        self.pwm.enable();
        Ok(())
    }
}

// Encoder structure for tracking motor position
struct Encoder<A, B> {
    pin_a: A,
    pin_b: B,
    count: i32,
    last_a_state: bool,
}

impl<A, B> Encoder<A, B>
where
    A: InputPin,
    B: InputPin,
{
    fn new(pin_a: A, pin_b: B) -> Self {
        let last_a_state = pin_a.is_high().unwrap_or(false);
        Encoder {
            pin_a,
            pin_b,
            count: 0,
            last_a_state,
        }
    }
    
    fn update(&mut self) -> Result<(), ()> {
        let a_state = self.pin_a.is_high().map_err(|_| ())?;
        let b_state = self.pin_b.is_high().map_err(|_| ())?;
        
        // Detect rising edge on A
        if a_state && !self.last_a_state {
            // If B is high while A is rising, count up; otherwise count down
            if b_state {
                self.count += 1;
            } else {
                self.count -= 1;
            }
        }
        
        self.last_a_state = a_state;
        Ok(())
    }
    
    fn get_count(&self) -> i32 {
        self.count
    }
    
    fn reset(&mut self) {
        self.count = 0;
    }
}

#[entry]
fn main() -> ! {
    // Get access to device peripherals
    let dp = pac::Peripherals::take().unwrap();
    let cp = cortex_m::Peripherals::take().unwrap();
    
    // Set up clocks
    let rcc = dp.RCC.constrain();
    let clocks = rcc.cfgr.sysclk(48.MHz()).freeze();
    
    // Set up GPIO
    let gpioa = dp.GPIOA.split();
    let gpiob = dp.GPIOB.split();
    
    // Configure PWM for motor control
    let tim2 = Timer::tim2(dp.TIM2, 20.kHz(), &clocks);
    let pwm = tim2.pwm_hz(
        gpioa.pa0.into_alternate(),
        20.kHz(),
    );
    
    // Configure motor direction pins
    let dir1 = gpioa.pa1.into_push_pull_output();
    let dir2 = gpioa.pa2.into_push_pull_output();
    
    // Create motor controller
    let mut motor = Motor::new(pwm.into_ch1(), dir1, dir2);
    
    // Configure encoder pins
    let enc_a = gpiob.pb0.into_pull_up_input();
    let enc_b = gpiob.pb1.into_pull_up_input();
    
    // Create encoder
    let mut encoder = Encoder::new(enc_a, enc_b);
    
    // Configure limit switch
    let mut limit_switch = gpiob.pb2.into_pull_up_input();
    
    // Main control loop
    let mut position_target = 1000;
    let mut current_speed = 0;
    
    loop {
        // Update encoder position
        let _ = encoder.update();
        let current_position = encoder.get_count();
        
        // Check limit switch
        if limit_switch.is_low().unwrap_or(false) {
            // Emergency stop if limit switch is triggered
            let _ = motor.brake();
            encoder.reset();
            position_target = 0;
            current_speed = 0;
            continue;
        }
        
        // Simple position control
        let position_error = position_target - current_position;
        
        // Proportional control
        let kp = 5;
        let mut new_speed = (position_error * kp / 100).clamp(-100, 100) as i16;
        
        // Apply speed changes gradually
        if new_speed > current_speed {
            current_speed = (current_speed + 5).min(new_speed);
        } else if new_speed < current_speed {
            current_speed = (current_speed - 5).max(new_speed);
        }
        
        // Set motor speed
        let _ = motor.set_speed(current_speed);
        
        // Delay for control loop timing
        cortex_m::asm::delay(48_000_000 / 1000); // ~1ms delay at 48MHz
    }
}

// Serial servo control
// Cargo.toml:
// [dependencies]
// serialport = "4.2"
// anyhow = "1.0"

use serialport::SerialPort;
use std::time::Duration;

// Dynamixel servo protocol implementation
struct DynamixelServo {
    port: Box<dyn SerialPort>,
    id: u8,
}

impl DynamixelServo {
    fn new(port: Box<dyn SerialPort>, id: u8) -> Self {
        DynamixelServo { port, id }
    }
    
    fn write_position(&mut self, position: u16) -> anyhow::Result<()> {
        // Dynamixel Protocol 2.0 packet structure
        let instruction = 0x03; // WRITE
        let address = 0x74; // Goal Position address for X series
        let length = 4; // 4 bytes for position (32-bit)
        
        // Packet: Header(3) + Reserved(1) + ID(1) + Length(2) + Instruction(1) + Address(2) + Data(4) + CRC(2)
        let mut packet = vec![
            0xFF, 0xFF, 0xFD, 0x00, // Header and reserved
            self.id, // ID
            (length + 5) as u8, 0x00, // Length (low, high)
            instruction, // Instruction
            address as u8, (address >> 8) as u8, // Address (low, high)
        ];
        
        // Add position data (little-endian)
        packet.push((position & 0xFF) as u8);
        packet.push(((position >> 8) & 0xFF) as u8);
        packet.push(0x00); // High bytes for 32-bit position
        packet.push(0x00);
        
        // Send packet
        self.port.write_all(&packet)?;
        
        // Wait for response
        std::thread::sleep(Duration::from_millis(10));
        
        Ok(())
    }
    
    fn read_position(&mut self) -> anyhow::Result<u16> {
        // Dynamixel Protocol 2.0 packet structure
        let instruction = 0x02; // READ
        let address = 0x84; // Present Position address for X series
        let length = 4; // 4 bytes for position (32-bit)
        
        // Packet: Header(3) + Reserved(1) + ID(1) + Length(2) + Instruction(1) + Address(2) + Length(2) + CRC(2)
        let mut packet = vec![
            0xFF, 0xFF, 0xFD, 0x00, // Header and reserved
            self.id, // ID
            0x07, 0x00, // Length (low, high)
            instruction, // Instruction
            address as u8, (address >> 8) as u8, // Address (low, high)
            length as u8, 0x00, // Data length (low, high)
        ];
        
        // Send packet
        self.port.write_all(&packet)?;
        
        // Wait for response
        std::thread::sleep(Duration::from_millis(10));
        
        // Read response
        let mut response = [0u8; 15]; // Response size for 4-byte data
        self.port.read_exact(&mut response)?;
        
        // Extract position (first 2 bytes of the 4-byte position)
        let position = (response[9] as u16) | ((response[10] as u16) << 8);
        
        Ok(position)
    }
}

fn main() -> anyhow::Result<()> {
    // Open serial port
    let port = serialport::new("/dev/ttyUSB0", 1_000_000)
        .timeout(Duration::from_millis(10))
        .open()?;
    
    // Create servo controller
    let mut servo = DynamixelServo::new(port, 1);
    
    // Read current position
    let current_position = servo.read_position()?;
    println!("Current position: {}", current_position);
    
    // Move to position 2000
    println!("Moving to position 2000...");
    servo.write_position(2000)?;
    
    // Wait for movement to complete
    std::thread::sleep(Duration::from_secs(1));
    
    // Read new position
    let new_position = servo.read_position()?;
    println!("New position: {}", new_position);
    
    Ok(())
}

// Robot kinematics
// Cargo.toml:
// [dependencies]
// nalgebra = "0.32"

use nalgebra::{Matrix3, Matrix4, Vector3};
use std::f64::consts::PI;

// DH parameters for a 3-DOF robot arm
struct DHParameters {
    theta: f64, // Joint angle (radians)
    d: f64,     // Link offset (m)
    a: f64,     // Link length (m)
    alpha: f64, // Link twist (radians)
}

// 3-DOF robot arm
struct RobotArm {
    links: Vec<DHParameters>,
}

impl RobotArm {
    fn new() -> Self {
        // Example: 3-DOF planar robot arm
        let links = vec![
            DHParameters {
                theta: 0.0, // Will be set by joint angles
                d: 0.0,
                a: 0.25, // 25cm link
                alpha: 0.0,
            },
            DHParameters {
                theta: 0.0, // Will be set by joint angles
                d: 0.0,
                a: 0.20, // 20cm link
                alpha: 0.0,
            },
            DHParameters {
                theta: 0.0, // Will be set by joint angles
                d: 0.0,
                a: 0.15, // 15cm link
                alpha: 0.0,
            },
        ];
        
        RobotArm { links }
    }
    
    // Set joint angles (in radians)
    fn set_joint_angles(&mut self, angles: &[f64]) {
        for (i, angle) in angles.iter().enumerate() {
            if i < self.links.len() {
                self.links[i].theta = *angle;
            }
        }
    }
    
    // Forward kinematics: calculate end-effector position from joint angles
    fn forward_kinematics(&self) -> Vector3<f64> {
        // Start with identity transformation
        let mut t = Matrix4::identity();
        
        // Apply transformations for each link
        for link in &self.links {
            let ct = link.theta.cos();
            let st = link.theta.sin();
            let ca = link.alpha.cos();
            let sa = link.alpha.sin();
            
            // DH transformation matrix
            let dh_matrix = Matrix4::new(
                ct, -st * ca, st * sa, link.a * ct,
                st, ct * ca, -ct * sa, link.a * st,
                0.0, sa, ca, link.d,
                0.0, 0.0, 0.0, 1.0,
            );
            
            // Accumulate transformation
            t = t * dh_matrix;
        }
        
        // Extract position from transformation matrix
        Vector3::new(t[(0, 3)], t[(1, 3)], t[(2, 3)])
    }
    
    // Inverse kinematics using Jacobian pseudo-inverse method
    fn inverse_kinematics(&mut self, target: Vector3<f64>, max_iterations: usize, tolerance: f64) -> Vec<f64> {
        // Start with current joint angles
        let mut angles: Vec<f64> = self.links.iter().map(|link| link.theta).collect();
        
        for _ in 0..max_iterations {
            // Calculate current position
            self.set_joint_angles(&angles);
            let current_position = self.forward_kinematics();
            
            // Calculate error
            let error = target - current_position;
            let error_magnitude = error.norm();
            
            // Check if we're close enough
            if error_magnitude < tolerance {
                break;
            }
            
            // Calculate Jacobian (simplified for brevity)
            // In a real implementation, you would compute the full Jacobian matrix
            
            // Update joint angles (simplified)
            for i in 0..angles.len() {
                angles[i] += error[i % 3] * 0.1;
            }
        }
        
        angles
    }
}

fn main() {
    // Create a 3-DOF robot arm
    let mut robot = RobotArm::new();
    
    // Set joint angles (30°, 45°, -30°)
    let angles = [30.0 * PI / 180.0, 45.0 * PI / 180.0, -30.0 * PI / 180.0];
    robot.set_joint_angles(&angles);
    
    // Calculate forward kinematics
    let position = robot.forward_kinematics();
    println!("End-effector position: [{:.4}, {:.4}, {:.4}]", position[0], position[1], position[2]);
    
    // Calculate inverse kinematics for a target position
    let target = Vector3::new(0.4, 0.2, 0.0);
    println!("Target position: [{:.4}, {:.4}, {:.4}]", target[0], target[1], target[2]);
    
    let ik_angles = robot.inverse_kinematics(target, 100, 1e-6);
    println!("Joint angles (rad): [{:.4}, {:.4}, {:.4}]", ik_angles[0], ik_angles[1], ik_angles[2]);
}

// ROS 2 integration with r2r
// Cargo.toml:
// [dependencies]
// r2r = "0.7"
// anyhow = "1.0"

use r2r::geometry_msgs::msg::Twist;
use r2r::sensor_msgs::msg::JointState;
use r2r::std_msgs::msg::String as RosString;
use std::sync::{Arc, Mutex};
use std::time::Duration;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    // Initialize ROS 2 context
    let ctx = r2r::Context::create()?;
    let mut node = r2r::Node::create(ctx, "rust_robot_controller", "")?;
    
    // Create a publisher for velocity commands
    let vel_publisher = node.create_publisher::<Twist>("/cmd_vel", r2r::QosProfile::default())?;
    
    // Create a publisher for logging
    let log_publisher = node.create_publisher::<RosString>("/robot_logs", r2r::QosProfile::default())?;
    
    // Create a subscriber for joint states
    let joint_states = Arc::new(Mutex::new(None));
    let joint_states_clone = joint_states.clone();
    
    let _joint_sub = node.subscribe::<JointState>(
        "/joint_states",
        r2r::QosProfile::default(),
        move |msg| {
            let mut joint_states = joint_states_clone.lock().unwrap();
            *joint_states = Some(msg);
        },
    )?;
    
    // Create a timer for control loop
    let mut count = 0;
    let timer = node.create_wall_timer(Duration::from_millis(100), move |_| {
        count += 1;
        
        // Create velocity command
        let mut twist = Twist::default();
        
        // Simple motion pattern: move forward for 5 seconds, then rotate for 5 seconds
        if (count / 10) % 10 < 5 {
            // Move forward
            twist.linear.x = 0.2;
            twist.angular.z = 0.0;
            
            // Publish log message
            let mut log_msg = RosString::default();
            log_msg.data = format!("Moving forward: {:.2} m/s", twist.linear.x);
            let _ = log_publisher.publish(&log_msg);
        } else {
            // Rotate
            twist.linear.x = 0.0;
            twist.angular.z = 0.5;
            
            // Publish log message
            let mut log_msg = RosString::default();
            log_msg.data = format!("Rotating: {:.2} rad/s", twist.angular.z);
            let _ = log_publisher.publish(&log_msg);
        }
        
        // Publish velocity command
        let _ = vel_publisher.publish(&twist);
    })?;
    
    // Spin the node
    r2r::spin(&node)?;
    
    Ok(())
}

// Path planning with A* algorithm
// Cargo.toml:
// [dependencies]
// pathfinding = "4.3"

use pathfinding::prelude::astar;
use std::collections::HashMap;

// Define a grid-based map
struct GridMap {
    width: usize,
    height: usize,
    obstacles: Vec<Vec<bool>>,
}

impl GridMap {
    fn new(width: usize, height: usize) -> Self {
        let obstacles = vec![vec![false; width]; height];
        GridMap { width, height, obstacles }
    }
    
    fn add_obstacle(&mut self, x: usize, y: usize) {
        if x < self.width && y < self.height {
            self.obstacles[y][x] = true;
        }
    }
    
    fn is_obstacle(&self, x: usize, y: usize) -> bool {
        if x < self.width && y < self.height {
            self.obstacles[y][x]
        } else {
            true // Out of bounds is considered an obstacle
        }
    }
    
    fn neighbors(&self, pos: (usize, usize)) -> Vec<((usize, usize), usize)> {
        let (x, y) = pos;
        let mut neighbors = Vec::new();
        
        // Check 4 neighboring cells (cardinal directions)
        for &(dx, dy) in &[(0, 1), (1, 0), (0, -1), (-1, 0)] {
            let nx = x as isize + dx;
            let ny = y as isize + dy;
            
            if nx >= 0 && nx < self.width as isize && ny >= 0 && ny < self.height as isize {
                let nx = nx as usize;
                let ny = ny as usize;
                
                if !self.is_obstacle(nx, ny) {
                    neighbors.push(((nx, ny), 1));
                }
            }
        }
        
        neighbors
    }
    
    // A* path planning
    fn plan_path(&self, start: (usize, usize), goal: (usize, usize)) -> Option<(Vec<(usize, usize)>, usize)> {
        // Check if start or goal is an obstacle
        if self.is_obstacle(start.0, start.1) || self.is_obstacle(goal.0, goal.1) {
            return None;
        }
        
        // A* search
        astar(
            &start,
            |&pos| self.neighbors(pos),
            |&(x, y)| {
                // Manhattan distance heuristic
                let dx = if x > goal.0 { x - goal.0 } else { goal.0 - x };
                let dy = if y > goal.1 { y - goal.1 } else { goal.1 - y };
                dx + dy
            },
            |&pos| pos == goal,
        )
    }
    
    // Print the map with path
    fn print_with_path(&self, path: &[(usize, usize)]) {
        let mut path_map = HashMap::new();
        for (i, &pos) in path.iter().enumerate() {
            path_map.insert(pos, i);
        }
        
        for y in 0..self.height {
            for x in 0..self.width {
                if self.obstacles[y][x] {
                    print!("# "); // Obstacle
                } else if let Some(i) = path_map.get(&(x, y)) {
                    if *i == 0 {
                        print!("S "); // Start
                    } else if *i == path.len() - 1 {
                        print!("G "); // Goal
                    } else {
                        print!("* "); // Path
                    }
                } else {
                    print!(". "); // Free space
                }
            }
            println!();
        }
    }
}

fn main() {
    // Create a grid map
    let mut map = GridMap::new(10, 10);
    
    // Add some obstacles
    map.add_obstacle(2, 2);
    map.add_obstacle(2, 3);
    map.add_obstacle(2, 4);
    map.add_obstacle(2, 5);
    map.add_obstacle(3, 5);
    map.add_obstacle(4, 5);
    map.add_obstacle(5, 5);
    map.add_obstacle(6, 5);
    map.add_obstacle(7, 5);
    map.add_obstacle(7, 4);
    map.add_obstacle(7, 3);
    map.add_obstacle(7, 2);
    
    // Plan a path
    let start = (1, 1);
    let goal = (8, 8);
    
    if let Some((path, cost)) = map.plan_path(start, goal) {
        println!("Path found with cost {}", cost);
        map.print_with_path(&path);
    } else {
        println!("No path found");
    }
}