44 manager_.get()->send(cmd, cmd.size());
49 ROS_DEBUG(
"Calling initializeTCP() method..");
54 boost::shared_ptr<boost::asio::io_service> io_service(
new boost::asio::io_service);
55 boost::asio::ip::tcp::resolver::iterator endpoint;
59 boost::asio::ip::tcp::resolver resolver(*io_service);
64 endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(host, port));
66 catch (std::runtime_error& e)
68 throw std::runtime_error(
"Could not resolve " + host +
" on port " + port +
": " + e.what());
72 boost::shared_ptr<boost::asio::ip::tcp::socket> socket(
new boost::asio::ip::tcp::socket(*io_service));
79 socket->connect(*endpoint);
81 catch (std::runtime_error& e)
83 throw std::runtime_error(
"Could not connect to " + endpoint->host_name() +
": " +
84 endpoint->service_name() +
": " + e.what());
88 ROS_INFO(
"Connected to %s: %s.", endpoint->host_name().c_str(), endpoint->service_name().c_str());
92 ROS_ERROR(
"You have called the InitializeTCP() method though an AsyncManager object is already available! Start all anew..");
96 ROS_DEBUG(
"Leaving initializeTCP() method..");
103 ROS_DEBUG(
"Calling initializeSerial() method..");
108 boost::shared_ptr<boost::asio::io_service> io_service(
new boost::asio::io_service);
110 boost::shared_ptr<boost::asio::serial_port> serial(
new boost::asio::serial_port(*io_service));
117 catch (std::runtime_error& e)
120 throw std::runtime_error(
"Could not open serial port : " +
serial_port_ +
": " + e.what());
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)
133 int fd = serial->native_handle();
140 if (flowcontrol ==
"RTS|CTS")
142 tio.c_iflag &= ~(IXOFF | IXON);
143 tio.c_cflag |= CRTSCTS;
147 tio.c_iflag &= ~(IXOFF | IXON);
148 tio.c_cflag &= ~CRTSCTS;
155 tcsetattr(fd, TCSANOW, &tio);
161 ROS_ERROR(
"You have called the initializeSerial() method though an AsyncManager object is already available! Start all anew..");
164 ROS_DEBUG(
"Creating new Async-Manager object..");
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...");
173 serial->get_option(current_baudrate);
175 }
catch(boost::system::system_error& e)
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());
192 ROS_DEBUG(
"Current baudrate is %u", current_baudrate.value());
195 if (current_baudrate.value() ==
baudrate_)
206 serial->set_option(boost::asio::serial_port_base::baud_rate(
BAUDRATES[i]));
207 }
catch(boost::system::system_error& e)
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());
219 serial->get_option(current_baudrate);
220 }
catch(boost::system::system_error& e)
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());
234 ROS_DEBUG(
"Set ASIO baudrate to %u", current_baudrate.value());
236 ROS_INFO(
"Set ASIO baudrate to %u, leaving InitializeSerial() method", current_baudrate.value());
242 ROS_DEBUG(
"Called setManager() method");
246 ROS_DEBUG(
"Leaving setManager() method");
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));
259 }
catch (std::runtime_error& e)
261 throw std::runtime_error(
"Could not open serial port :" 272 serial->set_option(boost::asio::serial_port_base::baud_rate(
baudrate_));
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.