From 70a0d712c5ceeea8f373f9ef4c98b3ff5aeb0bf1 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 15 Dec 2012 07:36:59 -0800 Subject: [PATCH] First commit --- CMakeLists.txt | 17 + Makefile | 1 + README.md | 353 +++++++++++++++++ ros_arduino_firmware/CMakeLists.txt | 30 ++ ros_arduino_firmware/Makefile | 1 + ros_arduino_firmware/mainpage.dox | 26 ++ ros_arduino_firmware/manifest.xml | 14 + .../MegaRobogaiaPololu/MegaRobogaiaPololu.ino | 307 +++++++++++++++ .../libraries/MegaRobogaiaPololu/commands.h | 22 ++ .../MegaRobogaiaPololu/diff_controller.h | 68 ++++ .../libraries/MegaRobogaiaPololu/sensors.h | 34 ++ .../src/libraries/MegaRobogaiaPololu/servos.h | 16 + ros_arduino_msgs/CMakeLists.txt | 30 ++ ros_arduino_msgs/Makefile | 1 + ros_arduino_msgs/mainpage.dox | 26 ++ ros_arduino_msgs/manifest.xml | 13 + ros_arduino_msgs/msg/Analog.msg | 3 + ros_arduino_msgs/msg/AnalogFloat.msg | 3 + ros_arduino_msgs/msg/Digital.msg | 4 + ros_arduino_msgs/msg/SensorState.msg | 4 + .../src/ros_arduino_msgs/__init__.py | 0 .../src/ros_arduino_msgs/__init__.pyc | Bin 0 -> 209 bytes ros_arduino_msgs/srv/DigitalSetDirection.srv | 3 + ros_arduino_msgs/srv/DigitalWrite.srv | 3 + ros_arduino_msgs/srv/ServoRead.srv | 3 + ros_arduino_msgs/srv/ServoWrite.srv | 3 + ros_arduino_python/CMakeLists.txt | 30 ++ ros_arduino_python/Makefile | 1 + ros_arduino_python/config/arduino_params.yaml | 38 ++ ros_arduino_python/launch/arduino.launch | 5 + ros_arduino_python/mainpage.dox | 26 ++ ros_arduino_python/manifest.xml | 22 ++ ros_arduino_python/nodes/arduino_node.py | 178 +++++++++ ros_arduino_python/src/__init__.py | 0 ros_arduino_python/src/arduino_driver.py | 369 ++++++++++++++++++ ros_arduino_python/src/arduino_driver.pyc | Bin 0 -> 13942 bytes ros_arduino_python/src/arduino_sensors.py | 230 +++++++++++ ros_arduino_python/src/arduino_sensors.pyc | Bin 0 -> 12063 bytes ros_arduino_python/src/base_controller.py | 227 +++++++++++ ros_arduino_python/src/base_controller.pyc | Bin 0 -> 7155 bytes .../src/ros_arduino_python/__init__.py | 0 .../src/ros_arduino_python/__init__.pyc | Bin 0 -> 213 bytes ros_arduino_python/src/sensor.pyc | Bin 0 -> 8180 bytes ros_arduino_python/src/sensors.pyc | Bin 0 -> 12149 bytes stack.xml | 14 + 45 files changed, 2125 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 Makefile create mode 100644 README.md create mode 100644 ros_arduino_firmware/CMakeLists.txt create mode 100644 ros_arduino_firmware/Makefile create mode 100644 ros_arduino_firmware/mainpage.dox create mode 100644 ros_arduino_firmware/manifest.xml create mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino create mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h create mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h create mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h create mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h create mode 100644 ros_arduino_msgs/CMakeLists.txt create mode 100644 ros_arduino_msgs/Makefile create mode 100644 ros_arduino_msgs/mainpage.dox create mode 100644 ros_arduino_msgs/manifest.xml create mode 100644 ros_arduino_msgs/msg/Analog.msg create mode 100644 ros_arduino_msgs/msg/AnalogFloat.msg create mode 100644 ros_arduino_msgs/msg/Digital.msg create mode 100755 ros_arduino_msgs/msg/SensorState.msg create mode 100644 ros_arduino_msgs/src/ros_arduino_msgs/__init__.py create mode 100644 ros_arduino_msgs/src/ros_arduino_msgs/__init__.pyc create mode 100755 ros_arduino_msgs/srv/DigitalSetDirection.srv create mode 100755 ros_arduino_msgs/srv/DigitalWrite.srv create mode 100755 ros_arduino_msgs/srv/ServoRead.srv create mode 100755 ros_arduino_msgs/srv/ServoWrite.srv create mode 100644 ros_arduino_python/CMakeLists.txt create mode 100644 ros_arduino_python/Makefile create mode 100644 ros_arduino_python/config/arduino_params.yaml create mode 100755 ros_arduino_python/launch/arduino.launch create mode 100644 ros_arduino_python/mainpage.dox create mode 100644 ros_arduino_python/manifest.xml create mode 100755 ros_arduino_python/nodes/arduino_node.py create mode 100644 ros_arduino_python/src/__init__.py create mode 100755 ros_arduino_python/src/arduino_driver.py create mode 100644 ros_arduino_python/src/arduino_driver.pyc create mode 100755 ros_arduino_python/src/arduino_sensors.py create mode 100644 ros_arduino_python/src/arduino_sensors.pyc create mode 100755 ros_arduino_python/src/base_controller.py create mode 100644 ros_arduino_python/src/base_controller.pyc create mode 100644 ros_arduino_python/src/ros_arduino_python/__init__.py create mode 100644 ros_arduino_python/src/ros_arduino_python/__init__.pyc create mode 100644 ros_arduino_python/src/sensor.pyc create mode 100644 ros_arduino_python/src/sensors.pyc create mode 100644 stack.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..28105dd --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of +# directories (or patterns, but directories should suffice) that should +# be excluded from the distro. This is not the place to put things that +# should be ignored everywhere, like "build" directories; that happens in +# rosbuild/rosbuild.cmake. Here should be listed packages that aren't +# ready for inclusion in a distro. +# +# This list is combined with the list in rosbuild/rosbuild.cmake. Note +# that CMake 2.6 may be required to ensure that the two lists are combined +# properly. CMake 2.4 seems to have unpredictable scoping rules for such +# variables. +#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) + +rosbuild_make_distribution(0.1.0) diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..a818cca --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..657f916 --- /dev/null +++ b/README.md @@ -0,0 +1,353 @@ +This ROS stack includes an Arduino sketch and a collection of ROS +packages for controlling an Arduino-based robot using standard ROS +messages and services. + +Supported sensors currently include Ping sonar and Sharp GP2D12 +infrared as well as generic analog and digital sensors (e.g bump +switches, voltage sensors, etc.) + +The package also includes a base controller for a differential drive +that accepts ROS Twist messages and publishes odometry data back to +the PC. In this version of the stack, the base controller requires the +use of an Arduino Mega 2560 controller together with a Pololu motor +controller shield (http://www.pololu.com/catalog/product/2502) and a +Robogaia Mega Encoder shield +(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html). + +System Requirements +------------------- +The current version of the stack requires an Arudino Mega controller + +Pololu Dual VNH5019 Motor Driver Shield + Robogaia Encoder shield. If +you do not have this hardware, you can still try the package for +reading sensors and controlling servos using other Arduino-compatible +controllers. See the NOTES section at the end of this document for +instructions on how to do this. + +To use the base controller you must also install the Polulu Arduino +library found at: + +https://github.com/pololu/Dual-VNH5019-Motor-Shield + +and the Robogaia Encoder library found at: + +http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz + +These libraries should be installed in your standard Arduino +sketchbook/libraries directory. + +Finally, it is assumed you are using version 1.0 or greater of the +Arduino IDE. + + +Installation +------------ + + $ cd ~/ros_workspace + $ git clone https://github.com/hbrobotics/ros_arduino_bridge.git + $ cd ros_arduino_bridge + $ rosmake + +The provided Arduino sketch is called MegaRobogaiaPololu and is +located in the ros\_arduino\_firmware package. This sketch is +specific to the hardware requirements above but it can also be used +with other Arduino-type boards (e.g. Uno) by turning off the base +controller as described in the NOTES section at the end of this +document. + +To install the MegaRobogaiaPololu sketch, follow these steps: + + $ cd SKETCHBOOK_PATH + +where SKETCHBOOK_PATH is the path to your Arduino sketchbook directory. + + $ ln -s `rospack find ros_arduino_firmware`/src/libraries/MegaRobogaiaPololu MegaRobogaiaPololu + +This last command creates a link in your sketchbook folder for the +MegaRobogaiaPololu sketch that you must run on your Arduino Mega +controller. By creating a link rather than copying the files, the +sketch will get updated along with other files in the +ros\_arduino\_bridge stack. + + +Loading the MegaRobogaiaPololu Sketch +------------------------------------- + +* Make sure you have already installed the DualVNH5019MotorShield and + MegaEncoderCounter libraries into your Arduino sketchbook/libraries + folder. + +* Launch the Arduino 1.0 IDE and load the MegaRobogaiaPololu sketch. + You should be able to find it by going to: + + File->Sketchbook->MegaRobogaiaPololu + +NOTE: If you don't have the Arduino Mega/Pololu/Robogaia hardware but +still want to try the code, see the notes at the end of the file. + +If you want to control PWM servos attached to your controller, change +the two lines that look like this: + +
+//#define USE_SERVOS
+#undef USE_SERVOS
+
+ +to this: + +
+#define USE_SERVOS
+//#undef USE_SERVOS
+
+ +You must then edit the include file servos.h and change the N_SERVOS +parameter as well as the pin numbers for the servos you have attached. + +* Compile and upload the sketch to your Arduino. + +Configuring the ros\_arduino\_python Node +----------------------------------------- +Now that your Arduino is running the required sketch, you can +configure the ROS side of things on your PC. You define your robot's +dimensions, PID parameters, and sensor configuration by editing the +YAML file in the directory ros\_arduino\_python/config. So first move +into that directory: + + $ roscd ros_arduino_python/config + +Now copy the provided config file to one you can modify: + + $ cp arduino_params.yaml my_arduino_params.yaml + +Bring up your copy of the params file (my\_arduino\_params.yaml) in +your favorite text editor. It should start off looking like this: + +
+port: /dev/ttyACM0
+baud: 57600
+timeout: 0.1
+
+rate: 50
+sensorstate_rate: 10
+
+use_base_controller: True
+base_controller_rate: 10
+
+# === Robot drivetrain parameters
+#wheel_diameter: 0.146
+#wheel_track: 0.2969
+#encoder_resolution: 8384 # from Pololu for 131:1 motors
+#gear_reduction: 1.0
+#motors_reversed: True
+
+# ===  PID parameters
+#Kp: 20
+#Kd: 12
+#Ki: 0
+#Ko: 50
+
+#  === Sensor definitions.  Examples only - edit for your robot.
+#      Sensor type can be one of the follow (case sensitive!):
+#	    Ping
+#	    GP2D12
+#	    Analog
+#	    Digital
+#	    PololuMotorCurrent
+
+sensors: {
+  motor_current_left:   {pin: 0, type: PololuMotorCurrent, rate: 5},
+  motor_current_right:  {pin: 1, type: PololuMotorCurrent, rate: 5},
+  ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
+  sonar_front_center:   {pin: 5, type: Ping, rate: 10},
+  arduino_led:          {pin: 13, type: Digital, rate: 2, direction: output}
+}
+
+ +Let's now look at each section of this file. + + _Port Settings_ + +The port will likely be either /dev/ttyACM0 or /dev/ttyUSB0 (e.g. for +an Xbee connection). Set accordingly. + +The MegaRobogaiaPololu Arudino sketch connects at 57600 baud by default. + +_Polling Rates_ + +The main rate parameter (50 Hz by default) determines how fast the +outside ROS loop runs. The default should suffice in most cases. In +any event, it should be at least as fast as your fastest sensor rate +(defined below). + +The sensorstate_rate determines how often to publish an aggregated +list of all sensor readings. Each sensor also publishes on its own +topic and rate. + +The base\_controller\_rate determines how often to publish odometry readings. + +_Defining Sensors_ + +The *sensors* parameter defines a dictionary of sensor names and +sensor parameters. (You can name each sensor whatever you like but +remember that the name for a sensor will also become the topic name +for that sensor.) + +The four most important parameters are pin, type, rate and direction. +The rate defines how many times per second you want to poll that +sensor. For example, a voltage sensor might only be polled once a +second (or even once every 2 seconds: rate=0.5), whereas a sonar +sensor might be polled at 20 times per second. The type must be one +of those listed (case sensitive!). The default direction is input so +to define an output pin, set the direction explicitly to output. In +the example above, the Arduino LED (pin 13) will be blinked on and off +at a rate of 2 times per second. + +_Setting Drivetrain and PID Parameters_ + +To use the base controller, you will have to uncomment and set the +robot drivetrain and PID parameters. The sample drivetrain parameters +are for 6" drive wheels that are 11.5" apart. Note that ROS uses +meters for distance so convert accordingly. The sample encoder +resolution (ticks per revolution) is from the specs for the Pololu +131:1 motor. Set the appropriate number for your motor/encoder +combination. Set the motors_reversed to True if you find your wheels +are turning backward, otherwise set to False. + +The PID parameters are trickier to set. You can start with the sample +values but be sure to place your robot on blocks before sending it +your first Twist command. + +Launching the ros\_arduino\_python Node +--------------------------------------- +Take a look at the launch file arduino.launch in the +ros\_arduino\_python/launch directory. As you can see, it points to a +config file called my\_arduino\_params.yaml. If you named your config +file something different, change the name in the launch file. + +With your Arduino connected and running the MegaRobogaiaPololu sketch, +launch the ros\_arduiono\_python node with your parameters: + + $ roslaunch ros_arduino_python arduino.launch + +You should see something like the following output: + +
+process[arduino-1]: started with pid [6098]
+Connecting to Arduino on port /dev/ttyACM0 ...
+Connected at 57600
+Arduino is ready.
+[INFO] [WallTime: 1355498525.954491] Connected to Arduino on port /dev/ttyACM0 at 57600 baud
+[INFO] [WallTime: 1355498525.966825] motor_current_right {'rate': 5, 'type': 'PololuMotorCurrent', 'pin': 1}
+[INFO]
+etc
+
+ +If you have any Ping sonar sensors on your robot and you defined them +in your config file, they should start flashing to indicate you have +made the connection. + +Viewing Sensor Data +------------------- +To see the aggregated sensor data, echo the sensor state topic: + + $ rostopic echo /arduino/sensors + +To see the data on any particular sensor, echo its topic name: + + $ rostopic echo /arduino/sensor/sensor_name + +For example, if you have a sensor called ir_front_center, you can see +its data using: + + $ rostopic echo /arduino/sensor/ir_front_center + +You can also graph the range data using rxplot: + + $ rxplot -p 60 /arduino/sensor/ir_front_center/range + + +Sending Twist Commands and Viewing Odometry Data +------------------------------------------------ + +Place your robot on blocks, then try publishing a Twist command: + + $ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{ angular: {z: 0.5} }' + +The wheels should turn in a direction consistent with a +counter-clockwise rotation (right wheel forward, left wheel backward). +If they turn in the opposite direction, set the motors_reversed +parameter in your config file to the opposite of its current setting, +then kill and restart the arduino.launch file. + +Stop the robot with the command: + + $ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}' + +To view odometry data: + + $ rostopic echo /odom + +or + + $ rxplot -p 60 /odom/pose/pose/position/x:y, /odom/twist/twist/linear/x, /odom/twist/twist/angular/z + +ROS Services +------------ +The ros_arduino_python package also defines a few ROS services as follows: + +**digital\_set\_direction** - set the direction of a digital pin + + $ rosservice call /arduino/digital_set_direction pin direction + +where pin is the pin number and direction is 0 for input and 1 for output. + +**digital\_write** - send a LOW (0) or HIGH (1) signal to a digital pin + + $ rosservice call /arduino/digital_write pin value + +where pin is the pin number and value is 0 for LOW and 1 for HIGH. + +**servo\_write** - set the position of a servo + + $ rosservice call /arduino/servo_write id pos + +where id is the index of the servo as defined in the Arduino sketch (servos.h) and pos is the position in degrees (0 - 180). + +**servo\_read** - read the position of a servo + + $ rosservice call /arduino/servo_read id + +where id is the index of the servo as defined in the Arduino sketch (servos.h) + +NOTES +----- +If you do not have the hardware required to run the base controller, +follow the instructions below so that you can still use your +Arduino-compatible controller to read sensors and control PWM servos. + +First, you need to edit the MegaRobogaiaPololu sketch. mAt the top of +the file, change the two lines that look like this: + +
+#define USE_BASE
+//#undef USE_BASE
+
+ +to this: + +
+//#define USE_BASE
+#undef USE_BASE
+
+ +You also need to comment out the line that looks like this: + + #include "MegaEncoderCounter.h" + +so it looks like this: + + //#include "MegaEncoderCounter.h" + +Compile the changes and upload to your controller. + +Next, edit your arduino_params.yaml file and set the +use\_base\_controller parameter to False. That's all there is to it. \ No newline at end of file diff --git a/ros_arduino_firmware/CMakeLists.txt b/ros_arduino_firmware/CMakeLists.txt new file mode 100644 index 0000000..f8f1c9c --- /dev/null +++ b/ros_arduino_firmware/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/ros_arduino_firmware/Makefile b/ros_arduino_firmware/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/ros_arduino_firmware/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/ros_arduino_firmware/mainpage.dox b/ros_arduino_firmware/mainpage.dox new file mode 100644 index 0000000..a28c5dd --- /dev/null +++ b/ros_arduino_firmware/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b ros_arduino_firmware is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/ros_arduino_firmware/manifest.xml b/ros_arduino_firmware/manifest.xml new file mode 100644 index 0000000..d601871 --- /dev/null +++ b/ros_arduino_firmware/manifest.xml @@ -0,0 +1,14 @@ + + + + ros_arduino_firmware + + + Patrick Goebel + BSD + + http://ros.org/wiki/ros_arduino_firmware + + + + diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino new file mode 100644 index 0000000..2f167de --- /dev/null +++ b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino @@ -0,0 +1,307 @@ +/********************************************************************* + * ROSArduinoBridge + + A set of simple serial commands to control a differential drive + robot and receive back sensor and odometry data. Default + configuration assumes use of an Arduino Mega + Pololu motor + controller shield + Robogaia Mega Encoder shield. Edit the + readEncoder() and setMotorSpeed() wrapper functions if using + different motor controller or encoder method. + + Created for the Pi Robot Project: http://www.pirobot.org + + Software License Agreement (BSD License) + + Copyright (c) 2012, Patrick Goebel. + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#define USE_BASE // Enable the base controller code +//#undef USE_BASE // Disable the base controller code + +//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h +#undef USE_SERVOS // Disable use of PWM servos + +/* Serial port baud rate */ +#define BAUDRATE 57600 + +/* Maximum PWM signal */ +#define MAX_PWM 255 + +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +/* Include definition of serial commands */ +#include "commands.h" + +/* Sensor functions */ +#include "sensors.h" + +#ifdef USE_SERVOS + #include + #include "servos.h" +#endif + +#ifdef USE_BASE + /* The Pololu motor driver shield */ + #include "DualVNH5019MotorShield.h" + + /* The Robogaia Mega Encoder shield */ + #include "MegaEncoderCounter.h" + + /* Create the motor driver object */ + DualVNH5019MotorShield drive; + + /* Create the encoder shield object */ + MegaEncoderCounter encoders(4); // Initializes the Mega Encoder Counter in the 4X Count Mode + + /* PID parameters and functions */ + #include "diff_controller.h" + + /* Run the PID loop at 30 times per second */ + #define PID_RATE 30 // Hz + const int PID_INTERVAL = 1000 / PID_RATE; + unsigned long nextPID = PID_INTERVAL; + + /* Stop the robot if it hasn't received a movement command + in this number of milliseconds */ + #define AUTO_STOP_INTERVAL 2000 + long lastMotorCommand = AUTO_STOP_INTERVAL; + + // Wrap the encoder reading function + long readEncoder(int i) { + if (i == LEFT) return encoders.YAxisGetCount(); + else return encoders.XAxisGetCount(); + } + + // Wrap the encoder reset function + void resetEncoder(int i) { + if (i == LEFT) return encoders.YAxisReset(); + else return encoders.XAxisReset(); + } + + void resetEncoders() { + resetEncoder(LEFT); + resetEncoder(RIGHT); + } + + // Wrap the drive motor set speed function + void setMotorSpeed(int i, int spd) { + if (i == LEFT) drive.setM1Speed(spd); + else drive.setM2Speed(spd); + } + + // A convenience function for setting both motor speeds + void setMotorSpeeds(int leftSpeed, int rightSpeed) { + setMotorSpeed(LEFT, leftSpeed); + setMotorSpeed(RIGHT, rightSpeed); + } +#endif + +int mode = 0; +int index = 0; +char chr; +char cmd; +char argv1[16]; +char argv2[16]; +long arg1; +long arg2; + +// Clear the current command parameters +void resetCommand() { + cmd = NULL; + memset(argv1, 0, sizeof(argv1)); + memset(argv2, 0, sizeof(argv2)); + arg1 = 0; + arg2 = 0; + mode = 0; + index = 0; +} + +// Run a command +int runCommand() { + int i = 0; + char *p = argv1; + char *str; + int pid_args[4]; + arg1 = atoi(argv1); + arg2 = atoi(argv2); + + switch(cmd) { + case GET_BAUDRATE: + Serial.println(BAUDRATE); + break; + case ANALOG_READ: + Serial.println(analogRead(arg1)); + break; + case DIGITAL_READ: + Serial.println(digitalRead(arg1)); + break; + case ANALOG_WRITE: + analogWrite(arg1, arg2); + Serial.println("OK"); + break; + case DIGITAL_WRITE: + if (arg2 == 0) digitalWrite(arg1, LOW); + else if (arg2 == 1) digitalWrite(arg1, HIGH); + Serial.println("OK"); + break; + case PIN_MODE: + if (arg2 == 0) pinMode(arg1, INPUT); + else if (arg2 == 1) pinMode(arg1, OUTPUT); + Serial.println("OK"); + break; + case PING: + Serial.println(Ping(arg1)); + break; +#ifdef USE_SERVOS + case SERVO_WRITE: + servos[arg1].write(arg2); + Serial.print(arg1); + Serial.print(arg2); + Serial.println("OK"); + break; + case SERVO_READ: + Serial.println(servos[arg1].read()); + break; +#endif + +#ifdef USE_BASE + case READ_ENCODERS: + Serial.print(readEncoder(0)); + Serial.print(" "); + Serial.println(readEncoder(1)); + break; + case RESET_ENCODERS: + resetEncoders(); + Serial.println("OK"); + break; + case MOTOR_SPEEDS: + /* Reset the auto stop timer */ + lastMotorCommand = millis(); + if (arg1 == 0 && arg2 == 0) { + setMotorSpeeds(0, 0); + moving = 0; + } + else moving = 1; + leftPID.TargetTicksPerFrame = arg1; + rightPID.TargetTicksPerFrame = arg2; + Serial.println("OK"); + break; + case UPDATE_PID: + while ((str = strtok_r(p, ":", &p)) != '\0') { + pid_args[i] = atoi(str); + i++; + } + Kp = pid_args[0]; + Kd = pid_args[1]; + Ki = pid_args[2]; + Ko = pid_args[3]; + Serial.println("OK"); + break; +#endif + default: + Serial.println("Invalid Command"); + break; + } +} + +void setup() { + Serial.begin(BAUDRATE); + +#ifdef USE_BASE + drive.init(); +#endif + +#ifdef USE_SERVOS + int i; + for (i = 0; i < N_SERVOS; i++) { + servos[i].attach(servoPins[i]); + } +#endif +} + +void loop() { + while (Serial.available() > 0) { + + chr = Serial.read(); + + // Terminate a command with a CR + if (chr == 13) { + if (mode == 1) argv1[index] = NULL; + else if (mode == 2) argv2[index] = NULL; + runCommand(); + resetCommand(); + } + // Use spaces to delimit parts of the command + else if (chr == ' ') { + if (mode == 0) mode = 1; + else if (mode == 1) { + argv1[index] = NULL; + mode = 2; + index = 0; + } + continue; + } + else { + if (mode == 0) { + cmd = chr; + } + else if (mode == 1) { + argv1[index] = chr; + index++; + } + else if (mode == 2) { + argv2[index] = chr; + index++; + } + } + } + +// If we using base control, run a PID loop at the appropriate intervals +#ifdef USE_BASE + if (millis() > nextPID) { + updatePID(); + nextPID += PID_INTERVAL; + } + + if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {; + setMotorSpeeds(0, 0); + moving = 0; + } + +#endif +} + + + + + + diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h new file mode 100644 index 0000000..18ff0ab --- /dev/null +++ b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h @@ -0,0 +1,22 @@ +/* Define single-letter commands that will be sent by the PC over the + serial link. +*/ + +#define ANALOG_READ 'a' +#define GET_BAUDRATE 'b' +#define PIN_MODE 'c' +#define DIGITAL_READ 'd' +#define READ_ENCODERS 'e' +#define MOTOR_SPEEDS 'm' +#define PING 'p' +#define RESET_ENCODERS 'r' +#define SERVO_WRITE 's' +#define SERVO_READ 't' +#define UPDATE_PID 'u' +#define DIGITAL_WRITE 'w' +#define ANALOG_WRITE 'x' +#define LEFT 0 +#define RIGHT 1 + + + diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h new file mode 100644 index 0000000..292833b --- /dev/null +++ b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h @@ -0,0 +1,68 @@ +/* Functions and type-defs for PID control. Take mostly from Mike Ferguson's + ArbotiX code. +*/ + +/* PID setpoint info For a Motor */ +typedef struct { + double TargetTicksPerFrame; // target speed in ticks per frame + long Encoder; // encoder count + long PrevEnc; // last encoder count + int PrevErr; // last error + int Ierror; // integrated error + int output; // last motor setting +} +SetPointInfo; + +SetPointInfo leftPID, rightPID; + +/* PID Parameters */ +int Kp = 20; +int Kd = 12; +int Ki = 0; +int Ko = 50; + +unsigned char moving = 0; // is the base in motion? + +/* PID routine to compute the next motor commands */ +void doPID(SetPointInfo * p) { + long Perror; + long output; + + Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); + + // Derivative error is the delta Perror + output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; + p->PrevErr = Perror; + p->PrevEnc = p->Encoder; + + output += p->output; + // Accumulate Integral error *or* Limit output. + // Stop accumulating when output saturates + if (output >= MAX_PWM) + output = MAX_PWM; + else if (output <= -MAX_PWM) + output = -MAX_PWM; + else + p->Ierror += Perror; + + p->output = output; +} + +/* Read the encoder values and call the PID routine */ +void updatePID() { + /* Read the encoders */ + leftPID.Encoder = readEncoder(0); + rightPID.Encoder = readEncoder(1); + + /* If we're not moving there is nothing more to do */ + if (!moving) + return; + + /* Compute PID update for each motor */ + doPID(&rightPID); + doPID(&leftPID); + + /* Set the motor speeds accordingly */ + setMotorSpeeds(leftPID.output, rightPID.output); +} + diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h new file mode 100644 index 0000000..75caeb7 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h @@ -0,0 +1,34 @@ +/* Functions for various sensor types */ + +float microsecondsToCm(long microseconds) +{ + // The speed of sound is 340 m/s or 29 microseconds per cm. + // The ping travels out and back, so to find the distance of the + // object we take half of the distance travelled. + return microseconds / 29 / 2; +} + +long Ping(int pin) { + long duration, range; + + // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. + // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: + pinMode(pin, OUTPUT); + digitalWrite(pin, LOW); + delayMicroseconds(2); + digitalWrite(pin, HIGH); + delayMicroseconds(5); + digitalWrite(pin, LOW); + + // The same pin is used to read the signal from the PING))): a HIGH + // pulse whose duration is the time (in microseconds) from the sending + // of the ping to the reception of its echo off of an object. + pinMode(pin, INPUT); + duration = pulseIn(pin, HIGH); + + // convert the time into meters + range = microsecondsToCm(duration); + + return(range); +} + diff --git a/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h new file mode 100644 index 0000000..8b953b7 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h @@ -0,0 +1,16 @@ +/* Define the attachment of any servos here. + The example shows one servo attached on pin 11. +*/ + +#define N_SERVOS 2 + +Servo servos [N_SERVOS]; +byte servoPins [N_SERVOS] = {3, 6}; + + + + + + + + diff --git a/ros_arduino_msgs/CMakeLists.txt b/ros_arduino_msgs/CMakeLists.txt new file mode 100644 index 0000000..f1c1a19 --- /dev/null +++ b/ros_arduino_msgs/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +rosbuild_genmsg() +#uncomment if you have defined services +rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/ros_arduino_msgs/Makefile b/ros_arduino_msgs/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/ros_arduino_msgs/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/ros_arduino_msgs/mainpage.dox b/ros_arduino_msgs/mainpage.dox new file mode 100644 index 0000000..4a0daab --- /dev/null +++ b/ros_arduino_msgs/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b ros_arduino_msgs is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/ros_arduino_msgs/manifest.xml b/ros_arduino_msgs/manifest.xml new file mode 100644 index 0000000..dd70516 --- /dev/null +++ b/ros_arduino_msgs/manifest.xml @@ -0,0 +1,13 @@ + + + + ros_arduino_msgs + + + Patrick Goebel + BSD + + http://ros.org/wiki/ros_arduino_msgs + + + diff --git a/ros_arduino_msgs/msg/Analog.msg b/ros_arduino_msgs/msg/Analog.msg new file mode 100644 index 0000000..8ffeb67 --- /dev/null +++ b/ros_arduino_msgs/msg/Analog.msg @@ -0,0 +1,3 @@ +# Reading from a single analog IO pin. +Header header +uint8 value diff --git a/ros_arduino_msgs/msg/AnalogFloat.msg b/ros_arduino_msgs/msg/AnalogFloat.msg new file mode 100644 index 0000000..1f78bb1 --- /dev/null +++ b/ros_arduino_msgs/msg/AnalogFloat.msg @@ -0,0 +1,3 @@ +# Float message from a single analog IO pin. +Header header +float64 value \ No newline at end of file diff --git a/ros_arduino_msgs/msg/Digital.msg b/ros_arduino_msgs/msg/Digital.msg new file mode 100644 index 0000000..4e8b76c --- /dev/null +++ b/ros_arduino_msgs/msg/Digital.msg @@ -0,0 +1,4 @@ +# Reading on a digital pin +Header header +uint8 value + diff --git a/ros_arduino_msgs/msg/SensorState.msg b/ros_arduino_msgs/msg/SensorState.msg new file mode 100755 index 0000000..272a24a --- /dev/null +++ b/ros_arduino_msgs/msg/SensorState.msg @@ -0,0 +1,4 @@ +Header header + +string[] name +float32[] value diff --git a/ros_arduino_msgs/src/ros_arduino_msgs/__init__.py b/ros_arduino_msgs/src/ros_arduino_msgs/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros_arduino_msgs/src/ros_arduino_msgs/__init__.pyc b/ros_arduino_msgs/src/ros_arduino_msgs/__init__.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d360e186b2a49bd4cfea88b8a24534246422be64 GIT binary patch literal 209 zcmZSn%*z#f_e4N40~9a@{9E|l8T^QT_9JtAUho-9G_T}Qkt2UAD>i|nUbE0 tB9vR4UaVhSl#D4DAD@|*SrQ+wS5R5P0kqyGH$SB`C)Ez*$YLO7008PoI@ACF literal 0 HcmV?d00001 diff --git a/ros_arduino_msgs/srv/DigitalSetDirection.srv b/ros_arduino_msgs/srv/DigitalSetDirection.srv new file mode 100755 index 0000000..01a2e12 --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSetDirection.srv @@ -0,0 +1,3 @@ +uint8 pin +bool direction +--- diff --git a/ros_arduino_msgs/srv/DigitalWrite.srv b/ros_arduino_msgs/srv/DigitalWrite.srv new file mode 100755 index 0000000..9a4a031 --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalWrite.srv @@ -0,0 +1,3 @@ +uint8 pin +bool value +--- diff --git a/ros_arduino_msgs/srv/ServoRead.srv b/ros_arduino_msgs/srv/ServoRead.srv new file mode 100755 index 0000000..d14b4e1 --- /dev/null +++ b/ros_arduino_msgs/srv/ServoRead.srv @@ -0,0 +1,3 @@ +uint8 id +--- +int16 value diff --git a/ros_arduino_msgs/srv/ServoWrite.srv b/ros_arduino_msgs/srv/ServoWrite.srv new file mode 100755 index 0000000..e4fa991 --- /dev/null +++ b/ros_arduino_msgs/srv/ServoWrite.srv @@ -0,0 +1,3 @@ +uint8 id +int16 value +--- diff --git a/ros_arduino_python/CMakeLists.txt b/ros_arduino_python/CMakeLists.txt new file mode 100644 index 0000000..f8f1c9c --- /dev/null +++ b/ros_arduino_python/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/ros_arduino_python/Makefile b/ros_arduino_python/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/ros_arduino_python/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml new file mode 100644 index 0000000..7f22bfb --- /dev/null +++ b/ros_arduino_python/config/arduino_params.yaml @@ -0,0 +1,38 @@ +port: /dev/ttyACM0 +baud: 57600 +timeout: 0.1 + +rate: 50 +sensorstate_rate: 10 + +use_base_controller: True +base_controller_rate: 10 + +# === Robot drivetrain parameters +#wheel_diameter: 0.146 +#wheel_track: 0.2969 +#encoder_resolution: 8384 # from Pololu for 131:1 motors +#gear_reduction: 1.0 +#motors_reversed: True + +# === PID parameters +#Kp: 20 +#Kd: 12 +#Ki: 0 +#Ko: 50 + +# === Sensor definitions. Examples only - edit for your robot. +# Sensor type can be one of the follow (case sensitive!): +# * Ping +# * GP2D12 +# * Analog +# * Digital +# * PololuMotorCurrent + +sensors: { + motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5}, + motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5}, + ir_front_center: {pin: 2, type: GP2D12, rate: 10}, + sonar_front_center: {pin: 5, type: Ping, rate: 10}, + arduino_led: {pin: 13, type: Digital, rate: 2, direction: output} +} diff --git a/ros_arduino_python/launch/arduino.launch b/ros_arduino_python/launch/arduino.launch new file mode 100755 index 0000000..0dd803a --- /dev/null +++ b/ros_arduino_python/launch/arduino.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ros_arduino_python/mainpage.dox b/ros_arduino_python/mainpage.dox new file mode 100644 index 0000000..10ef6c2 --- /dev/null +++ b/ros_arduino_python/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b ros_arduino_python is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/ros_arduino_python/manifest.xml b/ros_arduino_python/manifest.xml new file mode 100644 index 0000000..f12f132 --- /dev/null +++ b/ros_arduino_python/manifest.xml @@ -0,0 +1,22 @@ + + + + ros_arduino_python + + + Patrick Goebel + BSD + + http://ros.org/wiki/ros_arduino_python + + + + + + + + + + + + diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py new file mode 100755 index 0000000..4b2fb44 --- /dev/null +++ b/ros_arduino_python/nodes/arduino_node.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python + +""" + A ROS Node for the Arduino microcontroller + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2012 Patrick Goebel. All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details at: + + http://www.gnu.org/licenses/gpl.html +""" + +import roslib; roslib.load_manifest('ros_arduino_python') +import rospy +from arduino_driver import Arduino +from arduino_sensors import * +from ros_arduino_msgs.srv import * +from base_controller import BaseController +from geometry_msgs.msg import Twist +import os, time +import thread + +class ArduinoROS(): + def __init__(self): + rospy.init_node('Arduino', log_level=rospy.DEBUG) + + # Cleanup when termniating the node + rospy.on_shutdown(self.shutdown) + + self.port = rospy.get_param("~port", "/dev/ttyACM0") + self.baud = int(rospy.get_param("~baud", 57600)) + self.timeout = rospy.get_param("~timeout", 0.5) + + # Overall loop rate: should be faster than fastest sensor rate + self.rate = int(rospy.get_param("~rate", 50)) + r = rospy.Rate(self.rate) + + # Rate at which summary SensorState message is published. Individual sensors publish + # at their own rates. + self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) + + self.use_base_controller = rospy.get_param("~use_base_controller", False) + + # Set up the time for publishing the next SensorState message + now = rospy.Time.now() + self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) + self.t_next_sensors = now + self.t_delta_sensors + + # Initialize a Twist message + self.cmd_vel = Twist() + + # A cmd_vel publisher so we can stop the robot when shutting down + self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) + + # The SensorState publisher periodically publishes the values of all sensors on + # a single topic. + self.sensorStatePub = rospy.Publisher('~sensors', SensorState) + + # A service to position a PWM servo + rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler) + + # A service to read the position of a PWM servo + rospy.Service('~servo_read', ServoRead, self.ServoReadHandler) + + # A service to turn set the direction of a digital pin (0 = input, 1 = output) + rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler) + + # A service to turn a digital sensor on or off + rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler) + + # Initialize the controlller + self.controller = Arduino(self.port, self.baud, self.timeout) + + # Make the connection + self.controller.connect() + + rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") + + # Reservce a thread lock + mutex = thread.allocate_lock() + + # Initialize any sensors + self.mySensors = list() + + sensor_params = rospy.get_param("~sensors", dict({})) + + for name, params in sensor_params.iteritems(): + if params['type'] == "Ping": + sensor = Ping(self.controller, name, params['pin'], params['rate']) + elif params['type'] == "GP2D12": + sensor = GP2D12(self.controller, name, params['pin'], params['rate']) + elif params['type'] == 'Digital': + sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction']) + elif params['type'] == 'Analog': + sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], direction=params['direction']) + elif params['type'] == 'PololuMotorCurrent': + sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate']) + +# if params['type'] == "MaxEZ1": +# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] +# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] + + self.mySensors.append(sensor) + rospy.loginfo(name + " " + str(params)) + + # Initialize the base controller if used + if self.use_base_controller: + self.myBaseController = BaseController(self.controller) + + # Start polling the sensors and base controller + while not rospy.is_shutdown(): + for sensor in self.mySensors: + mutex.acquire() + sensor.poll() + mutex.release() + + if self.use_base_controller: + mutex.acquire() + self.myBaseController.poll() + mutex.release() + + # Publish all sensor values on a single topic for convenience + now = rospy.Time.now() + + if now > self.t_next_sensors: + msg = SensorState() + msg.header.frame_id = 'base_link' + msg.header.stamp = now + for i in range(len(self.mySensors)): + msg.name.append(self.mySensors[i].name) + msg.value.append(self.mySensors[i].value) + try: + self.sensorStatePub.publish(msg) + except: + pass + + self.t_next_sensors = now + self.t_delta_sensors + + r.sleep() + + # Service callback functions + def ServoWriteHandler(self, req): + self.controller.servo_write(req.id, req.value) + return ServoWriteResponse() + + def ServoReadHandler(self, req): + self.controller.servo_read(req.id) + return ServoReadResponse() + + def DigitalSetDirectionHandler(self, req): + self.controller.pin_mode(req.pin, req.direction) + return DigitalSetDirectionResponse() + + def DigitalWriteHandler(self, req): + self.controller.digital_write(req.pin, req.value) + return DigitalWriteResponse() + + def shutdown(self): + # Stop the robot + try: + rospy.loginfo("Stopping the robot...") + self.cmd_vel_pub.Publish(Twist()) + rospy.sleep(2) + except: + pass + rospy.loginfo("Shutting down Arduino Node...") + +if __name__ == '__main__': + myArduino = ArduinoROS() diff --git a/ros_arduino_python/src/__init__.py b/ros_arduino_python/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros_arduino_python/src/arduino_driver.py b/ros_arduino_python/src/arduino_driver.py new file mode 100755 index 0000000..1228437 --- /dev/null +++ b/ros_arduino_python/src/arduino_driver.py @@ -0,0 +1,369 @@ +#!/usr/bin/env python + +""" + A Python driver for the Arduino microcontroller running the + ROSArduinoBridge firmware. + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2012 Patrick Goebel. All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details at: + + http://www.gnu.org/licenses/gpl.html + +""" + +import thread +from math import pi as PI +import os +import time +import sys +from serial.serialutil import SerialException +from serial import Serial + +class Arduino: + ''' Configuration Parameters + ''' + N_ANALOG_PORTS = 6 + N_DIGITAL_PORTS = 12 + + def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): + + self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller. + self.PID_INTERVAL = 1000 / 30 + + self.port = port + self.baudrate = baudrate + self.timeout = timeout + self.encoder_count = 0 + self.writeTimeout = timeout + self.interCharTimeout = timeout / 30. + + # Keep things thread safe + self.mutex = thread.allocate_lock() + + # An array to cache analog sensor readings + self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS + + # An array to cache digital sensor readings + self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS + + def connect(self): + try: + print "Connecting to Arduino on port", self.port, "..." + self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) + # The next line is necessary to give the firmware time to wake up. + time.sleep(1) + test = self.get_baud() + if test != self.baudrate: + time.sleep(1) + test = self.get_baud() + if test != self.baudrate: + raise SerialException + print "Connected at", self.baudrate + print "Arduino is ready." + + except SerialException: + print "Cannot connect to Arduino!" + print "Make sure you are plugged in and turned on." + os._exit(1) + + def open(self): + ''' Open the serial port. + ''' + self.port.open() + + def close(self): + ''' Close the serial port. + ''' + self.port.close() + + def send(self, cmd): + ''' This command should not be used on its own: it is called by the execute commands + below in a thread safe manner. + ''' + self.port.write(cmd + '\r') + + def recv(self, timeout=0.5): + timeout = min(timeout, self.timeout) + ''' This command should not be used on its own: it is called by the execute commands + below in a thread safe manner. Note: we use read() instead of readline() since + readline() tends to return garbage characters from the Arduino + ''' + c = '' + value = '' + attempts = 0 + while c != '\r': + c = self.port.read(1) + value += c + attempts += 1 + if attempts * self.interCharTimeout > timeout: + return None + + value = value.strip('\r') + + return value + + def recv_ack(self): + ''' This command should not be used on its own: it is called by the execute commands + below in a thread safe manner. + ''' + ack = self.recv(self.timeout) + return ack == 'OK' + + def recv_int(self): + ''' This command should not be used on its own: it is called by the execute commands + below in a thread safe manner. + ''' + value = self.recv(self.timeout) + try: + return int(value) + except: + return None + + def recv_array(self): + ''' This command should not be used on its own: it is called by the execute commands + below in a thread safe manner. + ''' + try: + values = self.recv(self.timeout * self.N_ANALOG_PORTS).split() + return map(int, values) + except: + return [] + + def execute(self, cmd): + ''' Thread safe execution of "cmd" on the Arduino returning a single integer value. + ''' + self.mutex.acquire() + + try: + self.port.flushInput() + except: + pass + + ntries = 1 + attempts = 0 + + try: + self.port.write(cmd + '\r') + value = self.recv(self.timeout) + while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): + try: + self.port.flushInput() + self.port.write(cmd + '\r') + value = self.recv(self.timeout) + except: + print "Exception executing command: " + cmd + attempts += 1 + except: + self.mutex.release() + if not self.shutdown: + print "Exception executing command: " + cmd + value = None + + self.mutex.release() + return int(value) + + def execute_array(self, cmd): + ''' Thread safe execution of "cmd" on the Arduino returning an array. + ''' + self.mutex.acquire() + + try: + self.port.flushInput() + except: + pass + + ntries = 1 + attempts = 0 + + try: + self.port.write(cmd + '\r') + values = self.recv_array() + while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): + try: + self.port.flushInput() + self.port.write(cmd + '\r') + values = self.recv_array() + except: + print("Exception executing command: " + cmd) + attempts += 1 + except: + self.mutex.release() + print "Exception executing command: " + cmd + if not self.shutdown: + raise SerialException + return [] + + try: + values = map(int, values) + except: + values = [] + + self.mutex.release() + return values + + def execute_ack(self, cmd): + ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. + ''' + self.mutex.acquire() + + try: + self.port.flushInput() + except: + pass + + ntries = 1 + attempts = 0 + + try: + self.port.write(cmd + '\r') + ack = self.recv(self.timeout) + while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): + try: + self.port.flushInput() + self.port.write(cmd + '\r') + ack = self.recv(self.timeout) + except: + print "Exception executing command: " + cmd + attempts += 1 + except: + self.mutex.release() + if not self.shutdown: + print "execute_ack exception when executing", cmd + print sys.exc_info() + return 0 + + self.mutex.release() + return ack == 'OK' + + def update_pid(self, Kp, Kd, Ki, Ko): + ''' Set the PID parameters on the Arduino + ''' + print "Updating PID parameters" + cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko) + self.execute_ack(cmd) + + def get_baud(self): + ''' Get the current baud rate on the serial port. + ''' + return int(self.execute('b')); + + def get_encoder_counts(self): + values = self.execute_array('e') + if len(values) != 2: + print "Encoder count was not 2" + raise SerialException + return None + else: + return values + + def reset_encoders(self): + ''' Reset the encoder counts to 0 + ''' + return self.execute_ack('r') + + def drive(self, right, left): + ''' Speeds are given in encoder ticks per PID interval + ''' + return self.execute_ack('m %d %d' %(right, left)) + + def drive_m_per_s(self, right, left): + ''' Set the motor speeds in meters per second. + ''' + left_revs_per_second = float(left) / (self.wheel_diameter * PI) + right_revs_per_second = float(right) / (self.wheel_diameter * PI) + + left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) + right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) + + self.drive(right_ticks_per_loop , left_ticks_per_loop ) + + def stop(self): + ''' Stop both motors. + ''' + self.drive(0, 0) + + def analog_read(self, pin): + return self.execute('a %d' %pin) + + def analog_write(self, pin, value): + return self.execute_ack('x %d %d' %(pin, value)) + + def digital_read(self, pin): + return self.execute('d %d' %pin) + + def digital_write(self, pin, value): + return self.execute_ack('w %d %d' %(pin, value)) + + def pin_mode(self, pin, mode): + return self.execute_ack('c %d %d' %(pin, mode)) + + def servo_write(self, id, pos): + ''' Usage: servo_write(id, pos) + ''' + return self.execute_ack('s %d %d' %(id, pos)) + + def servo_read(self, id): + ''' Usage: servo_read(id) + ''' + return self.execute('t %d' %id) + + def ping(self, pin): + ''' The srf05/Ping command queries an SRF05/Ping sonar sensor + connected to the General Purpose I/O line pinId for a distance, + and returns the range in cm. Sonar distance resolution is integer based. + ''' + return self.execute('p %d' %pin); + +# def get_maxez1(self, triggerPin, outputPin): +# ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar +# sensor connected to the General Purpose I/O lines, triggerPin, and +# outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE +# SURE there's nothing directly in front of the MaxSonar-EZ1 upon +# power up, otherwise it wont range correctly for object less than 6 +# inches away! The sensor reading defaults to use English units +# (inches). The sensor distance resolution is integer based. Also, the +# maxsonar trigger pin is RX, and the echo pin is PW. +# ''' +# return self.execute('z %d %d' %(triggerPin, outputPin)) + + +""" Basic test for connectivity """ +if __name__ == "__main__": + if os.name == "posix": + portName = "/dev/ttyACM0" + else: + portName = "COM43" # Windows style COM port. + + baudRate = 57600 + + myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5) + myArduino.connect() + + print "Sleeping for 1 second..." + time.sleep(1) + + print "Reading on analog port 0", myArduino.analog_read(0) + print "Reading on digital port 0", myArduino.digital_read(0) + print "Blinking the LED 3 times" + for i in range(3): + myArduino.digital_write(13, 1) + time.sleep(1.0) + #print "Current encoder counts", myArduino.encoders() + + print "Connection test successful.", + + myArduino.stop() + myArduino.close() + + print "Shutting down Arduino." + diff --git a/ros_arduino_python/src/arduino_driver.pyc b/ros_arduino_python/src/arduino_driver.pyc new file mode 100644 index 0000000000000000000000000000000000000000..65e6182f305d054eecafe342c7a4a4a913e7fde3 GIT binary patch literal 13942 zcmdU0%WoXXd9R)a$)QMzq$qLq*xiy6N3-@Whm!W;NQSi(B~i>uL!yT=B___KIo%vK zInzDr>Ji02${}7O$;Ai|AUDH*f{{c1i=1-_3~Ua8a}1D6kjv)x`>N+b?Mq%`L1s8z z^L2Gqb=CK-uRiAgdvg4L|KVTO8Y=sY;`d`*#&eYNmD)ts-0Jx z1?6PVifXgiYd4}cM|$w6+8h#Q`@x9XJ*|AyIH)nSUKfn1D}(DpPqF#{PT7w)ZpcO&3QYG3m7oMf^(`?zDQJ0 z=am}Q_$=zRgLEg3w4a2}f<$k}iB5L{y_opjFp70MY$S0bj?yGq;bJkg+p!M(oLdr3CpyE;x}I%O~A&8i*fQEnZ^&Dv7~-#H z)Fp3KKqu13RGOXE{7%|#jgN<)<5Mj|3#l}Rq7z#96yRErgkJ0J^F|;Dw+}}84^k?z zOb{9Xg*^~fKQquxJVa*6It)P+EYliBF094f( z0idGJ3INSCCjj)(8v@KJ|C|8OMdt-Lr~C^7oLBxu0WK*2O#v<{|1AODRQ}rnyrq0C zz}w2Ll`j!RZ=ujjakL#ayNL(`#2i8!fTfMlFeJ9a2{vX?@q=fTG(C7!yS)&qe^;uB zN_;-M)hOVH7iqYJ%lsV*;QWPBby0$V&+rCSwl=3;;ES5PdVwzhAq;_HuV#dRU$8w# zRXV12OVqAx1R`JpGS=>dnq?J`->*7ptAKqlQn`rPR6{N}{7;7c%LCUpt;v|)3TVB@(!UEyIG;9Zu#dH!af~XNgVbvQ@ zFR2W&pM+_!o;Bfn!U))1+VPTJB~hI2KwbMXgx6}t4UApK&tA%Tx1k`P%LJ-%6r?9n zRjn^p7ay!H*Vk6v^;&uwRbJ$^;%41Iz+z0mYwQFmg{WG;v$DLhzW5+(JBvC$Y=)`V zI@Gw#70O#o(Asu6E0+T15-(+3wlGftRd(WbQ0Z8cP`Q)DovrwJMeIx1FqI@Wm7Ogy zDY$(Px9@fKnyjvS*2LAf#Bd#|>WEFOn50qZwX*hc9#1%ldOeK7v|j%sz~(Fpl`A=u zxO#t+`BVAR`37|f*|3C5Sn)Fe2meHFTn3lyDzywXuzWuiC&$)+#|S`8fcF4^>%e;e z3ShSa05CC#u?qz;DyE;$04epd$I$4!msiQJUccR#`fEo${gtAZFlOh*c|r3m-`|MJ zs(@whjjQC}l;b#1LwIz8^De1fFu$Eg!&2{qveE*Sw8KdbN|e1@OyYL51+vH=+AkwTKbDL!pNo z5{YefiL0YVkxLd{#bro7WySe*SFh1SFSTO$f%N&larsoQ4K_bU>!*=g$JJXu(ZGv> zE7X4jK%e?BrXcM_0lwGcT#gKXq5zwXRyo4P$HBwQyg*lcgmS@#_@OB8Oxo@_1ytda*3@kqOkJA z5)=eE#E654h(%>rnpyEei09x_&fJnQ@4W3y zlZ-uE#Yu~%y9O0*#Vqp^52oc_3=wGPY(%3JGTB5}3kGYJR|o&xYX*BHyCiWiIF#KU zjfm2Ok^5WOsblAZx&1aXWYD4J?*%SWpJwo=1a^O`4nFFQEo|gizQR81YhSbxO^G(Z zrVQQ=0;|~eFf`zZ0e$)xYGWg+utm1qF>#A|Q$paCvFEHGwpybKmR1J9V|;DfbevPB z<|$K=89D**p$5K<^@kWCL=f^a{FTdeq3CAzHb+Uo4Gs5Qo;LVtA+Cwtev<)0TF`-C zBsMz48w# zPX^g;bxXC6j>8$}iR5B!UMM&Ny~jed*zFB0X#0 zh4et`=p6pyE9B-MhvY_J=u^s&t?$dw)z_6F`?_zCo=r;6!jSYxU}GRO6r(X^<{U&G z5V1hGLbUwBX$7H4Pl((=Zxqx6@M(eiy<+M(mrOzZw#u--y^0hRn^d>9J>u zzxlSN2iAr0x1I^X?PFZrkY(m)Bl=)Scn+DKiw?}qGHegRcH?UI1kFFle z1SZ6vhkHxUPUtId+JCPalhnQ)mfa~!6VWqMY8@<1S zKvdsox!*^_YWeFWY~Z2M91g@xJT!A)WE+Y%AQ-$I4{cdBTMvZo2e>_QVWVXpLd?J7 z)=-yxnW!DMPSfI;(>j#^6=|QSS14Y2TuZpj8VYbAqQbc>A_4DZ4iSPm#wT+0W%_p# zHt64ppELK;`f}L%7TMN848n#KOa&w35D*bvcsDo((VhOW&?`QVoDwl0WaiN4QHN)$ z@JQdDXbM1{oXRK(Z50vXD&!i(Xb-D`QCjw4jAl%B1=ZR`hK6Oc zY-o!m!y}u8z6=XS6U#*+mmFWb|05SWI%G^U>Ep&s5-TLW$~P!(yb8Dk7gR=lT!3w$ z4YzaZ>_GImJYDMqfsd>O985Klh=X`Ij8(=(DUw}~NrJ5#b(tL6Vq?}vl^MkVZT&qT z-ywu?5;iDDZ7u0B5^`+?+ed%)#3{*%>dY&s<61a#LEWbS`u2l3gkU1>a|U4nR*QfR zS0e@_(P-8&ZjGO!{TY#H5v&Xw3^01SFaweyXyGm*DkwD@N3s3mrOkkx5-PD-LKgSOM>A397)<=~kWlq3U4QX| z16!;dGBnUcRf&>&eKjI>oc*mD;Ru}&xd8Y`aYXqS^khAoVZw;mmHuL#-(C4uMR`8| zYUSxZ#@miZY>9i7Y~6Bd`Te1V|ISn%7>mPHmh~PviMiBXVi0Nk-q2FXhP+(w54J#L~0I9CQk&}H4WQB&Qb28U|pM{(6VI=*?c+KDjGcTUS zb$ies`tKqG#k|uu&LaQjt1%>&IrPO}N5>;a5tZN2fVenL^foo;5Pm3T z<6&}5_teCZ2j@@H8~dC@S1(JaLgETGGYnV^62lX4PoP&StGX3N$UY0B6`K+7 zY35^lQ6sRK)*Oc)rpnk;9Bhp+6mqGXJjBtBHq)YI3ca3;tz~{9qiemBo-G`t3^}4= z-aB7KQ5w1T(2W~$qYQ)xl4rJV6n_%g$4a=_a`hiSny2F zWn9$#Qx<>5;?G%lEVfuQPz*3C(9yU|E>(iYCBgL(ow8;F1 ztckqgHbicZ15l10+`Xf(Yi4koka!7cbF&%!*Ddk>m$Ix?o~a^J6M3VJMqmv78Dd_N zeQP+El-MEtAqCTDtGydZKV>R-2~FAu*-@~S0Y)J#&yrn*Jfct}&JtSo z6~3mN91EtE_5H{S`HzV+Lm?f>O&~#)DXILO%ANjPeaKYRQVwjLyMPm*BJ!|jaKKu` PiO`fYoiF5&fR+0{ZwL*o literal 0 HcmV?d00001 diff --git a/ros_arduino_python/src/arduino_sensors.py b/ros_arduino_python/src/arduino_sensors.py new file mode 100755 index 0000000..403148f --- /dev/null +++ b/ros_arduino_python/src/arduino_sensors.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python + +""" + Sensor class for the arudino_python package + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2012 Patrick Goebel. All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details at: + + http://www.gnu.org/licenses/gpl.html +""" + +import roslib; roslib.load_manifest('ros_arduino_python') +import rospy +from sensor_msgs.msg import Range +from ros_arduino_msgs.msg import * + +LOW = 0 +HIGH = 1 + +INPUT = 0 +OUTPUT = 1 + +class MessageType: + ANALOG = 0 + DIGITAL = 1 + RANGE = 2 + FLOAT = 3 + INT = 4 + BOOL = 5 + +class Sensor(object): + def __init__(self, controller, name, pin, rate, frame_id="/base_link", direction="input", **kwargs): + self.controller = controller + self.name = name + self.pin = pin + self.rate = rate + self.direction = direction + + self.frame_id = frame_id + self.value = None + + self.t_delta = rospy.Duration(1.0 / self.rate) + self.t_next = rospy.Time.now() + self.t_delta + + def poll(self): + now = rospy.Time.now() + if now > self.t_next: + if self.direction == "input": + try: + self.value = self.read_value() + except: + return + else: + try: + self.ack = self.write_value() + except: + return + + # The Arduino returns ranges in cm so convert to meters + if self.message_type == MessageType.RANGE: + try: + self.value /= 100.0 + self.msg.range = self.value + except: + return + else: + self.msg.value = self.value + + + # At a timestamp and publish the message + self.msg.header.stamp = rospy.Time.now() + self.pub.publish(self.msg) + + self.t_next = now + self.t_delta + +class AnalogSensor(Sensor): + def __init__(self, *args, **kwargs): + super(AnalogSensor, self).__init__(*args, **kwargs) + + self.message_type = MessageType.ANALOG + + self.msg = Analog() + self.msg.header.frame_id = self.frame_id + + self.pub = rospy.Publisher("~sensor/" + self.name, Analog) + + def read_value(self): + return self.controller.analog_read(self.pin) + + def write_value(self, value): + return self.controller.analog_write(self.pin, value) + +class AnalogFloatSensor(Sensor): + def __init__(self, *args, **kwargs): + super(AnalogFloatSensor, self).__init__(*args, **kwargs) + + self.message_type = MessageType.ANALOG + + self.msg = AnalogFloat() + self.msg.header.frame_id = self.frame_id + + self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat) + + +class DigitalSensor(Sensor): + def __init__(self, *args, **kwargs): + super(DigitalSensor, self).__init__(*args, **kwargs) + + self.message_type = MessageType.BOOL + + self.msg = Digital() + self.msg.header.frame_id = self.frame_id + + self.pub = rospy.Publisher("~sensor/" + self.name, Digital) + + if self.direction == "output": + self.controller.pin_mode(self.pin, OUTPUT) + else: + self.controller.pin_mode(self.pin, INPUT) + + self.value = LOW + + def read_value(self): + return self.controller.digital_read(self.pin) + + def write_value(self): + # Alternate HIGH/LOW when writing at a fixed rate + self.value = not self.value + return self.controller.digital_write(self.pin, self.value) + + +class RangeSensor(Sensor): + def __init__(self, *args, **kwargs): + super(RangeSensor, self).__init__(*args, **kwargs) + + self.message_type = MessageType.RANGE + + self.msg = Range() + self.msg.header.frame_id = self.frame_id + + self.pub = rospy.Publisher("~sensor/" + self.name, Range) + + def read_value(self): + self.msg.header.stamp = rospy.Time.now() + + +class SonarSensor(RangeSensor): + def __init__(self, *args, **kwargs): + super(SonarSensor, self).__init__(*args, **kwargs) + + self.msg.radiation_type = Range.ULTRASOUND + + +class IRSensor(RangeSensor): + def __init__(self, *args, **kwargs): + super(IRSensor, self).__init__(*args, **kwargs) + + self.msg.radiation_type = Range.INFRARED + +class Ping(SonarSensor): + def __init__(self,*args, **kwargs): + super(Ping, self).__init__(*args, **kwargs) + + #self.controller.pin_mode(self.pin, INPUT) + + self.msg.field_of_view = 0.785398163 + self.msg.min_range = 0.02 + self.msg.max_range = 3.0 + + def read_value(self): + return self.controller.ping(self.pin) + + +class GP2D12(IRSensor): + def __init__(self, *args, **kwargs): + super(GP2D12, self).__init__(*args, **kwargs) + + self.msg.field_of_view = 0.001 + self.msg.min_range = 0.10 + self.msg.max_range = 0.80 + + def read_value(self): + value = self.controller.analog_read(self.pin) + try: + distance = (6787 / (value - 3)) - 4 + except: + distance = 80 + if distance > 80: distance = 80 + if distance < 10: distance = 10 + + return distance + +class PololuMotorCurrent(AnalogFloatSensor): + def __init__(self, *args, **kwargs): + super(PololuMotorCurrent, self).__init__(*args, **kwargs) + + def read_value(self): + # From the Pololu source code + milliamps = self.controller.analog_read(self.pin) * 34 + return milliamps / 1000.0 + +class MaxEZ1Sensor(SonarSensor): + def __init__(self, *args, **kwargs): + super(MaxEZ1Sensor, self).__init__(*args, **kwargs) + + self.trigger_pin = kwargs['trigger_pin'] + self.output_pin = kwargs['output_pin'] + + self.msg.field_of_view = 0.785398163 + self.msg.min_range = 0.02 + self.msg.max_range = 3.0 + + def read_value(self): + return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin) + + +if __name__ == '__main__': + myController = Controller() + mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10) + \ No newline at end of file diff --git a/ros_arduino_python/src/arduino_sensors.pyc b/ros_arduino_python/src/arduino_sensors.pyc new file mode 100644 index 0000000000000000000000000000000000000000..899ea6d54c5d25adac93fcb382530de46a7bb1a1 GIT binary patch literal 12063 zcmdT~O>7&-6`m#aZ_2VPo3>*oS=&vbB(@m4hqg{yo0e@^h)fYHx{lSuV#OU%E0Md* z?$WZ_q!*`XFKsUEAqAR4(H`0YIrY#S3iRFp1&a1uE|C%2#R`)KY#%)h)GbDQ&6sjPkSU zfu*eWgREN4N}~Z)A5e`!wLEB^4XXN(s`x`{d06?wYI#I4KPve#<)iql@<-KMNG=ae3o+VO7fLFFIp)J}A3CzOAvQ#;wMJ*51@o!Y5x?WFRL zC_O3t9_`j1R(`%yd#qbKrTpVcAC|T!y0u4?|Af+0QhTyndsO+Sl%9~H;1Csl}f z$*862=Qvj%W|SJU@mbPg62*44?j?!6id?#;Z7**5K^VEst#mC4?WR}V@M>Dx3z?0z zmukP;z8u(2v=XIuIgZwKHN9l7rD^k0p|H8RInxYcKAVZ+nt3^DZpA@uEw!hs&)XMY zx^U4hdube0H|)7cuju-WZ5QixTiPUctP>sI)Benuyj@uf61y2kwb*Oe$gaj(+ex&V zZhEnP+1`p;cGU~}0KPP=aGWMPM%FV^w6}wovYu_n4PO((E`!ZT%*6~!|)26SW(X0m; zFhm@CVY-DeNEf%RJF_?OPVwse!hGc}hGO5CuavGYE!j5~9lL0ki%w;J_Vz;2vCFre z^5W8U%pCW4NVk3&R?d#MF>WS^8m*gEGJ^ED(Thy%r=81EZ6l%@-%v##0 zj|HFLQ%Vm*;o~TAz1VMUYxOkHN~MD!ju+OnDYEW`07M1y_yBUZbdo^(D_c$7#XLT| zij6i46*e1-ux(_z!ZxntEl66t z3tR#O4*_$e2?bVI@e=LUgK$IS5roZFTBW=wTK4%WQqlr>!%}ZR+Q?)igJM=PC=N&l z#UaU{I4BuREZXC(Vb0Y$T$FUOo>fQT==fkv9vo2XIf685*2mN_K7fBxz+p~(sRn}2 zKQB)o#FtETH44)>s@HWavqlw=X>10eRKx&3{fT-QTmo}v+)H3RO zUcF^DbsT=D1sZmIgtFEs)WYn1;IYcr^ zGDSi=lh-x?6t!gEwXAJJK^Ua2`vRCt$f2@EJAWDEGAFDmt(mhyrI*x|rBkRzimWzZqp7@DIB-Ff|w-Pgm3oll2$FwYlrYW<*cO|wI4 zeNx&(FA&%kswag}a>ha1I(qlYv`ljzRn7^LCrD0`$kfy55E%HbD4lZ@ z#gqWL83(C0m7>UqCQ{mc!vL$B0$BwS9BQXDX(TmKU$~o^7M)r{$FK+jWRiM~rZI`u ziYRg0Cp*XSRP4$*g`DuW|5`}HX~LerM%Jl@%9@bBtTkn2t&xoR9?Tv_X`)>>F>u_P z$_SV_?7-o`ou^2i2iYE#|x%3m6fVAEuqT zBW+V_Lv~IbV{3!`z2KZsAHQ$hR>1(Y=wGX;V}~D{PDUb+_qvDq#N0^m@W`wnoN7r~WtfkeQ67uL{}dItO`z;Vv= zWr^2z;*2xH2mOOFYhY&=#u(7psfc}|ew-2pMq)BUR?0x>T2Kp8ufCVs(HrbiJ9r>E zywBtLev7yN&z_H%hHbKp#GpSEh?`LBha@wpu#N`ASz-R62UGGOJ;DbrcC)(S;|>QZ zjuHC`ScXkY3@NJpRSc^~wW-j?jSeH%7U<_tYl%Q*X(CZS)=Dmx%Ua-8ei+*v<=YjB zALmQV(UKMx??{-s?R@*qC@|xlWlP8Lp2|$X3Fp6r1Ey2*jjr~u2mS#^-uA$RW+u+r zEbE^IF&;Qaoihblt+7@X?+8PBJhEU2ovqOkT5CrFJ${+L(I-KO4OV`HB%!s+j9ZU| zQ>Ha1Ol_w;38yS3&2`*fxX5MXUbqOqOdaS`zx)bV>jAG37SE7~A`$O*MgNbDGkzPq z?{da62OJnDM6B=>GQHa*#`drTb7gm|6AjDKNDzN}*Fa?R4BmHW%KaAozXP7y*z?FG zIAh^Hi9@J@So?OC;G<(=Z%a|=#e3P?{}TS5L)V`jb*v96>`;l{r-GEG>AX$o`c38} zbmbG-;Jm$1af(Zew@cT=v8P-V#-1O@rljl!^{+pE7rYush7a!J@RVZz1$m{hB0MED z%{zUe5m&%Uk46Y*r5jGsxo-5qxr_$=s{y|TcaMVxP|Uv~uWb!r1XK>fS{2{&p*>zj zO1_Z)E#l2RfuVaA;$n|n_SF!R(J7e74EZ)%kf~D5{_%L_t%11T>>2Zbq~YB+rT*2X-+@h8C1GQ2 zAZO*wVFag!(DwMR!~{Ww>?cH{N`VOy|q%lT>JSu{Jh`Ed~~_Tc;+ykZR2sMSI#w(>mdDW9q)s?j&)$uxosU; z)G~1tNdWS&b>P5m3m0gx4J|j3jvoc`9Dg%|L$`4`H5*VUgqC^@CkHs7=b2ua8P=jd zR1T^Qo@O!Atu&|irQiV&v**yh3>Jbh5F<#2Gfsho=c7Hsl>;%lJ6_lD!m8d5A@^$& zm4WP^kdh-H$~uY@s65h`l~bckk3NCwqwtrC{Ra+!=!ae4VY%&5j_Ogpbt_7vc(xVC zI!vp0ET1pu^|)KQx7Xvo7yEA?cD#tJGf%?!w?`3XSjVpRUjfM32S{yfmNgzdPuKgL zM&V)mf@K{shvIPWN85+R!5OqQuKUW{R~~5$?A9D|iGRkmqyT5R`+rryp-TS+$wYRz z6wvSMKI+w zMRJyej^c4}cYg<;awCeTtmD@Bw#y@`WRW4vqMr`rh`aXn!NSZKpHVzG>9(u>5c(V7ax0>!8?!k;8ns;L^P;WkLlG zbehU47g$`^kE-pPeYmJq4_0J>gF{T;ZFpg@s*}`QAK*b=A{XggT#`vcHAJ%3WkKGSIi~LZ&8}bOCUp; roEk^udB_^e;F_YOsgeBo{A50tKbfD%59f#S2lG?;(fmXX7t8(&vJLJK literal 0 HcmV?d00001 diff --git a/ros_arduino_python/src/base_controller.py b/ros_arduino_python/src/base_controller.py new file mode 100755 index 0000000..45fe25f --- /dev/null +++ b/ros_arduino_python/src/base_controller.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python + +""" + A base controller class for the Arduino microcontroller + Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code. + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2010 Patrick Goebel. All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details at: + + http://www.gnu.org/licenses +""" +import roslib; roslib.load_manifest('ros_arduino_python') +import rospy +import os + +from math import sin, cos, pi +from geometry_msgs.msg import Quaternion, Twist, Pose +from nav_msgs.msg import Odometry +from tf.broadcaster import TransformBroadcaster + +""" Class to receive Twist commands and publish Odometry data """ +class BaseController: + def __init__(self, arduino): + self.arduino = arduino + self.rate = float(rospy.get_param("~base_controller_rate", 10)) + self.timeout = rospy.get_param('~base_controller_timeout', 1.0) + self.stopped = False + + pid_params = dict() + pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") + pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") + pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") + pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0) + pid_params['Kp'] = rospy.get_param("~Kp", 20) + pid_params['Kd'] = rospy.get_param("~Kd", 12) + pid_params['Ki'] = rospy.get_param("~Ki", 0) + pid_params['Ko'] = rospy.get_param("~Ko", 50) + + self.motors_reversed = rospy.get_param("~motors_reversed", False) + self.accel_limit = rospy.get_param('~accel_limit', 0.1) + + # Set up PID parameters and check for missing values + self.setup_pid(pid_params) + + # How many encoder ticks are there per meter? + self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi) + self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate + + # Track how often we get a bad encoder count (if any) + self.bad_encoder_count = 0 + + now = rospy.Time.now() + self.then = now # time for determining dx/dy + self.t_delta = rospy.Duration(1.0 / self.rate) + self.t_next = now + self.t_delta + + # internal data + self.enc_left = None # encoder readings + self.enc_right = None + self.x = 0 # position in xy plane + self.y = 0 + self.th = 0 # rotation in radians + self.v_left = 0 + self.v_right = 0 + self.v_des_left = 0 # cmd_vel setpoint + self.v_des_right = 0 + self.last_cmd_vel = now + + # subscriptions + rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) + + # Clear any old odometry info + self.arduino.reset_encoders() + + # Set up the odometry broadcaster + self.odomPub = rospy.Publisher('odom', Odometry) + self.odomBroadcaster = TransformBroadcaster() + + rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") + rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz") + + def setup_pid(self, pid_params): + # Check to see if any PID parameters are missing + missing_params = False + for param in pid_params: + if pid_params[param] == "": + print("*** PID Parameter " + param + " is missing. ***") + missing_params = True + + if missing_params: + os._exit(1) + + self.wheel_diameter = pid_params['wheel_diameter'] + self.wheel_track = pid_params['wheel_track'] + self.encoder_resolution = pid_params['encoder_resolution'] + self.gear_reduction = pid_params['gear_reduction'] + + self.Kp = pid_params['Kp'] + self.Kd = pid_params['Kd'] + self.Ki = pid_params['Ki'] + self.Ko = pid_params['Ko'] + + self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko) + + def poll(self): + now = rospy.Time.now() + if now > self.t_next: + # Read the encoders + try: + right_enc, left_enc = self.arduino.get_encoder_counts() + except: + self.bad_encoder_count += 1 + rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) + return + + dt = now - self.then + self.then = now + dt = dt.to_sec() + + # calculate odometry + if self.enc_left == None: + dright = 0 + dleft = 0 + else: + dright = (right_enc - self.enc_right) / self.ticks_per_meter + dleft = (left_enc - self.enc_left) / self.ticks_per_meter + + self.enc_right = right_enc + self.enc_left = left_enc + + dxy_ave = (dright + dleft) / 2.0 + dth = (dright - dleft) / self.wheel_track + vxy = dxy_ave / dt + vth = dth / dt + + if (dxy_ave != 0): + dx = cos(dth) * dxy_ave + dy = -sin(dth) * dxy_ave + self.x += (cos(self.th) * dx - sin(self.th) * dy) + self.y += (sin(self.th) * dx + cos(self.th) * dy) + + if (dth != 0): + self.th += dth + + quaternion = Quaternion() + quaternion.x = 0.0 + quaternion.y = 0.0 + quaternion.z = sin(self.th / 2.0) + quaternion.w = cos(self.th / 2.0) + + # Create the odometry transform frame broadcaster. + self.odomBroadcaster.sendTransform( + (self.x, self.y, 0), + (quaternion.x, quaternion.y, quaternion.z, quaternion.w), + rospy.Time.now(), + "base_link", + "odom" + ) + + odom = Odometry() + odom.header.frame_id = "odom" + odom.child_frame_id = "base_link" + odom.header.stamp = now + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0 + odom.pose.pose.orientation = quaternion + odom.twist.twist.linear.x = vxy + odom.twist.twist.linear.y = 0 + odom.twist.twist.angular.z = vth + + + self.odomPub.publish(odom) + + if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): + self.v_left = 0 + self.v_right = 0 + + # Set motor speeds in encoder ticks per PID loop + if not self.stopped: + if self.motors_reversed: + self.arduino.drive(self.v_left, self.v_right) + else: + self.arduino.drive(self.v_right, self.v_left) + + def stop(self): + self.stopped = True + self.arduino.drive(0, 0) + + def cmdVelCallback(self, req): + # Handle velocity-based movement requests + self.last_cmd_vel = rospy.Time.now() + + x = req.linear.x # m/s + th = req.angular.z # rad/s + + if x == 0: + # Turn in place + right = th * self.wheel_track * self.gear_reduction / 2.0 + left = -right + elif th == 0: + # Pure forward/backward motion + left = right = x + else: + # Rotation about a point in space + left = x - th * self.wheel_track * self.gear_reduction / 2.0 + right = x + th * self.wheel_track * self.gear_reduction / 2.0 + + self.v_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE) + self.v_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE) + + + + + + + diff --git a/ros_arduino_python/src/base_controller.pyc b/ros_arduino_python/src/base_controller.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f687ccbb946df2f17dc6b25340cbfff7e6719e5d GIT binary patch literal 7155 zcmd5=OLH5?5uOD=kRSn)l1Pzyk1g4PD2b3=&P#G^QPeZGDSAm+63eNr;lzWHU><@{4Fo$%m@d*J*N9L%?Qg zq8B`kcOYP~w}(kXwaAjVSk9DhAT6JIHIiYFSIw&bH!ZK`+?YmH;cJX&T-t880^i{m_cy&Y-U z%hFcncO|OLOev|;rrGf`bw}={eHr)(Mum}v(9OP4GBVOn!UpWrO~a_U$AUoYCn2mz zwlga0YMC~<@o43le54YU`LSH>Z^ls|KaT>H!1zAqCW>xj^_x}@jekg5tPQk~4^v3? zO_U~gq>9j=$!(QswB?8SBm;>zWSX%$b>EQknM`{$@ESDUlQFD39DB(A$Bl5X)=)+X zFQT3Hu&g$uVxc<`?769AU#n(6zQGg1GV=M8^~X=2t;?mAm-6|N>n^RVzq|u((@xQ= zww09?b$fAy1;fOdpO`(Yfkk}rz+HZfF-!NJeEwwpC6*!|K3QLRu(l>2K6T-+t4r?s zljUchFS&B{nY;RQ?E&_74W@!kSi*lXHThOI&6Et4@uOHv-`wUgTpaSRR?;WmYG9ve zkMwx-FZ@=_Nt~W6)t+z9h1c7|vXVLl0z-w45`G7%HU+eMk?k8t{mVXCKEXzq5j59# z;7q23+G?s*-9g2Sp?DgmU1hR8OPL`~7)V3)?!7Ga!@$=NVIuS>NDRX6%R=F?4n?lZpnaj)u?MB7N1EC0CLuzG6 zMMN7?Cx+B&J`p6;hSW(BPUUi^hg5{YlR5R&kUAs6*_?WMNSzbmd`>+xq@Jav43<^N z7jaI6=X2sXEwnQsUb9IYYE+q26EoD96lO}y%#vBAnQjM6+uoAk9_TsC20O`9oD!WG zHd|4=#y^J6GW|3yorGf8!2WXsj?SEzBgT2!eX5+zD^S+TKA%@E zMuQw_E|g+!K&V>6J7SI!Daix-yE!{tjMy*RUMzL3eg)3DqGwTlOX=-U-tjV=!H7+DgS<82LDuIC|8@=kF+8R}Xki<_BAba;@dzwmwA+ zDxt)edB{O&+^4wAQRVw&J9kU@#2fYlW~F0)l%$I!^|M|?p#sJc;7Dl9!rG*1p z6oE9LMFUzIeK@40dK&d_yQxVtjfpAAsL-y>4+7XBj=GU){pQ#7`{So(6zoAa^tM&Z z(JV#$((i+~X8ep2`f-FoLD;ucBuAFqMSchsCkd%(iP9i-BQGLTK?QvQB+i<;9VIPE zQ&5PMNQgegaLEgikH4wk0w=39h|nBrn{nzJt|4|KWLLn{QpW4~NcgNYxdjg!H~=$0 z^kZ$Q+N8an3Yj&G0)z7_#4!``ZXdD`&w}y&p4W>)w}ufeEzKpbaB0(63NO^dLOttT zq+8eD^;nWQf!3xUdbxK5Kmj&stiw||)gxJ}}a00pC$7;x}XlINUVqbhjEUG7J z*N$FE6UEFB?8T~SSX*i^LPBg8VwcN3b|2Gb3EP&J1aA*G=`3wy5^cL^akgm%kpluz z8ZUPd;$Q1;>Hs)OWiBNeK1=BTO2x~59B-eho!=1nwj_O@C?9%6>gEDBrd2+6{ThR%YW}Vq$t$5KnTdWkOz<0)(E>`e9_E$U&u34vA zXgEQh>uEohX^y|4K%_&O-u;M@{{5fd5O0uqkmHNu3}D1oL1Z<85ophacpg25!+`%) zjsBk_`(h+GiDp3+^E>b7&pdkqr!~| z+dvM*n5u0c_j1zh)VP9CXo4MOo}Bkf_(q~+K?ifi<&;3^Zed|Tu0FYs&j-W?3ipx& zB7GKgBMp4lT9n|4=!Dp#OulsJcvEO_DTcaqcwLfN=W~A!HFmf4tgrY~0VI;WJ+&K| z|5et3pZ0qQe~N+&d$Rjen(h;tgr*xPJMd(lhr``lcV|~ZyRY$Rs)$0_sX5blYN$;Y z@aB5{4#>`AQVL+~n7=~kgkyI2<9Z(+3Xf#q2`IW%64`gfy)(SiCD9onl<{?eyb`|q zJo!L44xy7$r%XurM+L9UvPQ{|an8s=fbD=M2x*Xeyco!?i1|0{uwe1vNf16x!ik7M z69ld3MErs9^cz50lx30B2?P>DP0Z74Qp}>$A~*~h(%|tHC&OVayn_pav5cbyWcLP# zfLlCZ3Iym18g)S#G^hsG#zJ6)qR4*h7+^~vZJ1_M%)FTp$R)Nt!30L6r4G=g#h|B{ zbwbP#A2hY?s+gIFl}-|=1O$4dlb8<=2J;-4X9S&d;CHLZpRB=85edDgsMi2&!*&QC zSXE6x{-APb23j|1kM7e4N*g0&AmkhRf4j&WR?zPB1EW z9)ur!bL@`*R-R>*@X-LfPKwSs;Q<|EUnYfjfj!I;FY>Gqjpil(m!v~dQ&{%nDRes}YV;PlSPFOHHem%wbjfx?}m;tCa% zjooWhaIU7vN+|9KHq!28kQuBX=c_DpKc)eMM=b_QJ*@)wIuRGBxIqPFRmKD^h4u4&Qd`L+5Lcu^Hf|!!Pz0qqHR^@?8TXctj(LkK#r`;iV;}k23aaJY*Y-z)ti;V zV3MW}clSJh+hWuZ_^}Au_U@j2qs`-Y?Q4&L*Q?QyfY*XLxBF5ok$p?}Q zyuzo=;)AKpfv$e}-O~EKdlRaVa_aaOrmE!%(<2y!H-fxQ8~YxfJlx>-V>I1Q!NB;`Ra9$IL81>9$hEihA?VWGfW)KeYXuSKPGzq0Wu;`bqjbn| z-}AyWz}jdJ@!c6mo1CU_2NHT+KZ%-38=F;^Xb5d$*EemO^*H~L$MMxeOWBJSud7>n z5e26~{%Cheydl?0{OyB&HZl8Ja}l4x`E@maowNS`AEsn4_}K3ICv*@rnXl|rMGe8Y MT7bhKcuyDq4cwSK{r~^~ literal 0 HcmV?d00001 diff --git a/ros_arduino_python/src/ros_arduino_python/__init__.py b/ros_arduino_python/src/ros_arduino_python/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros_arduino_python/src/ros_arduino_python/__init__.pyc b/ros_arduino_python/src/ros_arduino_python/__init__.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6a9f90b7b77a625ac5f8bfd372e625075867eeae GIT binary patch literal 213 zcmZSn%*&NB=U6~80~9a@{9E|l8T^QT_9JtAUho-9G_T}Qkt2UAD>i|nUbE0 uB2-XWl98XMUtE-oO*lS2GcU6wK3)%~j05Ndo80`A(wtN~kVA`sm;nIFg*6<=~LfOC4MAcc|t;hiWd+sNUc=9b7p3D?V5^8 zC<)`t-E+^JbMLwLp8K71Z*u?a>-oh?zg{hi^vU6O25?UG~_oi_#TO;sQf4hW!bZ$NLE0_TS{7C-FE%JtnJ5JfiG)T zdB<`T%O^@TRdy?GIZ;!QjIABz%SsqjGrNcU2gnE~m0i(iWeVLCEIht(c4tWl$r9Ns9@g}9}OTSAy1bS6;EF@b6a6R37F(I#$nG0`q=frb_YK!fNc%ZTWv zuD5}e-0XIz7zQrWhhF~BC$^6h!h*BiD@N%9AdvB!M$|3{i93ap^b9EM5>BFjLb&Lp zn}ja_h%w*`fMF~x5zq|sOs&I#g#f7ECSpfyT!IvNj)+sV|92%?jD z!Z)Z5rp=90Wt2=MV(1VztJ62`H2gqAOWR{C2eol=!`F%PK$GKtFKxvh6Fa&H2lazi zoocZPbY_+RQmS^Vn+LOeNbPis8t8`F{x65)F z<9^t@y5-s+qq%w*DnCBJ%jAYyZx z8%iSC!@-P zC%A}QP=&3astUCvu_(-kL2Wa*S>PkcRQdOw;w<*fbwO&(RDDp>-er)5o=O{gl?LUDPh>gZde59Q24bq7Q0=sQ07Z zk3W=4twvIS3B~NEoav+19PpSJ~VgU}7)HPaYbbW$99q3x3=cH?2i2Veu5mMU3a56kV z_4OO`dPC?FZemD;FKhb`GtDuebx4>LbNHav2boQnS=jV84-$$aDPkrn{@a-trW<0v z&%_e5GP_gZdM4b-!XIbCNwhd5d^r>DV&SvA*f3z6`G*wR7rD?tSPZlNkC3unrWr;gb^zSWj{tQ6@4gDlwO}P2x1Jv zU}Jy^!XXAw<1|$trb1Fd8z$M}j}rA56|{*N#I}@$1f9_^imhsmp@H`sP~D_ZlwiUs zQXmi^b`o$}a3cGVgdc8=!I0sFQH0bo@TUES+}&b`-A#iDp%Hg^D4-nXXF-MVsa3uu zur%z%@yiPw;|YjzU+KBRpfHoCoo`S=$+L9Npp7P@hDi4Uexnyr2!^OL+L$&7DyN;s z+r`wQppF5=QeG~njnX&08|A}H^+T&)dnkMLQ;-?Lmo|M!U&&L5O`((ms%i2>nI=wb zb&slKODU`?;{g!H1QlOF(VUxmfc*xg@DguC)^%v7p5&!0ZdqY%*(ig@4`KNwya$g6 z+qnGz%JFFcFpEnMU?wg-?Bs~hMMnf`*7^9IfBfO<=4zGq5e{R!_&&oKEbau4-vYL>|R5o0XgFBR(Gkli= zD`5+AEi(SwAS1F&=nOo@^B|3BDrm&UQ&iAsV}hZIt32BbD(0@M_KY+1?Qtq-hMJ?f z7ydE}3vT0$2$VXIA#~86o?u#S_KTC}*I)Rw+dKy0HX1!OnvCDnOBX>G@L1i7Urla!X`Kq=;$LcIUdj7z!5{q99Sf{m?S)Q)=7i|#AWS%PVA zW#*<}MgH&9rtHb7GAMo;5$-Wmk93=(po-E!S;7S)j-_rHx2;KvMli*w6#}L*KuC!X z?}396;v^0xzWV>+VDG@k|EaH`hL zBHdqn+S3kPxBl0;lzgMF^wA@@G(y|_INyEsql>lQjUN&I2ucw9DCFs4_+)~3pEJfe z6@Jdnq#b@|CYk9*mSx=YnD$Ey^RsfRv-r-rl`&7${p2^xFzDxG{=syLPo@8Jk7?Xn zqC-4gO^>0)q)o)`n*!unF5%1GjXi)zl+rE!I zq7z2HN9G^&h-A~zh*~JRlrRVj`LY35f1bO^=LcLW+Gf@A-3l&C`Nkq9EQn%oBPW_b z(Y(Fn%S^snPOb!-B5l+@$978ljRh)}sJKi8`7S$Zi$!3Gy9>R1^7wLc$bOz~StGh{ c?a+HgA8sd4YXkaFd8lWkcjVYe?@-Ub0n<={m;e9( literal 0 HcmV?d00001 diff --git a/ros_arduino_python/src/sensors.pyc b/ros_arduino_python/src/sensors.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5b6fabf3dc071d8808b3c5e47e87c8ea5f23c752 GIT binary patch literal 12149 zcmds7&2QYs6(8>E+aIzmS&n1Jp;ISW=R+I2ZP3O|?6qVm0_?R)NpP$SK`losuDIle z^+e?~$5XEj|JzI$* zyWv$rc*;#g5{uMy6M!;yj309%Sl|fuLY~xp3+IRna0t}sh!FL((%T+oyN_i zqU|8!Lu4Q?S&FmZw%cmdev<8FZ*`xxl4Xszip5)AhPo2_ zdb|?Xi|MVXm?h0j@x!A3~gzX+KEnea!dQ;L(+SG zB}nZ?5?2$iZlk!IXpNaz&eps{U$EEXrd{zO^zwrggDy2Q%ur@~kza&0>aiazuggHF zY(~B|7Rq!|PwjYF%r#wJu%~sT6EC!9n@b@Kb2X@7-eE~Rn>FbQv|lm<(ZW-dVy-2H zJr!eQFAL)6f~^DGPwZPdNpWqTZ$G3p;%j!Ch&Tl=qwt9xH`wtkBwn{e%yOsixU}1L z61uwjEjZXKaYI8Jj0#290?g-IR#?1V?*B9pPQu(HRqvSZH^8C#U zXbqU+uD%5jQoEqu2m>gLDNej7TZaxZ#I-BV#JlKIx;%4rX8tBrv8QI{%U9;+?5XRH zU9x9O&iu^8!qt*v&n`H#*XORlwsV-OHekPCs_8uSIMKGRGcO3!Z9zTtOr_nDwaZATHa!&g#x|EG2B$2t#5?jv&)yNCC8u3}Q>lhIhgps(+ z=G0;jtx2nhkph}C;3O0TAzZ*6B1A?nl}lHzPYb{%XQpT7OIHn!O6BP*yF3~DAmbUN zl!))M_?J=m$shrFgMGHn$tZ4bofFIk)$h(mfhcvl6Tx|zDbFs<%anj>P~2tZhau8g zq?DqH>MYE!V-3dt@}pfZBc)#s5~gR*Po6t3oH9F8o|dV&R(kKs;<;U(3KHY%)N=z4 zSTsRQXF6z3nU3=)v_R0K{LVkHmnloMdT&6yB`@?R4nq(G^ox{Pamh=y8wSy8g*arQ zM@=B5A0fNbro=lr;YM8cN&%O7DIo8W0`fj7puvC?a5*T2esu@`z*ndrlENSOkO zM_TygmKQcn37ihz9zd7LCOW~5g?uvC*J0*KulWF?akis)O(e_QNZ-y16vN?C=p1AA z3^QWiIl=5WGm7h+Vs?_*bIhJc)`jqsrgUohbtLgpWeslkmpg9B!b_IwpzI&G$Y5RB zP{GpQhxrW_@@ln5C7+A!d${h``ebYHU9~?<6h%qwSG(n#OBLVM-A58VGR0lrw=w5t*0I&Bnd=3HNvjWvLZ{p$4-Y; z?TCzXP_~6FHAW0<^Rh?TjNF@-3M6yq1!gZYdx_c0%*5OzNaqkTK_eo4-xZKKT=NC7 zYe|r4Qz>Y|e#XE>aCDH(IN9~IDiB4?h|q~xMo{d@Edt(oWV`;FM~o#v}?c; zNgD|8CLl%n8Zu=Kc#a_0ZuJZp}zX4KU5wNzq^ZZ!R{zaumyzF@C=lJ7(CCcFUWfp&M)rfdd2Aq z#$>2Y$-iy7}ASUCVE$&bs|5Q z&*<2bK{d#{@IgHCEi~E4Bf3Y`;L(On;N6Bw`sw(j60D4dN(LW7B_)jeP%1HN`D3!l z0G{NKJ{@x61|s2-=XBQ1y+$cNg!C;OK02k)-GowngMz1uQ+~8T!^hziqTF!GeOf+E zr1Imgs^tRrTixBbA(d^g`gG9?ed`C*!qCep(u>d`!^j7bOA#HmAs0jsCV<#NE(lM0 z&_}|At>Fny0L2HBD1-N*-!k~O7@ry(#!Z>8lFJ>Hv`Z%vL zrQ}b>1XJDFUUcK?O1U)~eK-5uL<$^>3n?ivv^x9pH2MEodPyqt4&pOp7?m8Do zUh*J!eF-hMaaV-=ChBjyYx}sBM7GsSXHolDD?hvxKFNS`i049&;FcF4!VcV0w$ck% z=bh5r^@Z}JcxgtPCU^%YXEiQQi}sMC{u08A!Re^mhp!L9>@LHVCi{@m)8BCN&8Tk1 z`N|`eJMNtzRi>jn<&>N&2B*&3=(&fG`5TD4H<3vb{hHFZkV)2?4Wb90%zVi~4+#13 z=oH_TkV^*`^x(6SRvF=r;aJqSIF^b%v4<^O*2ufJYf zn!Q;4)kpk&)-HU0af2&!UUuNh4v%{r&Y8pgz#j6|Pa*D?Nf?5q)YK!9#?F$mFb*3m;yF5@)TreF4f_-M^TQ0R&sudU;gP8?!% zKY$3(!F&Z_UN7>p3p!Iei8^_=?Ye$k!N+i1WI z%}|?G({W^u7?>pL5IPy(ETr + ros_arduino_bridge + Maintained by Patrick Goebel + BSD + + http://ros.org/wiki/ros_arduino_bridge + + + + + + + +