From 5f0125da8b1e1551b2b0b7227c33af7963ba0cd1 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 7 Dec 2015 06:44:50 -0800 Subject: [PATCH 001/108] Fixed some indentation --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 84 +++++++++---------- 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 5de37268..18bfc22a 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -237,34 +237,34 @@ 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(); + } + #endif } From eacd9267533d1d921ddd7299f85675bec0e4dbea Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 7 Dec 2015 06:51:56 -0800 Subject: [PATCH 002/108] Rearranged some lines for readability --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 18bfc22a..efb1065b 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -66,8 +66,11 @@ #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 +/* Include servo support if required */ +#ifdef USE_SERVOS + #include + #include "servos.h" +#endif /* Maximum PWM signal */ #define MAX_PWM 255 @@ -84,12 +87,6 @@ /* 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" @@ -116,6 +113,7 @@ #endif /* Variable initialization */ +#define BAUDRATE 57600 // A pair of varibles to help parse serial commands (thanks Fergs) int arg = 0; From 5a4affcfbace101cc58e060deeb838cc84d6f6f2 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 7 Dec 2015 06:52:56 -0800 Subject: [PATCH 003/108] Moved MAX_PWM into diff_controller.h --- .../src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino | 3 --- .../src/libraries/ROSArduinoBridge/diff_controller.h | 4 ++++ 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index efb1065b..d34af006 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -72,9 +72,6 @@ #include "servos.h" #endif -/* Maximum PWM signal */ -#define MAX_PWM 255 - #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" #else diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h index f6482091..678800ce 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); From 56686b53d2086210c3fb3a596e090156f324c8f5 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 7 Dec 2015 07:01:11 -0800 Subject: [PATCH 004/108] Fixed #elif statement for ARDUINO_ENC_COUNTER --- .../src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino | 3 +-- .../src/libraries/ROSArduinoBridge/encoder_driver.ino | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index d34af006..420be6d1 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -45,8 +45,7 @@ * 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 the motor controller and encoder library you are using */ #ifdef USE_BASE diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino index 85d401db..ef2b8b11 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino @@ -27,7 +27,7 @@ if (i == LEFT) return encoders.YAxisReset(); else return encoders.XAxisReset(); } -#elif ARDUINO_ENC_COUNTER +#elif defined(ARDUINO_ENC_COUNTER) volatile long left_enc_pos = 0L; volatile long right_enc_pos = 0L; static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0}; //encoder lookup table From 58fc3dee094a03d3fdeb02f66f9ea6e2116ea4a9 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 7 Dec 2015 07:32:31 -0800 Subject: [PATCH 005/108] Simplified #ifdef for PWM servos --- .../src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 420be6d1..c426c89b 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -62,12 +62,11 @@ //#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 PWM servos as defined in servos.h /* Include servo support if required */ #ifdef USE_SERVOS - #include + #include "Servo.h" #include "servos.h" #endif From fcffbda0f720b1829203d9d76f49b3b77cbb79f8 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 8 Dec 2015 07:56:28 -0800 Subject: [PATCH 006/108] Removed obsolete load_manifest statements --- ros_arduino_python/src/ros_arduino_python/arduino_sensors.py | 2 +- ros_arduino_python/src/ros_arduino_python/base_controller.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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..b059cea4 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -19,7 +19,7 @@ http://www.gnu.org/licenses/gpl.html """ -import roslib; roslib.load_manifest('ros_arduino_python') +import roslib import rospy from sensor_msgs.msg import Range from ros_arduino_msgs.msg import * 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..c69a0819 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -20,7 +20,7 @@ http://www.gnu.org/licenses """ -import roslib; roslib.load_manifest('ros_arduino_python') +import roslib import rospy import os From 33104433ca4fac58c804c3989c84c5d1e1c98a4c Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 08:06:59 -0800 Subject: [PATCH 007/108] Added new servo support --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 55 +++++- .../src/libraries/ROSArduinoBridge/commands.h | 4 + .../src/libraries/ROSArduinoBridge/servos2.h | 45 +++++ .../libraries/ROSArduinoBridge/servos2.ino | 89 +++++++++ ros_arduino_msgs/CMakeLists.txt | 10 +- ros_arduino_msgs/srv/Enable.srv | 3 + ros_arduino_msgs/srv/Relax.srv | 2 + ros_arduino_msgs/srv/SetServoSpeed.srv | 3 + ros_arduino_msgs/srv/SetSpeed.srv | 2 + ros_arduino_python/nodes/arduino_node.py | 107 +++++++--- .../src/ros_arduino_python/arduino_driver.py | 24 ++- .../src/ros_arduino_python/arduino_sensors.py | 1 - .../src/ros_arduino_python/base_controller.py | 2 +- .../joint_state_publisher.py | 80 ++++++++ .../ros_arduino_python/servo_controller.py | 185 ++++++++++++++++++ 15 files changed, 573 insertions(+), 39 deletions(-) create mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h create mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino create mode 100755 ros_arduino_msgs/srv/Enable.srv create mode 100755 ros_arduino_msgs/srv/Relax.srv create mode 100755 ros_arduino_msgs/srv/SetServoSpeed.srv create mode 100755 ros_arduino_msgs/srv/SetSpeed.srv create mode 100644 ros_arduino_python/src/ros_arduino_python/joint_state_publisher.py create mode 100755 ros_arduino_python/src/ros_arduino_python/servo_controller.py diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index c426c89b..059b4644 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 @@ -62,12 +62,20 @@ //#define ARDUINO_ENC_COUNTER #endif -#define USE_SERVOS // Enable/disable use of PWM servos as defined in servos.h +//#define USE_SERVOS // Enable/disable use of old PWM servo support as defined in servos.h -/* Include servo support if required */ +#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" + +/* 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 @@ -141,13 +149,13 @@ 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); - + switch(cmd) { case GET_BAUDRATE: Serial.println(BAUDRATE); @@ -183,6 +191,33 @@ int runCommand() { case SERVO_READ: Serial.println(servos[arg1].getServo().read()); break; +#elif defined(USE_SERVOS2) + case CONFIG_SERVO: + myServos[arg1].initServo(arg1, arg2); + myServoPins[nServos] = arg1; + nServos++; + Serial.println("OK"); + break; + case SERVO_WRITE: + myServos[arg1].setTargetPosition(arg2); + Serial.println("OK"); + break; + case SERVO_READ: + Serial.println(myServos[arg1].getCurrentPosition()); + break; + case SERVO_DELAY: + myServos[arg1].setServoDelay(arg1, arg2); + Serial.println("OK"); + break; + case DETACH_SERVO: + myServos[arg1].getServo().detach(); + Serial.println("OK"); + break; + case ATTACH_SERVO: + myServos[arg1].getServo().attach(arg1); + Serial.println("OK"); + break; + #endif #ifdef USE_BASE @@ -209,6 +244,7 @@ int runCommand() { Serial.println("OK"); break; case UPDATE_PID: + i = 0; while ((str = strtok_r(p, ":", &p)) != '\0') { pid_args[i] = atoi(str); i++; @@ -334,6 +370,13 @@ void loop() { for (i = 0; i < N_SERVOS; i++) { servos[i].doSweep(); } - #endif + + #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..402f11cd 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h @@ -10,14 +10,18 @@ #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 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..e6e0dc39 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.h @@ -0,0 +1,45 @@ +#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, + int stepDelayMs); + + void setServoDelay( + int servoPin, + int stepDelayMs); + + void moveServo(); + void setTargetPosition(int position); + void setServoDelay(int delay); + int getCurrentPosition(); + + Servo getServo(); + + private: + Servo servo; + int stepDelayMs; + int currentPositionDegrees; + int targetPositionDegrees; + long lastSweepCommand; +}; + +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..d8cf5945 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino @@ -0,0 +1,89 @@ +/*************************************************************** + 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 = 0; + this->targetPositionDegrees = 90; + this->lastSweepCommand = 0; +} + +// Init +void SweepServo2::initServo( + int servoPin, + int stepDelayMs) +{ + this->stepDelayMs = stepDelayMs; + this->currentPositionDegrees = 0; + this->targetPositionDegrees = 90; + this->lastSweepCommand = millis(); + this->servo.attach(servoPin); +} + +// Init +void SweepServo2::setServoDelay( + int servoPin, + int stepDelayMs) +{ + this->stepDelayMs = stepDelayMs; +} + +// Perform Sweep +void SweepServo2::moveServo() +{ + + // Get ellapsed time + int delta = millis() - this->lastSweepCommand; + + // Check if time for a step + if (delta > this->stepDelayMs) { + // Check step direction + if (this->targetPositionDegrees > this->currentPositionDegrees) { + this->currentPositionDegrees++; + this->servo.write(this->currentPositionDegrees); + } + else if (this->targetPositionDegrees < this->currentPositionDegrees) { + this->currentPositionDegrees--; + this->servo.write(this->currentPositionDegrees); + } + // if target == current position, do nothing + + // reset timer + this->lastSweepCommand = millis(); + } +} + +// Set a new target position +void SweepServo2::setTargetPosition(int position) +{ + this->targetPositionDegrees = position; +} + +int SweepServo2::getCurrentPosition() +{ + return this->currentPositionDegrees; +} + +// Accessor for servo object +Servo SweepServo2::getServo() +{ + return this->servo; +} + + +#endif diff --git a/ros_arduino_msgs/CMakeLists.txt b/ros_arduino_msgs/CMakeLists.txt index 373ae0af..91b7c093 100644 --- a/ros_arduino_msgs/CMakeLists.txt +++ b/ros_arduino_msgs/CMakeLists.txt @@ -12,13 +12,17 @@ add_message_files(FILES ) add_service_files(FILES + AnalogWrite.srv + AnalogRead.srv + DigitalRead.srv DigitalSetDirection.srv DigitalWrite.srv - DigitalRead.srv + Enable.srv + Relax.srv ServoRead.srv ServoWrite.srv - AnalogWrite.srv - AnalogRead.srv + SetSpeed.srv + SetServoSpeed.srv ) generate_messages( 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/SetServoSpeed.srv b/ros_arduino_msgs/srv/SetServoSpeed.srv new file mode 100755 index 00000000..5a9170ba --- /dev/null +++ b/ros_arduino_msgs/srv/SetServoSpeed.srv @@ -0,0 +1,3 @@ +uint8 id +float32 value +--- diff --git a/ros_arduino_msgs/srv/SetSpeed.srv b/ros_arduino_msgs/srv/SetSpeed.srv new file mode 100755 index 00000000..effb1670 --- /dev/null +++ b/ros_arduino_msgs/srv/SetSpeed.srv @@ -0,0 +1,2 @@ +float32 value +--- diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 103179d5..fa08337c 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -24,9 +24,12 @@ from ros_arduino_python.arduino_sensors import * from ros_arduino_msgs.srv import * from ros_arduino_python.base_controller import BaseController +from ros_arduino_python.servo_controller import Servo, ServoController +from ros_arduino_python.joint_state_publisher import JointStatePublisher from geometry_msgs.msg import Twist import os, time import thread +from math import radians class ArduinoROS(): def __init__(self): @@ -86,11 +89,11 @@ def __init__(self): # 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) # Make the connection - self.controller.connect() + self.device.connect() rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") @@ -100,8 +103,10 @@ def __init__(self): # Initialize any sensors self.mySensors = 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: @@ -110,19 +115,19 @@ def __init__(self): params['direction'] = 'input' if params['type'] == "Ping": - sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame) + sensor = Ping(self.device, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == "GP2D12": - sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame) + sensor = GP2D12(self.device, 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']) + sensor = DigitalSensor(self.device, 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']) + sensor = AnalogSensor(self.device, 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) + sensor = PololuMotorCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == 'PhidgetsVoltage': - sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame) + sensor = PhidgetsVoltage(self.device, name, params['pin'], params['rate'], self.base_frame) elif params['type'] == 'PhidgetsCurrent': - sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) + sensor = PhidgetsCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) # if params['type'] == "MaxEZ1": # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] @@ -130,12 +135,37 @@ def __init__(self): self.mySensors.append(sensor) rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) + + # Initialize any joints (servos) + self.joints = dict() + + # Read in the joints (if any) + joint_params = rospy.get_param("~joints", dict()) + + if len(joint_params) != 0: + self.have_joints = True + + # Configure each servo + for name, params in joint_params.iteritems(): + self.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, self.joints, "ServoController") + + # The joint state publisher publishes the latest joint values on the /joint_states topic + self.joint_state_publisher = JointStatePublisher() + + else: + self.have_joints = False # Initialize the base controller if used if self.use_base_controller: - self.myBaseController = BaseController(self.controller, self.base_frame) + self.myBaseController = BaseController(self.device, self.base_frame) - # Start polling the sensors and base controller + # Start polling the sensors, base controller, and servo controller while not rospy.is_shutdown(): for sensor in self.mySensors: mutex.acquire() @@ -146,6 +176,12 @@ def __init__(self): mutex.acquire() self.myBaseController.poll() mutex.release() + + if self.have_joints: + mutex.acquire() + self.servo_controller.poll() + self.joint_state_publisher.poll(self.joints.values()) + mutex.release() # Publish all sensor values on a single topic for convenience now = rospy.Time.now() @@ -168,42 +204,59 @@ def __init__(self): # Service callback functions def ServoWriteHandler(self, req): - self.controller.servo_write(req.id, req.value) + self.device.servo_write(req.id, req.value) return ServoWriteResponse() + def SetServoSpeedWriteHandler(self, req): + index = self.joint.values['pin'].index(req.pin) + name = self.joints.keys[index] + + # Convert servo speed in deg/s to a step delay in milliseconds + step_delay = self.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) + pos = self.device.servo_read(req.id) return ServoReadResponse(pos) def DigitalSetDirectionHandler(self, req): - self.controller.pin_mode(req.pin, req.direction) + self.device.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 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 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...") + + # Stop the robot + rospy.loginfo("Stopping the robot...") + self.cmd_vel_pub.publish(Twist()) + rospy.sleep(2) + + # Detach any servos + if self.have_joints: + rospy.loginfo("Detaching servos...") + for joint in self.joints.values(): + self.device.detach_servo(joint.pin) + rospy.sleep(0.1) if __name__ == '__main__': myArduino = ArduinoROS() 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..5e4ae2ce 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -309,11 +309,15 @@ def digital_write(self, pin, value): def 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 %d' %(pin, step_delay)) 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): @@ -321,7 +325,25 @@ def servo_read(self, id): The returned position is converted from degrees to radians ''' return radians(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, 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 b059cea4..f28e313d 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -19,7 +19,6 @@ http://www.gnu.org/licenses/gpl.html """ -import roslib import rospy from sensor_msgs.msg import Range from ros_arduino_msgs.msg import * 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 c69a0819..9c8ec216 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -20,7 +20,7 @@ http://www.gnu.org/licenses """ -import roslib + import rospy import os 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..fd0d0a6c --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -0,0 +1,185 @@ +#!/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 math import radians, degrees + +class Joint: + def __init__(self, device, name): + self.device = device + self.name = name + + self.target_position = 0.0 # radians + self.position = 0.0 # radians + self.position_last = 0.0 # radians + self.velocity = 0.0 # rad/s + +class Servo(Joint): + def __init__(self, device, name, ns="~joints"): + Joint.__init__(self, device, name) + + # Construct the namespace for the joint + n = ns + "/" + name + "/" + + # The Arduino pin used by this servo + self.pin = int(rospy.get_param(n + "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(n + "rated_speed", 0.24) # seconds per 60 degrees + + self.max_deg_per_sec = 60.0 / self.rated_speed + self.ms_per_deg = 1000.0 / self.max_deg_per_sec + + # Convert initial servo speed in deg/s to a step delay in milliseconds + step_delay = self.get_step_delay(rospy.get_param(n + "init_speed", 90.0)) + + # Update the servo speed + self.device.config_servo(self.pin, step_delay) + + # Min/max/neutral values + self.neutral = radians(rospy.get_param(n + "neutral", 90.0)) # degrees + self.max_angle = radians(rospy.get_param(n + "max_angle", 90.0)) # degrees + self.min_angle = radians(rospy.get_param(n + "min_angle", -90.0)) # degrees + self.range = radians(rospy.get_param(n + "range", 180)) # degrees + self.max_speed = radians(rospy.get_param(n + "max_speed", 250.0)) # deg/s + + # Do we want to reverse positive motion + self.invert = rospy.get_param(n + "invert", False) + + # Where to we want the servo positioned + self.desired = self.neutral - radians(rospy.get_param(n + "init_position", 90)) + + # Where is the servo positioned now + self.position = 0.0 + + # 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, req): + # Check limits + if req.data > self.max_angle: + req.data = self.max_angle + + if req.data < self.min_angle: + req.data = self.min_angle + + # Adjust for the neutral offset + if self.invert: + target_adjusted = self.neutral - req.data + else: + target_adjusted = self.neutral + req.data + + # Set the target position for the next servo controller update + self.desired = target_adjusted + + def get_step_delay(self, target_speed=90): + # Don't allow negative speeds + target_speed = abs(target_speed) + + if target_speed > self.max_deg_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 = self.ms_per_deg * (self.max_deg_per_sec / target_speed - 1) + except: + step_delay = 32767 + + # Minimum step delay is 1 millisecond + step_delay = max(1, step_delay) + + return int(step_delay) + + def get_current_position(self): + return self.device.servo_read(self.pin) - self.neutral + + def set_target_position(self): + return self.device.servo_read(self.pin) + + 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) + else: + self.device.detach_servo(self.pin) + + return EnableResponse() + + def set_speed_cb(self, req): + # Convert servo speed in deg/s to a step delay in milliseconds + step_delay = self.get_step_delay(req.value) + + # Update the servo speed + self.device.set_servo_delay(self.pin, step_delay) + + return SetSpeedResponse() + +class ServoController(): + def __init__(self, device, joints, name): + self.device = device + self.servos = list() + + joint_update_rate = rospy.get_param("~joint_update_rate", 10.0) + + # Get the servo objects from the joint list + for servo in joints.values(): + self.servos.append(servo) + servo.position_last = servo.get_current_position() + + self.w_delta = rospy.Duration(1.0 / joint_update_rate) + self.w_next = rospy.Time.now() + self.w_delta + + self.r_delta = rospy.Duration(1.0 / joint_update_rate) + self.r_next = rospy.Time.now() + self.r_delta + + def poll(self): + """ Read and write servo positions and velocities. """ + if rospy.Time.now() > self.r_next: + for servo in self.servos: + servo.position = servo.get_current_position() + + # Compute velocity + servo.velocity = (servo.position - servo.position_last) / self.r_delta.to_sec() + servo.position_last = servo.position + + self.r_next = rospy.Time.now() + self.r_delta + + if rospy.Time.now() > self.w_next: + for servo in self.servos: + self.device.servo_write(servo.pin, servo.desired) + self.w_next = rospy.Time.now() + self.w_delta + + \ No newline at end of file From 5c63db732db753b9f1fa18bf3f291b80369e7457 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 08:19:13 -0800 Subject: [PATCH 008/108] Added clear_params=true to arduino.launch file --- ros_arduino_python/launch/arduino.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 @@ - + From 11105cf959843567a115898383a75c047afbde3f Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 08:20:18 -0800 Subject: [PATCH 009/108] Fixed init_position default in servo_controller.py --- ros_arduino_python/src/ros_arduino_python/servo_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index fd0d0a6c..98f941d4 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -63,14 +63,14 @@ def __init__(self, device, name, ns="~joints"): self.neutral = radians(rospy.get_param(n + "neutral", 90.0)) # degrees self.max_angle = radians(rospy.get_param(n + "max_angle", 90.0)) # degrees self.min_angle = radians(rospy.get_param(n + "min_angle", -90.0)) # degrees - self.range = radians(rospy.get_param(n + "range", 180)) # degrees + self.range = radians(rospy.get_param(n + "range", 180)) # degrees self.max_speed = radians(rospy.get_param(n + "max_speed", 250.0)) # deg/s # Do we want to reverse positive motion self.invert = rospy.get_param(n + "invert", False) # Where to we want the servo positioned - self.desired = self.neutral - radians(rospy.get_param(n + "init_position", 90)) + self.desired = self.neutral - radians(rospy.get_param(n + "init_position", 0)) # Where is the servo positioned now self.position = 0.0 From a24d34eab2ce42d722ff966642c05c3fadb661ac Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 08:21:21 -0800 Subject: [PATCH 010/108] Added servo example to ros_arduino_python --- .../config/servo_example_params.yaml | 31 +++++ .../launch/servo_example.launch | 5 + ros_arduino_python/nodes/sweep_servo.py | 128 ++++++++++++++++++ 3 files changed, 164 insertions(+) create mode 100644 ros_arduino_python/config/servo_example_params.yaml create mode 100755 ros_arduino_python/launch/servo_example.launch create mode 100755 ros_arduino_python/nodes/sweep_servo.py 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..18596c1b --- /dev/null +++ b/ros_arduino_python/config/servo_example_params.yaml @@ -0,0 +1,31 @@ +# A UDEV rule maps the Arduino serial number to /dev/arduino + +port: /dev/arduino +baud: 57600 +timeout: 0.1 + +rate: 50 +sensorstate_rate: 10 + +use_base_controller: False + +joint_update_rate: 10 + +# === Sensor definitions. Examples only - edit for your robot. +# Sensor type can be one of the follow (case sensitive!): +# * Analog +# * Digital +# * Ping +# * GP2D12 +# * PololuMotorCurrent +# * PhidgetsVoltage +# * PhidgetsCurrent (20 Amp, DC) + +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: 30, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False} +} 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/nodes/sweep_servo.py b/ros_arduino_python/nodes/sweep_servo.py new file mode 100755 index 00000000..f061695e --- /dev/null +++ b/ros_arduino_python/nodes/sweep_servo.py @@ -0,0 +1,128 @@ +#!/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 == '': + rospy.logino("Joint name for servo must be specified in launch file.") + os._exit(1) + + servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin') + max_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_angle')) + min_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_angle')) + + # Set the sweep limits to 90% of the max limits + target_max = 0.9 * max_angle + target_min = 0.9 * min_angle + + # How fast should we sweep the servo + speed = rospy.get_param('~speed', 60) # degrees per second + + # Time between between sweeps + delay = rospy.get_param('~delay', 2) # 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(speed) + + rospy.loginfo('Sweeping servo...') + + while not rospy.is_shutdown(): + joint_pub.publish(target_max) + + while abs(self.get_joint_position(joint_name) - target_max) > 0.1: + rospy.sleep(0.1) + + rospy.sleep(delay) + + joint_pub.publish(target_min) + + while abs(self.get_joint_position(joint_name) - target_min) > 0.1: + 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 From ab0f16a6ffb3582e34c7d38af4786c79fd6eac22 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 16:33:09 -0800 Subject: [PATCH 011/108] Added servo params to same arduino_params.yaml file --- ros_arduino_python/config/arduino_params.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index 9acbe3dc..1d23d8be 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -47,5 +47,11 @@ 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} } From aefe39bf41f09b30407b821ee264288c3f4d6171 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 16:35:26 -0800 Subject: [PATCH 012/108] Cleaned up object constructors and fixed set_speed calculation --- ros_arduino_python/nodes/arduino_node.py | 40 ++++++++++++++----- .../ros_arduino_python/servo_controller.py | 32 ++++++++------- 2 files changed, 49 insertions(+), 23 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index fa08337c..32988822 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -25,15 +25,18 @@ from ros_arduino_msgs.srv import * 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 from geometry_msgs.msg import Twist import os, time import thread from math import radians +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) # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) @@ -137,7 +140,7 @@ def __init__(self): rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) # Initialize any joints (servos) - self.joints = dict() + self.device.joints = dict() # Read in the joints (if any) joint_params = rospy.get_param("~joints", dict()) @@ -147,23 +150,42 @@ def __init__(self): # Configure each servo for name, params in joint_params.iteritems(): - self.joints[name] = Servo(self.device, name) + 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, self.joints, "ServoController") + 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() +# # 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() else: self.have_joints = False - + # Initialize the base controller if used if self.use_base_controller: self.myBaseController = BaseController(self.device, self.base_frame) + + print "==> ROS Arduino Bridge ready for action!" # Start polling the sensors, base controller, and servo controller while not rospy.is_shutdown(): @@ -180,7 +202,7 @@ def __init__(self): if self.have_joints: mutex.acquire() self.servo_controller.poll() - self.joint_state_publisher.poll(self.joints.values()) + self.joint_state_publisher.poll(self.device.joints.values()) mutex.release() # Publish all sensor values on a single topic for convenience @@ -209,10 +231,10 @@ def ServoWriteHandler(self, req): def SetServoSpeedWriteHandler(self, req): index = self.joint.values['pin'].index(req.pin) - name = self.joints.keys[index] + name = self.device.joints.keys[index] # Convert servo speed in deg/s to a step delay in milliseconds - step_delay = self.joints[name].get_step_delay(req.value) + step_delay = self.device.joints[name].get_step_delay(req.value) # Update the servo speed self.device.config_servo(pin, step_delay) @@ -254,7 +276,7 @@ def shutdown(self): # Detach any servos if self.have_joints: rospy.loginfo("Detaching servos...") - for joint in self.joints.values(): + for joint in self.device.joints.values(): self.device.detach_servo(joint.pin) rospy.sleep(0.1) diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index 98f941d4..e77325aa 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -23,6 +23,7 @@ import rospy from std_msgs.msg import Float64 from ros_arduino_msgs.srv import Relax, Enable, SetSpeed, SetSpeedResponse, RelaxResponse, EnableResponse +from controllers import * from math import radians, degrees @@ -30,6 +31,7 @@ class Joint: def __init__(self, device, name): self.device = device self.name = name + self.controller = None self.target_position = 0.0 # radians self.position = 0.0 # radians @@ -50,11 +52,12 @@ def __init__(self, device, name, ns="~joints"): # A value of 0.24 seconds per 60 degrees is typical. self.rated_speed = rospy.get_param(n + "rated_speed", 0.24) # seconds per 60 degrees - self.max_deg_per_sec = 60.0 / self.rated_speed - self.ms_per_deg = 1000.0 / self.max_deg_per_sec + # Conversion factors to compute servo speed to step delay between updates + self.max_rad_per_sec = radians(60.0) / self.rated_speed + self.ms_per_rad = 1000.0 / self.max_rad_per_sec # Convert initial servo speed in deg/s to a step delay in milliseconds - step_delay = self.get_step_delay(rospy.get_param(n + "init_speed", 90.0)) + step_delay = self.get_step_delay(radians(rospy.get_param(n + "init_speed", 90.0))) # Update the servo speed self.device.config_servo(self.pin, step_delay) @@ -70,7 +73,7 @@ def __init__(self, device, name, ns="~joints"): self.invert = rospy.get_param(n + "invert", False) # Where to we want the servo positioned - self.desired = self.neutral - radians(rospy.get_param(n + "init_position", 0)) + self.desired = self.neutral + radians(rospy.get_param(n + "init_position", 0)) # Where is the servo positioned now self.position = 0.0 @@ -100,22 +103,22 @@ def command_cb(self, req): # Set the target position for the next servo controller update self.desired = target_adjusted - def get_step_delay(self, target_speed=90): + def get_step_delay(self, target_speed=1.0): # Don't allow negative speeds target_speed = abs(target_speed) - if target_speed > self.max_deg_per_sec: + if target_speed > self.max_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 = self.ms_per_deg * (self.max_deg_per_sec / target_speed - 1) + step_delay = 1000.0 / degrees(1.0) * (1.0 / target_speed - 1.0 / self.max_rad_per_sec) except: step_delay = 32767 - # Minimum step delay is 1 millisecond - step_delay = max(1, step_delay) + # Minimum step delay is 0 millisecond + step_delay = max(0, step_delay) return int(step_delay) @@ -139,7 +142,7 @@ def enable_cb(self, req): return EnableResponse() def set_speed_cb(self, req): - # Convert servo speed in deg/s to a step delay in milliseconds + # Convert servo speed in rad/s to a step delay in milliseconds step_delay = self.get_step_delay(req.value) # Update the servo speed @@ -147,15 +150,16 @@ def set_speed_cb(self, req): return SetSpeedResponse() -class ServoController(): - def __init__(self, device, joints, name): - self.device = device +class ServoController(Controller): + def __init__(self, device, name): + Controller.__init__(self, device, name) + self.servos = list() joint_update_rate = rospy.get_param("~joint_update_rate", 10.0) # Get the servo objects from the joint list - for servo in joints.values(): + for servo in self.device.joints.values(): self.servos.append(servo) servo.position_last = servo.get_current_position() From 973b6ae5a1e29feb2f7a361ae1c083045d362643 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 16:36:05 -0800 Subject: [PATCH 013/108] Added controllers.py --- .../src/ros_arduino_python/controllers.py | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 ros_arduino_python/src/ros_arduino_python/controllers.py 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..96f191a3 --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/controllers.py @@ -0,0 +1,73 @@ +#!/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 + + ## @brief Get a diagnostics message for this joint. + ## + ## @return Diagnostics message. + def getDiagnostics(self): + msg = DiagnosticStatus() + msg.name = self.name + msg.level = DiagnosticStatus.OK + msg.message = "OK" + return msg From 812c4025f7bf85daaac652c66872d36bf8e2ecc8 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 16:36:23 -0800 Subject: [PATCH 014/108] Updated README with new servo instructions --- README.md | 115 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 90 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 10366eaa..13896f71 100644 --- a/README.md +++ b/README.md @@ -129,9 +129,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 +139,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: -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. +
+//#define USE_BASE
+
+ +so it looks like this: + +
+#define USE_BASE
+
+ +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, only the Robogaia Mega Encoder shield is supported and it is chosen by default. +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. -If you want to control PWM servos attached to your controller, change -the two lines that look like this: +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 +189,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 +221,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 +300,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 +354,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 +449,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 +477,35 @@ 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. 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 +538,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" From 413908c57cf2269d9f7871bacd8e031d3a07eebe Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 13 Dec 2015 16:40:26 -0800 Subject: [PATCH 015/108] Fixed README typos --- README.md | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 13896f71..f9f0412d 100644 --- a/README.md +++ b/README.md @@ -479,9 +479,14 @@ 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. For example, a joint called head_pan_joint in the YAML config file can be controlled using the topic: +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: -/head_pan_joint/command +**/\/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: @@ -493,15 +498,15 @@ will move the servo to angle 1.0 radians from the neutral point; i.e. about 147 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. +**/\/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. +**/\/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. +**/\/set_speed** - Set the movement speed of servo in radians per second. $ rosservice call /head_pan_joint/set_speed 1.0 From 14aa21ac13f40a52ad2aa6fba693cd6e3181d1f9 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 14 Dec 2015 15:27:12 -0800 Subject: [PATCH 016/108] Added table of contents to README --- README.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/README.md b/README.md index f9f0412d..d792c78d 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,23 @@ +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. From 702624e336fa2bbb5e99f623b892e49d507137c8 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 14 Dec 2015 15:30:56 -0800 Subject: [PATCH 017/108] Fixed indentation for table of contents of README --- README.md | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index d792c78d..3a4c4dc1 100644 --- a/README.md +++ b/README.md @@ -1,22 +1,22 @@ 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](#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 -------- From 932282526de2f90c9edbaa517d571cf94e7b8f55 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 14 Dec 2015 18:32:17 -0800 Subject: [PATCH 018/108] Added check for base_controller before stopping robot when node exits --- ros_arduino_python/nodes/arduino_node.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 32988822..af211cbd 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -266,12 +266,13 @@ def AnalogReadHandler(self, req): return AnalogReadResponse(value) def shutdown(self): - rospy.loginfo("Shutting down Arduino Node...") + rospy.loginfo("Shutting down Arduino node...") # Stop the robot - rospy.loginfo("Stopping the robot...") - self.cmd_vel_pub.publish(Twist()) - rospy.sleep(2) + if self.use_base_controller: + rospy.loginfo("Stopping the robot...") + self.cmd_vel_pub.publish(Twist()) + rospy.sleep(2) # Detach any servos if self.have_joints: From 4fd35aa2b5aef2c1aa7d6f28c505fe0f8ef6ca92 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Dec 2015 06:45:31 -0800 Subject: [PATCH 019/108] Added check for duplicate servo pins in servo array --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 6 ++++-- .../src/libraries/ROSArduinoBridge/servos2.ino | 14 ++++++++++++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 059b4644..1464a46f 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -194,8 +194,10 @@ int runCommand() { #elif defined(USE_SERVOS2) case CONFIG_SERVO: myServos[arg1].initServo(arg1, arg2); - myServoPins[nServos] = arg1; - nServos++; + if (!haveServo(arg1)) { + myServoPins[nServos] = arg1; + nServos++; + } Serial.println("OK"); break; case SERVO_WRITE: diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino index d8cf5945..5cd0afb3 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino @@ -18,7 +18,7 @@ // Constructor SweepServo2::SweepServo2() { - this->currentPositionDegrees = 0; + this->currentPositionDegrees = 90; this->targetPositionDegrees = 90; this->lastSweepCommand = 0; } @@ -29,7 +29,7 @@ void SweepServo2::initServo( int stepDelayMs) { this->stepDelayMs = stepDelayMs; - this->currentPositionDegrees = 0; + this->currentPositionDegrees = 90; this->targetPositionDegrees = 90; this->lastSweepCommand = millis(); this->servo.attach(servoPin); @@ -74,11 +74,21 @@ void SweepServo2::setTargetPosition(int position) this->targetPositionDegrees = position; } +// Get the current servo position int SweepServo2::getCurrentPosition() { return this->currentPositionDegrees; } +// 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; +} + // Accessor for servo object Servo SweepServo2::getServo() { From 3559a4461ae78a58bd6c2937e3efb59390fb2904 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Dec 2015 06:46:24 -0800 Subject: [PATCH 020/108] Fixed sweep_demo.py loop to prevent stalling and added sweep_demo.launch file --- ros_arduino_python/launch/sweep_servo.launch | 7 ++++++ ros_arduino_python/nodes/sweep_servo.py | 23 +++++++++----------- 2 files changed, 17 insertions(+), 13 deletions(-) create mode 100755 ros_arduino_python/launch/sweep_servo.launch diff --git a/ros_arduino_python/launch/sweep_servo.launch b/ros_arduino_python/launch/sweep_servo.launch new file mode 100755 index 00000000..3ba78d0c --- /dev/null +++ b/ros_arduino_python/launch/sweep_servo.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/ros_arduino_python/nodes/sweep_servo.py b/ros_arduino_python/nodes/sweep_servo.py index f061695e..64992f6f 100755 --- a/ros_arduino_python/nodes/sweep_servo.py +++ b/ros_arduino_python/nodes/sweep_servo.py @@ -35,7 +35,7 @@ def __init__(self): self.node_name = "sweep_servo" # Initialize the node - rospy.init_node(self.node_name) + rospy.init_node(self.node_name, anonymous=True) # Set a shutdown function to clean up when termniating the node rospy.on_shutdown(self.shutdown) @@ -44,19 +44,18 @@ def __init__(self): joint_name = rospy.get_param('~joint', 'head_pan_joint') if joint_name == '': - rospy.logino("Joint name for servo must be specified in launch file.") + rospy.logino("Joint name for servo must be specified in parameter file.") os._exit(1) servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin') max_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_angle')) min_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_angle')) - # Set the sweep limits to 90% of the max limits - target_max = 0.9 * max_angle - target_min = 0.9 * min_angle + target_max = max_angle + target_min = min_angle # How fast should we sweep the servo - speed = rospy.get_param('~speed', 60) # degrees per second + speed = rospy.get_param('~speed', 1.0) # rad/s # Time between between sweeps delay = rospy.get_param('~delay', 2) # seconds @@ -93,17 +92,15 @@ def __init__(self): rospy.loginfo('Sweeping servo...') - while not rospy.is_shutdown(): - joint_pub.publish(target_max) - + 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) - joint_pub.publish(target_min) - - while abs(self.get_joint_position(joint_name) - target_min) > 0.1: + while abs(self.get_joint_position(joint_name) - target_min) > 0.1: + joint_pub.publish(target_min) rospy.sleep(0.1) rospy.sleep(delay) @@ -125,4 +122,4 @@ def shutdown(self): except: rospy.loginfo('Unexpected error: ' + str(sys.exc_info()[0])) raise - \ No newline at end of file + \ No newline at end of file From dfebe5af8b4795fa57a292c0b3fd8c61e2a4c0b3 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Dec 2015 06:47:31 -0800 Subject: [PATCH 021/108] Added have_joints variable to top of script to prevent error when exiting node --- ros_arduino_python/nodes/arduino_node.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index af211cbd..10d17016 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -56,6 +56,9 @@ def __init__(self): self.use_base_controller = rospy.get_param("~use_base_controller", False) + # 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) @@ -178,9 +181,7 @@ def __init__(self): # # for controller in self.device.controllers: # controller.startup() - else: - self.have_joints = False - + # Initialize the base controller if used if self.use_base_controller: self.myBaseController = BaseController(self.device, self.base_frame) @@ -279,7 +280,7 @@ def shutdown(self): rospy.loginfo("Detaching servos...") for joint in self.device.joints.values(): self.device.detach_servo(joint.pin) - rospy.sleep(0.1) + rospy.sleep(0.2) if __name__ == '__main__': myArduino = ArduinoROS() From dc0a18b8b0c5a1d6eeb26c5b9b6b32ce7b2b9c3c Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Dec 2015 06:48:07 -0800 Subject: [PATCH 022/108] Removed extraneous set_target_position function --- .../src/ros_arduino_python/servo_controller.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index e77325aa..3de30181 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -74,7 +74,7 @@ def __init__(self, device, name, ns="~joints"): # Where to we want the servo positioned self.desired = self.neutral + radians(rospy.get_param(n + "init_position", 0)) - + # Where is the servo positioned now self.position = 0.0 @@ -125,9 +125,6 @@ def get_step_delay(self, target_speed=1.0): def get_current_position(self): return self.device.servo_read(self.pin) - self.neutral - def set_target_position(self): - return self.device.servo_read(self.pin) - def relax_cb(self, req): self.device.detach_servo(self.pin) From 04712b660955124e3e06edadd75be3763631137d Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Dec 2015 17:45:36 -0800 Subject: [PATCH 023/108] Made sensor type case insensitive since otherwise it is a pain to remember --- ros_arduino_python/nodes/arduino_node.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 10d17016..8fb1b7f2 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -120,19 +120,19 @@ def __init__(self): except: params['direction'] = 'input' - if params['type'] == "Ping": + if params['type'].lower() == 'Ping'.lower(): sensor = Ping(self.device, name, params['pin'], params['rate'], self.base_frame) - elif params['type'] == "GP2D12": + elif params['type'].lower() == 'GP2D12'.lower(): sensor = GP2D12(self.device, name, params['pin'], params['rate'], self.base_frame) - elif params['type'] == 'Digital': + elif params['type'].lower() == 'Digital'.lower(): sensor = DigitalSensor(self.device, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) - elif params['type'] == 'Analog': + elif params['type'].lower() == 'Analog'.lower(): sensor = AnalogSensor(self.device, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) - elif params['type'] == 'PololuMotorCurrent': + elif params['type'].lower() == 'PololuMotorCurrent'.lower(): sensor = PololuMotorCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) - elif params['type'] == 'PhidgetsVoltage': + elif params['type'].lower() == 'PhidgetsVoltage'.lower(): sensor = PhidgetsVoltage(self.device, name, params['pin'], params['rate'], self.base_frame) - elif params['type'] == 'PhidgetsCurrent': + elif params['type'].lower() == 'PhidgetsCurrent'.lower(): sensor = PhidgetsCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) # if params['type'] == "MaxEZ1": From 4297345cd4eb748e02b063b8d040926cd6f2368a Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Dec 2015 17:47:47 -0800 Subject: [PATCH 024/108] Removed case-sensitive note for sensor type in example params file --- ros_arduino_python/config/arduino_params.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index 1d23d8be..a53c8522 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -31,7 +31,7 @@ base_frame: base_link #accel_limit: 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,7 +41,6 @@ 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}, From 828984dd96c8ef3be64536277021e102945a42d0 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Dec 2015 17:48:23 -0800 Subject: [PATCH 025/108] Fixed MaxEZ1 sensor type test to be case insensitive --- ros_arduino_python/nodes/arduino_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 8fb1b7f2..0cf40824 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -135,7 +135,7 @@ def __init__(self): elif params['type'].lower() == 'PhidgetsCurrent'.lower(): sensor = PhidgetsCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) -# if params['type'] == "MaxEZ1": +# 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'] From 3f98ea4df8bf9f98ecac1d6b13fac703e23fe523 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 16 Dec 2015 06:20:34 -0800 Subject: [PATCH 026/108] Changed return value for GP2D12 IR sensor from max_range to NaN when raw values are out of range --- .../src/ros_arduino_python/arduino_sensors.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 f28e313d..8ea69bbc 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -210,20 +210,21 @@ def __init__(self, *args, **kwargs): def read_value(self): value = self.controller.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 = (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 From 12890911118fdbed522a5f608c5d60f41f8bda90 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 16 Dec 2015 06:29:44 -0800 Subject: [PATCH 027/108] Updated formula for computing distance for the Sharp GP2D12 IR range sensor --- ros_arduino_python/src/ros_arduino_python/arduino_sensors.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 8ea69bbc..d535f8db 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -22,6 +22,7 @@ import rospy from sensor_msgs.msg import Range from ros_arduino_msgs.msg import * +from math import pow LOW = 0 HIGH = 1 @@ -210,12 +211,14 @@ def __init__(self, *args, **kwargs): def read_value(self): value = self.controller.analog_read(self.pin) + # The GP2D12 cannot provide a meaning result closer than 3 cm. if value <= 3.0: return float('NaN') try: - distance = (6787.0 / (float(value) - 3.0)) - 4.0 + distance = pow(4187.8 / value, 1.106) + #distance = (6787.0 / (float(value) - 3.0)) - 4.0 except: return float('NaN') From 489debcdd5dc737809be80c4da1c4dc4f0d8df99 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 17 Dec 2015 16:17:25 -0800 Subject: [PATCH 028/108] Changed min_angle to min_position and max_angle to max_position to allow for joint types other than servos --- ros_arduino_python/config/arduino_params.yaml | 4 ++-- ros_arduino_python/config/servo_example_params.yaml | 2 +- .../src/ros_arduino_python/servo_controller.py | 12 ++++++------ 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index a53c8522..eb7a5876 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -51,6 +51,6 @@ sensors: { # 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} + 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 index 18596c1b..c2504f50 100644 --- a/ros_arduino_python/config/servo_example_params.yaml +++ b/ros_arduino_python/config/servo_example_params.yaml @@ -27,5 +27,5 @@ sensors: { # Joint name and pin assignment is an example only joints: { - head_pan_joint: {pin: 3, init_position: 30, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False} + head_pan_joint: {pin: 3, init_position: 30, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False} } diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index 3de30181..526f92f2 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -64,8 +64,8 @@ def __init__(self, device, name, ns="~joints"): # Min/max/neutral values self.neutral = radians(rospy.get_param(n + "neutral", 90.0)) # degrees - self.max_angle = radians(rospy.get_param(n + "max_angle", 90.0)) # degrees - self.min_angle = radians(rospy.get_param(n + "min_angle", -90.0)) # degrees + self.max_position = radians(rospy.get_param(n + "max_position", 90.0)) # degrees + self.min_position = radians(rospy.get_param(n + "min_position", -90.0)) # degrees self.range = radians(rospy.get_param(n + "range", 180)) # degrees self.max_speed = radians(rospy.get_param(n + "max_speed", 250.0)) # deg/s @@ -88,11 +88,11 @@ def __init__(self, device, name, ns="~joints"): def command_cb(self, req): # Check limits - if req.data > self.max_angle: - req.data = self.max_angle + if req.data > self.max_position: + req.data = self.max_position - if req.data < self.min_angle: - req.data = self.min_angle + if req.data < self.min_position: + req.data = self.min_position # Adjust for the neutral offset if self.invert: From 7bcd5c461d0980e52c74bb865c93b65aaec4b713 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 17 Dec 2015 16:18:16 -0800 Subject: [PATCH 029/108] Changed min_angle to min_position and max_angle to max_position in sweep_servo.py example --- ros_arduino_python/nodes/sweep_servo.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ros_arduino_python/nodes/sweep_servo.py b/ros_arduino_python/nodes/sweep_servo.py index 64992f6f..72424f4e 100755 --- a/ros_arduino_python/nodes/sweep_servo.py +++ b/ros_arduino_python/nodes/sweep_servo.py @@ -48,11 +48,11 @@ def __init__(self): os._exit(1) servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin') - max_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_angle')) - min_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_angle')) + 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_angle - target_min = min_angle + target_max = max_position + target_min = min_position # How fast should we sweep the servo speed = rospy.get_param('~speed', 1.0) # rad/s From 3e6ab35cbf211e82a7a39efaeee29d0f3a0b65c3 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 17 Dec 2015 16:19:44 -0800 Subject: [PATCH 030/108] Replaced self.base_frame with params['frame_id'] for sensors --- ros_arduino_python/nodes/arduino_node.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 0cf40824..5b136436 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -121,13 +121,13 @@ def __init__(self): params['direction'] = 'input' if params['type'].lower() == 'Ping'.lower(): - sensor = Ping(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = Ping(self.device, name, params['pin'], params['rate'], params['frame_id']) elif params['type'].lower() == 'GP2D12'.lower(): - sensor = GP2D12(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = GP2D12(self.device, name, params['pin'], params['rate'], params['frame_id']) elif params['type'].lower() == 'Digital'.lower(): - sensor = DigitalSensor(self.device, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) + sensor = DigitalSensor(self.device, name, params['pin'], params['rate'], params['frame_id'], direction=params['direction']) elif params['type'].lower() == 'Analog'.lower(): - sensor = AnalogSensor(self.device, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) + sensor = AnalogSensor(self.device, name, params['pin'], params['rate'], params['frame_id'], direction=params['direction']) elif params['type'].lower() == 'PololuMotorCurrent'.lower(): sensor = PololuMotorCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) elif params['type'].lower() == 'PhidgetsVoltage'.lower(): From c2614fc25ecfff90c64f034c6d745b3b408a5a15 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 17 Dec 2015 16:20:52 -0800 Subject: [PATCH 031/108] Removed min/max check on servo_write since the range 0-180 does not apply to all joint types --- ros_arduino_python/src/ros_arduino_python/arduino_driver.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) 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 5e4ae2ce..4c1f9f8b 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -29,9 +29,6 @@ from serial.serialutil import SerialException from serial import Serial -SERVO_MAX = 180 -SERVO_MIN = 0 - class Arduino: ''' Configuration Parameters ''' @@ -318,7 +315,7 @@ 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))))) + return self.execute_ack('s %d %d' %(id, degrees(pos))) def servo_read(self, id): ''' Usage: servo_read(id) From 6a8f9413b9528087f8552bf3797d51c0e7778dad Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Fri, 18 Dec 2015 07:34:38 -0800 Subject: [PATCH 032/108] Added default for frame_id parameter --- ros_arduino_python/nodes/arduino_node.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 5b136436..080778ac 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -119,6 +119,12 @@ def __init__(self): params['direction'] except: params['direction'] = 'input' + + # Set the frame_id to the base frame if not set + try: + params['frame_id'] + except: + params['frame_id'] = self.base_frame if params['type'].lower() == 'Ping'.lower(): sensor = Ping(self.device, name, params['pin'], params['rate'], params['frame_id']) From 896e60d417600d21f7eca673a6e7e3a00b2b87e7 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Fri, 18 Dec 2015 14:42:59 -0800 Subject: [PATCH 033/108] Added fix to arduino_driver.py so that serial port now opens consistently. --- .../src/ros_arduino_python/arduino_driver.py | 51 +++++++++++-------- 1 file changed, 30 insertions(+), 21 deletions(-) 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 4c1f9f8b..ec1392d0 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -27,7 +27,7 @@ import time import sys, traceback from serial.serialutil import SerialException -from serial import Serial +import serial class Arduino: ''' Configuration Parameters @@ -59,12 +59,21 @@ def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): 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) + + # 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.port: + time.sleep(0.5) + + # Now open the port with the real settings + self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) + + # Test the connection by reading the baudrate test = self.get_baud() if test != self.baudrate: - time.sleep(1) + time.sleep(0.5) test = self.get_baud() if test != self.baudrate: raise SerialException @@ -82,18 +91,18 @@ def connect(self): def open(self): ''' Open the serial port. ''' - self.port.open() + self.serial_port.open() def close(self): ''' Close the serial port. ''' - self.port.close() + 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.port.write(cmd + '\r') + self.serial_port.write(cmd + '\r') def recv(self, timeout=0.5): timeout = min(timeout, self.timeout) @@ -105,7 +114,7 @@ def recv(self, timeout=0.5): value = '' attempts = 0 while c != '\r': - c = self.port.read(1) + c = self.serial_port.read(1) value += c attempts += 1 if attempts * self.interCharTimeout > timeout: @@ -148,7 +157,7 @@ def execute(self, cmd): self.mutex.acquire() try: - self.port.flushInput() + self.serial_port.flushInput() except: pass @@ -156,12 +165,12 @@ def execute(self, cmd): attempts = 0 try: - self.port.write(cmd + '\r') + self.serial_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') + self.serial_port.flushInput() + self.serial_port.write(cmd + '\r') value = self.recv(self.timeout) except: print "Exception executing command: " + cmd @@ -180,7 +189,7 @@ def execute_array(self, cmd): self.mutex.acquire() try: - self.port.flushInput() + self.serial_port.flushInput() except: pass @@ -188,12 +197,12 @@ def execute_array(self, cmd): attempts = 0 try: - self.port.write(cmd + '\r') + self.serial_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') + self.serial_port.flushInput() + self.serial_port.write(cmd + '\r') values = self.recv_array() except: print("Exception executing command: " + cmd) @@ -218,7 +227,7 @@ def execute_ack(self, cmd): self.mutex.acquire() try: - self.port.flushInput() + self.serial_port.flushInput() except: pass @@ -226,12 +235,12 @@ def execute_ack(self, cmd): attempts = 0 try: - self.port.write(cmd + '\r') + self.serial_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') + self.serial_port.flushInput() + self.serial_port.write(cmd + '\r') ack = self.recv(self.timeout) except: print "Exception executing command: " + cmd From 2de2028192d95859d35085a73387940f280714d0 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Fri, 18 Dec 2015 15:25:13 -0800 Subject: [PATCH 034/108] Added ROS services for reading and writing from/to all named sensors --- ros_arduino_msgs/CMakeLists.txt | 8 ++ ros_arduino_msgs/msg/AnalogFloat.msg | 2 +- .../srv/AnalogFloatSensorRead.srv | 2 + .../srv/AnalogFloatSensorWrite.srv | 2 + ros_arduino_msgs/srv/AnalogSensorRead.srv | 2 + ros_arduino_msgs/srv/AnalogSensorWrite.srv | 2 + ros_arduino_msgs/srv/DigitalSensorRead.srv | 2 + .../srv/DigitalSensorSetDirection.srv | 2 + ros_arduino_msgs/srv/DigitalSensorWrite.srv | 2 + ros_arduino_python/nodes/arduino_node.py | 33 ++---- .../src/ros_arduino_python/arduino_sensors.py | 101 ++++++++++++------ 11 files changed, 105 insertions(+), 53 deletions(-) create mode 100755 ros_arduino_msgs/srv/AnalogFloatSensorRead.srv create mode 100644 ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv create mode 100755 ros_arduino_msgs/srv/AnalogSensorRead.srv create mode 100644 ros_arduino_msgs/srv/AnalogSensorWrite.srv create mode 100755 ros_arduino_msgs/srv/DigitalSensorRead.srv create mode 100755 ros_arduino_msgs/srv/DigitalSensorSetDirection.srv create mode 100755 ros_arduino_msgs/srv/DigitalSensorWrite.srv diff --git a/ros_arduino_msgs/CMakeLists.txt b/ros_arduino_msgs/CMakeLists.txt index 91b7c093..0f66e15c 100644 --- a/ros_arduino_msgs/CMakeLists.txt +++ b/ros_arduino_msgs/CMakeLists.txt @@ -13,12 +13,20 @@ add_message_files(FILES add_service_files(FILES AnalogWrite.srv + AnalogSensorWrite.srv + AnalogFloatSensorWrite.srv AnalogRead.srv + AnalogSensorRead.srv + AnalogFloatSensorRead.srv DigitalRead.srv + DigitalSensorRead.srv DigitalSetDirection.srv + DigitalSensorSetDirection.srv DigitalWrite.srv + DigitalSensorWrite.srv Enable.srv Relax.srv + AnalogSensorRead.srv ServoRead.srv ServoWrite.srv SetSpeed.srv 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/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/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/DigitalSensorSetDirection.srv b/ros_arduino_msgs/srv/DigitalSensorSetDirection.srv new file mode 100755 index 00000000..2b76f950 --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSensorSetDirection.srv @@ -0,0 +1,2 @@ +bool direction +--- 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_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 080778ac..52e4dde9 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -114,32 +114,20 @@ def __init__(self): # 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' - - # Set the frame_id to the base frame if not set - try: - params['frame_id'] - except: - params['frame_id'] = self.base_frame - if params['type'].lower() == 'Ping'.lower(): - sensor = Ping(self.device, name, params['pin'], params['rate'], params['frame_id']) + sensor = Ping(self.device, name, **params) elif params['type'].lower() == 'GP2D12'.lower(): - sensor = GP2D12(self.device, name, params['pin'], params['rate'], params['frame_id']) + sensor = GP2D12(self.device, name, **params) elif params['type'].lower() == 'Digital'.lower(): - sensor = DigitalSensor(self.device, name, params['pin'], params['rate'], params['frame_id'], direction=params['direction']) + sensor = DigitalSensor(self.device, name, **params) elif params['type'].lower() == 'Analog'.lower(): - sensor = AnalogSensor(self.device, name, params['pin'], params['rate'], params['frame_id'], direction=params['direction']) + sensor = AnalogSensor(self.device, name, **params) elif params['type'].lower() == 'PololuMotorCurrent'.lower(): - sensor = PololuMotorCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = PololuMotorCurrent(self.device, name, **params) elif params['type'].lower() == 'PhidgetsVoltage'.lower(): - sensor = PhidgetsVoltage(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = PhidgetsVoltage(self.device, name, **params) elif params['type'].lower() == 'PhidgetsCurrent'.lower(): - sensor = PhidgetsCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = PhidgetsCurrent(self.device, name, **params) # if params['type'].lower() == 'MaxEZ1'.lower(): # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] @@ -197,9 +185,10 @@ def __init__(self): # 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() + if sensor.rate != 0: + mutex.acquire() + sensor.poll() + mutex.release() if self.use_base_controller: mutex.acquire() 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 d535f8db..e34e8936 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -22,6 +22,7 @@ import rospy from sensor_msgs.msg import Range from ros_arduino_msgs.msg import * +from ros_arduino_msgs.srv import * from math import pow LOW = 0 @@ -29,7 +30,7 @@ INPUT = 0 OUTPUT = 1 - + class MessageType: ANALOG = 0 DIGITAL = 1 @@ -39,19 +40,21 @@ class MessageType: BOOL = 5 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, type=None, rate=0, direction="input", frame_id="base_link", **kwargs): + self.device = device self.name = name self.pin = pin + self.type = type 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 - + if self.rate != 0: + 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: @@ -87,20 +90,33 @@ def __init__(self, *args, **kwargs): self.msg = Analog() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.pin_mode(self.pin, OUTPUT) + # Create the write service + rospy.Service('~' + self.name + '/write', AnalogSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.pin_mode(self.pin, INPUT) + # Create the read service + 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.device.analog_read(self.pin) 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): + value = self.read_value() + return AnalogSensorReadResponse(value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + return AnalogSensorWriteResponse() class AnalogFloatSensor(Sensor): def __init__(self, *args, **kwargs): @@ -111,20 +127,33 @@ def __init__(self, *args, **kwargs): self.msg = AnalogFloat() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.pin_mode(self.pin, OUTPUT) + # Create the write service + rospy.Service('~' + self.name + '/write', AnalogFloatSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.pin_mode(self.pin, INPUT) + # Create the read service + 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.device.analog_read(self.pin) 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): + value = self.read_value() + return AnalogFloatSensorReadResponse(value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + return AnalogFloatSensorWriteResponse() class DigitalSensor(Sensor): @@ -136,33 +165,49 @@ def __init__(self, *args, **kwargs): self.msg = Digital() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.pin_mode(self.pin, OUTPUT) + # Create the write service + rospy.Service('~' + self.name + '/write', DigitalSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.pin_mode(self.pin, INPUT) + # Create the read service + rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) self.value = LOW def read_value(self): - return self.controller.digital_read(self.pin) + return self.device.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_write(self.pin, self.value) + + def sensor_read_handler(self, req=None): + value = self.read_value() + return DigitalSensorReadResponse(value) + def sensor_write_handler(self, req): + self.write_value(req.value) + return DigitalSensorWriteResponse() -class RangeSensor(Sensor): + +class RangeSensor(AnalogFloatSensor): def __init__(self, *args, **kwargs): super(RangeSensor, self).__init__(*args, **kwargs) + self.direction = "input" + self.message_type = MessageType.RANGE self.msg = Range() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) def read_value(self): @@ -192,7 +237,7 @@ def __init__(self,*args, **kwargs): 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 @@ -209,7 +254,7 @@ def __init__(self, *args, **kwargs): 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. @@ -237,7 +282,7 @@ def __init__(self, *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): @@ -246,7 +291,7 @@ def __init__(self, *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): @@ -255,7 +300,7 @@ def __init__(self, *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): @@ -270,10 +315,6 @@ def __init__(self, *args, **kwargs): self.msg.max_range = 3.0 def read_value(self): - return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin) + return self.device.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 From fcb662907e3701b3070edbe1a4bcd9d4a9e0572c Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 21 Dec 2015 08:06:23 -0800 Subject: [PATCH 035/108] Fixed bug in Range sensor message type --- .../src/ros_arduino_python/arduino_sensors.py | 36 ++++++++----------- 1 file changed, 14 insertions(+), 22 deletions(-) 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 e34e8936..16ed44cc 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -40,11 +40,10 @@ class MessageType: BOOL = 5 class Sensor(object): - def __init__(self, device, name, pin=None, type=None, rate=0, direction="input", frame_id="base_link", **kwargs): + 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.type = type self.rate = rate self.direction = direction self.frame_id = frame_id @@ -54,7 +53,7 @@ def __init__(self, device, name, pin=None, type=None, rate=0, direction="input", if self.rate != 0: 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: @@ -92,7 +91,7 @@ def __init__(self, *args, **kwargs): # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) - + if self.direction == "output": self.device.pin_mode(self.pin, OUTPUT) # Create the write service @@ -109,7 +108,7 @@ def read_value(self): def write_value(self, value): return self.device.analog_write(self.pin, value) - + def sensor_read_handler(self, req=None): value = self.read_value() return AnalogSensorReadResponse(value) @@ -117,7 +116,7 @@ def sensor_read_handler(self, req=None): def sensor_write_handler(self, req): self.write_value(req.value) return AnalogSensorWriteResponse() - + class AnalogFloatSensor(Sensor): def __init__(self, *args, **kwargs): super(AnalogFloatSensor, self).__init__(*args, **kwargs) @@ -129,7 +128,7 @@ def __init__(self, *args, **kwargs): # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) - + if self.direction == "output": self.device.pin_mode(self.pin, OUTPUT) # Create the write service @@ -154,7 +153,6 @@ def sensor_read_handler(self, req=None): def sensor_write_handler(self, req): self.write_value(req.value) return AnalogFloatSensorWriteResponse() - class DigitalSensor(Sensor): def __init__(self, *args, **kwargs): @@ -164,10 +162,10 @@ def __init__(self, *args, **kwargs): self.msg = Digital() self.msg.header.frame_id = self.frame_id - - # Create the publisher + + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) - + if self.direction == "output": self.device.pin_mode(self.pin, OUTPUT) # Create the write service @@ -195,32 +193,27 @@ def sensor_write_handler(self, req): self.write_value(req.value) return DigitalSensorWriteResponse() - -class RangeSensor(AnalogFloatSensor): +class RangeSensor(Sensor): def __init__(self, *args, **kwargs): super(RangeSensor, self).__init__(*args, **kwargs) - self.direction = "input" - self.message_type = MessageType.RANGE self.msg = Range() self.msg.header.frame_id = self.frame_id - - # Create the publisher + + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) def read_value(self): self.msg.header.stamp = rospy.Time.now() - class SonarSensor(RangeSensor): def __init__(self, *args, **kwargs): super(SonarSensor, self).__init__(*args, **kwargs) self.msg.radiation_type = Range.ULTRASOUND - class IRSensor(RangeSensor): def __init__(self, *args, **kwargs): super(IRSensor, self).__init__(*args, **kwargs) @@ -243,7 +236,6 @@ def read_value(self): distance = cm / 100.0 return distance - class GP2D12(IRSensor): def __init__(self, *args, **kwargs): @@ -256,7 +248,6 @@ def __init__(self, *args, **kwargs): def read_value(self): value = self.device.analog_read(self.pin) - # The GP2D12 cannot provide a meaning result closer than 3 cm. if value <= 3.0: return float('NaN') @@ -317,4 +308,5 @@ def __init__(self, *args, **kwargs): def read_value(self): return self.device.get_MaxEZ1(self.trigger_pin, self.output_pin) - \ No newline at end of file + + From 2c71342827538a1fd9d10970123cd585cf709002 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 9 Jan 2016 08:24:18 -0800 Subject: [PATCH 036/108] Added pin mode services --- ...talSetDirection.srv => DigitalPinMode.srv} | 0 ...Direction.srv => DigitalSensorPinMode.srv} | 0 .../{nodes => examples}/sweep_servo.py | 0 ros_arduino_python/nodes/arduino_node.py | 74 ++++++++++++++----- 4 files changed, 56 insertions(+), 18 deletions(-) rename ros_arduino_msgs/srv/{DigitalSetDirection.srv => DigitalPinMode.srv} (100%) rename ros_arduino_msgs/srv/{DigitalSensorSetDirection.srv => DigitalSensorPinMode.srv} (100%) rename ros_arduino_python/{nodes => examples}/sweep_servo.py (100%) diff --git a/ros_arduino_msgs/srv/DigitalSetDirection.srv b/ros_arduino_msgs/srv/DigitalPinMode.srv similarity index 100% rename from ros_arduino_msgs/srv/DigitalSetDirection.srv rename to ros_arduino_msgs/srv/DigitalPinMode.srv diff --git a/ros_arduino_msgs/srv/DigitalSensorSetDirection.srv b/ros_arduino_msgs/srv/DigitalSensorPinMode.srv similarity index 100% rename from ros_arduino_msgs/srv/DigitalSensorSetDirection.srv rename to ros_arduino_msgs/srv/DigitalSensorPinMode.srv diff --git a/ros_arduino_python/nodes/sweep_servo.py b/ros_arduino_python/examples/sweep_servo.py similarity index 100% rename from ros_arduino_python/nodes/sweep_servo.py rename to ros_arduino_python/examples/sweep_servo.py diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 52e4dde9..8cb3c6ef 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -74,6 +74,12 @@ def __init__(self): # 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) @@ -81,8 +87,14 @@ def __init__(self): 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) @@ -134,7 +146,15 @@ def __init__(self): # 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) + + 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") + # Initialize any joints (servos) self.device.joints = dict() @@ -180,7 +200,7 @@ def __init__(self): if self.use_base_controller: self.myBaseController = BaseController(self.device, self.base_frame) - print "==> ROS Arduino Bridge ready for action!" + print "\n==> ROS Arduino Bridge ready for action!" # Start polling the sensors, base controller, and servo controller while not rospy.is_shutdown(): @@ -188,7 +208,7 @@ def __init__(self): if sensor.rate != 0: mutex.acquire() sensor.poll() - mutex.release() + mutex.release() if self.use_base_controller: mutex.acquire() @@ -221,28 +241,42 @@ def __init__(self): 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.device.servo_write(req.id, req.value) + self.device.servo_write(req.id, req.position) return ServoWriteResponse() - def SetServoSpeedWriteHandler(self, req): - index = self.joint.values['pin'].index(req.pin) - name = self.device.joints.keys[index] - - # 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 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.device.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.device.pin_mode(req.pin, req.direction) + self.device.digital_pin_mode(req.pin, req.direction) return DigitalSetDirectionResponse() def DigitalWriteHandler(self, req): @@ -252,6 +286,10 @@ def DigitalWriteHandler(self, req): def DigitalReadHandler(self, req): 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.device.analog_write(req.pin, req.value) From fb24de4223e9af558a3fd6c79217b54486b3efa6 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 9 Jan 2016 08:25:36 -0800 Subject: [PATCH 037/108] Reverted units back to degrees for raw servo commands --- .../src/ros_arduino_python/arduino_driver.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) 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 ec1392d0..8752feff 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -65,7 +65,7 @@ def connect(self): # Needed for Leonardo only while not self.port: - time.sleep(0.5) + time.sleep(self.timeout) # Now open the port with the real settings self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) @@ -73,7 +73,7 @@ def connect(self): # Test the connection by reading the baudrate test = self.get_baud() if test != self.baudrate: - time.sleep(0.5) + time.sleep(self.timeout) test = self.get_baud() if test != self.baudrate: raise SerialException @@ -300,6 +300,9 @@ 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): return self.execute('a %d' %pin) @@ -313,24 +316,24 @@ def digital_read(self, pin): def digital_write(self, pin, value): return self.execute_ack('w %d %d' %(pin, value)) - def pin_mode(self, pin, mode): + 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 %d' %(pin, step_delay)) + 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 radians and converted to degrees before sending + Position is given in degrees from 0-180 ''' - return self.execute_ack('s %d %d' %(id, degrees(pos))) + return self.execute_ack('s %d %d' %(id, pos)) def servo_read(self, id): ''' Usage: servo_read(id) - The returned position is converted from degrees to radians + The returned position is in degrees ''' - return radians(self.execute('t %d' %id)) + return int(self.execute('t %d' %id)) def set_servo_delay(self, id, delay): ''' Usage: set_servo_delay(id, delay) From fc3a4509fe03ca315af6b2cffc046632fca00c96 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 9 Jan 2016 08:26:30 -0800 Subject: [PATCH 038/108] Conditionalized sensor publisher on rate != 0 --- .../src/ros_arduino_python/arduino_sensors.py | 71 ++++++++++++------- 1 file changed, 45 insertions(+), 26 deletions(-) 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 16ed44cc..e6b7276e 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -90,14 +90,15 @@ def __init__(self, *args, **kwargs): self.msg.header.frame_id = self.frame_id # Create the publisher - self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) + if self.rate != 0: + self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) if self.direction == "output": - self.device.pin_mode(self.pin, OUTPUT) + self.device.analog_pin_mode(self.pin, OUTPUT) # Create the write service rospy.Service('~' + self.name + '/write', AnalogSensorWrite, self.sensor_write_handler) else: - self.device.pin_mode(self.pin, INPUT) + self.device.analog_pin_mode(self.pin, INPUT) # Create the read service rospy.Service('~' + self.name + '/read', AnalogSensorRead, self.sensor_read_handler) @@ -110,11 +111,12 @@ def write_value(self, value): return self.device.analog_write(self.pin, value) def sensor_read_handler(self, req=None): - value = self.read_value() - return AnalogSensorReadResponse(value) + 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(Sensor): @@ -127,14 +129,15 @@ def __init__(self, *args, **kwargs): self.msg.header.frame_id = self.frame_id # Create the publisher - self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) + if self.rate != 0: + self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) if self.direction == "output": - self.device.pin_mode(self.pin, OUTPUT) + self.device.analog_pin_mode(self.pin, OUTPUT) # Create the write service rospy.Service('~' + self.name + '/write', AnalogFloatSensorWrite, self.sensor_write_handler) else: - self.device.pin_mode(self.pin, INPUT) + self.device.analog_pin_mode(self.pin, INPUT) # Create the read service rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler) @@ -147,11 +150,12 @@ def write_value(self, value): return self.device.analog_write(self.pin, value) def sensor_read_handler(self, req=None): - value = self.read_value() - return AnalogFloatSensorReadResponse(value) + 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): @@ -163,34 +167,44 @@ def __init__(self, *args, **kwargs): self.msg = Digital() self.msg.header.frame_id = self.frame_id - # Create the publisher - self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) + # Create the publisher + if self.rate != 0: + self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) + + # Get the initial state + self.value = self.read_value() if self.direction == "output": - self.device.pin_mode(self.pin, OUTPUT) + self.device.digital_pin_mode(self.pin, OUTPUT) + # Create the write service rospy.Service('~' + self.name + '/write', DigitalSensorWrite, self.sensor_write_handler) else: - self.device.pin_mode(self.pin, INPUT) - # Create the read service - rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) + self.device.digital_pin_mode(self.pin, INPUT) + + # Create the read service + rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) - self.value = LOW def read_value(self): return self.device.digital_read(self.pin) - def write_value(self): - # Alternate HIGH/LOW when writing at a fixed rate - self.value = not self.value + 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): - value = self.read_value() - return DigitalSensorReadResponse(value) + 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): @@ -202,11 +216,16 @@ def __init__(self, *args, **kwargs): self.msg = Range() self.msg.header.frame_id = self.frame_id - # Create the publisher - self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) + # Create the publisher + if self.rate != 0: + self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) - def read_value(self): - self.msg.header.stamp = rospy.Time.now() + # Create the read service + rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler) + + def sensor_read_handler(self, req=None): + self.value = self.read_value() + return AnalogFloatSensorReadResponse(self.value) class SonarSensor(RangeSensor): def __init__(self, *args, **kwargs): From b8b51d4714af922a32c02d97a95d4726aae55b5b Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 9 Jan 2016 09:31:11 -0800 Subject: [PATCH 039/108] Increased maxium delay value for setting servo speed --- .../ros_arduino_python/servo_controller.py | 41 ++++++++++--------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index 526f92f2..3ba4cc7b 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -43,37 +43,40 @@ def __init__(self, device, name, ns="~joints"): Joint.__init__(self, device, name) # Construct the namespace for the joint - n = ns + "/" + name + "/" + namespace = ns + "/" + name + "/" # The Arduino pin used by this servo - self.pin = int(rospy.get_param(n + "pin")) + 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(n + "rated_speed", 0.24) # seconds per 60 degrees + self.rated_speed = rospy.get_param(namespace + "rated_speed", 0.24) # seconds per 60 degrees # Conversion factors to compute servo speed to step delay between updates self.max_rad_per_sec = radians(60.0) / self.rated_speed self.ms_per_rad = 1000.0 / self.max_rad_per_sec # Convert initial servo speed in deg/s to a step delay in milliseconds - step_delay = self.get_step_delay(radians(rospy.get_param(n + "init_speed", 90.0))) + step_delay = self.get_step_delay(radians(rospy.get_param(namespace + "init_speed", 60.0))) + + # Enable the servo + self.device.attach_servo(self.pin) - # Update the servo speed + # Set the servo speed self.device.config_servo(self.pin, step_delay) # Min/max/neutral values - self.neutral = radians(rospy.get_param(n + "neutral", 90.0)) # degrees - self.max_position = radians(rospy.get_param(n + "max_position", 90.0)) # degrees - self.min_position = radians(rospy.get_param(n + "min_position", -90.0)) # degrees - self.range = radians(rospy.get_param(n + "range", 180)) # degrees - self.max_speed = radians(rospy.get_param(n + "max_speed", 250.0)) # deg/s + self.neutral = radians(rospy.get_param(namespace + "neutral", 90.0)) # degrees + self.max_position = radians(rospy.get_param(namespace + "max_position", 90.0)) # degrees + self.min_position = radians(rospy.get_param(namespace + "min_position", -90.0)) # degrees + self.range = radians(rospy.get_param(namespace + "range", 180)) # degrees + self.max_speed = radians(rospy.get_param(namespace + "max_speed", 250.0)) # deg/s # Do we want to reverse positive motion - self.invert = rospy.get_param(n + "invert", False) + self.invert = rospy.get_param(namespace + "invert", False) - # Where to we want the servo positioned - self.desired = self.neutral + radians(rospy.get_param(n + "init_position", 0)) + # The desired position of the servo + self.desired = self.neutral + radians(rospy.get_param(namespace + "init_position", 0)) # Where is the servo positioned now self.position = 0.0 @@ -115,15 +118,15 @@ def get_step_delay(self, target_speed=1.0): try: step_delay = 1000.0 / degrees(1.0) * (1.0 / target_speed - 1.0 / self.max_rad_per_sec) except: - step_delay = 32767 - + step_delay = 4294967295 # 2^32 - 1 + # Minimum step delay is 0 millisecond step_delay = max(0, step_delay) return int(step_delay) def get_current_position(self): - return self.device.servo_read(self.pin) - self.neutral + return radians(self.device.servo_read(self.pin)) - self.neutral def relax_cb(self, req): self.device.detach_servo(self.pin) @@ -140,7 +143,7 @@ def enable_cb(self, req): 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.value) + step_delay = self.get_step_delay(req.speed) # Update the servo speed self.device.set_servo_delay(self.pin, step_delay) @@ -180,7 +183,5 @@ def poll(self): if rospy.Time.now() > self.w_next: for servo in self.servos: - self.device.servo_write(servo.pin, servo.desired) + self.device.servo_write(servo.pin, degrees(servo.desired)) self.w_next = rospy.Time.now() + self.w_delta - - \ No newline at end of file From 5933239b7012f6897a29880cbbf0e95dd6c5f187 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 27 Jan 2016 06:25:41 -0800 Subject: [PATCH 040/108] Added support for RoboGaia 3-axis encoder shield --- README.md | 8 +- .../MegaRobogaiaPololu/MegaRobogaiaPololu.ino | 341 ------------------ .../libraries/MegaRobogaiaPololu/commands.h | 22 -- .../MegaRobogaiaPololu/diff_controller.h | 71 ---- .../libraries/MegaRobogaiaPololu/sensors.h | 34 -- .../src/libraries/MegaRobogaiaPololu/servos.h | 16 - .../ROSArduinoBridge/ROSArduinoBridge.ino | 59 +-- .../ROSArduinoBridge/encoder_driver.h | 15 +- .../ROSArduinoBridge/encoder_driver.ino | 174 ++++++++- .../src/libraries/ROSArduinoBridge/servos.h | 1 + .../src/libraries/ROSArduinoBridge/servos.ino | 1 + .../src/libraries/ROSArduinoBridge/servos2.h | 15 +- .../libraries/ROSArduinoBridge/servos2.ino | 33 +- ros_arduino_msgs/CMakeLists.txt | 8 +- ros_arduino_msgs/srv/AnalogPinMode.srv | 3 + ros_arduino_msgs/srv/DigitalSetDirection.srv | 3 + ros_arduino_msgs/srv/ServoAttach.srv | 3 + ros_arduino_msgs/srv/ServoDetach.srv | 3 + ros_arduino_msgs/srv/ServoRead.srv | 2 +- ros_arduino_msgs/srv/ServoWrite.srv | 2 +- ros_arduino_msgs/srv/SetServoSpeed.srv | 2 +- ros_arduino_msgs/srv/SetSpeed.srv | 2 +- ros_arduino_python/examples/sweep_servo.py | 18 +- ros_arduino_python/launch/sweep_servo.launch | 4 +- 24 files changed, 287 insertions(+), 553 deletions(-) delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h delete mode 100644 ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h create mode 100755 ros_arduino_msgs/srv/AnalogPinMode.srv create mode 100755 ros_arduino_msgs/srv/DigitalSetDirection.srv create mode 100755 ros_arduino_msgs/srv/ServoAttach.srv create mode 100755 ros_arduino_msgs/srv/ServoDetach.srv diff --git a/README.md b/README.md index 3a4c4dc1..1c2dabdb 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ 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: @@ -43,7 +43,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. @@ -91,7 +93,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 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 1464a46f..dbe1db4c 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -50,13 +50,22 @@ /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE /* The Pololu VNH5019 dual motor driver shield */ - #define POLOLU_VNH5019 + //#define POLOLU_VNH5019 /* The Pololu MC33926 dual motor driver shield */ //#define POLOLU_MC33926 + /* The Ardunino Motor Shield R3 */ + //#define ARDUINO_MOTOR_SHIELD_R3 + + /* For testing only */ + //#define NO_MOTOR_CONTROLLER + /* The RoboGaia encoder shield */ - #define ROBOGAIA + //#define ROBOGAIA + + /* The RoboGaia 3-axis encoder shield */ + //#define ROBOGAIA_3_AXIS /* Encoders directly attached to Arduino board */ //#define ARDUINO_ENC_COUNTER @@ -94,6 +103,11 @@ /* 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" @@ -193,15 +207,18 @@ int runCommand() { break; #elif defined(USE_SERVOS2) case CONFIG_SERVO: - myServos[arg1].initServo(arg1, arg2); if (!haveServo(arg1)) { + myServos[arg1].initServo(arg1, arg2); myServoPins[nServos] = arg1; + myServos[arg1].enable(); nServos++; } Serial.println("OK"); break; case SERVO_WRITE: - myServos[arg1].setTargetPosition(arg2); + if (myServos[arg1].isEnabled()) { + myServos[arg1].setTargetPosition(arg2); + } Serial.println("OK"); break; case SERVO_READ: @@ -213,10 +230,19 @@ int runCommand() { break; case DETACH_SERVO: myServos[arg1].getServo().detach(); + myServos[arg1].disable(); Serial.println("OK"); break; case ATTACH_SERVO: - myServos[arg1].getServo().attach(arg1); + if (!haveServo(arg1)) { + myServos[arg1].initServo(arg1, 0); + myServoPins[nServos] = arg1; + nServos++; + } + else { + myServos[arg1].getServo().attach(arg1); + } + myServos[arg1].enable(); Serial.println("OK"); break; @@ -270,27 +296,8 @@ void setup() { // Initialize the motor controller if used */ #ifdef USE_BASE - #ifdef ARDUINO_ENC_COUNTER - //set as inputs - DDRD &= ~(1<stepDelayMs = stepDelayMs; this->currentPositionDegrees = 90; @@ -38,7 +38,7 @@ void SweepServo2::initServo( // Init void SweepServo2::setServoDelay( int servoPin, - int stepDelayMs) + unsigned long stepDelayMs) { this->stepDelayMs = stepDelayMs; } @@ -48,7 +48,7 @@ void SweepServo2::moveServo() { // Get ellapsed time - int delta = millis() - this->lastSweepCommand; + unsigned long delta = millis() - this->lastSweepCommand; // Check if time for a step if (delta > this->stepDelayMs) { @@ -80,13 +80,16 @@ int SweepServo2::getCurrentPosition() return this->currentPositionDegrees; } -// 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; +void SweepServo2::enable() { + this->enabled = true; +} + +void SweepServo2::disable() { + this->enabled = false; +} + +bool SweepServo2::isEnabled() { + return this->enabled; } // Accessor for servo object @@ -96,4 +99,14 @@ Servo SweepServo2::getServo() } +// 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 0f66e15c..a2f82115 100644 --- a/ros_arduino_msgs/CMakeLists.txt +++ b/ros_arduino_msgs/CMakeLists.txt @@ -14,19 +14,23 @@ add_message_files(FILES add_service_files(FILES AnalogWrite.srv AnalogSensorWrite.srv - AnalogFloatSensorWrite.srv + AnalogFloatSensorWrite.srv + AnalogPinMode.srv AnalogRead.srv AnalogSensorRead.srv AnalogFloatSensorRead.srv + DigitalPinMode.srv DigitalRead.srv DigitalSensorRead.srv DigitalSetDirection.srv - DigitalSensorSetDirection.srv + DigitalSensorPinMode.srv DigitalWrite.srv DigitalSensorWrite.srv Enable.srv Relax.srv AnalogSensorRead.srv + ServoAttach.srv + ServoDetach.srv ServoRead.srv ServoWrite.srv SetSpeed.srv 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/DigitalSetDirection.srv b/ros_arduino_msgs/srv/DigitalSetDirection.srv new file mode 100755 index 00000000..01a2e12c --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSetDirection.srv @@ -0,0 +1,3 @@ +uint8 pin +bool direction +--- 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 index 5a9170ba..667284e6 100755 --- a/ros_arduino_msgs/srv/SetServoSpeed.srv +++ b/ros_arduino_msgs/srv/SetServoSpeed.srv @@ -1,3 +1,3 @@ uint8 id -float32 value +float32 speed --- diff --git a/ros_arduino_msgs/srv/SetSpeed.srv b/ros_arduino_msgs/srv/SetSpeed.srv index effb1670..ef7ce7dd 100755 --- a/ros_arduino_msgs/srv/SetSpeed.srv +++ b/ros_arduino_msgs/srv/SetSpeed.srv @@ -1,2 +1,2 @@ -float32 value +float32 speed --- diff --git a/ros_arduino_python/examples/sweep_servo.py b/ros_arduino_python/examples/sweep_servo.py index 72424f4e..eb75df46 100755 --- a/ros_arduino_python/examples/sweep_servo.py +++ b/ros_arduino_python/examples/sweep_servo.py @@ -35,7 +35,7 @@ def __init__(self): self.node_name = "sweep_servo" # Initialize the node - rospy.init_node(self.node_name, anonymous=True) + rospy.init_node(self.node_name) # Set a shutdown function to clean up when termniating the node rospy.on_shutdown(self.shutdown) @@ -43,11 +43,10 @@ def __init__(self): # Name of the joint we want to control joint_name = rospy.get_param('~joint', 'head_pan_joint') - if joint_name == '': + if joint_name is None or joint_name == '': rospy.logino("Joint name for servo must be specified in parameter file.") os._exit(1) - servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin') 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')) @@ -55,11 +54,11 @@ def __init__(self): target_min = min_position # How fast should we sweep the servo - speed = rospy.get_param('~speed', 1.0) # rad/s - - # Time between between sweeps - delay = rospy.get_param('~delay', 2) # seconds + 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) @@ -88,7 +87,7 @@ def __init__(self): set_speed = rospy.ServiceProxy('/' + joint_name + '/set_speed', SetSpeed) # Set the initial servo speed - set_speed(speed) + set_speed(servo_speed) rospy.loginfo('Sweeping servo...') @@ -121,5 +120,4 @@ def shutdown(self): SweepServo() except: rospy.loginfo('Unexpected error: ' + str(sys.exc_info()[0])) - raise - \ No newline at end of file + raise \ No newline at end of file diff --git a/ros_arduino_python/launch/sweep_servo.launch b/ros_arduino_python/launch/sweep_servo.launch index 3ba78d0c..adcab6f8 100755 --- a/ros_arduino_python/launch/sweep_servo.launch +++ b/ros_arduino_python/launch/sweep_servo.launch @@ -1,7 +1,9 @@ - + + + From f7c01d41c5e4f1aec86e4e872470031a059c8bb9 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 27 Jan 2016 06:26:10 -0800 Subject: [PATCH 041/108] Added support for Arduino Motor Shield R3 --- .../libraries/ROSArduinoBridge/motor_driver.h | 1 + .../ROSArduinoBridge/motor_driver.ino | 62 ++++++++++++++++++- 2 files changed, 62 insertions(+), 1 deletion(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h index 0e3248cf..2c91e255 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h @@ -5,3 +5,4 @@ void initMotorController(); void setMotorSpeed(int i, int spd); void setMotorSpeeds(int leftSpeed, int rightSpeed); + diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino index 27c76bcc..e59606fd 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino @@ -55,8 +55,68 @@ setMotorSpeed(LEFT, leftSpeed); setMotorSpeed(RIGHT, rightSpeed); } +#elif defined ARDUINO_MOTOR_SHIELD_R3 + /* Include the pin definitions for the shield */ + #include "arduino_motor_shield_r3.h" + + /* Wrap the motor driver initialization */ + void initMotorController() { + // Set up Channel A - LEFT + pinMode(LEFT_MOTOR_PIN_DIR, OUTPUT); // Initiates Motor Channel A pin + pinMode(LEFT_MOTOR_PIN_BRAKE, OUTPUT); // Initiates Brake Channel A pin + + // Set up Channel B - RIGHT + pinMode(RIGHT_MOTOR_PIN_DIR, OUTPUT); // Initiates Motor Channel B pin + pinMode(RIGHT_MOTOR_PIN_BRAKE, OUTPUT); // Initiates Brake 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); + + if (spd == 0) digitalWrite(LEFT_MOTOR_PIN_BRAKE, HIGH); + else digitalWrite(LEFT_MOTOR_PIN_BRAKE, LOW); + + analogWrite(LEFT_MOTOR_PIN_SPEED, spd); + } + else { + if (spd < 0) digitalWrite(RIGHT_MOTOR_PIN_DIR, LOW); + else digitalWrite(RIGHT_MOTOR_PIN_DIR, HIGH); + + if (spd == 0) digitalWrite(RIGHT_MOTOR_PIN_BRAKE, HIGH); + else digitalWrite(RIGHT_MOTOR_PIN_BRAKE, LOW); + + 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 + From bf88d3fa0319faf5fb84810cdf0b791a190fb448 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Mar 2016 07:46:42 -0700 Subject: [PATCH 042/108] Fixed indentation in encoder_driver.h --- .../src/libraries/ROSArduinoBridge/encoder_driver.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h index 45e8abad..93e0d848 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h @@ -19,8 +19,8 @@ #define chipSelectPin2 9 #define chipSelectPin3 8 -#define CNTR B00100000 -#define CLR B00000000 + #define CNTR B00100000 + #define CLR B00000000 #endif void initEncoders(); From 453eae91b5a3d4be5a8d29fb159b1957061d8844 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Mar 2016 07:47:23 -0700 Subject: [PATCH 043/108] Added blank line for readability in encoder_driver.ino --- .../src/libraries/ROSArduinoBridge/encoder_driver.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino index df4fb55e..86eb6706 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino @@ -32,6 +32,7 @@ if (i == LEFT) return encoders.YAxisReset(); else return encoders.XAxisReset(); } + #elif defined(ROBOGAIA_3_AXIS) /* Robogaia 3-axis encoder shield */ From 038e0cf9b43eb741c64853ee409e4a56036147ae Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 15 Mar 2016 07:49:35 -0700 Subject: [PATCH 044/108] Added support for Adafruit Motor Shield V2 and Arduino Motor Shield V3 --- .../libraries/ROSArduinoBridge/motor_driver.h | 35 +++++++++++ .../ROSArduinoBridge/motor_driver.ino | 61 +++++++++++++++---- 2 files changed, 85 insertions(+), 11 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h index 2c91e255..b07d20ca 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h @@ -6,3 +6,38 @@ 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 e59606fd..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,19 +56,53 @@ 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 - /* Include the pin definitions for the shield */ - #include "arduino_motor_shield_r3.h" - /* 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 - pinMode(LEFT_MOTOR_PIN_BRAKE, OUTPUT); // Initiates Brake Channel A pin + 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 - pinMode(RIGHT_MOTOR_PIN_BRAKE, OUTPUT); // Initiates Brake Channel B pin } /* Wrap the drive motor set speed function */ @@ -76,17 +111,21 @@ if (spd < 0) digitalWrite(LEFT_MOTOR_PIN_DIR, LOW); else digitalWrite(LEFT_MOTOR_PIN_DIR, HIGH); - if (spd == 0) digitalWrite(LEFT_MOTOR_PIN_BRAKE, HIGH); - else digitalWrite(LEFT_MOTOR_PIN_BRAKE, LOW); + #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); - - if (spd == 0) digitalWrite(RIGHT_MOTOR_PIN_BRAKE, HIGH); - else digitalWrite(RIGHT_MOTOR_PIN_BRAKE, LOW); + + #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); } From 63a329600798d81b880a81b5b1d8564b4cc4529d Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 19 Mar 2016 19:34:34 -0700 Subject: [PATCH 045/108] Added error message for unsuccessful connection --- ros_arduino_python/src/ros_arduino_python/arduino_driver.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 8752feff..f951662c 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -264,7 +264,11 @@ def update_pid(self, Kp, Kd, Ki, Ko): def get_baud(self): ''' Get the current baud rate on the serial port. ''' - return int(self.execute('b')); + try: + return int(self.execute('b')) + except: + print "Failed to determine baud rate of Arduino so aborting!" + os._exit(1) def get_encoder_counts(self): values = self.execute_array('e') From cc5fa475b368df1162b2f44280ed43708abd4e58 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 20 Mar 2016 18:31:00 -0700 Subject: [PATCH 046/108] Commented out follow_controller code since it is not yet ready --- ros_arduino_python/nodes/arduino_node.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 8cb3c6ef..32d02071 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -25,14 +25,14 @@ from ros_arduino_msgs.srv import * 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.follow_controller import FollowController from ros_arduino_python.joint_state_publisher import JointStatePublisher from geometry_msgs.msg import Twist import os, time import thread from math import radians -controller_types = { "follow_controller" : FollowController } +#controller_types = { "follow_controller" : FollowController } class ArduinoROS(): def __init__(self): @@ -167,7 +167,13 @@ def __init__(self): # Configure each servo for name, params in joint_params.iteritems(): - self.device.joints[name] = Servo(self.device, name) + 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)) From 1bb6bb8f523af64f746ea39d7baebd496932c3fa Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 27 Mar 2016 07:14:12 -0700 Subject: [PATCH 047/108] Added odom_linear_scale_correction and odom_angular_scale_correction parameters to base_controller --- .../src/ros_arduino_python/base_controller.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 9c8ec216..2ae923d6 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -36,6 +36,8 @@ def __init__(self, arduino, base_frame): 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.stopped = False pid_params = dict() @@ -142,8 +144,8 @@ 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) / self.wheel_track vxy = dxy_ave / dt vth = dth / dt From 6441cb5abef8811d7cd68069e1f943b866decac0 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 27 Mar 2016 19:35:10 -0700 Subject: [PATCH 048/108] Added default covariance values for odometry message as well as use_imu_heading parameter --- .../src/ros_arduino_python/base_controller.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) 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 2ae923d6..599d3fff 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -28,6 +28,7 @@ from geometry_msgs.msg import Quaternion, Twist, Pose from nav_msgs.msg import Odometry from tf.broadcaster import TransformBroadcaster +from sys import float_info """ Class to receive Twist commands and publish Odometry data """ class BaseController: @@ -38,6 +39,7 @@ def __init__(self, arduino, base_frame): 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.stopped = False pid_params = dict() @@ -184,6 +186,22 @@ def poll(self): odom.twist.twist.linear.x = vxy odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth + + """ + Covariance values taken from Kobuki node odometry.hpp 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 + odom.pose.covariance[35] = 0.05 if self.use_imu_heading else 0.2 + + odom.pose.covariance[14] = float_info.max # set a non-zero covariance on unused + odom.pose.covariance[21] = float_info.max # dimensions (z, pitch and roll); this + odom.pose.covariance[28] = float_info.max # is a requirement of robot_pose_ekf self.odomPub.publish(odom) From afad6f10d937ea0c7261f12639c9c58b43421369 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 27 Mar 2016 20:14:34 -0700 Subject: [PATCH 049/108] Added reset_odometry service to base controller --- ros_arduino_python/nodes/arduino_node.py | 11 +++++++++++ .../src/ros_arduino_python/base_controller.py | 11 +++++------ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 32d02071..4a479ca6 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -28,6 +28,7 @@ #from ros_arduino_python.follow_controller import FollowController from ros_arduino_python.joint_state_publisher import JointStatePublisher from geometry_msgs.msg import Twist +from std_srvs.srv import Empty, EmptyResponse import os, time import thread from math import radians @@ -107,6 +108,9 @@ def __init__(self): # A service to read the value of an analog sensor rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler) + # A service to reset the odometry values to 0 + rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler) + # Initialize the device self.device = Arduino(self.port, self.baud, self.timeout) @@ -304,6 +308,13 @@ def AnalogWriteHandler(self, req): def AnalogReadHandler(self, req): value = self.device.analog_read(req.pin) return AnalogReadResponse(value) + + def ResetOdometryHandler(self, req): + if self.use_base_controller: + self.myBaseController.reset_odometry() + else: + rospy.logwarn("Not using base controller!") + return EmptyResponse() def shutdown(self): rospy.loginfo("Shutting down Arduino node...") 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 599d3fff..a078dbac 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -232,7 +232,7 @@ def poll(self): 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) @@ -259,9 +259,8 @@ def cmdVelCallback(self, req): 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 - - - - From 485353340091543b5eecfa42ef1a0085e692e6db Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 2 Apr 2016 18:53:20 -0700 Subject: [PATCH 050/108] Added heart beat test for the serial connection which allows unplugging and plugging back in the serial line --- ros_arduino_python/nodes/arduino_node.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 4a479ca6..8fb54074 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -214,6 +214,20 @@ def __init__(self): # Start polling the sensors, base controller, and servo controller while not rospy.is_shutdown(): + # Heartbeat test for the serial connection + try: + self.device.serial_port.inWaiting() + except IOError: + rospy.loginfo("Lost serial connection...") + self.device.close() + while True: + try: + self.device.open() + rospy.loginfo("Serial connection re-established.") + break + except: + r.sleep() + for sensor in self.mySensors: if sensor.rate != 0: mutex.acquire() From 77a5a9a4d5c43a9a516aa3e2fc40215d042681b7 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 2 Apr 2016 19:50:18 -0700 Subject: [PATCH 051/108] Updated check for missing PID parameters --- ros_arduino_python/src/ros_arduino_python/base_controller.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 a078dbac..aed4949b 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -39,7 +39,6 @@ def __init__(self, arduino, base_frame): 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.stopped = False pid_params = dict() @@ -101,7 +100,7 @@ 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 @@ -130,7 +129,7 @@ def poll(self): 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() From 6adb1bde269aafde3489e47ba55e3a0d5ff5526b Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 4 Apr 2016 08:45:51 -0700 Subject: [PATCH 052/108] Added preliminary support for Adafruit 9-DOF IMU to firmware but not yet to ROS nodes --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 88 +++++++++++++++---- .../src/libraries/ROSArduinoBridge/commands.h | 1 + .../src/libraries/ROSArduinoBridge/imu.h | 26 ++++++ .../src/libraries/ROSArduinoBridge/imu.ino | 68 ++++++++++++++ 4 files changed, 165 insertions(+), 18 deletions(-) create mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h create mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index dbe1db4c..046cf1e4 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -45,30 +45,41 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -//#define USE_BASE // Enable/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 MC33926 dual motor driver shield */ - //#define POLOLU_MC33926 + /* The Pololu VNH5019 dual motor driver shield */ + //#define POLOLU_VNH5019 - /* The Ardunino Motor Shield R3 */ - //#define ARDUINO_MOTOR_SHIELD_R3 + /* The Pololu MC33926 dual motor driver shield */ + //#define POLOLU_MC33926 - /* For testing only */ - //#define NO_MOTOR_CONTROLLER + /* The Adafruit motor shield V2 */ + #define ADAFRUIT_MOTOR_SHIELD_V2 - /* 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 + /* 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/disable use of old PWM servo support as defined in servos.h @@ -129,6 +140,13 @@ long lastMotorCommand = AUTO_STOP_INTERVAL; #endif +#ifdef USE_IMU + #include "imu.h" + + #define ADAFRUIT_9DOF + +#endif + /* Variable initialization */ #define BAUDRATE 57600 @@ -169,9 +187,11 @@ int runCommand() { int pid_args[4]; arg1 = atoi(argv1); arg2 = atoi(argv2); + String output; switch(cmd) { case GET_BAUDRATE: + Serial.print(" "); Serial.println(BAUDRATE); break; case ANALOG_READ: @@ -197,6 +217,34 @@ int runCommand() { case PING: Serial.println(Ping(arg1)); break; +#ifdef USE_IMU + case READ_IMU: + imu_data = readIMU(); + Serial.print(imu_data.ax); + Serial.print(" "); + Serial.print(imu_data.ay); + Serial.print(" "); + Serial.print(imu_data.az); + Serial.print(" "); + Serial.print(imu_data.gx); + Serial.print(" "); + Serial.print(imu_data.gy); + Serial.print(" "); + Serial.print(imu_data.gz); + Serial.print(" "); + Serial.print(imu_data.mx); + Serial.print(" "); + Serial.print(imu_data.my); + Serial.print(" "); + Serial.print(imu_data.mz); + Serial.print(" "); + Serial.print(imu_data.roll); + Serial.print(" "); + Serial.print(imu_data.pitch); + Serial.print(" "); + Serial.println(imu_data.uh); + break; +#endif #ifdef USE_SERVOS case SERVO_WRITE: servos[arg1].setTargetPosition(arg2); @@ -302,6 +350,10 @@ void setup() { resetPID(); #endif + #ifdef USE_IMU + initIMU(); + #endif + /* Attach servos if used */ #ifdef USE_SERVOS int i; diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h index 402f11cd..026c3885 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h @@ -10,6 +10,7 @@ #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' diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h new file mode 100644 index 00000000..64c5f9f8 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h @@ -0,0 +1,26 @@ +/* Provide a unified interface to IMU devices */ + +#ifndef IMU_H +#define IMU_H + +typedef struct imuData_s + { + float ax = -999; + float ay = -999; + float az = -999; + float gx = -999; + float gy = -999; + float gz = -999; + float mx = -999; + float my = -999; + float mz = -999; + float roll = -999; + float pitch = -999; + float uh = -999; + } imuData; + + void initIMU(); + imuData readIMU(); + imuData imu_data; + +#endif diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino new file mode 100644 index 00000000..fdc3a4d8 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino @@ -0,0 +1,68 @@ +#ifdef USE_IMU + + #ifdef ADAFRUIT_9DOF + + #include + #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); + + /* Update this with the correct SLP for accurate altitude measurements */ + float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA; + + void initIMU() + { + if(!accel.begin()) + { + /* There was a problem detecting the LSM303 ... check your connections */ + Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!")); + while(1); + } + if(!mag.begin()) + { + /* There was a problem detecting the LSM303 ... check your connections */ + Serial.println("Ooops, no LSM303 detected ... Check your wiring!"); + while(1); + } + } + + imuData readIMU() { + imuData_s data; + + sensors_event_t accel_event; + sensors_event_t mag_event; + sensors_vec_t orientation; + + /* 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; + + if (dof.accelGetOrientation(&accel_event, &orientation)) + { + /* 'orientation' should have valid .roll and .pitch fields */ + 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)) + { + /* 'orientation' should have valid .heading data now */ + data.mz = orientation.heading; + } + + return data; + } + + #endif + +#endif From 77f5963e054fd13ea8b6bc0665c551d0a58aa348 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 4 Apr 2016 09:24:11 -0700 Subject: [PATCH 053/108] Fixed bug in heartbeat check --- ros_arduino_python/nodes/arduino_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 8fb54074..483a7024 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -227,6 +227,7 @@ def __init__(self): break except: r.sleep() + continue for sensor in self.mySensors: if sensor.rate != 0: From 9a84380e7ce03d65332055b8419d54a60f59912b Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 6 Apr 2016 06:29:19 -0700 Subject: [PATCH 054/108] Replaced all strings with F() macro in Serial.print statements --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 54 +++++++++---------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 046cf1e4..839fa8ea 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -47,7 +47,7 @@ #define USE_BASE // Enable/disable the base controller code -//#define USE_IMU // Enable/disable use of an IMU +#define USE_IMU // Enable/disable use of an IMU /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE @@ -191,7 +191,7 @@ int runCommand() { switch(cmd) { case GET_BAUDRATE: - Serial.print(" "); + Serial.print(F(" ")); Serial.println(BAUDRATE); break; case ANALOG_READ: @@ -202,17 +202,17 @@ 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)); @@ -221,34 +221,34 @@ int runCommand() { case READ_IMU: imu_data = readIMU(); Serial.print(imu_data.ax); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.ay); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.az); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.gx); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.gy); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.gz); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.mx); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.my); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.mz); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.roll); - Serial.print(" "); + Serial.print(F(" ")); Serial.print(imu_data.pitch); - Serial.print(" "); + Serial.print(F(" ")); Serial.println(imu_data.uh); 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()); @@ -261,25 +261,25 @@ int runCommand() { myServos[arg1].enable(); nServos++; } - Serial.println("OK"); + Serial.println(F("OK")); break; case SERVO_WRITE: if (myServos[arg1].isEnabled()) { myServos[arg1].setTargetPosition(arg2); } - Serial.println("OK"); + Serial.println(F("OK")); break; case SERVO_READ: Serial.println(myServos[arg1].getCurrentPosition()); break; case SERVO_DELAY: myServos[arg1].setServoDelay(arg1, arg2); - Serial.println("OK"); + Serial.println(F("OK")); break; case DETACH_SERVO: myServos[arg1].getServo().detach(); myServos[arg1].disable(); - Serial.println("OK"); + Serial.println(F("OK")); break; case ATTACH_SERVO: if (!haveServo(arg1)) { @@ -291,7 +291,7 @@ int runCommand() { myServos[arg1].getServo().attach(arg1); } myServos[arg1].enable(); - Serial.println("OK"); + Serial.println(F("OK")); break; #endif @@ -299,13 +299,13 @@ int runCommand() { #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 */ @@ -317,7 +317,7 @@ int runCommand() { else moving = 1; leftPID.TargetTicksPerFrame = arg1; rightPID.TargetTicksPerFrame = arg2; - Serial.println("OK"); + Serial.println(F("OK")); break; case UPDATE_PID: i = 0; @@ -329,11 +329,11 @@ int runCommand() { 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; } } From 142e529de902fbf0e688e16953525b57bfd23ccc Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 6 Apr 2016 06:29:51 -0700 Subject: [PATCH 055/108] Replaced all strings with F() macro in Serial.print statements --- ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino index fdc3a4d8..aefbecc0 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino @@ -22,13 +22,11 @@ { /* There was a problem detecting the LSM303 ... check your connections */ Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!")); - while(1); } if(!mag.begin()) { /* There was a problem detecting the LSM303 ... check your connections */ - Serial.println("Ooops, no LSM303 detected ... Check your wiring!"); - while(1); + Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!")); } } From 59488c7b36d4e8d8008b29c435e7c44089d4df25 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 6 Apr 2016 06:43:30 -0700 Subject: [PATCH 056/108] Removed DOS-like CR/LF line terminators in arduino_driver.py --- .../src/ros_arduino_python/arduino_driver.py | 838 +++++++++--------- 1 file changed, 429 insertions(+), 409 deletions(-) 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 f951662c..8f55ddb4 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -1,409 +1,429 @@ -#!/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 -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, "..." - - # 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.port: - time.sleep(self.timeout) - - # Now open the port with the real settings - self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) - - # Test the connection by reading the baudrate - test = self.get_baud() - if test != self.baudrate: - time.sleep(self.timeout) - 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.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 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.serial_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.serial_port.flushInput() - except: - pass - - ntries = 1 - attempts = 0 - - try: - self.serial_port.write(cmd + '\r') - value = self.recv(self.timeout) - while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): - try: - self.serial_port.flushInput() - self.serial_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.serial_port.flushInput() - except: - pass - - ntries = 1 - attempts = 0 - - try: - self.serial_port.write(cmd + '\r') - values = self.recv_array() - while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): - try: - self.serial_port.flushInput() - self.serial_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.serial_port.flushInput() - except: - pass - - ntries = 1 - attempts = 0 - - try: - self.serial_port.write(cmd + '\r') - ack = self.recv(self.timeout) - while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): - try: - self.serial_port.flushInput() - self.serial_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. - ''' - try: - return int(self.execute('b')) - except: - print "Failed to determine baud rate of Arduino so aborting!" - os._exit(1) - - 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_pin_mode(self, pin, mode): - return self.execute_ack('c A%d %d' %(pin, mode)) - - 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 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, - 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 thread +from math import pi as PI, degrees, radians +import os +import time +import sys, traceback +from serial.serialutil import SerialException +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, "..." + + # 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.port: + time.sleep(self.timeout) + + # Now open the port with the real settings + self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) + + # Test the connection by reading the baudrate + test = self.get_baud() + if test != self.baudrate: + time.sleep(self.timeout) + 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.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 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 = '' + start = time.time() + + while c != '\r': + c = self.serial_port.read(1) + value += c + if time.time() - start > timeout: + return None + + value = value.strip('\r') + + return value + +# 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.serial_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.serial_port.flushInput() + except: + pass + + ntries = 1 + attempts = 0 + + try: + self.serial_port.write(cmd + '\r') + value = self.recv(self.timeout) + while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): + try: + self.serial_port.flushInput() + self.serial_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.serial_port.flushInput() + except: + pass + + ntries = 1 + attempts = 0 + + try: + self.serial_port.write(cmd + '\r') + values = self.recv_array() + while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): + try: + self.serial_port.flushInput() + self.serial_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.serial_port.flushInput() + except: + pass + + ntries = 1 + attempts = 0 + + try: + self.serial_port.write(cmd + '\r') + ack = self.recv(self.timeout) + while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): + try: + self.serial_port.flushInput() + self.serial_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. + ''' + try: + return int(self.execute('b')) + except: + print "Failed to determine baud rate of Arduino so aborting!" + os._exit(1) + + 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_pin_mode(self, pin, mode): + return self.execute_ack('c A%d %d' %(pin, mode)) + + 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 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, + 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." + From 03b56660944ed334afbe5b71ac09aabf927a17af Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 9 Apr 2016 06:19:57 -0700 Subject: [PATCH 057/108] Modified the execute() and recv() functions to be more robust --- .../src/ros_arduino_python/arduino_driver.py | 82 ++++++------------- 1 file changed, 24 insertions(+), 58 deletions(-) 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 8f55ddb4..15430134 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -45,7 +45,6 @@ def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): self.timeout = timeout self.encoder_count = 0 self.writeTimeout = timeout - self.interCharTimeout = timeout / 30. # Keep things thread safe self.mutex = thread.allocate_lock() @@ -105,44 +104,22 @@ def send(self, cmd): self.serial_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 = '' - start = time.time() - + while c != '\r': - c = self.serial_port.read(1) - value += c - if time.time() - start > timeout: + c = self.serial_port.read() + if c == '': return None + value += c value = value.strip('\r') return value - -# 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.serial_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 @@ -165,31 +142,25 @@ 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 [] + return self.recv(self.timeout).split() - def execute(self, cmd): - ''' Thread safe execution of "cmd" on the Arduino returning a single integer value. + def execute(self, cmd, max_attempts=5): + ''' Thread safe execution of "cmd" on the Arduino returning a single value. ''' self.mutex.acquire() - try: - self.serial_port.flushInput() - except: - pass + self.serial_port.flushInput() + self.serial_port.flushOutput() - ntries = 1 attempts = 0 try: self.serial_port.write(cmd + '\r') value = self.recv(self.timeout) - while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): + while attempts < max_attempts and (value == '' or value == 'Invalid Command' or value == None): try: self.serial_port.flushInput() + self.serial_port.flushOutput() self.serial_port.write(cmd + '\r') value = self.recv(self.timeout) except: @@ -201,27 +172,26 @@ def execute(self, cmd): value = None self.mutex.release() - return int(value) - def execute_array(self, cmd): + return value + + def execute_array(self, cmd, max_attempts=5): ''' Thread safe execution of "cmd" on the Arduino returning an array. ''' self.mutex.acquire() - try: - self.serial_port.flushInput() - except: - pass + self.serial_port.flushInput() + self.serial_port.flushOutput() - ntries = 1 attempts = 0 try: self.serial_port.write(cmd + '\r') values = self.recv_array() - while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): + while attempts < max_attempts and (values == '' or values == 'Invalid Command' or values == [] or values == None): try: self.serial_port.flushInput() + self.serial_port.flushOutput() self.serial_port.write(cmd + '\r') values = self.recv_array() except: @@ -232,13 +202,9 @@ def execute_array(self, cmd): 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): @@ -285,7 +251,7 @@ def get_baud(self): ''' Get the current baud rate on the serial port. ''' try: - return int(self.execute('b')) + return int(self.execute('b', max_attempts=5)) except: print "Failed to determine baud rate of Arduino so aborting!" os._exit(1) @@ -297,13 +263,13 @@ def get_encoder_counts(self): raise SerialException return None else: - return values + return map(int, 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 ''' @@ -329,13 +295,13 @@ def analog_pin_mode(self, pin, mode): return self.execute_ack('c A%d %d' %(pin, mode)) def analog_read(self, pin): - return self.execute('a %d' %pin) + return int(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) + return int(self.execute('d %d' %pin)) def digital_write(self, pin, value): return self.execute_ack('w %d %d' %(pin, value)) From e996d12280d7569c23958021ecb75dc3af252e27 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 9 Apr 2016 16:16:32 -0700 Subject: [PATCH 058/108] Added get_imu_data function and cleaned up unused variables --- .../src/ros_arduino_python/arduino_driver.py | 45 ++++++++++--------- 1 file changed, 25 insertions(+), 20 deletions(-) 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 15430134..484fecdf 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -22,7 +22,7 @@ """ import thread -from math import pi as PI, degrees, radians +from math import pi as PI import os import time import sys, traceback @@ -30,11 +30,6 @@ 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. @@ -48,13 +43,7 @@ def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): # 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, "..." @@ -207,25 +196,23 @@ def execute_array(self, cmd, max_attempts=5): return values - def execute_ack(self, cmd): + def execute_ack(self, cmd, max_attempts=5): ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. ''' self.mutex.acquire() - try: - self.serial_port.flushInput() - except: - pass + self.serial_port.flushInput() + self.serial_port.flushOutput() - ntries = 1 attempts = 0 try: self.serial_port.write(cmd + '\r') ack = self.recv(self.timeout) - while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): + while attempts < max_attempts and (ack == '' or ack == 'Invalid Command' or ack == None): try: self.serial_port.flushInput() + self.serial_port.flushOutput() self.serial_port.write(cmd + '\r') ack = self.recv(self.timeout) except: @@ -270,6 +257,24 @@ def reset_encoders(self): ''' 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, uh] + + where a stands for accelerometer, g for gyroscope and m for magnetometer. + The last value uh stands for "unified heading" that some IMU's compute + from both gyroscope and compass data. + ''' + values = self.execute_array('i') + if len(values) != 12: + print "IMU data incomplete!" + raise SerialException + return None + else: + return map(float, values) + def drive(self, right, left): ''' Speeds are given in encoder ticks per PID interval ''' From e99871c6b37d662566889fc2f2ac0e6ed1e74208 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 10 Apr 2016 07:22:05 -0700 Subject: [PATCH 059/108] Updated execute() commands to use readline() instead of multiple read() calls --- .../src/ros_arduino_python/arduino_driver.py | 138 +++--------------- 1 file changed, 17 insertions(+), 121 deletions(-) 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 484fecdf..63b8d1ca 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -92,73 +92,14 @@ def send(self, cmd): ''' self.serial_port.write(cmd + '\r') - def recv(self, timeout=0.5): - ''' 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 = '' - - while c != '\r': - c = self.serial_port.read() - if c == '': - return None - value += c - - 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. - ''' - return self.recv(self.timeout).split() - def execute(self, cmd, max_attempts=5): ''' Thread safe execution of "cmd" on the Arduino returning a single value. ''' self.mutex.acquire() + + self.serial_port.write(cmd + '\r') - self.serial_port.flushInput() - self.serial_port.flushOutput() - - attempts = 0 - - try: - self.serial_port.write(cmd + '\r') - value = self.recv(self.timeout) - while attempts < max_attempts and (value == '' or value == 'Invalid Command' or value == None): - try: - self.serial_port.flushInput() - self.serial_port.flushOutput() - self.serial_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 + value = self.serial_port.readline().strip('\n') self.mutex.release() @@ -167,65 +108,16 @@ def execute(self, cmd, max_attempts=5): def execute_array(self, cmd, max_attempts=5): ''' Thread safe execution of "cmd" on the Arduino returning an array. ''' - self.mutex.acquire() - - self.serial_port.flushInput() - self.serial_port.flushOutput() - - attempts = 0 - - try: - self.serial_port.write(cmd + '\r') - values = self.recv_array() - while attempts < max_attempts and (values == '' or values == 'Invalid Command' or values == [] or values == None): - try: - self.serial_port.flushInput() - self.serial_port.flushOutput() - self.serial_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 [] - - self.mutex.release() + values = self.execute(cmd, max_attempts).split() return values - + def execute_ack(self, cmd, max_attempts=5): ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. ''' - self.mutex.acquire() - - self.serial_port.flushInput() - self.serial_port.flushOutput() - - attempts = 0 + ack = self.execute(cmd, max_attempts) - try: - self.serial_port.write(cmd + '\r') - ack = self.recv(self.timeout) - while attempts < max_attempts and (ack == '' or ack == 'Invalid Command' or ack == None): - try: - self.serial_port.flushInput() - self.serial_port.flushOutput() - self.serial_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' + return ack == 'OK' def update_pid(self, Kp, Kd, Ki, Ko): ''' Set the PID parameters on the Arduino @@ -240,8 +132,7 @@ def get_baud(self): try: return int(self.execute('b', max_attempts=5)) except: - print "Failed to determine baud rate of Arduino so aborting!" - os._exit(1) + return None def get_encoder_counts(self): values = self.execute_array('e') @@ -270,7 +161,6 @@ def get_imu_data(self): values = self.execute_array('i') if len(values) != 12: print "IMU data incomplete!" - raise SerialException return None else: return map(float, values) @@ -300,13 +190,19 @@ def analog_pin_mode(self, pin, mode): return self.execute_ack('c A%d %d' %(pin, mode)) def analog_read(self, pin): - return int(self.execute('a %d' %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): - return int(self.execute('d %d' %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)) @@ -327,7 +223,7 @@ def servo_write(self, 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): From 6234b9260ad600199e9cd343338f16b73e742c3a Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 10 Apr 2016 07:35:51 -0700 Subject: [PATCH 060/108] Added max_attempts loop to execute() function --- .../src/ros_arduino_python/arduino_driver.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) 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 63b8d1ca..a9bd44fe 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -92,27 +92,34 @@ def send(self, cmd): ''' self.serial_port.write(cmd + '\r') - def execute(self, cmd, max_attempts=5): + def execute(self, cmd, max_attempts=3): ''' Thread safe execution of "cmd" on the Arduino returning a single value. ''' self.mutex.acquire() + attempts = 1 + self.serial_port.write(cmd + '\r') value = self.serial_port.readline().strip('\n') + while len(value) == 0 and attempts < max_attempts: + self.serial_port.write(cmd + '\r') + value = self.serial_port.readline().strip('\n') + attempts += 1 + self.mutex.release() return value - def execute_array(self, cmd, max_attempts=5): + def execute_array(self, cmd, max_attempts=3): ''' Thread safe execution of "cmd" on the Arduino returning an array. ''' values = self.execute(cmd, max_attempts).split() return values - def execute_ack(self, cmd, max_attempts=5): + def execute_ack(self, cmd, max_attempts=3): ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. ''' ack = self.execute(cmd, max_attempts) @@ -136,9 +143,8 @@ def get_baud(self): 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 map(int, values) @@ -159,8 +165,8 @@ def get_imu_data(self): from both gyroscope and compass data. ''' values = self.execute_array('i') + if len(values) != 12: - print "IMU data incomplete!" return None else: return map(float, values) From 79e5a3b1a806faf6ff2b1b24d496e6ddf776461f Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 11 Apr 2016 08:03:37 -0700 Subject: [PATCH 061/108] Updated imu.h and imu.ino and increased command argv array size from 16 to 32 to handel PID update --- .../ROSArduinoBridge/ROSArduinoBridge.ino | 14 ++++++++----- .../src/libraries/ROSArduinoBridge/imu.h | 12 ++++++++++- .../src/libraries/ROSArduinoBridge/imu.ino | 21 ++++++++++--------- 3 files changed, 31 insertions(+), 16 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 839fa8ea..a8e7201e 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -143,6 +143,7 @@ #ifdef USE_IMU #include "imu.h" + // The only IMU currently supported is the Adafruit 9-DOF IMU #define ADAFRUIT_9DOF #endif @@ -161,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; @@ -220,6 +221,9 @@ int runCommand() { #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); @@ -242,7 +246,7 @@ int runCommand() { Serial.print(F(" ")); Serial.print(imu_data.pitch); Serial.print(F(" ")); - Serial.println(imu_data.uh); + Serial.println(imu_data.ch); break; #endif #ifdef USE_SERVOS @@ -322,8 +326,8 @@ int runCommand() { 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]; diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h index 64c5f9f8..e1fc0774 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h @@ -3,6 +3,16 @@ #ifndef IMU_H #define IMU_H +/* + 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. +*/ + typedef struct imuData_s { float ax = -999; @@ -16,7 +26,7 @@ typedef struct imuData_s float mz = -999; float roll = -999; float pitch = -999; - float uh = -999; + float ch = -999; } imuData; void initIMU(); diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino index aefbecc0..13bd8110 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino @@ -12,22 +12,17 @@ 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() { - if(!accel.begin()) - { - /* There was a problem detecting the LSM303 ... check your connections */ - Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!")); - } - if(!mag.begin()) - { - /* There was a problem detecting the LSM303 ... check your connections */ - Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!")); - } + accel.begin(); + mag.begin(); + gyro.begin(); } imuData readIMU() { @@ -36,6 +31,7 @@ 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); @@ -43,6 +39,11 @@ 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)) { /* 'orientation' should have valid .roll and .pitch fields */ From 17d6a44e3ea04e9009afc86f7534c74ed01b70a8 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 12 Apr 2016 06:28:00 -0700 Subject: [PATCH 062/108] Removed print statement from update_pid() function --- .../src/ros_arduino_python/arduino_driver.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 a9bd44fe..70b52b2b 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -129,7 +129,6 @@ def execute_ack(self, cmd, max_attempts=3): 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) @@ -158,11 +157,11 @@ 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, uh] + [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 uh stands for "unified heading" that some IMU's compute - from both gyroscope and compass data. + 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') From 97946646d276d5a97a47f5f27e030295e17a5182 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 12 Apr 2016 06:30:07 -0700 Subject: [PATCH 063/108] Added initial IMU support to arduino_sensors and base_controller --- .../src/ros_arduino_python/arduino_sensors.py | 81 ++++++++++++++++--- .../src/ros_arduino_python/base_controller.py | 24 +++--- 2 files changed, 83 insertions(+), 22 deletions(-) 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 e6b7276e..90904ecd 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -20,10 +20,11 @@ """ import rospy -from sensor_msgs.msg import Range +from sensor_msgs.msg import Range, Imu from ros_arduino_msgs.msg import * from ros_arduino_msgs.srv import * -from math import pow +from math import pow, radians +from tf.transformations import quaternion_from_euler LOW = 0 HIGH = 1 @@ -38,6 +39,7 @@ class MessageType: FLOAT = 3 INT = 4 BOOL = 5 + IMU = 6 class Sensor(object): def __init__(self, device, name, pin=None, rate=0, direction="input", frame_id="base_link", **kwargs): @@ -58,25 +60,22 @@ def poll(self): now = rospy.Time.now() if now > self.t_next: if self.direction == "input": - try: - self.value = self.read_value() - except: - return + self.value = self.read_value() else: - try: - self.ack = self.write_value() - except: - return + self.ack = self.write_value() # For range sensors, assign the value to the range message field if self.message_type == MessageType.RANGE: self.msg.range = self.value - else: + elif self.message_type != MessageType.IMU: self.msg.value = self.value # Add a timestamp and publish the message self.msg.header.stamp = rospy.Time.now() - self.pub.publish(self.msg) + try: + self.pub.publish(self.msg) + except: + rospy.logerr("Invalid value read from sensor: " + str(self.name)) self.t_next = now + self.t_delta @@ -286,6 +285,64 @@ def read_value(self): return distance +class IMU(Sensor): + def __init__(self,*args, **kwargs): + super(IMU, self).__init__(*args, **kwargs) + + self.message_type = MessageType.IMU + + self.rpy = None + + self.msg = Imu() + self.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] + + # Create the publisher + if self.rate != 0: + self.pub = rospy.Publisher("~sensor/" + self.name, Imu, 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 uh stands for "unified heading" that some IMU's compute + from both gyroscope and compass data. + + ''' + data = self.device.get_imu_data() + + try: + ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch = 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 = radians(ax) + self.msg.linear_acceleration.y = radians(ay) + self.msg.linear_acceleration.z = radians(az) + + self.msg.angular_velocity.x = radians(gx) + self.msg.angular_velocity.y = radians(gy) + self.msg.angular_velocity.z = radians(gz) + + if ch != -999: + yaw = -radians(uh) + 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 + class PololuMotorCurrent(AnalogFloatSensor): def __init__(self, *args, **kwargs): super(PololuMotorCurrent, self).__init__(*args, **kwargs) 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 aed4949b..38f3278d 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -39,6 +39,8 @@ def __init__(self, arduino, base_frame): 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 pid_params = dict() @@ -118,6 +120,7 @@ def setup_pid(self, pid_params): self.Ko = pid_params['Ko'] 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)) def poll(self): now = rospy.Time.now() @@ -166,13 +169,14 @@ def poll(self): 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" @@ -198,9 +202,9 @@ def poll(self): odom.pose.covariance[7] = 0.1 odom.pose.covariance[35] = 0.05 if self.use_imu_heading else 0.2 - odom.pose.covariance[14] = float_info.max # set a non-zero covariance on unused - odom.pose.covariance[21] = float_info.max # dimensions (z, pitch and roll); this - odom.pose.covariance[28] = float_info.max # is a requirement of robot_pose_ekf + odom.pose.covariance[14] = 1e6 # set a non-zero covariance on unused + odom.pose.covariance[21] = 1e6 # dimensions (z, pitch and roll); this + odom.pose.covariance[28] = 1e6 # is a requirement of robot_pose_ekf self.odomPub.publish(odom) From d03061ae6099a04771c0430ae5ca928899ecd848 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:29:00 -0700 Subject: [PATCH 064/108] Added missing resetEncoders() function to encoder_driver.ino --- .../src/libraries/ROSArduinoBridge/encoder_driver.ino | 6 ++++++ ros_arduino_python/src/ros_arduino_python/arduino_driver.py | 6 ++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino index 86eb6706..91b90ca7 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino @@ -32,6 +32,12 @@ 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 */ 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 70b52b2b..f2de5e50 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -100,10 +100,10 @@ def execute(self, cmd, max_attempts=3): attempts = 1 self.serial_port.write(cmd + '\r') - value = self.serial_port.readline().strip('\n') - + while len(value) == 0 and attempts < max_attempts: + print "Command", cmd, "failed", attempts, "time(s)" self.serial_port.write(cmd + '\r') value = self.serial_port.readline().strip('\n') attempts += 1 @@ -141,6 +141,8 @@ def get_baud(self): return None def get_encoder_counts(self): + ''' Read the current encoder counts + ''' values = self.execute_array('e') if len(values) != 2: From 2b4567ec08827b8ce20c506fcac7430d6cd13f79 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:30:15 -0700 Subject: [PATCH 065/108] Added resetPID() call when robot velocities are 0 to fix motion perserverance bug --- .../src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino | 1 + .../src/libraries/ROSArduinoBridge/diff_controller.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index a8e7201e..4c144d6e 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -316,6 +316,7 @@ int runCommand() { lastMotorCommand = millis(); if (arg1 == 0 && arg2 == 0) { setMotorSpeeds(0, 0); + resetPID(); moving = 0; } else moving = 1; diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h index 678800ce..5f2f8c48 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h @@ -100,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; From 1373f49137a95adf0b613daf16c3bff6064a483a Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:36:06 -0700 Subject: [PATCH 066/108] Fixed typo in comment --- ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h index e1fc0774..4098798b 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.h @@ -10,7 +10,7 @@ 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. + compute to compensate magnetic heading for the current roll and pitch. */ typedef struct imuData_s From cf51f8cea8dcdc234dbe93b053ec8932fcc84ada Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:44:14 -0700 Subject: [PATCH 067/108] Added UpdatePID service and dynamic reconfigure support --- ros_arduino_msgs/CMakeLists.txt | 1 + ros_arduino_msgs/srv/UpdatePID.srv | 5 ++ ros_arduino_python/CMakeLists.txt | 12 +++- ros_arduino_python/cfg/ROSArduinoBridge.cfg | 19 +++++ ros_arduino_python/nodes/arduino_node.py | 80 +++++++++++++++++---- ros_arduino_python/package.xml | 4 ++ 6 files changed, 104 insertions(+), 17 deletions(-) create mode 100755 ros_arduino_msgs/srv/UpdatePID.srv create mode 100755 ros_arduino_python/cfg/ROSArduinoBridge.cfg diff --git a/ros_arduino_msgs/CMakeLists.txt b/ros_arduino_msgs/CMakeLists.txt index a2f82115..dbcbf4d5 100644 --- a/ros_arduino_msgs/CMakeLists.txt +++ b/ros_arduino_msgs/CMakeLists.txt @@ -35,6 +35,7 @@ add_service_files(FILES ServoWrite.srv SetSpeed.srv SetServoSpeed.srv + UpdatePID.srv ) generate_messages( 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..b435ceee --- /dev/null +++ b/ros_arduino_python/cfg/ROSArduinoBridge.cfg @@ -0,0 +1,19 @@ +#! /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) + +exit(gen.generate(PACKAGE, "ros_arduino_bridge", "ROSArduinoBridge")) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 483a7024..67b84dec 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -23,6 +23,9 @@ from ros_arduino_python.arduino_driver import Arduino from ros_arduino_python.arduino_sensors import * from ros_arduino_msgs.srv import * +from dynamic_reconfigure.server import Server +import dynamic_reconfigure.client +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 @@ -37,7 +40,7 @@ class ArduinoROS(): def __init__(self): - rospy.init_node('Arduino', log_level=rospy.INFO) + rospy.init_node('arduino', log_level=rospy.DEBUG) # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) @@ -111,23 +114,32 @@ def __init__(self): # 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 = Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback) + + # Connect to the dynamic_reconfigure client + dyn_client = dynamic_reconfigure.client.Client("arduino", timeout=5) + # Initialize the device self.device = Arduino(self.port, self.baud, self.timeout) - + # Make the connection self.device.connect() - + rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # Reserve a thread lock mutex = thread.allocate_lock() # Initialize any sensors - self.mySensors = list() - + self.sensors = 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(): if params['type'].lower() == 'Ping'.lower(): @@ -144,12 +156,14 @@ def __init__(self): 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) # 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) + self.sensors.append(sensor) if params['rate'] != None and params['rate'] != 0: rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) @@ -208,7 +222,7 @@ def __init__(self): # Initialize the base controller if used if self.use_base_controller: - self.myBaseController = BaseController(self.device, self.base_frame) + self.base_controller = BaseController(self.device, self.base_frame) print "\n==> ROS Arduino Bridge ready for action!" @@ -218,7 +232,7 @@ def __init__(self): try: self.device.serial_port.inWaiting() except IOError: - rospy.loginfo("Lost serial connection...") + rospy.loginfo("Lost serial connection. Waiting to reconnect...") self.device.close() while True: try: @@ -229,7 +243,7 @@ def __init__(self): r.sleep() continue - for sensor in self.mySensors: + for sensor in self.sensors: if sensor.rate != 0: mutex.acquire() sensor.poll() @@ -237,7 +251,7 @@ def __init__(self): if self.use_base_controller: mutex.acquire() - self.myBaseController.poll() + self.base_controller.poll() mutex.release() if self.have_joints: @@ -253,9 +267,10 @@ def __init__(self): 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.sensors)): + if self.sensors[i].message_type != MessageType.IMU: + msg.name.append(self.sensors[i].name) + msg.value.append(self.sensors[i].value) try: self.sensorStatePub.publish(msg) except: @@ -326,10 +341,45 @@ def AnalogReadHandler(self, req): def ResetOdometryHandler(self, req): if self.use_base_controller: - self.myBaseController.reset_odometry() + self.base_controller.reset_odometry() 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 + + 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...") diff --git a/ros_arduino_python/package.xml b/ros_arduino_python/package.xml index 5bd96a00..391d96e2 100644 --- a/ros_arduino_python/package.xml +++ b/ros_arduino_python/package.xml @@ -10,6 +10,8 @@ http://ros.org/wiki/ros_arduino_python catkin + + dynamic_reconfigure rospy std_msgs @@ -19,4 +21,6 @@ tf ros_arduino_msgs python-serial + dynamic_reconfigure + From 8dd8a19d8cf11689ff7b20920823776a8796cc56 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:44:48 -0700 Subject: [PATCH 068/108] Updated sample arduino_params.yaml file --- ros_arduino_python/config/arduino_params.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index eb7a5876..1e0931b1 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -10,12 +10,21 @@ timeout: 0.1 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,6 +39,10 @@ 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 following: # * Ping From 0e04388b5dfe850afa61f5cef81b2897cb1facec Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:45:52 -0700 Subject: [PATCH 069/108] Added flushInput() and flushOutput() to execute() when timeout or error occurs --- .../src/ros_arduino_python/arduino_driver.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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 f2de5e50..3b55c2cf 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -30,7 +30,7 @@ import serial class Arduino: - def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): + def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, debug=True): self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller. self.PID_INTERVAL = 1000 / 30 @@ -40,6 +40,7 @@ def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): self.timeout = timeout self.encoder_count = 0 self.writeTimeout = timeout + self.debug = debug # Keep things thread safe self.mutex = thread.allocate_lock() @@ -103,7 +104,11 @@ def execute(self, cmd, max_attempts=3): value = self.serial_port.readline().strip('\n') while len(value) == 0 and attempts < max_attempts: - print "Command", cmd, "failed", attempts, "time(s)" + if self.debug: + print "Command", cmd, "failed", attempts, "time(s)" + + self.serial_port.flushInput() + self.serial_port.flushOutput() self.serial_port.write(cmd + '\r') value = self.serial_port.readline().strip('\n') attempts += 1 From a43f33e1b69ba0bd6a6878566ee21c1f343e1288 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:46:36 -0700 Subject: [PATCH 070/108] Tweaked description for IMU sensor type --- ros_arduino_python/src/ros_arduino_python/arduino_sensors.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 90904ecd..89fa4a9a 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -311,9 +311,8 @@ def read_value(self): [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 uh stands for "unified heading" that some IMU's compute - from both gyroscope and compass data. - + 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() From 171ed94a31c95e8d150bc3196cc6a9770aa994ca Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 18 Apr 2016 06:47:36 -0700 Subject: [PATCH 071/108] Enforced float() type on wheel_track for dth calculation --- .../src/ros_arduino_python/base_controller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 38f3278d..43bd46df 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -44,16 +44,16 @@ def __init__(self, arduino, base_frame): self.stopped = False 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.accel_limit = rospy.get_param('~accel_limit', 1.0) self.motors_reversed = rospy.get_param("~motors_reversed", False) # Set up PID parameters and check for missing values @@ -149,7 +149,7 @@ def poll(self): self.enc_left = left_enc dxy_ave = self.odom_linear_scale_correction * (dright + dleft) / 2.0 - dth = self.odom_angular_scale_correction * (dright - dleft) / self.wheel_track + dth = self.odom_angular_scale_correction * (dright - dleft) / float(self.wheel_track) vxy = dxy_ave / dt vth = dth / dt From bc93573f6b6365f59de8d669dd59bd2adae85fd3 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 28 May 2016 08:27:12 -0700 Subject: [PATCH 072/108] Updated README for master branch --- README.md | 4 ++++ ros_arduino_python/cfg/ROSArduinoBridge.cfg | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/README.md b/README.md index 1c2dabdb..dead802e 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,7 @@ +**UNSTABLE BRANCH*** + +This branch is under active development and is not likely to work. Please use the official hydro-devel or indigo-devel branche for ROS Hydro or ROS Indigo, respectively. + Table of Contents ================= diff --git a/ros_arduino_python/cfg/ROSArduinoBridge.cfg b/ros_arduino_python/cfg/ROSArduinoBridge.cfg index b435ceee..1bf775c3 100755 --- a/ros_arduino_python/cfg/ROSArduinoBridge.cfg +++ b/ros_arduino_python/cfg/ROSArduinoBridge.cfg @@ -16,4 +16,8 @@ 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")) From ee4485b966b1be1eafa440872a3d011ce1f0048c Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 28 May 2016 08:27:54 -0700 Subject: [PATCH 073/108] Updated README for master branch --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index dead802e..aaf665d0 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -**UNSTABLE BRANCH*** +**UNSTABLE BRANCH** This branch is under active development and is not likely to work. Please use the official hydro-devel or indigo-devel branche for ROS Hydro or ROS Indigo, respectively. From f796a1a38ffa9219da009f301106d27e19d9629a Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 28 May 2016 08:28:25 -0700 Subject: [PATCH 074/108] Updated README for master branch --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index aaf665d0..bb35f559 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ **UNSTABLE BRANCH** -This branch is under active development and is not likely to work. Please use the official hydro-devel or indigo-devel branche for ROS Hydro or ROS Indigo, respectively. +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 ================= From d8dd6a5b13c2a5ebb23dd1fc036e4a1bbcb7ac4b Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 1 Jun 2016 05:52:48 -0700 Subject: [PATCH 075/108] Restored defaults to ROSArduinoBridge.ino --- .../src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 4c144d6e..79ade3ce 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -45,9 +45,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#define USE_BASE // Enable/disable the base controller code +//#define USE_BASE // Enable/disable the base controller code -#define USE_IMU // Enable/disable use of an IMU +//#define USE_IMU // Enable/disable use of an IMU /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE From c8fe760785ef59e3cf3bd146ce1d9e95e3b262f7 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 1 Jun 2016 06:11:29 -0700 Subject: [PATCH 076/108] Added error codes to arduino_driver.py to be used with ROS diagnostics --- .../src/ros_arduino_python/arduino_driver.py | 142 ++++++++++++------ 1 file changed, 100 insertions(+), 42 deletions(-) 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 3b55c2cf..e84c70ea 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -21,16 +21,32 @@ """ +import rospy import thread from math import pi as PI -import os -import time -import sys, traceback -from serial.serialutil import SerialException +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, debug=True): + def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, 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 @@ -41,41 +57,59 @@ def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, debug=True) self.encoder_count = 0 self.writeTimeout = timeout self.debug = debug - + # Keep things thread safe self.mutex = thread.allocate_lock() def connect(self): try: - print "Connecting to Arduino on port", self.port, "..." + 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.port: + while not self.serial_port.isOpen(): time.sleep(self.timeout) # Now open the port with the real settings self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) + # When an Arduino is first plugged in, it can take time for the serial port to wake up + max_attempts = 10 + attempts = 0 + + # Wake up the serial port + self.serial_port.write('\r') + while attempts < max_attempts and self.serial_port.read() == '': + rospy.loginfo("Waking up serial port...") + self.serial_port.write('\r') + attempts += 1 + time.sleep(1) + # Test the connection by reading the baudrate - test = self.get_baud() - if test != self.baudrate: - time.sleep(self.timeout) - test = self.get_baud() - if test != self.baudrate: - raise SerialException - print "Connected at", self.baudrate - print "Arduino is ready." + attepmpts = 0 + while self.get_baud() != self.baudrate and attempts < max_attempts: + attempts += 1 + self.serial_port.flushInput() + self.serial_port.flushOutput() + rospy.loginfo("Connecting...") + try: + self.serial_port.inWaiting() + rospy.loginfo("Connected at " + str(self.baudrate)) + rospy.loginfo("Arduino is ready.") + except IOError: + raise SerialException except SerialException: - print "Serial Exception:" - print sys.exc_info() - print "Traceback follows:" + rospy.logerr("Serial Exception:") + rospy.logerr(sys.exc_info()) + rospy.logerr("Traceback follows:") traceback.print_exc(file=sys.stdout) - print "Cannot connect to Arduino!" - os._exit(1) + 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. @@ -93,55 +127,79 @@ def send(self, cmd): ''' self.serial_port.write(cmd + '\r') - def execute(self, cmd, max_attempts=3): + def execute(self, cmd, timeout=0.5): ''' Thread safe execution of "cmd" on the Arduino returning a single value. ''' self.mutex.acquire() - attempts = 1 - - self.serial_port.write(cmd + '\r') - value = self.serial_port.readline().strip('\n') - - while len(value) == 0 and attempts < max_attempts: - if self.debug: - print "Command", cmd, "failed", attempts, "time(s)" + 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') - attempts += 1 + 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, max_attempts=3): + def execute_array(self, cmd): ''' Thread safe execution of "cmd" on the Arduino returning an array. ''' - values = self.execute(cmd, max_attempts).split() + try: + values = self.execute(cmd).split() + except CommandException as e: + values = None return values - def execute_ack(self, cmd, max_attempts=3): + def execute_ack(self, cmd): ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. ''' - ack = self.execute(cmd, max_attempts) - - return ack == 'OK' + 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) - self.execute_ack(cmd) + return self.execute_ack(cmd) def get_baud(self): ''' Get the current baud rate on the serial port. ''' try: - return int(self.execute('b', max_attempts=5)) + return int(self.execute('b')) except: return None @@ -258,10 +316,10 @@ def attach_servo(self, 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, + 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 self.execute('p %d' %pin); + return int(self.execute('p %d' %pin)); # def get_maxez1(self, triggerPin, outputPin): # ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar From 82b2bbb54679b31e835e51d73af08f5a1d783e55 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 26 Jun 2016 18:43:21 -0700 Subject: [PATCH 077/108] Improved initial connection loop in arduino_driver.py --- .../src/ros_arduino_python/arduino_driver.py | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) 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 e84c70ea..2f1d90c5 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -77,15 +77,31 @@ def connect(self): # When an Arduino is first plugged in, it can take time for the serial port to wake up max_attempts = 10 - attempts = 0 + attempts = 1 + timeout = self.timeout # Wake up the serial port - self.serial_port.write('\r') - while attempts < max_attempts and self.serial_port.read() == '': - rospy.loginfo("Waking up serial port...") + while attempts < max_attempts: self.serial_port.write('\r') + time.sleep(timeout) + test = self.serial_port.read() + if test != '': + break + + rospy.loginfo("Waking up serial port attempt " + str(attempts) + " of " + str(max_attempts)) + # Increase timeout by 10% + timeout *= 1.1 + self.serial_port.timeout = timeout + self.serial_port.writeTimeout = timeout + self.serial_port.flushInput() + self.serial_port.flushOutput() attempts += 1 - time.sleep(1) + + if test == '': + raise SerialException + + if timeout != self.timeout: + rospy.loginfo("Found best timeout to be " + str(timeout) + " seconds") # Test the connection by reading the baudrate attepmpts = 0 @@ -97,7 +113,7 @@ def connect(self): try: self.serial_port.inWaiting() rospy.loginfo("Connected at " + str(self.baudrate)) - rospy.loginfo("Arduino is ready.") + rospy.loginfo("Arduino is ready!") except IOError: raise SerialException From 6975ea2e747c106b3950a3d99f91f98698a726f5 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 27 Jun 2016 05:50:38 -0700 Subject: [PATCH 078/108] Improved initial connection loop in arduino_driver.py --- .../src/ros_arduino_python/arduino_driver.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) 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 2f1d90c5..91c3c238 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -75,41 +75,41 @@ def connect(self): # Now open the port with the real settings self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) - # When an Arduino is first plugged in, it can take time for the serial port to wake up + # It can take time for the serial port to wake up max_attempts = 10 - attempts = 1 + attempts = 0 timeout = self.timeout # Wake up the serial port + self.serial_port.write('\r\r\r') + self.serial_port.read() + + # Keep trying for max_attempts while attempts < max_attempts: - self.serial_port.write('\r') - time.sleep(timeout) + attempts += 1 + self.serial_port.write('\r\r\r') test = self.serial_port.read() if test != '': break rospy.loginfo("Waking up serial port attempt " + str(attempts) + " of " + str(max_attempts)) - # Increase timeout by 10% + + # Increase timeout by 10% timeout *= 1.1 self.serial_port.timeout = timeout self.serial_port.writeTimeout = timeout - self.serial_port.flushInput() - self.serial_port.flushOutput() - attempts += 1 if test == '': raise SerialException - if timeout != self.timeout: - rospy.loginfo("Found best timeout to be " + str(timeout) + " seconds") - # Test the connection by reading the baudrate - attepmpts = 0 + 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)) From 9a6229656b7042c0cda0c174712a2f6e07b8c515 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 27 Jun 2016 07:05:17 -0700 Subject: [PATCH 079/108] Improved initial connection loop in arduino_driver.py --- .../src/ros_arduino_python/arduino_driver.py | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) 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 91c3c238..b77f3c09 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -72,36 +72,33 @@ def connect(self): while not self.serial_port.isOpen(): time.sleep(self.timeout) - # Now open the port with the real settings - self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) + # 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 - # Wake up the serial port - self.serial_port.write('\r\r\r') - self.serial_port.read() + self.serial_port.write('\r') + time.sleep(timeout) + test = self.serial_port.read() - # Keep trying for max_attempts - while attempts < max_attempts: + # Wake up the serial port + while attempts < max_attempts and test == '': attempts += 1 - self.serial_port.write('\r\r\r') + self.serial_port.write('\r') + time.sleep(timeout) test = self.serial_port.read() - if test != '': - break - rospy.loginfo("Waking up serial port attempt " + str(attempts) + " of " + str(max_attempts)) - # Increase timeout by 10% - timeout *= 1.1 - self.serial_port.timeout = timeout - self.serial_port.writeTimeout = timeout - if test == '': raise SerialException + # Reset the timeout to the user specified timeout + self.serial_port.setTimeout(self.timeout) + self.serial_port.setWriteTimeout(self.timeout) + # Test the connection by reading the baudrate attempts = 0 while self.get_baud() != self.baudrate and attempts < max_attempts: From 9bcf7fff889ce20d698700d2f001f428bec4a9f6 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 05:38:44 -0700 Subject: [PATCH 080/108] Restored ROSArduinoBridge.ino default values --- .../src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino index 79ade3ce..712aa441 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino @@ -52,13 +52,13 @@ /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE /* The Pololu VNH5019 dual motor driver shield */ - //#define POLOLU_VNH5019 + #define POLOLU_VNH5019 /* The Pololu MC33926 dual motor driver shield */ //#define POLOLU_MC33926 /* The Adafruit motor shield V2 */ - #define ADAFRUIT_MOTOR_SHIELD_V2 + //#define ADAFRUIT_MOTOR_SHIELD_V2 /* The Ardunino Motor Shield R3 */ //#define ARDUINO_MOTOR_SHIELD_R3 @@ -73,10 +73,10 @@ // #define NO_MOTOR_CONTROLLER /* The RoboGaia encoder shield */ - //#define ROBOGAIA + #define ROBOGAIA /* The RoboGaia 3-axis encoder shield */ - #define ROBOGAIA_3_AXIS + //#define ROBOGAIA_3_AXIS /* Encoders directly attached to Arduino board */ //#define ARDUINO_ENC_COUNTER From b8931b271d36487a7b2fb2504893b0c92e1b2f61 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 05:40:22 -0700 Subject: [PATCH 081/108] Added fusionGetOrientation function to Adafruit 9-dof imu in imu.ino --- .../src/libraries/ROSArduinoBridge/imu.ino | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino index 13bd8110..c7f77688 100644 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino @@ -43,21 +43,30 @@ data.gx = event.gyro.x; data.gy = event.gyro.y; data.gz = event.gyro.z; - +/* if (dof.accelGetOrientation(&accel_event, &orientation)) { - /* 'orientation' should have valid .roll and .pitch fields */ 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)) { - /* 'orientation' should have valid .heading data now */ 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; } From dd58da63d13eae52712adff4beceecd37005fcf9 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 05:41:23 -0700 Subject: [PATCH 082/108] Fixed comment in sensors.h --- ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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); From d0a61ed5723b8a9fac71c51cb264227bc3500706 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 05:45:30 -0700 Subject: [PATCH 083/108] Updated servo_example_params.yaml --- .../config/servo_example_params.yaml | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/ros_arduino_python/config/servo_example_params.yaml b/ros_arduino_python/config/servo_example_params.yaml index c2504f50..73409188 100644 --- a/ros_arduino_python/config/servo_example_params.yaml +++ b/ros_arduino_python/config/servo_example_params.yaml @@ -2,24 +2,14 @@ port: /dev/arduino baud: 57600 -timeout: 0.1 +timeout: 0.5 -rate: 50 -sensorstate_rate: 10 +rate: 30 +sensorstate_rate: 10 use_base_controller: False - joint_update_rate: 10 -# === Sensor definitions. Examples only - edit for your robot. -# Sensor type can be one of the follow (case sensitive!): -# * Analog -# * Digital -# * Ping -# * GP2D12 -# * PololuMotorCurrent -# * PhidgetsVoltage -# * PhidgetsCurrent (20 Amp, DC) sensors: { onboard_led: {pin: 13, type: Digital, direction: output, rate: 1} @@ -27,5 +17,5 @@ sensors: { # Joint name and pin assignment is an example only joints: { - head_pan_joint: {pin: 3, init_position: 30, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False} + head_pan_joint: {pin: 3, init_position: 0, init_speed: 60, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False} } From 0fcdcd02e560ef28c3505250c8f64382844c0f86 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 05:49:12 -0700 Subject: [PATCH 084/108] Updated arduino_node.py including adding a diagnostics rate parameters --- ros_arduino_python/nodes/arduino_node.py | 237 ++++++++++++++++------- 1 file changed, 162 insertions(+), 75 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 67b84dec..4b98862e 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -23,24 +23,26 @@ 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 dynamic_reconfigure.server import Server import dynamic_reconfigure.client 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.follow_controller import FollowController from ros_arduino_python.joint_state_publisher import JointStatePublisher from geometry_msgs.msg import Twist from std_srvs.srv import Empty, EmptyResponse -import os, time -import thread +import os, time, thread from math import radians +from serial.serialutil import SerialException -#controller_types = { "follow_controller" : FollowController } +controller_types = { "follow_controller" : FollowController } class ArduinoROS(): def __init__(self): - rospy.init_node('arduino', log_level=rospy.DEBUG) + self.name = 'arduino' + rospy.init_node(self.name, log_level=rospy.INFO) # Cleanup when termniating the node rospy.on_shutdown(self.shutdown) @@ -51,15 +53,22 @@ def __init__(self): self.base_frame = rospy.get_param("~base_frame", 'base_link') # Overall loop rate: should be faster than fastest sensor rate - self.rate = int(rospy.get_param("~rate", 50)) + 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. 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 @@ -111,31 +120,41 @@ def __init__(self): # A service to read the value of an analog sensor rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler) - # A service to reset the odometry values to 0 - rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler) + # Initialize the device + self.device = Arduino(self.port, self.baud, self.timeout, debug=True) - # A service to update the PID parameters Kp, Kd, Ki, and Ko - rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler) + # Make the connection + 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) + #rospy.signal_shutdown("No serial connection found.") + # Initialize the base controller if used + if self.use_base_controller: + self.base_controller = BaseController(self.device, self.base_frame) + + # 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 = Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback) # Connect to the dynamic_reconfigure client dyn_client = dynamic_reconfigure.client.Client("arduino", timeout=5) - - # Initialize the device - self.device = Arduino(self.port, self.baud, self.timeout) - - # Make the connection - self.device.connect() - - rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") # Reserve a thread lock mutex = thread.allocate_lock() # Initialize any sensors - self.sensors = 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({})) @@ -144,12 +163,14 @@ def __init__(self): for name, params in sensor_params.iteritems(): if params['type'].lower() == 'Ping'.lower(): sensor = Ping(self.device, name, **params) - elif params['type'].lower() == 'GP2D12'.lower(): + 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(): @@ -158,12 +179,28 @@ def __init__(self): 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.sensors.append(sensor) + 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) @@ -173,9 +210,9 @@ def __init__(self): else: rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write") - - # Initialize any joints (servos) - self.device.joints = dict() + # 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()) @@ -183,13 +220,17 @@ def __init__(self): 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) + self.device.joints[name] = Servo(self.device, name) except: self.device.joints[name] = Servo(self.device, name) @@ -202,64 +243,87 @@ def __init__(self): # The joint state publisher publishes the latest joint values on the /joint_states topic self.joint_state_publisher = JointStatePublisher() -# # 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() - - # Initialize the base controller if used - if self.use_base_controller: - self.base_controller = BaseController(self.device, self.base_frame) + # 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(): - # Heartbeat test for the serial connection + # 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() - while True: - try: - self.device.open() - rospy.loginfo("Serial connection re-established.") - break - except: - r.sleep() - continue - - for sensor in self.sensors: + 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: - mutex.acquire() - sensor.poll() - mutex.release() - + with mutex: + sensor.poll() + + # Poll the base controller if self.use_base_controller: - mutex.acquire() - self.base_controller.poll() - mutex.release() + with mutex: + self.base_controller.poll() + # Poll any joints if self.have_joints: - mutex.acquire() - self.servo_controller.poll() - self.joint_state_publisher.poll(self.device.joints.values()) - mutex.release() - + 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() @@ -267,16 +331,19 @@ def __init__(self): msg = SensorState() msg.header.frame_id = self.base_frame msg.header.stamp = now - for i in range(len(self.sensors)): - if self.sensors[i].message_type != MessageType.IMU: - msg.name.append(self.sensors[i].name) - msg.value.append(self.sensors[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() @@ -342,6 +409,8 @@ def AnalogReadHandler(self, req): 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() @@ -373,6 +442,12 @@ def dynamic_reconfigure_server_callback(self, config, level): 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)) @@ -383,7 +458,7 @@ def dynamic_reconfigure_server_callback(self, config, level): def shutdown(self): rospy.loginfo("Shutting down Arduino node...") - + # Stop the robot if self.use_base_controller: rospy.loginfo("Stopping the robot...") @@ -396,6 +471,18 @@ def shutdown(self): for joint in self.device.joints.values(): self.device.detach_servo(joint.pin) rospy.sleep(0.2) + + # Close the serial port + #self.device.serial_port.close() if __name__ == '__main__': - myArduino = ArduinoROS() + try: + myArduino = ArduinoROS() + except KeyboardInterrupt: + try: + myArduino.device.serial_port.close() + except: + os._exit(0) + except SerialException: + os._exit(0) + From 8595db70b27d6c4bb4c0fc8baca01eeccaab2e8c Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 05:52:13 -0700 Subject: [PATCH 085/108] Added diagnostics dependencies to package.xml --- ros_arduino_python/package.xml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ros_arduino_python/package.xml b/ros_arduino_python/package.xml index 391d96e2..dc3c0c76 100644 --- a/ros_arduino_python/package.xml +++ b/ros_arduino_python/package.xml @@ -12,12 +12,19 @@ catkin dynamic_reconfigure + diagnostic_updater + diagnostic_aggregator + diagnostic_msgs rospy std_msgs sensor_msgs geometry_msgs nav_msgs + diagnostic_msgs + diagnostic_updater + diagnostic_updater + diagnostic_aggregator tf ros_arduino_msgs python-serial From e7bfde4b80965e17f7c4003205c06727818309da Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 06:03:54 -0700 Subject: [PATCH 086/108] Added diagnostics, IMU and Gyro support to arduino_sensors --- .../src/ros_arduino_python/arduino_sensors.py | 341 ++++++++++++++---- 1 file changed, 264 insertions(+), 77 deletions(-) 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 89fa4a9a..83e38757 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -21,10 +21,14 @@ import rospy from sensor_msgs.msg import Range, Imu +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 @@ -50,33 +54,86 @@ def __init__(self, device, name, pin=None, rate=0, direction="input", frame_id=" 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 - + + # 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": - self.value = self.read_value() - else: - self.ack = self.write_value() - - # For range sensors, assign the value to the range message field - if self.message_type == MessageType.RANGE: - self.msg.range = self.value - elif self.message_type != MessageType.IMU: - self.msg.value = self.value - - # Add a timestamp and publish the message - self.msg.header.stamp = rospy.Time.now() + # 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.pub.publish(self.msg) - except: + 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): @@ -87,38 +144,34 @@ def __init__(self, *args, **kwargs): self.msg = Analog() self.msg.header.frame_id = self.frame_id - - # Create the publisher - if self.rate != 0: - self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) + def create_publisher(self): + self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) + + def create_services(self): if self.direction == "output": self.device.analog_pin_mode(self.pin, OUTPUT) - # Create the write service rospy.Service('~' + self.name + '/write', AnalogSensorWrite, self.sensor_write_handler) else: self.device.analog_pin_mode(self.pin, INPUT) - # Create the read service rospy.Service('~' + self.name + '/read', AnalogSensorRead, self.sensor_read_handler) - self.value = LOW - def read_value(self): - return self.device.analog_read(self.pin) - + return self.scale * (self.device.analog_read(self.pin) - self.offset) + def write_value(self, value): 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(Sensor): +class AnalogFloatSensor(AnalogSensor): def __init__(self, *args, **kwargs): super(AnalogFloatSensor, self).__init__(*args, **kwargs) @@ -126,24 +179,20 @@ def __init__(self, *args, **kwargs): self.msg = AnalogFloat() self.msg.header.frame_id = self.frame_id - - # Create the publisher - if self.rate != 0: - self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) + def create_publisher(self): + self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) + + def create_services(self): if self.direction == "output": self.device.analog_pin_mode(self.pin, OUTPUT) - # Create the write service rospy.Service('~' + self.name + '/write', AnalogFloatSensorWrite, self.sensor_write_handler) else: self.device.analog_pin_mode(self.pin, INPUT) - # Create the read service rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler) - self.value = LOW - def read_value(self): - return self.device.analog_read(self.pin) + return self.scale * (self.device.analog_read(self.pin) - self.offset) def write_value(self, value): return self.device.analog_write(self.pin, value) @@ -166,25 +215,20 @@ def __init__(self, *args, **kwargs): self.msg = Digital() self.msg.header.frame_id = self.frame_id - # Create the publisher - if self.rate != 0: - self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) - # 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.device.digital_pin_mode(self.pin, OUTPUT) - - # Create the write service rospy.Service('~' + self.name + '/write', DigitalSensorWrite, self.sensor_write_handler) else: self.device.digital_pin_mode(self.pin, INPUT) - - # Create the read service - rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) + rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) - def read_value(self): return self.device.digital_read(self.pin) @@ -215,13 +259,18 @@ def __init__(self, *args, **kwargs): self.msg = Range() self.msg.header.frame_id = self.frame_id - # Create the publisher - if self.rate != 0: - self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) - - # Create the read service + def create_publisher(self): + self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) + + 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) @@ -242,7 +291,7 @@ 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 @@ -256,10 +305,11 @@ def read_value(self): 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 @@ -271,8 +321,8 @@ def read_value(self): return float('NaN') try: - distance = pow(4187.8 / value, 1.106) - #distance = (6787.0 / (float(value) - 3.0)) - 4.0 + #distance = pow(4187.8 / value, 1.106) + distance = (6787.0 / (float(value) - 3.0)) - 4.0 except: return float('NaN') @@ -284,14 +334,13 @@ def read_value(self): if distance < self.msg.min_range: distance = float('NaN') return distance - + class IMU(Sensor): - def __init__(self,*args, **kwargs): + def __init__(self, *args, **kwargs): super(IMU, self).__init__(*args, **kwargs) self.message_type = MessageType.IMU - - self.rpy = None + self.direction = "input" self.msg = Imu() self.msg.header.frame_id = self.frame_id @@ -300,9 +349,8 @@ def __init__(self,*args, **kwargs): 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] - # Create the publisher - if self.rate != 0: - self.pub = rospy.Publisher("~sensor/" + self.name, Imu, queue_size=5) + def create_publisher(self): + self.pub = rospy.Publisher("~sensor/" + self.name, Imu, queue_size=5) def read_value(self): ''' @@ -325,16 +373,16 @@ def read_value(self): roll = radians(roll) pitch = -radians(pitch) - self.msg.linear_acceleration.x = radians(ax) - self.msg.linear_acceleration.y = radians(ay) - self.msg.linear_acceleration.z = radians(az) + self.msg.linear_acceleration.x = ax + self.msg.linear_acceleration.y = ay + self.msg.linear_acceleration.z = az self.msg.angular_velocity.x = radians(gx) self.msg.angular_velocity.y = radians(gy) self.msg.angular_velocity.z = radians(gz) if ch != -999: - yaw = -radians(uh) + yaw = -radians(ch) else: yaw = -radians(mz) @@ -342,24 +390,164 @@ def read_value(self): return data + def publish_message(self): + self.read_value() + self.msg.header.stamp = rospy.Time.now() + self.pub.publish(self.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.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.device.analog_read(self.pin) - 500.) return voltage - + class PhidgetsCurrent(AnalogFloatSensor): def __init__(self, *args, **kwargs): super(PhidgetsCurrent, self).__init__(*args, **kwargs) @@ -368,20 +556,19 @@ def read_value(self): # From the Phidgets documentation for the 20 amp DC sensor 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.device.get_MaxEZ1(self.trigger_pin, self.output_pin) - From 9d0ed30c9c6cccc75596a84a1f97b039e1be8abd Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 06:06:53 -0700 Subject: [PATCH 087/108] Added diagnostics and default covariance values to base controller module --- .../src/ros_arduino_python/base_controller.py | 64 +++++++++++++------ 1 file changed, 46 insertions(+), 18 deletions(-) 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 43bd46df..0fa0c124 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -22,18 +22,20 @@ """ 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 sys import float_info +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) @@ -41,7 +43,9 @@ def __init__(self, arduino, base_frame): 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", "") @@ -55,19 +59,28 @@ def __init__(self, arduino, base_frame): self.accel_limit = rospy.get_param('~accel_limit', 1.0) self.motors_reversed = rospy.get_param("~motors_reversed", False) - + + # 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() self.then = now # time for determining dx/dy self.t_delta = rospy.Duration(1.0 / self.rate) @@ -87,14 +100,14 @@ 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") @@ -119,16 +132,22 @@ def setup_pid(self, pid_params): self.Ki = pid_params['Ki'] self.Ko = pid_params['Ko'] - 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)) + 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 @@ -190,8 +209,12 @@ 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.hpp at: + 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 @@ -200,11 +223,15 @@ def poll(self): """ odom.pose.covariance[0] = 0.1 odom.pose.covariance[7] = 0.1 - odom.pose.covariance[35] = 0.05 if self.use_imu_heading else 0.2 + 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] = 1e6 # set a non-zero covariance on unused - odom.pose.covariance[21] = 1e6 # dimensions (z, pitch and roll); this - odom.pose.covariance[28] = 1e6 # is a requirement of robot_pose_ekf + 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) @@ -266,4 +293,5 @@ def reset_odometry(self): self.x = 0.0 self.y = 0.0 self.th = 0.0 + From 0a2877a6b89328e1c16bd51885e39bfba169a272 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 06:10:16 -0700 Subject: [PATCH 088/108] Removed getDiagnostics from controllers.py --- .../src/ros_arduino_python/controllers.py | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/controllers.py b/ros_arduino_python/src/ros_arduino_python/controllers.py index 96f191a3..61302505 100644 --- a/ros_arduino_python/src/ros_arduino_python/controllers.py +++ b/ros_arduino_python/src/ros_arduino_python/controllers.py @@ -61,13 +61,4 @@ def shutdown(self): ## @brief Is the controller actively sending commands to joints? def active(self): return False - - ## @brief Get a diagnostics message for this joint. - ## - ## @return Diagnostics message. - def getDiagnostics(self): - msg = DiagnosticStatus() - msg.name = self.name - msg.level = DiagnosticStatus.OK - msg.message = "OK" - return msg + From ba4ded38b4b381de84a726959a87e6c75138f027 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 06:20:15 -0700 Subject: [PATCH 089/108] Added joint_speed_scale_correction parameter for servos and corrected interplated position function --- .../ros_arduino_python/servo_controller.py | 319 ++++++++++++++---- 1 file changed, 248 insertions(+), 71 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index 3ba4cc7b..c1aa7992 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -23,21 +23,46 @@ 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 +from math import radians, degrees, copysign class Joint: def __init__(self, device, name): self.device = device self.name = name - self.controller = None - self.target_position = 0.0 # radians self.position = 0.0 # radians + self.start_position = 0.0 + self.goal_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) @@ -50,138 +75,290 @@ def __init__(self, device, name, ns="~joints"): # 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 - - # Conversion factors to compute servo speed to step delay between updates - self.max_rad_per_sec = radians(60.0) / self.rated_speed - self.ms_per_rad = 1000.0 / self.max_rad_per_sec - - # Convert initial servo speed in deg/s to a step delay in milliseconds - step_delay = self.get_step_delay(radians(rospy.get_param(namespace + "init_speed", 60.0))) - + 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.config_servo(self.pin, step_delay) + self.device.set_servo_delay(self.pin, step_delay) # Min/max/neutral values - self.neutral = radians(rospy.get_param(namespace + "neutral", 90.0)) # degrees - self.max_position = radians(rospy.get_param(namespace + "max_position", 90.0)) # degrees - self.min_position = radians(rospy.get_param(namespace + "min_position", -90.0)) # degrees - self.range = radians(rospy.get_param(namespace + "range", 180)) # degrees - self.max_speed = radians(rospy.get_param(namespace + "max_speed", 250.0)) # deg/s + 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) + self.invert = rospy.get_param(namespace + 'invert', False) - # The desired position of the servo - self.desired = self.neutral + radians(rospy.get_param(namespace + "init_position", 0)) - + # 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 = 0.0 - + #self.position = self.desired + # Subscribe to the servo's command topic for setting its position - rospy.Subscriber("/" + name + '/command', Float64, self.command_cb) - + 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, req): + def command_cb(self, msg): # Check limits - if req.data > self.max_position: - req.data = self.max_position - - if req.data < self.min_position: - req.data = self.min_position - + 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 - req.data + target_adjusted = self.neutral - msg.data else: - target_adjusted = self.neutral + req.data + 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() + + # Record the goal_postion + self.goal_position = msg.data # Set the target position for the next servo controller update - self.desired = target_adjusted - + 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.max_rad_per_sec: + + 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 / degrees(1.0) * (1.0 / target_speed - 1.0 / self.max_rad_per_sec) + 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 + 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) - - return EnableResponse() - + 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 ServoController(Controller): - def __init__(self, device, name): - Controller.__init__(self, device, name) +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) + +# def command_cb(self, msg): +# # Check limits +# if msg.data > self.max_speed: +# msg.data = self.max_speed +# +# if msg.data < self.min_speed: +# msg.data = self.min_speed +# +# # Adjust for the neutral offset +# if self.invert: +# servo_ticks = self.neutral - msg.data * self.ticks_per_rad_per_sec +# else: +# servo_ticks = self.neutral + msg.data * self.ticks_per_rad_per_sec +# +# # Set the target speed for the next servo controller update +# self.desired = radians(servo_ticks) + + +class ServoController(): + def __init__(self, device, name): + self.name = name + self.device = device self.servos = list() - - joint_update_rate = rospy.get_param("~joint_update_rate", 10.0) # 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.w_delta = rospy.Duration(1.0 / joint_update_rate) - self.w_next = rospy.Time.now() + self.w_delta - - self.r_delta = rospy.Duration(1.0 / joint_update_rate) - self.r_next = rospy.Time.now() + self.r_delta + 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.r_next: + if rospy.Time.now() > self.next_update: for servo in self.servos: - servo.position = servo.get_current_position() - - # Compute velocity - servo.velocity = (servo.position - servo.position_last) / self.r_delta.to_sec() - servo.position_last = servo.position - - self.r_next = rospy.Time.now() + self.r_delta + + # 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 + 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 - if rospy.Time.now() > self.w_next: - for servo in self.servos: - self.device.servo_write(servo.pin, degrees(servo.desired)) - self.w_next = rospy.Time.now() + self.w_delta From 9fe0923fa21c0783309def071530cdb56670c452 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 06:22:26 -0700 Subject: [PATCH 090/108] Added diagnostcs and follow_controller modules --- .../src/ros_arduino_python/diagnostics.py | 135 +++++++++++++++ .../ros_arduino_python/follow_controller.py | 161 ++++++++++++++++++ 2 files changed, 296 insertions(+) create mode 100644 ros_arduino_python/src/ros_arduino_python/diagnostics.py create mode 100644 ros_arduino_python/src/ros_arduino_python/follow_controller.py 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..94cc1fe3 --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/diagnostics.py @@ -0,0 +1,135 @@ +#!/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: + 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..1999d4e9 --- /dev/null +++ b/ros_arduino_python/src/ros_arduino_python/follow_controller.py @@ -0,0 +1,161 @@ +#!/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() + + 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]].desired = last[i] + else: + 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 From be4bb3e219794c591134611f46bd3ac82db8b982 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 06:23:07 -0700 Subject: [PATCH 091/108] Minor edit to arduino_python node --- ros_arduino_python/nodes/arduino_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 4b98862e..f38c42e7 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -24,13 +24,13 @@ from ros_arduino_python.arduino_sensors import * from ros_arduino_msgs.srv import * from ros_arduino_python.diagnostics import DiagnosticsUpdater, DiagnosticsPublisher -from dynamic_reconfigure.server import Server -import dynamic_reconfigure.client 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 +from dynamic_reconfigure.server import Server +import dynamic_reconfigure.client from geometry_msgs.msg import Twist from std_srvs.srv import Empty, EmptyResponse import os, time, thread From f5b825c8ee082fd45028080df19285b3333760b1 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 13 Sep 2016 07:46:23 -0700 Subject: [PATCH 092/108] Tweaked diagnostics.py so that rqt_robot_monitor does not complain about missing joint frequencies --- .../src/ros_arduino_python/diagnostics.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/diagnostics.py b/ros_arduino_python/src/ros_arduino_python/diagnostics.py index 94cc1fe3..28450d95 100644 --- a/ros_arduino_python/src/ros_arduino_python/diagnostics.py +++ b/ros_arduino_python/src/ros_arduino_python/diagnostics.py @@ -69,12 +69,14 @@ def __init__(self, component, name, error_threshold=10, rate=1.0, create_watchdo 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 + # TODO: Need to add frequency check for servo controller (joint_update_rate) if self.component.rate > 0: - 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) - + if "Servo" not in str(self.component): + 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) From 5f35efe852cfdd3d9fd970902f6bb32d30de11aa Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 13 Sep 2016 07:58:33 -0700 Subject: [PATCH 093/108] Undid last commit since it introduced another bug... --- ros_arduino_python/src/ros_arduino_python/diagnostics.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/diagnostics.py b/ros_arduino_python/src/ros_arduino_python/diagnostics.py index 28450d95..182bff20 100644 --- a/ros_arduino_python/src/ros_arduino_python/diagnostics.py +++ b/ros_arduino_python/src/ros_arduino_python/diagnostics.py @@ -71,11 +71,13 @@ def __init__(self, component, name, error_threshold=10, rate=1.0, create_watchdo self.diag_updater.setHardwareID(self.name) # Create a frequency monitor that tracks the publishing frequency for this component - # TODO: Need to add frequency check for servo controller (joint_update_rate) if self.component.rate > 0: - if "Servo" not in str(self.component): - freq_bounds = diagnostic_updater.FrequencyStatusParam({'min': self.component.rate, 'max': self.component.rate}, 0.3, 5) + 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) From 7e844b5991c08a92cfc6d4632489602d93855f08 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 2 Oct 2016 08:29:52 -0700 Subject: [PATCH 094/108] Removed harmless extra space --- ros_arduino_python/src/ros_arduino_python/servo_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index c1aa7992..b76bf5ae 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -87,7 +87,7 @@ def __init__(self, device, name, ns="~joints"): 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.servo_speed = radians(rospy.get_param(namespace + 'init_speed', 60.0)) self.direction = copysign(1, -self.servo_speed) From 44137ce34c78abead883b54d4d15f1936bc897d0 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 6 Oct 2016 05:29:24 -0700 Subject: [PATCH 095/108] Increased default timeout from 0.1 to 0.5 seconds --- ros_arduino_python/config/arduino_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_arduino_python/config/arduino_params.yaml b/ros_arduino_python/config/arduino_params.yaml index 1e0931b1..50e581ac 100644 --- a/ros_arduino_python/config/arduino_params.yaml +++ b/ros_arduino_python/config/arduino_params.yaml @@ -5,7 +5,7 @@ port: /dev/ttyACM0 baud: 57600 -timeout: 0.1 +timeout: 0.5 rate: 50 sensorstate_rate: 10 From aa8ba65e1ddccddfcec3f0111d6f61b242f407dd Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 6 Oct 2016 06:35:26 -0700 Subject: [PATCH 096/108] Tweaked namespace for dynamic reconfigure and base controller to allow for more than one Arduino simultaneously --- ros_arduino_python/nodes/arduino_node.py | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index f38c42e7..39982d3d 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -29,7 +29,7 @@ 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 -from dynamic_reconfigure.server import Server +import dynamic_reconfigure.server import dynamic_reconfigure.client from geometry_msgs.msg import Twist from std_srvs.srv import Empty, EmptyResponse @@ -41,12 +41,14 @@ class ArduinoROS(): def __init__(self): - self.name = 'arduino' - rospy.init_node(self.name, log_level=rospy.INFO) - + 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) @@ -129,11 +131,10 @@ def __init__(self): else: rospy.logerr("No serial connection found.") os._exit(0) - #rospy.signal_shutdown("No serial connection found.") # Initialize the base controller if used if self.use_base_controller: - self.base_controller = BaseController(self.device, self.base_frame) + 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) @@ -141,11 +142,11 @@ def __init__(self): # 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 = Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback) + # 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("arduino", timeout=5) + # Connect to the dynamic_reconfigure client + dyn_client = dynamic_reconfigure.client.Client(self.name, timeout=5) # Reserve a thread lock mutex = thread.allocate_lock() From d9968a7f4b324bac10169b1ef2b5557400d08129 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 1 Nov 2016 15:48:03 -0700 Subject: [PATCH 097/108] Added try-except around append(sensor) --- ros_arduino_python/nodes/arduino_node.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 39982d3d..97b93162 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -146,7 +146,7 @@ def __init__(self): 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) + dyn_client = dynamic_reconfigure.client.Client (self.name, timeout=5) # Reserve a thread lock mutex = thread.allocate_lock() @@ -201,15 +201,18 @@ def __init__(self): # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] - self.device.sensors.append(sensor) + 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") + if params['rate'] != None and params['rate'] != 0: + rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) else: - rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write") + 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: From 6d6390fa53db43544b5413f515d9f444c01d9d60 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sun, 6 Nov 2016 06:08:37 -0800 Subject: [PATCH 098/108] Finished tweaks on follow_controller.py to execute joint action trajectories --- .../ros_arduino_python/follow_controller.py | 10 ++++++- .../ros_arduino_python/servo_controller.py | 28 +++---------------- 2 files changed, 13 insertions(+), 25 deletions(-) diff --git a/ros_arduino_python/src/ros_arduino_python/follow_controller.py b/ros_arduino_python/src/ros_arduino_python/follow_controller.py index 1999d4e9..39968c9c 100644 --- a/ros_arduino_python/src/ros_arduino_python/follow_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/follow_controller.py @@ -61,7 +61,7 @@ def startup(self): 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: @@ -115,6 +115,10 @@ def execute_trajectory(self, traj): 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 ] @@ -138,8 +142,12 @@ def execute_trajectory(self, traj): 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 diff --git a/ros_arduino_python/src/ros_arduino_python/servo_controller.py b/ros_arduino_python/src/ros_arduino_python/servo_controller.py index b76bf5ae..6a0c638e 100755 --- a/ros_arduino_python/src/ros_arduino_python/servo_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/servo_controller.py @@ -36,7 +36,6 @@ def __init__(self, device, name): self.position = 0.0 # radians self.start_position = 0.0 - self.goal_position = 0.0 self.position_last = 0.0 # radians self.velocity = 0.0 # rad/s self.servo_speed = 0.0 # rad/s @@ -144,9 +143,6 @@ def command_cb(self, msg): # Record the starting position self.start_position = self.get_current_position() - # Record the goal_postion - self.goal_position = msg.data - # Set the target position for the next servo controller update self.desired = msg.data @@ -280,24 +276,6 @@ def command_cb(self, req): self.is_moving = True self.direction = copysign(1, self.desired - self.position) -# def command_cb(self, msg): -# # Check limits -# if msg.data > self.max_speed: -# msg.data = self.max_speed -# -# if msg.data < self.min_speed: -# msg.data = self.min_speed -# -# # Adjust for the neutral offset -# if self.invert: -# servo_ticks = self.neutral - msg.data * self.ticks_per_rad_per_sec -# else: -# servo_ticks = self.neutral + msg.data * self.ticks_per_rad_per_sec -# -# # Set the target speed for the next servo controller update -# self.desired = radians(servo_ticks) - - class ServoController(): def __init__(self, device, name): self.name = name @@ -317,6 +295,7 @@ def poll(self): 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: @@ -324,11 +303,12 @@ def poll(self): 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: @@ -337,7 +317,7 @@ def poll(self): # 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 From c15c0be2a84546683631e1ecf0bf88d1f029bb52 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 17 Nov 2016 06:00:47 -0800 Subject: [PATCH 099/108] Close serial port on exit --- ros_arduino_python/nodes/arduino_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 97b93162..e2749a36 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -477,7 +477,7 @@ def shutdown(self): rospy.sleep(0.2) # Close the serial port - #self.device.serial_port.close() + self.device.close() if __name__ == '__main__': try: @@ -486,7 +486,9 @@ def shutdown(self): 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) From 9d29c18769c438492106ce08cae856363c76137d Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 22 Nov 2016 06:59:36 -0800 Subject: [PATCH 100/108] Removed duplicate line in package.xml --- ros_arduino_python/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/ros_arduino_python/package.xml b/ros_arduino_python/package.xml index dc3c0c76..c1508eda 100644 --- a/ros_arduino_python/package.xml +++ b/ros_arduino_python/package.xml @@ -23,7 +23,6 @@ nav_msgs diagnostic_msgs diagnostic_updater - diagnostic_updater diagnostic_aggregator tf ros_arduino_msgs From 6c77321d660d57e370d9329193732d693fa8faad Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Tue, 22 Nov 2016 07:05:12 -0800 Subject: [PATCH 101/108] Added optional encoder jump detection --- .../src/ros_arduino_python/base_controller.py | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) 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 0fa0c124..0ef3db28 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -59,6 +59,8 @@ def __init__(self, arduino, base_frame, name='base_controller'): 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) @@ -152,6 +154,30 @@ def poll(self): 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() From 8da758f29bdeea914b13010dde3c7596849d34ba Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 1 Dec 2016 06:23:33 -0800 Subject: [PATCH 102/108] Added ROS dependencies to README --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index bb35f559..f129e6cb 100644 --- a/README.md +++ b/README.md @@ -64,6 +64,9 @@ 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 From b56f3d7e657742da3efd0bc5a198a7c0bbc2a3b8 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Thu, 1 Dec 2016 06:24:24 -0800 Subject: [PATCH 103/108] Added ROS dependencies to README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index f129e6cb..d2ca0d5a 100644 --- a/README.md +++ b/README.md @@ -65,6 +65,7 @@ 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: From dd403d06fe37b7adabe9c3631437a5509b9de62a Mon Sep 17 00:00:00 2001 From: Chaoyang Liu Date: Tue, 15 Aug 2017 15:50:54 +0800 Subject: [PATCH 104/108] Add gitignore file To ignore files generated by PVM like pyc and some others generated temporarily by vim or other editors. --- .gitignore | 106 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 .gitignore 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 +*~ From 7810b422fd3c127f67c31563bdda4014f291dd8f Mon Sep 17 00:00:00 2001 From: Chaoyang Liu Date: Tue, 15 Aug 2017 18:27:47 +0800 Subject: [PATCH 105/108] Fix #42 issue Update method to set serial read-timeout and write-timeout. In pyserial v3.x, using setTimeout and setWriteTimeout to set read or write timeout is not supported. The newest to set them is by attributes "timeout" and "write_timeout". --- .../src/ros_arduino_python/arduino_driver.py | 85 +++++++++---------- 1 file changed, 42 insertions(+), 43 deletions(-) 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 b77f3c09..5719bccd 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -3,7 +3,7 @@ """ 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. @@ -11,12 +11,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 """ @@ -47,10 +47,10 @@ def __str__(self): class Arduino: def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, 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 @@ -64,10 +64,10 @@ def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, debug=False 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) @@ -96,8 +96,8 @@ def connect(self): raise SerialException # Reset the timeout to the user specified timeout - self.serial_port.setTimeout(self.timeout) - self.serial_port.setWriteTimeout(self.timeout) + self.serial_port.timeout = self.timeout + self.serial_port.write_timeout = self.timeout # Test the connection by reading the baudrate attempts = 0 @@ -124,16 +124,16 @@ def connect(self): return True - def open(self): + def open(self): ''' Open the serial port. ''' self.serial_port.open() - def close(self): + def close(self): ''' Close the serial port. ''' - self.serial_port.close() - + 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. @@ -201,7 +201,7 @@ def execute_ack(self, cmd): 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 ''' @@ -234,11 +234,11 @@ def reset_encoders(self): 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 + 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') @@ -252,7 +252,7 @@ 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. ''' @@ -263,36 +263,36 @@ def drive_m_per_s(self, right, left): 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)) @@ -302,13 +302,13 @@ def servo_write(self, 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. @@ -318,22 +318,22 @@ def set_servo_delay(self, 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 @@ -344,8 +344,8 @@ def ping(self, pin): # (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)) - +# return self.execute('z %d %d' %(triggerPin, outputPin)) + """ Basic test for connectivity """ if __name__ == "__main__": @@ -353,15 +353,15 @@ def ping(self, pin): 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) - + 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" @@ -369,11 +369,10 @@ def ping(self, pin): 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." - From ffccb9457dd8aea5a78615d086ef43f08481c0ea Mon Sep 17 00:00:00 2001 From: Chaoyang Liu Date: Sun, 20 Aug 2017 21:37:02 +0800 Subject: [PATCH 106/108] Make parameter 'motors_reversed' take effect --- ros_arduino_python/nodes/arduino_node.py | 155 +++++++++--------- .../src/ros_arduino_python/arduino_driver.py | 8 +- .../src/ros_arduino_python/base_controller.py | 76 +++++---- 3 files changed, 120 insertions(+), 119 deletions(-) diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index e2749a36..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 """ @@ -53,77 +53,77 @@ def __init__(self): 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", 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 device - self.device = Arduino(self.port, self.baud, self.timeout, debug=True) + self.device = Arduino(self.port, self.baud, self.timeout, self.motors_reversed, debug=True) # Make the connection if self.device.connect(): @@ -135,19 +135,19 @@ def __init__(self): # 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() @@ -196,14 +196,14 @@ def __init__(self): 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'] 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: @@ -217,17 +217,17 @@ def __init__(self): # 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) + + # 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: @@ -240,24 +240,24 @@ def __init__(self): # 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) @@ -265,14 +265,14 @@ def __init__(self): except Exception as e: if type(e) == KeyError: rospy.logerr("Unrecognized controller: " + params["type"]) - else: + 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(): # Heartbeat/watchdog test for the serial connection @@ -310,27 +310,27 @@ def __init__(self): 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: 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 @@ -343,69 +343,69 @@ def __init__(self): 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.device.servo_write(req.id, req.position) return ServoWriteResponse() - + # def ServoSpeedWriteHandler(self, req): -# delay = +# 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): 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.device.digital_pin_mode(req.pin, req.direction) return DigitalSetDirectionResponse() - + def DigitalWriteHandler(self, req): self.device.digital_write(req.pin, req.value) return DigitalWriteResponse() - + def DigitalReadHandler(self, req): 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.device.analog_write(req.pin, req.value) return AnalogWriteResponse() - + def AnalogReadHandler(self, req): value = self.device.analog_read(req.pin) return AnalogReadResponse(value) @@ -448,7 +448,7 @@ def dynamic_reconfigure_server_callback(self, config, level): 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'] @@ -459,10 +459,10 @@ def dynamic_reconfigure_server_callback(self, config, level): print e return config - + def shutdown(self): rospy.loginfo("Shutting down Arduino node...") - + # Stop the robot if self.use_base_controller: rospy.loginfo("Stopping the robot...") @@ -475,10 +475,10 @@ def shutdown(self): 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__': try: myArduino = ArduinoROS() @@ -491,4 +491,3 @@ def shutdown(self): except SerialException: rospy.logerr("Serial exception trying to open port.") os._exit(0) - 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 5719bccd..b67599e7 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -46,14 +46,14 @@ def __str__(self): return repr(self.code) class Arduino: - def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, debug=False): + 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 @@ -224,6 +224,8 @@ def get_encoder_counts(self): 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): @@ -251,6 +253,8 @@ def get_imu_data(self): 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): 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 0ef3db28..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,12 +12,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 """ @@ -30,7 +30,7 @@ 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, name='base_controller'): @@ -46,7 +46,7 @@ def __init__(self, arduino, base_frame, name='base_controller'): self.stopped = False self.current_speed = Twist() - + pid_params = dict() pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") @@ -56,9 +56,9 @@ def __init__(self, arduino, base_frame, name='base_controller'): 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', 1.0) - self.motors_reversed = rospy.get_param("~motors_reversed", False) + # 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) @@ -83,12 +83,12 @@ def __init__(self, arduino, base_frame, name='base_controller'): # 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 @@ -112,7 +112,7 @@ def __init__(self, arduino, base_frame, name='base_controller'): 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 @@ -120,20 +120,20 @@ def setup_pid(self, pid_params): 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'] - + 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: @@ -181,7 +181,7 @@ def poll(self): dt = now - self.then self.then = now dt = dt.to_sec() - + # Calculate odometry if self.enc_left == None: dright = 0 @@ -192,37 +192,37 @@ def poll(self): self.enc_right = right_enc self.enc_left = left_enc - + 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. if self.publish_odom_base_transform: self.odomBroadcaster.sendTransform( - (self.x, self.y, 0), + (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 @@ -234,7 +234,7 @@ def poll(self): odom.twist.twist.linear.x = vxy 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 @@ -242,7 +242,7 @@ def poll(self): """ 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 @@ -254,17 +254,17 @@ def poll(self): 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: @@ -273,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: @@ -282,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 @@ -311,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 - - From 0c1c188a3ac2f22b80050d1078f62a8fb78ff025 Mon Sep 17 00:00:00 2001 From: Chaoyang Liu Date: Sat, 26 Aug 2017 15:16:16 +0800 Subject: [PATCH 107/108] Add parameters of real robot --- .../config/my_arduino_params.yaml | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 ros_arduino_python/config/my_arduino_params.yaml 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} +} From d2f6d00ed58b37a99a427e763e15712115e2a846 Mon Sep 17 00:00:00 2001 From: Chaoyang Liu Date: Thu, 31 Aug 2017 16:14:34 +0800 Subject: [PATCH 108/108] Add magnetic message and remove some useless infos * add magnetic message * remove roll, pitch, yaw --- .../src/ros_arduino_python/arduino_driver.py | 2 +- .../src/ros_arduino_python/arduino_sensors.py | 137 +++++++++--------- 2 files changed, 73 insertions(+), 66 deletions(-) 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 b67599e7..9d0527dc 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py @@ -245,7 +245,7 @@ def get_imu_data(self): ''' values = self.execute_array('i') - if len(values) != 12: + if len(values) != 9: return None else: return map(float, values) 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 83e38757..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,17 +10,17 @@ it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. - + This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details at: - + http://www.gnu.org/licenses/gpl.html """ import rospy -from sensor_msgs.msg import Range, Imu +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 @@ -44,7 +44,7 @@ class MessageType: INT = 4 BOOL = 5 IMU = 6 - + class Sensor(object): def __init__(self, device, name, pin=None, rate=0, direction="input", frame_id="base_link", **kwargs): self.device = device @@ -65,7 +65,7 @@ def __init__(self, device, name, pin=None, rate=0, direction="input", frame_id=" 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) + self.diagnostics = DiagnosticsUpdater(self, name + '_sensor', diagnotics_error_threshold, diagnostics_rate) # Initialize the component's value self.value = None @@ -112,7 +112,7 @@ def publish_message(self): 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: @@ -135,13 +135,13 @@ def poll(self): # 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 @@ -174,9 +174,9 @@ def sensor_write_handler(self, req): 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 @@ -193,25 +193,25 @@ def create_services(self): def read_value(self): return self.scale * (self.device.analog_read(self.pin) - self.offset) - + def write_value(self, 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 @@ -231,40 +231,40 @@ def create_services(self): def read_value(self): 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() - + 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 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 @@ -278,61 +278,61 @@ def sensor_read_handler(self, req=None): 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.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.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.09 self.msg.min_range = 0.10 self.msg.max_range = 0.80 - + def read_value(self): value = self.device.analog_read(self.pin) - + # The GP2D12 cannot provide a meaning result closer than 3 cm. if value <= 3.0: return float('NaN') - + try: #distance = pow(4187.8 / value, 1.106) distance = (6787.0 / (float(value) - 3.0)) - 4.0 except: 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 = float('NaN') if distance < self.msg.min_range: distance = float('NaN') - + return distance class IMU(Sensor): @@ -343,7 +343,9 @@ def __init__(self, *args, **kwargs): 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] @@ -351,6 +353,7 @@ def __init__(self, *args, **kwargs): 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): ''' @@ -359,41 +362,47 @@ def read_value(self): [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. + 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, roll, pitch, ch = data + 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) + ## roll = radians(roll) + ## pitch = -radians(pitch) - self.msg.linear_acceleration.x = ax - self.msg.linear_acceleration.y = ay - self.msg.linear_acceleration.z = az + self.msg.linear_acceleration.x = ay + self.msg.linear_acceleration.y = ax + self.msg.linear_acceleration.z = -az - self.msg.angular_velocity.x = radians(gx) - self.msg.angular_velocity.y = radians(gy) - self.msg.angular_velocity.z = radians(gz) + 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) + # 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): @@ -456,7 +465,7 @@ def read_value(self): # 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() @@ -551,7 +560,7 @@ def read_value(self): 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.device.analog_read(self.pin) - 500.) @@ -570,5 +579,3 @@ def __init__(self, *args, **kwargs): def read_value(self): return self.device.get_MaxEZ1(self.trigger_pin, self.output_pin) - -