diff --git a/src/rove_control_board/rove_control_board/api.py b/src/rove_control_board/rove_control_board/api.py index 632c012..2a86fdb 100644 --- a/src/rove_control_board/rove_control_board/api.py +++ b/src/rove_control_board/rove_control_board/api.py @@ -1,3 +1,209 @@ +# from enum import Enum, Flag +# from typing import NoReturn +# import capra_micro_comm_py as comm +# from rove_control_board.canutils import CanBusCommandManager + +# # manager = comm.SerialCommandManager() +# manager = CanBusCommandManager() + +# @manager.enum('H') +# class StatusCode(Enum): +# STNotInitialized = 0 +# STInitialized = 1 + +# @manager.enum('H') +# class ErrorCode(Enum): +# ERNone = 0 +# ERAdapterNotInit = 1 +# ERServoXNACK = 2 +# ERServoYNACK = 3 +# ERWinchLocked = 4 + +# @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): +# def __init__(self, x:int=0, y:int=0): +# super().__init__(x=x, y=y) +# self.x:int +# self.y:int + +# @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) +# self.r:int +# 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): +# super().__init__(pos=pos, statusCode=statusCode, errorCode=errorCode) +# self.pos:Vector2D +# self.statusCode:int +# self.errorCode:int + +# @manager.struct('ff') +# class Bounds(comm.BinaryData): +# def __init__(self, lower:float=0, upper:float=0): +# super().__init__(lower=lower, upper=upper) +# self.lower:float +# self.upper:float + +# @manager.command(Vector2D, comm.Bool_) +# def setServoPosition(pos:Vector2D) -> comm.Bool_: +# pass + +# @manager.command(comm.Void, Vector2D) +# def getServoPosition() -> Vector2D: +# pass + +# @manager.command(comm.Void, comm.Bool_) +# def setServoPositionZero() -> comm.Bool_: +# pass + +# @manager.command(Vector2D, comm.Bool_) +# def setServoSpeed(speed:Vector2D) -> comm.Bool_: +# pass + +# @manager.command(comm.Void, Vector2D) +# def getServoSpeed() -> Vector2D: +# pass + +# @manager.command(comm.Void, comm.Short) +# def getServoPositionX() -> comm.Short: +# pass + +# @manager.command(comm.Void, comm.Short) +# def getServoPositionY() -> comm.Short: +# pass + +# @manager.command(comm.Void, comm.Short) +# def getServoSpeedX() -> comm.Short: +# pass + +# @manager.command(comm.Void, comm.Short) +# def getServoSpeedY() -> comm.Short: +# pass + +# @manager.command(comm.Byte, comm.Bool_) +# def setServoXAcc(acc:comm.Byte) -> comm.Bool_: +# pass + +# @manager.command(comm.Byte, comm.Bool_) +# def setServoYAcc(acc:comm.Byte) -> comm.Bool_: +# pass + +# @manager.command(comm.Void, comm.Byte) +# def getServoXAcc() -> comm.Byte: +# pass + +# @manager.command(comm.Void, comm.Byte) +# def getServoYAcc() -> comm.Byte: +# pass + +# @manager.command(comm.Bool_, comm.Bool_) +# def setLEDFront(state:comm.Bool_) -> comm.Bool_: +# pass + +# @manager.command(comm.Bool_, comm.Bool_) +# def setLEDBack(state:comm.Bool_) -> comm.Bool_: +# pass + +# @manager.command(comm.Bool_, comm.Bool_) +# def setLEDStrobe(state:comm.Bool_) -> comm.Bool_: +# pass + +# @manager.command(comm.Void, comm.Bool_) +# def getLEDFront() -> comm.Bool_: +# pass + +# @manager.command(comm.Void, comm.Bool_) +# def getLEDBack() -> comm.Bool_: +# pass + +# @manager.command(comm.Void, comm.Bool_) +# def getLEDStrobe() -> comm.Bool_: +# pass + +# @manager.command(comm.Void, Report) +# 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 from enum import Enum, Flag from typing import NoReturn import capra_micro_comm_py as comm @@ -8,29 +214,20 @@ @manager.enum('H') class StatusCode(Enum): - STNotInitialized = 0 - STInitialized = 1 + STNone = 0 @manager.enum('H') class ErrorCode(Enum): - ERNone = 0 - ERAdapterNotInit = 1 - ERServoXNACK = 2 - ERServoYNACK = 3 - ERWinchLocked = 4 + ERNone = 0b00000000 + ERAdapterNotInit = 0b00000001 + ERServoXNACK = 0b00000010 + ERServoYNACK = 0b00000100 -@manager.enum("H") +@manager.enum("I") 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): @@ -39,7 +236,7 @@ def __init__(self, x:int=0, y:int=0): self.x:int self.y:int -@manager.struct('BBBx') +@manager.struct('BBB') class RGB(comm.BinaryData): def __init__(self, r:int=0,g:int=0,b:int=0): super().__init__(r=r,g=g,b=b) @@ -47,13 +244,6 @@ 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): @@ -149,58 +339,4 @@ 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 e386c28..b4afaef 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 @@ -105,6 +105,9 @@ def toStep(valueDeg:float, stepCount:int, offsetDeg:float, minDeg:float, maxDeg: class Bridge(Node): def __init__(self): super().__init__('control_board_bridge', namespace='control_board_bridge') + from rove_control_board.api import manager + self.manager = manager + self.declare_parameters( namespace='', parameters=[ @@ -119,9 +122,9 @@ def __init__(self): ] ) self._xStepCount = self.get_parameter_or('tpv_x_step_count', STEP_COUNT) - self._xMin = self.get_parameter_or('tpv_x_min', 0) - self._xMax = self.get_parameter_or('tpv_x_max', 360) - self._xOffset = self.get_parameter_or('tpv_x_offset', 0) + self._xMin = self.get_parameter_or('tpv_x_min', -180) + self._xMax = self.get_parameter_or('tpv_x_max', 180) + self._xOffset = self.get_parameter_or('tpv_x_offset', 180) self._yStepCount = self.get_parameter_or('tpv_y_step_count', STEP_COUNT) @@ -141,7 +144,7 @@ def __init__(self): 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.heartbeat = self.create_timer(1/30, self.statusReport) - # self.create_timer(1/30, self.ping) + # self.create_timer(1/100, self.ping) # self.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) @@ -158,6 +161,8 @@ def checkAPI(self): from rove_control_board.api import manager if (not manager.apiCheck()): self.get_logger().fatal("API hash mismatch between rove_control_board and control board microcontroller") + return False + return True def _xToDeg(self, step:int) -> float: return toDeg(step, self._xStepCount, self._xOffset, self._xMin, self._xMax) @@ -254,6 +259,7 @@ def getLEDStrobe(self) -> Bool: @debugFunc def statusReport(self): r = api.getReport(comm.Void()) + if r.errorCode != api.ErrorCode.ERNone.value: try: self.get_logger().error(f"Got {api.ErrorCode(r.errorCode).name} from report.") @@ -462,14 +468,18 @@ def main(args=None): # openSLCan(dev=None, prefix=DEV_ID_PREFIX) openSocket() + rclpy.init(args=args) - bridge = Bridge() - bridge.checkAPI() - rclpy.spin(bridge) - - bridge.destroy_node() - rclpy.shutdown() + try: + while True: + bridge = Bridge() + if not bridge.checkAPI(): + sleep(5) + rclpy.spin(bridge) + bridge.destroy_node() + finally: + rclpy.shutdown() if __name__ == '__main__':