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


libcreate
Author(s): Jacob Perron
autogenerated on Wed May 24 2023 02:24:57