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

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

#include <communication_core.hpp>

Collaboration diagram for io_comm_mosaic::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 mosaic 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 mosaicMessage
 

Detailed Description

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

Definition at line 116 of file communication_core.hpp.

Constructor & Destructor Documentation

◆ Comm_IO()

io_comm_mosaic::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_mosaic::Comm_IO::~Comm_IO ( )
virtualdefault

Default destructor of the class Comm_IO.

Member Function Documentation

◆ GetHandlers()

CallbackHandlers io_comm_mosaic::Comm_IO::GetHandlers ( ) const
inline

Definition at line 160 of file communication_core.hpp.

References handlers_, and Send().

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

160 {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_mosaic::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 mosaic serial ports COM1, COM2 and COM3)
Returns
True if connection could be established, false otherwise

Definition at line 94 of file communication_core.cpp.

References baudrate_, io_comm_mosaic::baudrates, manager_, serial_port_, set_baudrate_sleep_, and SetManager().

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

95 {
96  ROS_DEBUG("mosaic: Calling InitializeSerial method..");
97  serial_port_ = port;
98  baudrate_ = baudrate;
99  // The io_context, of which io_service is a typedef of; it represents your program's link to the operating system's I/O services.
100  boost::shared_ptr<boost::asio::io_service> io_service(new boost::asio::io_service);
101  // To perform I/O operations the program needs an I/O object, here "serial".
102  boost::shared_ptr<boost::asio::serial_port> serial(new boost::asio::serial_port(*io_service));
103 
104  // We attempt the opening of the serial port..
105  try
106  {
107  serial->open(serial_port_);
108  }
109  catch (std::runtime_error& e)
110  {
111  // and return an error message in case it fails.
112  throw std::runtime_error("mosaic: Could not open serial port : " + serial_port_ + ": " + e.what());
113  return false;
114  }
115 
116  ROS_INFO("mosaic: Opened serial port %s", serial_port_.c_str());
117  ROS_DEBUG("Our boost version is %u.", BOOST_VERSION);
118  if(BOOST_VERSION < 106600) // E.g. for ROS melodic (i.e. Ubuntu 18.04), the version is 106501, standing for 1.65.1.
119  {
120  // Workaround to set some options for the port manually,
121  // cf. https://github.com/mavlink/mavros/pull/971/files
122  // This function native_handle() may be used to obtain
123  // the underlying representation of the serial port.
124  // Conversion from type native_handle_type to int is done implicitly.
125  int fd = serial->native_handle();
126  termios tio;
127  // Get terminal attribute, follows the syntax
128  // int tcgetattr(int fd, struct termios *termios_p);
129  tcgetattr(fd, &tio);
130 
131  // Hardware flow control settings..
132  if (flowcontrol == "RTS|CTS")
133  {
134  tio.c_iflag &= ~(IXOFF | IXON);
135  tio.c_cflag |= CRTSCTS;
136  }
137  else
138  {
139  tio.c_iflag &= ~(IXOFF | IXON);
140  tio.c_cflag &= ~CRTSCTS;
141  }
142  // Setting serial port to "raw" mode to prevent EOF exit..
143  cfmakeraw(&tio);
144 
145  // Commit settings, syntax is
146  // int tcsetattr(int fd, int optional_actions, const struct termios *termios_p);
147  tcsetattr(fd, TCSANOW, &tio);
148  }
149 
150  // Set the I/O manager
151  if (manager_)
152  {
153  ROS_ERROR("You have called the InitializeSerial() method though an AsyncManager object is already available! Start all anew..");
154  return false;
155  }
156  ROS_DEBUG("Creating new Async-Manager object..");
157  SetManager(boost::shared_ptr<Manager>(new AsyncManager<boost::asio::serial_port>(serial, io_service)));
158 
159  // Setting the baudrate, incrementally..
160  ROS_DEBUG("Gradually increasing the baudrate to the desired value...");
161  boost::asio::serial_port_base::baud_rate current_baudrate;
162  ROS_DEBUG("Initiated current_baudrate object...");
163  try
164  {
165  serial->get_option(current_baudrate); // Note that this sets current_baudrate.value() often to 115200, since by default, all mosaic COM ports
166  // are set to a baudrate of 115200 baud, using 8 data-bits, no parity and 1 stop-bit.
167  } catch(boost::system::system_error& e)
168  {
169 
170  ROS_ERROR("get_option failed due to %s", e.what());
171  ROS_INFO("Additional info about error is %s", boost::diagnostic_information(e).c_str());
172  /*
173  boost::system::error_code e_loop;
174  do // Caution: Might cause infinite loop..
175  {
176  serial->get_option(current_baudrate, e_loop);
177  } while(e_loop);
178  */
179  return false;
180  }
181  // Gradually increase the baudrate to the desired value
182  // The desired baudrate can be lower or larger than the
183  // current baudrate; the for loop takes care of both scenarios.
184  ROS_DEBUG("Current baudrate is %u", current_baudrate.value());
185  for (uint8_t i = 0; i < sizeof(baudrates)/sizeof(baudrates[0]); i++)
186  {
187  if (current_baudrate.value() == baudrate_)
188  {
189  break; // Break if the desired baudrate has been reached.
190  }
191  if(current_baudrate.value() >= baudrates[i] && baudrate_ > baudrates[i])
192  {
193  continue;
194  }
195  // Increment until Baudrate[i] matches current_baudrate.
196  try
197  {
198  serial->set_option(boost::asio::serial_port_base::baud_rate(baudrates[i]));
199  } catch(boost::system::system_error& e)
200  {
201 
202  ROS_ERROR("set_option failed due to %s", e.what());
203  ROS_INFO("Additional info about error is %s", boost::diagnostic_information(e).c_str());
204  return false;
205  }
206  usleep(set_baudrate_sleep_);
207  //boost::this_thread::sleep(boost::posix_time::milliseconds(set_baudrate_sleep_*1000)); // This yields an error message with exit code -7 the second time it is called, hence we use sleep() or usleep().
208  try
209  {
210  serial->get_option(current_baudrate);
211  } catch(boost::system::system_error& e)
212  {
213 
214  ROS_ERROR("get_option failed due to %s", e.what());
215  ROS_INFO("Additional info about error is %s", boost::diagnostic_information(e).c_str());
216  /*
217  boost::system::error_code e_loop;
218  do // Caution: Might cause infinite loop..
219  {
220  serial->get_option(current_baudrate, e_loop);
221  } while(e_loop);
222  */
223  return false;
224  }
225  ROS_DEBUG("mosaic: Set ASIO baudrate to %u", current_baudrate.value());
226  }
227  ROS_INFO("mosaic: Set ASIO baudrate to %u, leaving InitializeSerial() method", current_baudrate.value());
228  return true;
229 }
static const unsigned int set_baudrate_sleep_
void SetManager(const boost::shared_ptr< Manager > &manager)
Set the I/O manager.
boost::shared_ptr< Manager > manager_
static const uint32_t baudrates[]
Possible baudrates for mosaic.
std::string serial_port_
Saves the port description.
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_mosaic::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  host_ = host;
50  port_ = port;
51  // The io_context, of which io_service is a typedef of; it represents your program's link to the operating system's I/O services.
52  boost::shared_ptr<boost::asio::io_service> io_service(new boost::asio::io_service);
53  boost::asio::ip::tcp::resolver::iterator endpoint;
54 
55  try
56  {
57  boost::asio::ip::tcp::resolver resolver(*io_service);
58  // Note that tcp::resolver::query takes the host to resolve or the IP as the first parameter and the name of the service (as defined e.g. in /etc/services on Unix hosts) as second parameter.
59  // For the latter, one can also use a numeric service identifier (aka port number). In any case, it returns a list of possible endpoints, as there might be several entries for a single host.
60  endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port)); // Resolves query object..
61  }
62  catch (std::runtime_error& e)
63  {
64  throw std::runtime_error("mosaic: Could not resolve " + host + " on port " + port + ": " + e.what());
65  return false;
66  }
67 
68  boost::shared_ptr<boost::asio::ip::tcp::socket> socket(new boost::asio::ip::tcp::socket(*io_service));
69 
70  try
71  {
72  // The list of endpoints obtained above may contain both IPv4 and IPv6 endpoints, so we need to try each of them until we find one that works.
73  // This keeps the client program independent of a specific IP version. The boost::asio::connect() function does this for us automatically.
74  socket->connect(*endpoint);
75  }
76  catch (std::runtime_error& e)
77  {
78  throw std::runtime_error("mosaic: Could not connect to " + endpoint->host_name() + ": " + endpoint->service_name() + ": " + e.what());
79  return false;
80  }
81 
82  ROS_INFO("mosaic: Connected to %s: %s.", endpoint->host_name().c_str(), endpoint->service_name().c_str());
83 
84  if (manager_)
85  {
86  ROS_ERROR("You have called the InitializeTCP() method though an AsyncManager object is already available! Start all anew..");
87  return false;
88  }
89  SetManager(boost::shared_ptr<Manager>(new AsyncManager<boost::asio::ip::tcp::socket>(socket, io_service)));
90  return true;
91 }
void SetManager(const boost::shared_ptr< Manager > &manager)
Set the I/O manager.
std::string host_
Host currently connected to.
boost::shared_ptr< Manager > 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_mosaic::Comm_IO::ResetSerial ( std::string  port)

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

Parameters
[in]portThe device's port address

Definition at line 240 of file communication_core.cpp.

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

241 {
242  serial_port_ = port;
243  boost::shared_ptr<boost::asio::io_service> io_service(new boost::asio::io_service);
244  boost::shared_ptr<boost::asio::serial_port> serial(new boost::asio::serial_port(*io_service));
245 
246  // Try to open serial port
247  try
248  {
249  serial->open(serial_port_);
250  } catch (std::runtime_error& e)
251  {
252  throw std::runtime_error("mosaic: Could not open serial port :"
253  + serial_port_ + " " + e.what());
254  }
255 
256  ROS_INFO("mosaic: Reset serial port %s", serial_port_.c_str());
257 
258  // Sets the I/O worker
259  if (manager_) return;
260  SetManager(boost::shared_ptr<Manager>(new AsyncManager<boost::asio::serial_port>(serial, io_service)));
261 
262  // Set the baudrate
263  serial->set_option(boost::asio::serial_port_base::baud_rate(baudrate_));
264 }
void SetManager(const boost::shared_ptr< Manager > &manager)
Set the I/O manager.
boost::shared_ptr< Manager > manager_
std::string serial_port_
Saves the port description.
uint32_t baudrate_
Baudrate at the moment, unless InitializeSerial or ResetSerial fail.
Here is the call graph for this function:

◆ Send()

void io_comm_mosaic::Comm_IO::Send ( std::string  cmd)

Definition at line 41 of file communication_core.cpp.

References manager_.

Referenced by rosaic_node::ROSaicNode::ConfigureMosaic(), 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_mosaic::Comm_IO::SetManager ( const boost::shared_ptr< Manager > &  manager)

Set the I/O manager.

Parameters
[in]managerAn I/O handler

Definition at line 231 of file communication_core.cpp.

References handlers_, manager_, and io_comm_mosaic::CallbackHandlers::ReadCallback().

Referenced by InitializeSerial(), InitializeTCP(), and ResetSerial().

232 {
233  ROS_DEBUG("Called SetManager() method");
234  if (manager_) return;
235  manager_ = manager;
236  manager_->SetCallback(boost::bind(&CallbackHandlers::ReadCallback, &handlers_, _1, _2));
237  ROS_DEBUG("Leaving SetManager() method");
238 }
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.
void ReadCallback(const uint8_t *data, std::size_t &size)
Searches for mosaic messages that could potentially be decoded/parsed/published.
boost::shared_ptr< Manager > manager_
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 177 of file communication_core.hpp.

◆ mosaicMessage

friend class mosaicMessage
friend

Definition at line 178 of file communication_core.hpp.

Field Documentation

◆ baudrate_

uint32_t io_comm_mosaic::Comm_IO::baudrate_
private

Baudrate at the moment, unless InitializeSerial or ResetSerial fail.

Definition at line 175 of file communication_core.hpp.

Referenced by InitializeSerial(), and ResetSerial().

◆ handlers_

CallbackHandlers io_comm_mosaic::Comm_IO::handlers_

Callback handlers for the inwards streaming messages.

Definition at line 165 of file communication_core.hpp.

Referenced by rosaic_node::ROSaicNode::DefineMessages(), GetHandlers(), and SetManager().

◆ host_

std::string io_comm_mosaic::Comm_IO::host_
private

Host currently connected to.

Definition at line 181 of file communication_core.hpp.

Referenced by InitializeTCP().

◆ manager_

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

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

Definition at line 173 of file communication_core.hpp.

Referenced by InitializeSerial(), InitializeTCP(), ResetSerial(), Send(), and SetManager().

◆ port_

std::string io_comm_mosaic::Comm_IO::port_
private

Port over which TCP/IP connection is currently established.

Definition at line 183 of file communication_core.hpp.

Referenced by InitializeTCP().

◆ serial_port_

std::string io_comm_mosaic::Comm_IO::serial_port_
private

Saves the port description.

Definition at line 170 of file communication_core.hpp.

Referenced by InitializeSerial(), and ResetSerial().

◆ set_baudrate_sleep_

const unsigned int io_comm_mosaic::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 186 of file communication_core.hpp.

Referenced by InitializeSerial().


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