From 7c9e68b61663f839c19f11671c571160ece3c51f Mon Sep 17 00:00:00 2001 From: ilithebutterfly Date: Fri, 10 May 2024 11:55:49 -0400 Subject: [PATCH] Added functions to api --- src/rove_control_board/api/api.h | 83 +++++++++++++++++-- src/rove_control_board/config/base.yaml | 14 ++++ .../rove_control_board/api.py | 83 +++++++++++++++++-- .../control_board_bridge.py | 52 +++++++++--- src/rove_control_board/setup.py | 3 + 5 files changed, 210 insertions(+), 25 deletions(-) create mode 100644 src/rove_control_board/config/base.yaml diff --git a/src/rove_control_board/api/api.h b/src/rove_control_board/api/api.h index 501491d..f0c6a14 100644 --- a/src/rove_control_board/api/api.h +++ b/src/rove_control_board/api/api.h @@ -7,7 +7,8 @@ enum StatusCode : euint16_t { - STNone = 0, + STNotInitialized = 0, + STInitialized = 1, }; enum ErrorCode : euint16_t @@ -15,16 +16,25 @@ enum ErrorCode : euint16_t ERNone = 0, ERAdapterNotInit = 1, ERServoXNACK = 2, - ERServoYNACK = 4, + ERServoYNACK = 3, + ERWinchLocked = 4, }; -enum ServoControlMode : euint32_t +enum ServoControlMode : euint16_t { SCMNone = 0, SCMPosition = 1, SCMSpeed = 2, }; +enum WinchMode : euint16_t +{ + WMFreeWheel = 0, + WMBrake = 1, + WMReverse = 2, + WMForward = 3, +}; + @@ -104,6 +114,13 @@ struct RGB }; static_assert(sizeof(RGB) == 3); +struct RGBLed +{ + euint8_t index; + RGB rgb; +}; +static_assert(sizeof(RGBLed) == 4); + struct Report { Vector2D pos; @@ -186,6 +203,48 @@ static_assert((sizeof(Void)+1) == 2); Report getReport(Void); static_assert((sizeof(Void)+1) == 2); +UShort getWinchMode(Void); +static_assert((sizeof(Void)+1) == 2); + +Void setWinchMode(UShort); +static_assert((sizeof(UShort)+1) == 3); + +Bool_ getWinchLock(Void); +static_assert((sizeof(Void)+1) == 2); + +Bool_ setWinchLock(Void); +static_assert((sizeof(Void)+1) == 2); + +UShort getServoControlMode(Void); +static_assert((sizeof(Void)+1) == 2); + +UShort getServoControlMode(Void); +static_assert((sizeof(Void)+1) == 2); + +Bool_ getGPIO1(Void); +static_assert((sizeof(Void)+1) == 2); + +Bool_ setGPIO1(Bool_); +static_assert((sizeof(Bool_)+1) == 2); + +Bool_ getGPIO2(Void); +static_assert((sizeof(Void)+1) == 2); + +Bool_ setGPIO2(Bool_); +static_assert((sizeof(Bool_)+1) == 2); + +Bool_ getGPIO3(Void); +static_assert((sizeof(Void)+1) == 2); + +Bool_ setGPIO3(Bool_); +static_assert((sizeof(Bool_)+1) == 2); + +RGB getRGBLed(Int); +static_assert((sizeof(Int)+1) == 5); + +Bool_ setRGBLed(RGBLed); +static_assert((sizeof(RGBLed)+1) == 5); + static BaseFunction_ptr commands[] = { new Function(&ping), new Function(&hashCheck), @@ -209,8 +268,22 @@ static BaseFunction_ptr commands[] = { new Function(&getLEDBack), new Function(&getLEDStrobe), new Function(&getReport), + new Function(&getWinchMode), + new Function(&setWinchMode), + new Function(&getWinchLock), + new Function(&setWinchLock), + new Function(&getServoControlMode), + new Function(&getServoControlMode), + new Function(&getGPIO1), + new Function(&setGPIO1), + new Function(&getGPIO2), + new Function(&setGPIO2), + new Function(&getGPIO3), + new Function(&setGPIO3), + new Function(&getRGBLed), + new Function(&setRGBLed), }; -#define COMMANDS_COUNT 22 +#define COMMANDS_COUNT 36 #define MAX_DECODED_SIZE 9 #define MAX_ENCODED_SIZE 13 -#define API_HASH 6660753000537812969UL \ No newline at end of file +#define API_HASH 3739127867455560190UL \ No newline at end of file diff --git a/src/rove_control_board/config/base.yaml b/src/rove_control_board/config/base.yaml new file mode 100644 index 0000000..9c37f0a --- /dev/null +++ b/src/rove_control_board/config/base.yaml @@ -0,0 +1,14 @@ +rove_control_board: + ros_parameters: + tpv_x_step_count: + 4096 + tpv_x_min: + 0 + tpv_x_max: + 360 + tpv_y_step_count: + 4096 + tpv_y_min: + 0 + tpv_y_max: + 90 \ No newline at end of file diff --git a/src/rove_control_board/rove_control_board/api.py b/src/rove_control_board/rove_control_board/api.py index b42dedd..632c012 100644 --- a/src/rove_control_board/rove_control_board/api.py +++ b/src/rove_control_board/rove_control_board/api.py @@ -8,20 +8,29 @@ @manager.enum('H') class StatusCode(Enum): - STNone = 0 + STNotInitialized = 0 + STInitialized = 1 @manager.enum('H') class ErrorCode(Enum): - ERNone = 0b00000000 - ERAdapterNotInit = 0b00000001 - ERServoXNACK = 0b00000010 - ERServoYNACK = 0b00000100 + ERNone = 0 + ERAdapterNotInit = 1 + ERServoXNACK = 2 + ERServoYNACK = 3 + ERWinchLocked = 4 -@manager.enum("I") +@manager.enum("H") class ServoControlMode(Enum): SCMNone = 0 SCMPosition = 1 SCMSpeed = 2 + +@manager.enum("H") +class WinchMode(Enum): + WMFreeWheel = 0 + WMBrake = 1 + WMReverse = 2 + WMForward = 3 @manager.struct('hh') class Vector2D(comm.BinaryData): @@ -30,7 +39,7 @@ def __init__(self, x:int=0, y:int=0): self.x:int self.y:int -@manager.struct('BBB') +@manager.struct('BBBx') class RGB(comm.BinaryData): def __init__(self, r:int=0,g:int=0,b:int=0): super().__init__(r=r,g=g,b=b) @@ -38,6 +47,13 @@ def __init__(self, r:int=0,g:int=0,b:int=0): self.g:int self.b:int +@manager.struct('_B') +class RGBLed(comm.BinaryData): + def __init__(self, rgb:RGB=RGB(), index:int=0,): + super().__init__(rgb=rgb, index=index) + self.index:int + self.rgb:RGB + @manager.struct('_HH') class Report(comm.BinaryData): def __init__(self, pos:Vector2D=Vector2D(), statusCode:int=0, errorCode:int=0): @@ -133,5 +149,58 @@ def getLEDStrobe() -> comm.Bool_: def getReport() -> Report: pass +@manager.command(comm.Void, comm.UShort) +def getWinchMode() -> comm.UShort: + pass + +@manager.command(comm.UShort, comm.Bool_) +def setWinchMode(winchMode:comm.UShort): + pass + +@manager.command(comm.Void, comm.Bool_) +def getWinchLock() -> comm.Bool_: + pass + +@manager.command(comm.Void, comm.Bool_) +def setWinchLock(lock:comm.Bool_): + pass + +@manager.command(comm.Void, comm.UShort) +def getServoControlMode() -> comm.UShort: + pass +@manager.command(comm.UShort, comm.UShort) +def setServoControlMode(servoControlMode:comm.UShort) -> comm.Bool_: + pass +@manager.command(comm.Void, comm.Bool_) +def getGPIO1() -> comm.Bool_: + pass + +@manager.command(comm.Bool_, comm.Bool_) +def setGPIO1(state:comm.Bool_) -> comm.Bool_: + pass + +@manager.command(comm.Void, comm.Bool_) +def getGPIO2() -> comm.Bool_: + pass + +@manager.command(comm.Bool_, comm.Bool_) +def setGPIO2(state:comm.Bool_) -> comm.Bool_: + pass + +@manager.command(comm.Void, comm.Bool_) +def getGPIO3() -> comm.Bool_: + pass + +@manager.command(comm.Bool_, comm.Bool_) +def setGPIO3(state:comm.Bool_) -> comm.Bool_: + pass + +@manager.command(comm.Int, RGB) +def getRGBLed(index:comm.Int) -> RGB: + pass + +@manager.command(RGBLed, comm.Bool_) +def setRGBLed(led:RGBLed) -> comm.Bool_: + pass diff --git a/src/rove_control_board/rove_control_board/control_board_bridge.py b/src/rove_control_board/rove_control_board/control_board_bridge.py index b1c940e..83e7e71 100644 --- a/src/rove_control_board/rove_control_board/control_board_bridge.py +++ b/src/rove_control_board/rove_control_board/control_board_bridge.py @@ -12,6 +12,7 @@ import rclpy import rclpy.logging +import rclpy.parameter from rclpy.node import Node import rclpy.waitable from rove_control_board import api, canutils @@ -27,12 +28,17 @@ DEV = '/dev/ttyACM0/' # TPV bounds ((horiz min, horiz max), (verti min, verti max)) -TPV_BOUNDS:Tuple[Tuple[float, float], Tuple[float,float]] = ((-180, 180), (-45, 90)) +TPV_BOUNDS:Tuple[Tuple[float, float], Tuple[float,float]] = ((-10000, 10000), (-10000, 10000)) MAX_SPEED_X = 10000 MAX_SPEED_Y = 10000 CANBUS_BITRATE = 500000 +STEP_COUNT = 4096 + +def degToStep(deg:float) -> int: + pass + @api.setServoPosition.preCall def setServoPositionValidator(pos:api.Vector2D) -> NoReturn: if all([lb <= p <= hb for p, (lb, hb) in zip([pos.x, pos.y], TPV_BOUNDS)]): @@ -66,13 +72,27 @@ def findDevice(): def debugFunc(func:Callable[[Bridge], Any]): def pred(self:Bridge, *args, **kwargs): - self.get_logger().debug(f"{func.__name__}({args}, {kwargs})") - return func(self, *args, **kwargs) + res = func(self, *args, **kwargs) + self.get_logger().debug(f"{func.__name__}({args}, {kwargs}) = {res}") + return res return pred class Bridge(Node): def __init__(self): super().__init__('control_board_bridge', namespace='control_board_bridge') + self.declare_parameters( + namespace='', + parameters=[ + ('tpv_x_step_count', rclpy.Parameter.Type.INTEGER), + ('tpv_x_min', rclpy.Parameter.Type.DOUBLE), + ('tpv_x_max', rclpy.Parameter.Type.DOUBLE), + ('tpv_y_step_count', rclpy.Parameter.Type.INTEGER), + ('tpv_y_min', rclpy.Parameter.Type.DOUBLE), + ('tpv_y_max', rclpy.Parameter.Type.DOUBLE), + ] + ) + + self.servo_pos = self.create_publisher(Vector3Stamped, 'servo_pos', 0) self.servo_set_pos = self.create_subscription(Vector3Stamped, 'servo_set_pos', self.setPos, 3) self.servo_set_vel = self.create_subscription(Vector3Stamped, 'servo_set_vel', self.setVel, 3) @@ -84,8 +104,8 @@ def __init__(self): self.led_back_set = self.create_subscription(Bool, 'led_back_set', self.setLEDBack, 3) self.led_strobe = self.create_publisher(Bool, 'led_strobe', 0) self.led_strobe_set = self.create_subscription(Bool, 'led_strobe_set', self.setLEDStrobe, 3) - self.create_timer(1/30, self.statusReport) - self.create_timer(1/30, self.ping) + self.heartbeat = self.create_timer(1/30, self.statusReport) + # self.create_timer(1/30, self.ping) # self.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) @@ -106,7 +126,7 @@ def checkAPI(self): @debugFunc def setPos(self, pos:Vector3Stamped): try: - r = api.setServoPosition(api.Vector2D(pos.vector.x, pos.vector.y)) + r = api.setServoPosition(api.Vector2D(int(pos.vector.x), int(pos.vector.y))) if not r.b: self.get_logger().warning(f"Last command didn't ack (from setPos)") except ValueError as e: @@ -115,7 +135,7 @@ def setPos(self, pos:Vector3Stamped): @debugFunc def setVel(self, vel:Vector3Stamped): try: - r = api.setServoSpeed(api.Vector2D(vel.vector.x, vel.vector.y)) + r = api.setServoSpeed(api.Vector2D(int(vel.vector.x), int(vel.vector.y))) if not r.b: self.get_logger().warning(f"Last command didn't ack (from setVel)") except ValueError as e: @@ -124,10 +144,10 @@ def setVel(self, vel:Vector3Stamped): @debugFunc def setAcc(self, acc:Vector3Stamped): try: - r = api.setServoXAcc(comm.Byte(acc.vector.x)) + r = api.setServoXAcc(comm.Byte(int(acc.vector.x))) if not r.b: self.get_logger().warning(f"Last command didn't ack (from setAcc X)") - r = api.setServoYAcc(comm.Byte(acc.vector.y)) + r = api.setServoYAcc(comm.Byte(int(acc.vector.y))) if not r.b: self.get_logger().warning(f"Last command didn't ack (from setAcc Y)") except ValueError as e: @@ -186,7 +206,13 @@ def getLEDStrobe(self) -> Bool: def statusReport(self): r = api.getReport(comm.Void()) if r.errorCode != api.ErrorCode.ERNone.value: - self.get_logger().error(f"Got {api.ErrorCode(r.errorCode).name} from report.") + try: + self.get_logger().error(f"Got {api.ErrorCode(r.errorCode).name} from report.") + except ValueError as e: + self.get_logger().error(str(e)) + + self._pos[0] = r.pos.x + self._pos[1] = r.pos.y self.servo_pos.publish(self.getPos()) self.led_front.publish(self.getLEDFront()) @@ -197,7 +223,7 @@ def statusReport(self): def ping(self): from rove_control_board.api import manager p = manager.ping() - self.get_logger().info(f"Ping: {round(p*1000, 2)} us") + self.get_logger().info(f"Ping: {round(p*1000, 2)} ms") def finddir(directory:str, prefix:str): directory = directory.removesuffix('/') @@ -292,7 +318,7 @@ def openSLCan(dev:str=DEV, prefix:str=None, tryOtherPorts:bool=False): manager._default() manager._stream = canutils.CanBusStream(manager._bus, manager._maxSize()*2, manager._notifier) p = manager.ping() - print(f"Ping: {round(p, 2)} ms on {chan}") + print(f"Ping: {round(p*1000, 2)} ms on {chan}") if not isnan(p): break @@ -355,7 +381,7 @@ def openSocket(): - print(f"Ping: {round(p*1000, 2)} us on {chan}") + print(f"Ping: {round(p*1000, 2)} ms on {chan}") if not isnan(p): break diff --git a/src/rove_control_board/setup.py b/src/rove_control_board/setup.py index 9c0a49c..37c4447 100644 --- a/src/rove_control_board/setup.py +++ b/src/rove_control_board/setup.py @@ -1,3 +1,4 @@ +from glob import glob import io import os import pip @@ -13,6 +14,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), ], install_requires=[ # ROS2 does not follow the python setup procedure. Dependencies added here won't be installed 'setuptools',