From 94c4bfbec1ebba64704831e9b0455b14876a6b63 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Mon, 24 Apr 2023 14:32:47 +0200 Subject: [PATCH] Ros1 motor enable srv (#113) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * add motors enable srv * add reseting can callback times * disable motors if script reset fails * working solution * fix * IO initial state pth1.05 * remove unneccessary method * fix type * update README * Update panther_power_control/README.md Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * review changes --------- Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> --- panther_driver/src/driver_node.py | 29 ++++++++++---- panther_power_control/README.md | 10 ++++- panther_power_control/src/power_board_node.py | 40 +++++++++++++++++++ panther_power_control/src/relays_node.py | 34 +++++++++++----- 4 files changed, 93 insertions(+), 20 deletions(-) diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index aef920109..1afee362c 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -13,8 +13,7 @@ from std_msgs.msg import Bool from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse -from panther_msgs.msg import DriverState, FaultFlag, ScriptFlag, RuntimeError - +from panther_msgs.msg import DriverState, FaultFlag, IOState, RuntimeError, ScriptFlag from panther_can import PantherCANSDO, PantherCANPDO from panther_kinematics import PantherDifferential, PantherMecanum @@ -96,6 +95,7 @@ def __init__(self, name: str) -> None: self._driver_state_timer_period = 1.0 / 10.0 # freq. 10 Hz self._safety_timer_period = 1.0 / 20.0 # freq. 20 Hz self._time_last = rospy.Time.now() + self._motor_off_last_time = rospy.Time.now() self._cmd_vel_command_last_time = rospy.Time.now() self._cmd_vel_timeout = 0.2 @@ -107,6 +107,7 @@ def __init__(self, name: str) -> None: self._motors_effort = [0.0, 0.0, 0.0, 0.0] self._e_stop_cliented = False + self._motor_on = False self._stop_cmd_vel_cb = True # ------------------------------- @@ -217,6 +218,9 @@ def __init__(self, name: str) -> None: self._cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, self._cmd_vel_cb, queue_size=1) self._e_stop_sub = rospy.Subscriber('hardware/e_stop', Bool, self._e_stop_cb, queue_size=1) + self._io_state_sub = rospy.Subscriber( + 'hardware/io_state', IOState, self._io_state_cb, queue_size=1 + ) # ------------------------------- # Service clients @@ -351,11 +355,19 @@ def _safety_timer_cb(self, *args) -> None: self._trigger_panther_e_stop() self._stop_cmd_vel_cb = True - rospy.logerr_throttle( - 10.0, - f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure). ' - f'Please ensure that controllers are powered on.', - ) + if not self._motor_on: + rospy.logwarn_throttle( + 60.0, f'[{rospy.get_name()}] Motor controllers are not powered on' + ) + self._motor_off_last_time = rospy.Time.now() + else: + # wait for motor drivers to power on before logging an error + if rospy.Time.now() - self._motor_off_last_time < rospy.Duration(2.0): + return + rospy.logerr_throttle( + 10.0, + f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure)', + ) elif self._e_stop_cliented: self._stop_cmd_vel_cb = True else: @@ -373,6 +385,9 @@ def _cmd_vel_cb(self, data: Twist) -> None: self._cmd_vel_command_last_time = rospy.Time.now() + def _io_state_cb(self, io_state: IOState) -> None: + self._motor_on = io_state.motor_on + def _reset_roboteq_script_cb(self, req: TriggerRequest) -> TriggerResponse: try: if self._panther_can.restart_roboteq_script(): diff --git a/panther_power_control/README.md b/panther_power_control/README.md index a6c2d66bc..4a2389f0e 100644 --- a/panther_power_control/README.md +++ b/panther_power_control/README.md @@ -17,6 +17,7 @@ Node responsible for management of the safety board and the power board. Availab - `charger_enabled` is related to service `charger_enable`, - `digital_power` is related to service `digital_power_enable`, - `fan` is related to service `fan_enable`, + - `motor_on` is related to service `motor_enable`, - `power_button` indicates if power button is pressed. #### Subscribes @@ -31,7 +32,11 @@ Node responsible for management of the safety board and the power board. Availab - `/panther/hardware/e_stop_reset` [*std_srvs/Trigger*]: reset emergency stop. - `/panther/hardware/e_stop_trigger` [*std_srvs/Trigger*]: trigger emergency stop. - `/panther/hardware/fan_enable` [*std_srvs/SetBool*]: enable or disable internal fan. -- `/panther/hardware/motors_enable` [*std_srvs/SetBool*]: enable or disable motor drivers. +- `/panther/hardware/motor_enable` [*std_srvs/SetBool*]: enable or disable motor drivers. + +#### Service clients + +- `/panther/driver/reset_roboteq_script` [*std_srvs/Trigger*]: used to reset Roboteq drivers script when enabling motor drivers. ### relays_node.py @@ -40,7 +45,8 @@ This node is responsible for power management using relays. Available in Panther #### Publishes - `/panther/hardware/e_stop` [*std_msgs/Bool*, *latched*]: the current state of the emulated emergency stop. -- `/panther/hardware/motor_on` [*std_msgs/Bool*, *latched*]: informs if motor controllers are on. +- `/panther/hardware/io_state` [*panther_msgs/IOState*, *latched*]: publishes state of panther IO pins. Used for driver compatybility with Panther version 1.06 and below. Message fields with real hardware representation are: + - `motor_on` indicates if motor drivers are powered on. #### Services advertised diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 2e26d5c49..2b6c606b8 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -89,6 +89,7 @@ def __init__(self, name: str) -> None: 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) self._io_state_pub.publish(io_state) # ------------------------------- @@ -120,6 +121,17 @@ def __init__(self, name: str) -> None: 'hardware/e_stop_trigger', Trigger, self._e_stop_trigger_cb ) self._fan_enable_server = rospy.Service('hardware/fan_enable', SetBool, self._fan_enable_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 @@ -229,6 +241,34 @@ def _fan_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: 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): + 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') + if not res.success: + return res + + self._publish_io_state('motor_on', 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: + res = self._set_bool_srv_handle(False, self._pins.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) + except rospy.ServiceException as e: + res = self._set_bool_srv_handle(False, self._pins.DRIVER_EN, 'Motor drivers enable') + if res.success: + self._publish_io_state('motor_on', False) + return SetBoolResponse(False, f'Failed to reset roboteq script: {e}') + + return res + def _set_bool_srv_handle(self, value: bool, pin: int, name: str) -> SetBoolResponse: rospy.logdebug(f'[{rospy.get_name()}] Requested {name} = {value}') self._write_to_pin(pin, value) diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index b3b4d3940..53509cdeb 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -9,7 +9,7 @@ from std_msgs.msg import Bool from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse -from panther_msgs.msg import DriverState +from panther_msgs.msg import DriverState, IOState @dataclass @@ -36,8 +36,21 @@ def __init__(self, name: str) -> None: # Publishers # ------------------------------- - self._motor_on_pub = rospy.Publisher('hardware/motor_on', Bool, queue_size=1, latch=True) 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) + + # init e-stop state + 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.aux_power = False + self._io_state.charger_connected = False + self._io_state.fan = False + self._io_state.power_button = False + self._io_state.digital_power = True + self._io_state.charger_enabled = False + self._io_state_pub.publish(self._io_state) # ------------------------------- # Subscribers @@ -64,11 +77,9 @@ def __init__(self, name: str) -> None: # ------------------------------- # check motor state at 10 Hz - self._set_motor_state_timer = rospy.Timer(rospy.Duration(0.1), self._set_motor_state_timer_cb) - - # init e-stop state - self._e_stop_state_pub.publish(self._e_stop_state) - self._motor_on_pub.publish(GPIO.input(self._pins.STAGE2_INPUT)) + self._set_motor_state_timer = rospy.Timer( + rospy.Duration(0.1), self._set_motor_state_timer_cb + ) rospy.loginfo(f'[{rospy.get_name()}] Node started') @@ -89,7 +100,7 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: False, 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', ) - + self._e_stop_state = False self._e_stop_state_pub.publish(self._e_stop_state) return TriggerResponse(True, 'E-STOP reset successful') @@ -97,13 +108,14 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: self._e_stop_state = True self._e_stop_state_pub.publish(self._e_stop_state) - return TriggerResponse(True, 'E-SROP triggered successful') + return TriggerResponse(True, 'E-STOP 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) - if motor_state != self._motor_on_pub.impl.latch.data: - self._motor_on_pub.publish(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)