serialIO.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include "serialIO.h"
19 #include "sys/select.h"
20 #include <iostream>
21 #include <cstring>
22 
23 #include <ros/ros.h>
24 
26  _fd(-1), _device_string(""), _baudrate(9600)
27 {
28 }
29 
31 {
32  stop();
33  closePort();
34 }
35 
36 // Open Serial Port
37 int SerialIO::openPort(std::string devicestring, int baudrate)
38 {
39  if(_fd != -1) return _fd;
40 
41  _device_string = devicestring;
42  _baudrate = baudrate;
43 
44  speed_t baud = getBaudFromInt(baudrate);
45  std::memset(&port_settings,0,sizeof(port_settings));
46  port_settings.c_iflag = 0;
47  port_settings.c_oflag = 0;
48  port_settings.c_cflag = CS8|CREAD|CLOCAL;
49  port_settings.c_lflag = 0;
50  port_settings.c_cc[VMIN]=1;
51  port_settings.c_cc[VTIME]=5;
52 
53  _fd=open(devicestring.c_str(), O_RDWR | O_NONBLOCK);
54  cfsetospeed(&port_settings, baud);
55  cfsetispeed(&port_settings, baud);
56 
57  tcsetattr(_fd, TCSANOW, &port_settings);
58 
59  return _fd;
60 }
61 
62 // Send Data to Serial Port
63 int SerialIO::sendData(std::string value)
64 {
65  boost::mutex::scoped_lock lock(_mutex);
66  int wrote = -1;
67  if(_fd != -1)
68  wrote = write(_fd, value.c_str(), value.length());
69  return wrote;
70 }
71 
72 // Send Data to Serial Port
73 int SerialIO::sendData(const char* data, size_t len)
74 {
75  boost::mutex::scoped_lock lock(_mutex);
76  int wrote = -1;
77  if(_fd != -1)
78  wrote = write(_fd, data, len);
79  return wrote;
80 }
81 
82 // Read Data from Serial Port
83 int SerialIO::readData(std::string &value, size_t nBytes)
84 {
85  boost::mutex::scoped_lock lock(_mutex);
86  fd_set fds;
87  FD_ZERO(&fds);
88  FD_SET(_fd, &fds);
89  struct timeval timeout = {0, 100000};
90  char buffer[32];
91  size_t rec = -1;
92  if(select(_fd+1, &fds, NULL, NULL, &timeout))
93  {
94  rec = read(_fd, buffer, nBytes);
95  value = std::string(buffer, rec);
96  }
97 
98  return rec;
99 }
100 
102 {
103  if(_thread == NULL)
104  _thread.reset(new boost::thread(&SerialIO::run, this));
105 }
106 
108 {
109  if(_thread != NULL)
110  {
111  _thread->interrupt();
112  _thread->join();
113  _thread.reset();
114  }
115 }
116 
118 {
120  std::vector<ioData_t> data;
121  while(true)
122  {
123  _oQueue.wait_pop(data);
124  for(size_t i = 0; i < data.size(); i++)
125  this->sendData(data[i].buf, data[i].len);
126  r.sleep();
127  }
128 }
129 
130 void SerialIO::enqueueData(std::vector<ioData_t> data)
131 {
132  _oQueue.push(data);
133 }
134 
135 void SerialIO::enqueueData(const char* buf, size_t len)
136 {
137  struct ioData data;
138  data.buf=buf;
139  data.len=len;
140  std::vector<ioData_t> vec;
141  vec.push_back(data);
142  _oQueue.push(vec);
143 }
144 
145 // Check if Serial Port is opened
147 {
148  return (_fd != -1);
149 }
150 
151 // Close Serial Port
153 {
154  if(_fd != -1)
155  {
156  close(_fd);
157  _fd = -1;
158  }
159 }
160 
162 {
163  closePort();
165  {
166  usleep(50000);
167  tcflush(_fd, TCIOFLUSH);
168  return true;
169  }
170  else
171  return false;
172 }
173 
174 
175 speed_t SerialIO::getBaudFromInt(int baud)
176 {
177  speed_t ret;
178  switch(baud)
179  {
180  case 0: ret=B0; break;
181  case 50: ret=B50; break;
182  case 75: ret=B75; break;
183  case 110: ret=B110; break;
184  case 134: ret=B134; break;
185  case 150: ret=B150; break;
186  case 200: ret=B200; break;
187  case 300: ret=B300; break;
188  case 1200: ret=B1200; break;
189  case 1800: ret=B1800; break;
190  case 2400: ret=B2400; break;
191  case 4800: ret=B4800; break;
192  case 9600: ret=B9600; break;
193  case 19200: ret=B19200; break;
194  case 38400: ret=B38400; break;
195  case 57600: ret=B57600; break;
196  case 115200: ret=B115200; break;
197  case 230400: ret=B230400; break;
198  default:
199  ret=B230400;
200  break;
201  }
202  return ret;
203 }
void stop()
Definition: serialIO.cpp:107
void wait_pop(T &data)
int openPort(std::string devicestring, int baudrate)
Definition: serialIO.cpp:37
void enqueueData(std::vector< ioData_t > data)
Definition: serialIO.cpp:130
static const int maxUpdateRate
Definition: serialIO.h:94
int _fd
Definition: serialIO.h:83
ConcurrentQueue< std::vector< struct ioData > > _oQueue
Definition: serialIO.h:76
speed_t getBaudFromInt(int baud)
Definition: serialIO.cpp:175
~SerialIO()
Definition: serialIO.cpp:30
bool recover()
Definition: serialIO.cpp:161
void push(T const &data)
struct termios port_settings
Definition: serialIO.h:85
const char * buf
Definition: serialIO.h:35
boost::shared_ptr< boost::thread > _thread
Definition: serialIO.h:78
void closePort()
Definition: serialIO.cpp:152
void run()
Definition: serialIO.cpp:117
int sendData(std::string value)
Definition: serialIO.cpp:63
bool isOpen()
Definition: serialIO.cpp:146
int _baudrate
Definition: serialIO.h:89
size_t len
Definition: serialIO.h:36
bool sleep()
SerialIO()
Definition: serialIO.cpp:25
std::string _device_string
Definition: serialIO.h:87
int readData(std::string &value, size_t nBytes)
Definition: serialIO.cpp:83
boost::mutex _mutex
Definition: serialIO.h:79
void start()
Definition: serialIO.cpp:101


cob_light
Author(s): Benjamin Maidel
autogenerated on Wed Apr 7 2021 02:11:39