ROSController.cpp

00001 /*
00002  * Author: Joseph DelPreto
00003  */
00004 
00005 #include "ROSController.h"
00006 
00007 #ifdef rosControl
00008 
00009 // The static instance
00010 ROSController rosController;
00011 
00012 // Define global methods that call methods of the singleton, to make callbacks easier
00013 // TODO find out how to get a function pointer to a member function of a specific object (so we won't need these methods)
00014 void processROSMessageStatic(const fish_msgs::JoystickState& msg)
00015 {
00016     rosController.processROSMessage(msg);
00017 }
00018 void lowBatteryCallbackROSStatic()
00019 {
00020     rosController.lowBatteryCallback();
00021 }
00022 
00023 // Initialization
00024 ROSController::ROSController(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */):
00025         subscriber(rosTopicName, &processROSMessageStatic),
00026         terminated(false)
00027 {
00028     #ifdef debugLEDsROS
00029     rosLEDs[0] = new DigitalOut(LED1);
00030     rosLEDs[1] = new DigitalOut(LED2);
00031     rosLEDs[2] = new DigitalOut(LED3);
00032     rosLEDs[3] = new DigitalOut(LED4);
00033     #endif
00034     init(serialObject, usbSerialObject);
00035 }
00036 
00037 void ROSController::init(Serial* serialObject /* = NULL */, Serial* usbSerialObject /* = NULL */)
00038 {
00039     // Create serial object or use provided one
00040     if(serialObject == NULL)
00041     {
00042         serialObject = new Serial(rosDefaultTX, rosDefaultRX);
00043         serialObject->baud(rosDefaultBaud);
00044     }
00045     serial = serialObject;
00046     // Create usb serial object or use provided one
00047     if(usbSerialObject == NULL)
00048     {
00049         usbSerialObject = new Serial(USBTX, USBRX);
00050         usbSerialObject->baud(rosDefaultBaudUSB);
00051     }
00052     usbSerial = usbSerialObject;
00053 
00054     // Will check for low battery at startup and using an interrupt
00055     lowBatteryVoltageInput = new DigitalIn(lowBatteryVoltagePin);
00056     lowBatteryVoltageInput->mode(PullUp);
00057     detectedLowBattery = false;
00058     lowBatteryTicker.attach(&lowBatteryCallbackROSStatic, 5);
00059 
00060     // ROS setup
00061     nodeHandle.initNode();
00062     nodeHandle.subscribe(subscriber);
00063 
00064     // Debug
00065     #ifdef debugLEDsROS
00066     rosLEDs[0]->write(1);
00067     rosLEDs[1]->write(0);
00068     rosLEDs[2]->write(0);
00069     rosLEDs[3]->write(0);
00070     #endif
00071 }
00072 
00073 // Process a received ROS message
00074 void ROSController::processROSMessage(const fish_msgs::JoystickState& msg)
00075 {
00076     #ifdef debugLEDsROS
00077     rosLEDs[2]->write(1);
00078     #endif
00079 
00080     // Extract the desired fish state from msg
00081     // TODO check that this matches your message format
00082     bool selectButton = 0;
00083     float pitch = ((msg.depth_ctrl+128) * (rosMaxPitch - rosMinPitch) / 255.0) + rosMinPitch;
00084     float yaw = ((msg.yaw_ctrl+127) * (rosMaxYaw - rosMinYaw) / 254.0) + rosMinYaw; // 0 MUST map to 0
00085     float thrust = ((msg.speed_ctrl+128) * (rosMaxThrust - rosMinThrust) / 255.0) + rosMinThrust;
00086     float frequency = ((msg.freq_ctrl+128) * (rosMaxFrequency- rosMinFrequency) / 255.0) + rosMinFrequency;
00087 
00088     // Apply the new state to the fish
00089     fishController.setSelectButton(selectButton);
00090     fishController.setPitch(pitch);
00091     fishController.setYaw(yaw);
00092     fishController.setThrust(thrust);
00093     fishController.setFrequency(frequency);
00094 
00095     #ifdef printStatusROSController
00096     usbSerial->printf("Start %d\t Pitch %f\t Yaw %f\t Thrust %f\t Freq %.8f\r\n", selectButton, pitch, yaw, thrust, frequency);
00097     #endif
00098     #ifdef debugLEDsROS
00099     rosLEDs[2]->write(0);
00100     #endif
00101 }
00102 
00103 // Stop the controller (will also stop the fish controller)
00104 //
00105 void ROSController::stop()
00106 {
00107     terminated = true;
00108 }
00109 
00110 // Main loop
00111 // This is blocking - will not return until terminated by timeout or by calling stop() in another thread
00112 void ROSController::run()
00113 {
00114 
00115     #ifdef rosControllerControlFish
00116     // Start the fish controller
00117     fishController.start();
00118     #endif
00119 
00120     #ifdef printStatusROSController
00121     usbSerial->printf("\r\nStarting to listen for ROS commands\r\n");
00122     #endif
00123 
00124     #ifdef debugLEDsROS
00125     rosLEDs[0]->write(1);
00126     rosLEDs[1]->write(1);
00127     rosLEDs[2]->write(0);
00128     rosLEDs[3]->write(0);
00129     #endif
00130 
00131     // Process any incoming ROS messages
00132     programTimer.reset();
00133     programTimer.start();
00134     while(!terminated)
00135     {
00136         // Handle any messages that have been received
00137         nodeHandle.spinOnce();
00138         // See if we've run for the desired amount of time
00139         #ifndef infiniteLoopROS
00140         if(programTimer.read_ms() > runTimeROS)
00141             stop();
00142         #endif
00143     }
00144     programTimer.stop();
00145     #ifdef debugLEDsROS
00146     rosLEDs[0]->write(0);
00147     rosLEDs[1]->write(0);
00148     rosLEDs[2]->write(0);
00149     rosLEDs[3]->write(0);
00150     #endif
00151 
00152     // TODO stop the ROS node handle / unsubscribe
00153     // note this is for when we call stop() on the mbed, independent of when the main ROS program terminates on the Pi
00154 
00155     // Stop the fish controller
00156     #ifdef rosControllerControlFish
00157     fishController.stop();
00158     // If battery died, wait a bit for pi to clean up and shutdown and whatnot
00159     if(lowBatteryVoltageInput == 0)
00160     {
00161         wait(90); // Give the Pi time to shutdown
00162         fishController.setLEDs(255, false);
00163     }
00164     #endif
00165 
00166     #ifdef printStatusROSController
00167     usbSerial->printf("\r\nROS controller done!\r\n");
00168     #endif
00169 }
00170 
00171 
00172 void ROSController::lowBatteryCallback()
00173 {
00174     if(lowBatteryVoltageInput == 0 && detectedLowBattery)
00175     {
00176         // Stop the ROS controller
00177         // This will end the main loop, causing main to terminate
00178         // Main will also stop the fish controller once this method ends
00179         stop();
00180         // Also force the pin low to signal the Pi
00181         // (should have already been done, but just in case)
00182         // TODO check that this really forces it low after this method ends and the pin object may be deleted
00183         DigitalOut simBatteryLow(lowBatteryVoltagePin);
00184         simBatteryLow = 0;
00185         #ifdef printStatusROSController
00186         usbSerial->printf("\r\nLow battery! Shutting down.\r\n");
00187         wait(0.5); // wait for the message to actually flush
00188         #endif
00189     }
00190     else if(lowBatteryVoltageInput == 0)
00191     {
00192         detectedLowBattery = true;
00193     }
00194 }
00195 
00196 #endif // #ifdef rosControl
00197