#include "Serial.h"#include "StreamBasedLauncher.h"#include "Utilities.h"#include "Stream.h"#include "ReliableStream.h"#include "ServerProtocol.h"#include <termios.h>#include <unistd.h>#include <fcntl.h>#include <arpa/inet.h>#include <poll.h>#include <chrono>#include <iostream> namespace po = boost::program_options;using namespace std::literals::chrono_literals; class SerialStream : public WaitableStream{    static const long kReadBufferSize = 4096;    uint8_t readBuffer[kReadBufferSize];public:    int fd;    int baud;     virtual void write(const void* p, size_t n) override;     virtual void wait() override;     SerialStream(po::variables_map &options);    ~SerialStream();}; class SerialLauncher : public StreamBasedLauncher{    SerialStream stream;    ReliableStream rStream;public:    SerialLauncher(po::variables_map& options);};  SerialStream::SerialStream(po::variables_map &options){    std::string port = options["serial-port"].as<std::string>();    baud = options["serial-baud"].as<int>();    fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY );    if(fd < 0)        throw std::runtime_error("Cannot open serial port.\n");     struct termios tios;    tcgetattr(fd,&tios);     tios.c_cflag = CRTSCTS | CS8 | CLOCAL | CREAD;     switch(baud)    {        case 9600:      tios.c_cflag |= B9600; break;        case 19200:     tios.c_cflag |= B19200; break;        case 38400:     tios.c_cflag |= B38400; break;        case 57600:     tios.c_cflag |= B57600; break;        case 115200:    tios.c_cflag |= B115200; break;        case 230400:    tios.c_cflag |= B230400; break;        default:            throw std::runtime_error("Unsupported baud rate.\n");    }     tios.c_iflag = 0;//IGNPAR | ICRNL;    tios.c_lflag = 0;    tios.c_oflag = 0;    tios.c_cc[VTIME]    = 0;   /* inter-character timer unused */    tios.c_cc[VMIN]     = 1;   /* blocking read until 1 chars received */    tcsetattr(fd,TCSANOW,&tios);    usleep(500000);}SerialStream::~SerialStream(){    tcdrain(fd);    usleep(500000);    close(fd);} void SerialStream::write(const void* p, size_t n){    while(n)    {        struct pollfd pfd;        pfd.fd = fd;        pfd.events = POLLOUT;        pfd.revents = 0;        poll(&pfd, 1, 1000);        if(pfd.revents & POLLOUT)        {            ssize_t written = ::write(fd, p, n);            if(written > 0)            {                p = (const void*) ( (const char*)p + written );                n -= written;            }        }    }} void SerialStream::wait(){    struct pollfd pfd;    pfd.fd = fd;    pfd.events = POLLIN;    pfd.revents = 0;    poll(&pfd, 1, 1000);    if(pfd.revents & POLLIN)    {        ssize_t n = ::read(fd, readBuffer, kReadBufferSize);        if(n > 0)        {            notifyReceive(readBuffer, n);        }    }} SerialLauncher::SerialLauncher(po::variables_map &options)    : StreamBasedLauncher(options), stream(options), rStream(&stream){    SetupStream(&stream, &rStream);     do    {        rStream.reset(1);        std::cerr << "Connecting... (" << stream.baud << " baud)" << std::endl;        using clock = std::chrono::steady_clock;        auto startTime = clock::now();        while(!rStream.resetResponseArrived() && clock::now() - startTime < 5s)            stream.wait();    } while(!rStream.resetResponseArrived());     std::cerr << "Connected." << std::endl;} void Serial::GetOptions(options_description &desc){    desc.add_options()        ("serial-port", po::value<std::string>()->default_value("/dev/ttyUSB0"), "serial port to use")        ("serial-baud", po::value<int>()->default_value(19200), "serial port speed")        ;} bool Serial::CheckOptions(variables_map &options){    return true;} std::unique_ptr<Launcher> Serial::MakeLauncher(variables_map &options){    return std::unique_ptr<Launcher>(new SerialLauncher(options));}