From 08f4e111253c28073465921c665e5e09c0af0cfe Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 10 May 2023 11:25:03 +0000 Subject: [PATCH 01/17] Initial setup --- panther_power_control/package.xml | 2 +- panther_power_control/src/power_board_node.py | 182 ++++++++---------- panther_power_control/src/relays_node.py | 42 ++-- 3 files changed, 95 insertions(+), 131 deletions(-) diff --git a/panther_power_control/package.xml b/panther_power_control/package.xml index fec0562d2..62c3d6c0a 100644 --- a/panther_power_control/package.xml +++ b/panther_power_control/package.xml @@ -24,6 +24,6 @@ std_srvs - python-rpi.gpio-pip + python3-libgpiod \ No newline at end of file diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 53651c19f..77358c1e4 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -1,7 +1,7 @@ #!/usr/bin/python3 from dataclasses import dataclass -import RPi.GPIO as GPIO +import gpiod from threading import Lock import rospy @@ -14,36 +14,16 @@ from panther_msgs.msg import DriverState, IOState -@dataclass -class PatherGPIO: - AUX_PW_EN = 18 # Enable auxiliary power, eg. supply to robotic arms etc. - CHRG_DISABLE = 19 # Disable charger - CHRG_SENSE = 7 # Charger sensor (1 - charger plugged in) - DRIVER_EN = 23 # Enable motor drivers (1 - on) - E_STOP_RESET = 27 # Works as IN/OUT, IN - gives info if E-stop in on (1 - off), - # OUT - send 1 to reset estop - FAN_SW = 15 # Turn on the fan (1 - on) - SHDN_INIT = 16 # Shutdown Init managed by systemd service - VDIG_OFF = 21 # Turn the digital power off eg. NUC, Router etc. (1 - off) - VMOT_ON = 6 # Enable mamin power supply to motors (1 - on) - WATCHDOG = 14 # Watchdog pin, if PWM is on this pin Panther will work - - # define inverse logic pins here to be used by _read_pin() method - inverse_logic_pins = [VDIG_OFF, E_STOP_RESET, CHRG_SENSE] - - def __setattr__(self, name: str, value: int) -> None: - raise AttributeError(f'Can\'t reassign constant {name}') - - class Watchdog: - def __init__(self, pin: int) -> None: - self._pin = pin + def __init__(self, line: gpiod.Line) -> None: self._last_state = False self._enabled = True + self._watchdog_pin = line + def __call__(self) -> None: if self._enabled: - GPIO.output(self._pin, self._last_state) + self._watchdog_pin.set_value(self._last_state) self._last_state = not self._last_state def turn_on(self) -> None: @@ -52,22 +32,48 @@ def turn_on(self) -> None: def turn_off(self) -> None: self._enabled = False self._last_state = False - GPIO.output(self._pin, self._last_state) + self._watchdog_pin.set_value(self._last_state) class PowerBoardNode: def __init__(self, name: str) -> None: - rospy.init_node(name, anonymous=False) + self._node_name = name + rospy.init_node(self._node_name, anonymous=False) self._pins_lock = Lock() self._e_stop_lock = Lock() self._clearing_e_stop = False - self._pins = PatherGPIO() - self._setup_gpio() + line_names = [ + 'AUX_PW_EN' # Enable auxiliary power, eg. supply to robotic arms etc. + 'CHRG_DISABLE' # Disable charger + 'CHRG_SENSE' # Charger sensor (1 - charger plugged in) + 'DRIVER_EN' # Enable motor drivers (1 - on) + 'E_STOP_RESET' # Works as IN/OUT, IN - gives info if E-stop in on (1 - off), + # OUT - send 1 to reset estop + 'FAN_SW' # Turn on the fan (1 - on) + 'SHDN_INIT' # Shutdown Init managed by systemd service + 'VDIG_OFF' # Turn the digital power off eg. NUC, Router etc. (1 - off) + 'VMOT_ON' # Enable mamin power supply to motors (1 - on) + 'WATCHDOG' # Watchdog pin, if PWM is on this pin Panther will work + ] + inv_lines = ['VDIG_OFF', 'E_STOP_RESET', 'CHRG_SENSE'] + + chip = gpiod.chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) + self._lines = {name: chip.find_line(name) for name in line_names} + for ln in self._lines: + self._lines[ln].request(self._node_name, type=gpiod.LINE_REQ_DIR_AS_IS) + if ln in inv_lines: + self._lines[ln].request(self._node_name, type=gpiod.LINE_REQ_FLAG_ACTIVE_LOW) + if self._lines[ln].direction() == gpiod.Line.DIRECTION_OUTPUT: + self._lines[ln].set_value(0) + self._lines['CHRG_DISABLE'].set_value(1) + self._lines['E_STOP_RESET'].request(self._node_name, type=gpiod.LINE_REQ_EV_BOTH_EDGES) + self._lines['SHDN_INIT'].request(self._node_name, type=gpiod.LINE_REQ_EV_RISING_EDGE) + + self._watchdog = Watchdog(self._lines['WATCHDOG']) self._motor_start_sequence() - self._watchdog = Watchdog(self._pins.WATCHDOG) self._gpio_wait = 0.05 # seconds self._e_stop_interrupt_time = float('inf') @@ -83,17 +89,17 @@ def __init__(self, name: str) -> None: self._e_stop_state_pub = rospy.Publisher('hardware/e_stop', Bool, queue_size=1, latch=True) self._io_state_pub = rospy.Publisher('hardware/io_state', IOState, queue_size=1, latch=True) - msg = Bool(self._read_pin(self._pins.E_STOP_RESET)) + msg = Bool(self._lines['E_STOP_RESET'].get_value()) self._e_stop_state_pub.publish(msg) io_state = IOState() - io_state.aux_power = self._read_pin(self._pins.AUX_PW_EN) - io_state.charger_connected = self._read_pin(self._pins.CHRG_SENSE) - io_state.fan = self._read_pin(self._pins.FAN_SW) + io_state.aux_power = self._lines['AUX_PW_EN'].get_value() + io_state.charger_connected = self._lines['CHRG_SENSE'].get_value() + io_state.fan = self._lines['FAN_SW'].get_value() io_state.power_button = False - io_state.digital_power = self._read_pin(self._pins.VDIG_OFF) - io_state.charger_enabled = not self._read_pin(self._pins.CHRG_DISABLE) - io_state.motor_on = self._read_pin(self._pins.DRIVER_EN) + io_state.digital_power = self._lines['VDIG_OFF'].get_value() + io_state.charger_enabled = not self._lines['CHRG_DISABLE'].get_value() + io_state.motor_on = self._lines['DRIVER_EN'].get_value() self._io_state_pub.publish(io_state) # ------------------------------- @@ -146,21 +152,12 @@ def __init__(self, name: str) -> None: # 50 Hz of software PWM. Timer running at 100 Hz for raising and falling edges self._watchdog_timer = rospy.Timer(rospy.Duration(0.01), self._watchdog_cb) - # ------------------------------- - # GPIO callbacks - # ------------------------------- - - # register e-stop and power button pin change imminently - GPIO.add_event_detect( - self._pins.E_STOP_RESET, GPIO.BOTH, callback=self._gpio_interrupt_cb, bouncetime=200 - ) - - GPIO.add_event_detect( - self._pins.SHDN_INIT, GPIO.RISING, callback=self._gpio_interrupt_cb, bouncetime=200 - ) - rospy.loginfo(f'[{rospy.get_name()}] Node started') + def __del__(self): + for line in self._lines.values(): + line.release() + def _cmd_vel_cb(self, *args) -> None: with self._e_stop_lock: self._cmd_vel_msg_time = rospy.get_time() @@ -171,22 +168,22 @@ def _motor_controllers_state_cb(self, msg: DriverState) -> None: {msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err} ) - def _gpio_interrupt_cb(self, pin: int) -> None: - with self._pins_lock: - if pin == self._pins.SHDN_INIT: - self._chrg_sense_interrupt_time = rospy.get_time() - - if pin == self._pins.E_STOP_RESET: - self._e_stop_interrupt_time = rospy.get_time() - def _publish_pin_state_cb(self, *args) -> None: with self._pins_lock: - charger_state = self._read_pin(self._pins.CHRG_SENSE) + charger_state = self._lines['CHRG_SENSE'].get_value() self._publish_io_state('charger_connected', charger_state) + if self._lines['SHDN_INIT'].event_read() is not None: + if self._lines['SHDN_INIT'].get_value() == True: + self._chrg_sense_interrupt_time = rospy.get_time() + + if self._lines['E_STOP_RESET'].event_read() is not None: + if self._lines['E_STOP_RESET'].get_value() == True: + self._e_stop_interrupt_time = rospy.get_time() + # filter short spikes of voltage on GPIO if rospy.get_time() - self._chrg_sense_interrupt_time > self._gpio_wait: - if self._read_pin(self._pins.SHDN_INIT): + if self._lines['SHDN_INIT'].get_value(): rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') self._publish_io_state('power_button', True) self._chrg_sense_interrupt_time = float('inf') @@ -218,7 +215,7 @@ def _digital_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: with self._e_stop_lock: - if self._validate_gpio_pin(self._pins.E_STOP_RESET, False): + if self._lines['E_STOP_RESET'].get_value() == False: return TriggerResponse(True, 'E-STOP is not active, reset is not needed') elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: return TriggerResponse( @@ -233,7 +230,7 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: self._reset_e_stop() - if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): + if self._lines['E_STOP_RESET'].get_value() == True: self._watchdog.turn_off() return TriggerResponse( False, @@ -247,16 +244,16 @@ def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(True, f'E-STOP triggered, watchdog turned off') def _fan_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(req.data, self._pins.FAN_SW, 'Fan enable') + res = self._set_bool_srv_handle(req.data, 'FAN_SW', 'Fan enable') if res.success: self._publish_io_state('fan', req.data) return res def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - if self._validate_gpio_pin(self._pins.DRIVER_EN, req.data): + if self._lines['DRIVER_EN'].get_value() == req.data: return SetBoolResponse(True, f'Motor state already set to: {req.data}') - res = self._set_bool_srv_handle(req.data, self._pins.DRIVER_EN, 'Motor drivers enable') + res = self._set_bool_srv_handle(req.data, 'DRIVER_EN', 'Motor drivers enable') if not res.success: return res @@ -268,7 +265,7 @@ def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: try: reset_script_res = self._reset_roboteq_script_client.call() if not reset_script_res.success: - res = self._set_bool_srv_handle(False, self._pins.DRIVER_EN, 'Motor drivers enable') + res = self._set_bool_srv_handle(False, 'DRIVER_EN', 'Motor drivers enable') if res.success: self._publish_io_state('motor_on', False) return SetBoolResponse(reset_script_res.success, reset_script_res.message) @@ -280,10 +277,10 @@ def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: return res - def _set_bool_srv_handle(self, value: bool, pin: int, name: str) -> SetBoolResponse: + def _set_bool_srv_handle(self, value: bool, pin_name: str, name: str) -> SetBoolResponse: rospy.logdebug(f'[{rospy.get_name()}] Requested {name} = {value}') - self._write_to_pin(pin, value) - success = self._validate_gpio_pin(pin, value) + self._lines[pin_name].set_value(value) + success = self._lines[pin_name].get_value() == value msg = f'{name} write {value} failed' if success: msg = f'{name} write {value} successful' @@ -292,19 +289,24 @@ def _set_bool_srv_handle(self, value: bool, pin: int, name: str) -> SetBoolRespo def _reset_e_stop(self) -> None: self._clearing_e_stop = True - GPIO.setup(self._pins.E_STOP_RESET, GPIO.OUT) + config = gpiod.line_request() + config.consumer = self._node_name + config.request_type = gpiod.line_request.DIRECTION_OUTPUT + self._lines['E_STOP_RESET'].request(config) + self._watchdog.turn_on() - self._write_to_pin(self._pins.E_STOP_RESET, False) + self._lines['E_STOP_RESET'].set_value(True) rospy.sleep(0.1) - GPIO.setup(self._pins.E_STOP_RESET, GPIO.IN) + config.request_type = gpiod.line_request.DIRECTION_OUTPUT + self._lines['E_STOP_RESET'].request(config) rospy.sleep(0.1) self._clearing_e_stop = False self._e_stop_event() def _e_stop_event(self) -> None: - e_stop_state = self._read_pin(self._pins.E_STOP_RESET) + e_stop_state = self._lines['E_STOP_RESET'].get_value() if e_stop_state != self._e_stop_state_pub.impl.latch.data and not self._clearing_e_stop: self._e_stop_state_pub.publish(e_stop_state) @@ -315,39 +317,11 @@ def _publish_io_state(self, attribute: str, val: bool) -> None: self._io_state_pub.publish(last_msg) def _motor_start_sequence(self) -> None: - self._write_to_pin(self._pins.VMOT_ON, 1) + self._lines['VMOT_ON'].set_value(True) rospy.sleep(0.5) - self._write_to_pin(self._pins.DRIVER_EN, 1) + self._lines['DRIVER_EN'].set_value(True) rospy.sleep(0.2) - def _setup_gpio(self) -> None: - GPIO.setwarnings(False) - GPIO.setmode(GPIO.BCM) - GPIO.setup(self._pins.AUX_PW_EN, GPIO.OUT, initial=0) - GPIO.setup(self._pins.CHRG_DISABLE, GPIO.OUT, initial=1) - GPIO.setup(self._pins.CHRG_SENSE, GPIO.IN) - GPIO.setup(self._pins.DRIVER_EN, GPIO.OUT, initial=0) - GPIO.setup(self._pins.E_STOP_RESET, GPIO.IN) # USED AS I/O - GPIO.setup(self._pins.FAN_SW, GPIO.OUT, initial=0) - GPIO.setup(self._pins.SHDN_INIT, GPIO.IN) - GPIO.setup(self._pins.VDIG_OFF, GPIO.OUT, initial=0) - GPIO.setup(self._pins.VMOT_ON, GPIO.OUT, initial=0) - GPIO.setup(self._pins.WATCHDOG, GPIO.OUT, initial=0) - - def _read_pin(self, pin: int) -> bool: - if pin in self._pins.inverse_logic_pins: - return not GPIO.input(pin) - return GPIO.input(pin) - - def _write_to_pin(self, pin: int, value: bool) -> None: - if pin in self._pins.inverse_logic_pins: - GPIO.output(pin, not value) - return - GPIO.output(pin, value) - - def _validate_gpio_pin(self, pin: int, value: bool) -> bool: - return self._read_pin(pin) == value - def main(): power_board_node = PowerBoardNode('power_board_node') @@ -359,5 +333,3 @@ def main(): main() except rospy.ROSInterruptException: pass - finally: - GPIO.cleanup() diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 650a14501..c4071eb65 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -1,7 +1,7 @@ #!/usr/bin/python3 from dataclasses import dataclass -import RPi.GPIO as GPIO +import gpiod from threading import Lock import rospy @@ -13,25 +13,21 @@ from panther_msgs.msg import DriverState, IOState -@dataclass -class PatherGPIO: - MOTOR_ON = 6 # Pin to enable motor controllers - STAGE2_INPUT = 22 # Check if power can be forwarded to motor controllers - - def __setattr__(self, name: str, value: int) -> None: - raise AttributeError(f'Can\'t reassign constant {name}') - - class RelaysNode: def __init__(self, name: str) -> None: - rospy.init_node(name, anonymous=False) + self._node_name = name + rospy.init_node(self._node_name, anonymous=False) self._lock = Lock() - self._pins = PatherGPIO() - self._setup_gpio() + chip = gpiod.chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) - self._e_stop_state = not GPIO.input(self._pins.STAGE2_INPUT) + line_names = ['MOTOR_ON' 'STAGE2_INPUT'] + self._lines = {name: chip.find_line(name) for name in line_names} + self._lines['MOTOR_ON'].request(self._node_name, type=gpiod.LINE_REQ_DIR_OUT) + self._lines['STAGE2_INPUT'].request(self._node_name, type=gpiod.LINE_REQ_DIR_IN) + + self._e_stop_state = not self._lines['STAGE2_INPUT'].get_value() self._cmd_vel_msg_time = rospy.get_time() self._can_net_err = True @@ -46,7 +42,7 @@ def __init__(self, name: str) -> None: self._e_stop_state_pub.publish(self._e_stop_state) self._io_state = IOState() - self._io_state.motor_on = GPIO.input(self._pins.STAGE2_INPUT) + self._io_state.motor_on = self._lines['STAGE2_INPUT'].get_value() self._io_state.aux_power = False self._io_state.charger_connected = False self._io_state.fan = False @@ -86,6 +82,10 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') + def __del__(self): + for line in self._lines.values(): + line.release() + def _cmd_vel_cb(self, *args) -> None: with self._lock: self._cmd_vel_msg_time = rospy.get_time() @@ -120,18 +120,12 @@ def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(True, 'E-SROP triggered successful') def _set_motor_state_timer_cb(self, *args) -> None: - motor_state = GPIO.input(self._pins.STAGE2_INPUT) - GPIO.output(self._pins.MOTOR_ON, motor_state) + motor_state = self._lines['STAGE2_INPUT'].get_value() + self._lines['MOTOR_ON'].set_value(motor_state) if self._io_state.motor_on != motor_state: self._io_state.motor_on = motor_state self._io_state_pub.publish(self._io_state) - def _setup_gpio(self) -> None: - GPIO.setwarnings(False) - GPIO.setmode(GPIO.BCM) - GPIO.setup(self._pins.MOTOR_ON, GPIO.OUT) - GPIO.setup(self._pins.STAGE2_INPUT, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) - def main(): relays_node = RelaysNode('relays_node') @@ -143,5 +137,3 @@ def main(): main() except rospy.ROSInterruptException: pass - finally: - GPIO.cleanup() From 4197beb96dec901ce3cbe19e3efe0810501d9102 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 10 May 2023 13:30:34 +0000 Subject: [PATCH 02/17] Fix after tests on hardware --- panther_power_control/src/power_board_node.py | 97 ++++++++++--------- panther_power_control/src/relays_node.py | 6 +- 2 files changed, 53 insertions(+), 50 deletions(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 77358c1e4..9b81ba749 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -45,32 +45,34 @@ def __init__(self, name: str) -> None: self._clearing_e_stop = False - line_names = [ - 'AUX_PW_EN' # Enable auxiliary power, eg. supply to robotic arms etc. - 'CHRG_DISABLE' # Disable charger - 'CHRG_SENSE' # Charger sensor (1 - charger plugged in) - 'DRIVER_EN' # Enable motor drivers (1 - on) - 'E_STOP_RESET' # Works as IN/OUT, IN - gives info if E-stop in on (1 - off), + out_line_names = [ + 'AUX_PW_EN', # Enable auxiliary power, eg. supply to robotic arms etc. + 'CHRG_DISABLE', # Disable charger + 'DRIVER_EN', # Enable motor drivers (1 - on) # OUT - send 1 to reset estop - 'FAN_SW' # Turn on the fan (1 - on) - 'SHDN_INIT' # Shutdown Init managed by systemd service - 'VDIG_OFF' # Turn the digital power off eg. NUC, Router etc. (1 - off) - 'VMOT_ON' # Enable mamin power supply to motors (1 - on) - 'WATCHDOG' # Watchdog pin, if PWM is on this pin Panther will work + 'FAN_SW', # Turn on the fan (1 - on) + 'VDIG_OFF', # Turn the digital power off eg. NUC, Router etc. (1 - off) + 'VMOT_ON', # Enable mamin power supply to motors (1 - on) + 'WATCHDOG', # Watchdog pin, if PWM is on this pin Panther will work + ] + in_line_names = [ + 'CHRG_SENSE', # Charger sensor (1 - charger plugged in) + 'E_STOP_RESET', # Works as IN/OUT, IN - gives info if E-stop in on (1 - off), + 'SHDN_INIT', # Shutdown Init managed by systemd service ] inv_lines = ['VDIG_OFF', 'E_STOP_RESET', 'CHRG_SENSE'] - chip = gpiod.chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) - self._lines = {name: chip.find_line(name) for name in line_names} - for ln in self._lines: - self._lines[ln].request(self._node_name, type=gpiod.LINE_REQ_DIR_AS_IS) - if ln in inv_lines: - self._lines[ln].request(self._node_name, type=gpiod.LINE_REQ_FLAG_ACTIVE_LOW) - if self._lines[ln].direction() == gpiod.Line.DIRECTION_OUTPUT: - self._lines[ln].set_value(0) - self._lines['CHRG_DISABLE'].set_value(1) - self._lines['E_STOP_RESET'].request(self._node_name, type=gpiod.LINE_REQ_EV_BOTH_EDGES) - self._lines['SHDN_INIT'].request(self._node_name, type=gpiod.LINE_REQ_EV_RISING_EDGE) + self._chip = gpiod.Chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) + self._lines = {name: self._chip.find_line(name) for name in out_line_names + in_line_names} + for name, line in self._lines.items(): + req_type = gpiod.LINE_REQ_DIR_OUT if name in out_line_names else gpiod.LINE_REQ_DIR_IN + req_flag = gpiod.LINE_REQ_FLAG_ACTIVE_LOW if name in inv_lines else 0 + line.request(self._node_name, type=req_type, flags=req_flag) + + self._lines['CHRG_DISABLE'].set_value(True) + self._lines['AUX_PW_EN'].set_value(False) + self._lines['FAN_SW'].set_value(False) + self._lines['VDIG_OFF'].set_value(True) self._watchdog = Watchdog(self._lines['WATCHDOG']) self._motor_start_sequence() @@ -157,6 +159,7 @@ def __init__(self, name: str) -> None: def __del__(self): for line in self._lines.values(): line.release() + self._chip.close() def _cmd_vel_cb(self, *args) -> None: with self._e_stop_lock: @@ -173,42 +176,40 @@ def _publish_pin_state_cb(self, *args) -> None: charger_state = self._lines['CHRG_SENSE'].get_value() self._publish_io_state('charger_connected', charger_state) - if self._lines['SHDN_INIT'].event_read() is not None: - if self._lines['SHDN_INIT'].get_value() == True: + # filter short spikes of voltage on GPIO + if self._lines['SHDN_INIT'].get_value(): + if rospy.get_time() - self._chrg_sense_interrupt_time > self._gpio_wait: + if self._lines['SHDN_INIT'].get_value(): + rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') + self._publish_io_state('power_button', True) + self._chrg_sense_interrupt_time = float('inf') + else: self._chrg_sense_interrupt_time = rospy.get_time() - if self._lines['E_STOP_RESET'].event_read() is not None: - if self._lines['E_STOP_RESET'].get_value() == True: + if self._lines['E_STOP_RESET'].get_value(): + if rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: + self._e_stop_event() + self._e_stop_interrupt_time = float('inf') + else: self._e_stop_interrupt_time = rospy.get_time() - # filter short spikes of voltage on GPIO - if rospy.get_time() - self._chrg_sense_interrupt_time > self._gpio_wait: - if self._lines['SHDN_INIT'].get_value(): - rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') - self._publish_io_state('power_button', True) - self._chrg_sense_interrupt_time = float('inf') - - if rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: - self._e_stop_event() - self._e_stop_interrupt_time = float('inf') - def _watchdog_cb(self, *args) -> None: self._watchdog() def _aux_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(req.data, self._pins.AUX_PW_EN, 'Aux power enable') + res = self._set_bool_srv_handle(req.data, 'AUX_PW_EN', 'Aux power enable') if res.success: self._publish_io_state('aux_power', req.data) return res def _charger_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(not req.data, self._pins.CHRG_DISABLE, 'Charger disable') + res = self._set_bool_srv_handle(not req.data, 'CHRG_DISABLE', 'Charger disable') if res.success: self._publish_io_state('charger_enabled', req.data) return res def _digital_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(req.data, self._pins.VDIG_OFF, 'Digital power enable') + res = self._set_bool_srv_handle(req.data, 'VDIG_OFF', 'Digital power enable') if res.success: self._publish_io_state('digital_power', req.data) return res @@ -289,18 +290,20 @@ def _set_bool_srv_handle(self, value: bool, pin_name: str, name: str) -> SetBool def _reset_e_stop(self) -> None: self._clearing_e_stop = True - config = gpiod.line_request() - config.consumer = self._node_name - config.request_type = gpiod.line_request.DIRECTION_OUTPUT - self._lines['E_STOP_RESET'].request(config) + + req_type = gpiod.LINE_REQ_DIR_OUT + req_flag = gpiod.LINE_REQ_FLAG_ACTIVE_LOW + self._lines['E_STOP_RESET'].release() + self._lines['E_STOP_RESET'].request(self._node_name, type=req_type, flags=req_flag) self._watchdog.turn_on() - self._lines['E_STOP_RESET'].set_value(True) + self._lines['E_STOP_RESET'].set_value(False) rospy.sleep(0.1) - config.request_type = gpiod.line_request.DIRECTION_OUTPUT - self._lines['E_STOP_RESET'].request(config) + req_type = gpiod.LINE_REQ_DIR_IN + self._lines['E_STOP_RESET'].release() + self._lines['E_STOP_RESET'].request(self._node_name, type=req_type, flags=req_flag) rospy.sleep(0.1) self._clearing_e_stop = False self._e_stop_event() diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index c4071eb65..d69e87e7b 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -20,10 +20,10 @@ def __init__(self, name: str) -> None: self._lock = Lock() - chip = gpiod.chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) + chip = gpiod.Chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) - line_names = ['MOTOR_ON' 'STAGE2_INPUT'] - self._lines = {name: chip.find_line(name) for name in line_names} + line_names = {'MOTOR_ON': 6, 'STAGE2_INPUT': 22} + self._lines = {name: chip.get_line(line_names[name]) for name in line_names} self._lines['MOTOR_ON'].request(self._node_name, type=gpiod.LINE_REQ_DIR_OUT) self._lines['STAGE2_INPUT'].request(self._node_name, type=gpiod.LINE_REQ_DIR_IN) From 317d48641507d8edf29d301dd9e4c5f978a6dc9e Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 10 May 2023 13:53:59 +0000 Subject: [PATCH 03/17] Increase update frequency --- panther_power_control/src/power_board_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 9b81ba749..3a50cdf79 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -149,8 +149,8 @@ def __init__(self, name: str) -> None: # Timers # ------------------------------- - # 5 Hz publish non asynch pin state - self._publish_pin_state_timer = rospy.Timer(rospy.Duration(0.2), self._publish_pin_state_cb) + # 20 Hz publish non asynch pin state + self._publish_pin_state_timer = rospy.Timer(rospy.Duration(0.05), self._publish_pin_state_cb) # 50 Hz of software PWM. Timer running at 100 Hz for raising and falling edges self._watchdog_timer = rospy.Timer(rospy.Duration(0.01), self._watchdog_cb) From d955e1fa35df15d7bc2096c3b5138b6be389287c Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 10 May 2023 17:36:32 +0000 Subject: [PATCH 04/17] Fix timout error --- panther_power_control/src/power_board_node.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 3a50cdf79..3bf39610d 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -2,6 +2,7 @@ from dataclasses import dataclass import gpiod +import math from threading import Lock import rospy @@ -78,6 +79,7 @@ def __init__(self, name: str) -> None: self._motor_start_sequence() self._gpio_wait = 0.05 # seconds + self._last_e_stop_state = self._lines['E_STOP_RESET'].get_value() self._e_stop_interrupt_time = float('inf') self._chrg_sense_interrupt_time = float('inf') @@ -183,14 +185,14 @@ def _publish_pin_state_cb(self, *args) -> None: rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') self._publish_io_state('power_button', True) self._chrg_sense_interrupt_time = float('inf') - else: + elif math.isinf(self._chrg_sense_interrupt_time): self._chrg_sense_interrupt_time = rospy.get_time() - if self._lines['E_STOP_RESET'].get_value(): + if self._lines['E_STOP_RESET'].get_value() != self._last_e_stop_state: if rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: self._e_stop_event() self._e_stop_interrupt_time = float('inf') - else: + elif math.isinf(self._e_stop_interrupt_time): self._e_stop_interrupt_time = rospy.get_time() def _watchdog_cb(self, *args) -> None: @@ -310,7 +312,8 @@ def _reset_e_stop(self) -> None: def _e_stop_event(self) -> None: e_stop_state = self._lines['E_STOP_RESET'].get_value() - if e_stop_state != self._e_stop_state_pub.impl.latch.data and not self._clearing_e_stop: + if e_stop_state != self._last_e_stop_state and not self._clearing_e_stop: + self._last_e_stop_state = e_stop_state self._e_stop_state_pub.publish(e_stop_state) def _publish_io_state(self, attribute: str, val: bool) -> None: From 9806e152b23d163d6dff42add654f04c684d681b Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Thu, 11 May 2023 16:10:33 +0200 Subject: [PATCH 05/17] Update panther_power_control/src/power_board_node.py Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_power_control/src/power_board_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 3bf39610d..66eab47ef 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -58,7 +58,7 @@ def __init__(self, name: str) -> None: ] in_line_names = [ 'CHRG_SENSE', # Charger sensor (1 - charger plugged in) - 'E_STOP_RESET', # Works as IN/OUT, IN - gives info if E-stop in on (1 - off), + 'E_STOP_RESET', # Works as IN/OUT, IN - gives info if E-stop is on (1 - off), 'SHDN_INIT', # Shutdown Init managed by systemd service ] inv_lines = ['VDIG_OFF', 'E_STOP_RESET', 'CHRG_SENSE'] From 9c2f3a15a6fff1b7d6ded9688f05b2974a3a0378 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Thu, 11 May 2023 16:10:56 +0200 Subject: [PATCH 06/17] Update panther_power_control/src/power_board_node.py Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_power_control/src/power_board_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 66eab47ef..40589eba7 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -218,7 +218,7 @@ def _digital_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: with self._e_stop_lock: - if self._lines['E_STOP_RESET'].get_value() == False: + if not self._lines['E_STOP_RESET'].get_value(): return TriggerResponse(True, 'E-STOP is not active, reset is not needed') elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: return TriggerResponse( From 51ba6d46c46dcf8cfc83da1d6c059390eda31e5f Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Thu, 11 May 2023 16:11:08 +0200 Subject: [PATCH 07/17] Update panther_power_control/src/power_board_node.py Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com> --- panther_power_control/src/power_board_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 40589eba7..158e78f83 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -233,7 +233,7 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: self._reset_e_stop() - if self._lines['E_STOP_RESET'].get_value() == True: + if self._lines['E_STOP_RESET'].get_value(): self._watchdog.turn_off() return TriggerResponse( False, From 878c51ef336a5c752a9dc873bbb6e6a6a0f1abe9 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Fri, 12 May 2023 11:16:02 +0200 Subject: [PATCH 08/17] REmove inverted pins --- panther_power_control/src/power_board_node.py | 78 ++++++++++--------- 1 file changed, 41 insertions(+), 37 deletions(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 158e78f83..7de1a4a25 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -50,7 +50,6 @@ def __init__(self, name: str) -> None: 'AUX_PW_EN', # Enable auxiliary power, eg. supply to robotic arms etc. 'CHRG_DISABLE', # Disable charger 'DRIVER_EN', # Enable motor drivers (1 - on) - # OUT - send 1 to reset estop 'FAN_SW', # Turn on the fan (1 - on) 'VDIG_OFF', # Turn the digital power off eg. NUC, Router etc. (1 - off) 'VMOT_ON', # Enable mamin power supply to motors (1 - on) @@ -58,30 +57,32 @@ def __init__(self, name: str) -> None: ] in_line_names = [ 'CHRG_SENSE', # Charger sensor (1 - charger plugged in) - 'E_STOP_RESET', # Works as IN/OUT, IN - gives info if E-stop is on (1 - off), + 'E_STOP_RESET', # Works as IN/OUT, + # IN - gives info if E-stop is on (1 - off), OUT - send 1 to reset estop 'SHDN_INIT', # Shutdown Init managed by systemd service ] - inv_lines = ['VDIG_OFF', 'E_STOP_RESET', 'CHRG_SENSE'] self._chip = gpiod.Chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) self._lines = {name: self._chip.find_line(name) for name in out_line_names + in_line_names} for name, line in self._lines.items(): - req_type = gpiod.LINE_REQ_DIR_OUT if name in out_line_names else gpiod.LINE_REQ_DIR_IN - req_flag = gpiod.LINE_REQ_FLAG_ACTIVE_LOW if name in inv_lines else 0 - line.request(self._node_name, type=req_type, flags=req_flag) + line.request( + self._node_name, + gpiod.LINE_REQ_DIR_OUT if name in out_line_names else gpiod.LINE_REQ_DIR_IN, + ) self._lines['CHRG_DISABLE'].set_value(True) self._lines['AUX_PW_EN'].set_value(False) self._lines['FAN_SW'].set_value(False) - self._lines['VDIG_OFF'].set_value(True) + # This pin is inverted so default has to be high + self._lines['VDIG_OFF'].set_value(False) self._watchdog = Watchdog(self._lines['WATCHDOG']) self._motor_start_sequence() self._gpio_wait = 0.05 # seconds - self._last_e_stop_state = self._lines['E_STOP_RESET'].get_value() + self._last_e_stop_state = not self._lines['E_STOP_RESET'].get_value() self._e_stop_interrupt_time = float('inf') - self._chrg_sense_interrupt_time = float('inf') + self._chrg_sense_pressed_time = float('inf') self._cmd_vel_msg_time = rospy.get_time() self._can_net_err = True @@ -93,15 +94,15 @@ def __init__(self, name: str) -> None: self._e_stop_state_pub = rospy.Publisher('hardware/e_stop', Bool, queue_size=1, latch=True) self._io_state_pub = rospy.Publisher('hardware/io_state', IOState, queue_size=1, latch=True) - msg = Bool(self._lines['E_STOP_RESET'].get_value()) + msg = Bool(not self._lines['E_STOP_RESET'].get_value()) self._e_stop_state_pub.publish(msg) io_state = IOState() io_state.aux_power = self._lines['AUX_PW_EN'].get_value() - io_state.charger_connected = self._lines['CHRG_SENSE'].get_value() + io_state.charger_connected = not self._lines['CHRG_SENSE'].get_value() io_state.fan = self._lines['FAN_SW'].get_value() io_state.power_button = False - io_state.digital_power = self._lines['VDIG_OFF'].get_value() + io_state.digital_power = not self._lines['VDIG_OFF'].get_value() io_state.charger_enabled = not self._lines['CHRG_DISABLE'].get_value() io_state.motor_on = self._lines['DRIVER_EN'].get_value() self._io_state_pub.publish(io_state) @@ -152,7 +153,9 @@ def __init__(self, name: str) -> None: # ------------------------------- # 20 Hz publish non asynch pin state - self._publish_pin_state_timer = rospy.Timer(rospy.Duration(0.05), self._publish_pin_state_cb) + self._publish_pin_state_timer = rospy.Timer( + rospy.Duration(0.05), self._publish_pin_state_cb + ) # 50 Hz of software PWM. Timer running at 100 Hz for raising and falling edges self._watchdog_timer = rospy.Timer(rospy.Duration(0.01), self._watchdog_cb) @@ -175,25 +178,27 @@ def _motor_controllers_state_cb(self, msg: DriverState) -> None: def _publish_pin_state_cb(self, *args) -> None: with self._pins_lock: - charger_state = self._lines['CHRG_SENSE'].get_value() + charger_state = not self._lines['CHRG_SENSE'].get_value() self._publish_io_state('charger_connected', charger_state) # filter short spikes of voltage on GPIO - if self._lines['SHDN_INIT'].get_value(): - if rospy.get_time() - self._chrg_sense_interrupt_time > self._gpio_wait: - if self._lines['SHDN_INIT'].get_value(): - rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') - self._publish_io_state('power_button', True) - self._chrg_sense_interrupt_time = float('inf') - elif math.isinf(self._chrg_sense_interrupt_time): - self._chrg_sense_interrupt_time = rospy.get_time() - - if self._lines['E_STOP_RESET'].get_value() != self._last_e_stop_state: - if rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: - self._e_stop_event() - self._e_stop_interrupt_time = float('inf') - elif math.isinf(self._e_stop_interrupt_time): - self._e_stop_interrupt_time = rospy.get_time() + shdn_init_val = self._lines['SHDN_INIT'].get_value() + if shdn_init_val and math.isinf(self._chrg_sense_pressed_time): + self._chrg_sense_pressed_time = rospy.get_time() + elif rospy.get_time() - self._chrg_sense_pressed_time > self._gpio_wait: + if shdn_init_val: + rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') + self._publish_io_state('power_button', True) + self._chrg_sense_pressed_time = float('inf') + + if ( + math.isinf(self._e_stop_interrupt_time) + and self._lines['E_STOP_RESET'].get_value() != self._last_e_stop_state + ): + self._e_stop_interrupt_time = rospy.get_time() + elif rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: + self._e_stop_event() + self._e_stop_interrupt_time = float('inf') def _watchdog_cb(self, *args) -> None: self._watchdog() @@ -211,14 +216,14 @@ def _charger_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: return res def _digital_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(req.data, 'VDIG_OFF', 'Digital power enable') + res = self._set_bool_srv_handle(not req.data, 'VDIG_OFF', 'Digital power enable') if res.success: self._publish_io_state('digital_power', req.data) return res def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: with self._e_stop_lock: - if not self._lines['E_STOP_RESET'].get_value(): + if self._lines['E_STOP_RESET'].get_value(): return TriggerResponse(True, 'E-STOP is not active, reset is not needed') elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: return TriggerResponse( @@ -233,7 +238,7 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: self._reset_e_stop() - if self._lines['E_STOP_RESET'].get_value(): + if not self._lines['E_STOP_RESET'].get_value(): self._watchdog.turn_off() return TriggerResponse( False, @@ -294,24 +299,23 @@ def _reset_e_stop(self) -> None: self._clearing_e_stop = True req_type = gpiod.LINE_REQ_DIR_OUT - req_flag = gpiod.LINE_REQ_FLAG_ACTIVE_LOW self._lines['E_STOP_RESET'].release() - self._lines['E_STOP_RESET'].request(self._node_name, type=req_type, flags=req_flag) + self._lines['E_STOP_RESET'].request(self._node_name, type=req_type) self._watchdog.turn_on() - self._lines['E_STOP_RESET'].set_value(False) + self._lines['E_STOP_RESET'].set_value(True) rospy.sleep(0.1) req_type = gpiod.LINE_REQ_DIR_IN self._lines['E_STOP_RESET'].release() - self._lines['E_STOP_RESET'].request(self._node_name, type=req_type, flags=req_flag) + self._lines['E_STOP_RESET'].request(self._node_name, type=req_type) rospy.sleep(0.1) self._clearing_e_stop = False self._e_stop_event() def _e_stop_event(self) -> None: - e_stop_state = self._lines['E_STOP_RESET'].get_value() + e_stop_state = not self._lines['E_STOP_RESET'].get_value() if e_stop_state != self._last_e_stop_state and not self._clearing_e_stop: self._last_e_stop_state = e_stop_state self._e_stop_state_pub.publish(e_stop_state) From b095daffb011379071a3738b84fc73bdda65e7e9 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Mon, 15 May 2023 14:41:24 +0000 Subject: [PATCH 09/17] Clean up code --- panther_power_control/src/power_board_node.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 7de1a4a25..88b1acc72 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -56,7 +56,7 @@ def __init__(self, name: str) -> None: 'WATCHDOG', # Watchdog pin, if PWM is on this pin Panther will work ] in_line_names = [ - 'CHRG_SENSE', # Charger sensor (1 - charger plugged in) + 'CHRG_SENSE', # Charger sensor (0 - charger connected, 1 - not connected) 'E_STOP_RESET', # Works as IN/OUT, # IN - gives info if E-stop is on (1 - off), OUT - send 1 to reset estop 'SHDN_INIT', # Shutdown Init managed by systemd service @@ -73,7 +73,6 @@ def __init__(self, name: str) -> None: self._lines['CHRG_DISABLE'].set_value(True) self._lines['AUX_PW_EN'].set_value(False) self._lines['FAN_SW'].set_value(False) - # This pin is inverted so default has to be high self._lines['VDIG_OFF'].set_value(False) self._watchdog = Watchdog(self._lines['WATCHDOG']) @@ -81,7 +80,7 @@ def __init__(self, name: str) -> None: self._gpio_wait = 0.05 # seconds self._last_e_stop_state = not self._lines['E_STOP_RESET'].get_value() - self._e_stop_interrupt_time = float('inf') + self._e_stop_pressed_time = float('inf') self._chrg_sense_pressed_time = float('inf') self._cmd_vel_msg_time = rospy.get_time() @@ -192,13 +191,13 @@ def _publish_pin_state_cb(self, *args) -> None: self._chrg_sense_pressed_time = float('inf') if ( - math.isinf(self._e_stop_interrupt_time) + math.isinf(self._e_stop_pressed_time) and self._lines['E_STOP_RESET'].get_value() != self._last_e_stop_state ): - self._e_stop_interrupt_time = rospy.get_time() - elif rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: + self._e_stop_pressed_time = rospy.get_time() + elif rospy.get_time() - self._e_stop_pressed_time > self._gpio_wait: self._e_stop_event() - self._e_stop_interrupt_time = float('inf') + self._e_stop_pressed_time = float('inf') def _watchdog_cb(self, *args) -> None: self._watchdog() @@ -233,7 +232,8 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: elif self._can_net_err: return TriggerResponse( False, - 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', + 'E-STOP reset failed, unable to communicate with motor controllers! ' + 'Please check connection with motor controllers.', ) self._reset_e_stop() From 5c85bbb82640a7253470d569b180d5e332a17dc6 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Thu, 22 Jun 2023 15:26:21 +0000 Subject: [PATCH 10/17] Add motors enable service for pth 1.0x --- panther_power_control/src/relays_node.py | 78 ++++++++++++++++++++---- 1 file changed, 66 insertions(+), 12 deletions(-) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index d69e87e7b..60688bb58 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -8,6 +8,7 @@ from geometry_msgs.msg import Twist from std_msgs.msg import Bool +from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse from panther_msgs.msg import DriverState, IOState @@ -18,7 +19,8 @@ def __init__(self, name: str) -> None: self._node_name = name rospy.init_node(self._node_name, anonymous=False) - self._lock = Lock() + self._motors_lock = Lock() + self._e_stop_lock = Lock() chip = gpiod.Chip('gpiochip0', gpiod.Chip.OPEN_BY_NAME) @@ -30,6 +32,7 @@ def __init__(self, name: str) -> None: self._e_stop_state = not self._lines['STAGE2_INPUT'].get_value() self._cmd_vel_msg_time = rospy.get_time() self._can_net_err = True + self._motor_enabled = True # ------------------------------- # Publishers @@ -70,6 +73,17 @@ def __init__(self, name: str) -> None: self._e_stop_trigger_server = rospy.Service( 'hardware/e_stop_trigger', Trigger, self._e_stop_trigger_cb ) + self._motor_enable_server = rospy.Service( + 'hardware/motor_enable', SetBool, self._motor_enable_cb + ) + + # ------------------------------- + # Service clients + # ------------------------------- + + self._reset_roboteq_script_client = rospy.ServiceProxy( + 'driver/reset_roboteq_script', Trigger + ) # ------------------------------- # Timers @@ -86,18 +100,18 @@ def __del__(self): for line in self._lines.values(): line.release() - def _cmd_vel_cb(self, *args) -> None: - with self._lock: + def _cmd_vel_cb(self, msg: Twist) -> None: + with self._e_stop_lock: self._cmd_vel_msg_time = rospy.get_time() def _motor_controllers_state_cb(self, msg: DriverState) -> None: - with self._lock: + with self._e_stop_lock: self._can_net_err = any( {msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err} ) def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: + with self._e_stop_lock: if rospy.get_time() - self._cmd_vel_msg_time <= 2.0: return TriggerResponse( False, @@ -106,7 +120,8 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: elif self._can_net_err: return TriggerResponse( False, - 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', + 'E-STOP reset failed, unable to communicate with motor controllers! ' + 'Please check connection with motor controllers.', ) self._e_stop_state = False @@ -114,17 +129,56 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(True, 'E-STOP reset successful') def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: + with self._e_stop_lock: self._e_stop_state = True self._e_stop_state_pub.publish(self._e_stop_state) return TriggerResponse(True, 'E-SROP triggered successful') + def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: + with self._motors_lock: + stage2_value = self._lines['STAGE2_INPUT'].get_value() + if not stage2_value: + self._motor_enabled = req.data + return SetBoolResponse( + not req.data, + f'Three-position Main switch is in Stage 1. ' + f'Motors are {"already " if not req.data else ""}powered off', + ) + + if self._motor_enabled == req.data: + return SetBoolResponse(True, f'Motor state already set to: {req.data}') + + self._motor_enabled = req.data + self._lines['MOTOR_ON'].set_value(req.data) + + if req.data: + # wait for motor drivers to power on + rospy.sleep(rospy.Duration(2.0)) + try: + reset_script_res = self._reset_roboteq_script_client.call() + if not reset_script_res.success: + self._lines['MOTOR_ON'].set_value(False) + self._publish_io_state('motor_on', False) + return SetBoolResponse(reset_script_res.success, reset_script_res.message) + except rospy.ServiceException as e: + self._lines['MOTOR_ON'].set_value(False) + self._publish_io_state('motor_on', False) + return SetBoolResponse(False, f'Failed to reset roboteq script: {e}') + + return SetBoolResponse(True, f'Motors {"enabled" if req.data else "disabled"}') + def _set_motor_state_timer_cb(self, *args) -> None: - motor_state = self._lines['STAGE2_INPUT'].get_value() - self._lines['MOTOR_ON'].set_value(motor_state) - if self._io_state.motor_on != motor_state: - self._io_state.motor_on = motor_state - self._io_state_pub.publish(self._io_state) + with self._motors_lock: + motor_state = self._lines['STAGE2_INPUT'].get_value() + should_enable_motors = motor_state and self._motor_enabled + self._lines['MOTOR_ON'].set_value(should_enable_motors) + self._publish_io_state('motor_on', should_enable_motors) + + def _publish_io_state(self, attribute: str, val: bool) -> None: + last_msg = self._io_state_pub.impl.latch + if getattr(last_msg, attribute) != val: + setattr(last_msg, attribute, val) + self._io_state_pub.publish(last_msg) def main(): From becb57a5bf14df0d6e3ca2e6062e9f09fece4737 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Thu, 22 Jun 2023 15:33:45 +0000 Subject: [PATCH 11/17] Simplify state publishing --- panther_power_control/src/relays_node.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 60688bb58..0def1eec6 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -158,11 +158,11 @@ def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: reset_script_res = self._reset_roboteq_script_client.call() if not reset_script_res.success: self._lines['MOTOR_ON'].set_value(False) - self._publish_io_state('motor_on', False) + self._publish_motor_state(False) return SetBoolResponse(reset_script_res.success, reset_script_res.message) except rospy.ServiceException as e: self._lines['MOTOR_ON'].set_value(False) - self._publish_io_state('motor_on', False) + self._publish_motor_state(False) return SetBoolResponse(False, f'Failed to reset roboteq script: {e}') return SetBoolResponse(True, f'Motors {"enabled" if req.data else "disabled"}') @@ -172,12 +172,12 @@ def _set_motor_state_timer_cb(self, *args) -> None: motor_state = self._lines['STAGE2_INPUT'].get_value() should_enable_motors = motor_state and self._motor_enabled self._lines['MOTOR_ON'].set_value(should_enable_motors) - self._publish_io_state('motor_on', should_enable_motors) + self._publish_motor_state(should_enable_motors) - def _publish_io_state(self, attribute: str, val: bool) -> None: + def _publish_motor_state(self, val: bool) -> None: last_msg = self._io_state_pub.impl.latch - if getattr(last_msg, attribute) != val: - setattr(last_msg, attribute, val) + if last_msg.motor_on != val: + last_msg.motor_on = val self._io_state_pub.publish(last_msg) From cbc14d9fa1982681f05760abd204af7c3c1228ef Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Jul 2023 11:50:04 +0000 Subject: [PATCH 12/17] Main switch overwrites motores enable --- panther_power_control/src/relays_node.py | 56 +++++++++++++++--------- 1 file changed, 36 insertions(+), 20 deletions(-) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 471b9aa6a..76f667d60 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -18,8 +18,8 @@ def __init__(self, name: str) -> None: self._node_name = name rospy.init_node(self._node_name, anonymous=False) - self._motors_lock = Lock() self._e_stop_lock = Lock() + self._motors_lock = Lock() line_names = { 'MOTOR_ON': False, # Used to enable motors @@ -43,6 +43,7 @@ def __init__(self, name: str) -> None: ) self._e_stop_state = not self._lines['STAGE2_INPUT'].get_value() + self._last_motor_state = self._lines['STAGE2_INPUT'].get_value() self._cmd_vel_msg_time = rospy.get_time() self._can_net_err = True self._motor_enabled = True @@ -58,7 +59,7 @@ def __init__(self, name: str) -> None: self._e_stop_state_pub.publish(self._e_stop_state) self._io_state = IOState() - self._io_state.motor_on = self._lines['STAGE2_INPUT'].get_value() + self._io_state.motor_on = self._last_motor_state self._io_state.aux_power = False self._io_state.charger_connected = False self._io_state.fan = False @@ -117,7 +118,7 @@ def __del__(self): self._chip.close() def _cmd_vel_cb(self, *args) -> None: - with self._lock: + with self._e_stop_lock: self._cmd_vel_msg_time = rospy.get_time() def _motor_controllers_state_cb(self, msg: DriverState) -> None: @@ -127,10 +128,10 @@ def _motor_controllers_state_cb(self, msg: DriverState) -> None: ) def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: + with self._e_stop_lock: if not self._e_stop_state: return TriggerResponse(True, 'E-STOP is not active, reset is not needed') - + if rospy.get_time() - self._cmd_vel_msg_time <= 2.0: return TriggerResponse( False, @@ -148,31 +149,33 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(True, 'E-STOP reset successful') def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: + with self._e_stop_lock: if self._e_stop_state: return TriggerResponse(True, 'E-SROP already triggered') - + self._e_stop_state = True self._e_stop_state_pub.publish(self._e_stop_state) return TriggerResponse(True, 'E-SROP triggered successful') def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: with self._motors_lock: - stage2_value = self._lines['STAGE2_INPUT'].get_value() - if not stage2_value: + if not self._lines['STAGE2_INPUT'].get_value(): self._motor_enabled = req.data return SetBoolResponse( not req.data, f'Three-position Main switch is in Stage 1. ' - f'Motors are {"already " if not req.data else ""}powered off', + f'Motors are {"already " if not req.data else ""}disabled', ) - if self._motor_enabled == req.data: - return SetBoolResponse(True, f'Motor state already set to: {req.data}') + # if both values are equal + if not (self._motor_enabled ^ req.data): + return SetBoolResponse( + True, f'Motors are already {"enabled" if self._motor_enabled else "disabled"}' + ) - self._motor_enabled = req.data self._lines['MOTOR_ON'].set_value(req.data) + # if motors not enabled and requested to power on if req.data: # wait for motor drivers to power on rospy.sleep(rospy.Duration(2.0)) @@ -180,20 +183,33 @@ def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: reset_script_res = self._reset_roboteq_script_client.call() if not reset_script_res.success: self._lines['MOTOR_ON'].set_value(False) - self._publish_motor_state(False) return SetBoolResponse(reset_script_res.success, reset_script_res.message) except rospy.ServiceException as e: self._lines['MOTOR_ON'].set_value(False) - self._publish_motor_state(False) return SetBoolResponse(False, f'Failed to reset roboteq script: {e}') - return SetBoolResponse(True, f'Motors {"enabled" if req.data else "disabled"}') + self._motor_enabled = req.data + self._publish_motor_state(req.data) + return SetBoolResponse( + True, f'Motors {"enabled" if self._motor_enabled else "disabled"}' + ) def _set_motor_state_timer_cb(self, *args) -> None: - motor_state = self._lines['STAGE2_INPUT'].get_value() - self._lines['MOTOR_ON'].set_value(motor_state) - if self._io_state.motor_on != motor_state: - self._io_state.motor_on = motor_state + with self._motors_lock: + motor_state = self._lines['STAGE2_INPUT'].get_value() + # if switch changes from off to on overwrite service value + if not self._last_motor_state and motor_state: + self._motor_enabled = True + + self._last_motor_state = motor_state + + state_to_set = motor_state and self._motor_enabled + self._lines['MOTOR_ON'].set_value(state_to_set) + self._publish_motor_state(state_to_set) + + def _publish_motor_state(self, val: bool) -> None: + if self._io_state.motor_on != val: + self._io_state.motor_on = val self._io_state_pub.publish(self._io_state) From d64f308fea04d2709157c3ec5407f54934e4e8f1 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Jul 2023 12:06:16 +0000 Subject: [PATCH 13/17] Simplify logic and modify string on powered off motors --- panther_power_control/src/relays_node.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 76f667d60..0e6dea3f5 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -160,15 +160,14 @@ def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: with self._motors_lock: if not self._lines['STAGE2_INPUT'].get_value(): - self._motor_enabled = req.data + self._motor_enabled = False return SetBoolResponse( not req.data, - f'Three-position Main switch is in Stage 1. ' - f'Motors are {"already " if not req.data else ""}disabled', + f'Motors are {"already " if not req.data else ""}disabled. ', + f'(Main switch set to Stage 1)', ) - # if both values are equal - if not (self._motor_enabled ^ req.data): + if self._motor_enabled == req.data: return SetBoolResponse( True, f'Motors are already {"enabled" if self._motor_enabled else "disabled"}' ) From 5dbc69bd6490de124f7b0cf2607fe0dcfdc4488b Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Jul 2023 12:10:18 +0000 Subject: [PATCH 14/17] Remove reseting the script --- panther_power_control/src/relays_node.py | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 0e6dea3f5..93350cb8a 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -172,26 +172,10 @@ def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: True, f'Motors are already {"enabled" if self._motor_enabled else "disabled"}' ) - self._lines['MOTOR_ON'].set_value(req.data) - - # if motors not enabled and requested to power on - if req.data: - # wait for motor drivers to power on - rospy.sleep(rospy.Duration(2.0)) - try: - reset_script_res = self._reset_roboteq_script_client.call() - if not reset_script_res.success: - self._lines['MOTOR_ON'].set_value(False) - return SetBoolResponse(reset_script_res.success, reset_script_res.message) - except rospy.ServiceException as e: - self._lines['MOTOR_ON'].set_value(False) - return SetBoolResponse(False, f'Failed to reset roboteq script: {e}') - self._motor_enabled = req.data + self._lines['MOTOR_ON'].set_value(req.data) self._publish_motor_state(req.data) - return SetBoolResponse( - True, f'Motors {"enabled" if self._motor_enabled else "disabled"}' - ) + return SetBoolResponse(True, f'Motors {"enabled" if req.data else "disabled"}') def _set_motor_state_timer_cb(self, *args) -> None: with self._motors_lock: From b59619013ba41d3b1cec7dd3ca92f284bb483bdf Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Jul 2023 12:20:42 +0000 Subject: [PATCH 15/17] Fix string formatting --- panther_power_control/src/relays_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 93350cb8a..4d952cc08 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -163,8 +163,8 @@ def _motor_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: self._motor_enabled = False return SetBoolResponse( not req.data, - f'Motors are {"already " if not req.data else ""}disabled. ', - f'(Main switch set to Stage 1)', + f'Motors are {"already " if not req.data else ""}disabled. ' + + '(Main switch set to Stage 1)', ) if self._motor_enabled == req.data: From 1f0a26e7f02467bed70035b7d019815bf6f8e819 Mon Sep 17 00:00:00 2001 From: Kotochleb Date: Wed, 5 Jul 2023 13:45:30 +0000 Subject: [PATCH 16/17] Update README --- panther_power_control/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_power_control/README.md b/panther_power_control/README.md index 4a2389f0e..6e8de32ee 100644 --- a/panther_power_control/README.md +++ b/panther_power_control/README.md @@ -52,4 +52,4 @@ This node is responsible for power management using relays. Available in Panther - `/panther/hardware/e_stop_reset` [*std_srvs/Trigger*]: reset emergency stop. - `/panther/hardware/e_stop_trigger` [*std_srvs/Trigger*]: trigger emergency stop. - +- `/panther/hardware/motor_enable` [*std_srvs/SetBool*]: enable or disable motor drivers. Acts in conjunction with the three-position Main switch. Motors can not be enabled if switch is in Stage 1. In case of switch transitioning from Stage 1 to Stage 2, state set by service will be overwritten and motors will be enabled. From 88736e7a9c9d9d3bdc78976f99dabd79c7c9e8e1 Mon Sep 17 00:00:00 2001 From: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> Date: Thu, 6 Jul 2023 16:36:22 +0200 Subject: [PATCH 17/17] Update panther_power_control/src/relays_node.py Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- panther_power_control/src/relays_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 4d952cc08..3ab37cf82 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -190,9 +190,9 @@ def _set_motor_state_timer_cb(self, *args) -> None: self._lines['MOTOR_ON'].set_value(state_to_set) self._publish_motor_state(state_to_set) - def _publish_motor_state(self, val: bool) -> None: - if self._io_state.motor_on != val: - self._io_state.motor_on = val + def _publish_motor_state(self, desired_state: bool) -> None: + if self._io_state.motor_on != desired_state: + self._io_state.motor_on = desired_state self._io_state_pub.publish(self._io_state)