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


libcreate
Author(s): Jacob Perron
autogenerated on Thu Dec 5 2019 04:03:13