ROSController.h

00001 /*
00002  * ROSController.h
00003  *
00004  * Author: Joseph DelPreto
00005  */
00006 
00007 //#define rosControl
00008 
00009 #ifdef rosControl
00010 
00011 #ifndef ROSCONTROL_ROSCONTROLLER_H_
00012 #define ROSCONTROL_ROSCONTROLLER_H_
00013 
00014 #include "FishController.h"
00015 #include "mbed.h"
00016 #include <ros.h>
00017 #include <std_msgs/Empty.h>
00018 #include <fish_msgs/JoystickState.h>
00019 
00020 // ROS setup
00021 // TODO replace this with your topic name
00022 #define rosTopicName "to_serial"
00023 
00024 // Pins and comm
00025 #define rosDefaultBaudUSB 115200
00026 #define rosDefaultBaud 115200
00027 #define rosDefaultTX p13
00028 #define rosDefaultRX p14
00029 // note lowBatteryVoltagePin is defined in FishController
00030 
00031 //#define printStatusROSController // whether to print what's going on (i.e. when it gets commands, etc.)
00032 #define debugLEDsROS      // LED1: initialized LED2: running LED3: receiving a message LED4: done (others turn off)
00033 #define runTimeROS 10000  // how long to run for (in milliseconds) if inifiniteLoopROS is undefined
00034 #define infiniteLoopROS   // if defined, will run forever (or until stop() is called from another thread)
00035 
00036 #define rosControllerControlFish // whether to start fishController to control the servos and motor
00037 
00038 // Map bytes sent over ROS serial to ranges for each fish property
00039 #define rosMinPitch     fishMinPitch
00040 #define rosMaxPitch     fishMaxPitch
00041 #define rosMinYaw       fishMinYaw
00042 #define rosMaxYaw       fishMaxYaw
00043 #define rosMinThrust    fishMinThrust
00044 #define rosMaxThrust    fishMaxThrust
00045 #define rosMinFrequency fishMinFrequency
00046 #define rosMaxFrequency fishMaxFrequency
00047 
00048 class ROSController
00049 {
00050     public:
00051         // Initialization
00052         ROSController(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if objects are null, ones will be created
00053         void init(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if serial objects are null, ones will be created
00054         // Execution control
00055         void run();
00056         void stop();
00057         void lowBatteryCallback();
00058         // ROS
00059         ros::NodeHandle nodeHandle;
00060         ros::Subscriber<fish_msgs::JoystickState> subscriber;
00061         void processROSMessage(const fish_msgs::JoystickState& msg);
00062     private:
00063         Timer programTimer;
00064         bool terminated;
00065         Serial* usbSerial;
00066         Serial* serial;
00067 
00068         // Low battery monitor
00069         DigitalIn* lowBatteryVoltageInput;
00070         Ticker lowBatteryTicker;
00071         bool detectedLowBattery;
00072 
00073         // Debug LEDs
00074         #ifdef debugLEDsROS
00075         DigitalOut* rosLEDs[4];
00076         #endif
00077 };
00078 
00079 // Create a static instance of ROSController to be used by anyone doing ROS control
00080 extern ROSController rosController;
00081 
00082 #endif /* ROSCONTROL_ROSCONTROLLER_H_ */
00083 
00084 #endif // #ifdef rosControl