serial.cpp
Go to the documentation of this file.
1 #include <chrono>
2 #include <functional>
3 #include <iostream>
4 
5 #include "create/serial.h"
6 #include "create/types.h"
7 
8 namespace create {
9 
10  Serial::Serial(std::shared_ptr<Data> d) :
11  signals(io, SIGINT, SIGTERM),
12  port(io),
13  dataReady(false),
14  isReading(false),
15  data(d),
16  corruptPackets(0),
17  totalPackets(0) {
18  }
19 
21  disconnect();
22  }
23 
24  void Serial::signalHandler(const boost::system::error_code& error, int signal_number) {
25  if (!error) {
26  if (connected()) {
27  // Ensure not in Safe/Full modes
29  // Stop OI
31  exit(signal_number);
32  }
33  }
34  }
35 
36  bool Serial::connect(const std::string& portName, const int& baud, std::function<void()> cb) {
37  using namespace boost::asio;
38  port.open(portName);
39  port.set_option(serial_port::baud_rate(baud));
40  port.set_option(serial_port::character_size(8));
41  port.set_option(serial_port::parity(serial_port::parity::none));
42  port.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
43  port.set_option(serial_port::flow_control(serial_port::flow_control::none));
44 
45  signals.async_wait(std::bind(&Serial::signalHandler, this, std::placeholders::_1, std::placeholders::_2));
46 
47  usleep(1000000);
48 
49  if (port.is_open()) {
50  callback = cb;
51  bool startReadSuccess = startReading();
52  if (!startReadSuccess) {
53  port.close();
54  }
55  return startReadSuccess;
56  }
57  return false;
58  }
59 
61  if (isReading) {
62  stopReading();
63  }
64 
65  if (connected()) {
66  // Ensure not in Safe/Full modes
68  // Stop OI
70  port.close();
71  }
72  }
73 
75  if (!connected()) return false;
76 
77  if (!data) {
78  CERR("[create::Serial] ", "data pointer not initialized.");
79  return false;
80  }
81 
82  // Only allow once
83  if (isReading) return true;
84 
85  // Start OI
87 
88  if (!startSensorStream()) return false;
89 
90  io.reset();
91 
92  // Start continuously reading one byte at a time
93  boost::asio::async_read(port,
94  boost::asio::buffer(&byteRead, 1),
95  std::bind(&Serial::onData,
96  shared_from_this(),
97  std::placeholders::_1,
98  std::placeholders::_2));
99 
100  ioThread = std::thread(std::bind(
101  static_cast<std::size_t(boost::asio::io_service::*)(void)>(
102  &boost::asio::io_service::run), &io));
103 
104  // Wait for first complete read to finish
105  std::unique_lock<std::mutex> lock(dataReadyMut);
106 
107  int attempts = 1;
108  int maxAttempts = 10;
109  while (!dataReady) {
110  if (dataReadyCond.wait_for(lock, std::chrono::milliseconds(500)) == std::cv_status::timeout) {
111  if (attempts >= maxAttempts) {
112  CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!");
113  io.stop();
114  ioThread.join();
115  return false;
116  }
117  attempts++;
118 
119  // Request data again
122  }
123  }
124 
125  isReading = true;
126  return true;
127  }
128 
130  if (isReading) {
131  io.stop();
132  ioThread.join();
133  isReading = false;
134  {
135  std::lock_guard<std::mutex> lock(dataReadyMut);
136  dataReady = false;
137  }
138  }
139  }
140 
141 
143  // Validate all packets
144  data->validateAll();
145 
146  // Notify first data packets ready
147  {
148  std::lock_guard<std::mutex> lock(dataReadyMut);
149  if (!dataReady) {
150  dataReady = true;
151  dataReadyCond.notify_one();
152  }
153  }
154  // Callback to notify data is ready
155  if (callback)
156  callback();
157  }
158 
159  void Serial::onData(const boost::system::error_code& e, const std::size_t& size) {
160  if (e) {
161  CERR("[create::Serial] ", "serial error - " << e.message());
162  return;
163  }
164 
165  // Should have read exactly one byte
166  if (size == 1) {
168  } // end if (size == 1)
169 
170  // Read the next byte
171  boost::asio::async_read(port,
172  boost::asio::buffer(&byteRead, 1),
173  std::bind(&Serial::onData,
174  shared_from_this(),
175  std::placeholders::_1,
176  std::placeholders::_2));
177  }
178 
179  bool Serial::send(const uint8_t* bytes, unsigned int numBytes) {
180  if (!connected()) {
181  CERR("[create::Serial] ", "send failed, not connected.");
182  return false;
183  }
184  // TODO: catch boost exceptions
185  boost::asio::write(port, boost::asio::buffer(bytes, numBytes));
186  return true;
187  }
188 
189  bool Serial::sendOpcode(const Opcode& code) {
190  uint8_t oc = (uint8_t) code;
191  return send(&oc, 1);
192  }
193 
194  uint64_t Serial::getNumCorruptPackets() const {
195  return corruptPackets;
196  }
197 
198  uint64_t Serial::getTotalPackets() const {
199  return totalPackets;
200  }
201 } // namespace create
std::function< void()> callback
Definition: serial.h:70
uint8_t byteRead
Definition: serial.h:65
uint64_t getTotalPackets() const
Definition: serial.cpp:198
boost::asio::serial_port port
Definition: serial.h:56
bool connected() const
Definition: serial.h:93
virtual void processByte(uint8_t byteRead)=0
bool sendOpcode(const Opcode &code)
Definition: serial.cpp:189
void notifyDataReady()
Definition: serial.cpp:142
std::condition_variable dataReadyCond
Definition: serial.h:60
Opcode
Definition: types.h:154
Definition: create.h:47
uint64_t totalPackets
Definition: serial.h:79
#define CERR(prefix, msg)
Definition: util.h:38
bool startReading()
Definition: serial.cpp:74
void stopReading()
Definition: serial.cpp:129
std::shared_ptr< Data > data
Definition: serial.h:76
boost::asio::signal_set signals
Definition: serial.h:55
void disconnect()
Definition: serial.cpp:60
bool send(const uint8_t *bytes, const uint32_t numBytes)
Definition: serial.cpp:179
virtual bool startSensorStream()=0
void signalHandler(const boost::system::error_code &error, int signal_number)
Definition: serial.cpp:24
bool dataReady
Definition: serial.h:62
bool connect(const std::string &port, const int &baud=115200, std::function< void()> cb=0)
Definition: serial.cpp:36
std::thread ioThread
Definition: serial.h:59
void onData(const boost::system::error_code &e, const std::size_t &size)
Definition: serial.cpp:159
std::mutex dataReadyMut
Definition: serial.h:61
boost::asio::io_service io
Definition: serial.h:54
bool isReading
Definition: serial.h:63
Serial(std::shared_ptr< Data > data)
Definition: serial.cpp:10
uint64_t getNumCorruptPackets() const
Definition: serial.cpp:194
uint64_t corruptPackets
Definition: serial.h:78


libcreate
Author(s): Jacob Perron
autogenerated on Mon Nov 2 2020 03:59:34