Project ROS

Getting Started with ROS

[INSERT TUTORIAL WALKTHROUGH HERE]

For reference, a list of commonly-used, core ROS commands and functions have been compiled for your convenience and can be accessed HERE.

Controlling the Donkey Car Using ROS Nodes

Utilizing the Donkeycar framework, in theory, should be simple: a direct publisher-subscriber connection should suffice to directly operate and control the Donkeycar. To accomplish this, the following is implemented:

  > A publisher node to receive specified keyboard inputs and post them to the controls topic for subscribers to draw from.
  > A subscriber node to receive values posted to the controls topic and send them to the motors to operate the Donkeycar.

Specified details are described below. All of the following code can be found on GitHub at the following repository:

Winter 2018 "driving_donkey" ROS Package

Main Publisher Node: keyboard_inputs

Our central publisher node is named keyboard_inputs.py, and utilizes Python as the language of operation. This publisher node is organized such that additional keyboard-input features can easily be implemented for future projects.

The backbone of this node is built from the curses library, a powerful set of functions leveraging user keyboard inputs. Please note that this publisher node utilizes the curses library, so if you do not have it downloaded on your RPi (or similarly, if you receive an error stating that "import curses" failed), run the command:

  pip install curses

This will install the curses library on your machine.

Documentation for the curses library can be found HERE. We highly recommend taking a look through the various functions available through this library, to understand the scope of its functionality as well as to inspire potential add-ons to the current set of curses functions implemented in our publisher node.

In general, this program takes in a specified keyboard input (namely, the arrow keys, q, or spacebar) to modify the servo and speed control motors' PWM values. A list of currently-accepted keyboard inputs is shown below:

  > q: Shutsdown the user interface and node. NOTE: This command must be used in place of CTRL-C.
     Attempting to use CTRL-C to exit the node will effectively crash the terminal window.
  > spacebar: Fast stop. Pressing this immediately resets the speed control PWM to its neutral value.
  > left/right arrow: Decreases/Increases the servo (steering) motor PWM values posted to the controls topic
  > up/down arrow: Decreases/increases the speed control motor PWM values posted to the controls topic

Let's break down the code to see exactly what is going on. The first thing to note is that this file references the config.py file utilized in our traditional Donkey framework:

  from config import *

From here, the script pulls the max/neutral/min steering/throttle PWM values determined from the original Donkeycar calibration. Make sure an (updated) copy of this file identical to the one in your d2 folder exists in the same directory where keyboard_input.py exists. The next thing to note is that the type of message we'll be uploading to the controls topic is a double-precision float set of arrays:

  from std_msgs.msg import Int16MultiArray

This allows us to post all the PWM values simultaneously to our topic without having to send multiple messages. Each PWM value will be held in a different element of the array. This also allows to easily expand the scope of our message to send additional commands to the topic, should future features be implemented (for example, a binary on/off switch to initialize stereo vision). The next few sets of lines initialize the curses UI. Specific documentation of these commands may be found in the curses library link posted above. The next important line is the initialization of our safety timeout value:

  no_response_max_time = 5000 # milliseconds

Every time a keyboard input is triggered, a built-in counter within the curses framework resets to 0. This timer then counts the number of milliseconds before another keyboard input occurs; if no input is detected within the specified time (in this case, 5000 ms = 5 seconds), the interrupt-style function screen.getch() outputs -1 (in contrast to 'q', 'b', or any other character on the keyboard). This allows us to create a case that does something in the case that we experience a user timeout, i.e.: screen.getch() == -1. For our purposes, this will be a safety switch that automatically resets both the steering and throttle to its neutral positions (more on that below). The next important lines are the initialization of important parameters:

  rate = rospy.Rate(100) # 100hz
  steering_neutral = round((STEERING_RIGHT_PWM + STEERING_LEFT_PWM)/2.0)
  throttle_neutral = THROTTLE_STOPPED_PWM + 10
  throttle_max = THROTTLE_STOPPED_PWM + THROTTLE_RANGE
  throttle_min = THROTTLE_STOPPED_PWM - THROTTLE_RANGE
  steering_scale = 15

Our rospy rate is set to 100Hz -- thus messages are being published to the controls topic at a maximum rate of 100/second. This, of course, will rarely happen since messages are only going to be published when the user presses a key! Steering_neutral, throttle_neutral, throttle_max, throttle_min are all straightforward -- as mentioned earlier, these commands draw upon the config.py values to determine the bounds of potential PWM values to be posted to the topic. Lastly, steering_scale determines how much the PWM changes each time the user presses the right/left arrow keys. Next is our initialization of our publisher message:

  pwm_input = Int16MultiArray()
  pwm_input.data = [steering_neutral,throttle_neutral]

We initialize message pwm_input or type Int16MultiArray, which currently includes [steering, throttle] values set to their respective neutral values. These values will be dynamically updated and reposted to the controls topic as the user provides keyboard inputs. Additional parameters can easily be appended to the message by adding additional elements to the current array.

Now, onto the bulk of the script -- the case-based commands. In general, right/left and up/down keys do the same thing, but in a reverse direction, so only one of each will be documented here. The first part of the right key case checks to see if the current PWM is already maxed out -- if it is, nothing changes:

  elif char == curses.KEY_RIGHT:
       # print doesn't work with curses, use addstr instead
       if pwm_input.data[0] >= STEERING_RIGHT_PWM:
            # limit the steering max value (right side)
            pwm_input.data[0] = STEERING_RIGHT_PWM

If the current steering PWM falls between the MIN and MAX values, it will be changed by +steering_scale (+15):

       else:
            pwm_input.data[0] += steering_scale

The next chunk of text is purely for ease-of-use purposes. If a user is remotely controlling the car, they will be unable to visually see how far their wheels are turned -- that is, it will be hard to intuitively know how dramatic they are currently turning. Thus a visual aid was provided: Each time the right arrow is triggered, a '>' added to the screen so the user can see relatively how far over the user is turning. For example, if the node is currently posting a steering_PWM of neutral+3*steering_scale, the user would see the following on the terminal window:

  [         O>>>   ]

The reverse would be true for the user pressing the left arrow key.

The up/down cases are much more straightforward. Similar to the right/left case, the node checks to make sure the user input doesn't cause the resulting PWM throttle value to fall outside of its MIN/MAX range:

  elif char == curses.KEY_UP:
       if pwm_input.data[1] >= throttle_max:
            # limit the steering max value
            pwm_input.data[1] = throttle_max

Otherwise, the node adjusts its throttle PWM by +1

       else:
       pwm_input.data[1] += 1

Now, let's analyze the special cases. The first occurs when the user presses spacebar. This immediately resets the throttle to zero:

  elif char == ord(' '):
       # hard stop -- hitting spacebar resets the throttle PWM to neutral 
       pwm_input.data[1] = throttle_neutral

Lastly, the timeout case is utilized -- this resets both the steering and throttle PWMs to neutral:

  elif char == -1:
       # Increase the safety counter, in case connection is lost this will trigger a stop
       pwm_input.data = [steering_neutral,throttle_neutral]

In each case, the user terminal screen is updated to reflect the current state of the steering/throttle PWM values:

       screen.addstr(3, 0, \
                   'Currently outputting:'\
                   '[Steering PWM, Throttle PWM] = ')
       screen.addstr(3, 52, str(pwm_input.data))

The remainder of the code cleans up the curses platform in the case the node is exited.

LOGISTICAL CHALLENGES: We (sadly) found out that most laptop keyboards only accept 2 simultaneous keys at once, and can only read a single key continuously at a time. This holds problems when trying to quickly and dynamically control the car, such as rapidly changing steering from right to left, or trying to control both throttle and steering simultaneously (ex: holding both the right and up keys at once). This can be solved by utilizing an external/separate keyboard, but this is clearly not optimal. It would be practical to find an alternate solution to this problem.

Main Subscriber Node: controls_receiver

Our central subscriber node is named controls_receiver.py, and utilizes Python as the language of operation. The controls_receiver node makes use of the actuator.py file located in the same folder. The actuator.py file is explained in more detail in the following sub-section.

To run this node, make sure roscore is running. On the terminal:

roscore

Then run the command:

rosrun (package location) controls_receiver.py

By default, if you are using driving_donkey as the name for the package:

rosrun driving_donkey controls_receiver.py

Now, let's break down each line in the code:

import rospy

We must import rospy in order to use classes and functions for ROS.

from std_msgs.msg import Int16MultiArray as PWM

This imports the Int16MultiArray message type from the std_msgs/msg folder, and uses PWM as a nickname.

from actuator import PWMController

This imports the PWMController class from actuator.py, and allows us to send PWM to the Raspberry Pi and PWM controller.

from config import *

This imports all of our custom settings, such as the STEERING_CHANNEL, THROTTLE_STOPPED_PWM, etc.

STEERING = 0
THROTTLE = 1

This is the array position for steering or throttle in the Int16MultiArray message that is sent from the publisher.

steering_controller = PWMController(channel=STEERING_CHANNEL) 
throttle_controller = PWMController(channel=THROTTLE_CHANNEL)

Here we create an instance of PWMController to control each of the steering and throttle.

slower_downer = 0

This variable needs a better name. It is not entirely tested to actually work, but it was created in an attempt to slow down our motor, since it was too fast. Explained in detail later.

def listener():
    rospy.init_node("actuator", anonymous=True)
    print("Running actuator subscriber")
    rospy.Subscriber("controls", PWM, callback)
    rospy.spin()    

The listener function receives the messages from the publisher via the controls topic. The first line creates the node and names it actuator. The third line creates a subscriber so that we can receive PWM type messages from the controls topic. When a message is received, the function callback is called. Finally, rospy.spin() makes it so Python will not exit until this node is stopped.

def callback(data):
    global slower_downer
    rospy.loginfo(rospy.get_caller_id() + " Data received: %s", data)
   
    new_throttle = data.data[THROTTLE]
    new_steering = data.data[STEERING]

    slower_downer += 1
    slower_downer %= 10
    print("slower downer %d" % slower_downer)
    if (slower_downer == 0):
        throttle_controller.set_pwm(THROTTLE_STOPPED_PWM)
    elif (new_throttle < THROTTLE_STOPPED_PWM+THROTTLE_RANGE and new_throttle > THROTTLE_STOPPED_PWM- THROTTLE_RANGE):
        print("new throttle %d" % new_throttle)
        throttle_controller.set_pwm(new_throttle)
    
    print("new steering %d" % new_steering)
    steering_controller.set_pwm(new_steering)

This function is called whenever a message is received by the listener. We use global slower_downer to keep track of how many times slower_downer has been incremented. rospy.loginfo(rospy.get_caller_id() + " Data received: %s", data) allows us to print the data that is received from the controls topic to the terminal.

    new_throttle = data.data[THROTTLE]
    new_steering = data.data[STEERING]

This is the new PWM for both the throttle and the steering received from the controls topic.

    slower_downer += 1
    slower_downer %= 10
    print("slower downer %d" % slower_downer)
    if (slower_downer == 0):
        throttle_controller.set_pwm(THROTTLE_STOPPED_PWM)

The slower downer attempts to set the PWM to the stopped PWM every 10 times callback is called. This does not work entirely as expected, since callback is called only when a message is sent, so when we actually change the steering/throttle PWM from the keyboard_input node.

elif (new_throttle < THROTTLE_STOPPED_PWM+THROTTLE_RANGE and new_throttle > THROTTLE_STOPPED_PWM- THROTTLE_RANGE):
         print("new throttle %d" % new_throttle)
         throttle_controller.set_pwm(new_throttle)

The if condition allows us to set a maximum PWM (or minimum PWM for reverse direction) in order to prevent our car from moving too fast. The PWM for the throttle is then set to that of new_throttle.

    steering_controller.set_pwm(new_steering)

This simply sets the steering PWM to the value of new_steering.

if __name__ == '__main__':
    listener()

This simply calls the listener() function if controls_receiver.py is called from the terminal.

Controlling the Motor and Servo: actuator

The actuator.py script allows us to control the motors and servo. To use actuator.py, make sure you have the Adafruit_PCA9685 library installed. Installation instructions here.

   def __init__(self, channel, frequency=60):
       import Adafruit_PCA9685
       self.pwm = Adafruit_PCA9685.PCA9685() 
       self.pwm.set_pwm_freq(frequency)
       self.channel = channel

This creates an instance of the PWMController class. The user must provide a channel when creating an instance of this class. The user can also set a frequency, but it is set to 60 Hz by default. The first line of the function imports the Adafruit_PCA9685 library. Next we set the class variable pwm, which allows us to control the PWM at a specific channel on the PWM controller. The frequency of pwm is then set to the provided frequency. By default it is 60 Hz. Next, the channel for the PWM is set to self.channel.

   def set_pwm(self, pulse):
       self.pwm.set_pwm(self.channel, 0, pulse)

This function is a wrapper that makes it simpler to control the PWM by just providing the PWM pulse as input.

Additional Nodes

NOTE: These nodes DO NOT CURRENTLY WORK. Our team was unable to solve the logistical bug issues that resulted when attempting to run these nodes. Specific errors and their speculated causes are described below.

Challenges

One of the major problems we faced was inconsistent platform support from ROS. The version of ROS which we used, ROS Kinetic, was built for Ubuntu systems, so we ran into many issues with running it on the Raspberry Pi, which ran on a Raspbian.