diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..396c4957 --- /dev/null +++ b/.gitignore @@ -0,0 +1,106 @@ + +# Created by https://www.gitignore.io/api/python + +### Python ### +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +env/ +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*,cover +.hypothesis/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# SageMath parsed files +*.sage.py + +# dotenv +.env + +# virtualenv +.venv +venv/ +ENV/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# End of https://www.gitignore.io/api/python +# temporary files +*~ diff --git a/README.md b/README.md index 10366eaa..d2ca0d5a 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,32 @@ +**UNSTABLE BRANCH** + +This branch is under active development and is not likely to work. Please use the official hydro-devel or indigo-devel branch for ROS Hydro or ROS Indigo, respectively. + +Table of Contents +================= + +* [Overview](#overview) +* [Official ROS Documentation](#official-ros-documentation) +* [System Requirements](#system-requirements) +* [Preparing your Serial Port under Linux](#preparing-your-serial-port-under-linux) +* [Installation of the ros_arduino_bridge Stack](#installation-of-the-ros_arduino_bridge-stack) +* [Loading the ROSArduinoBridge Sketch](#loading-the-rosarduinobridge-sketch) +* [Firmware Commands](#firmware-commands) +* [Testing your Wiring Connections](#testing-your-wiring-connections) +* [Configuring the ros_arduino_python Node](#configuring-the-ros_arduino_python-node) +* [Launching the ros_arduino_python Node](#launching-the-ros_arduino_python-node) +* [Viewing Sensor Data](#viewing-sensor-data) +* [Sending Twist Commands and Viewing Odometry Data](#sending-twist-commands-and-viewing-odometry-data) +* [ROS Services for Sensors and Servos](#ros-services-for-sensors-and-servos) +* [ROS Joint Topics and Services](#ros-joint-topics-and-services) +* [Using the on-board wheel encoder counters (Arduino Uno only)](#using-the-on-board-wheel-encoder-counters-arduino-uno-only) +* [NOTES](#notes) + 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. +This ROS metapackage 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. Features of the stack include: @@ -23,7 +47,9 @@ the PC. The base controller requires the use of a motor controller and encoders * 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. +(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) + +* Instead of the Encoder shield, wheel encoders can be [connected directly](#using-the-on-board-wheel-encoder-counters-arduino-uno-only) if using an Arduino Uno **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. @@ -38,6 +64,10 @@ http://www.ros.org/wiki/ros_arduino_bridge System Requirements ------------------- +**ROS Dependencies** + + $ sudo apt-get install ros-indigo-diagnostic-updater ros-indigo-control-msgs ros-indigo-nav-msgs + **Python Serial:** To install the python-serial package under Ubuntu, use the command: $ sudo apt-get install python-serial @@ -71,7 +101,7 @@ 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 +Finally, it is assumed you are using version 1.6.6 or greater of the Arduino IDE. Preparing your Serial Port under Linux @@ -129,9 +159,9 @@ To install the ROSArduinoBridge library, follow these steps: where SKETCHBOOK_PATH is the path to your Arduino sketchbook directory. - $ cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge ROSArduinoBridge + $ \cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge -T 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. +This last command copies the ROSArduinoBridge sketch files into your sketchbook folder and overwrites any existing files with the same name. The next section describes how to configure, compile and upload this sketch. Loading the ROSArduinoBridge Sketch @@ -139,35 +169,39 @@ 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. +* Launch the Arduino 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. +**NOTE:** If you have the required hardware to use the base controller, uncomment the line that looks like this: + +
+//#define USE_BASE
+
-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. +so it looks like this: -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. +
+#define USE_BASE
+
-If you want to control PWM servos attached to your controller, change -the two lines that look like this: +You will also need to 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, the two options are the Robogaia Mega Encoder shield (chosen by default) and the directo connection ARDUINO_ENC_COUNTER option that works for Arduino Uno compatible boards. + +By default, the sketch will provide support to control PWM servos attached to your Arduino. If you do not need servo support, you can comment out the line that looks like this: 
-//#define USE_SERVOS
-#undef USE_SERVOS
+#define USE_SERVOS2
-to this: +so that it looks like this: 
-#define USE_SERVOS
-//#undef USE_SERVOS
+//#define USE_SERVOS2
-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. @@ -185,14 +219,20 @@ The list of commands can be found in the file commands.h. The current list incl #define PIN_MODE 'c' #define DIGITAL_READ 'd' #define READ_ENCODERS 'e' +#define CONFIG_SERVO 'j' #define MOTOR_SPEEDS 'm' #define PING 'p' #define RESET_ENCODERS 'r' #define SERVO_WRITE 's' #define SERVO_READ 't' #define UPDATE_PID 'u' +#define SERVO_DELAY 'v' #define DIGITAL_WRITE 'w' #define ANALOG_WRITE 'x' +#define ATTACH_SERVO 'y' +#define DETACH_SERVO 'z' +#define LEFT 0 +#define RIGHT 1 For example, to get the analog reading on pin 3, use the command: @@ -211,6 +251,18 @@ To move the robot forward at 20 encoder ticks per second: m 20 20 +To intialize a PWM servo on pin 3 with speed delay 100ms: + +j 3 100 + +To move the servo on pin 3 to position 120 degrees: + +s 3 120 + +To detach servo on pin 3: + +z 3 + Testing your Wiring Connections ------------------------------- @@ -278,8 +330,16 @@ sensors: { #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} + onboard_led: {pin: 13, type: Digital, rate: 5, direction: output} } + +# Joint name and configuration is an example only +joints: { + head_pan_joint: {pin: 3, init_position: 0, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False}, + head_tilt_joint: {pin: 5, init_position: 0, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False} +} + + **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. @@ -324,6 +384,12 @@ 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. +_Defining Servo Configurations_ + +The *joints* parameter defines a dictionary of joint names and servo parameters. (You can name each joint whatever you like but rememember that joint names will become part of the servo's ROS topic and service names.) + +The most important parameter is *pin* which of course must match the pin the servo attaches to on your Arduino. Most PWM servos operate from 0 to 180 degrees with a "neutral" point of 90 degrees. ROS uses radians instead of degrees for joint positions but it is usually easier for programmers to specify the angular limits in the config file using degrees. The ROS Arduino Bridge pacakge takes care of the conversion to radians. An *init_position* of 0 therefore means 0 degrees relative to the neutral point of 90 degrees. A *max_angle* of 90 degrees maps into 180 degrees at the servo. + _Setting Drivetrain and PID Parameters_ To use the base controller, you will have to uncomment and set the @@ -413,9 +479,9 @@ 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: +ROS Services for Sensors and Servos +----------------------------------- +The ros\_arduino\_python package also defines a few ROS services for sensors and servos as follows: **digital\_set\_direction** - set the direction of a digital pin @@ -441,6 +507,40 @@ where id is the index of the servo as defined in the Arduino sketch (servos.h) a where id is the index of the servo as defined in the Arduino sketch (servos.h) +ROS Joint Topics and Services +----------------------------- +At the ROS level, a servo is called a joint and each joint has its own topics and services. To change the position of a joint, publish the position +in radians to the topic: + +**/\/command** + +For example, a joint called head_pan_joint in the YAML config file can be controlled using the topic: + +**/head_pan_joint/command** + +which takes a Float64 argument specifying the desired position in radians. For example, the command: + + $ rostopic pub -1 /head_pan_joint/command std_msgs/Float64 -- 1.0 + +will move the servo to angle 1.0 radians from the neutral point; i.e. about 147 degrees when using the default neutral point of 90 degrees. Using a negative value moves the servo in the other direction: + + $ rostopic pub -1 /head_pan_joint/command std_msgs/Float64 -- -1.0 + +A number of services are also available for each joint: + +**/\/enable** - Enable or disable a joint. Disabling also detachs the underlying servo so that it can be moved by hand. + + $ rosservice call /head_pan_joint/enable false + +**/\/relax** - Another way to detach the underlying servo so that it can be moved by hand. + + $ rosservice call /head_pan_joint/relax + +**/\/set_speed** - Set the movement speed of servo in radians per second. + + $ rosservice call /head_pan_joint/set_speed 1.0 + + Using the on-board wheel encoder counters (Arduino Uno only) ------------------------------------------------------------ The firmware supports on-board wheel encoder counters for Arduino Uno. @@ -473,21 +573,21 @@ 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: +the file, comment out the line that looks like this: 
 #define USE_BASE
-//#undef USE_BASE
-to this: +so it looks like this: 
 //#define USE_BASE
-#undef USE_BASE
-**NOTE:** You also need to comment out the line that looks like this in the file encoder_driver.ino: +(You may find that it is already commented out.) + +**NOTE:** If you are using a version of the Arduino IDE earlier than 1.6.6, then you also need to comment out the line that looks like this in the file encoder_driver.ino: #include "MegaEncoderCounter.h" diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino deleted file mode 100644 index 0f664299..00000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino +++ /dev/null @@ -1,341 +0,0 @@ -/********************************************************************* - * 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 - - 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 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 - #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 - - /* 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; - - /* 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(); - } - - /* Wrap the encoder reset function */ - void resetEncoders() { - resetEncoder(LEFT); - resetEncoder(RIGHT); - } - - /* 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); - } -#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].write(arg2); - Serial.println("OK"); - break; - case SERVO_READ: - Serial.println(servos[arg1].read()); - break; -#endif - -#ifdef USE_BASE - case READ_ENCODERS: - Serial.print(readEncoder(LEFT)); - Serial.print(" "); - Serial.println(readEncoder(RIGHT)); - 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; - } -} - -/* Setup function--runs once at startup. */ -void setup() { - Serial.begin(BAUDRATE); - -// Initialize the motor controller if used */ -#ifdef USE_BASE - initMotorController(); -#endif - -/* Attach servos if used */ -#ifdef USE_SERVOS - int i; - for (i = 0; i < N_SERVOS; i++) { - servos[i].attach(servoPins[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 -} - - - - - - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h deleted file mode 100644 index 18ff0abb..00000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h +++ /dev/null @@ -1,22 +0,0 @@ -/* 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 - - - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h deleted file mode 100644 index 02846812..00000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h +++ /dev/null @@ -1,71 +0,0 @@ -/* 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 - 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); -} - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h deleted file mode 100644 index 75caeb77..00000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h +++ /dev/null @@ -1,34 +0,0 @@ -/* 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); -} - diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h deleted file mode 100644 index 61007234..00000000 --- a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h +++ /dev/null @@ -1,16 +0,0 @@ -/* Define the attachment of any servos here. - The example shows two servos attached on pins 3 and 5. -*/ - -#define N_SERVOS 2 - -Servo servos [N_SERVOS]; -byte servoPins [N_SERVOS] = {3, 5}; - - - - - - - - diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 5de37268..712aa441 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -11,7 +11,7 @@ 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 + Authors: Patrick Goebel, James Nugen, Nathaniel Gallinger Inspired and modeled after the ArbotiX driver by Michael Ferguson @@ -45,32 +45,58 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -//#define USE_BASE // Enable the base controller code -#undef USE_BASE // Disable the base controller code +//#define USE_BASE // Enable/disable the base controller code + +//#define USE_IMU // Enable/disable use of an IMU /* 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 VNH5019 dual motor driver shield */ + #define POLOLU_VNH5019 + + /* The Pololu MC33926 dual motor driver shield */ + //#define POLOLU_MC33926 - /* The Pololu MC33926 dual motor driver shield */ - //#define POLOLU_MC33926 + /* The Adafruit motor shield V2 */ + //#define ADAFRUIT_MOTOR_SHIELD_V2 - /* The RoboGaia encoder shield */ - #define ROBOGAIA - - /* Encoders directly attached to Arduino board */ - //#define ARDUINO_ENC_COUNTER + /* The Ardunino Motor Shield R3 */ + //#define ARDUINO_MOTOR_SHIELD_R3 + + /* The brake uses digital pins 8 and 9 and is not compatible with the Robogaia 3-axis + * endcoder shield. Cut the brake jumpers on the R3 motor shield if you want to use + * it with the 3-axis encoder shield. + */ + //#define USE_ARDUINO_MOTOR_SHIELD_R3_BRAKE + + /* For testing only */ + // #define NO_MOTOR_CONTROLLER + + /* The RoboGaia encoder shield */ + #define ROBOGAIA + + /* The RoboGaia 3-axis encoder shield */ + //#define ROBOGAIA_3_AXIS + + /* 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 +//#define USE_SERVOS // Enable/disable use of old PWM servo support as defined in servos.h -/* Serial port baud rate */ -#define BAUDRATE 57600 +#define USE_SERVOS2 // Enable/disable use of new PWM servo support as defined in servos2.h + +/* Include old servo support if required */ +#ifdef USE_SERVOS + #include "Servo.h" + #include "servos.h" -/* Maximum PWM signal */ -#define MAX_PWM 255 +/* Include new servo support if desired */ +#elif defined(USE_SERVOS2) + #include "Servo.h" + #include "servos2.h" + int nServos = 0; +#endif #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" @@ -84,16 +110,15 @@ /* Sensor functions */ #include "sensors.h" -/* Include servo support if required */ -#ifdef USE_SERVOS - #include - #include "servos.h" -#endif - #ifdef USE_BASE /* Motor driver function definitions */ #include "motor_driver.h" + #ifdef ROBOGAIA_3_AXIS + // The sensor communicates using SPI, so include the library: + #include + #endif + /* Encoder driver function definitions */ #include "encoder_driver.h" @@ -115,7 +140,16 @@ long lastMotorCommand = AUTO_STOP_INTERVAL; #endif +#ifdef USE_IMU + #include "imu.h" + + // The only IMU currently supported is the Adafruit 9-DOF IMU + #define ADAFRUIT_9DOF + +#endif + /* Variable initialization */ +#define BAUDRATE 57600 // A pair of varibles to help parse serial commands (thanks Fergs) int arg = 0; @@ -128,8 +162,8 @@ char chr; char cmd; // Character arrays to hold the first and second arguments -char argv1[16]; -char argv2[16]; +char argv1[32]; +char argv2[32]; // The arguments converted to integers long arg1; @@ -148,15 +182,17 @@ void resetCommand() { /* Run a command. Commands are defined in commands.h */ int runCommand() { - int i = 0; + int i; char *p = argv1; char *str; int pid_args[4]; arg1 = atoi(argv1); arg2 = atoi(argv2); - + String output; + switch(cmd) { case GET_BAUDRATE: + Serial.print(F(" ")); Serial.println(BAUDRATE); break; case ANALOG_READ: @@ -167,68 +203,142 @@ int runCommand() { break; case ANALOG_WRITE: analogWrite(arg1, arg2); - Serial.println("OK"); + Serial.println(F("OK")); break; case DIGITAL_WRITE: if (arg2 == 0) digitalWrite(arg1, LOW); else if (arg2 == 1) digitalWrite(arg1, HIGH); - Serial.println("OK"); + Serial.println(F("OK")); break; case PIN_MODE: if (arg2 == 0) pinMode(arg1, INPUT); else if (arg2 == 1) pinMode(arg1, OUTPUT); - Serial.println("OK"); + Serial.println(F("OK")); break; case PING: Serial.println(Ping(arg1)); break; +#ifdef USE_IMU + case READ_IMU: + imu_data = readIMU(); + /* Send the IMU data base in the following order + * [ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch] + */ + Serial.print(imu_data.ax); + Serial.print(F(" ")); + Serial.print(imu_data.ay); + Serial.print(F(" ")); + Serial.print(imu_data.az); + Serial.print(F(" ")); + Serial.print(imu_data.gx); + Serial.print(F(" ")); + Serial.print(imu_data.gy); + Serial.print(F(" ")); + Serial.print(imu_data.gz); + Serial.print(F(" ")); + Serial.print(imu_data.mx); + Serial.print(F(" ")); + Serial.print(imu_data.my); + Serial.print(F(" ")); + Serial.print(imu_data.mz); + Serial.print(F(" ")); + Serial.print(imu_data.roll); + Serial.print(F(" ")); + Serial.print(imu_data.pitch); + Serial.print(F(" ")); + Serial.println(imu_data.ch); + break; +#endif #ifdef USE_SERVOS case SERVO_WRITE: servos[arg1].setTargetPosition(arg2); - Serial.println("OK"); + Serial.println(F("OK")); break; case SERVO_READ: Serial.println(servos[arg1].getServo().read()); break; +#elif defined(USE_SERVOS2) + case CONFIG_SERVO: + if (!haveServo(arg1)) { + myServos[arg1].initServo(arg1, arg2); + myServoPins[nServos] = arg1; + myServos[arg1].enable(); + nServos++; + } + Serial.println(F("OK")); + break; + case SERVO_WRITE: + if (myServos[arg1].isEnabled()) { + myServos[arg1].setTargetPosition(arg2); + } + Serial.println(F("OK")); + break; + case SERVO_READ: + Serial.println(myServos[arg1].getCurrentPosition()); + break; + case SERVO_DELAY: + myServos[arg1].setServoDelay(arg1, arg2); + Serial.println(F("OK")); + break; + case DETACH_SERVO: + myServos[arg1].getServo().detach(); + myServos[arg1].disable(); + Serial.println(F("OK")); + break; + case ATTACH_SERVO: + if (!haveServo(arg1)) { + myServos[arg1].initServo(arg1, 0); + myServoPins[nServos] = arg1; + nServos++; + } + else { + myServos[arg1].getServo().attach(arg1); + } + myServos[arg1].enable(); + Serial.println(F("OK")); + break; + #endif #ifdef USE_BASE case READ_ENCODERS: Serial.print(readEncoder(LEFT)); - Serial.print(" "); + Serial.print(F(" ")); Serial.println(readEncoder(RIGHT)); break; case RESET_ENCODERS: resetEncoders(); resetPID(); - Serial.println("OK"); + Serial.println(F("OK")); break; case MOTOR_SPEEDS: /* Reset the auto stop timer */ lastMotorCommand = millis(); if (arg1 == 0 && arg2 == 0) { setMotorSpeeds(0, 0); + resetPID(); moving = 0; } else moving = 1; leftPID.TargetTicksPerFrame = arg1; rightPID.TargetTicksPerFrame = arg2; - Serial.println("OK"); + Serial.println(F("OK")); break; case UPDATE_PID: + i = 0; while ((str = strtok_r(p, ":", &p)) != '\0') { - pid_args[i] = atoi(str); - i++; + 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"); + Serial.println(F("OK")); break; #endif default: - Serial.println("Invalid Command"); + Serial.println(F("Invalid Command")); break; } } @@ -237,34 +347,19 @@ int runCommand() { void setup() { Serial.begin(BAUDRATE); -// Initialize the motor controller if used */ -#ifdef USE_BASE - #ifdef ARDUINO_ENC_COUNTER - //set as inputs - DDRD &= ~(1< nextPID) { - updatePID(); - nextPID += PID_INTERVAL; - } + // 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 + // 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 + // Sweep servos + #ifdef USE_SERVOS + int i; + for (i = 0; i < N_SERVOS; i++) { + servos[i].doSweep(); + } + + #elif defined(USE_SERVOS2) + int i, pin; + for (i = 0; i < nServos; i++) { + pin = myServoPins[i]; + myServos[pin].moveServo(); + } + #endif } diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h index a1c6672f..026c3885 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h @@ -10,14 +10,19 @@ #define PIN_MODE 'c' #define DIGITAL_READ 'd' #define READ_ENCODERS 'e' +#define READ_IMU 'i' +#define CONFIG_SERVO 'j' #define MOTOR_SPEEDS 'm' #define PING 'p' #define RESET_ENCODERS 'r' #define SERVO_WRITE 's' #define SERVO_READ 't' #define UPDATE_PID 'u' +#define SERVO_DELAY 'v' #define DIGITAL_WRITE 'w' #define ANALOG_WRITE 'x' +#define ATTACH_SERVO 'y' +#define DETACH_SERVO 'z' #define LEFT 0 #define RIGHT 1 diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h index f6482091..5f2f8c48 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h @@ -48,6 +48,10 @@ unsigned char moving = 0; // is the base in motion? * 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. */ + +/* Maximum PWM signal */ +#define MAX_PWM 255 + void resetPID(){ leftPID.TargetTicksPerFrame = 0.0; leftPID.Encoder = readEncoder(LEFT); @@ -96,7 +100,7 @@ void doPID(SetPointInfo * p) { /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ - p->ITerm += Ki * Perror; + p->ITerm += Ki * Perror; p->output = output; p->PrevInput = input; diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h index 8d35ab1c..93e0d848 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h @@ -11,10 +11,21 @@ //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 + #define RIGHT_ENC_PIN_B PC5 //pin A5 #endif - + +#ifdef ROBOGAIA_3_AXIS + #define chipSelectPin1 10 + #define chipSelectPin2 9 + #define chipSelectPin3 8 + + #define CNTR B00100000 + #define CLR B00000000 +#endif + +void initEncoders(); long readEncoder(int i); void resetEncoder(int i); void resetEncoders(); + diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino index ef2b8b11..91b90ca7 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino @@ -22,11 +22,156 @@ else return encoders.XAxisGetCount(); } + /* Wrap the encoder initializeing to be run in the main setup() function */ + void initEncoders() { + /* Nothing to do here */ + } + /* Wrap the encoder reset function */ void resetEncoder(int i) { if (i == LEFT) return encoders.YAxisReset(); else return encoders.XAxisReset(); } + + /* Wrap the encoder reset function */ + void resetEncoders() { + resetEncoder(LEFT); + resetEncoder(RIGHT); + } + +#elif defined(ROBOGAIA_3_AXIS) + /* Robogaia 3-axis encoder shield */ + + //***************************************************** + void initEncoders() + //***************************************************** + { + pinMode(chipSelectPin1, OUTPUT); + pinMode(chipSelectPin2, OUTPUT); + pinMode(chipSelectPin3, OUTPUT); + + digitalWrite(chipSelectPin1, HIGH); + digitalWrite(chipSelectPin2, HIGH); + digitalWrite(chipSelectPin3, HIGH); + + LS7366_Init(); + + delay(100); + } + + long readEncoder(int encoder) + //***************************************************** + { + unsigned int count1Value, count2Value, count3Value, count4Value; + long result; + + /* The 3-axis encoder uses a 1-based index */ + if (encoder == 2) encoder = 3; + if (encoder == 1) encoder = 2; + if (encoder == 0) encoder = 1; + + selectEncoder(encoder); + + SPI.transfer(0x60); // Request count + count1Value = SPI.transfer(0x00); // Read highest order byte + count2Value = SPI.transfer(0x00); + count3Value = SPI.transfer(0x00); + count4Value = SPI.transfer(0x00); // Read lowest order byte + + deselectEncoder(encoder); + + result= ((long)count1Value<<24) + ((long)count2Value<<16) + ((long)count3Value<<8) + (long)count4Value; + + return result; + }//end func + + //************************************************* + void selectEncoder(int encoder) + //************************************************* + { + switch(encoder) + { + case 1: + digitalWrite(chipSelectPin1,LOW); + break; + case 2: + digitalWrite(chipSelectPin2,LOW); + break; + case 3: + digitalWrite(chipSelectPin3,LOW); + break; + }//end switch + + }//end func + + //************************************************* + void deselectEncoder(int encoder) + //************************************************* + { + switch(encoder) + { + case 1: + digitalWrite(chipSelectPin1,HIGH); + break; + case 2: + digitalWrite(chipSelectPin2,HIGH); + break; + case 3: + digitalWrite(chipSelectPin3,HIGH); + break; + }//end switch + + }//end func + + //************************************************* + void resetEncoder(int encoder) + //************************************************* + { + /* The 3-axis encoder uses a 1-based index */ + if (encoder == 2) encoder = 3; + if (encoder == 1) encoder = 2; + if (encoder == 0) encoder = 1; + + selectEncoder(encoder); + SPI.transfer(CLR | CNTR); + deselectEncoder(encoder); + } + + /* Wrap the encoder reset function */ + void resetEncoders() { + resetEncoder(LEFT); + resetEncoder(RIGHT); + } + + // LS7366 Initialization and configuration + //************************************************* + void LS7366_Init(void) + //************************************************* + { + // SPI initialization + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1Mhz (on 16Mhz clock) + delay(10); + + digitalWrite(chipSelectPin1,LOW); + SPI.transfer(0x88); + SPI.transfer(0x03); + digitalWrite(chipSelectPin1,HIGH); + + + digitalWrite(chipSelectPin2,LOW); + SPI.transfer(0x88); + SPI.transfer(0x03); + digitalWrite(chipSelectPin2,HIGH); + + + digitalWrite(chipSelectPin3,LOW); + SPI.transfer(0x88); + SPI.transfer(0x03); + digitalWrite(chipSelectPin3,HIGH); + + }//end func + #elif defined(ARDUINO_ENC_COUNTER) volatile long left_enc_pos = 0L; volatile long right_enc_pos = 0L; @@ -51,6 +196,29 @@ right_enc_pos += ENC_STATES[(enc_last & 0x0f)]; } + + /* Wrap the encoder initializeing to be run in the main setup() function */ + void initEncoders() { + //set as inputs + DDRD &= ~(1< + #include + #include + #include + #include + + /* Assign a unique ID to the sensors */ + Adafruit_9DOF dof = Adafruit_9DOF(); + Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301); + Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302); + Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20); + + + /* Update this with the correct SLP for accurate altitude measurements */ + float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA; + + void initIMU() + { + accel.begin(); + mag.begin(); + gyro.begin(); + } + + imuData readIMU() { + imuData_s data; + + sensors_event_t accel_event; + sensors_event_t mag_event; + sensors_vec_t orientation; + sensors_event_t event; + + /* Calculate pitch and roll from the raw accelerometer data */ + accel.getEvent(&accel_event); + data.ax = accel_event.acceleration.x; + data.ay = accel_event.acceleration.y; + data.az = accel_event.acceleration.z; + + gyro.getEvent(&event); + data.gx = event.gyro.x; + data.gy = event.gyro.y; + data.gz = event.gyro.z; +/* + if (dof.accelGetOrientation(&accel_event, &orientation)) + { + data.roll = orientation.roll; + data.pitch = orientation.pitch; + } +*/ + + /* Calculate the heading using the magnetometer */ + mag.getEvent(&mag_event); + +/* + if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) + { + data.mz = orientation.heading; + } +*/ + + if (dof.fusionGetOrientation(&accel_event, &mag_event, &orientation)) + { + data.roll = orientation.roll; + data.pitch = orientation.pitch; + data.ch = orientation.heading; + } + + return data; + } + + #endif + +#endif diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h index 0e3248cf..b07d20ca 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h @@ -5,3 +5,39 @@ void initMotorController(); void setMotorSpeed(int i, int spd); void setMotorSpeeds(int leftSpeed, int rightSpeed); + + +/********** + * Pin assignments for the Arduino Motor Shield R3 + */ + +#ifdef ARDUINO_MOTOR_SHIELD_R3 + + #define LEFT_MOTOR_PIN_DIR 12 + #define LEFT_MOTOR_PIN_BRAKE 9 + #define LEFT_MOTOR_PIN_SPEED 3 + + #define RIGHT_MOTOR_PIN_DIR 13 + #define RIGHT_MOTOR_PIN_BRAKE 8 + #define RIGHT_MOTOR_PIN_SPEED 11 + +#endif + +#ifdef ADAFRUIT_MOTOR_SHIELD_V2 + + #define LEFT_MOTOR_HEADER 1 + #define RIGHT_MOTOR_HEADER 2 + + #include + #include + + // Create the motor shield object with the default I2C address + Adafruit_MotorShield AFMS = Adafruit_MotorShield(); + // Or, create it with a different I2C address (say for stacking) + // Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); + + // Select which 'port' M1, M2, M3 or M4. NOTE: M1 and M2 conflict with the Robogaia 3-axis encoder shield. + Adafruit_DCMotor *myLeftMotor = AFMS.getMotor(LEFT_MOTOR_HEADER); + Adafruit_DCMotor *myRightMotor = AFMS.getMotor(RIGHT_MOTOR_HEADER); +#endif + diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino index 27c76bcc..de95434f 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino @@ -32,6 +32,7 @@ setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); } + #elif defined POLOLU_MC33926 /* Include the Pololu library */ #include "DualMC33926MotorShield.h" @@ -55,8 +56,106 @@ setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); } + +#elif defined ADAFRUIT_MOTOR_SHIELD_V2 + /* Wrap the motor driver initialization */ + void initMotorController() { + AFMS.begin(); + } + + /* Wrap the drive motor set speed function */ + void setMotorSpeed(int i, int spd) { + if (i == LEFT) { + if (spd < 0) { + myLeftMotor->run(BACKWARD); + spd = -spd; + } + else myLeftMotor->run(FORWARD); + myLeftMotor->setSpeed(spd); + } + else { + if (spd < 0) { + myRightMotor->run(BACKWARD); + spd = -spd; + } + else myRightMotor->run(FORWARD); + myRightMotor->setSpeed(spd); + } + } + + // A convenience function for setting both motor speeds + void setMotorSpeeds(int leftSpeed, int rightSpeed) { + setMotorSpeed(LEFT, leftSpeed); + setMotorSpeed(RIGHT, rightSpeed); + } + +#elif defined ARDUINO_MOTOR_SHIELD_R3 + /* Wrap the motor driver initialization */ + void initMotorController() { + /* If the brake function is enabled */ + #ifdef USE_ARDUINO_MOTOR_SHIELD_R3_BRAKE + pinMode(LEFT_MOTOR_PIN_BRAKE, OUTPUT); // Initiates Brake Channel A pin + pinMode(RIGHT_MOTOR_PIN_BRAKE, OUTPUT); // Initiates Brake Channel B pin + #endif + + // Set up Channel A - LEFT + pinMode(LEFT_MOTOR_PIN_DIR, OUTPUT); // Initiates Motor Channel A pin + + // Set up Channel B - RIGHT + pinMode(RIGHT_MOTOR_PIN_DIR, OUTPUT); // Initiates Motor Channel B pin + } + + /* Wrap the drive motor set speed function */ + void setMotorSpeed(int i, int spd) { + if (i == LEFT) { + if (spd < 0) digitalWrite(LEFT_MOTOR_PIN_DIR, LOW); + else digitalWrite(LEFT_MOTOR_PIN_DIR, HIGH); + + #ifdef USE_ARDUINO_MOTOR_SHIELD_R3_BRAKE + if (spd == 0) digitalWrite(LEFT_MOTOR_PIN_BRAKE, HIGH); + else digitalWrite(LEFT_MOTOR_PIN_BRAKE, LOW); + #endif + + analogWrite(LEFT_MOTOR_PIN_SPEED, spd); + } + else { + if (spd < 0) digitalWrite(RIGHT_MOTOR_PIN_DIR, LOW); + else digitalWrite(RIGHT_MOTOR_PIN_DIR, HIGH); + + #ifdef USE_ARDUINO_MOTOR_SHIELD_R3_BRAKE + if (spd == 0) digitalWrite(RIGHT_MOTOR_PIN_BRAKE, HIGH); + else digitalWrite(RIGHT_MOTOR_PIN_BRAKE, LOW); + #endif + + analogWrite(RIGHT_MOTOR_PIN_SPEED, spd); + } + } + + /* A convenience function for setting both motor speeds */ + void setMotorSpeeds(int leftSpeed, int rightSpeed) { + setMotorSpeed(LEFT, leftSpeed); + setMotorSpeed(RIGHT, rightSpeed); + } + +/* For testing only! */ +#elif defined(NO_MOTOR_CONTROLLER) + /* Wrap the motor driver initialization */ + void initMotorController() { } + + /* A convenience function for setting both motor speeds */ + void setMotorSpeeds(int leftSpeed, int rightSpeed) { + setMotorSpeed(LEFT, leftSpeed); + setMotorSpeed(RIGHT, rightSpeed); + } + + /* Wrap the drive motor set speed function */ + void setMotorSpeed(int i, int spd) { + if (i == LEFT) {} + else {} + } #else - #error A motor driver must be selected! + //#error A motor driver must be selected! #endif #endif + diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h index 75caeb77..fc3511bb 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h @@ -26,7 +26,7 @@ long Ping(int pin) { pinMode(pin, INPUT); duration = pulseIn(pin, HIGH); - // convert the time into meters + // convert the time into centimeters range = microsecondsToCm(duration); return(range); diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h index d2b6d21b..59578347 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h @@ -42,3 +42,4 @@ class SweepServo SweepServo servos [N_SERVOS]; #endif + diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino index 5ba344c7..7f92a6ff 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino @@ -76,3 +76,4 @@ Servo SweepServo::getServo() #endif + diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h new file mode 100644 index 00000000..3bbd64dd --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h @@ -0,0 +1,50 @@ +#ifndef SERVOS2_H +#define SERVOS2_H + +#define MAX_N_SERVOS 48 + +// 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. + +class SweepServo2 +{ + public: + SweepServo2(); + + void initServo( + int servoPin, + unsigned long stepDelayMs); + + void setServoDelay( + int servoPin, + unsigned long stepDelayMs); + + void moveServo(); + void setTargetPosition(int position); + void setServoDelay(int delay); + int getCurrentPosition(); + void enable(); + void disable(); + bool isEnabled(); + + Servo getServo(); + + private: + Servo servo; + unsigned long stepDelayMs; + int currentPositionDegrees; + int targetPositionDegrees; + unsigned long lastSweepCommand; + bool enabled; +}; + +SweepServo2 myServos [MAX_N_SERVOS]; + +int myServoPins [MAX_N_SERVOS]; + +#endif + diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino new file mode 100644 index 00000000..7eb41614 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino @@ -0,0 +1,112 @@ +/*************************************************************** + 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_SERVOS2 + +// NOTE: ServoTimer2.cpp moves the servo to 90 degrees after +// an attach() commmand so we set targetPositionDegrees to 90 +// when constructing the servo object. + +// Constructor +SweepServo2::SweepServo2() +{ + this->currentPositionDegrees = 90; + this->targetPositionDegrees = 90; + this->lastSweepCommand = 0; +} + +// Init +void SweepServo2::initServo( + int servoPin, + unsigned long stepDelayMs) +{ + this->stepDelayMs = stepDelayMs; + this->currentPositionDegrees = 90; + this->targetPositionDegrees = 90; + this->lastSweepCommand = millis(); + this->servo.attach(servoPin); +} + +// Init +void SweepServo2::setServoDelay( + int servoPin, + unsigned long stepDelayMs) +{ + this->stepDelayMs = stepDelayMs; +} + +// Perform Sweep +void SweepServo2::moveServo() +{ + + // Get ellapsed time + unsigned long 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 SweepServo2::setTargetPosition(int position) +{ + this->targetPositionDegrees = position; +} + +// Get the current servo position +int SweepServo2::getCurrentPosition() +{ + return this->currentPositionDegrees; +} + +void SweepServo2::enable() { + this->enabled = true; +} + +void SweepServo2::disable() { + this->enabled = false; +} + +bool SweepServo2::isEnabled() { + return this->enabled; +} + +// Accessor for servo object +Servo SweepServo2::getServo() +{ + return this->servo; +} + + +// Check whether we have already configured this servo +bool haveServo(int pin) { + int i; + for (i = 0; i < nServos; i++) { + if (myServoPins[i] == pin) return true; + } + return false; +} + +#endif + diff --git a/ros_arduino_msgs/CMakeLists.txt b/ros_arduino_msgs/CMakeLists.txt index 373ae0af..dbcbf4d5 100644 --- a/ros_arduino_msgs/CMakeLists.txt +++ b/ros_arduino_msgs/CMakeLists.txt @@ -12,13 +12,30 @@ add_message_files(FILES ) add_service_files(FILES + AnalogWrite.srv + AnalogSensorWrite.srv + AnalogFloatSensorWrite.srv + AnalogPinMode.srv + AnalogRead.srv + AnalogSensorRead.srv + AnalogFloatSensorRead.srv + DigitalPinMode.srv + DigitalRead.srv + DigitalSensorRead.srv DigitalSetDirection.srv + DigitalSensorPinMode.srv DigitalWrite.srv - DigitalRead.srv + DigitalSensorWrite.srv + Enable.srv + Relax.srv + AnalogSensorRead.srv + ServoAttach.srv + ServoDetach.srv ServoRead.srv ServoWrite.srv - AnalogWrite.srv - AnalogRead.srv + SetSpeed.srv + SetServoSpeed.srv + UpdatePID.srv ) generate_messages( diff --git a/ros_arduino_msgs/msg/AnalogFloat.msg b/ros_arduino_msgs/msg/AnalogFloat.msg index 1f78bb11..899697a1 100644 --- a/ros_arduino_msgs/msg/AnalogFloat.msg +++ b/ros_arduino_msgs/msg/AnalogFloat.msg @@ -1,3 +1,3 @@ # Float message from a single analog IO pin. Header header -float64 value \ No newline at end of file +float32 value \ No newline at end of file diff --git a/ros_arduino_msgs/srv/AnalogFloatSensorRead.srv b/ros_arduino_msgs/srv/AnalogFloatSensorRead.srv new file mode 100755 index 00000000..be1ec078 --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogFloatSensorRead.srv @@ -0,0 +1,2 @@ +--- +float32 value \ No newline at end of file diff --git a/ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv b/ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv new file mode 100644 index 00000000..effb1670 --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv @@ -0,0 +1,2 @@ +float32 value +--- diff --git a/ros_arduino_msgs/srv/AnalogPinMode.srv b/ros_arduino_msgs/srv/AnalogPinMode.srv new file mode 100755 index 00000000..01a2e12c --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogPinMode.srv @@ -0,0 +1,3 @@ +uint8 pin +bool direction +--- diff --git a/ros_arduino_msgs/srv/AnalogSensorRead.srv b/ros_arduino_msgs/srv/AnalogSensorRead.srv new file mode 100755 index 00000000..ed4504b8 --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogSensorRead.srv @@ -0,0 +1,2 @@ +--- +uint16 value \ No newline at end of file diff --git a/ros_arduino_msgs/srv/AnalogSensorWrite.srv b/ros_arduino_msgs/srv/AnalogSensorWrite.srv new file mode 100644 index 00000000..9ce3d3c0 --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogSensorWrite.srv @@ -0,0 +1,2 @@ +uint16 value +--- diff --git a/ros_arduino_msgs/srv/DigitalPinMode.srv b/ros_arduino_msgs/srv/DigitalPinMode.srv new file mode 100755 index 00000000..01a2e12c --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalPinMode.srv @@ -0,0 +1,3 @@ +uint8 pin +bool direction +--- diff --git a/ros_arduino_msgs/srv/DigitalSensorPinMode.srv b/ros_arduino_msgs/srv/DigitalSensorPinMode.srv new file mode 100755 index 00000000..2b76f950 --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSensorPinMode.srv @@ -0,0 +1,2 @@ +bool direction +--- diff --git a/ros_arduino_msgs/srv/DigitalSensorRead.srv b/ros_arduino_msgs/srv/DigitalSensorRead.srv new file mode 100755 index 00000000..e3e4ce7c --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSensorRead.srv @@ -0,0 +1,2 @@ +--- +bool value diff --git a/ros_arduino_msgs/srv/DigitalSensorWrite.srv b/ros_arduino_msgs/srv/DigitalSensorWrite.srv new file mode 100755 index 00000000..662c8fee --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSensorWrite.srv @@ -0,0 +1,2 @@ +bool value +--- diff --git a/ros_arduino_msgs/srv/Enable.srv b/ros_arduino_msgs/srv/Enable.srv new file mode 100755 index 00000000..aea1a109 --- /dev/null +++ b/ros_arduino_msgs/srv/Enable.srv @@ -0,0 +1,3 @@ +bool enable +--- +bool state \ No newline at end of file diff --git a/ros_arduino_msgs/srv/Relax.srv b/ros_arduino_msgs/srv/Relax.srv new file mode 100755 index 00000000..88a0cbf0 --- /dev/null +++ b/ros_arduino_msgs/srv/Relax.srv @@ -0,0 +1,2 @@ + +--- diff --git a/ros_arduino_msgs/srv/ServoAttach.srv b/ros_arduino_msgs/srv/ServoAttach.srv new file mode 100755 index 00000000..9233d46e --- /dev/null +++ b/ros_arduino_msgs/srv/ServoAttach.srv @@ -0,0 +1,3 @@ +uint8 id +--- + diff --git a/ros_arduino_msgs/srv/ServoDetach.srv b/ros_arduino_msgs/srv/ServoDetach.srv new file mode 100755 index 00000000..9233d46e --- /dev/null +++ b/ros_arduino_msgs/srv/ServoDetach.srv @@ -0,0 +1,3 @@ +uint8 id +--- + diff --git a/ros_arduino_msgs/srv/ServoRead.srv b/ros_arduino_msgs/srv/ServoRead.srv index fd68ffaa..9d4e6fd0 100755 --- a/ros_arduino_msgs/srv/ServoRead.srv +++ b/ros_arduino_msgs/srv/ServoRead.srv @@ -1,3 +1,3 @@ uint8 id --- -float32 value +uint8 position diff --git a/ros_arduino_msgs/srv/ServoWrite.srv b/ros_arduino_msgs/srv/ServoWrite.srv index 5a9170ba..1c2473a5 100755 --- a/ros_arduino_msgs/srv/ServoWrite.srv +++ b/ros_arduino_msgs/srv/ServoWrite.srv @@ -1,3 +1,3 @@ uint8 id -float32 value +uint8 position --- diff --git a/ros_arduino_msgs/srv/SetServoSpeed.srv b/ros_arduino_msgs/srv/SetServoSpeed.srv new file mode 100755 index 00000000..667284e6 --- /dev/null +++ b/ros_arduino_msgs/srv/SetServoSpeed.srv @@ -0,0 +1,3 @@ +uint8 id +float32 speed +--- diff --git a/ros_arduino_msgs/srv/SetSpeed.srv b/ros_arduino_msgs/srv/SetSpeed.srv new file mode 100755 index 00000000..ef7ce7dd --- /dev/null +++ b/ros_arduino_msgs/srv/SetSpeed.srv @@ -0,0 +1,2 @@ +float32 speed +--- diff --git a/ros_arduino_msgs/srv/UpdatePID.srv b/ros_arduino_msgs/srv/UpdatePID.srv new file mode 100755 index 00000000..a97f2e7b --- /dev/null +++ b/ros_arduino_msgs/srv/UpdatePID.srv @@ -0,0 +1,5 @@ +float32 Kp +float32 Kd +float32 Ki +float32 Ko +--- diff --git a/ros_arduino_python/CMakeLists.txt b/ros_arduino_python/CMakeLists.txt index a79bb49f..6ff5f642 100644 --- a/ros_arduino_python/CMakeLists.txt +++ b/ros_arduino_python/CMakeLists.txt @@ -1,10 +1,16 @@ cmake_minimum_required(VERSION 2.8.3) project(ros_arduino_python) -find_package(catkin REQUIRED) -catkin_package(DEPENDS) +find_package(catkin REQUIRED dynamic_reconfigure) + catkin_python_setup() +generate_dynamic_reconfigure_options(cfg/ROSArduinoBridge.cfg) + +catkin_package(DEPENDS) + +catkin_package(CATKIN_DEPENDS dynamic_reconfigure) + install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) @@ -16,3 +22,5 @@ install(DIRECTORY launch install(DIRECTORY nodes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + + diff --git a/ros_arduino_python/cfg/ROSArduinoBridge.cfg b/ros_arduino_python/cfg/ROSArduinoBridge.cfg new file mode 100755 index 00000000..1bf775c3 --- /dev/null +++ b/ros_arduino_python/cfg/ROSArduinoBridge.cfg @@ -0,0 +1,23 @@ +#! /usr/bin/env python + +PACKAGE = "ros_arduino_python" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("Kp", double_t, 0, "Kp - Proportional", 10.0, 0.0, 50.0) + +gen.add("Kd", double_t, 0, "Kd - Differential", 12.0, 0.0, 50.0) + +gen.add("Ki", double_t, 0, "Ki - Integral", 0.0, 0.0, 10.0) + +gen.add("Ko", double_t, 0, "Ko - Offset", 50.0, 0.0, 100.0) + +gen.add("accel_limit", double_t, 0, "Acceleration limit", 1.0, 0.0, 10.0) + +gen.add("odom_linear_scale_correction", double_t, 0, "Linear Scale Correction", 1.0, 0.0, 2.0) + +gen.add("odom_angular_scale_correction", double_t, 0, "Angular Scale Correction", 1.0, 0.0, 2.0) + +exit(gen.generate(PACKAGE, "ros_arduino_bridge", "ROSArduinoBridge")) diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index 9acbe3dc..50e581ac 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -5,17 +5,26 @@ port: /dev/ttyACM0 baud: 57600 -timeout: 0.1 +timeout: 0.5 rate: 50 sensorstate_rate: 10 +# Are we using the base controller code use_base_controller: False + +# Rate to publish odom info base_controller_rate: 10 # For a robot that uses base_footprint, change base_frame to base_footprint base_frame: base_link +# Are we also using an IMU for heading data +use_imu_heading: False + +# Publish the odom->base_link transform? +publish_odom_base_transform: True + # === Robot drivetrain parameters #wheel_diameter: 0.146 #wheel_track: 0.2969 @@ -30,8 +39,12 @@ base_frame: base_link #Ko: 50 #accel_limit: 1.0 +# == Odometry calibration correction factors +odom_linear_scale_correction: 1.0 +odom_angular_scale_correction: 1.0 + # === Sensor definitions. Examples only - edit for your robot. -# Sensor type can be one of the follow (case sensitive!): +# Sensor type can be one of the following: # * Ping # * GP2D12 # * Analog @@ -41,11 +54,16 @@ base_frame: base_link # * 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} + onboard_led: {pin: 13, type: Digital, rate: 5, direction: output} +} + +# Joint name and configuration is an example only +joints: { + head_pan_joint: {pin: 3, init_position: 0, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False}, + head_tilt_joint: {pin: 5, init_position: 0, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False} } diff --git a/ros_arduino_python/config/my_arduino_params.yaml b/ros_arduino_python/config/my_arduino_params.yaml new file mode 100644 index 00000000..2184ef6b --- /dev/null +++ b/ros_arduino_python/config/my_arduino_params.yaml @@ -0,0 +1,70 @@ +# 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.5 + +rate: 50 +sensorstate_rate: 10 + +# Are we using the base controller code +use_base_controller: True + +# Rate to publish odom info +base_controller_rate: 10 + +# For a robot that uses base_footprint, change base_frame to base_footprint +base_frame: base_footprint + +# Are we also using an IMU for heading data +use_imu_heading: True + +# Publish the odom->base_link transform? +publish_odom_base_transform: False + +# === Robot drivetrain parameters +wheel_diameter: 0.065 +wheel_track: 0.213 +encoder_resolution: 26 # from Pololu for 131:1 motors +gear_reduction: 30 +motors_reversed: True + +# === PID parameters +Kp: 10 +Kd: 12 +Ki: 0 +Ko: 50 +accel_limit: 1.0 + +# == Odometry calibration correction factors +odom_linear_scale_correction: 1.0 +odom_angular_scale_correction: 1.0 + +# === Sensor definitions. Examples only - edit for your robot. +# Sensor type can be one of the following: +# * 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}, + #onboard_led: {pin: 13, type: Digital, rate: 5, direction: output}, + GY85: {pin: 21, type: IMU, rate: 5} +} + +# Joint name and configuration is an example only +joints: { +# head_pan_joint: {pin: 3, init_position: 0, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False}, +# head_tilt_joint: {pin: 5, init_position: 0, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False} +} diff --git a/ros_arduino_python/config/servo_example_params.yaml b/ros_arduino_python/config/servo_example_params.yaml new file mode 100644 index 00000000..73409188 --- /dev/null +++ b/ros_arduino_python/config/servo_example_params.yaml @@ -0,0 +1,21 @@ +# A UDEV rule maps the Arduino serial number to /dev/arduino + +port: /dev/arduino +baud: 57600 +timeout: 0.5 + +rate: 30 + +sensorstate_rate: 10 +use_base_controller: False +joint_update_rate: 10 + + +sensors: { + onboard_led: {pin: 13, type: Digital, direction: output, rate: 1} +} + +# Joint name and pin assignment is an example only +joints: { + head_pan_joint: {pin: 3, init_position: 0, init_speed: 60, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False} +} diff --git a/ros_arduino_python/examples/sweep_servo.py b/ros_arduino_python/examples/sweep_servo.py new file mode 100755 index 00000000..eb75df46 --- /dev/null +++ b/ros_arduino_python/examples/sweep_servo.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python + +''' sweep_servo.py - Version 1.0 2015-12-04 + + Move a servo back and forth between two positions + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2015 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.5 + + 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 +import sys +import os +from std_msgs.msg import Float64 +from ros_arduino_msgs.srv import SetSpeed +from sensor_msgs.msg import JointState +from math import radians + +class SweepServo(): + def __init__(self): + # Set a name for the node + self.node_name = "sweep_servo" + + # Initialize the node + rospy.init_node(self.node_name) + + # Set a shutdown function to clean up when termniating the node + rospy.on_shutdown(self.shutdown) + + # Name of the joint we want to control + joint_name = rospy.get_param('~joint', 'head_pan_joint') + + if joint_name is None or joint_name == '': + rospy.logino("Joint name for servo must be specified in parameter file.") + os._exit(1) + + max_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_position')) + min_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_position')) + + target_max = max_position + target_min = min_position + + # How fast should we sweep the servo + servo_speed = rospy.get_param('~servo_speed', 1.0) # rad/s + + # Time delay between between sweeps + delay = rospy.get_param('~delay', 0) # seconds + + # Create a publisher for setting the joint position + joint_pub = rospy.Publisher('/' + joint_name + '/command', Float64, queue_size=5) + + self.joint_state = JointState() + + # Subscribe to the /joint_states topic so we know where the servo is positioned + rospy.Subscriber('/joint_states', JointState, self.update_joint_state) + + # Wait for the /joint_state message to come online + rospy.wait_for_message('/joint_states', JointState, 60) + + # Wait until we actually have a message + while self.joint_state == JointState(): + rospy.sleep(0.1) + + # Wait for the set_speed service + rospy.loginfo('Waiting for set_speed services...') + try: + rospy.wait_for_service('/' + joint_name + '/set_speed', 60) + rospy.loginfo('Ready.') + except: + rospy.loginfo('Could not connect to service!') + os._exit(1) + + # Create a proxy for the set speed service + set_speed = rospy.ServiceProxy('/' + joint_name + '/set_speed', SetSpeed) + + # Set the initial servo speed + set_speed(servo_speed) + + rospy.loginfo('Sweeping servo...') + + while not rospy.is_shutdown(): + while abs(self.get_joint_position(joint_name) - target_max) > 0.1: + joint_pub.publish(target_max) + rospy.sleep(0.1) + + rospy.sleep(delay) + + while abs(self.get_joint_position(joint_name) - target_min) > 0.1: + joint_pub.publish(target_min) + rospy.sleep(0.1) + + rospy.sleep(delay) + + def update_joint_state(self, msg): + self.joint_state = msg + + def get_joint_position(self, joint_name): + index = self.joint_state.name.index(joint_name) + return self.joint_state.position[index] + + def shutdown(self): + rospy.loginfo('Shutting down ' + str(self.node_name)) + os._exit(0) + +if __name__ == '__main__': + try: + SweepServo() + except: + rospy.loginfo('Unexpected error: ' + str(sys.exc_info()[0])) + raise \ No newline at end of file diff --git a/ros_arduino_python/launch/arduino.launch b/ros_arduino_python/launch/arduino.launch index 0dd803a9..03516bb9 100755 --- a/ros_arduino_python/launch/arduino.launch +++ b/ros_arduino_python/launch/arduino.launch @@ -1,5 +1,5 @@ - + diff --git a/ros_arduino_python/launch/servo_example.launch b/ros_arduino_python/launch/servo_example.launch new file mode 100755 index 00000000..d0097bb9 --- /dev/null +++ b/ros_arduino_python/launch/servo_example.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ros_arduino_python/launch/sweep_servo.launch b/ros_arduino_python/launch/sweep_servo.launch new file mode 100755 index 00000000..adcab6f8 --- /dev/null +++ b/ros_arduino_python/launch/sweep_servo.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 103179d5..a40f9891 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -2,7 +2,7 @@ """ 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. @@ -10,12 +10,12 @@ 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 """ @@ -23,187 +23,471 @@ 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.diagnostics import DiagnosticsUpdater, DiagnosticsPublisher +from ros_arduino_python.cfg import ROSArduinoBridgeConfig from ros_arduino_python.base_controller import BaseController +from ros_arduino_python.servo_controller import Servo, ServoController +from ros_arduino_python.follow_controller import FollowController +from ros_arduino_python.joint_state_publisher import JointStatePublisher +import dynamic_reconfigure.server +import dynamic_reconfigure.client from geometry_msgs.msg import Twist -import os, time -import thread +from std_srvs.srv import Empty, EmptyResponse +import os, time, thread +from math import radians +from serial.serialutil import SerialException + +controller_types = { "follow_controller" : FollowController } class ArduinoROS(): def __init__(self): - rospy.init_node('Arduino', log_level=rospy.DEBUG) - + rospy.init_node('arduino', log_level=rospy.INFO) + + # Find the actual node name in case it is set in the launch file + self.name = rospy.get_name() + # 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') - + self.motors_reversed = rospy.get_param("~motors_reversed", False) # Overall loop rate: should be faster than fastest sensor rate - self.rate = int(rospy.get_param("~rate", 50)) + self.rate = int(rospy.get_param("~rate", 30)) r = rospy.Rate(self.rate) # Rate at which summary SensorState message is published. Individual sensors publish - # at their own rates. + # at their own rates. self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) - + + # Are we using the base_controller? self.use_base_controller = rospy.get_param("~use_base_controller", False) - + + # Default error threshold (percent) before getting a diagnostics warning + self.diagnotics_error_threshold = rospy.get_param("~diagnotics_error_threshold", 10) + + # Diagnostics update rate + self.diagnotics_rate = rospy.get_param("~diagnotics_rate", 1.0) + + # Assume we don't have any joints by default + self.have_joints = 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 attach a PWM servo to a specified pin + rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler) + + # A service to detach a PWM servo to a specified pin + rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler) + # 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_pin_mode', DigitalPinMode, self.DigitalPinModeHandler) + + # Obsolete: Use DigitalPinMode instead rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler) - + + # A service to turn set the direction of an analog pin (0 = input, 1 = output) + rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler) + # 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) + 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) - + # Initialize the device + self.device = Arduino(self.port, self.baud, self.timeout, self.motors_reversed, debug=True) + # Make the connection - self.controller.connect() - - rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") - + if self.device.connect(): + rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") + else: + rospy.logerr("No serial connection found.") + os._exit(0) + + # Initialize the base controller if used + if self.use_base_controller: + self.base_controller = BaseController(self.device, self.base_frame, self.name + "_base_controller") + + # A service to reset the odometry values to 0 + rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler) + + # A service to update the PID parameters Kp, Kd, Ki, and Ko + rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler) + + # Fire up the dynamic_reconfigure server + dyn_server = dynamic_reconfigure.server.Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback, namespace=self.name) + + # Connect to the dynamic_reconfigure client + dyn_client = dynamic_reconfigure.client.Client (self.name, timeout=5) + # Reserve a thread lock mutex = thread.allocate_lock() # Initialize any sensors - self.mySensors = list() - + self.device.sensors = list() + + # Keep a list of IMUs or gyros used for odometry so they can be reset + self.imu_for_odom = list() + + # Read in the sensors parameter dictionary sensor_params = rospy.get_param("~sensors", dict({})) - + + # Initialize individual sensors appropriately 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": + if params['type'].lower() == 'Ping'.lower(): + sensor = Ping(self.device, name, **params) + elif params['type'].lower() == 'GP2D12'.lower() or params['type'].lower() == 'GP2Y0A21YK0F'.lower(): + sensor = GP2D12(self.device, name, **params) + elif params['type'].lower() == 'Digital'.lower(): + sensor = DigitalSensor(self.device, name, **params) + elif params['type'].lower() == 'Analog'.lower(): + sensor = AnalogSensor(self.device, name, **params) + elif params['type'].lower() == 'AnalogFloat'.lower(): + sensor = AnalogFloatSensor(self.device, name, **params) + elif params['type'].lower() == 'PololuMotorCurrent'.lower(): + sensor = PololuMotorCurrent(self.device, name, **params) + elif params['type'].lower() == 'PhidgetsVoltage'.lower(): + sensor = PhidgetsVoltage(self.device, name, **params) + elif params['type'].lower() == 'PhidgetsCurrent'.lower(): + sensor = PhidgetsCurrent(self.device, name, **params) + elif params['type'].lower() == 'IMU'.lower(): + sensor = IMU(self.device, name, **params) + try: + if params['use_for_odom']: + self.imu_for_odom.append(sensor) + except: + pass + elif params['type'].lower() == 'Gyro'.lower(): + try: + sensor = Gyro(self.device, name, base_controller=self.base_controller, **params) + except: + sensor = Gyro(self.device, name, **params) + + try: + if params['use_for_odom']: + self.imu_for_odom.append(sensor) + except: + pass + +# if params['type'].lower() == 'MaxEZ1'.lower(): # 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 + try: + self.device.sensors.append(sensor) + + if params['rate'] != None and params['rate'] != 0: + rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) + else: + if sensor.direction == "input": + rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read") + else: + rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write") + except: + rospy.logerr("Sensor type " + str(params['type'] + " not recognized.")) + + # If at least one IMU or gyro is used for odometry, set the use_imu_heading flag on the base controller + if self.use_base_controller and len(self.imu_for_odom) > 0: + self.base_controller.use_imu_heading = True + + # Read in the joints (if any) + joint_params = rospy.get_param("~joints", dict()) + + if len(joint_params) != 0: + self.have_joints = True + + self.device.joints = dict() + + self.device.joint_update_rate = float(rospy.get_param("~joint_update_rate", 10)) + + # Configure each servo + for name, params in joint_params.iteritems(): + try: + if params['continuous'] == True: + self.device.joints[name] = ContinuousServo(self.device, name) + else: + self.device.joints[name] = Servo(self.device, name) + except: + self.device.joints[name] = Servo(self.device, name) + + # Display the joint setup on the terminal + rospy.loginfo(name + " " + str(params)) + + # The servo controller determines when to read and write position values to the servos + self.servo_controller = ServoController(self.device, "ServoController") + + # The joint state publisher publishes the latest joint values on the /joint_states topic + self.joint_state_publisher = JointStatePublisher() + + # Create the diagnostics updater for the Arduino device + self.device.diagnostics = DiagnosticsUpdater(self, self.name, self.diagnotics_error_threshold, self.diagnotics_rate, create_watchdog=True) + + # Create the overall diagnostics publisher + self.diagnostics_publisher = DiagnosticsPublisher(self) + + # Initialize any trajectory action follow controllers + controllers = rospy.get_param("~controllers", dict()) + + self.device.controllers = list() + + for name, params in controllers.items(): + try: + controller = controller_types[params["type"]](self.device, name) + self.device.controllers.append(controller) + except Exception as e: + if type(e) == KeyError: + rospy.logerr("Unrecognized controller: " + params["type"]) + else: + rospy.logerr(str(type(e)) + str(e)) + + for controller in self.device.controllers: + controller.startup() + + print "\n==> ROS Arduino Bridge ready for action!" + + # Start polling the sensors, base controller, and servo controller while not rospy.is_shutdown(): - for sensor in self.mySensors: - mutex.acquire() - sensor.poll() - mutex.release() - + # Heartbeat/watchdog test for the serial connection + try: + # Update read counters + self.device.diagnostics.reads += 1 + self.device.diagnostics.total_reads += 1 + self.device.serial_port.inWaiting() + # Add this heartbeat to the frequency status diagnostic task + self.device.diagnostics.freq_diag.tick() + # Let the diagnostics updater know we're still alive + self.device.diagnostics.watchdog = True + except IOError: + # Update error counter + self.device.diagnostics.errors += 1 + rospy.loginfo("Lost serial connection. Waiting to reconnect...") + # Let the diagnostics updater know that we're down + self.device.diagnostics.watchdog = False + self.device.close() + with mutex: + while True: + try: + self.device.open() + while True: + self.device.serial_port.write('\r') + test = self.device.serial_port.readline().strip('\n').strip('\r') + rospy.loginfo("Waking up serial port...") + if test == 'Invalid Command': + self.device.serial_port.flushInput() + self.device.serial_port.flushOutput() + break + rospy.loginfo("Serial connection re-established.") + break + except: + r.sleep() + self.diagnostics_publisher.update() + continue + + # Poll any sensors + for sensor in self.device.sensors: + if sensor.rate != 0: + with mutex: + sensor.poll() + + # Poll the base controller if self.use_base_controller: - mutex.acquire() - self.myBaseController.poll() - mutex.release() - + with mutex: + self.base_controller.poll() + + # Poll any joints + if self.have_joints: + with mutex: + self.servo_controller.poll() + self.joint_state_publisher.poll(self.device.joints.values()) + # 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) + for i in range(len(self.device.sensors)): + if self.device.sensors[i].message_type != MessageType.IMU: + msg.name.append(self.device.sensors[i].name) + msg.value.append(self.device.sensors[i].value) try: self.sensorStatePub.publish(msg) except: pass - + self.t_next_sensors = now + self.t_delta_sensors - + + # Update diagnostics and publish + self.diagnostics_publisher.update() + r.sleep() - + # Service callback functions + def ServoAttachHandler(self, req): + self.device.attach_servo(req.id) + return ServoAttachResponse() + + def ServoDetachHandler(self, req): + self.device.detach_servo(req.id) + return ServoDetachResponse() + def ServoWriteHandler(self, req): - self.controller.servo_write(req.id, req.value) + self.device.servo_write(req.id, req.position) return ServoWriteResponse() - + +# def ServoSpeedWriteHandler(self, req): +# delay = +# self.device.servo_delay(req.id, delay) +# return ServoSpeedResponse() +# +# # Convert servo speed in deg/s to a step delay in milliseconds +# step_delay = self.device.joints[name].get_step_delay(req.value) +# +# # Update the servo speed +# self.device.config_servo(pin, step_delay) +# +# return SetServoSpeedResponse() + def ServoReadHandler(self, req): - pos = self.controller.servo_read(req.id) - return ServoReadResponse(pos) - + position = self.device.servo_read(req.id) + return ServoReadResponse(position) + + def DigitalPinModeHandler(self, req): + self.device.digital_pin_mode(req.pin, req.direction) + return DigitalPinModeResponse() + + # Obsolete: use DigitalPinMode instead def DigitalSetDirectionHandler(self, req): - self.controller.pin_mode(req.pin, req.direction) + self.device.digital_pin_mode(req.pin, req.direction) return DigitalSetDirectionResponse() - + def DigitalWriteHandler(self, req): - self.controller.digital_write(req.pin, req.value) + self.device.digital_write(req.pin, req.value) return DigitalWriteResponse() - + def DigitalReadHandler(self, req): - value = self.controller.digital_read(req.pin) + value = self.device.digital_read(req.pin) return DigitalReadResponse(value) - + + def AnalogPinModeHandler(self, req): + self.device.analog_pin_mode(req.pin, req.direction) + return AnalogPinModeResponse() + def AnalogWriteHandler(self, req): - self.controller.analog_write(req.pin, req.value) + self.device.analog_write(req.pin, req.value) return AnalogWriteResponse() - + def AnalogReadHandler(self, req): - value = self.controller.analog_read(req.pin) + value = self.device.analog_read(req.pin) return AnalogReadResponse(value) - + + def ResetOdometryHandler(self, req): + if self.use_base_controller: + self.base_controller.reset_odometry() + for imu in self.imu_for_odom: + imu.reset() + else: + rospy.logwarn("Not using base controller!") + return EmptyResponse() + + def UpdatePIDHandler(self, req): + if self.use_base_controller: + self.device.update_pid(req.Kp, req.Kd, req.Ki, req.Ko) + rospy.loginfo("Updating PID parameters: Kp=%0.3f, Kd=%0.3f, Ki=%0.3f, Ko=%0.3f" %(req.Kp, req.Kd, req.Ki, req.Ko)) + else: + rospy.logwarn("Not using base controller!") + return UpdatePIDResponse() + + def dynamic_reconfigure_server_callback(self, config, level): + if self.use_base_controller: + try: + if self.base_controller.Kp != config['Kp']: + self.base_controller.Kp = config['Kp'] + + if self.base_controller.Kd != config['Kd']: + self.base_controller.Kd = config['Kd'] + + if self.base_controller.Ki != config['Ki']: + self.base_controller.Ki = config['Ki'] + + if self.base_controller.Ko != config['Ko']: + self.base_controller.Ko = config['Ko'] + + if self.base_controller.accel_limit != config['accel_limit']: + self.base_controller.accel_limit = config['accel_limit'] + self.base_controller.max_accel = self.base_controller.accel_limit * self.base_controller.ticks_per_meter / self.base_controller.rate + + if self.base_controller.odom_linear_scale_correction != config['odom_linear_scale_correction']: + self.base_controller.odom_linear_scale_correction = config['odom_linear_scale_correction'] + + if self.base_controller.odom_angular_scale_correction != config['odom_angular_scale_correction']: + self.base_controller.odom_angular_scale_correction = config['odom_angular_scale_correction'] + + self.device.update_pid(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko) + + rospy.loginfo("Updating PID parameters: Kp=%0.2f, Kd=%0.2f, Ki=%0.2f, Ko=%0.2f, accel_limit=%0.2f" %(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko, self.base_controller.accel_limit)) + except Exception as e: + print e + + return config + def shutdown(self): + rospy.loginfo("Shutting down Arduino node...") + # Stop the robot - try: + if self.use_base_controller: rospy.loginfo("Stopping the robot...") - self.cmd_vel_pub.Publish(Twist()) + self.cmd_vel_pub.publish(Twist()) rospy.sleep(2) - except: - pass - rospy.loginfo("Shutting down Arduino Node...") - + + # Detach any servos + if self.have_joints: + rospy.loginfo("Detaching servos...") + for joint in self.device.joints.values(): + self.device.detach_servo(joint.pin) + rospy.sleep(0.2) + + # Close the serial port + self.device.close() + if __name__ == '__main__': - myArduino = ArduinoROS() + try: + myArduino = ArduinoROS() + except KeyboardInterrupt: + try: + myArduino.device.serial_port.close() + except: + rospy.logerr("Serial exception trying to close port.") + os._exit(0) + except SerialException: + rospy.logerr("Serial exception trying to open port.") + os._exit(0) diff --git a/ros_arduino_python/package.xml b/ros_arduino_python/package.xml index 5bd96a00..c1508eda 100644 --- a/ros_arduino_python/package.xml +++ b/ros_arduino_python/package.xml @@ -10,13 +10,23 @@ http://ros.org/wiki/ros_arduino_python catkin + + dynamic_reconfigure + diagnostic_updater + diagnostic_aggregator + diagnostic_msgs rospy std_msgs sensor_msgs geometry_msgs nav_msgs + diagnostic_msgs + diagnostic_updater + diagnostic_aggregator tf ros_arduino_msgs python-serial + dynamic_reconfigure + diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py index f210d144..9d0527dc 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -1,374 +1,382 @@ -#!/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." - +#!/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 rospy +import thread +from math import pi as PI +import os, time, sys, traceback +from serial.serialutil import SerialException, SerialTimeoutException +import serial +from exceptions import Exception + +# Possible errors when reading/writing to the Arduino +class CommandErrorCode: + SUCCESS = 0 + TIMEOUT = 1 + NOVALUE = 2 + SERIALEXCEPTION = 3 + + ErrorCodeStrings = ['SUCCESS', 'TIMEOUT', 'NOVALUE', 'SERIALEXCEPTION'] + +# An exception class to handle these errors +class CommandException(Exception): + def __init__(self, code): + self.code = code + def __str__(self): + return repr(self.code) + +class Arduino: + def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False, debug=False): + + 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.motors_reversed = motors_reversed + self.encoder_count = 0 + self.writeTimeout = timeout + self.debug = debug + + # Keep things thread safe + self.mutex = thread.allocate_lock() + + def connect(self): + try: + rospy.loginfo("Looking for the Arduino on port " + str(self.port) + " ...") + + # The port has to be open once with the default baud rate before opening again for real + self.serial_port = serial.Serial(port=self.port) + + # Needed for Leonardo only + while not self.serial_port.isOpen(): + time.sleep(self.timeout) + + # Now open the port with the real settings. An initial timeout of at least 1.0 seconds seems to work best + self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=max(1.0, self.timeout)) + + # It can take time for the serial port to wake up + max_attempts = 10 + attempts = 0 + timeout = self.timeout + + self.serial_port.write('\r') + time.sleep(timeout) + test = self.serial_port.read() + + # Wake up the serial port + while attempts < max_attempts and test == '': + attempts += 1 + self.serial_port.write('\r') + time.sleep(timeout) + test = self.serial_port.read() + rospy.loginfo("Waking up serial port attempt " + str(attempts) + " of " + str(max_attempts)) + + if test == '': + raise SerialException + + # Reset the timeout to the user specified timeout + self.serial_port.timeout = self.timeout + self.serial_port.write_timeout = self.timeout + + # Test the connection by reading the baudrate + attempts = 0 + while self.get_baud() != self.baudrate and attempts < max_attempts: + attempts += 1 + self.serial_port.flushInput() + self.serial_port.flushOutput() + rospy.loginfo("Connecting...") + time.sleep(timeout) + try: + self.serial_port.inWaiting() + rospy.loginfo("Connected at " + str(self.baudrate)) + rospy.loginfo("Arduino is ready!") + except IOError: + raise SerialException + + except SerialException: + rospy.logerr("Serial Exception:") + rospy.logerr(sys.exc_info()) + rospy.logerr("Traceback follows:") + traceback.print_exc(file=sys.stdout) + rospy.logerr("Cannot connect to Arduino! Make sure it is plugged in to your computer.") + return False + + return True + + def open(self): + ''' Open the serial port. + ''' + self.serial_port.open() + + def close(self): + ''' Close the serial port. + ''' + self.serial_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.serial_port.write(cmd + '\r') + + def execute(self, cmd, timeout=0.5): + ''' Thread safe execution of "cmd" on the Arduino returning a single value. + ''' + self.mutex.acquire() + + value = None + error = None + + try: + start = time.time() + self.serial_port.flushInput() + self.serial_port.flushOutput() + self.serial_port.write(cmd + '\r') + value = self.serial_port.readline().strip('\n').strip('\r') + except SerialException: + self.print_debug_msg("Command " + str(cmd) + " failed with Serial Exception") + error = CommandErrorCode.SERIALEXCEPTION + except SerialTimeoutException: + self.print_debug_msg("Command " + str(cmd) + " timed out") + error = CommandErrorCode.TIMEOUT + + duration = time.time() - start + + if error is None and (value is None or len(value) == 0): + duration = time.time() - start + if duration > timeout: + self.print_debug_msg("Command " + str(cmd) + " timed out") + error = CommandErrorCode.TIMEOUT + else: + self.print_debug_msg("Command " + str(cmd) + " did not return a value") + error = CommandErrorCode.NOVALUE + + self.mutex.release() + + if error: + raise CommandException(error) + + return value + + def execute_array(self, cmd): + ''' Thread safe execution of "cmd" on the Arduino returning an array. + ''' + try: + values = self.execute(cmd).split() + except CommandException as e: + values = None + + return values + + def execute_ack(self, cmd): + ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. + ''' + try: + ack = self.execute(cmd) + return ack == "OK" + except CommandException as e: + return False + + def print_debug_msg(self, msg): + if self.debug: + rospy.logwarn(msg) + + def update_pid(self, Kp, Kd, Ki, Ko): + ''' Set the PID parameters on the Arduino + ''' + cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko) + return self.execute_ack(cmd) + + def get_baud(self): + ''' Get the current baud rate on the serial port. + ''' + try: + return int(self.execute('b')) + except: + return None + + def get_encoder_counts(self): + ''' Read the current encoder counts + ''' + values = self.execute_array('e') + + if len(values) != 2: + return None + else: + if self.motors_reversed: + values[0], values[1] = -int(values[0]), -int(values[1]) + return map(int, values) + + def reset_encoders(self): + ''' Reset the encoder counts to 0 + ''' + return self.execute_ack('r') + + def get_imu_data(self): + ''' + IMU data is assumed to be returned in the following order: + + [ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch] + + where a stands for accelerometer, g for gyroscope and m for magnetometer. + The last value ch stands for "compensated heading" that some IMU's can + compute to compensate magnetic heading from the current roll and pitch. + ''' + values = self.execute_array('i') + + if len(values) != 9: + return None + else: + return map(float, values) + + def drive(self, right, left): + ''' Speeds are given in encoder ticks per PID interval + ''' + if self.motors_reversed: + left, right = -left, -right + 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_pin_mode(self, pin, mode): + return self.execute_ack('c A%d %d' %(pin, mode)) + + def analog_read(self, pin): + try: + return int(self.execute('a %d' %pin)) + except: + return None + + def analog_write(self, pin, value): + return self.execute_ack('x %d %d' %(pin, value)) + + def digital_read(self, pin): + try: + return int(self.execute('d %d' %pin)) + except: + return None + + def digital_write(self, pin, value): + return self.execute_ack('w %d %d' %(pin, value)) + + def digital_pin_mode(self, pin, mode): + return self.execute_ack('c %d %d' %(pin, mode)) + + def config_servo(self, pin, step_delay): + ''' Configure a PWM servo ''' + return self.execute_ack('j %d %u' %(pin, step_delay)) + + def servo_write(self, id, pos): + ''' Usage: servo_write(id, pos) + Position is given in degrees from 0-180 + ''' + return self.execute_ack('s %d %d' %(id, pos)) + + def servo_read(self, id): + ''' Usage: servo_read(id) + The returned position is in degrees + ''' + return int(self.execute('t %d' %id)) + + def set_servo_delay(self, id, delay): + ''' Usage: set_servo_delay(id, delay) + Set the delay in ms inbetween servo position updates. Controls speed of servo movement. + ''' + return self.execute_ack('v %d %d' %(id, delay)) + + def detach_servo(self, id): + ''' Usage: detach_servo(id) + Detach a servo from control by the Arduino + ''' + return self.execute_ack('z %d' %id) + + def attach_servo(self, id): + ''' Usage: attach_servo(id) + Attach a servo to the Arduino + ''' + return self.execute_ack('y %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,q + and returns the range in cm. Sonar distance resolution is integer based. + ''' + return int(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." diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py index f8f17d44..75bb55b5 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -2,7 +2,7 @@ """ 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. @@ -10,26 +10,32 @@ 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 sensor_msgs.msg import Range, Imu, MagneticField +from geometry_msgs.msg import Twist, Quaternion, Vector3 +from ros_arduino_python.arduino_driver import CommandErrorCode, CommandException +from ros_arduino_python.diagnostics import DiagnosticsUpdater from ros_arduino_msgs.msg import * +from ros_arduino_msgs.srv import * +from math import pow, radians +from tf.transformations import quaternion_from_euler +import sys LOW = 0 HIGH = 1 INPUT = 0 OUTPUT = 1 - + class MessageType: ANALOG = 0 DIGITAL = 1 @@ -37,240 +43,539 @@ class MessageType: FLOAT = 3 INT = 4 BOOL = 5 - + IMU = 6 + class Sensor(object): - def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs): - self.controller = controller + def __init__(self, device, name, pin=None, rate=0, direction="input", frame_id="base_link", **kwargs): + self.device = device self.name = name self.pin = pin self.rate = rate self.direction = direction - self.frame_id = frame_id + + # Set min/max/offset/scale if specified + self.min = self.get_kwargs(kwargs, 'min', 0) + self.max = self.get_kwargs(kwargs, 'max', float('inf')) + self.offset = self.get_kwargs(kwargs, 'offset', 0) + self.scale = self.get_kwargs(kwargs, 'scale', 1) + + # Track diagnostics for this component + diagnotics_error_threshold = self.get_kwargs(kwargs, 'diagnotics_error_threshold', 10) + diagnostics_rate = float(self.get_kwargs(kwargs, 'diagnostics_rate', 1)) + + # The DiagnosticsUpdater class is defined in the diagnostics.py module + self.diagnostics = DiagnosticsUpdater(self, name + '_sensor', diagnotics_error_threshold, diagnostics_rate) + + # Initialize the component's value self.value = None - - self.t_delta = rospy.Duration(1.0 / self.rate) - self.t_next = rospy.Time.now() + self.t_delta - + + # Create the default publisher + if self.rate != 0: + self.create_publisher() + + # Create any appropriate services + self.create_services() + + # Intialize the next polling time stamp + if self.rate != 0: + self.t_delta = rospy.Duration(1.0 / self.rate) + self.t_next = rospy.Time.now() + self.t_delta + + def get_kwargs(self, kwargs, arg, default): + try: + return kwargs[arg] + except: + return default + + def create_publisher(self): + # Override per sensor type + pass + + def create_services(self): + # Override per sensor type + pass + + def read_value(self): + pass + + def write_value(self): + pass + + def publish_message(self): + # Override this method if necessary for particular sensor types + if self.direction == "input": + self.value = self.read_value() + else: + self.write_value() + + self.msg.value = self.value + self.msg.header.stamp = rospy.Time.now() + self.pub.publish(self.msg) + 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) - + # Update read counters + self.diagnostics.reads += 1 + self.diagnostics.total_reads += 1 + # Add a successful poll to the frequency status diagnostic task + self.diagnostics.freq_diag.tick() + try: + self.publish_message() + except CommandException as e: + # Update error counter + self.diagnostics.errors += 1 + rospy.logerr('Command Exception: ' + CommandErrorCode.ErrorCodeStrings[e.code]) + rospy.logerr("Invalid value read from sensor: " + str(self.name)) + except TypeError as e: + # Update error counter + self.diagnostics.errors += 1 + rospy.logerr('Type Error: ' + e.message) + + # Compute the next polling time stamp 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 - + + def create_publisher(self): self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) - + + def create_services(self): if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.analog_pin_mode(self.pin, OUTPUT) + rospy.Service('~' + self.name + '/write', AnalogSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.analog_pin_mode(self.pin, INPUT) + rospy.Service('~' + self.name + '/read', AnalogSensorRead, self.sensor_read_handler) - self.value = LOW - def read_value(self): - return self.controller.analog_read(self.pin) - + return self.scale * (self.device.analog_read(self.pin) - self.offset) + def write_value(self, value): - return self.controller.analog_write(self.pin, value) - -class AnalogFloatSensor(Sensor): + return self.device.analog_write(self.pin, value) + + def sensor_read_handler(self, req=None): + self.value = self.read_value() + return AnalogSensorReadResponse(self.value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + self.value = req.value + return AnalogSensorWriteResponse() + +class AnalogFloatSensor(AnalogSensor): 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 - + + def create_publisher(self): self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) - + + def create_services(self): if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.analog_pin_mode(self.pin, OUTPUT) + rospy.Service('~' + self.name + '/write', AnalogFloatSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.analog_pin_mode(self.pin, INPUT) + rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler) - self.value = LOW - def read_value(self): - return self.controller.analog_read(self.pin) - + return self.scale * (self.device.analog_read(self.pin) - self.offset) + def write_value(self, value): - return self.controller.analog_write(self.pin, value) - - + return self.device.analog_write(self.pin, value) + + def sensor_read_handler(self, req=None): + self.value = self.read_value() + return AnalogFloatSensorReadResponse(self.value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + self.value = req.value + return AnalogFloatSensorWriteResponse() + 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 - + + # Get the initial state + self.value = self.read_value() + + def create_publisher(self): self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) - + + def create_services(self): if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.digital_pin_mode(self.pin, OUTPUT) + rospy.Service('~' + self.name + '/write', DigitalSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.digital_pin_mode(self.pin, INPUT) + rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) - 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) - - + return self.device.digital_read(self.pin) + + def write_value(self, value=None): + # Alternate HIGH/LOW when publishing at a fixed rate + if self.rate != 0: + self.value = not self.value + else: + self.value = value + + return self.device.digital_write(self.pin, self.value) + + def sensor_read_handler(self, req=None): + self.value = self.read_value() + return DigitalSensorReadResponse(self.value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + self.value = req.value + return DigitalSensorWriteResponse() + 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 - + + def create_publisher(self): self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) - - def read_value(self): + + def create_services(self): + rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler) + + def publish_message(self): + self.value = self.read_value() + self.msg.range = self.value self.msg.header.stamp = rospy.Time.now() + self.pub.publish(self.msg) + + def sensor_read_handler(self, req=None): + self.value = self.read_value() + return AnalogFloatSensorReadResponse(self.value) - 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.field_of_view = 0.7 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) - + cm = self.device.ping(self.pin) + # Convert it to meters for ROS distance = cm / 100.0 - + return distance - - + class GP2D12(IRSensor): + # The GP2D12 has been replaced by the GP2Y0A21YK0F def __init__(self, *args, **kwargs): super(GP2D12, self).__init__(*args, **kwargs) - - self.msg.field_of_view = 0.001 + + self.msg.field_of_view = 0.09 self.msg.min_range = 0.10 self.msg.max_range = 0.80 - + def read_value(self): - value = self.controller.analog_read(self.pin) - + value = self.device.analog_read(self.pin) + + # The GP2D12 cannot provide a meaning result closer than 3 cm. if value <= 3.0: - return self.msg.max_range - + return float('NaN') + try: + #distance = pow(4187.8 / value, 1.106) distance = (6787.0 / (float(value) - 3.0)) - 4.0 except: - return self.msg.max_range - + return float('NaN') + # 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 - + if distance > self.msg.max_range: distance = float('NaN') + if distance < self.msg.min_range: distance = float('NaN') + return distance - + +class IMU(Sensor): + def __init__(self, *args, **kwargs): + super(IMU, self).__init__(*args, **kwargs) + + self.message_type = MessageType.IMU + self.direction = "input" + + self.msg = Imu() + self.mag_msg = MagneticField() + self.msg.header.frame_id = self.frame_id + self.mag_msg.header.frame_id = self.frame_id + + self.msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] + self.msg.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] + self.msg.linear_acceleration_covariance = [1e-6, 0, 0, 0, 1e6, 0, 0, 0, 1e6] + + def create_publisher(self): + self.pub = rospy.Publisher("~sensor/" + self.name, Imu, queue_size=5) + self.mag_pub = rospy.Publisher("imu/mag", MagneticField, queue_size=5) + + def read_value(self): + ''' + IMU data is assumed to be returned in the following order: + + [ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch] + + where a stands for accelerometer, g for gyroscope and m for magnetometer. + The last value ch stands for "compensated heading" that some IMU's can + compute to compensate magnetic heading for the current roll and pitch. + ''' + data = self.device.get_imu_data() + + try: + ax, ay, az, gx, gy, gz, mx, my, mz = data + except: + rospy.logerr("Invalid value read from sensor: " + str(self.name)) + return None + + ## roll = radians(roll) + ## pitch = -radians(pitch) + + self.msg.linear_acceleration.x = ay + self.msg.linear_acceleration.y = ax + self.msg.linear_acceleration.z = -az + + self.msg.angular_velocity.x = radians(gy) + self.msg.angular_velocity.y = radians(gx) + self.msg.angular_velocity.z = -radians(gz) + # magnetic data + self.mag_msg.magnetic_field.x = my + self.mag_msg.magnetic_field.y = mx + self.mag_msg.magnetic_field.z = mz + + # if ch != -999: + # yaw = -radians(ch) + # else: + # yaw = -radians(mz) + # + # (self.msg.orientation.x, self.msg.orientation.y, self.msg.orientation.z, self.msg.orientation.w) = quaternion_from_euler(roll, pitch, yaw) + + return data + + def publish_message(self): + self.read_value() + self.msg.header.stamp = rospy.Time.now() + self.mag_msg.header.stamp = rospy.Time.now() + self.pub.publish(self.msg) + self.mag_pub.publish(self.mag_msg) + +class Gyro(Sensor): + def __init__(self, *args, **kwargs): + super(Gyro, self).__init__(*args, **kwargs) + + try: + self.base_controller = kwargs['base_controller'] + except: + self.base_controller = None + + self.message_type = MessageType.IMU + self.direction = "input" + + self.sensitivity = rospy.get_param('~sensors/' + self.name + '/sensitivity', None) + self.voltage = rospy.get_param('~sensors/' + self.name + '/voltage', 5.0) + self.gyro_scale_correction = rospy.get_param('~sensors/' + self.name + '/gyro_scale_correction', 1.0) + + # Time in seconds to collect initial calibration data at startup + self.cal_start_interval = rospy.get_param('~sensors/' + self.name + '/cal_start_interval', 5.0) + + if self.sensitivity is None: + rospy.logerr("Missing sensitivity parameter for gyro.") + rospy.signal_shutdown("Missing sensitivity parameter for gyro.") + + self.rad_per_sec_per_adc_unit = radians(self.voltage / 1023.0 / self.sensitivity) + + self.orientation = 0.0 + self.last_time = None + + self.cal_offset = None + self.cal_drift_threshold = rospy.get_param('~sensors/' + self.name + '/cal_drift_threshold', 0.1) + self.cal_buffer = [] + self.cal_drift_buffer = [] + self.cal_buffer_length = 1000 + self.cal_drift_buffer_length = 1000 + + self.msg = Imu() + + self.msg.header.frame_id = self.frame_id + + self.msg.orientation_covariance = [sys.float_info.max, 0, 0, 0, sys.float_info.max, 0, 0, 0, 0.05] + self.msg.angular_velocity_covariance = [sys.float_info.max, 0, 0, 0, sys.float_info.max, 0, 0, 0, 0.05] + self.msg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] + + print "\n*** DO NOT MOVE GYRO FOR", self.cal_start_interval, "SECONDS TO ALLOW OFFSET CALLIBRATION ***\n" + update_interval = 1.0 / self.rate + cal_time = 0.0 + while cal_time < self.cal_start_interval: + gyro_data = self.device.analog_read(self.pin) + self.update_calibration(gyro_data) + rospy.sleep(update_interval) + cal_time += update_interval + + def create_publisher(self): + self.pub = rospy.Publisher("~sensor/" + self.name, Imu, queue_size=5) + + def read_value(self): + gyro_data = self.device.analog_read(self.pin) + + # If the robot is not moving, update the gyro calibration + if self.base_controller is not None and self.base_controller.current_speed == Twist(): + self.update_calibration(gyro_data) + + # If this is the first measurement, just record the current time + if self.last_time is None: + self.last_time = rospy.Time.now() + return + + # Store the current time + current_time = rospy.Time.now() + + # Compute the time since the last measurement + dt = (current_time - self.last_time).to_sec() + + self.last_time = current_time + + # Compute angular velocity from the raw gyro data + angular_velocity = self.gyro_scale_correction * self.rad_per_sec_per_adc_unit * (gyro_data - self.cal_offset) + + # Right-hand coordinate system + angular_velocity = -1.0 * angular_velocity + + # Ignore small values that are likely due to drift + if abs(angular_velocity) < self.cal_drift_threshold: + angular_velocity = 0 + + # Update the orientation by integrating angular velocity over time + self.orientation += angular_velocity * dt + + # Fill in the Imu message + self.msg.header.stamp = current_time + self.msg.angular_velocity.z = angular_velocity + (self.msg.orientation.x, self.msg.orientation.y, self.msg.orientation.z, self.msg.orientation.w) = quaternion_from_euler(0, 0, self.orientation) + + return self.msg + + def publish_message(self): + self.read_value() + self.msg.header.stamp = rospy.Time.now() + self.pub.publish(self.msg) + + def update_calibration(self, gyro_data): + # Collect raw analog values when stoped so we can compute the ADC offset + self.cal_buffer.append(gyro_data) + + if len(self.cal_buffer) > self.cal_buffer_length: + del self.cal_buffer[:-self.cal_buffer_length] + + if len(self.cal_drift_buffer) > self.cal_drift_buffer_length: + del self.cal_drift_buffer[:-self.cal_drift_buffer_length] + + try: + # Collect angular velocity values when stopped to compute the drift estimate + angular_velocity = self.gyro_scale_correction * self.rad_per_sec_per_adc_unit * (gyro_data - self.cal_offset) + self.cal_drift_buffer.append(abs(angular_velocity)) + except: + pass + + try: + # Use the max absolute angular velocity when stopped as the drift estimated + self.cal_drift_offset = max(self.cal_drift_buffer, key=lambda x: abs(x)) + except: + pass + + try: + self.cal_offset = sum(self.cal_buffer) / len(self.cal_buffer) + except: + pass + + def reset(self): + self.orientation = 0 + self.msg.orientation = Quaternion() + self.msg.orientation.w = 1.0 + self.msg.angular_velocity = Vector3() + self.msg.linear_acceleration = Vector3() + 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 + milliamps = self.device.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.) + voltage = 0.06 * (self.device.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.) + current = 0.05 * (self.device.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) - \ No newline at end of file + def read_value(self): + return self.device.get_MaxEZ1(self.trigger_pin, self.output_pin) diff --git a/ros_arduino_python/src/ros_arduino_python/base_controller.py b/ros_arduino_python/src/ros_arduino_python/base_controller.py index 46d19ede..62f74654 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -2,9 +2,9 @@ """ 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. @@ -12,63 +12,83 @@ 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 +import sys, 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 - +from ros_arduino_python.diagnostics import DiagnosticsUpdater + + """ Class to receive Twist commands and publish Odometry data """ class BaseController: - def __init__(self, arduino, base_frame): + def __init__(self, arduino, base_frame, name='base_controller'): self.arduino = arduino + self.name = name 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.odom_linear_scale_correction = rospy.get_param("~odom_linear_scale_correction", 1.0) + self.odom_angular_scale_correction = rospy.get_param("~odom_angular_scale_correction", 1.0) + self.use_imu_heading = rospy.get_param("~use_imu_heading", False) + self.publish_odom_base_transform = rospy.get_param("~publish_odom_base_transform", True) + self.stopped = False - + self.current_speed = Twist() + pid_params = dict() - pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") + 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['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) - + + self.accel_limit = rospy.get_param('~accel_limit', 1.0) + # self.motors_reversed = rospy.get_param("~motors_reversed", False) + self.detect_enc_jump_error = rospy.get_param("~detect_enc_jump_error", False) + self.enc_jump_error_threshold = rospy.get_param("~enc_jump_error_threshold", 1000) + + # Default error threshold (percent) before getting a diagnostics warning + self.base_diagnotics_error_threshold = rospy.get_param("~base_diagnotics_error_threshold", 10) + + # Diagnostics update rate + self.base_diagnotics_rate = rospy.get_param("~base_diagnotics_rate", 1.0) + + # Create the diagnostics updater for the Arduino device + self.diagnostics = DiagnosticsUpdater(self, self.name, self.base_diagnotics_error_threshold, self.base_diagnotics_rate) + # 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() + + 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 + # Internal data self.enc_left = None # encoder readings self.enc_right = None self.x = 0 # position in xy plane @@ -82,55 +102,86 @@ def __init__(self, arduino, base_frame): # 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] == "": + if pid_params[param] is None or 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) + + if self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko): + rospy.loginfo("PID parameters update to: Kp=%d, Kd=%d, Ki=%d, Ko=%d" %(self.Kp, self.Kd, self.Ki, self.Ko)) + else: + rospy.logerr("Updating PID parameters failed!") def poll(self): now = rospy.Time.now() if now > self.t_next: # Read the encoders try: + self.diagnostics.reads += 1 + self.diagnostics.total_reads += 1 left_enc, right_enc = self.arduino.get_encoder_counts() + self.diagnostics.freq_diag.tick() except: + self.diagnostics.errors += 1 self.bad_encoder_count += 1 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) return - + + # Check for jumps in encoder readings + if self.detect_enc_jump_error: + try: + #rospy.loginfo("Left: %d LEFT: %d Right: %d RIGHT: %d", left_enc, self.enc_left, right_enc, self.enc_right) + enc_jump_error = False + if abs(right_enc - self.enc_right) > self.enc_jump_error_threshold: + self.diagnostics.errors += 1 + self.bad_encoder_count += 1 + rospy.logerr("RIGHT encoder jump error from %d to %d", self.enc_right, right_enc) + self.enc_right = right_enc + enc_jump_error = True + + if abs(left_enc - self.enc_left) > self.enc_jump_error_threshold: + self.diagnostics.errors += 1 + self.bad_encoder_count += 1 + rospy.logerr("LEFT encoder jump error from %d to %d", self.enc_left, left_enc) + self.enc_left = left_enc + enc_jump_error = True + + if enc_jump_error: + return + except: + pass + dt = now - self.then self.then = now dt = dt.to_sec() - + # Calculate odometry if self.enc_left == None: dright = 0 @@ -141,36 +192,37 @@ def poll(self): self.enc_right = right_enc self.enc_left = left_enc - - dxy_ave = (dright + dleft) / 2.0 - dth = (dright - dleft) / self.wheel_track + + dxy_ave = self.odom_linear_scale_correction * (dright + dleft) / 2.0 + dth = self.odom_angular_scale_correction * (dright - dleft) / float(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 - + self.th += dth + quaternion = Quaternion() - quaternion.x = 0.0 + 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" - ) - + if self.publish_odom_base_transform: + 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 @@ -183,12 +235,36 @@ def poll(self): odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth + self.current_speed = Twist() + self.current_speed.linear.x = vxy + self.current_speed.angular.z = vth + + """ + Covariance values taken from Kobuki node odometry.cpp at: + https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp + + Pose covariance (required by robot_pose_ekf) TODO: publish realistic values + Odometry yaw covariance must be much bigger than the covariance provided + by the imu, as the later takes much better measures + """ + odom.pose.covariance[0] = 0.1 + odom.pose.covariance[7] = 0.1 + if self.use_imu_heading: + #odom.pose.covariance[35] = 0.2 + odom.pose.covariance[35] = 0.05 + else: + odom.pose.covariance[35] = 0.05 + + odom.pose.covariance[14] = sys.float_info.max # set a non-zero covariance on unused + odom.pose.covariance[21] = sys.float_info.max # dimensions (z, pitch and roll); this + odom.pose.covariance[28] = sys.float_info.max # is a requirement of robot_pose_ekf + 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: @@ -197,7 +273,7 @@ def poll(self): 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: @@ -206,21 +282,21 @@ def poll(self): 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 @@ -235,13 +311,11 @@ def cmdVelCallback(self, req): # 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) - - - - - - + def reset_odometry(self): + self.x = 0.0 + self.y = 0.0 + self.th = 0.0 diff --git a/ros_arduino_python/src/ros_arduino_python/controllers.py b/ros_arduino_python/src/ros_arduino_python/controllers.py new file mode 100644 index 00000000..61302505 --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/controllers.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python + +# Copyright (c) 2010-2011 Vanadium Labs LLC. +# All right 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. +# * Neither the name of Vanadium Labs LLC nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS 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. + +## @file controllers.py Base class and support functions for a controllers. + +## @brief Controllers interact with ArbotiX hardware. +class Controller: + + ## @brief Constructs a Controller instance. + ## + ## @param device The arbotix instance. + ## + ## @param name The controller name. + def __init__(self, device, name): + self.name = name + self.device = device + self.pause = False + + # output for joint states publisher + self.joint_names = list() + self.joint_positions = list() + self.joint_velocities = list() + + ## @brief Start the controller, do any hardware setup needed. + def startup(self): + pass + + ## @brief Do any read/writes to device. + def update(self): + pass + + ## @brief Stop the controller, do any hardware shutdown needed. + def shutdown(self): + pass + + ## @brief Is the controller actively sending commands to joints? + def active(self): + return False + diff --git a/ros_arduino_python/src/ros_arduino_python/diagnostics.py b/ros_arduino_python/src/ros_arduino_python/diagnostics.py new file mode 100644 index 00000000..182bff20 --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/diagnostics.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python + +""" + Diagnostics classes for the arudino_python package + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2016 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 +import diagnostic_updater +from diagnostic_msgs.msg import DiagnosticStatus, KeyValue + +class DiagnosticsPublisher(): + """ Class to handle publications of diagnostics messages. """ + + def __init__(self, arduino): + self.arduino = arduino + self.device = self.arduino.device + + def update(self): + # First the arduino itself + self.device.diagnostics.diag_updater.update() + + # Then the sensors + for sensor in self.device.sensors: + sensor.diagnostics.diag_updater.update() + + # Next the servos + if self.arduino.have_joints: + for servo in self.arduino.servo_controller.servos: + servo.diagnostics.diag_updater.update() + + # Finally, the base controller + if self.arduino.use_base_controller: + self.arduino.base_controller.diagnostics.diag_updater.update() + +class DiagnosticsUpdater(): + """ Class to return diagnostic status from a component. """ + + def __init__(self, component, name, error_threshold=10, rate=1.0, create_watchdog=False): + self.component = component + + self.name = name + + # Determines the OK, WARN and ERROR status flags + self.error_threshold = error_threshold + + # Create a diagnostics updater + self.diag_updater = diagnostic_updater.Updater() + + # Set the period from the rate + self.diag_updater.period = 1.0 / rate + + # Set the hardware ID to name + pin + try: + self.diag_updater.setHardwareID(self.name + '_pin_' + str(self.component.pin)) + except: + self.diag_updater.setHardwareID(self.name) + + # Create a frequency monitor that tracks the publishing frequency for this component + if self.component.rate > 0: + if "Servo" in str(self.component): + freq_bounds = diagnostic_updater.FrequencyStatusParam({'min': self.component.device.joint_update_rate, 'max': self.component.device.joint_update_rate}, 0.3, 5) + self.freq_diag = diagnostic_updater.HeaderlessTopicDiagnostic(self.component.name + '_freq', self.diag_updater, freq_bounds) + else: + freq_bounds = diagnostic_updater.FrequencyStatusParam({'min': self.component.rate, 'max': self.component.rate}, 0.3, 5) + self.freq_diag = diagnostic_updater.HeaderlessTopicDiagnostic(self.component.name + '_freq', self.diag_updater, freq_bounds) + + # Create an error monitor that tracks timeouts, serial exceptions etc + self.error_diag = diagnostic_updater.FunctionDiagnosticTask(self.name, self.get_error_rate) + self.diag_updater.add(self.error_diag) + + # Create a watchdog diagnostic to monitor the connection status + if create_watchdog: + self.watchdog_diag = diagnostic_updater.FunctionDiagnosticTask(self.name, self.get_connection_status) + self.diag_updater.add(self.watchdog_diag) + + # Counters used for diagnostics + self.reads = 0 + self.total_reads = 0 + self.errors = 0 + self.error_rates = [0.0] + + # Connection status for device + self.watchdog = False + + def get_error_rate(self, stat): + error_rate = 0.0 + + # Wait until we have some data before computing error rates + if self.reads + self.errors > 100: + error_rate = (self.errors * 100.0) / (self.reads + self.errors) + self.error_rates.append(error_rate) + # Keep only the past 1000 measurements (100 x 10) so that error rates are current + if len(self.error_rates) > 10: + self.error_rates = self.error_rates[-10:] + self.reads = 0 + self.errors = 0 + + error_rate = sum(self.error_rates) / len(self.error_rates) + + if error_rate > 3 * self.error_threshold: + stat.summary(DiagnosticStatus.ERROR, "Excessive Error Rate") + + elif error_rate > 2 * self.error_threshold: + stat.summary(DiagnosticStatus.WARN, "Moderate Error Rate") + + else: + stat.summary(DiagnosticStatus.OK, "Error Rate OK") + + stat.add("Reads", self.total_reads) + stat.add("Error Rate", error_rate) + + return stat + + def get_connection_status(self, stat): + stat.path = self.name + "/Watchdog" + if not self.watchdog: + stat.summary(DiagnosticStatus.ERROR, "Connnection lost!") + else: + stat.summary(DiagnosticStatus.OK, "Alive") + + stat.add("Watchdog", self.watchdog) + return stat + + \ No newline at end of file diff --git a/ros_arduino_python/src/ros_arduino_python/follow_controller.py b/ros_arduino_python/src/ros_arduino_python/follow_controller.py new file mode 100644 index 00000000..39968c9c --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/follow_controller.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python + +""" + follow_controller.py - controller for a kinematic chain + Copyright (c) 2011 Vanadium Labs LLC. All right 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. + * Neither the name of Vanadium Labs LLC nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS 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. +""" + +import rospy, actionlib + +from control_msgs.msg import FollowJointTrajectoryAction +from trajectory_msgs.msg import JointTrajectory +from controllers import * + +class FollowController(Controller): + """ A controller for joint chains, exposing a FollowJointTrajectory action. """ + + def __init__(self, device, name): + Controller.__init__(self, device, name) + # Parameters: rates and joints + self.rate = rospy.get_param('~controllers/' + name + '/rate', 50.0) + self.joints = rospy.get_param('~controllers/' + name + '/joints') + self.index = rospy.get_param('~controllers/'+ name + '/index', len(device.controllers)) + for joint in self.joints: + self.device.joints[joint].controller = self + + # Action server + name = rospy.get_param('~controllers/' + name + '/action_name', 'follow_joint_trajectory') + self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False) + + # Good old trajectory + rospy.Subscriber(self.name + '/command', JointTrajectory, self.command_cb) + self.executing = False + + rospy.loginfo("Started FollowController ("+self.name+"). Joints: " + str(self.joints) + " on C" + str(self.index)) + + def startup(self): + self.server.start() + + def execute_cb(self, goal): + rospy.loginfo(self.name + ": Action goal recieved.") + traj = goal.trajectory + + if set(self.joints) != set(traj.joint_names): + for j in self.joints: + if j not in traj.joint_names: + msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names) + rospy.logerr(msg) + self.server.set_aborted(text=msg) + return + rospy.logwarn("Extra joints in trajectory") + + if not traj.points: + msg = "Trajectory empy." + rospy.logerr(msg) + self.server.set_aborted(text=msg) + return + + try: + indexes = [traj.joint_names.index(joint) for joint in self.joints] + except ValueError as val: + msg = "Trajectory invalid." + rospy.logerr(msg) + self.server.set_aborted(text=msg) + return + + if self.execute_trajectory(traj): + self.server.set_succeeded() + else: + self.server.set_aborted(text="Execution failed.") + + rospy.loginfo(self.name + ": Done.") + + def command_cb(self, msg): + # Don't execute if executing an action + if self.server.is_active(): + rospy.loginfo(self.name + ": Received trajectory, but action is active") + return + self.executing = True + self.execute_trajectory(msg) + self.executing = False + + def execute_trajectory(self, traj): + rospy.loginfo("Executing trajectory") + rospy.logdebug(traj) + # carry out trajectory + try: + indexes = [traj.joint_names.index(joint) for joint in self.joints] + except ValueError as val: + rospy.logerr("Invalid joint in trajectory.") + return False + + # Get starting timestamp, MoveIt uses 0, need to fill in + start = traj.header.stamp + if start.secs == 0 and start.nsecs == 0: + start = rospy.Time.now() + + # Set the start time for each joint + for i in range(len(self.joints)): + self.device.joints[self.joints[i]].time_move_started = start + + r = rospy.Rate(self.rate) + last = [ self.device.joints[joint].position for joint in self.joints ] + for point in traj.points: + while rospy.Time.now() + rospy.Duration(0.01) < start: + rospy.sleep(0.01) + desired = [ point.positions[k] for k in indexes ] + endtime = start + point.time_from_start + while rospy.Time.now() + rospy.Duration(0.01) < endtime: + err = [ (d-c) for d,c in zip(desired,last) ] + velocity = [ abs(x / (self.rate * (endtime - rospy.Time.now()).to_sec())) for x in err ] + rospy.logdebug(err) + for i in range(len(self.joints)): + if err[i] > 0.001 or err[i] < -0.001: + cmd = err[i] + top = velocity[i] + if cmd > top: + cmd = top + elif cmd < -top: + cmd = -top + last[i] += cmd + self.device.joints[self.joints[i]].time_move_started = rospy.Time.now() + self.device.joints[self.joints[i]].in_trajectory = True + self.device.joints[self.joints[i]].is_moving = True + self.device.joints[self.joints[i]].desired = last[i] + else: + self.device.joints[self.joints[i]].speed = 0.0 + self.device.joints[self.joints[i]].in_trajectory = True + self.device.joints[self.joints[i]].is_moving = False + velocity[i] = 0 + r.sleep() + return True + + def active(self): + """ Is controller overriding servo internal control? """ + return self.server.is_active() or self.executing + + def getDiagnostics(self): + """ Get a diagnostics status. """ + msg = DiagnosticStatus() + msg.name = self.name + msg.level = DiagnosticStatus.OK + msg.message = "OK" + if self.active(): + msg.values.append(KeyValue("State", "Active")) + else: + msg.values.append(KeyValue("State", "Not Active")) + return msg diff --git a/ros_arduino_python/src/ros_arduino_python/joint_state_publisher.py b/ros_arduino_python/src/ros_arduino_python/joint_state_publisher.py new file mode 100644 index 00000000..9f976dda --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/joint_state_publisher.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python + +""" + Class to publish joint states used with the ROS Arduino Bridge 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 + + Borrowed heavily from Mike Ferguson's ArbotiX package. + + Original copyright: + + Copyright (c) 2011 Vanadium Labs LLC. All right 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. + * Neither the name of Vanadium Labs LLC nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS 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. +""" + +import rospy +from sensor_msgs.msg import JointState + +class JointStatePublisher: + """ Class to publish joint_states message. """ + + def __init__(self): + self.rate = rospy.get_param("~joint_update_rate", 10.0) + self.t_delta = rospy.Duration(1.0 / self.rate) + self.t_next = rospy.Time.now() + self.t_delta + + # Publisher + self.pub = rospy.Publisher('joint_states', JointState, queue_size=5) + + def poll(self, joints): + """ Publish joint states. """ + if rospy.Time.now() > self.t_next: + msg = JointState() + msg.header.stamp = rospy.Time.now() + msg.name = list() + msg.position = list() + msg.velocity = list() + + for joint in joints: + msg.name.append(joint.name) + msg.position.append(joint.position) + msg.velocity.append(joint.velocity) + + self.pub.publish(msg) + + self.t_next = rospy.Time.now() + self.t_delta diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py new file mode 100755 index 00000000..6a0c638e --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -0,0 +1,344 @@ +#!/usr/bin/env python + +""" + Servo class for the arudino_python package + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2015 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 + + Borrowed heavily from Mike Feguson's ArbotiX servos_controller.py code. +""" +import rospy +from std_msgs.msg import Float64 +from ros_arduino_msgs.srv import Relax, Enable, SetSpeed, SetSpeedResponse, RelaxResponse, EnableResponse +from ros_arduino_python.diagnostics import DiagnosticsUpdater +from ros_arduino_python.arduino_driver import CommandErrorCode, CommandException +from controllers import * + +from math import radians, degrees, copysign + +class Joint: + def __init__(self, device, name): + self.device = device + self.name = name + + self.position = 0.0 # radians + self.start_position = 0.0 + self.position_last = 0.0 # radians + self.velocity = 0.0 # rad/s + self.servo_speed = 0.0 # rad/s + self.direction = 1 + self.is_moving = False + self.in_trajectory = False + + # Stamp the start of the movement + self.time_move_started = rospy.Time.now() + + # Joint update rates are all the same joint_update_rate parameter + self.rate = self.device.joint_update_rate + + # Track diagnostics for this component + diagnotics_error_threshold = self.get_kwargs('diagnotics_error_threshold', 10) + diagnostics_rate = float(self.get_kwargs('diagnostics_rate', 1)) + + # The DiagnosticsUpdater class is defined in the diagnostics.py module + self.diagnostics = DiagnosticsUpdater(self, name + '_joint', diagnotics_error_threshold, diagnostics_rate) + + def get_kwargs(self, arg, default): + try: + return kwargs['arg'] + except: + return default + +class Servo(Joint): + def __init__(self, device, name, ns="~joints"): + Joint.__init__(self, device, name) + + # Construct the namespace for the joint + namespace = ns + "/" + name + "/" + + # The Arduino pin used by this servo + self.pin = int(rospy.get_param(namespace + "pin")) + + # Hobby servos have a rated speed giving in seconds per 60 degrees + # A value of 0.24 seconds per 60 degrees is typical. + self.rated_speed = rospy.get_param(namespace + 'rated_speed', 0.24) # seconds per 60 degrees + + # Convert rated speed to degrees per second + self.rated_speed_deg_per_sec = 60.0 / self.rated_speed + + # Convert rated speed to radians per second + self.rated_speed_rad_per_sec = radians(self.rated_speed_deg_per_sec) + + # The rated speed might require an emperical correction factor + self.joint_speed_scale_correction = rospy.get_param(namespace + 'joint_speed_scale_correction', 1.0) + + # Get the initial servo speed in degrees per second + self.servo_speed = radians(rospy.get_param(namespace + 'init_speed', 60.0)) + + self.direction = copysign(1, -self.servo_speed) + + # Convert initial servo speed in rad/s to a step delay in milliseconds + step_delay = self.get_step_delay(self.servo_speed) + + # Enable the servo + self.device.attach_servo(self.pin) + + # Set the servo speed + self.device.set_servo_delay(self.pin, step_delay) + + # Min/max/neutral values + self.neutral = rospy.get_param(namespace + 'neutral', 90.0) # degrees + self.max_position = radians(rospy.get_param(namespace + 'max_position', 90.0)) # degrees to radians + self.min_position = radians(rospy.get_param(namespace + 'min_position', -90.0)) # degrees to radians + self.range = radians(rospy.get_param(namespace + 'range', 180)) # degrees to radians + self.max_speed = radians(rospy.get_param(namespace + 'max_speed', 250.0)) # deg/s to rad/s + + # Do we want to reverse positive motion + self.invert = rospy.get_param(namespace + 'invert', False) + + # Intialize the desired position of the servo from the init_position parameter + self.desired = radians(rospy.get_param(namespace + 'init_position', 0)) + + # Where is the servo positioned now + #self.position = self.desired + + # Subscribe to the servo's command topic for setting its position + rospy.Subscriber('/' + name + '/command', Float64, self.command_cb) + + # Provide a number of services for controlling the servos + rospy.Service(name + '/relax', Relax, self.relax_cb) + rospy.Service(name + '/enable', Enable, self.enable_cb) + rospy.Service(name + '/set_speed', SetSpeed, self.set_speed_cb) + + def command_cb(self, msg): + # Check limits + if msg.data > self.max_position: + msg.data = self.max_position + + if msg.data < self.min_position: + msg.data = self.min_position + + # Adjust for the neutral offset + if self.invert: + target_adjusted = self.neutral - msg.data + else: + target_adjusted = self.neutral + msg.data + + # Stamp the start of the movement + self.time_move_started = rospy.Time.now() + + # Record the starting position + self.start_position = self.get_current_position() + + # Set the target position for the next servo controller update + self.desired = msg.data + + # Flag to indicate the servo is moving + self.is_moving = True + + # The direction of movement + self.direction = copysign(1, self.desired - self.position) + + def get_step_delay(self, target_speed=1.0): + # Don't allow negative speeds + target_speed = abs(target_speed) + + if target_speed > self.rated_speed_rad_per_sec: + rospy.logdebug("Target speed exceeds max servo speed. Using max.") + step_delay = 0 + else: + # Catch division by zero and set to slowest speed possible + try: + step_delay = 1000.0 * radians(1.0) * ((1.0 / (self.joint_speed_scale_correction * target_speed)) - (1.0 / self.rated_speed_rad_per_sec)) + except: + step_delay = 4294967295 # 2^32 - 1 + + # Minimum step delay is 0 millisecond + step_delay = max(0, step_delay) + + return int(step_delay) + + def radians_to_servo_degrees(self, rad): + if self.invert: + servo_degrees = self.neutral - degrees(rad) + else: + servo_degrees = degrees(rad) + self.neutral + + return servo_degrees + + def get_current_position(self): + #return radians(self.device.servo_read(self.pin)) + self.neutral + return radians(self.device.servo_read(self.pin) - self.neutral) + + def get_interpolated_position(self): + time_since_start = rospy.Time.now() - self.time_move_started + + return self.start_position + self.servo_speed * self.direction * time_since_start.to_sec() + + def relax_cb(self, req): + self.device.detach_servo(self.pin) + + return RelaxResponse() + + def enable_cb(self, req): + if req.enable: + self.device.attach_servo(self.pin) + state = True + else: + self.device.detach_servo(self.pin) + state = False + + return EnableResponse(state) + + def set_speed_cb(self, req): + # Convert servo speed in rad/s to a step delay in milliseconds + step_delay = self.get_step_delay(req.speed) + + # Update the servo speed + self.device.set_servo_delay(self.pin, step_delay) + + self.servo_speed = req.speed + + return SetSpeedResponse() + +class ContinousServo(Joint): + def __init__(self, device, name, ns="~joints"): + Joint.__init__(self, device, name) + + # Construct the namespace for the joint + namespace = ns + "/" + name + "/" + + # The Arduino pin used by this servo + self.pin = int(rospy.get_param(namespace + "pin")) + + # Min/max/neutral values + self.neutral = rospy.get_param(namespace + "neutral", 90.0) # degrees + self.max_speed = radians(rospy.get_param(namespace + "max_speed", 250.0)) # deg/s + self.min_speed = radians(rospy.get_param(namespace + "min_speed", 0)) # deg/s + self.range = radians(rospy.get_param(namespace + "range", 180)) # degrees + + # Do we want to reverse positive motion + self.invert = rospy.get_param(namespace + "invert", False) + + # The initial speed of the servo + self.init_speed = self.neutral + radians(rospy.get_param(namespace + "init_speed", 0)) + + # Conversion factors to compute servo speed in rad/s from control input + self.max_rad_per_sec = radians(60.0) / self.rated_speed + + #self.ticks_per_rad_per_sec = (self.range / 2.0) / self.max_rad_per_sec + + # What is the current speed + self.velocity = 0.0 + + # Enable the servo + self.device.attach_servo(self.pin) + + # Subscribe to the servo's command topic for setting its speed + rospy.Subscriber("/" + name + '/command', Float64, self.command_cb) + + def command_cb(self, req): + # Check limits + if msg.data > self.max_position: + msg.data = self.max_position + + if msg.data < self.min_position: + msg.data = self.min_position + + # Adjust for the neutral offset + if self.invert: + target_adjusted = self.neutral - msg.data + else: + target_adjusted = self.neutral + msg.data + + # Stamp the start of the movement + self.time_move_started = rospy.Time.now() + + # Record the starting position + self.start_position = self.get_current_position() + + # Set the target position for the next servo controller update + self.desired = msg.data + + self.is_moving = True + self.direction = copysign(1, self.desired - self.position) + +class ServoController(): + def __init__(self, device, name): + self.name = name + self.device = device + self.servos = list() + + # Get the servo objects from the joint list + for servo in self.device.joints.values(): + self.servos.append(servo) + servo.position_last = servo.get_current_position() + + self.delta_t = rospy.Duration(1.0 / self.device.joint_update_rate) + self.next_update = rospy.Time.now() + self.delta_t + + def poll(self): + """ Read and write servo positions and velocities. """ + if rospy.Time.now() > self.next_update: + for servo in self.servos: + + + # Check to see if we are within 2 degrees of the desired position or gone past it + if not servo.in_trajectory and servo.is_moving: + if abs(servo.position - servo.desired) < radians(2.0) or copysign(1, servo.desired - servo.position) != servo.direction: + servo.position = servo.desired + servo.is_moving = False + servo.velocity = 0.0 + duration = rospy.Time.now() - servo.time_move_started + + # If the servo is still moving, update its interpolated position and velocity + if servo.is_moving: + try: + # Get the interpolated current position of the servo + servo.position = servo.get_interpolated_position() + if not servo.in_trajectory: + servo.position = servo.get_interpolated_position() + else: + servo.position = servo.get_current_position() + + # We cannot interpolate both position and velocity so just set velocity to the current speed + servo.velocity = servo.servo_speed + servo.position_last = servo.position + + # Update diagnostics counters + servo.diagnostics.reads += 1 + servo.diagnostics.total_reads += 1 + + # Add a successful poll to the frequency status diagnostic task + servo.diagnostics.freq_diag.tick() + except (CommandException, TypeError) as e: + # Update error counter + servo.diagnostics.errors += 1 + rospy.logerr('Command Exception: ' + CommandErrorCode.ErrorCodeStrings[e.code]) + rospy.logerr("Invalid value read from joint: " + str(self.name)) + + try: + # Send desired position to the servo and update diagnostics + self.device.servo_write(servo.pin, servo.radians_to_servo_degrees(servo.desired)) + servo.diagnostics.reads += 1 + servo.diagnostics.total_reads += 1 + except (CommandException, TypeError) as e: + servo.diagnostics.errors += 1 + rospy.logerr('Command Exception: ' + CommandErrorCode.ErrorCodeStrings[e.code]) + rospy.logerr("Invalid value read from joint: " + str(self.name)) + + self.next_update = rospy.Time.now() + self.delta_t +