ROSaic
communication_core.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // *****************************************************************************
30 
32 
40 
41 void io_comm_rx::Comm_IO::send(std::string cmd)
42 {
43  // Determine byte size of cmd and hand over to send() method of manager_
44  manager_.get()->send(cmd, cmd.size());
45 }
46 
47 bool io_comm_rx::Comm_IO::initializeTCP(std::string host, std::string port)
48 {
49  ROS_DEBUG("Calling initializeTCP() method..");
50  host_ = host;
51  port_ = port;
52  // The io_context, of which io_service is a typedef of; it represents your program's link to the
53  // operating system's I/O services.
54  boost::shared_ptr<boost::asio::io_service> io_service(new boost::asio::io_service);
55  boost::asio::ip::tcp::resolver::iterator endpoint;
56 
57  try
58  {
59  boost::asio::ip::tcp::resolver resolver(*io_service);
60  // Note that tcp::resolver::query takes the host to resolve or the IP as the first parameter and
61  // the name of the service (as defined e.g. in /etc/services on Unix hosts) as second parameter.
62  // For the latter, one can also use a numeric service identifier (aka port number). In any case,
63  // it returns a list of possible endpoints, as there might be several entries for a single host.
64  endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); // Resolves query object..
65  }
66  catch (std::runtime_error& e)
67  {
68  throw std::runtime_error("Could not resolve " + host + " on port " + port + ": " + e.what());
69  return false;
70  }
71 
72  boost::shared_ptr<boost::asio::ip::tcp::socket> socket(new boost::asio::ip::tcp::socket(*io_service));
73 
74  try
75  {
76  // The list of endpoints obtained above may contain both IPv4 and IPv6 endpoints, so we need to
77  // try each of them until we find one that works. This keeps the client program independent of
78  // a specific IP version. The boost::asio::connect() function does this for us automatically.
79  socket->connect(*endpoint);
80  }
81  catch (std::runtime_error& e)
82  {
83  throw std::runtime_error("Could not connect to " + endpoint->host_name() + ": " +
84  endpoint->service_name() + ": " + e.what());
85  return false;
86  }
87 
88  ROS_INFO("Connected to %s: %s.", endpoint->host_name().c_str(), endpoint->service_name().c_str());
89 
90  if (manager_)
91  {
92  ROS_ERROR("You have called the InitializeTCP() method though an AsyncManager object is already available! Start all anew..");
93  return false;
94  }
95  setManager(boost::shared_ptr<Manager>(new AsyncManager<boost::asio::ip::tcp::socket>(socket, io_service)));
96  ROS_DEBUG("Leaving initializeTCP() method..");
97  return true;
98 }
99 
100 
101 bool io_comm_rx::Comm_IO::initializeSerial(std::string port, uint32_t baudrate, std::string flowcontrol)
102 {
103  ROS_DEBUG("Calling initializeSerial() method..");
104  serial_port_ = port;
105  baudrate_ = baudrate;
106  // The io_context, of which io_service is a typedef of; it represents your program's link to the
107  // operating system's I/O services.
108  boost::shared_ptr<boost::asio::io_service> io_service(new boost::asio::io_service);
109  // To perform I/O operations the program needs an I/O object, here "serial".
110  boost::shared_ptr<boost::asio::serial_port> serial(new boost::asio::serial_port(*io_service));
111 
112  // We attempt the opening of the serial port..
113  try
114  {
115  serial->open(serial_port_);
116  }
117  catch (std::runtime_error& e)
118  {
119  // and return an error message in case it fails.
120  throw std::runtime_error("Could not open serial port : " + serial_port_ + ": " + e.what());
121  return false;
122  }
123 
124  ROS_INFO("Opened serial port %s", serial_port_.c_str());
125  ROS_DEBUG("Our boost version is %u.", BOOST_VERSION);
126  if(BOOST_VERSION < 106600) // E.g. for ROS melodic (i.e. Ubuntu 18.04), the version is 106501, standing for 1.65.1.
127  {
128  // Workaround to set some options for the port manually,
129  // cf. https://github.com/mavlink/mavros/pull/971/files
130  // This function native_handle() may be used to obtain
131  // the underlying representation of the serial port.
132  // Conversion from type native_handle_type to int is done implicitly.
133  int fd = serial->native_handle();
134  termios tio;
135  // Get terminal attribute, follows the syntax
136  // int tcgetattr(int fd, struct termios *termios_p);
137  tcgetattr(fd, &tio);
138 
139  // Hardware flow control settings..
140  if (flowcontrol == "RTS|CTS")
141  {
142  tio.c_iflag &= ~(IXOFF | IXON);
143  tio.c_cflag |= CRTSCTS;
144  }
145  else
146  {
147  tio.c_iflag &= ~(IXOFF | IXON);
148  tio.c_cflag &= ~CRTSCTS;
149  }
150  // Setting serial port to "raw" mode to prevent EOF exit..
151  cfmakeraw(&tio);
152 
153  // Commit settings, syntax is
154  // int tcsetattr(int fd, int optional_actions, const struct termios *termios_p);
155  tcsetattr(fd, TCSANOW, &tio);
156  }
157 
158  // Set the I/O manager
159  if (manager_)
160  {
161  ROS_ERROR("You have called the initializeSerial() method though an AsyncManager object is already available! Start all anew..");
162  return false;
163  }
164  ROS_DEBUG("Creating new Async-Manager object..");
165  setManager(boost::shared_ptr<Manager>(new AsyncManager<boost::asio::serial_port>(serial, io_service)));
166 
167  // Setting the baudrate, incrementally..
168  ROS_DEBUG("Gradually increasing the baudrate to the desired value...");
169  boost::asio::serial_port_base::baud_rate current_baudrate;
170  ROS_DEBUG("Initiated current_baudrate object...");
171  try
172  {
173  serial->get_option(current_baudrate); // Note that this sets current_baudrate.value() often to 115200, since by default, all Rx COM ports,
174  // at least for mosaic Rxs, are set to a baudrate of 115200 baud, using 8 data-bits, no parity and 1 stop-bit.
175  } catch(boost::system::system_error& e)
176  {
177 
178  ROS_ERROR("get_option failed due to %s", e.what());
179  ROS_INFO("Additional info about error is %s", boost::diagnostic_information(e).c_str());
180  /*
181  boost::system::error_code e_loop;
182  do // Caution: Might cause infinite loop..
183  {
184  serial->get_option(current_baudrate, e_loop);
185  } while(e_loop);
186  */
187  return false;
188  }
189  // Gradually increase the baudrate to the desired value
190  // The desired baudrate can be lower or larger than the
191  // current baudrate; the for loop takes care of both scenarios.
192  ROS_DEBUG("Current baudrate is %u", current_baudrate.value());
193  for (uint8_t i = 0; i < sizeof(BAUDRATES)/sizeof(BAUDRATES[0]); i++)
194  {
195  if (current_baudrate.value() == baudrate_)
196  {
197  break; // Break if the desired baudrate has been reached.
198  }
199  if(current_baudrate.value() >= BAUDRATES[i] && baudrate_ > BAUDRATES[i])
200  {
201  continue;
202  }
203  // Increment until Baudrate[i] matches current_baudrate.
204  try
205  {
206  serial->set_option(boost::asio::serial_port_base::baud_rate(BAUDRATES[i]));
207  } catch(boost::system::system_error& e)
208  {
209 
210  ROS_ERROR("set_option failed due to %s", e.what());
211  ROS_INFO("Additional info about error is %s", boost::diagnostic_information(e).c_str());
212  return false;
213  }
214  usleep(SET_BAUDRATE_SLEEP_);
215  //boost::this_thread::sleep(boost::posix_time::milliseconds(SET_BAUDRATE_SLEEP_*1000));
216  // Boost's sleep would yield an error message with exit code -7 the second time it is called, hence we use sleep() or usleep().
217  try
218  {
219  serial->get_option(current_baudrate);
220  } catch(boost::system::system_error& e)
221  {
222 
223  ROS_ERROR("get_option failed due to %s", e.what());
224  ROS_INFO("Additional info about error is %s", boost::diagnostic_information(e).c_str());
225  /*
226  boost::system::error_code e_loop;
227  do // Caution: Might cause infinite loop..
228  {
229  serial->get_option(current_baudrate, e_loop);
230  } while(e_loop);
231  */
232  return false;
233  }
234  ROS_DEBUG("Set ASIO baudrate to %u", current_baudrate.value());
235  }
236  ROS_INFO("Set ASIO baudrate to %u, leaving InitializeSerial() method", current_baudrate.value());
237  return true;
238 }
239 
240 void io_comm_rx::Comm_IO::setManager(const boost::shared_ptr<Manager>& manager)
241 {
242  ROS_DEBUG("Called setManager() method");
243  if (manager_) return;
244  manager_ = manager;
245  manager_->setCallback(boost::bind(&CallbackHandlers::readCallback, &handlers_, _1, _2));
246  ROS_DEBUG("Leaving setManager() method");
247 }
248 
249 void io_comm_rx::Comm_IO::resetSerial(std::string port)
250 {
251  serial_port_ = port;
252  boost::shared_ptr<boost::asio::io_service> io_service(new boost::asio::io_service);
253  boost::shared_ptr<boost::asio::serial_port> serial(new boost::asio::serial_port(*io_service));
254 
255  // Try to open serial port
256  try
257  {
258  serial->open(serial_port_);
259  } catch (std::runtime_error& e)
260  {
261  throw std::runtime_error("Could not open serial port :"
262  + serial_port_ + " " + e.what());
263  }
264 
265  ROS_INFO("Reset serial port %s", serial_port_.c_str());
266 
267  // Sets the I/O worker
268  if (manager_) return;
269  setManager(boost::shared_ptr<Manager>(new AsyncManager<boost::asio::serial_port>(serial, io_service)));
270 
271  // Set the baudrate
272  serial->set_option(boost::asio::serial_port_base::baud_rate(baudrate_));
273 }
274 
void readCallback(const uint8_t *data, std::size_t &size)
Searches for Rx messages that could potentially be decoded/parsed/published.
This is the central interface between ROSaic and the Rx(s), managing I/O operations such as reading m...
std::string serial_port_
Saves the port description.
Highest-Level view on communication services.
bool initializeTCP(std::string host, std::string port)
Initializes the TCP I/O.
boost::shared_ptr< Manager > manager_
bool initializeSerial(std::string port, uint32_t baudrate=115200, std::string flowcontrol="None")
Initializes the serial port.
void send(std::string cmd)
std::string host_
Host currently connected to.
static const unsigned int SET_BAUDRATE_SLEEP_
void setManager(const boost::shared_ptr< Manager > &manager)
Set the I/O manager.
static const uint32_t BAUDRATES[]
Possible baudrates for the Rx.
std::string port_
Port over which TCP/IP connection is currently established.
void resetSerial(std::string port)
Reset the Serial I/O port, e.g. after a Rx reset.
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.
Comm_IO()
Default constructor of the class Comm_IO.
uint32_t baudrate_
Baudrate at the moment, unless InitializeSerial or ResetSerial fail.