First commit

CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro.  This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake.  Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake.  Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly.  CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild_make_distribution(0.1.0)

Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake_stack.mk

README.md

@@ -0,0 +1,353 @@
+This ROS stack includes an Arduino sketch and a collection of ROS
+packages for controlling an Arduino-based robot using standard ROS
+messages and services.
+
+Supported sensors currently include Ping sonar and Sharp GP2D12
+infrared as well as generic analog and digital sensors (e.g bump
+switches, voltage sensors, etc.)
+
+The package also includes a base controller for a differential drive
+that accepts ROS Twist messages and publishes odometry data back to
+the PC. In this version of the stack, the base controller requires the
+use of an Arduino Mega 2560 controller together with a Pololu motor
+controller shield (http://www.pololu.com/catalog/product/2502) and a
+Robogaia Mega Encoder shield
+(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html).
+
+System Requirements
+-------------------
+The current version of the stack requires an Arudino Mega controller +
+Pololu Dual VNH5019 Motor Driver Shield + Robogaia Encoder shield.  If
+you do not have this hardware, you can still try the package for
+reading sensors and controlling servos using other Arduino-compatible
+controllers.  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 Polulu Arduino
+library found at:
+
+https://github.com/pololu/Dual-VNH5019-Motor-Shield
+
+and the Robogaia Encoder library 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.
+
+
+Installation
+------------
+
+    $ cd ~/ros_workspace
+    $ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
+    $ cd ros_arduino_bridge
+    $ rosmake
+
+The provided Arduino sketch is called MegaRobogaiaPololu 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 MegaRobogaiaPololu sketch, follow these steps:
+
+    $ cd SKETCHBOOK_PATH
+
+where SKETCHBOOK_PATH is the path to your Arduino sketchbook directory.
+
+    $ ln -s `rospack find ros_arduino_firmware`/src/libraries/MegaRobogaiaPololu MegaRobogaiaPololu
+
+This last command creates a link in your sketchbook folder for the
+MegaRobogaiaPololu sketch that you must run on your Arduino Mega
+controller.  By creating a link rather than copying the files, the
+sketch will get updated along with other files in the
+ros\_arduino\_bridge stack.
+
+
+Loading the MegaRobogaiaPololu Sketch
+-------------------------------------
+
+* Make sure you have already installed the DualVNH5019MotorShield and
+  MegaEncoderCounter libraries into your Arduino sketchbook/libraries
+  folder.
+
+* Launch the Arduino 1.0 IDE and load the MegaRobogaiaPololu sketch.
+  You should be able to find it by going to:
+
+    File->Sketchbook->MegaRobogaiaPololu
+  
+NOTE: If you don't have the Arduino Mega/Pololu/Robogaia hardware but
+still want to try the code, see the notes at the end of the file.
+
+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.
+
+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/ttyACM0
+baud: 57600
+timeout: 0.1
+
+rate: 50
+sensorstate_rate: 10
+
+use_base_controller: True
+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
+
+#  === Sensor definitions.  Examples only - edit for your robot.
+#      Sensor type can be one of the follow (case sensitive!):
+#	    Ping
+#	    GP2D12
+#	    Analog
+#	    Digital
+#	    PololuMotorCurrent
+
+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: 2, direction: output}
+}
+</pre>
+
+Let's now look at each section of this file.
+
+ _Port Settings_
+
+The port will likely be either /dev/ttyACM0 or /dev/ttyUSB0 (e.g. for
+an Xbee connection).  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 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 blinked 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\_arduiono\_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/ttyACM0 ...
+Connected at 57600
+Arduino is ready.
+[INFO] [WallTime: 1355498525.954491] Connected to Arduino on port /dev/ttyACM0 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/sensors
+
+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 degrees (0 - 180).
+
+**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)
+
+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 MegaRobogaiaPololu sketch. mAt 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>
+
+You also need to comment out the line that looks like this:
+
+    #include "MegaEncoderCounter.h"
+
+so it looks like this:
+
+    //#include "MegaEncoderCounter.h"
+
+Compile the changes and upload to your controller.
+
+Next, edit your arduino_params.yaml file and set the
+use\_base\_controller parameter to False.  That's all there is to it.

ros_arduino_firmware/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

ros_arduino_firmware/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

ros_arduino_firmware/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ros_arduino_firmware is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

ros_arduino_firmware/manifest.xml

@@ -0,0 +1,14 @@
+<package>
+  <description brief="ros_arduino_firmware">
+
+     ros_arduino_firmware
+
+  </description>
+  <author>Patrick Goebel</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ros_arduino_firmware</url>
+
+</package>
+
+

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino

@@ -0,0 +1,307 @@
+/*********************************************************************
+ *  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
+
+    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 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"
+
+#ifdef USE_SERVOS
+   #include <Servo.h>
+   #include "servos.h"
+#endif
+
+#ifdef USE_BASE
+  /* The Pololu motor driver shield */
+  #include "DualVNH5019MotorShield.h"
+
+  /* The Robogaia Mega Encoder shield */
+  #include "MegaEncoderCounter.h"
+
+  /* Create the motor driver object */
+  DualVNH5019MotorShield drive;
+
+  /* Create the encoder shield object */
+  MegaEncoderCounter encoders(4); // Initializes the Mega Encoder Counter in the 4X Count Mode
+
+  /* PID parameters and functions */
+  #include "diff_controller.h"
+
+  /* Run the PID loop at 30 times per second */
+  #define PID_RATE           30     // Hz
+  const int PID_INTERVAL = 1000 / PID_RATE;
+  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;
+
+  // 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();
+  }
+
+  void resetEncoders() {
+    resetEncoder(LEFT);
+    resetEncoder(RIGHT);
+  }
+
+  // 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);
+  }
+#endif
+
+int mode = 0;
+int index = 0;
+char chr;
+char cmd;
+char argv1[16];
+char argv2[16];
+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;
+  mode = 0;
+  index = 0;
+}
+
+// Run a command
+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].write(arg2);
+    Serial.print(arg1);
+    Serial.print(arg2);
+    Serial.println("OK");
+    break;
+  case SERVO_READ:
+    Serial.println(servos[arg1].read());
+    break;
+#endif
+    
+#ifdef USE_BASE
+  case READ_ENCODERS:
+    Serial.print(readEncoder(0));
+    Serial.print(" ");
+    Serial.println(readEncoder(1));
+    break;
+   case RESET_ENCODERS:
+    resetEncoders();
+    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;
+  }
+}
+
+void setup() {
+  Serial.begin(BAUDRATE);
+
+#ifdef USE_BASE
+  drive.init();
+#endif
+
+#ifdef USE_SERVOS
+  int i;
+  for (i = 0; i < N_SERVOS; i++) {
+    servos[i].attach(servoPins[i]);
+  }
+#endif
+}
+
+void loop() {
+  while (Serial.available() > 0) {
+    
+    chr = Serial.read();
+
+    // Terminate a command with a CR
+    if (chr == 13) {
+      if (mode == 1) argv1[index] = NULL;
+      else if (mode == 2) argv2[index] = NULL;
+      runCommand();
+      resetCommand();
+    }
+    // Use spaces to delimit parts of the command
+    else if (chr == ' ') {
+      if (mode == 0) mode = 1;
+      else if (mode == 1)  {
+        argv1[index] = NULL;
+        mode = 2;
+        index = 0;
+      }
+      continue;
+    }
+    else {
+      if (mode == 0) {
+        cmd = chr;
+      }
+      else if (mode == 1) {
+        argv1[index] = chr;
+        index++;
+      }
+      else if (mode == 2) {
+        argv2[index] = chr;
+        index++;
+      }
+    }
+  }
+  
+// If we using base control, run a PID loop at the appropriate intervals
+#ifdef USE_BASE
+  if (millis() > nextPID) {
+    updatePID();
+    nextPID += PID_INTERVAL;
+  }
+  
+  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
+    setMotorSpeeds(0, 0);
+    moving = 0;
+  }
+
+#endif
+}
+
+
+
+
+
+

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h

@@ -0,0 +1,22 @@
+/* Define single-letter commands that will be sent by the PC over the
+   serial link.
+*/
+
+#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'
+#define LEFT            0
+#define RIGHT           1
+
+
+

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h

@@ -0,0 +1,68 @@
+/* Functions and type-defs for PID control.  Take mostly from Mike Ferguson's
+   ArbotiX code.
+*/
+
+/* 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
+  int PrevErr;                   // last error
+  int Ierror;                    // integrated error
+  int 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?
+
+/* PID routine to compute the next motor commands */
+void doPID(SetPointInfo * p) {
+  long Perror;
+  long output;
+
+  Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
+
+  // Derivative error is the delta Perror
+  output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
+  p->PrevErr = Perror;
+  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
+    p->Ierror += Perror;
+
+  p->output = output;
+}
+
+/* Read the encoder values and call the PID routine */
+void updatePID() {
+  /* Read the encoders */
+  leftPID.Encoder = readEncoder(0);
+  rightPID.Encoder = readEncoder(1);
+  
+  /* If we're not moving there is nothing more to do */
+  if (!moving)
+    return;
+
+  /* Compute PID update for each motor */
+  doPID(&rightPID);
+  doPID(&leftPID);
+
+  /* Set the motor speeds accordingly */
+  setMotorSpeeds(leftPID.output, rightPID.output);
+}
+

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h

@@ -0,0 +1,34 @@
+/* Functions for various sensor types */
+
+float microsecondsToCm(long microseconds)
+{
+  // The speed of sound is 340 m/s or 29 microseconds per cm.
+  // The ping travels out and back, so to find the distance of the
+  // object we take half of the distance travelled.
+  return microseconds / 29 / 2;
+}
+
+long Ping(int pin) {
+  long duration, range;
+
+  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
+  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
+  pinMode(pin, OUTPUT);
+  digitalWrite(pin, LOW);
+  delayMicroseconds(2);
+  digitalWrite(pin, HIGH);
+  delayMicroseconds(5);
+  digitalWrite(pin, LOW);
+
+  // The same pin is used to read the signal from the PING))): a HIGH
+  // pulse whose duration is the time (in microseconds) from the sending
+  // of the ping to the reception of its echo off of an object.
+  pinMode(pin, INPUT);
+  duration = pulseIn(pin, HIGH);
+
+  // convert the time into meters
+  range = microsecondsToCm(duration);
+  
+  return(range);
+}
+

ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h

@@ -0,0 +1,16 @@
+/* Define the attachment of any servos here.
+   The example shows one servo attached on pin 11.
+*/
+
+#define N_SERVOS 2
+
+Servo servos [N_SERVOS];
+byte servoPins [N_SERVOS] = {3, 6};
+
+
+
+
+
+
+
+

ros_arduino_msgs/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

ros_arduino_msgs/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

ros_arduino_msgs/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ros_arduino_msgs is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

ros_arduino_msgs/manifest.xml

@@ -0,0 +1,13 @@
+<package>
+  <description brief="ros_arduino_msgs">
+
+     ros_arduino_msgs
+
+  </description>
+  <author>Patrick Goebel</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ros_arduino_msgs</url>
+</package>
+
+

ros_arduino_msgs/msg/Analog.msg

@@ -0,0 +1,3 @@
+# Reading from a single analog IO pin.
+Header header
+uint8 value

ros_arduino_msgs/msg/AnalogFloat.msg

@@ -0,0 +1,3 @@
+# Float message from a single analog IO pin.
+Header header
+float64 value

ros_arduino_msgs/msg/Digital.msg

@@ -0,0 +1,4 @@
+# Reading on a digital pin
+Header header
+uint8 value
+

ros_arduino_msgs/msg/SensorState.msg

@@ -0,0 +1,4 @@
+Header header
+
+string[] name
+float32[] value

ros_arduino_msgs/srv/DigitalSetDirection.srv

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

ros_arduino_msgs/srv/DigitalWrite.srv

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

ros_arduino_msgs/srv/ServoRead.srv

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

ros_arduino_msgs/srv/ServoWrite.srv

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

ros_arduino_python/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

ros_arduino_python/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

ros_arduino_python/config/arduino_params.yaml

@@ -0,0 +1,38 @@
+port: /dev/ttyACM0
+baud: 57600
+timeout: 0.1
+
+rate: 50
+sensorstate_rate: 10
+
+use_base_controller: True
+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
+
+# === Sensor definitions.  Examples only - edit for your robot.
+#     Sensor type can be one of the follow (case sensitive!):
+#	  * Ping
+#	  * GP2D12
+#	  * Analog
+#	  * Digital
+#	  * PololuMotorCurrent
+
+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: 2, direction: output}
+}

ros_arduino_python/launch/arduino.launch

@@ -0,0 +1,5 @@
+<launch>
+   <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
+      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
+   </node>
+</launch>

ros_arduino_python/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ros_arduino_python is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

ros_arduino_python/manifest.xml

@@ -0,0 +1,22 @@
+<package>
+  <description brief="ros_arduino_python">
+
+     ros_arduino_python
+
+  </description>
+  <author>Patrick Goebel</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ros_arduino_python</url>
+  <depend package="roscpp"/>
+  <depend package="rospy"/>
+  <depend package="std_msgs"/>
+  <depend package="sensor_msgs"/>
+  <depend package="geometry_msgs"/>
+  <depend package="nav_msgs"/>
+  <depend package="tf"/>
+  <depend package="ros_arduino_msgs"/>
+
+</package>
+
+

ros_arduino_python/nodes/arduino_node.py

@@ -0,0 +1,178 @@
+#!/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 roslib; roslib.load_manifest('ros_arduino_python')
+import rospy
+from arduino_driver import Arduino
+from arduino_sensors import *
+from ros_arduino_msgs.srv import *
+from 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)
+
+        # 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)
+        
+        # The SensorState publisher periodically publishes the values of all sensors on
+        # a single topic.
+        self.sensorStatePub = rospy.Publisher('~sensors', SensorState)
+        
+        # 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)
+
+        # 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")
+     
+        # Reservce 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():
+            if params['type'] == "Ping":
+                sensor = Ping(self.controller, name, params['pin'], params['rate'])
+            elif params['type'] == "GP2D12":
+                sensor = GP2D12(self.controller, name, params['pin'], params['rate'])
+            elif params['type'] == 'Digital':
+                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction'])
+            elif params['type'] == 'Analog':
+                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction'])
+            elif params['type'] == 'PololuMotorCurrent':
+                sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'])
+                
+#                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))
+              
+        # Initialize the base controller if used
+        if self.use_base_controller:
+            self.myBaseController = BaseController(self.controller)
+    
+        # 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 = 'base_link'
+                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):
+        self.controller.servo_read(req.id)
+        return ServoReadResponse()
+    
+    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 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()

ros_arduino_python/src/arduino_driver.py

@@ -0,0 +1,369 @@
+#!/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
+import os
+import time
+import sys
+from serial.serialutil import SerialException
+from serial import Serial
+
+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 "Cannot connect to Arduino!"
+            print "Make sure you are plugged in and turned on."
+            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()
+            if not self.shutdown:
+                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
+            if not self.shutdown:
+                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()
+            if not self.shutdown:
+                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)
+        '''        
+        return self.execute_ack('s %d %d' %(id, pos))
+    
+    def servo_read(self, id):
+        ''' Usage: servo_read(id)
+        '''        
+        return 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."
+    

ros_arduino_python/src/arduino_sensors.py

@@ -0,0 +1,230 @@
+#!/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="/base_link", 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          
+    
+            # The Arduino returns ranges in cm so convert to meters
+            if self.message_type == MessageType.RANGE:
+                try:
+                    self.value /= 100.0
+                    self.msg.range = self.value
+                except:
+                    return
+            else:
+                self.msg.value = self.value
+                
+
+            # At 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)
+        
+    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)
+    
+        
+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)
+        
+        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)
+        
+    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.controller.pin_mode(self.pin, INPUT)
+        
+        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.ping(self.pin)
+    
+        
+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)
+        try:
+            distance = (6787 / (value - 3)) - 4
+        except:
+            distance = 80
+        if distance > 80: distance = 80
+        if distance < 10: distance = 10
+        
+        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 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)
+            

ros_arduino_python/src/base_controller.py

@@ -0,0 +1,227 @@
+#!/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):
+        self.arduino = arduino
+        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.motors_reversed = rospy.get_param("~motors_reversed", False)
+        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
+        
+        # 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)
+        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)
+        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")
+        
+    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:
+                right_enc, left_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(),
+                "base_link",
+                "odom"
+                )
+    
+            odom = Odometry()
+            odom.header.frame_id = "odom"
+            odom.child_frame_id = "base_link"
+            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_left = 0
+                self.v_right = 0
+            
+            # Set motor speeds in encoder ticks per PID loop
+            if not self.stopped:
+                if self.motors_reversed:
+                    self.arduino.drive(self.v_left, self.v_right)
+                else:
+                    self.arduino.drive(self.v_right, self.v_left)
+            
+    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_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
+        self.v_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
+        
+
+        
+
+    
+
+    

stack.xml

@@ -0,0 +1,14 @@
+<stack>
+  <description brief="ros_arduino_bridge">ros_arduino_bridge</description>
+  <author>Maintained by Patrick Goebel</author>
+  <license>BSD</license>  
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/ros_arduino_bridge</url>
+  <depend stack="ros" />
+  <depend stack="ros_comm" /> <!-- std_srvs, rospy, roscpp -->
+  <depend stack="common" /> <!-- actionlib -->
+  <depend stack="common_msgs" /> <!-- nav_msgs, geometry_msgs, sensor_msgs, diagnostic_msgs -->
+  <depend stack="control" /> <!-- control_msgs -->
+  <depend stack="geometry" /> <!-- tf -->
+
+</stack>