This is repository for driving DB19 chassis in SNAM. The package is simplified to save the limited resources on a Raspberry Pi 3B+ 
#!/usr/bin/env python

from math import fabs, floor

from include.hat_driver.motor import MotorDirection
# from dt_robot_utils import get_robot_configuration

from include.hat_driver.hat import HATv2

# M_value = MotorDirection()


class DaguWheelsDriver:
    """Class handling communication with motors."""

    LEFT_MOTOR_MIN_PWM  = 60        #: Minimum speed for left motor
    LEFT_MOTOR_MAX_PWM  = 255       #: Maximum speed for left motor
    RIGHT_MOTOR_MIN_PWM = 60        #: Minimum speed for right motor
    RIGHT_MOTOR_MAX_PWM = 255       #: Maximum speed for right motor
    SPEED_TOLERANCE     = 1.0e-2    #: Speed tolerance level

    def __init__(self):
        #rcfg = get_robot_configuration()
        self.hat = HATv2()
        self.leftMotor = self.hat.get_motor(1, "left")
        self.rightMotor = self.hat.get_motor(2, "right")
        # print out some stats
        # this = self.__class__.__name__
        # print(f"[{this}] Running in configuration `{rcfg.name}`, using driver `{DTHAT.__name__}`")
        # print(f"[{this}] Motor #1: {self.leftMotor}")
        # print(f"[{this}] Motor #2: {self.rightMotor}")
        # initialize state
        self.leftSpeed = 0.0
        self.rightSpeed = 0.0
        self._pwm_update()
        # hardware testing flag
        self._is_performing_test = False
    
    def start_hardware_test(self):
        self._is_performing_test = True
    
    def finish_hardware_test(self):
        self._is_performing_test = False

    def set_wheels_speed(self, left: float, right: float, is_test_cmd: bool = False):
        """Sets speed of motors.

        Args:
           left (:obj:`float`): speed for the left wheel, should be between -1 and 1
           right (:obj:`float`): speed for the right wheel, should be between -1 and 1
           is_test_cmd (:obj:`bool`): whether this is a command issue by the hardware test

        """
        if self._is_performing_test and is_test_cmd or not self._is_performing_test:
            self.leftSpeed = left
            self.rightSpeed = right
            self._pwm_update()

    def _pwm_value(self, v, min_pwm, max_pwm):
        """Transforms the requested speed into an int8 number.

        Args:
            v (:obj:`float`): requested speed, should be between -1 and 1.
            min_pwm (:obj:`int8`): minimum speed as int8
            max_pwm (:obj:`int8`): maximum speed as int8
        """
        pwm = 0
        if fabs(v) > self.SPEED_TOLERANCE:
            pwm = int(floor(fabs(v) * (max_pwm - min_pwm) + min_pwm))
        return min(pwm, max_pwm)

    def _pwm_update(self):
        """Sends commands to the microcontroller.

        Updates the current PWM signals (left and right) according to the
        linear velocities of the motors. The requested speed gets
        tresholded.
        """
        vl = self.leftSpeed
        vr = self.rightSpeed

        pwml = self._pwm_value(vl, self.LEFT_MOTOR_MIN_PWM, self.LEFT_MOTOR_MAX_PWM)
        pwmr = self._pwm_value(vr, self.RIGHT_MOTOR_MIN_PWM, self.RIGHT_MOTOR_MAX_PWM)
        leftMotorMode = 0
        rightMotorMode = 0

        if fabs(vl) < self.SPEED_TOLERANCE:
            pwml = 0
        elif vl > 0:
            leftMotorMode = MotorDirection.FORWARD
        elif vl < 0:
            leftMotorMode = MotorDirection.BACKWARD

        if fabs(vr) < self.SPEED_TOLERANCE:
            pwmr = 0
        elif vr > 0:
            rightMotorMode = MotorDirection.FORWARD
        elif vr < 0:
            rightMotorMode = MotorDirection.BACKWARD

        self.leftMotor.set(leftMotorMode, pwml)
        self.rightMotor.set(rightMotorMode, pwmr)

    def __del__(self):
        """Destructor method.

        Releases the motors and deletes tho object.
        """
        self.leftMotor.set(MotorDirection.RELEASE)
        self.rightMotor.set(MotorDirection.RELEASE)
        del self.hat

scripts/dagu_wheels_driver.py
VPASNAM/vpa_db19interface
#!/usr/bin/python
import re
import smbus

# from dt_device_utils import get_device_hardware_brand, DeviceHardwareBrand
# ROBOT_HARDWARE = get_device_hardware_brand()


# ===========================================================================
# Adafruit_I2C Class
# ===========================================================================


class Adafruit_I2C(object):
    @staticmethod
    def getPiRevision():
        """Gets the version number of the Raspberry Pi board"""
        #Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
        try:
            with open("/proc/cpuinfo", "r") as infile:
                for line in infile:
                    # Match a line of the form "Revision : 0002" while ignoring extra
                    # info in front of the revsion (like 1000 when the Pi was over-volted).
                    match = re.match("Revision\s+:\s+.*(\w{4})$", line)
                    if match and match.group(1) in ["0000", "0002", "0003"]:
                        # Return revision 1 if revision ends with 0000, 0002 or 0003.
                        return 1
                    elif match:
                        # Assume revision 2 if revision ends with any other 4 chars.
                        return 2
                # Couldn't find the revision, assume revision 0 like older code for compatibility.
                return 0
        except:
            return 0

    @staticmethod
    def getPiI2CBusNumber():
        # Gets the I2C bus number /dev/i2c#
        return 1 if Adafruit_I2C.getPiRevision() > 1 else 0

    def __init__(self, address, busnum=-1, debug=False):
        self.address = address
        # By default, the correct I2C bus is auto-detected using /proc/cpuinfo
        # Alternatively, you can hard-code the bus version below:
        # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's)
        self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber())

        self.debug = False

    def reverseByteOrder(self, data):
        """Reverses the byte order of an int (16-bit) or long (32-bit) value"""
        # Courtesy Vishal Sapre
        byteCount = len(hex(data)[2:].replace("L", "")[::2])
        val = 0
        for i in range(byteCount):
            val = (val << 8) | (data & 0xFF)
            data >>= 8
        return val

    def errMsg(self):
        return -1

    def write8(self, reg, value):
        """Writes an 8-bit value to the specified register/address"""
        try:
            self.bus.write_byte_data(self.address, reg, value)
            if self.debug:
                print(("I2C: Wrote 0x%02X to register 0x%02X" % (value, reg)))
        except IOError:
            return self.errMsg()

    def write16(self, reg, value):
        """Writes a 16-bit value to the specified register/address pair"""
        try:
            self.bus.write_word_data(self.address, reg, value)
            if self.debug:
                print(("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" % (value, reg, reg + 1)))
        except IOError:
            return self.errMsg()

    def writeRaw8(self, value):
        """Writes an 8-bit value on the bus"""
        try:
            self.bus.write_byte(self.address, value)
            if self.debug:
                print(("I2C: Wrote 0x%02X" % value))
        except IOError:
            return self.errMsg()

    def writeList(self, reg, list):
        """Writes an array of bytes using I2C format"""
        try:
            if self.debug:
                print(("I2C: Writing list to register 0x%02X:" % reg))
                print(list)
            self.bus.write_i2c_block_data(self.address, reg, list)
        except IOError:
            return self.errMsg()

    def readList(self, reg, length):
        """Read a list of bytes from the I2C device"""
        try:
            results = self.bus.read_i2c_block_data(self.address, reg, length)
            if self.debug:
                print(("I2C: Device 0x%02X returned the following from reg 0x%02X" % (self.address, reg)))
                print(results)
            return results
        except IOError:
            return self.errMsg()

    def readU8(self, reg):
        """Read an unsigned byte from the I2C device"""
        try:
            result = self.bus.read_byte_data(self.address, reg)
            if self.debug:
                print(
                    (
                        "I2C: Device 0x%02X returned 0x%02X from reg 0x%02X"
                        % (self.address, result & 0xFF, reg)
                    )
                )
            return result
        except IOError:
            return self.errMsg()

    def readS8(self, reg):
        """Reads a signed byte from the I2C device"""
        try:
            result = self.bus.read_byte_data(self.address, reg)
            if result > 127:
                result -= 256
            if self.debug:
                print(
                    (
                        "I2C: Device 0x%02X returned 0x%02X from reg 0x%02X"
                        % (self.address, result & 0xFF, reg)
                    )
                )
            return result
        except IOError:
            return self.errMsg()

    def readU16(self, reg, little_endian=True):
        """Reads an unsigned 16-bit value from the I2C device"""
        try:
            result = self.bus.read_word_data(self.address, reg)
            # Swap bytes if using big endian because read_word_data assumes little
            # endian on ARM (little endian) systems.
            if not little_endian:
                result = ((result << 8) & 0xFF00) + (result >> 8)
            if self.debug:
                print(
                    (
                        "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X"
                        % (self.address, result & 0xFFFF, reg)
                    )
                )
            return result
        except IOError:
            return self.errMsg()

    def readS16(self, reg, little_endian=True):
        """Reads a signed 16-bit value from the I2C device"""
        try:
            result = self.readU16(reg, little_endian)
            if result > 32767:
                result -= 65536
            return result
        except IOError:
            return self.errMsg()

scripts/include/adafruit_drivers/Adafruit_I2C.py
VPASNAM/vpa_db19interface
LOW = 0
HIGH = 1

scripts/include/hat_driver/constants.py
VPASNAM/vpa_db19interface
LOW = 0
HIGH = 1

scripts/include/hat_driver/gpio.py
VPASNAM/vpa_db19interface
from typing import Dict
from abc import abstractmethod, ABC

from include.adafruit_drivers.Adafruit_PWM_Servo_Driver import PWM

from .motor import Motor, MotorPins, MotorDirectionControl


class AbsHAT(ABC):
    def __init__(self, address=0x60, frequency=1600):
        # default I2C address of the HAT
        self._i2caddr = address
        # default @1600Hz PWM frequency
        self._frequency = frequency
        # configure PWM
        self._pwm = PWM(self._i2caddr, debug=False)
        self._pwm.setPWMFreq(self._frequency)

    @abstractmethod
    def get_motor(self, num: int, name: str) -> Motor:
        pass


class HATv1(AbsHAT):
    _MOTOR_NUM_TO_PINS: Dict[int, MotorPins] = {
        1: MotorPins(10, 9, 8, MotorDirectionControl.PWM),
        2: MotorPins(11, 12, 13, MotorDirectionControl.PWM),
        3: MotorPins(4, 3, 2, MotorDirectionControl.PWM),
        4: MotorPins(5, 6, 7, MotorDirectionControl.PWM),
    }

    def get_motor(self, num: int, name: str) -> Motor:
        if num not in self._MOTOR_NUM_TO_PINS:
            raise ValueError(
                f"Motor num `{num}` not supported. "
                f"Possible choices are `{self._MOTOR_NUM_TO_PINS.keys()}`."
            )
        pins = self._MOTOR_NUM_TO_PINS[num]
        return Motor(name, self._pwm, pins.in1, pins.in2, pins.pwm, control=pins.control)


class HATv2(HATv1):
    pass


class HATv3(HATv2):
    _MOTOR_NUM_TO_PINS: Dict[int, MotorPins] = {
        1: MotorPins(10, 9, 8, MotorDirectionControl.PWM),
        2: MotorPins(33, 31, 13, MotorDirectionControl.GPIO),
    }

scripts/include/hat_driver/hat.py
VPASNAM/vpa_db19interface
import dataclasses
from enum import IntEnum
from abc import abstractmethod, ABC

from include.adafruit_drivers.Adafruit_PWM_Servo_Driver import PWM

from .constants import LOW, HIGH
from .gpio import LOW as GPIO_LOW, HIGH as GPIO_HIGH
from .pwm import LOW as PWM_LOW, HIGH as PWM_HIGH

# from dt_device_utils import get_device_hardware_brand, DeviceHardwareBrand

# ROBOT_HARDWARE = get_device_hardware_brand()

# if ROBOT_HARDWARE in [DeviceHardwareBrand.RASPBERRY_PI, DeviceHardwareBrand.RASPBERRY_PI_64]:
#     import RPi.GPIO as GPIO
# elif ROBOT_HARDWARE == DeviceHardwareBrand.JETSON_NANO:
#     import Jetson.GPIO as GPIO
# else:
#     raise ValueError(f"Hardware `{ROBOT_HARDWARE.name}` not supported.")

import RPi.GPIO as GPIO

class MotorDirection(IntEnum):
    RELEASE = 0
    FORWARD = 1
    BACKWARD = -1


class MotorDirectionControl(IntEnum):
    PWM = 0
    GPIO = 1


@dataclasses.dataclass
class MotorPins:
    in1: int
    in2: int
    pwm: int
    control: MotorDirectionControl


class AbsMotorDirectionController(ABC):
    def __init__(self, in1_pin: int, in2_pin: int, *args, **kwargs):
        self._in1_pin = in1_pin
        self._in2_pin = in2_pin
        self.setup()

    @abstractmethod
    def setup(self):
        pass

    @abstractmethod
    def set(self, direction: MotorDirection):
        pass

    def __str__(self):
        return f"{self.__class__.__name__}[in1={self._in1_pin}, in2={self._in2_pin}]"


class PWMMotorDirectionController(AbsMotorDirectionController):
    _DIRECTION_TO_SIGNALS = {
        MotorDirection.RELEASE: (LOW, HIGH),
        MotorDirection.FORWARD: (HIGH, LOW),
        MotorDirection.BACKWARD: (LOW, HIGH),
    }
    _PWM_VALUES = {
        LOW: (PWM_LOW, PWM_HIGH),
        HIGH: (PWM_HIGH, PWM_LOW),
    }

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        if not isinstance(kwargs.get("pwm", None), PWM):
            raise ValueError(
                "You cannot instantiate `PWMMotorDirectionController` " "without passing a `PWM` object."
            )
        self._pwm = kwargs["pwm"]

    def setup(self):
        pass

    def set(self, direction: MotorDirection):
        in1_signal, in2_signal = self._DIRECTION_TO_SIGNALS[direction]
        in1_value, in2_value = self._PWM_VALUES[in1_signal], self._PWM_VALUES[in2_signal]
        self._pwm.setPWM(self._in1_pin, *in1_value)
        self._pwm.setPWM(self._in2_pin, *in2_value)


class GPIOMotorDirectionController(AbsMotorDirectionController):
    _DIRECTION_TO_SIGNALS = {
        MotorDirection.RELEASE: (HIGH, HIGH),
        MotorDirection.FORWARD: (HIGH, LOW),
        MotorDirection.BACKWARD: (LOW, HIGH),
    }
    _GPIO_VALUES = {LOW: GPIO_LOW, HIGH: GPIO_HIGH}

    def setup(self):
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(self._in1_pin, GPIO.OUT)
        GPIO.setup(self._in2_pin, GPIO.OUT)

    def set(self, direction: MotorDirection):
        in1_signal, in2_signal = self._DIRECTION_TO_SIGNALS[direction]
        in1_value, in2_value = self._GPIO_VALUES[in1_signal], self._GPIO_VALUES[in2_signal]
        GPIO.output(self._in1_pin, in1_value)
        GPIO.output(self._in2_pin, in2_value)


class Motor:
    _K = 16
    _CONTROLLER = {
        MotorDirectionControl.PWM: PWMMotorDirectionController,
        MotorDirectionControl.GPIO: GPIOMotorDirectionController,
    }

    def __init__(
        self, name: str, pwm: PWM, in1_pin: int, in2_pin: int, pwm_pin: int, control: MotorDirectionControl
    ):
        self._pwm = pwm
        self._name = name
        self._in1_pin = in1_pin
        self._in2_pin = in2_pin
        self._pwm_pin = pwm_pin
        self._control = control
        self._controller = self._CONTROLLER[control](in1_pin, in2_pin, pwm=self._pwm)

    def set(self, direction: MotorDirection, speed: int = 0):
        speed = max(0, min(speed, 255))
        self._controller.set(direction)
        self._pwm.setPWM(self._pwm_pin, 0, speed * self._K)

    def __str__(self):
        return (
            f"Motor[name={self._name}, in1={self._in1_pin}, in2={self._in2_pin}, "
            f"pwm={self._pwm_pin}, controller={self._controller}]"
        )

scripts/include/hat_driver/motor.py
VPASNAM/vpa_db19interface
LOW = 0
HIGH = 4096

scripts/include/hat_driver/pwm.py
VPASNAM/vpa_db19interface
"""
The wheel encoder package contains a single class (i.e., WheelEncoderDriver) that is responsible
for talking to a single wheel encoder.

.. autoclass:: wheel_encoder.WheelEncoderDriver

"""
from .wheel_encoder_driver import WheelEncoderDriver, WheelDirection

scripts/include/wheel_encoder/__init__.py
VPASNAM/vpa_db19interface
#!/usr/bin/env python3

from enum import IntEnum

# from dt_device_utils import get_device_hardware_brand, DeviceHardwareBrand

# ROBOT_HARDWARE = get_device_hardware_brand()

# if ROBOT_HARDWARE == DeviceHardwareBrand.JETSON_NANO:
#     import Jetson.GPIO as GPIO

# elif ROBOT_HARDWARE in [DeviceHardwareBrand.RASPBERRY_PI, DeviceHardwareBrand.RASPBERRY_PI_64]:
import RPi.GPIO as GPIO

# else:
    # raise Exception("Undefined Hardware!")


class WheelDirection(IntEnum):
    FORWARD = 1
    REVERSE = -1


class WheelEncoderDriver:
    """Class handling communication with a wheel encoder.

    An instance of this class reads data off of a wheel encoder calls a callback function
    with the new cumulative tick number as sole argument.
    The callback is called only when the encoder fires, thus there is no constant frequency.

        Args:
            gpio_pin (:obj:`int`): ID of the pin the encoder is connected to.
            callback (:obj:`callable`): callback function to receive new (unique) readings.
    """

    def __init__(self, gpio_pin, callback):
        # valid gpio_pin
        if not 1 <= gpio_pin <= 40:
            raise ValueError("The pin number must be within the range [1, 40].")
        # validate callback
        if not callable(callback):
            raise ValueError("The callback object must be a callable object")
        # configure GPIO pin
        self._gpio_pin = gpio_pin
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(gpio_pin, GPIO.IN)
        GPIO.add_event_detect(gpio_pin, GPIO.RISING, callback=self._cb)
        # ---
        self._callback = callback
        self._ticks = 0
        # wheel direction
        self._direction = WheelDirection.FORWARD

    def get_direction(self) -> WheelDirection:
        return self._direction

    def set_direction(self, direction: WheelDirection):
        self._direction = direction

    def _cb(self, _):
        self._ticks += self._direction.value
        self._callback(self._ticks)

    def shutdown(self):
        GPIO.remove_event_detect(self._gpio_pin)

scripts/include/wheel_encoder/wheel_encoder_driver.py
VPASNAM/vpa_db19interface
#!/usr/bin/env python3

import rospy

from vpa_db19interface.msg import WheelsCmd
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool
class KinematicsNode():
    """
    The `KinematicsNode` maps car commands send from various nodes to wheel commands that the robot can execute.
    `KinematicsNode` utilises the car geometry as well as a number of tunining and limiting
    parameters to calculate the wheel commands that the wheels should execute in order for the
    robot to perform the desired car commands (inverse kinematics).    

    Configuration:
        ~gain (:obj:`float`): scaling factor applied to the desired velocity, default is 1.0
        ~trim (:obj:`float`): trimming factor that is typically used to offset differences in the
            behaviour of the left and right motors, it is recommended to use a value that results in
            the robot moving in a straight line when forward command is given, default is 0.0
        ~baseline (:obj:`float`): the distance between the two wheels of the robot, default is 0.1
        ~radius (:obj:`float`): radius of the wheel, default is 0.0318
        ~k (:obj:`float`): motor constant, assumed equal for both motors, default is 27.0
        ~limit (:obj:`float`): limits the final commands sent to the motors, default is 1.0
        ~v_max (:obj:`float`): limits the input velocity, default is 1.0
        ~omega_max (:obj:`float`): limits the input steering angle, default is 8.0

    """

    def __init__(self) -> None:
        
        # Get the vehicle name
        self.veh_name = rospy.get_namespace().strip("/")

        # Read parameters from a robot-specific yaml file if such exists
        # self.read_params_from_calibration_file()

        self._k = rospy.get_param("~k")
        self.estop = True
        # may apply dynamic-reconfiguration to the following parameters
        self._gain      = rospy.get_param("~gain")
        self._trim      = rospy.get_param("~trim")
        self._limit     = rospy.get_param("~limit")
        self._baseline  = rospy.get_param("~baseline") 
        self._radius    = rospy.get_param("~radius")
        self._v_max     = rospy.get_param("~v_max")
        self._omega_max = rospy.get_param("~omega_max")

        # Setup publishers
        self.pub_wheels_cmd = rospy.Publisher(
            "wheels_cmd", WheelsCmd, queue_size=1
        )    
        # Setup subscribers
        self.sub_car_cmd = rospy.Subscriber("cmd_vel", Twist, self.car_cmd_callback)
        # ---
        self.sub_e_stop = rospy.Subscriber("/global_brake", Bool, self.estop_cb, queue_size=1)
        rospy.loginfo("Kinematics Node Initialized, Standing by for twist command")

    def car_cmd_callback(self, msg_car_cmd:Twist):

        #msg_car_cmd.linear.x    = self.trim(msg_car_cmd.linear.x, low=-self._v_max, high=self._v_max)
        #msg_car_cmd.angular.z   = self.trim(msg_car_cmd.angular.z, low=-self._omega_max, high=self._omega_max)

        msg_car_cmd.linear.x    = max(min(msg_car_cmd.linear.x,self._v_max),-self._v_max)
        msg_car_cmd.angular.z   = max(min(msg_car_cmd.angular.z,self._omega_max),-self._omega_max)

        # assuming same motor constants k for both motors
        k_r = k_l = self._k

        # adjusting k by gain and trim
        k_r_inv = (self._gain + self._trim) / k_r
        k_l_inv = (self._gain - self._trim) / k_l

        #
        omega_r = (msg_car_cmd.linear.x + 0.5 * msg_car_cmd.angular.z * self._baseline) / self._radius
        omega_l = (msg_car_cmd.linear.x - 0.5 * msg_car_cmd.angular.z * self._baseline) / self._radius

        # u_r = omega_r * k_r_inv
        # u_l = omega_l * k_l_inv

        # limiting output to limit, which is 1.0 for the duckiebot
        # u_r_limited = self.trim(u_r, -self._limit, self._limit)
        # u_l_limited = self.trim(u_l, -self._limit, self._limit)
        # u_r_limited = max(min(u_r,self._limit),-self._limit)
        # u_l_limited = max(min(u_l,self._limit),-self._limit)

        msg_wheels_cmd              = WheelsCmd()
        msg_wheels_cmd.vel_right    = omega_r
        msg_wheels_cmd.vel_left     = omega_l
        
        if self.estop:
            msg_wheels_cmd.vel_right = 0
            msg_wheels_cmd.vel_left  = 0
        
        self.pub_wheels_cmd.publish(msg_wheels_cmd)

    # def trim(value,low,high):
    #     return max(min(value, high), low)
    def estop_cb(self,msg):
        """
        Callback that enables/disables emergency stop

            Args:
                msg (BoolStamped): emergency_stop flag
        """
        self.estop = msg.data
        if self.estop:
            rospy.loginfo("Global Brake Activated")
        else:
            rospy.loginfo("Global Brake Released")
            
if __name__ == "__main__":
    rospy.init_node("kinematics_node")
    node = KinematicsNode()
    rospy.spin()
scripts/kinematics_node.py
VPASNAM/vpa_db19interface
#!/usr/bin/env python

import rospy
from dagu_wheels_driver import DaguWheelsDriver

from std_msgs.msg import Bool
from vpa_db19interface.msg import WheelsCmd,WheelEncoderStamped
from vpa_db19interface.cfg import omegaConfig
from dynamic_reconfigure.server import Server
class WheelsDriverNode():
    """Node handling the motor velocities communication.


    Subscribers:
       ~wheel_cmd (:obj:`self.defined`)
    """

    def __init__(self):
        
        

        # Setup the driver
        self.driver = DaguWheelsDriver()
        #self.log("Initialized.")
                
        self._left_omega      = 0
        self._right_omega     = 0
        self._vel_left_ref    = 0
        self._vel_right_ref   = 0
        
        self._vel_left        = 0
        self._vel_right       = 0
        
        self._err_int         = 0
        
        self.kp = 0.08
        self.ki = 0.0003
        
        # Subscribers
        self.sub_topic = rospy.Subscriber("wheels_cmd", WheelsCmd, self.wheels_cmd_cb, queue_size=1)
        
        self.sub_left_tick  = rospy.Subscriber("left_wheel_encoder_node/tick",WheelEncoderStamped,self.left_omega_callback,queue_size=1)
        self.sub_right_tick = rospy.Subscriber("right_wheel_encoder_node/tick",WheelEncoderStamped,self.right_omega_callback,queue_size=1)
        self.srv = Server(omegaConfig,self.dynamic_reconfigure_callback)
        rospy.loginfo("Wheel Driver Initialized")
        
    def wheels_cmd_cb(self, msg:WheelsCmd):
        """
        Callback that sets wheels' speeds.
        """
        # if self.estop:
        #     self.driver.set_wheels_speed(left=0, right=0)
        #     print('in the brake, set 0')
        # else:
        self._vel_left_ref    = msg.vel_left
        self._vel_right_ref   = msg.vel_right
            # vel_left    = self.omega_pid(self._left_omega,vel_left_ref)
            # vel_right   = self.omega_pid(self._right_omega,vel_right_ref)
        
        
        # it is likely we also do the latency check here, but for now it is ignored
    def dynamic_reconfigure_callback(self,config,level):
        self.kp = config.kp
        self.ki = config.ki
        return config

    def on_shutdown(self):
        """
        Shutdown procedure.

        Publishes a zero velocity command at shutdown.
        """
        self.driver.set_wheels_speed(left=0.0, right=0.0)
    
    def left_omega_callback(self,tick_msg:WheelEncoderStamped):
        self._left_omega    = tick_msg.omega
        self._vel_left      = self.omega_pid(self._left_omega,self._vel_left_ref)
        self.driver.set_wheels_speed(left=self._vel_left, right=self._vel_right)
        
        
    def right_omega_callback(self,tick_msg:WheelEncoderStamped):
        self._right_omega = tick_msg.omega
        self._vel_right   = self.omega_pid(self._right_omega,self._vel_right_ref)
        self.driver.set_wheels_speed(left=self._vel_left, right=self._vel_right)

    def omega_pid(self,cur_omega,cur_ref_omega):
        if cur_ref_omega < 0.01:
            return 0
        kp = self.kp
        ki = self.ki
        err_omega = cur_ref_omega - cur_omega
        self._err_int += err_omega
        
        #print('current_err',err_omega)
        throttle_control = kp * err_omega + ki * self._err_int
        if throttle_control > 1:
            throttle_control = 1
        if throttle_control < -1:
            throttle_control = -1
        return throttle_control
    
if __name__ == "__main__":
    rospy.init_node("wheel_driver")
    node = WheelsDriverNode()
    rospy.spin()
scripts/wheel_driver.py
VPASNAM/vpa_db19interface
#!/usr/bin/env python3
import rospy
import uuid

from vpa_db19interface.msg import WheelsCmd,WheelEncoderStamped
from include.wheel_encoder import WheelEncoderDriver, WheelDirection
from std_msgs.msg import Header
from math import pi
class WheelEncoderNode():
    """Node handling a single wheel encoder.

    This node is responsible for reading data off of a single wheel encoders.
    Robots with N wheels will need to spin N instances of this node.
    This node is compatible with any rotary encoder that signals ticks as rising edges
    on a digital GPIO pin.

    Though there are two encoders output from the motor, the DB18 decides to use only one of it.
    As a result, the direction must be implied from the command. This may be problematic in many cases

    """

    def __init__(self) -> None:
        # param from the launch file
        self._veh = rospy.get_param(param_name="~veh",default="")
        self._name = rospy.get_param(param_name="~name",default="left")
        self._gpio_pin = rospy.get_param(param_name="~gpio",default=18)
        self._resolution = rospy.get_param(param_name="~resolution",default=135)
        self._configuration = rospy.get_param(param_name="~configuration",default="left")
        self._publish_frequency = rospy.get_param(param_name="~publish_frequency",default=30)

        calib_file = "include/encoder_config/" +  self._name + "_wheel.yaml"

        # try:
        #     with open(calib_file,"r") as f:
        #         calib_data = yaml.safe_load(f)
        #     custom_resolution = int(calib_data["resolution"])
        #     rospy.set_param("~resolution", custom_resolution)
        #     self._resolution = custom_resolution
        #     # This seems unnecessary for now
        # except FileNotFoundError:
        #     rospy.logwarn(f"No custom encoder calibration found at: {calib_file}. " "Using default parameters.")
        # except:
        #     rospy.logwarn("Other errors")

        #tick storage
        self._tick = 0
        self._tick_last = 0
        
        self._omega = 0

        self._tick_pub = rospy.Publisher(
            "~tick", WheelEncoderStamped, queue_size=1
        )
        #
        self._driver = WheelEncoderDriver(self._gpio_pin, self._encoder_tick_cb)
        # from this topic, we guess which direction the wheel is spinning
        self.sub_topic = rospy.Subscriber("wheels_cmd", WheelsCmd, self._wheels_cmd_cb, queue_size=1)
        # publish the wheel speed
        self._timer = rospy.Timer(rospy.Duration(1.0 / self._publish_frequency), self._cb_publish)

    def _wheels_cmd_cb(self, msg):
        if self._configuration == "left":
            if msg.vel_left >= 0:
                self._driver.set_direction(WheelDirection.FORWARD)
            else:
                self._driver.set_direction(WheelDirection.REVERSE)
        elif self._configuration == "right":
            if msg.vel_right >= 0:
                self._driver.set_direction(WheelDirection.FORWARD)
            else:
                self._driver.set_direction(WheelDirection.REVERSE)

    def _encoder_tick_cb(self, tick_no):
        """
        Callback that receives new ticks from the encoder.

            Args:
                tick_no (int): cumulative total number of ticks
        """
    
        self._tick = tick_no

    
    def _frequency_change_cb(self):
        """
        Callback triggered when the publish frequency changes.
        """
        self._timer.shutdown()
        frequency = self._publish_frequency.value
        self._timer = rospy.Timer(rospy.Duration(1.0 / frequency), self._cb_publish)
        rospy.loginfo(f"Publish frequency now set to {frequency}Hz")
    
    def _cb_publish(self, _):
        delta_tick = self._tick - self._tick_last
        self._tick_last = self._tick
        
        _omega = self._publish_frequency * (delta_tick / self._resolution)*2*pi
        
        header = Header()
        header.frame_id = f"{self._veh}/{self._name}_wheel_axis"
        header.stamp = rospy.Time.now()
        self._tick_pub.publish(
            WheelEncoderStamped(
                header=header,
                data=self._tick,
                resolution=self._resolution,
                type=WheelEncoderStamped.ENCODER_TYPE_INCREMENTAL,
                omega=_omega
            )
        )

if __name__ == "__main__":
    rand = str(uuid.uuid4())[:8]
    rospy.init_node("wheel_encoder_%s" % (rand,))
    node = WheelEncoderNode()
    rospy.spin()
scripts/wheel_encoder.py
VPASNAM/vpa_db19interface