serial.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 
00003 #include "create/serial.h"
00004 #include "create/types.h"
00005 
00006 namespace create {
00007 
00008   Serial::Serial(boost::shared_ptr<Data> d) :
00009     data(d),
00010     port(io),
00011     isReading(false),
00012     dataReady(false),
00013     corruptPackets(0),
00014     totalPackets(0) {
00015   }
00016 
00017   Serial::~Serial() {
00018     disconnect();
00019   }
00020 
00021   bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
00022     using namespace boost::asio;
00023     port.open(portName);
00024     port.set_option(serial_port::baud_rate(baud));
00025     port.set_option(serial_port::flow_control(serial_port::flow_control::none));
00026 
00027     usleep(1000000);
00028 
00029     if (port.is_open()) {
00030       callback = cb;
00031       bool startReadSuccess = startReading();
00032       if (!startReadSuccess) {
00033         port.close();
00034       }
00035       return startReadSuccess;
00036     }
00037     return false;
00038   }
00039 
00040   void Serial::disconnect() {
00041     if (isReading) {
00042       stopReading();
00043     }
00044 
00045     if (connected()) {
00046       // Ensure not in Safe/Full modes
00047       sendOpcode(OC_START);
00048       // Stop OI
00049       sendOpcode(OC_STOP);
00050       port.close();
00051     }
00052   }
00053 
00054   bool Serial::startReading() {
00055     if (!connected()) return false;
00056 
00057     if (!data) {
00058       CERR("[create::Serial] ", "data pointer not initialized.");
00059       return false;
00060     }
00061 
00062     // Only allow once
00063     if (isReading) return true;
00064 
00065     // Start OI
00066     sendOpcode(OC_START);
00067 
00068     if (!startSensorStream()) return false;
00069 
00070     io.reset();
00071 
00072     // Start continuously reading one byte at a time
00073     boost::asio::async_read(port,
00074                             boost::asio::buffer(&byteRead, 1),
00075                             boost::bind(&Serial::onData, this, _1, _2));
00076 
00077     ioThread = boost::thread(boost::bind(&boost::asio::io_service::run, &io));
00078 
00079     // Wait for first complete read to finish
00080     boost::unique_lock<boost::mutex> lock(dataReadyMut);
00081 
00082     int attempts = 1;
00083     int maxAttempts = 10;
00084     while (!dataReady) {
00085       if (!dataReadyCond.timed_wait(lock, boost::get_system_time() + boost::posix_time::milliseconds(500))) {
00086         if (attempts >= maxAttempts) {
00087           CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!");
00088           io.stop();
00089           ioThread.join();
00090           return false;
00091         }
00092         attempts++;
00093 
00094         // Request data again
00095         sendOpcode(OC_START);
00096         startSensorStream();
00097       }
00098     }
00099 
00100     isReading = true;
00101     return true;
00102   }
00103 
00104   void Serial::stopReading() {
00105     if (isReading) {
00106       io.stop();
00107       ioThread.join();
00108       isReading = false;
00109       {
00110         boost::lock_guard<boost::mutex> lock(dataReadyMut);
00111         dataReady = false;
00112       }
00113     }
00114   }
00115 
00116 
00117   void Serial::notifyDataReady() {
00118     // Validate all packets
00119     data->validateAll();
00120 
00121     // Notify first data packets ready
00122     {
00123       boost::lock_guard<boost::mutex> lock(dataReadyMut);
00124       if (!dataReady) {
00125         dataReady = true;
00126         dataReadyCond.notify_one();
00127       }
00128     }
00129     // Callback to notify data is ready
00130     if (callback)
00131       callback();
00132   }
00133 
00134   void Serial::onData(const boost::system::error_code& e, const std::size_t& size) {
00135     if (e) {
00136       CERR("[create::Serial] ", "serial error - " << e.message());
00137       return;
00138     }
00139 
00140     // Should have read exactly one byte
00141     if (size == 1) {
00142       processByte(byteRead);
00143     } // end if (size == 1)
00144 
00145     // Read the next byte
00146     boost::asio::async_read(port,
00147                             boost::asio::buffer(&byteRead, 1),
00148                             boost::bind(&Serial::onData, this, _1, _2));
00149   }
00150 
00151   bool Serial::send(const uint8_t* bytes, unsigned int numBytes) {
00152     if (!connected()) {
00153       CERR("[create::Serial] ", "send failed, not connected.");
00154       return false;
00155     }
00156     // TODO: catch boost exceptions
00157     boost::asio::write(port, boost::asio::buffer(bytes, numBytes));
00158     return true;
00159   }
00160 
00161   bool Serial::sendOpcode(const Opcode& code) {
00162     uint8_t oc = (uint8_t) code;
00163     return send(&oc, 1);
00164   }
00165 
00166   uint64_t Serial::getNumCorruptPackets() const {
00167     return corruptPackets;
00168   }
00169 
00170   uint64_t Serial::getTotalPackets() const {
00171     return totalPackets;
00172   }
00173 } // namespace create


libcreate
Author(s): Jacob Perron
autogenerated on Sat Nov 26 2016 03:41:46