Building an Autonomous Car using a 1/10th Scale RC Car — Part 3
The first two parts went through setting up ROS and getting the sensors publishing. You can read the parts:
This part will cover sampling the RC signal with a microcontroller and reproducing it to drive the steering and motor. I will also install the ROS serial library so we can communicate to the Microprocessor through ROS nodes. This will serve as the controller for both Manual and Auto modes.
Teensy and Arduino Setup
I am going to use a Teensy 3.2 board. This board is great in that it is a small form factor, but more importantly it is a 32-bit microprocessor that runs at 72 MHz in normal mode and 96 MHz overclocked. It is Arduino compatible in that it can be programmed using the Arduino IDE along with the Teensyduino add-on software.
The Teensy can be found: https://www.pjrc.com/store/teensy32.html and the Teensyduino software: https://www.pjrc.com/teensy/teensyduino.html
I upgraded my Arduino IDE to 1.8.4 which can be downloaded from: https://www.arduino.cc/en/Main/Software
After installing the Arduino software and the Teensyduino add-on, when opening the Arduino IDE, you should be able to see the Teensy options under the Tools menu:
Reading RC signals
Wikipedia says, “RC signals are a small pulse and then long low duration. Most RC toys operate at either 27 MHz or 49 MHz. This pair of frequencies has been allocated by the FCC for basic consumer items, such as garage door openers, walkie-talkies and RC toys. Advanced RC models, such as the more sophisticated RC airplanes, use 72-MHz or 75-MHz frequencies.”
There are a couple of ways to read timing and width of the RC signal pulses.
PulseIn
From Arduino documentation, PulseIn “reads a pulse (either HIGH or LOW) on a pin. For example, if value is HIGH, pulseIn() waits for the pin to go HIGH, starts timing, then waits for the pin to go LOW and stops timing. Returns the length of the pulse in microseconds. Gives up and returns 0 if no pulse starts within a specified time out.”
The problem with PulseIn is that it blocks. What this means is that when trying to read two RC signals, they will block each other and you wouldn’t get a good measure of either signal. So, we will look at interrupts.
Interrupts
The processor at the heart of any Arduino has two different kinds of interrupts: “external”, and “pin change”. There are only two external interrupt pins on the ATmega168/328 (ie, in the Arduino Uno/Nano/Duemilanove), INT0 and INT1, and they are mapped to Arduino pins 2 and 3. These interrupts can be set to trigger on RISING or FALLING signal edges, or on low level. The triggers are interpreted by hardware, and the interrupt is very fast. The Arduino Mega has a few more external interrupt pins available.
On the other hand the pin change interrupts can be enabled on many more pins. For ATmega168/328-based Arduinos, they can be enabled on any or all 20 of the Arduino’s signal pins; on the ATmega-based Arduinos they can be enabled on 24 pins. They are triggered equally on RISING or FALLING signal edges, so it is up to the interrupt code to set the proper pins to receive interrupts, to determine what happened.
Here is a really good article on using an Arduino to read multiple channels from an RC transmitter: http://rcarduino.blogspot.co.uk/2012/04/how-to-read-multiple-rc-channels-draft.html
I leveraged the code in the multichannel file. I removed many of the comments for better readability. Below is my multichannel Arduino code:
// MultiChannels
//
// Code from: rcarduino.blogspot.com#include <Servo.h>
#include <MsTimer2.h>// Assign your channel in pins
#define THROTTLE_IN_PIN 3
#define STEERING_IN_PIN 4
#define AUX_IN_PIN 12// Assign your channel out pins
#define THROTTLE_OUT_PIN 5
#define STEERING_OUT_PIN 6
#define AUX_OUT_PIN 11// Servo objects
Servo servoThrottle;
Servo servoSteering;
Servo servoAux;// channels have new signals flags
#define THROTTLE_FLAG 1
#define STEERING_FLAG 2
#define AUX_FLAG 4// holds the update flags defined above
volatile uint8_t bUpdateFlagsShared;// shared variables are updated by the ISR and read by loop.
volatile uint16_t unThrottleInShared;
volatile uint16_t unSteeringInShared;
volatile uint16_t unAuxInShared;// To record the rising edge of a pulse in the calcInput functions
uint32_t ulThrottleStart;
uint32_t ulSteeringStart;
uint32_t ulAuxStart;// temporary variables
static uint16_t unThrottleIn;
static uint16_t unSteeringIn;
static uint16_t unAuxIn;
static uint8_t bUpdateFlags;// blink
const byte ledPin = 13;
boolean output = HIGH;void setup()
{
Serial.begin(115200);
Serial.println("RC Signal Test"); // attach servo objects
servoThrottle.attach(THROTTLE_OUT_PIN);
servoSteering.attach(STEERING_OUT_PIN);
servoAux.attach(AUX_OUT_PIN); // attach the interrupts
attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE);
attachInterrupt(STEERING_IN_PIN, calcSteering,CHANGE);
attachInterrupt(AUX_IN_PIN, calcAux,CHANGE); // setup LED blink
pinMode(ledPin, OUTPUT);
MsTimer2::set(500, flash);
MsTimer2::start();
}void loop()
{
// check if any channels have a new signal
if(bUpdateFlagsShared)
{
noInterrupts(); // turn interrupts off
bUpdateFlags = bUpdateFlagsShared;
if(bUpdateFlags & THROTTLE_FLAG)
{
unThrottleIn = unThrottleInShared;
}
if(bUpdateFlags & STEERING_FLAG)
{
unSteeringIn = unSteeringInShared;
}
if(bUpdateFlags & AUX_FLAG)
{
unAuxIn = unAuxInShared;
}
bUpdateFlagsShared = 0;
interrupts();
}
if(bUpdateFlags & THROTTLE_FLAG)
{
if(servoThrottle.readMicroseconds() != unThrottleIn)
{
servoThrottle.writeMicroseconds(unThrottleIn);
}
}
if(bUpdateFlags & STEERING_FLAG)
{
if(servoSteering.readMicroseconds() != unSteeringIn)
{
servoSteering.writeMicroseconds(unSteeringIn);
}
}
if(bUpdateFlags & AUX_FLAG)
{
if(servoAux.readMicroseconds() != unAuxIn)
{
servoAux.writeMicroseconds(unAuxIn);
}
}
bUpdateFlags = 0;
}// simple interrupt service routine
void calcThrottle()
{
// if the pin is high, its a rising edge
if(digitalRead(THROTTLE_IN_PIN) == HIGH)
{
ulThrottleStart = micros();
}
else
{
unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);
bUpdateFlagsShared |= THROTTLE_FLAG;
}
}void calcSteering()
{
if(digitalRead(STEERING_IN_PIN) == HIGH)
{
ulSteeringStart = micros();
}
else
{
unSteeringInShared = (uint16_t)(micros() - ulSteeringStart);
bUpdateFlagsShared |= STEERING_FLAG;
}
}void calcAux()
{
if(digitalRead(AUX_IN_PIN) == HIGH)
{
ulAuxStart = micros();
}
else
{
unAuxInShared = (uint16_t)(micros() - ulAuxStart);
bUpdateFlagsShared |= AUX_FLAG;
}
}void flash()
{
output = !output;
digitalWrite(ledPin, output);
}
I setup a Hobby King RC transmitter and receiver that I had and tested the input and resulting output of both of the pins setup to receive the steering and motor signals. Below are two images of max and min throttle signals from my oscilloscope:
The signal on the top is the input signal from the RC controller and the signal on the bottom is the reproduced signal from the Teensy. Using the HW interrupts to measure and then duplicating the signals with the servo library works very well. This setup with the Teensy should function well in both Manual and Auto modes.
ROS
For communication to the Teensy, we need to install a communication library. This library can be found: https://github.com/ros-drivers/rosserial
It is the rosserial library that has communication protocols for Arduinos, embedded Linux, Windows, and Xbee. We are going to download and compile the library so we could use it with the Arduino IDE and the Teensy. A good tutorial on setting up the library with Arduino is found on the ROS Wiki: http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup
In order to get the ROS serial node working, you need to change serial_node.py to an executable. This python file is located in the folder ~\catkin_ws\rosserial\rosserial_python\scripts\.
When we get ROS setup on the Teensy board, we will start the rosserial communication by running:
$ rosrun rosserial_python serial_node.py _baud:=57600 _port:=/dev/ttyACM0The options for the serial node are _baud and _port. The baud rate can be changed, but in the ros.h header file the default rate is 57600. This can be changed to 115200 if you want. The communication port is usually /dev/ttyACM0, but can be checked by watching the /dev directory as the Teensy is plugged in.
Teensy GPS and IMU sensors
I thought about setting up some of the sensors so the Teensy publishes the data instead of reading from the I2C and Serial ports on the Raspberry Pi3 or the Jetson TX-1 board. This will make the GPS and IMU sensors more portable as easily movable between robots and different controller boards.
I started with the example code from Adafruit for the GPS and IMU modules. The GPS documentation can be found: https://learn.adafruit.com/adafruit-ultimate-gps/built-in-logging?view=all#overview and the IMU module can be found: https://learn.adafruit.com/adafruit-10-dof-imu-breakout-lsm303-l3gd20-bmp180/connecting-it-up?view=all#introduction
I also setup the ROS library to be used on the Teensy board. The ROS wiki has a good tutorial here: http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup You will not need to install it if you followed the instructions in the last section, however, you will need to compile and install the ros_lib library as described in section 2.1 and 2.2 of that tutorial. After installing, you can try one of the examples and load it up on the Teensy board. You can connect to the Teensy by entering the command:
$ rosrun rosserial_python serial_node.py _baud:=57600 _port:=/dev/ttyACM0and you can check the topics being published by entering:
$ rostopic listI used the example code from the Adafruit libraries and setup a test Arduino program to test reading and publishing of the GPS and IMU sensors. Below is the Arduino code that I used:
// ROS Test
// Tests reading and publishing of GPS and IMU#if ARDUINO>=100
#include <Arduino.h> // Arduino 1.0
#else
#include <WProgram.h> // Arduino 0022
#endif// ROS includes
#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int64.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PointStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Imu.h>// Arduino libraries
#include <Wire.h>
#include <Servo.h>
#include <MsTimer2.h>
#include <Adafruit_GPS.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_10DOF.h>#define IMU_PUBLISH_RATE 10 //hz
#define GPS_PUBLISH_RATE 5 //hz// ROS messages
geometry_msgs::Twist vel_msg;
std_msgs::Int64 speed_msg;
std_msgs::Int64 steer_msg;
sensor_msgs::NavSatFix navSat_msg;
sensor_msgs::Imu imu_msg;
std_msgs::Float64 gps_vel_msg;// ROS message handlers and publishers
ros::NodeHandle nh;
ros::Publisher imuPub("imu", &imu_msg);
ros::Publisher gpsPub("gps", &navSat_msg);
ros::Publisher gpsVelPub("gps_vel", &gps_vel_msg);// Assign a unique ID to the sensors
Adafruit_10DOF dof = Adafruit_10DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_BMP085_Unified bmp = Adafruit_BMP085_Unified(18001);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);// Update with the correct SLP for accurate altitude measurements
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;// GPS definitions
#define GPSSerial Serial2
Adafruit_GPS GPS(&GPSSerial);// Set GPSECHO to 'false' to turn off echoing to the Serial console
// Set to 'true' to debug and listen to the raw GPS sentences
#define GPSECHO falseuint32_t timer = millis();// blink
const byte ledPin = 13;
boolean output = HIGH;// general variables
boolean bAutoMode = false;
boolean bSensorInit = false;
int iSensorError = 0;
sensors_event_t accel_event;
sensors_event_t mag_event;
sensors_event_t bmp_event;
sensors_vec_t orientation;
sensor_t sensor;
sensors_event_t event;
uint32_t publish_imu_time = 0;
uint32_t publish_gps_time = 0;void setup() {
// initialize ROS node
nh.initNode();
nh.getHardware()->setBaud(57600);
nh.advertise(imuPub);
nh.advertise(gpsPub);
nh.advertise(gpsVelPub);
// setup LED blink
pinMode(ledPin, OUTPUT);
MsTimer2::set(500, flash);
MsTimer2::start(); // init IMU sensor
initSensors(); // 9600 NMEA is the default baud rate for Adafruit MTK GPS's
GPS.begin(9600);
// uncomment this line to turn on RMC
GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
// uncomment this line to turn on only the "minimum" data
//GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
// For parsing data, we don't suggest using anything but
// either RMC only or RMC+GGA since
// the parser doesn't care about other sentences at this time
// Set the update rate
GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // 1 Hz update rate
// Request updates on antenna status, comment out to keep quiet
GPS.sendCommand(PGCMD_ANTENNA); while (!nh.connected()) {
nh.spinOnce();
}
}void loop() {
//this block publishes velocity based on defined rate
if ((millis() - publish_imu_time) >= 1000 / IMU_PUBLISH_RATE)
{
//publishVelocities();
mag.getEvent(&event);
imu_msg.orientation.x = event.magnetic.x;
imu_msg.orientation.y = event.magnetic.y;
imu_msg.orientation.z = event.magnetic.z;
gyro.getEvent(&event);
imu_msg.angular_velocity.x = event.gyro.x;
imu_msg.angular_velocity.y = event.gyro.y;
imu_msg.angular_velocity.z = event.gyro.z;
accel.getEvent(&event);
imu_msg.linear_acceleration.x = event.acceleration.x;
imu_msg.linear_acceleration.y = event.acceleration.y;
imu_msg.linear_acceleration.z = event.acceleration.z;
imuPub.publish(&imu_msg);
publish_imu_time = millis();
}
// read data from the GPS in the 'main loop'
char c = GPS.read(); if (GPS.newNMEAreceived()) {
if (!GPS.parse(GPS.lastNMEA()))
return;
} //this block publishes velocity based on defined rate
if ((millis() - publish_gps_time) >= (1000 / GPS_PUBLISH_RATE))
{
//publishVelocities();
navSat_msg.latitude = GPS.latitude;
navSat_msg.longitude = GPS.longitude;
navSat_msg.altitude = GPS.altitude;
gpsPub.publish(&navSat_msg);
// velocity
gps_vel_msg.data = GPS.speed;
gpsVelPub.publish(&gps_vel_msg);
publish_gps_time = millis();
}
}void flash()
{
output = !output;
digitalWrite(ledPin, output);
}void initSensors()
{
bSensorInit = true;
if(!accel.begin())
{
bSensorInit = false;
iSensorError = -1;
}
if(!mag.begin())
{
bSensorInit = false;
iSensorError = -2;
}
if(!bmp.begin())
{
bSensorInit = false;
iSensorError = -3;
}
}
After downloading this code to the Teensy, the led on the Teensy will start to flash at a 1Hz rate, or once per second. I always include this so I can tell that the Arduino or Teensy is functioning properly.
Now, to test the operation of the sensors and the connection with the Teensy. Make sure you have roscore running in a different Terminal window, and then enter the command:
$ rosrun rosserial_python serial_node.py _baud:=57600 _port:=/dev/ttyACM0
Then in another window check the ROS topics being published:
$ rostopic listThen we can listen to one of the topics by echoing:
$ rostopic echo /gpsThe output will be similar to the following:
Part 4 — Setup Jetson TX-1
The next part will talk about the setting up the Jetson TX-1 with ROS and the ROS packages like the Raspberry Pi3. I am also going to setup the camera on the Jetson board to be the onboard camera input instead of the Logitech C920 webcam.