Ros1 motor enable srv (#113)

* 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 <[email protected]>

* review changes

---------

Co-authored-by: Paweł Kowalski <[email protected]>

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():

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
 

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)

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,21 +100,22 @@ 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')
 
     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)