UWB Follower

uwb.py

# pip3 install pyserial

import time
import serial
import rclpy
import threading
from rclpy.node import Node
from std_msgs.msg import Int32


DESIRED_DISTANCE = 0.25
LINEAR_P = 4000
ROTATE_P = 15000


def clip(value, min_value, max_value):
    return max(min_value, min(value, max_value))


class UWB:
    def __init__(self, device='/dev/ttyACM0'):
        self.__device = serial.Serial(
            port=device, baudrate=115200, timeout=0.001)
        time.sleep(2)

    def read(self):
        try:
            line = self.__device.readline()
            if line:
                params = line.decode().split(',')
                if len(params) == 8:
                    return float(params[7])
        except Exception as ex:
            print(ex)
        return -1.0

    def __del__(self):
        self.__device.write('\r'.encode())
        self.__device.close()


class UWBROS2(rclpy.node.Node):
    def __init__(self):
        super().__init__('uwb_node')

        self.__motor_left_publisher = self.create_publisher(Int32, '/left_motor/raw', 1)
        self.__motor_right_publisher = self.create_publisher(Int32, '/right_motor/raw', 1)
        self.create_timer(0.1, self.regulate)

        self.__uwb_right = UWB(device='/dev/ttyACM1')
        print('Right UWB ready')
        self.__uwb_left = UWB(device='/dev/ttyACM0')
        print('Left UWB ready')

        self.__last_left_distance = DESIRED_DISTANCE
        self.__last_right_distance = DESIRED_DISTANCE

    def regulate(self):
        val = 0
        while val >= 0:
            val = self.__uwb_left.read()
            if val >= 0:
                self.__last_left_distance = val

        val = 0
        while val >= 0:
            val = self.__uwb_right.read()
            if val >= 0:
                self.__last_right_distance = val

        avg_distance = (self.__last_left_distance + self.__last_right_distance) / 2
        err_linear = avg_distance - DESIRED_DISTANCE
        err_rotate = self.__last_left_distance - self.__last_right_distance

        # Update velocities
        left_velocity = LINEAR_P * err_linear
        right_velocity = LINEAR_P * err_linear
        left_velocity += ROTATE_P * err_rotate
        right_velocity -= ROTATE_P * err_rotate
        print(left_velocity, right_velocity)
        self.__motor_left_publisher.publish(Int32(data=int(left_velocity)))
        self.__motor_right_publisher.publish(Int32(data=int(right_velocity)))


def main(args=None):
    rclpy.init(args=args)
    uwb_ros2 = UWBROS2()
    rclpy.spin(uwb_ros2)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

uwb_init.py

import serial
import time


for i in range(3):
    device = serial.Serial(port=f'/dev/ttyACM{i}', baudrate=115200, timeout=0.001)
    time.sleep(2)
    device.write('\r\r'.encode())
    time.sleep(2)
    device.write('lec\r'.encode())