yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 19:24:09 +05:30
Code Issues Projects Releases Wiki Activity

Merging indigo-devel into master

Browse Source
This commit is contained in:
Patrick Goebel 2015-12-07 06:10:06 -08:00
commit fcff8d342c
29 changed files with 2615 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

499
README.md
View File

@ -1,8 +1,507 @@
<<<<<<< HEAD
This is an old branch, please update to one of: This is an old branch, please update to one of:
=======
Overview
--------
This branch (indigo-devel) is intended for ROS Indigo and above, and uses the Catkin buildsystem. It may also be compatible with ROS Hydro.
This ROS stack includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services. The stack does **not** depend on ROS Serial.
>>>>>>> indigo-devel
* groovy-devel - uses rosmake * groovy-devel - uses rosmake
* hydro-devel - uses catkin, intended for ROS Hydro and later, may work on Groovy. * hydro-devel - uses catkin, intended for ROS Hydro and later, may work on Groovy.
To checkout a branch To checkout a branch
<<<<<<< HEAD
git checkout hydro-devel git checkout hydro-devel
=======
* Can also read data from generic analog and digital sensors
* Can control digital outputs (e.g. turn a switch or LED on and off)
* Support for PWM servos
* Configurable base controller if using the required hardware
The stack includes a base controller for a differential drive
robot that accepts ROS Twist messages and publishes odometry data back to
the PC. The base controller requires the use of a motor controller and encoders for reading odometry data. The current version of the stack provides support for the following base controller hardware:
* Pololu VNH5019 dual motor controller shield (http://www.pololu.com/catalog/product/2502) or Pololu MC33926 dual motor shield (http://www.pololu.com/catalog/product/2503).
* Robogaia Mega Encoder shield
(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters.
**NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega. The on-board wheel encoder counters are currently only supported by Arduino Uno.
* The library can be easily extended to include support for other motor controllers and encoder hardware or libraries.
Official ROS Documentation
--------------------------
A standard ROS-style version of this documentation can be found on the ROS wiki at:
http://www.ros.org/wiki/ros_arduino_bridge
System Requirements
-------------------
**Python Serial:** To install the python-serial package under Ubuntu, use the command:
$ sudo apt-get install python-serial
On non-Ubuntu systems, use either:
$ sudo pip install --upgrade pyserial
or
$ sudo easy_install -U pyserial
The stack should work with any Arduino-compatible controller for reading sensors and controlling PWM servos. However, to use the base controller, you will need a supported motor controller and encoder hardware as described above. If you do not have this hardware, you can still try the package for reading sensors and controlling servos. See the NOTES section at the end of this document for instructions on how to do this.
To use the base controller you must also install the appropriate libraries for your motor controller and encoders. For the Pololu VNH5019 Dual Motor Shield, the library can be found at:
https://github.com/pololu/Dual-VNH5019-Motor-Shield
For the Pololu MC33926 Dual Motor Shield, the library can be found at:
https://github.com/pololu/dual-mc33926-motor-shield
The Robogaia Mega Encoder library can be found at:
http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
These libraries should be installed in your standard Arduino
sketchbook/libraries directory.
Finally, it is assumed you are using version 1.0 or greater of the
Arduino IDE.
Preparing your Serial Port under Linux
--------------------------------------
Your Arduino will likely connect to your Linux computer as port /dev/ttyACM# or /dev/ttyUSB# where # is a number like 0, 1, 2, etc., depending on how many other devices are connected. The easiest way to make the determination is to unplug all other USB devices, plug in your Arduino, then run the command:
$ ls /dev/ttyACM*
or
$ ls /dev/ttyUSB*
Hopefully, one of these two commands will return the result you're looking for (e.g. /dev/ttyACM0) and the other will return the error "No such file or directory".
Next you need to make sure you have read/write access to the port. Assuming your Arduino is connected on /dev/ttyACM0, run the command:
$ ls -l /dev/ttyACM0
and you should see an output similar to the following:
crw-rw---- 1 root dialout 166, 0 2013-02-24 08:31 /dev/ttyACM0
Note that only root and the "dialout" group have read/write access. Therefore, you need to be a member of the dialout group. You only have to do this once and it should then work for all USB devices you plug in later on.
To add yourself to the dialout group, run the command:
$ sudo usermod -a -G dialout your_user_name
where your\_user\_name is your Linux login name. You will likely have to log out of your X-window session then log in again, or simply reboot your machine if you want to be sure.
When you log back in again, try the command:
$ groups
and you should see a list of groups you belong to including dialout.
Installation of the ros\_arduino\_bridge Stack
----------------------------------------------
$ cd ~/catkin_workspace/src
$ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
$ cd ~/catkin_workspace
$ catkin_make
The provided Arduino library is called ROSArduinoBridge and is
located in the ros\_arduino\_firmware package. This sketch is
specific to the hardware requirements above but it can also be used
with other Arduino-type boards (e.g. Uno) by turning off the base
controller as described in the NOTES section at the end of this
document.
To install the ROSArduinoBridge library, follow these steps:
$ cd SKETCHBOOK_PATH
where SKETCHBOOK_PATH is the path to your Arduino sketchbook directory.
$ cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge ROSArduinoBridge
This last command copies the ROSArduinoBridge sketch files into your sketchbook folder. The next section describes how to configure, compile and upload this sketch.
Loading the ROSArduinoBridge Sketch
-----------------------------------
* If you are using the base controller, make sure you have already installed the appropriate motor controller and encoder libraries into your Arduino sketchbook/librariesfolder.
* Launch the Arduino 1.0 IDE and load the ROSArduinoBridge sketch.
You should be able to find it by going to:
File->Sketchbook->ROSArduinoBridge
NOTE: If you don't have the required base controller hardware but
still want to try the code, see the notes at the end of the file.
Choose one of the supported motor controllers by uncommenting its #define statement and commenting out any others. By default, the Pololu VNH5019 driver is chosen.
Choose a supported encoder library by by uncommenting its #define statement and commenting out any others. At the moment, only the Robogaia Mega Encoder shield is supported and it is chosen by default.
If you want to control PWM servos attached to your controller, change
the two lines that look like this:
<pre>
//#define USE_SERVOS
#undef USE_SERVOS
</pre>
to this:
<pre>
#define USE_SERVOS
//#undef USE_SERVOS
</pre>
You must then edit the include file servos.h and change the N_SERVOS
parameter as well as the pin numbers for the servos you have attached.
* Compile and upload the sketch to your Arduino.
Firmware Commands
-----------------
The ROSArduinoLibrary accepts single-letter commands over the serial port for polling sensors, controlling servos, driving the robot, and reading encoders. These commands can be sent to the Arduino over any serial interface, including the Serial Monitor in the Arduino IDE.
**NOTE:** Before trying these commands, set the Serial Monitor baudrate to 57600 and the line terminator to "Carriage return" or "Both NL & CR" using the two pulldown menus on the lower right of the Serial Monitor window.
The list of commands can be found in the file commands.h. The current list includes:
<pre>
#define ANALOG_READ 'a'
#define GET_BAUDRATE 'b'
#define PIN_MODE 'c'
#define DIGITAL_READ 'd'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define PING 'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE 's'
#define SERVO_READ 't'
#define UPDATE_PID 'u'
#define DIGITAL_WRITE 'w'
#define ANALOG_WRITE 'x'
</pre>
For example, to get the analog reading on pin 3, use the command:
a 3
To change the mode of digital pin 3 to OUTPUT, send the command:
c 3 1
To get the current encoder counts:
e
To move the robot forward at 20 encoder ticks per second:
m 20 20
Testing your Wiring Connections
-------------------------------
On a differential drive robot, the motors are connected to the motor controller terminals with opposite polarities to each other. Similarly, the A/B leads from the encoders are connected in the reverse sense to each other. However, you still need to make sure that (a) the wheels move forward when given a positive motor speed and (b) that the encoder counts increase when the wheels move forward.
After **placing your robot on blocks**, you can use the Serial Monitor in the Arduino IDE to test both requirements. Use the 'm' command to activate the motors, the 'e' command to get the encoder counts, and the 'r' command to reset the encoders to 0. Remember that at the firmware level, motor speeds are given in encoder ticks per second so that for an encoder resolution of, say 4000 counts per wheel revolution, a command such as 'm 20 20' should move the wheels fairly slowly. (The wheels will only move for 2 seconds which is the default setting for the AUTO\_STOP\_INTERVAL.) Also remember that the first argument is the left motor speed and the second argument is the right motor speed. Similarly, when using the 'e' command, the first number returned is the left encoder count and the second number is the right encoder count.
Finally, you can use the 'r' and 'e' commands to verify the expected encoder counts by rotating the wheels by hand roughly one full turn and checking the reported counts.
Configuring the ros\_arduino\_python Node
-----------------------------------------
Now that your Arduino is running the required sketch, you can
configure the ROS side of things on your PC. You define your robot's
dimensions, PID parameters, and sensor configuration by editing the
YAML file in the directory ros\_arduino\_python/config. So first move
into that directory:
$ roscd ros_arduino_python/config
Now copy the provided config file to one you can modify:
$ cp arduino_params.yaml my_arduino_params.yaml
Bring up your copy of the params file (my\_arduino\_params.yaml) in
your favorite text editor. It should start off looking like this:
<pre>
port: /dev/ttyUSB0
baud: 57600
timeout: 0.1
rate: 50
sensorstate_rate: 10
use_base_controller: False
base_controller_rate: 10
# === Robot drivetrain parameters
#wheel_diameter: 0.146
#wheel_track: 0.2969
#encoder_resolution: 8384 # from Pololu for 131:1 motors
#gear_reduction: 1.0
#motors_reversed: True
# === PID parameters
#Kp: 20
#Kd: 12
#Ki: 0
#Ko: 50
#accel_limit: 1.0
# === Sensor definitions. Examples only - edit for your robot.
# Sensor type can be one of the follow (case sensitive!):
# * Ping
# * GP2D12
# * Analog
# * Digital
# * PololuMotorCurrent
# * PhidgetsVoltage
# * PhidgetsCurrent (20 Amp, DC)
sensors: {
#motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5},
#motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5},
#ir_front_center: {pin: 2, type: GP2D12, rate: 10},
#sonar_front_center: {pin: 5, type: Ping, rate: 10},
arduino_led: {pin: 13, type: Digital, rate: 5, direction: output}
}
</pre>
**NOTE**: Do not use tabs in your .yaml file or the parser will barf it back out when it tries to load it. Always use spaces instead. **ALSO**: When defining your sensor parameters, the last sensor in the list does **not** get a comma (,) at the end of the line but all the rest **must** have a comma.
Let's now look at each section of this file.
_Port Settings_
The port will likely be either /dev/ttyACM0 or /dev/ttyUSB0. Set accordingly.
The MegaRobogaiaPololu Arudino sketch connects at 57600 baud by default.
_Polling Rates_
The main *rate* parameter (50 Hz by default) determines how fast the
outside ROS loop runs. The default should suffice in most cases. In
any event, it should be at least as fast as your fastest sensor rate
(defined below).
The *sensorstate\_rate* determines how often to publish an aggregated
list of all sensor readings. Each sensor also publishes on its own
topic and rate.
The *use\_base\_controller* parameter is set to False by default. Set it to True to use base control (assuming you have the required hardware.) You will also have to set the PID paramters that follow.
The *base\_controller\_rate* determines how often to publish odometry readings.
_Defining Sensors_
The *sensors* parameter defines a dictionary of sensor names and
sensor parameters. (You can name each sensor whatever you like but
remember that the name for a sensor will also become the topic name
for that sensor.)
The four most important parameters are *pin*, *type*, *rate* and *direction*.
The *rate* defines how many times per second you want to poll that
sensor. For example, a voltage sensor might only be polled once a
second (or even once every 2 seconds: rate=0.5), whereas a sonar
sensor might be polled at 20 times per second. The *type* must be one
of those listed (case sensitive!). The default *direction* is input so
to define an output pin, set the direction explicitly to output. In
the example above, the Arduino LED (pin 13) will be turned on and off
at a rate of 2 times per second.
_Setting Drivetrain and PID Parameters_
To use the base controller, you will have to uncomment and set the
robot drivetrain and PID parameters. The sample drivetrain parameters
are for 6" drive wheels that are 11.5" apart. Note that ROS uses
meters for distance so convert accordingly. The sample encoder
resolution (ticks per revolution) is from the specs for the Pololu
131:1 motor. Set the appropriate number for your motor/encoder
combination. Set the motors_reversed to True if you find your wheels
are turning backward, otherwise set to False.
The PID parameters are trickier to set. You can start with the sample
values but be sure to place your robot on blocks before sending it
your first Twist command.
Launching the ros\_arduino\_python Node
---------------------------------------
Take a look at the launch file arduino.launch in the
ros\_arduino\_python/launch directory. As you can see, it points to a
config file called my\_arduino\_params.yaml. If you named your config
file something different, change the name in the launch file.
With your Arduino connected and running the MegaRobogaiaPololu sketch,
launch the ros\_arduino\_python node with your parameters:
$ roslaunch ros_arduino_python arduino.launch
You should see something like the following output:
<pre>
process[arduino-1]: started with pid [6098]
Connecting to Arduino on port /dev/ttyUSB0 ...
Connected at 57600
Arduino is ready.
[INFO] [WallTime: 1355498525.954491] Connected to Arduino on port /dev/ttyUSB0 at 57600 baud
[INFO] [WallTime: 1355498525.966825] motor_current_right {'rate': 5, 'type': 'PololuMotorCurrent', 'pin': 1}
[INFO]
etc
</pre>
If you have any Ping sonar sensors on your robot and you defined them
in your config file, they should start flashing to indicate you have
made the connection.
Viewing Sensor Data
-------------------
To see the aggregated sensor data, echo the sensor state topic:
$ rostopic echo /arduino/sensor_state
To see the data on any particular sensor, echo its topic name:
$ rostopic echo /arduino/sensor/sensor_name
For example, if you have a sensor called ir\_front\_center, you can see
its data using:
$ rostopic echo /arduino/sensor/ir_front_center
You can also graph the range data using rxplot:
$ rxplot -p 60 /arduino/sensor/ir_front_center/range
Sending Twist Commands and Viewing Odometry Data
------------------------------------------------
Place your robot on blocks, then try publishing a Twist command:
$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{ angular: {z: 0.5} }'
The wheels should turn in a direction consistent with a
counter-clockwise rotation (right wheel forward, left wheel backward).
If they turn in the opposite direction, set the motors_reversed
parameter in your config file to the opposite of its current setting,
then kill and restart the arduino.launch file.
Stop the robot with the command:
$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'
To view odometry data:
$ rostopic echo /odom
or
$ rxplot -p 60 /odom/pose/pose/position/x:y, /odom/twist/twist/linear/x, /odom/twist/twist/angular/z
ROS Services
------------
The ros\_arduino\_python package also defines a few ROS services as follows:
**digital\_set\_direction** - set the direction of a digital pin
$ rosservice call /arduino/digital_set_direction pin direction
where pin is the pin number and direction is 0 for input and 1 for output.
**digital\_write** - send a LOW (0) or HIGH (1) signal to a digital pin
$ rosservice call /arduino/digital_write pin value
where pin is the pin number and value is 0 for LOW and 1 for HIGH.
**servo\_write** - set the position of a servo
$ rosservice call /arduino/servo_write id pos
where id is the index of the servo as defined in the Arduino sketch (servos.h) and pos is the position in radians (0 - 3.14).
**servo\_read** - read the position of a servo
$ rosservice call /arduino/servo_read id
where id is the index of the servo as defined in the Arduino sketch (servos.h)
Using the on-board wheel encoder counters (Arduino Uno only)
------------------------------------------------------------
The firmware supports on-board wheel encoder counters for Arduino Uno.
This allows connecting wheel encoders directly to the Arduino board, without the need for any additional wheel encoder counter equipment (such as a RoboGaia encoder shield).
For speed, the code is directly addressing specific Atmega328p ports and interrupts, making this implementation Atmega328p (Arduino Uno) dependent. (It should be easy to adapt for other boards/AVR chips though.)
To use the on-board wheel encoder counters, connect your wheel encoders to Arduino Uno as follows:
Left wheel encoder A output -- Arduino UNO pin 2
Left wheel encoder B output -- Arduino UNO pin 3
Right wheel encoder A output -- Arduino UNO pin A4
Right wheel encoder B output -- Arduino UNO pin A5
Make the following changes in the ROSArduinoBridge sketch to disable the RoboGaia encoder shield, and enable the on-board one:
/* The RoboGaia encoder shield */
//#define ROBOGAIA
/* Encoders directly attached to Arduino board */
#define ARDUINO_ENC_COUNTER
Compile the changes and upload to your controller.
NOTES
-----
If you do not have the hardware required to run the base controller,
follow the instructions below so that you can still use your
Arduino-compatible controller to read sensors and control PWM servos.
First, you need to edit the ROSArduinoBridge sketch. At the top of
the file, change the two lines that look like this:
<pre>
#define USE_BASE
//#undef USE_BASE
</pre>
to this:
<pre>
//#define USE_BASE
#undef USE_BASE
</pre>
**NOTE:** You also need to comment out the line that looks like this in the file encoder_driver.ino:
#include "MegaEncoderCounter.h"
so it looks like this:
//#include "MegaEncoderCounter.h"
Compile the changes and upload to your controller.
Next, edit your my\_arduino_params.yaml file and make sure the
use\_base\_controller parameter is set to False. That's all there is to it.
>>>>>>> indigo-devel

4
ros_arduino_bridge/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
project(ros_arduino_bridge)
find_package(catkin REQUIRED)
catkin_metapackage()

21
ros_arduino_bridge/package.xml Normal file
View File

@ -0,0 +1,21 @@
<package>
<name>ros_arduino_bridge</name>
<version>0.2.0</version>
<description>
Metapackage for ros_arduino_bridge.
</description>
<author>Patrick Goebel</author>
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/ros_arduino_bridge</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>ros_arduino_firmware</run_depend>
<run_depend>ros_arduino_msgs</run_depend>
<run_depend>ros_arduino_python</run_depend>
<export>
<metapackage/>
</export>
</package>

9
ros_arduino_firmware/CMakeLists.txt Normal file
View File

@ -0,0 +1,9 @@
cmake_minimum_required(VERSION 2.8.3)
project(ros_arduino_firmware)
find_package(catkin REQUIRED)
catkin_package(DEPENDS)
install(DIRECTORY src
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

13
ros_arduino_firmware/package.xml Normal file
View File

@ -0,0 +1,13 @@
<package>
<name>ros_arduino_firmware</name>
<version>0.2.0</version>
<description>
ROS Arduino Firmware.
</description>
<author>Patrick Goebel</author>
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/ros_arduino_firmware</url>
<buildtool_depend>catkin</buildtool_depend>
</package>

346
ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino Normal file
View File

@ -0,0 +1,346 @@
/*********************************************************************
* ROSArduinoBridge
A set of simple serial commands to control a differential drive
robot and receive back sensor and odometry data. Default
configuration assumes use of an Arduino Mega + Pololu motor
controller shield + Robogaia Mega Encoder shield. Edit the
readEncoder() and setMotorSpeed() wrapper functions if using
different motor controller or encoder method.
Created for the Pi Robot Project: http://www.pirobot.org
and the Home Brew Robotics Club (HBRC): http://hbrobotics.org
Authors: Patrick Goebel, James Nugen
Inspired and modeled after the ArbotiX driver by Michael Ferguson
Software License Agreement (BSD License)
Copyright (c) 2012, Patrick Goebel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* 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.
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 OWNER 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.
*********************************************************************/
//#define USE_BASE // Enable the base controller code
#undef USE_BASE // Disable the base controller code
/* Define the motor controller and encoder library you are using */
#ifdef USE_BASE
/* The Pololu VNH5019 dual motor driver shield */
#define POLOLU_VNH5019
/* The Pololu MC33926 dual motor driver shield */
//#define POLOLU_MC33926
/* The RoboGaia encoder shield */
#define ROBOGAIA
/* Encoders directly attached to Arduino board */
//#define ARDUINO_ENC_COUNTER
#endif
#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
//#undef USE_SERVOS // Disable use of PWM servos
/* Serial port baud rate */
#define BAUDRATE 57600
/* Maximum PWM signal */
#define MAX_PWM 255
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
/* Include definition of serial commands */
#include "commands.h"
/* Sensor functions */
#include "sensors.h"
/* Include servo support if required */
#ifdef USE_SERVOS
#include <Servo.h>
#include "servos.h"
#endif
#ifdef USE_BASE
/* Motor driver function definitions */
#include "motor_driver.h"
/* Encoder driver function definitions */
#include "encoder_driver.h"
/* PID parameters and functions */
#include "diff_controller.h"
/* Run the PID loop at 30 times per second */
#define PID_RATE 30 // Hz
/* Convert the rate into an interval */
const int PID_INTERVAL = 1000 / PID_RATE;
/* Track the next time we make a PID calculation */
unsigned long nextPID = PID_INTERVAL;
/* Stop the robot if it hasn't received a movement command
in this number of milliseconds */
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;
#endif
/* Variable initialization */
// A pair of varibles to help parse serial commands (thanks Fergs)
int arg = 0;
int index = 0;
// Variable to hold an input character
char chr;
// Variable to hold the current single-character command
char cmd;
// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];
// The arguments converted to integers
long arg1;
long arg2;
/* Clear the current command parameters */
void resetCommand() {
cmd = NULL;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
arg1 = 0;
arg2 = 0;
arg = 0;
index = 0;
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch(cmd) {
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
case PING:
Serial.println(Ping(arg1));
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].setTargetPosition(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].getServo().read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
resetPID();
Serial.println("OK");
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
moving = 0;
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
Kp = pid_args[0];
Kd = pid_args[1];
Ki = pid_args[2];
Ko = pid_args[3];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
/* Setup function--runs once at startup. */
void setup() {
Serial.begin(BAUDRATE);
// Initialize the motor controller if used */
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
//set as inputs
DDRD &= ~(1<<LEFT_ENC_PIN_A);
DDRD &= ~(1<<LEFT_ENC_PIN_B);
DDRC &= ~(1<<RIGHT_ENC_PIN_A);
DDRC &= ~(1<<RIGHT_ENC_PIN_B);
//enable pull up resistors
PORTD |= (1<<LEFT_ENC_PIN_A);
PORTD |= (1<<LEFT_ENC_PIN_B);
PORTC |= (1<<RIGHT_ENC_PIN_A);
PORTC |= (1<<RIGHT_ENC_PIN_B);
// tell pin change mask to listen to left encoder pins
PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
// tell pin change mask to listen to right encoder pins
PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
initMotorController();
resetPID();
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].initServo(
servoPins[i],
stepDelay[i],
servoInitPosition[i]);
}
#endif
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();
// Terminate a command with a CR
if (chr == 13) {
if (arg == 1) argv1[index] = NULL;
else if (arg == 2) argv2[index] = NULL;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0) arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
}
else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
}
else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
}
else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
}
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
setMotorSpeeds(0, 0);
moving = 0;
}
#endif
// Sweep servos
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].doSweep();
}
#endif
}

30
ros_arduino_firmware/src/libraries/ROSArduinoBridge/base_controller_includes.h Normal file
View File

@ -0,0 +1,30 @@
/* Define the motor controller and encoder library you are using */
#ifdef USE_BASE
/* The Pololu VNH5019 dual motor driver shield */
#define POLOLU_VNH5019
/* The Pololu MC33926 dual motor driver shield */
//#define POLOLU_MC33926
/* The RoboGaia encoder shield */
#define ROBOGAIA
#ifdef ROBOGAIA
#include "MegaEncoderCounter.h"
#endif
/* Motor driver function definitions */
#include "motor_driver.h"
/* Encoder driver function definitions */
#include "encoder_driver.h"
/* PID parameters and functions */
#include "diff_controller.h"
/* Encoders directly attached to Arduino board */
#undef ARDUINO_ENC_COUNTER
#endif

130
ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h Normal file
View File

@ -0,0 +1,130 @@
/* Functions and type-defs for PID control.
Taken mostly from Mike Ferguson's ArbotiX code which lives at:
http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
*/
/* PID setpoint info For a Motor */
typedef struct {
double TargetTicksPerFrame; // target speed in ticks per frame
long Encoder; // encoder count
long PrevEnc; // last encoder count
/*
* Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
int PrevInput; // last input
//int PrevErr; // last error
/*
* Using integrated term (ITerm) instead of integrated error (Ierror),
* to allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//int Ierror;
int ITerm; //integrated term
long output; // last motor setting
}
SetPointInfo;
SetPointInfo leftPID, rightPID;
/* PID Parameters */
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;
unsigned char moving = 0; // is the base in motion?
/*
* Initialize PID variables to zero to prevent startup spikes
* when turning PID on to start moving
* In particular, assign both Encoder and PrevEnc the current encoder value
* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* Note that the assumption here is that PID is only turned on
* when going from stop to moving, that's why we can init everything on zero.
*/
void resetPID(){
leftPID.TargetTicksPerFrame = 0.0;
leftPID.Encoder = readEncoder(LEFT);
leftPID.PrevEnc = leftPID.Encoder;
leftPID.output = 0;
leftPID.PrevInput = 0;
leftPID.ITerm = 0;
rightPID.TargetTicksPerFrame = 0.0;
rightPID.Encoder = readEncoder(RIGHT);
rightPID.PrevEnc = rightPID.Encoder;
rightPID.output = 0;
rightPID.PrevInput = 0;
rightPID.ITerm = 0;
}
/* PID routine to compute the next motor commands */
void doPID(SetPointInfo * p) {
long Perror;
long output;
int input;
//Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAX_PWM)
output = MAX_PWM;
else if (output <= -MAX_PWM)
output = -MAX_PWM;
else
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += Ki * Perror;
p->output = output;
p->PrevInput = input;
}
/* Read the encoder values and call the PID routine */
void updatePID() {
/* Read the encoders */
leftPID.Encoder = readEncoder(LEFT);
rightPID.Encoder = readEncoder(RIGHT);
/* If we're not moving there is nothing more to do */
if (!moving){
/*
* Reset PIDs once, to prevent startup spikes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* PrevInput is considered a good proxy to detect
* whether reset has already happened
*/
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
return;
}
/* Compute PID update for each motor */
doPID(&rightPID);
doPID(&leftPID);
/* Set the motor speeds accordingly */
setMotorSpeeds(leftPID.output, rightPID.output);
}

20
ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h Normal file
View File

@ -0,0 +1,20 @@
/* *************************************************************
Encoder driver function definitions - by James Nugen
************************************************************ */
#ifdef ARDUINO_ENC_COUNTER
//below can be changed, but should be PORTD pins;
//otherwise additional changes in the code are required
#define LEFT_ENC_PIN_A PD2 //pin 2
#define LEFT_ENC_PIN_B PD3 //pin 3
//below can be changed, but should be PORTC pins
#define RIGHT_ENC_PIN_A PC4 //pin A4
#define RIGHT_ENC_PIN_B PC5 //pin A5
#endif
long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();

82
ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino Normal file
View File

@ -0,0 +1,82 @@
/* *************************************************************
Encoder definitions
Add an "#ifdef" block to this file to include support for
a particular encoder board or library. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
************************************************************ */
#ifdef USE_BASE
#ifdef ROBOGAIA
/* The Robogaia Mega Encoder shield */
#include "MegaEncoderCounter.h"
/* Create the encoder shield object */
MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode
/* Wrap the encoder reading function */
long readEncoder(int i) {
if (i == LEFT) return encoders.YAxisGetCount();
else return encoders.XAxisGetCount();
}
/* Wrap the encoder reset function */
void resetEncoder(int i) {
if (i == LEFT) return encoders.YAxisReset();
else return encoders.XAxisReset();
}
#elif ARDUINO_ENC_COUNTER
volatile long left_enc_pos = 0L;
volatile long right_enc_pos = 0L;
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0}; //encoder lookup table
/* Interrupt routine for LEFT encoder, taking care of actual counting */
ISR (PCINT2_vect){
static uint8_t enc_last=0;
enc_last <<=2; //shift previous state two places
enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
}
/* Interrupt routine for RIGHT encoder, taking care of actual counting */
ISR (PCINT1_vect){
static uint8_t enc_last=0;
enc_last <<=2; //shift previous state two places
enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
}
/* Wrap the encoder reading function */
long readEncoder(int i) {
if (i == LEFT) return left_enc_pos;
else return right_enc_pos;
}
/* Wrap the encoder reset function */
void resetEncoder(int i) {
if (i == LEFT){
left_enc_pos=0L;
return;
} else {
right_enc_pos=0L;
return;
}
}
#else
#error A encoder driver must be selected!
#endif
/* Wrap the encoder reset function */
void resetEncoders() {
resetEncoder(LEFT);
resetEncoder(RIGHT);
}
#endif

62
ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino Normal file
View File

@ -0,0 +1,62 @@
/***************************************************************
Motor driver definitions
Add a "#elif defined" block to this file to include support
for a particular motor driver. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
*************************************************************/
#ifdef USE_BASE
#ifdef POLOLU_VNH5019
/* Include the Pololu library */
#include "DualVNH5019MotorShield.h"
/* Create the motor driver object */
DualVNH5019MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#elif defined POLOLU_MC33926
/* Include the Pololu library */
#include "DualMC33926MotorShield.h"
/* Create the motor driver object */
DualMC33926MotorShield drive;
/* Wrap the motor driver initialization */
void initMotorController() {
drive.init();
}
/* Wrap the drive motor set speed function */
void setMotorSpeed(int i, int spd) {
if (i == LEFT) drive.setM1Speed(spd);
else drive.setM2Speed(spd);
}
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#else
#error A motor driver must be selected!
#endif
#endif

44
ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h Normal file
View File

@ -0,0 +1,44 @@
#ifndef SERVOS_H
#define SERVOS_H
#define N_SERVOS 2
// This delay in milliseconds determines the pause
// between each one degree step the servo travels. Increasing
// this number will make the servo sweep more slowly.
// Decreasing this number will make the servo sweep more quickly.
// Zero is the default number and will make the servos spin at
// full speed. 150 ms makes them spin very slowly.
int stepDelay [N_SERVOS] = { 0, 0 }; // ms
// Pins
byte servoPins [N_SERVOS] = { 3, 4 };
// Initial Position
byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees
class SweepServo
{
public:
SweepServo();
void initServo(
int servoPin,
int stepDelayMs,
int initPosition);
void doSweep();
void setTargetPosition(int position);
Servo getServo();
private:
Servo servo;
int stepDelayMs;
int currentPositionDegrees;
int targetPositionDegrees;
long lastSweepCommand;
};
SweepServo servos [N_SERVOS];
#endif

78
ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino Normal file
View File

@ -0,0 +1,78 @@
/***************************************************************
Servo Sweep - by Nathaniel Gallinger
Sweep servos one degree step at a time with a user defined
delay in between steps. Supports changing direction
mid-sweep. Important for applications such as robotic arms
where the stock servo speed is too fast for the strength
of your system.
*************************************************************/
#ifdef USE_SERVOS
// Constructor
SweepServo::SweepServo()
{
this->currentPositionDegrees = 0;
this->targetPositionDegrees = 0;
this->lastSweepCommand = 0;
}
// Init
void SweepServo::initServo(
int servoPin,
int stepDelayMs,
int initPosition)
{
this->servo.attach(servoPin);
this->stepDelayMs = stepDelayMs;
this->currentPositionDegrees = initPosition;
this->targetPositionDegrees = initPosition;
this->lastSweepCommand = millis();
}
// Perform Sweep
void SweepServo::doSweep()
{
// Get ellapsed time
int delta = millis() - this->lastSweepCommand;
// Check if time for a step
if (delta > this->stepDelayMs) {
// Check step direction
if (this->targetPositionDegrees > this->currentPositionDegrees) {
this->currentPositionDegrees++;
this->servo.write(this->currentPositionDegrees);
}
else if (this->targetPositionDegrees < this->currentPositionDegrees) {
this->currentPositionDegrees--;
this->servo.write(this->currentPositionDegrees);
}
// if target == current position, do nothing
// reset timer
this->lastSweepCommand = millis();
}
}
// Set a new target position
void SweepServo::setTargetPosition(int position)
{
this->targetPositionDegrees = position;
}
// Accessor for servo object
Servo SweepServo::getServo()
{
return this->servo;
}
#endif

29
ros_arduino_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 2.8.3)
project(ros_arduino_msgs)
find_package(catkin REQUIRED COMPONENTS std_msgs message_generation)
add_message_files(FILES
AnalogFloat.msg
Analog.msg
ArduinoConstants.msg
Digital.msg
SensorState.msg
)
add_service_files(FILES
DigitalSetDirection.srv
DigitalWrite.srv
DigitalRead.srv
ServoRead.srv
ServoWrite.srv
AnalogWrite.srv
AnalogRead.srv
)
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(CATKIN_DEPENDS message_runtime std_msgs)

5
ros_arduino_msgs/msg/ArduinoConstants.msg Normal file
View File

@ -0,0 +1,5 @@
# Arduino-style constants
uint8 LOW=0
uint8 HIGH=1
uint8 INPUT=0
uint8 OUTPUT=1

20
ros_arduino_msgs/package.xml Normal file
View File

@ -0,0 +1,20 @@
<package>
<name>ros_arduino_msgs</name>
<version>0.2.0</version>
<description>
ROS Arduino Messages.
</description>
<author>Patrick Goebel</author>
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/ros_arduino_msgs</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>message_runtime</run_depend>
<run_depend>std_msgs</run_depend>
</package>

3
ros_arduino_msgs/srv/AnalogRead.srv Executable file
View File

@ -0,0 +1,3 @@
uint8 pin
---
uint16 value

3
ros_arduino_msgs/srv/AnalogWrite.srv Normal file
View File

@ -0,0 +1,3 @@
uint8 pin
uint16 value
---

3
ros_arduino_msgs/srv/DigitalRead.srv Executable file
View File

@ -0,0 +1,3 @@
uint8 pin
---
bool value

3
ros_arduino_msgs/srv/ServoRead.srv Executable file
View File

@ -0,0 +1,3 @@
uint8 id
---
float32 value

3
ros_arduino_msgs/srv/ServoWrite.srv Executable file
View File

@ -0,0 +1,3 @@
uint8 id
float32 value
---

18
ros_arduino_python/CMakeLists.txt Normal file
View File

@ -0,0 +1,18 @@
cmake_minimum_required(VERSION 2.8.3)
project(ros_arduino_python)
find_package(catkin REQUIRED)
catkin_package(DEPENDS)
catkin_python_setup()
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY nodes
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

51
ros_arduino_python/config/arduino_params.yaml Normal file
View File

@ -0,0 +1,51 @@
# For a direct USB cable connection, the port name is typically
# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
# For a wireless connection like XBee, the port is typically
# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.
port: /dev/ttyACM0
baud: 57600
timeout: 0.1
rate: 50
sensorstate_rate: 10
use_base_controller: False
base_controller_rate: 10
# For a robot that uses base_footprint, change base_frame to base_footprint
base_frame: base_link
# === Robot drivetrain parameters
#wheel_diameter: 0.146
#wheel_track: 0.2969
#encoder_resolution: 8384 # from Pololu for 131:1 motors
#gear_reduction: 1.0
#motors_reversed: True
# === PID parameters
#Kp: 10
#Kd: 12
#Ki: 0
#Ko: 50
#accel_limit: 1.0
# === Sensor definitions. Examples only - edit for your robot.
# Sensor type can be one of the follow (case sensitive!):
# * Ping
# * GP2D12
# * Analog
# * Digital
# * PololuMotorCurrent
# * PhidgetsVoltage
# * PhidgetsCurrent (20 Amp, DC)
sensors: {
#motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5},
#motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5},
#ir_front_center: {pin: 2, type: GP2D12, rate: 10},
#sonar_front_center: {pin: 5, type: Ping, rate: 10},
arduino_led: {pin: 13, type: Digital, rate: 5, direction: output}
}

209
ros_arduino_python/nodes/arduino_node.py Executable file
View File

@ -0,0 +1,209 @@
#!/usr/bin/env python
"""
A ROS Node for the Arduino microcontroller
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import rospy
from ros_arduino_python.arduino_driver import Arduino
from ros_arduino_python.arduino_sensors import *
from ros_arduino_msgs.srv import *
from ros_arduino_python.base_controller import BaseController
from geometry_msgs.msg import Twist
import os, time
import thread
class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
self.base_frame = rospy.get_param("~base_frame", 'base_link')
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller", False)
# Set up the time for publishing the next SensorState message
now = rospy.Time.now()
self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
self.t_next_sensors = now + self.t_delta_sensors
# Initialize a Twist message
self.cmd_vel = Twist()
# A cmd_vel publisher so we can stop the robot when shutting down
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
# A service to position a PWM servo
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
# A service to read the position of a PWM servo
rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
# A service to turn set the direction of a digital pin (0 = input, 1 = output)
rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
# A service to turn a digital sensor on or off
rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
# A service to read the value of a digital sensor
rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
# A service to set pwm values for the pins
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
# A service to read the value of an analog sensor
rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# Make the connection
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
sensor_params = rospy.get_param("~sensors", dict({}))
for name, params in sensor_params.iteritems():
# Set the direction to input if not specified
try:
params['direction']
except:
params['direction'] = 'input'
if params['type'] == "Ping":
sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'PhidgetsCurrent':
sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller, self.base_frame)
# Start polling the sensors and base controller
while not rospy.is_shutdown():
for sensor in self.mySensors:
mutex.acquire()
sensor.poll()
mutex.release()
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
# Publish all sensor values on a single topic for convenience
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = self.base_frame
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
# Service callback functions
def ServoWriteHandler(self, req):
self.controller.servo_write(req.id, req.value)
return ServoWriteResponse()
def ServoReadHandler(self, req):
pos = self.controller.servo_read(req.id)
return ServoReadResponse(pos)
def DigitalSetDirectionHandler(self, req):
self.controller.pin_mode(req.pin, req.direction)
return DigitalSetDirectionResponse()
def DigitalWriteHandler(self, req):
self.controller.digital_write(req.pin, req.value)
return DigitalWriteResponse()
def DigitalReadHandler(self, req):
value = self.controller.digital_read(req.pin)
return DigitalReadResponse(value)
def AnalogWriteHandler(self, req):
self.controller.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def AnalogReadHandler(self, req):
value = self.controller.analog_read(req.pin)
return AnalogReadResponse(value)
def shutdown(self):
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
rospy.loginfo("Shutting down Arduino Node...")
if __name__ == '__main__':
myArduino = ArduinoROS()

22
ros_arduino_python/package.xml Normal file
View File

@ -0,0 +1,22 @@
<package>
<name>ros_arduino_python</name>
<version>0.2.0</version>
<description>
ROS Arduino Python.
</description>
<author>Patrick Goebel</author>
<maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/ros_arduino_python</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>ros_arduino_msgs</run_depend>
<run_depend>python-serial</run_depend>
</package>

11
ros_arduino_python/setup.py Normal file
View File

@ -0,0 +1,11 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['ros_arduino_python'],
package_dir={'': 'src'},
)
setup(**d)

374
ros_arduino_python/src/ros_arduino_python/arduino_driver.py Executable file
View File

@ -0,0 +1,374 @@
#!/usr/bin/env python
"""
A Python driver for the Arduino microcontroller running the
ROSArduinoBridge firmware.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import thread
from math import pi as PI, degrees, radians
import os
import time
import sys, traceback
from serial.serialutil import SerialException
from serial import Serial
SERVO_MAX = 180
SERVO_MIN = 0
class Arduino:
''' Configuration Parameters
'''
N_ANALOG_PORTS = 6
N_DIGITAL_PORTS = 12
def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5):
self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller.
self.PID_INTERVAL = 1000 / 30
self.port = port
self.baudrate = baudrate
self.timeout = timeout
self.encoder_count = 0
self.writeTimeout = timeout
self.interCharTimeout = timeout / 30.
# Keep things thread safe
self.mutex = thread.allocate_lock()
# An array to cache analog sensor readings
self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
# An array to cache digital sensor readings
self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
def connect(self):
try:
print "Connecting to Arduino on port", self.port, "..."
self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
# The next line is necessary to give the firmware time to wake up.
time.sleep(1)
test = self.get_baud()
if test != self.baudrate:
time.sleep(1)
test = self.get_baud()
if test != self.baudrate:
raise SerialException
print "Connected at", self.baudrate
print "Arduino is ready."
except SerialException:
print "Serial Exception:"
print sys.exc_info()
print "Traceback follows:"
traceback.print_exc(file=sys.stdout)
print "Cannot connect to Arduino!"
os._exit(1)
def open(self):
''' Open the serial port.
'''
self.port.open()
def close(self):
''' Close the serial port.
'''
self.port.close()
def send(self, cmd):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
self.port.write(cmd + '\r')
def recv(self, timeout=0.5):
timeout = min(timeout, self.timeout)
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner. Note: we use read() instead of readline() since
readline() tends to return garbage characters from the Arduino
'''
c = ''
value = ''
attempts = 0
while c != '\r':
c = self.port.read(1)
value += c
attempts += 1
if attempts * self.interCharTimeout > timeout:
return None
value = value.strip('\r')
return value
def recv_ack(self):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
ack = self.recv(self.timeout)
return ack == 'OK'
def recv_int(self):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
value = self.recv(self.timeout)
try:
return int(value)
except:
return None
def recv_array(self):
''' This command should not be used on its own: it is called by the execute commands
below in a thread safe manner.
'''
try:
values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
return map(int, values)
except:
return []
def execute(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
'''
self.mutex.acquire()
try:
self.port.flushInput()
except:
pass
ntries = 1
attempts = 0
try:
self.port.write(cmd + '\r')
value = self.recv(self.timeout)
while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
try:
self.port.flushInput()
self.port.write(cmd + '\r')
value = self.recv(self.timeout)
except:
print "Exception executing command: " + cmd
attempts += 1
except:
self.mutex.release()
print "Exception executing command: " + cmd
value = None
self.mutex.release()
return int(value)
def execute_array(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning an array.
'''
self.mutex.acquire()
try:
self.port.flushInput()
except:
pass
ntries = 1
attempts = 0
try:
self.port.write(cmd + '\r')
values = self.recv_array()
while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
try:
self.port.flushInput()
self.port.write(cmd + '\r')
values = self.recv_array()
except:
print("Exception executing command: " + cmd)
attempts += 1
except:
self.mutex.release()
print "Exception executing command: " + cmd
raise SerialException
return []
try:
values = map(int, values)
except:
values = []
self.mutex.release()
return values
def execute_ack(self, cmd):
''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
'''
self.mutex.acquire()
try:
self.port.flushInput()
except:
pass
ntries = 1
attempts = 0
try:
self.port.write(cmd + '\r')
ack = self.recv(self.timeout)
while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
try:
self.port.flushInput()
self.port.write(cmd + '\r')
ack = self.recv(self.timeout)
except:
print "Exception executing command: " + cmd
attempts += 1
except:
self.mutex.release()
print "execute_ack exception when executing", cmd
print sys.exc_info()
return 0
self.mutex.release()
return ack == 'OK'
def update_pid(self, Kp, Kd, Ki, Ko):
''' Set the PID parameters on the Arduino
'''
print "Updating PID parameters"
cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
self.execute_ack(cmd)
def get_baud(self):
''' Get the current baud rate on the serial port.
'''
return int(self.execute('b'));
def get_encoder_counts(self):
values = self.execute_array('e')
if len(values) != 2:
print "Encoder count was not 2"
raise SerialException
return None
else:
return values
def reset_encoders(self):
''' Reset the encoder counts to 0
'''
return self.execute_ack('r')
def drive(self, right, left):
''' Speeds are given in encoder ticks per PID interval
'''
return self.execute_ack('m %d %d' %(right, left))
def drive_m_per_s(self, right, left):
''' Set the motor speeds in meters per second.
'''
left_revs_per_second = float(left) / (self.wheel_diameter * PI)
right_revs_per_second = float(right) / (self.wheel_diameter * PI)
left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
self.drive(right_ticks_per_loop , left_ticks_per_loop )
def stop(self):
''' Stop both motors.
'''
self.drive(0, 0)
def analog_read(self, pin):
return self.execute('a %d' %pin)
def analog_write(self, pin, value):
return self.execute_ack('x %d %d' %(pin, value))
def digital_read(self, pin):
return self.execute('d %d' %pin)
def digital_write(self, pin, value):
return self.execute_ack('w %d %d' %(pin, value))
def pin_mode(self, pin, mode):
return self.execute_ack('c %d %d' %(pin, mode))
def servo_write(self, id, pos):
''' Usage: servo_write(id, pos)
Position is given in radians and converted to degrees before sending
'''
return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
def servo_read(self, id):
''' Usage: servo_read(id)
The returned position is converted from degrees to radians
'''
return radians(self.execute('t %d' %id))
def ping(self, pin):
''' The srf05/Ping command queries an SRF05/Ping sonar sensor
connected to the General Purpose I/O line pinId for a distance,
and returns the range in cm. Sonar distance resolution is integer based.
'''
return self.execute('p %d' %pin);
# def get_maxez1(self, triggerPin, outputPin):
# ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar
# sensor connected to the General Purpose I/O lines, triggerPin, and
# outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE
# SURE there's nothing directly in front of the MaxSonar-EZ1 upon
# power up, otherwise it wont range correctly for object less than 6
# inches away! The sensor reading defaults to use English units
# (inches). The sensor distance resolution is integer based. Also, the
# maxsonar trigger pin is RX, and the echo pin is PW.
# '''
# return self.execute('z %d %d' %(triggerPin, outputPin))
""" Basic test for connectivity """
if __name__ == "__main__":
if os.name == "posix":
portName = "/dev/ttyACM0"
else:
portName = "COM43" # Windows style COM port.
baudRate = 57600
myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
myArduino.connect()
print "Sleeping for 1 second..."
time.sleep(1)
print "Reading on analog port 0", myArduino.analog_read(0)
print "Reading on digital port 0", myArduino.digital_read(0)
print "Blinking the LED 3 times"
for i in range(3):
myArduino.digital_write(13, 1)
time.sleep(1.0)
#print "Current encoder counts", myArduino.encoders()
print "Connection test successful.",
myArduino.stop()
myArduino.close()
print "Shutting down Arduino."

276
ros_arduino_python/src/ros_arduino_python/arduino_sensors.py Executable file
View File

@ -0,0 +1,276 @@
#!/usr/bin/env python
"""
Sensor class for the arudino_python package
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2012 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import roslib; roslib.load_manifest('ros_arduino_python')
import rospy
from sensor_msgs.msg import Range
from ros_arduino_msgs.msg import *
LOW = 0
HIGH = 1
INPUT = 0
OUTPUT = 1
class MessageType:
ANALOG = 0
DIGITAL = 1
RANGE = 2
FLOAT = 3
INT = 4
BOOL = 5
class Sensor(object):
def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
self.controller = controller
self.name = name
self.pin = pin
self.rate = rate
self.direction = direction
self.frame_id = frame_id
self.value = None
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = rospy.Time.now() + self.t_delta
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
if self.direction == "input":
try:
self.value = self.read_value()
except:
return
else:
try:
self.ack = self.write_value()
except:
return
# For range sensors, assign the value to the range message field
if self.message_type == MessageType.RANGE:
self.msg.range = self.value
else:
self.msg.value = self.value
# Add a timestamp and publish the message
self.msg.header.stamp = rospy.Time.now()
self.pub.publish(self.msg)
self.t_next = now + self.t_delta
class AnalogSensor(Sensor):
def __init__(self, *args, **kwargs):
super(AnalogSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.ANALOG
self.msg = Analog()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
if self.direction == "output":
self.controller.pin_mode(self.pin, OUTPUT)
else:
self.controller.pin_mode(self.pin, INPUT)
self.value = LOW
def read_value(self):
return self.controller.analog_read(self.pin)
def write_value(self, value):
return self.controller.analog_write(self.pin, value)
class AnalogFloatSensor(Sensor):
def __init__(self, *args, **kwargs):
super(AnalogFloatSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.ANALOG
self.msg = AnalogFloat()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)
if self.direction == "output":
self.controller.pin_mode(self.pin, OUTPUT)
else:
self.controller.pin_mode(self.pin, INPUT)
self.value = LOW
def read_value(self):
return self.controller.analog_read(self.pin)
def write_value(self, value):
return self.controller.analog_write(self.pin, value)
class DigitalSensor(Sensor):
def __init__(self, *args, **kwargs):
super(DigitalSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.BOOL
self.msg = Digital()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
if self.direction == "output":
self.controller.pin_mode(self.pin, OUTPUT)
else:
self.controller.pin_mode(self.pin, INPUT)
self.value = LOW
def read_value(self):
return self.controller.digital_read(self.pin)
def write_value(self):
# Alternate HIGH/LOW when writing at a fixed rate
self.value = not self.value
return self.controller.digital_write(self.pin, self.value)
class RangeSensor(Sensor):
def __init__(self, *args, **kwargs):
super(RangeSensor, self).__init__(*args, **kwargs)
self.message_type = MessageType.RANGE
self.msg = Range()
self.msg.header.frame_id = self.frame_id
self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)
def read_value(self):
self.msg.header.stamp = rospy.Time.now()
class SonarSensor(RangeSensor):
def __init__(self, *args, **kwargs):
super(SonarSensor, self).__init__(*args, **kwargs)
self.msg.radiation_type = Range.ULTRASOUND
class IRSensor(RangeSensor):
def __init__(self, *args, **kwargs):
super(IRSensor, self).__init__(*args, **kwargs)
self.msg.radiation_type = Range.INFRARED
class Ping(SonarSensor):
def __init__(self,*args, **kwargs):
super(Ping, self).__init__(*args, **kwargs)
self.msg.field_of_view = 0.785398163
self.msg.min_range = 0.02
self.msg.max_range = 3.0
def read_value(self):
# The Arduino Ping code returns the distance in centimeters
cm = self.controller.ping(self.pin)
# Convert it to meters for ROS
distance = cm / 100.0
return distance
class GP2D12(IRSensor):
def __init__(self, *args, **kwargs):
super(GP2D12, self).__init__(*args, **kwargs)
self.msg.field_of_view = 0.001
self.msg.min_range = 0.10
self.msg.max_range = 0.80
def read_value(self):
value = self.controller.analog_read(self.pin)
if value <= 3.0:
return self.msg.max_range
try:
distance = (6787.0 / (float(value) - 3.0)) - 4.0
except:
return self.msg.max_range
# Convert to meters
distance /= 100.0
# If we get a spurious reading, set it to the max_range
if distance > self.msg.max_range: distance = self.msg.max_range
if distance < self.msg.min_range: distance = self.msg.max_range
return distance
class PololuMotorCurrent(AnalogFloatSensor):
def __init__(self, *args, **kwargs):
super(PololuMotorCurrent, self).__init__(*args, **kwargs)
def read_value(self):
# From the Pololu source code
milliamps = self.controller.analog_read(self.pin) * 34
return milliamps / 1000.0
class PhidgetsVoltage(AnalogFloatSensor):
def __init__(self, *args, **kwargs):
super(PhidgetsVoltage, self).__init__(*args, **kwargs)
def read_value(self):
# From the Phidgets documentation
voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.)
return voltage
class PhidgetsCurrent(AnalogFloatSensor):
def __init__(self, *args, **kwargs):
super(PhidgetsCurrent, self).__init__(*args, **kwargs)
def read_value(self):
# From the Phidgets documentation for the 20 amp DC sensor
current = 0.05 * (self.controller.analog_read(self.pin) - 500.)
return current
class MaxEZ1Sensor(SonarSensor):
def __init__(self, *args, **kwargs):
super(MaxEZ1Sensor, self).__init__(*args, **kwargs)
self.trigger_pin = kwargs['trigger_pin']
self.output_pin = kwargs['output_pin']
self.msg.field_of_view = 0.785398163
self.msg.min_range = 0.02
self.msg.max_range = 3.0
def read_value(self):
return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin)
if __name__ == '__main__':
myController = Controller()
mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10)

247
ros_arduino_python/src/ros_arduino_python/base_controller.py Executable file
View File

@ -0,0 +1,247 @@
#!/usr/bin/env python
"""
A base controller class for the Arduino microcontroller
Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2010 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses
"""
import roslib; roslib.load_manifest('ros_arduino_python')
import rospy
import os
from math import sin, cos, pi
from geometry_msgs.msg import Quaternion, Twist, Pose
from nav_msgs.msg import Odometry
from tf.broadcaster import TransformBroadcaster
""" Class to receive Twist commands and publish Odometry data """
class BaseController:
def __init__(self, arduino, base_frame):
self.arduino = arduino
self.base_frame = base_frame
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
self.stopped = False
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['Kp'] = rospy.get_param("~Kp", 20)
pid_params['Kd'] = rospy.get_param("~Kd", 12)
pid_params['Ki'] = rospy.get_param("~Ki", 0)
pid_params['Ko'] = rospy.get_param("~Ko", 50)
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = now + self.t_delta
# Internal data
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0 # rotation in radians
self.v_left = 0
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.last_cmd_vel = now
# Subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
def setup_pid(self, pid_params):
# Check to see if any PID parameters are missing
missing_params = False
for param in pid_params:
if pid_params[param] == "":
print("*** PID Parameter " + param + " is missing. ***")
missing_params = True
if missing_params:
os._exit(1)
self.wheel_diameter = pid_params['wheel_diameter']
self.wheel_track = pid_params['wheel_track']
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.Kp = pid_params['Kp']
self.Kd = pid_params['Kd']
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame,
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def stop(self):
self.stopped = True
self.arduino.drive(0, 0)
def cmdVelCallback(self, req):
# Handle velocity-based movement requests
self.last_cmd_vel = rospy.Time.now()
x = req.linear.x # m/s
th = req.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)