ROSaic
Public Member Functions | Data Fields | Private Attributes | Static Private Attributes | Friends
io_comm_rx::Comm_IO Class Reference

Handles communication with and configuration of the mosaic (and beyond) receiver(s) More...

#include <communication_core.hpp>

Collaboration diagram for io_comm_rx::Comm_IO:
Collaboration graph
[legend]

Public Member Functions

 Comm_IO ()
 Default constructor of the class Comm_IO. More...
 
virtual ~Comm_IO ()=default
 Default destructor of the class Comm_IO. More...
 
bool initializeSerial (std::string port, uint32_t baudrate=115200, std::string flowcontrol="None")
 Initializes the serial port. More...
 
bool initializeTCP (std::string host, std::string port)
 Initializes the TCP I/O. More...
 
void setManager (const boost::shared_ptr< Manager > &manager)
 Set the I/O manager. More...
 
void resetSerial (std::string port)
 Reset the Serial I/O port, e.g. after a Rx reset. More...
 
CallbackHandlers getHandlers () const
 
void send (std::string cmd)
 

Data Fields

CallbackHandlers handlers_
 Callback handlers for the inwards streaming messages. More...
 

Private Attributes

std::string serial_port_
 Saves the port description. More...
 
boost::shared_ptr< Managermanager_
 
uint32_t baudrate_
 Baudrate at the moment, unless InitializeSerial or ResetSerial fail. More...
 
std::string host_
 Host currently connected to. More...
 
std::string port_
 Port over which TCP/IP connection is currently established. More...
 

Static Private Attributes

static const unsigned int SET_BAUDRATE_SLEEP_ = 500000
 

Friends

class CallbackHandlers
 
class RxMessage
 

Detailed Description

Handles communication with and configuration of the mosaic (and beyond) receiver(s)

Definition at line 102 of file communication_core.hpp.

Constructor & Destructor Documentation

◆ Comm_IO()

io_comm_rx::Comm_IO::Comm_IO ( )

Default constructor of the class Comm_IO.

Definition at line 39 of file communication_core.cpp.

39 : handlers_() {}
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.

◆ ~Comm_IO()

virtual io_comm_rx::Comm_IO::~Comm_IO ( )
virtualdefault

Default destructor of the class Comm_IO.

Member Function Documentation

◆ getHandlers()

CallbackHandlers io_comm_rx::Comm_IO::getHandlers ( ) const
inline

Definition at line 147 of file communication_core.hpp.

References handlers_, and send().

Referenced by rosaic_node::ROSaicNode::defineMessages().

147 {return handlers_;}
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.
Here is the call graph for this function:
Here is the caller graph for this function:

◆ initializeSerial()

bool io_comm_rx::Comm_IO::initializeSerial ( std::string  port,
uint32_t  baudrate = 115200,
std::string  flowcontrol = "None" 
)

Initializes the serial port.

Parameters
[in]portThe device's port address
[in]baudrateThe chosen baud rate of the port
[in]flowcontrolDefault is "None", set variable (not yet checked) to "RTS|CTS" to activate hardware flow control (only for serial ports COM1, COM2 and COM3 (for mosaic))
Returns
True if connection could be established, false otherwise

Definition at line 101 of file communication_core.cpp.

References baudrate_, io_comm_rx::BAUDRATES, manager_, serial_port_, SET_BAUDRATE_SLEEP_, and setManager().

Referenced by rosaic_node::ROSaicNode::reconnect().

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 }
std::string serial_port_
Saves the port description.
boost::shared_ptr< Manager > manager_
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.
uint32_t baudrate_
Baudrate at the moment, unless InitializeSerial or ResetSerial fail.
Here is the call graph for this function:
Here is the caller graph for this function:

◆ initializeTCP()

bool io_comm_rx::Comm_IO::initializeTCP ( std::string  host,
std::string  port 
)

Initializes the TCP I/O.

Parameters
[in]hostThe TCP host
[in]portThe TCP port
Returns
True if connection could be established, false otherwise

Definition at line 47 of file communication_core.cpp.

References host_, manager_, port_, and setManager().

Referenced by rosaic_node::ROSaicNode::reconnect().

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 }
boost::shared_ptr< Manager > manager_
std::string host_
Host currently connected to.
void setManager(const boost::shared_ptr< Manager > &manager)
Set the I/O manager.
std::string port_
Port over which TCP/IP connection is currently established.
Here is the call graph for this function:
Here is the caller graph for this function:

◆ resetSerial()

void io_comm_rx::Comm_IO::resetSerial ( std::string  port)

Reset the Serial I/O port, e.g. after a Rx reset.

Parameters
[in]portThe device's port address

Definition at line 249 of file communication_core.cpp.

References baudrate_, manager_, serial_port_, and setManager().

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 }
std::string serial_port_
Saves the port description.
boost::shared_ptr< Manager > manager_
void setManager(const boost::shared_ptr< Manager > &manager)
Set the I/O manager.
uint32_t baudrate_
Baudrate at the moment, unless InitializeSerial or ResetSerial fail.
Here is the call graph for this function:

◆ send()

void io_comm_rx::Comm_IO::send ( std::string  cmd)

Definition at line 41 of file communication_core.cpp.

References manager_.

Referenced by rosaic_node::ROSaicNode::configureRx(), and getHandlers().

42 {
43  // Determine byte size of cmd and hand over to send() method of manager_
44  manager_.get()->send(cmd, cmd.size());
45 }
boost::shared_ptr< Manager > manager_
Here is the caller graph for this function:

◆ setManager()

void io_comm_rx::Comm_IO::setManager ( const boost::shared_ptr< Manager > &  manager)

Set the I/O manager.

Parameters
[in]managerAn I/O handler

Definition at line 240 of file communication_core.cpp.

References handlers_, manager_, and io_comm_rx::CallbackHandlers::readCallback().

Referenced by initializeSerial(), initializeTCP(), and resetSerial().

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 }
void readCallback(const uint8_t *data, std::size_t &size)
Searches for Rx messages that could potentially be decoded/parsed/published.
boost::shared_ptr< Manager > manager_
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.
Here is the call graph for this function:
Here is the caller graph for this function:

Friends And Related Function Documentation

◆ CallbackHandlers

friend class CallbackHandlers
friend

Definition at line 164 of file communication_core.hpp.

◆ RxMessage

friend class RxMessage
friend

Definition at line 165 of file communication_core.hpp.

Field Documentation

◆ baudrate_

uint32_t io_comm_rx::Comm_IO::baudrate_
private

Baudrate at the moment, unless InitializeSerial or ResetSerial fail.

Definition at line 162 of file communication_core.hpp.

Referenced by initializeSerial(), and resetSerial().

◆ handlers_

CallbackHandlers io_comm_rx::Comm_IO::handlers_

Callback handlers for the inwards streaming messages.

Definition at line 152 of file communication_core.hpp.

Referenced by rosaic_node::ROSaicNode::defineMessages(), getHandlers(), and setManager().

◆ host_

std::string io_comm_rx::Comm_IO::host_
private

Host currently connected to.

Definition at line 168 of file communication_core.hpp.

Referenced by initializeTCP().

◆ manager_

boost::shared_ptr<Manager> io_comm_rx::Comm_IO::manager_
private

Processes I/O stream data This declaration is deliberately stream-independent (Serial or TCP).

Definition at line 160 of file communication_core.hpp.

Referenced by initializeSerial(), initializeTCP(), resetSerial(), send(), and setManager().

◆ port_

std::string io_comm_rx::Comm_IO::port_
private

Port over which TCP/IP connection is currently established.

Definition at line 170 of file communication_core.hpp.

Referenced by initializeTCP().

◆ serial_port_

std::string io_comm_rx::Comm_IO::serial_port_
private

Saves the port description.

Definition at line 157 of file communication_core.hpp.

Referenced by initializeSerial(), and resetSerial().

◆ SET_BAUDRATE_SLEEP_

const unsigned int io_comm_rx::Comm_IO::SET_BAUDRATE_SLEEP_ = 500000
staticprivate

Sleep time in microseconds (there is no Unix command for milliseconds) after setting the baudrate to certain value (important between increments)

Definition at line 173 of file communication_core.hpp.

Referenced by initializeSerial().


The documentation for this class was generated from the following files: