Merge pull request #4 from srmainwaring/develop

Add a failsafe node to prevent runaway behaviour after the Arduino is reset.

README.md

@@ -423,6 +423,61 @@ Adjusting the linear velocity should cause the wheels to move forwards / backwar
 Adjusting the angular velocity should cause the corner steering joints to rotate
 and the wheels turn.
 
+#### *Test the base failsafe node [optional]*
+
+When using an Arduino to communicate with the servo bus board there
+are situations when the controller can leave the servos in a runaway
+state if you use the Arduino IDE and bootloader to upload sketches:
+
+- Arduino hardware reset
+- Arduino watchdog timer reset
+- rosserial error: 'Lost sync with device, restarting...'
+
+The Arduino may fail to run the curio firmware after the reset
+(i.e. setup()) is not called). This happens when rosserial continues
+to publish data to the Arduino via USB serial after the reset
+(i.e. rosserial_arduino has registered one or more subscribers).
+rosserial does not stop transmitting data while it attempts
+to resync, and this seems to be confusing the Arduino so that it
+never moves on from its bootloader sequence.
+
+As a result the servos are left powered on and running at
+whatever duty was last set in the Arduino control loop.
+
+The failsafe node takes advantage of the fact that communication via
+the serial header on the Lewansoul BusLinker board appears to have
+priority over the USB-to-TTL circuit. As a result we are able to run
+a background process that attempts to stop the servos which is ignored
+under usual operating conditions. However as soon as the Arduino is
+reset and stops transmitting and receiving to the servo board, the
+failsafe becomes effective and stops the servos.
+
+To use the failsafe node connect the micro USB to the
+Lewansoul BusLinker board *in addition* to the connection between
+the Arduino and the serial header:
+
+```bash
+roslaunch curio_base base_failsafe.launch port:=/dev/ttyUSB0
+```
+
+where you will need to substitute the appropriate port for your
+configuration.
+
+To test the failsafe follow the instructions above for testing the
+Ackermann steering and set the rover to move forward at half speed.
+When you press the reset button on the Arduino the drive servos
+should stop immediately. If you are monitoring the topic
+`servos/positions` you will notice the messages stop. The RX LED
+on the Arduino will continue to flash, and the `arduino_controller` node
+will report that it has lost sync and is attempting to restart. You will
+need to restart the `arduino_controller` node to re-stablish the
+connection.
+
+If you upload the curio firmware without a bootloader using ICSP then
+the firmware should run immediately after reset, the initialisation
+sequence should stop the servos without requiring the failsafe,
+and rosserial should resync automatically.
+
 ### `curio_teleop`
 
 This package is used to control the robot using a radio control setup.

ackermann_drive_controller/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>ackermann_drive_controller</name>
-    <version>0.2.1</version>
+    <version>0.2.2</version>
     <description>Ackermann drive controller for a 6 wheel drive 4 wheel steering robot</description>
     <maintainer email="[email protected]">Rhys Mainwaring</maintainer>
     <license>BSD-3-Clause</license>

curio_base/CMakeLists.txt

@@ -83,12 +83,14 @@ install(DIRECTORY include/curio_base/
 
 catkin_install_python(PROGRAMS
     nodes/curio_base_controller
+    nodes/curio_base_failsafe
     scripts/lx16a_cmd_vel_random.py
     scripts/lx16a_cmd_vel_sinusoid.py
     scripts/lx16a_cmd_vel_stepped.py
     scripts/lx16a_driver_test.py
     scripts/lx16a_encoder_filter_test.py
     scripts/lx16a_encoder_logger.py
+    scripts/lx16a_failsafe_test.py
     scripts/lx16a_mean_filter_test.py
     scripts/lx16a_odometry_test.py
     scripts/lx16a_train_classifier.py

curio_base/launch/base_failsafe.launch

@@ -0,0 +1,24 @@
+<!-- Launch the base failsafe node
+
+Parameters
+    port : str
+        The device name for the serial port
+    control_frequency : str
+        The frequency for the failsafe control loop
+-->
+<launch>
+    <arg name="port" default="/dev/ttyUSB1" />
+    <arg name="control_frequency" default="20" />
+
+    <!-- Controller for the mobile base  -->
+    <node pkg="curio_base" type="curio_base_failsafe" name="curio_base_failsafe"
+        respawn="true" output="screen">
+        <rosparam command="load" file="$(find curio_base)/config/base_controller.yaml" />
+        <rosparam subst_value="true">
+            port: $(arg port)
+        </rosparam>
+        <rosparam subst_value="true">
+            failsafe_control_frequency: $(arg control_frequency)
+        </rosparam>
+    </node>
+</launch>

curio_base/nodes/curio_base_controller

@@ -35,7 +35,7 @@
 #   POSSIBILITY OF SUCH DAMAGE.
 # 
 
-''' Base controller node
+''' Curio base controller node
 
 A mobile base controller node for a Sawppy Rover using LX-16A servos.
 

curio_base/nodes/curio_base_failsafe

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+# 
+#   Software License Agreement (BSD-3-Clause)
+#    
+#   Copyright (c) 2019 Rhys Mainwaring
+#   All rights reserved
+#    
+#   Redistribution and use in source and binary forms, with or without
+#   modification, are permitted provided that the following conditions
+#   are met:
+# 
+#   1.  Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+# 
+#   2.  Redistributions in binary form must reproduce the above
+#       copyright notice, this list of conditions and the following
+#       disclaimer in the documentation and/or other materials provided
+#       with the distribution.
+# 
+#   3.  Neither the name of the copyright holder nor the names of its
+#       contributors may be used to endorse or promote products derived
+#       from this software without specific prior written permission.
+#  
+#   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#   COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#   POSSIBILITY OF SUCH DAMAGE.
+# 
+
+''' Curio base failsafe node
+
+A mobile base failsafe node for a Sawppy Rover using LX-16A servos.
+
+See curio_base/src/base_failsafe.py for documentation.  
+'''
+
+import rospy
+import curio_base.base_failsafe
+
+if __name__ == '__main__':
+    rospy.init_node('curio_base_failsafe')
+    rospy.loginfo('Starting Curio base failsafe')
+
+    # Base failsafe
+    base_failsafe = curio_base.base_failsafe.BaseFailsafe()
+
+    # Start the control loop
+    control_frequency = 20.0
+    if rospy.has_param('~failsafe_control_frequency'):
+        control_frequency = rospy.get_param('~failsafe_control_frequency')
+
+    rospy.loginfo('Starting control loop at {} Hz'.format(control_frequency))
+    control_timer = rospy.Timer(
+        rospy.Duration(1.0 / control_frequency),
+        base_failsafe.update)
+
+    # Wait for shutdown
+    rospy.spin()

curio_base/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>curio_base</name>
-    <version>0.2.1</version>
+    <version>0.2.2</version>
     <description>Hardware controllers for Curio, a Swappy Rover</description>
     <maintainer email="[email protected]">Rhys Mainwaring</maintainer>
     <license>BSD-3-Clause</license>

curio_base/scripts/lx16a_failsafe_test.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+# 
+#   Software License Agreement (BSD-3-Clause)
+#    
+#   Copyright (c) 2019 Rhys Mainwaring
+#   All rights reserved
+#    
+#   Redistribution and use in source and binary forms, with or without
+#   modification, are permitted provided that the following conditions
+#   are met:
+# 
+#   1.  Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+# 
+#   2.  Redistributions in binary form must reproduce the above
+#       copyright notice, this list of conditions and the following
+#       disclaimer in the documentation and/or other materials provided
+#       with the distribution.
+# 
+#   3.  Neither the name of the copyright holder nor the names of its
+#       contributors may be used to endorse or promote products derived
+#       from this software without specific prior written permission.
+#  
+#   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#   COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#   POSSIBILITY OF SUCH DAMAGE.
+# 
+
+''' Lewansoul LX-16A failsafe test.
+
+This test node connects to the Lewansoul Servo BusLinker board via USB
+and attempts to stop all servos in the WHEEL_SERVO_IDS list at the
+rate given by CONTROL_FREQUENCY.
+
+See curio_base/base_failsafe.py for motivation and more detail.
+
+Here is a guide to Arduino bootloaders:
+https://www.instructables.com/id/Overview-the-Arduino-sketch-uploading-process-and-/
+'''
+
+import curio_base.lx16a_driver
+import rospy
+import serial
+
+SERVO_SERIAL_PORT   = '/dev/cu.wchusbserialfd5110'
+SERVO_BAUDRATE      = 115200
+SERVO_TIMEOUT       = 1.0 # [s]
+NUM_WHEELS          = 6
+WHEEL_SERVO_IDS     = [11, 12, 13, 21, 22, 23]
+CONTROL_FREQUENCY   = 20  # [Hz]
+
+class LX16AFailsafe(object):
+
+    def __init__(self):
+        # Initialise servo driver
+        self._servo_driver = curio_base.lx16a_driver.LX16ADriver()
+        self._servo_driver.set_port(SERVO_SERIAL_PORT)
+        self._servo_driver.set_baudrate(SERVO_BAUDRATE)
+        self._servo_driver.set_timeout(SERVO_TIMEOUT)
+        self._servo_driver.open()
+
+        rospy.loginfo('Open connection to servo bus board')
+        rospy.loginfo('is_open: {}'.format(self._servo_driver.is_open()))
+        rospy.loginfo('port: {}'.format(self._servo_driver.get_port()))
+        rospy.loginfo('baudrate: {}'.format(self._servo_driver.get_baudrate()))
+        rospy.loginfo('timeout: {}'.format(self._servo_driver.get_timeout()))
+
+    def update(self, event):
+        ''' Update will stop all wheel servos.
+
+        Parameters
+        ----------
+        event : rospy.Timer
+            A rospy.Timer event.
+        '''
+        for i in range(NUM_WHEELS):
+            servo_id = WHEEL_SERVO_IDS[i]
+            self._servo_driver.motor_mode_write(servo_id, 0)
+
+if __name__ == '__main__':
+    rospy.init_node('lx16a_failsafe')
+    rospy.loginfo('Starting Lewansoul LX-16A failsafe')
+
+    # Servo failsafe
+    lx16a_failsafe = LX16AFailsafe()
+
+    # Start the control loop
+    control_frequency = CONTROL_FREQUENCY
+
+    rospy.loginfo('Starting control loop at {} Hz'.format(control_frequency))
+    control_timer = rospy.Timer(
+        rospy.Duration(1.0 / control_frequency),
+        lx16a_failsafe.update)
+
+    rospy.spin()

curio_base/src/curio_base/base_controller.py

@@ -694,7 +694,7 @@ class BaseController(object):
         Maximum servo position (servo units), has (constant 1000)
     NUM_WHEELS : int
         The number of wheel servos), has (constant 6)
-    NUM_WHEELS : int
+    NUM_STEERS : int
         The number of steering servos), has (constant 4)
 
     ROS Parameters
@@ -988,7 +988,7 @@ def __init__(self):
         ''' Constructor
         '''
 
-        rospy.loginfo('Initialising mobile base controller...')
+        rospy.loginfo('Initialising BaseController...')
 
         # Wheel geometry on a flat surface - defaults
         self._wheel_radius                = 0.060