From f4430b48b0127e6680409623ce6510fe5d64aecb Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 18 Mar 2011 17:51:17 -0500 Subject: [PATCH] Serial read is working, but the timeout is a little buggy... write hasn't been tested and I am going to try this out on linux. --- serial/CMakeLists.txt | 1 + serial/include/serial.h | 242 ++++++++++++++++++++--- serial/src/serial.cpp | 386 ++++++++++++++++++++++++++++++++----- serial/src/test_serial.cpp | 23 ++- 4 files changed, 577 insertions(+), 75 deletions(-) diff --git a/serial/CMakeLists.txt b/serial/CMakeLists.txt index 8cde274..12148c7 100644 --- a/serial/CMakeLists.txt +++ b/serial/CMakeLists.txt @@ -28,3 +28,4 @@ rosbuild_add_boost_directories() rosbuild_link_boost(${PROJECT_NAME} system filesystem thread) rosbuild_add_executable(test_serial src/test_serial.cpp) target_link_libraries(test_serial serial) +# rosbuild_add_executable(boosttest src/boosttest.cpp) \ No newline at end of file diff --git a/serial/include/serial.h b/serial/include/serial.h index 4b7600e..8847184 100644 --- a/serial/include/serial.h +++ b/serial/include/serial.h @@ -37,6 +37,7 @@ #define SERIAL_H #include +#include #include #include @@ -44,12 +45,23 @@ #include #include -#define READ_BUFFER_SIZE 4 -#define SERIAL_BUFFER_SIZE 1024 +// DEFINES +#define DEFAULT_BAUDRATE 9600 +#define DEFAULT_TIMEOUT 0.0 +#define DEFAULT_BYTESIZE EIGHTBITS +#define DEFAULT_PARITY PARITY_NONE +#define DEFAULT_STOPBITS STOPBITS_ONE +#define DEFAULT_FLOWCONTROL FLOWCONTROL_NONE + +// Serial Port settings CONSTANTS +enum { FIVEBITS, SIXBITS, SEVENBITS, EIGHTBITS }; +enum { PARITY_NONE, PARITY_ODD, PARITY_EVEN }; +enum { STOPBITS_ONE, STOPBITS_ONE_POINT_FIVE, STOPBITS_TWO }; +enum { FLOWCONTROL_NONE, FLOWCONTROL_SOFTWARE, FLOWCONTROL_HARDWARE }; class Serial { public: - /** Constructor, creates a SerialPortBoost object without opening a port. */ + /** Constructor, Creates a Serial object but doesn't open the serial port. */ Serial(); /** @@ -62,16 +74,42 @@ public: * @param baudrate An integer that represents the buadrate * * @param timeout A double that represents the time (in seconds) until a - * timeout on reads occur. Setting this to a negative number will - * silently disable the timeout on reads. + * timeout on reads occur. Setting this to a number less than or + * equal to zero will silently disable the timeout on reads. + * + * @param bytesize Size of each byte in the serial transmission of data, + * default is EIGHTBITS, possible values are: FIVEBITS, + * SIXBITS, SEVENBITS, EIGHTBITS + * + * @param parity Method of parity, default is PARITY_NONE, possible values + * are: PARITY_NONE, PARITY_ODD, PARITY_EVEN + * + * @param stopbits Number of stop bits used, default is STOPBITS_ONE, possible + * values are: STOPBITS_ONE, STOPBITS_ONE_POINT_FIVE, STOPBITS_TWO + * + * @param flowcontrol Type of flowcontrol used, default is FLOWCONTROL_NONE, possible + * values are: FLOWCONTROL_NONE, FLOWCONTROL_SOFTWARE, FLOWCONTROL_HARDWARE + * + * @throw SerialPortAlreadyOpenException + * @throw SerialPortFailedToOpenException */ - Serial(std::string port, int baudrate = 9600, double timeout = -1.0); - // Serial(string port, int baudrate = 9600, int bytesize = EIGHTBITS, int parity = PARITY_NONE, int stopbits = STOPBITS_ONE, double timeout = -1.0, bool xonxoff = false, bool rtscts = false, double writeTimeout = -1.0, bool dsrdtr = false, double interCharTimeout = -1.0); + Serial(std::string port, + int baudrate = DEFAULT_BAUDRATE, + double timeout = DEFAULT_TIMEOUT, + int bytesize = DEFAULT_BYTESIZE, + int parity = DEFAULT_PARITY, + int stopbits = DEFAULT_STOPBITS, + int flowcontrol = DEFAULT_FLOWCONTROL); /** Destructor */ ~Serial(); - /** Opens the serial port as long as the portname is set and the port isn't alreay open. */ + /** + * Opens the serial port as long as the portname is set and the port isn't alreay open. + * + * @throw SerialPortAlreadyOpenException + * @throw SerialPortFailedToOpenException + */ void open(); /** Closes the serial port and terminates threads. */ @@ -97,7 +135,7 @@ public: * * @return A std::string containing the data read. */ - std::string read(int size=1); + std::string read(int size = 1); /** Write length bytes from buffer to the serial port. * @@ -117,15 +155,6 @@ public: */ int write(std::string data); - /** Checks the number of bytes waiting in the buffer. - * - * @return An integer representing the number of bytes in the serial buffer. - */ - int inWaiting(); - - /** Flushes the write buffer, blocks untill all data has been written. */ - void flush(); - /** Sets the logic level of the RTS line. * * @param level The logic level to set the RTS to. Defaults to true. @@ -149,19 +178,184 @@ public: * @return A boolean value that represents the current logic level of the DSR line. */ bool getDSR(); + + /** Sets the timeout for reads in seconds. + * + * @param timeout A long that specifies how long the read timeout is in seconds. + */ + void setTimeoutMilliseconds(long timeout); + + /** Gets the timeout for reads in seconds. + * + * @return A long that specifies how long the read timeout is in seconds. + */ + long getTimeoutMilliseconds(); + + /** Sets the baudrate for the serial port. + * + * @param baudrate An integer that sets the baud rate for the serial port. + */ + void setBaudrate(int baudrate); + + /** Gets the baudrate for the serial port. + * + * @return An integer that sets the baud rate for the serial port. + */ + int getBaudrate(); + + /** Sets the bytesize for the serial port. + * + * @param bytesize Size of each byte in the serial transmission of data, + * default is EIGHTBITS, possible values are: FIVEBITS, + * SIXBITS, SEVENBITS, EIGHTBITS + */ + void setBytesize(int bytesize); + + /** Gets the bytesize for the serial port. + * + * @return Size of each byte in the serial transmission of data, + * default is EIGHTBITS, possible values are: FIVEBITS, + * SIXBITS, SEVENBITS, EIGHTBITS + */ + int getBytesize(); + + /** Sets the parity for the serial port. + * + * @param parity Method of parity, default is PARITY_NONE, possible values + * are: PARITY_NONE, PARITY_ODD, PARITY_EVEN + */ + void setParity(int parity); + + /** Gets the parity for the serial port. + * + * @return Method of parity, default is PARITY_NONE, possible values + * are: PARITY_NONE, PARITY_ODD, PARITY_EVEN + */ + int getParity(); + + /** Sets the stopbits for the serial port. + * + * @param stopbits Number of stop bits used, default is STOPBITS_ONE, possible + * values are: STOPBITS_ONE, STOPBITS_ONE_POINT_FIVE, STOPBITS_TWO + */ + void setStopbits(int stopbits); + + /** Gets the stopbits for the serial port. + * + * @return Number of stop bits used, default is STOPBITS_ONE, possible + * values are: STOPBITS_ONE, STOPBITS_ONE_POINT_FIVE, STOPBITS_TWO + */ + int getStopbits(); + + /** Sets the flow control for the serial port. + * + * @param flowcontrol Type of flowcontrol used, default is FLOWCONTROL_NONE, possible + * values are: FLOWCONTROL_NONE, FLOWCONTROL_SOFTWARE, FLOWCONTROL_HARDWARE + */ + void setFlowcontrol(int flowcontrol); + + /** Gets the flow control for the serial port. + * + * @return Type of flowcontrol used, default is FLOWCONTROL_NONE, possible + * values are: FLOWCONTROL_NONE, FLOWCONTROL_SOFTWARE, FLOWCONTROL_HARDWARE + */ + int getFlowcontrol(); private: + void init(); + void read_complete(const boost::system::error_code& error, std::size_t bytes_transferred); + void timeout_callback(const boost::system::error_code& error); + boost::asio::io_service io_service; - boost::asio::serial_port *serial_port; - boost::thread * io_service_thread; - - char read_buffer[READ_BUFFER_SIZE]; + boost::asio::io_service::work * work; + boost::asio::serial_port * serial_port; + boost::asio::deadline_timer * timeout_timer; std::string port; - int baudrate; - double timeout; + boost::asio::serial_port_base::baud_rate * baudrate; + boost::posix_time::time_duration * timeout; + boost::asio::serial_port_base::character_size * bytesize; + boost::asio::serial_port_base::parity * parity; + boost::asio::serial_port_base::stop_bits * stopbits; + boost::asio::serial_port_base::flow_control * flowcontrol; + + int bytes_read; + int bytes_to_read; + bool reading; +}; + +class SerialPortAlreadyOpenException : public std::exception { + const char * port; +public: + SerialPortAlreadyOpenException(const char * port) {this->port = port;} + + virtual const char* what() const throw() { + std::stringstream ss; + ss << "Serial Port already open: " << this->port; + return ss.str().c_str(); + } +}; + +class SerialPortFailedToOpenException : public std::exception { + const char * e_what; +public: + SerialPortFailedToOpenException(const char * e_what) {this->e_what = e_what;} + + virtual const char* what() const throw() { + std::stringstream ss; + ss << "Serial Port failed to open: " << this->e_what; + return ss.str().c_str(); + } +}; + +class InvalidBytesizeException : public std::exception { + int bytesize; +public: + InvalidBytesizeException(int bytesize) {this->bytesize = bytesize;} + + virtual const char* what() const throw() { + std::stringstream ss; + ss << "Invalid bytesize provided: " << this->bytesize; + return ss.str().c_str(); + } +}; + +class InvalidParityException : public std::exception { + int parity; +public: + InvalidParityException(int parity) {this->parity = parity;} + + virtual const char* what() const throw() { + std::stringstream ss; + ss << "Invalid parity provided: " << this->parity; + return ss.str().c_str(); + } +}; + +class InvalidStopbitsException : public std::exception { + int stopbits; +public: + InvalidStopbitsException(int stopbits) {this->stopbits = stopbits;} + + virtual const char* what() const throw() { + std::stringstream ss; + ss << "Invalid stopbits provided: " << this->stopbits; + return ss.str().c_str(); + } +}; + +class InvalidFlowcontrolException : public std::exception { + int flowcontrol; +public: + InvalidFlowcontrolException(int flowcontrol) {this->flowcontrol = flowcontrol;} + + virtual const char* what() const throw() { + std::stringstream ss; + ss << "Invalid flowcontrol provided: " << this->flowcontrol; + return ss.str().c_str(); + } }; #endif \ No newline at end of file diff --git a/serial/src/serial.cpp b/serial/src/serial.cpp index e9df07f..b0a5d6d 100644 --- a/serial/src/serial.cpp +++ b/serial/src/serial.cpp @@ -1,95 +1,391 @@ #include "serial.h" +/** Completion Conditions **/ + +class transfer_at_least_ignore_invalid_argument { +public: + typedef bool result_type; + + explicit transfer_at_least_ignore_invalid_argument(std::size_t minimum) : minimum_(minimum) {} + + template + bool operator()(const Error& err, std::size_t bytes_transferred) { + std::cout << "Here2" << std::endl; + if(err) {// There is an Error + if(err == boost::asio::error::operation_aborted) { + std::cout << "Here1" << std::endl; + return 1; + } + if(err != boost::asio::error::invalid_argument) {// The Error is not invalid argument + return 1; // Stop reading + } + } + if(bytes_transferred >= minimum_) {// We have all the bytes we need + return 1; // Stop + } else { + return 0; // Continue + } + } + +private: + std::size_t minimum_; +}; + +/** Classes for Handshaking control **/ + +#if defined(BOOST_WINDOWS) || defined(__CYGWIN__) +# define BOOST_ASIO_OPTION_STORAGE DCB +#else +# define BOOST_ASIO_OPTION_STORAGE termios +#endif + +class DTRControl { +public: + explicit DTRControl(bool enable = false) : m_enable(enable) {}; + + boost::system::error_code store(BOOST_ASIO_OPTION_STORAGE& storage, + boost::system::error_code& ec) const + { + #if defined(BOOST_WINDOWS) || defined(__CYGWIN__) + if(m_enable) + storage.fDtrControl = DTR_CONTROL_ENABLE; + else + storage.fDtrControl = DTR_CONTROL_DISABLE; + #else + ec = boost::asio::error::operation_not_supported; + ec = boost::system::error_code(); + #endif + return ec; + }; + + boost::system::error_code load(const BOOST_ASIO_OPTION_STORAGE& storage, + boost::system::error_code& ec) + { + #if defined(BOOST_WINDOWS) || defined(__CYGWIN__) + if(storage.fDtrControl == DTR_CONTROL_ENABLE) + m_enable = true; + else + m_enable = true; + #else + #endif + return ec; + }; +private: + bool m_enable; +}; + +class RTSControl { +public: + explicit RTSControl(bool enable = false) : m_enable(enable) {}; + boost::system::error_code store(BOOST_ASIO_OPTION_STORAGE& storage, + boost::system::error_code& ec) const + { + #if defined(BOOST_WINDOWS) || defined(__CYGWIN__) + if(m_enable) + storage.fRtsControl = RTS_CONTROL_ENABLE; + else + storage.fRtsControl = RTS_CONTROL_DISABLE; + #else + ec = boost::asio::error::operation_not_supported; + ec = boost::system::error_code(); + #endif + return ec; + }; + + boost::system::error_code load(const BOOST_ASIO_OPTION_STORAGE& storage, + boost::system::error_code& ec) + { + #if defined(BOOST_WINDOWS) || defined(__CYGWIN__) + if(storage.fRtsControl == RTS_CONTROL_ENABLE) + m_enable = true; + else + m_enable = true; + #else + #endif + return ec; + }; +private: + bool m_enable; +}; + +/** Serial Class Implementation **/ + Serial::Serial() { - ; + this->init(); } -Serial::Serial(std::string port, int baudrate, double timeout) { +Serial::Serial(std::string port, + int baudrate, + double timeout, + int bytesize, + int parity, + int stopbits, + int flowcontrol) +{ + // Call default constructor to initialize variables + this->init(); + + // Write provided settings this->port = port; - this->baudrate = baudrate; - this->timeout = timeout; + this->setBaudrate(baudrate); + this->setTimeoutMilliseconds(timeout); + this->setBytesize(bytesize); + this->setParity(parity); + this->setStopbits(stopbits); + this->setFlowcontrol(flowcontrol); + + // Open the serial port this->open(); } +void Serial::init() { + // Boost asio variables + this->work = new boost::asio::io_service::work(this->io_service); + this->serial_port = NULL; + this->timeout_timer = new boost::asio::deadline_timer(this->io_service); + + // Serial Port settings + this->port = ""; + this->setBaudrate(DEFAULT_BAUDRATE); + this->setTimeoutMilliseconds(DEFAULT_TIMEOUT); + + // Private variables + this->bytes_read = 0; + this->bytes_to_read = 0; + this->reading = false; +} + Serial::~Serial() { - ; + this->close(); } void Serial::open() { - this->serial_port = new boost::asio::serial_port(this->io_service, this->port); - - if(not serial_port->is_open()) { - std::cerr << "[AX2550] Failed to open serial port: " << this->port << std::endl; - return; + // Make sure the Serial port is not already open. + if(this->serial_port != NULL && this->serial_port->is_open()) { + throw(SerialPortAlreadyOpenException(this->port.c_str())); } + // Try to open the serial port try { - this->serial_port->set_option(boost::asio::serial_port_base::baud_rate(this->baudrate)); - // this->serial_port->set_option( - // boost::asio::serial_port_base::flow_control( - // boost::asio::serial_port_base::flow_control::none)); - // this->serial_port->set_option( - // boost::asio::serial_port_base::parity( - // boost::asio::serial_port_base::parity::even)); - // this->serial_port->set_option( - // boost::asio::serial_port_base::stop_bits( - // boost::asio::serial_port_base::stop_bits::one)); - // this->serial_port->set_option(boost::asio::serial_port_base::character_size(7)); + this->serial_port = new boost::asio::serial_port(this->io_service, this->port); + + this->serial_port->set_option(*this->baudrate); + this->serial_port->set_option(*this->flowcontrol); + this->serial_port->set_option(*this->parity); + this->serial_port->set_option(*this->stopbits); + this->serial_port->set_option(*this->bytesize); } catch(std::exception &e) { - std::cerr << "[AX2550] Error opening serial port: " << e.what() << std::endl; - try { - if(this->serial_port->is_open()) - this->serial_port->close(); - } catch(std::exception &e) { - std::cerr << "[AX2550] Error closing serial port: " << e.what() << std::endl; - } + throw(SerialPortFailedToOpenException(e.what())); this->serial_port = NULL; } } void Serial::close() { - ; + // Cancel the current timeout timer and async reads + this->timeout_timer->cancel(); + this->serial_port->cancel(); + this->serial_port->close(); } int Serial::read(char* buffer, int size) { - return this->serial_port->read_some(boost::asio::buffer(buffer, size)); + this->reading = true; + boost::asio::async_read(*(this->serial_port), boost::asio::buffer(buffer, size), transfer_at_least_ignore_invalid_argument(size), + boost::bind(&Serial::read_complete, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); + timeout_timer->expires_from_now(*this->timeout); + timeout_timer->async_wait(boost::bind(&Serial::timeout_callback, this, + boost::asio::placeholders::error)); + + while(this->reading) + this->io_service.run_one(); + + this->bytes_to_read = size; + + return this->bytes_read; } std::string Serial::read(int size) { char serial_buffer[size]; - int number_read = this->serial_port->read_some(boost::asio::buffer(serial_buffer, size)); - serial_buffer[number_read] = NULL; - return std::string(serial_buffer); + int bytes_read_ = this->read(serial_buffer, size); + return std::string(serial_buffer, (std::size_t)bytes_read_); +} + +void Serial::read_complete(const boost::system::error_code& error, std::size_t bytes_transferred) { + // std::cout << "Here2" << std::endl; + if(!error || error != boost::asio::error::operation_aborted) { // If there was no error OR the error wasn't operation aborted (canceled), Cancel the timer + this->timeout_timer->cancel(); // will cause timeout_callback to fire with an error + } + + this->bytes_read = bytes_transferred; + + this->reading = false; +} + +void Serial::timeout_callback(const boost::system::error_code& error) { + if (!error) { + // The timeout wasn't canceled, so cancel the async read + std::cout << "Here3" << std::endl; + this->serial_port->cancel(); + } } int Serial::write(char data[], int length) { - return -1; + return boost::asio::write(*this->serial_port, boost::asio::buffer(data, length), boost::asio::transfer_all()); } int Serial::write(std::string data) { - return -1; -} - -int Serial::inWaiting() { - return -1; -} - -void Serial::flush() { - ; + char * cstr; + cstr = new char[data.size()+1]; + std::strcpy(cstr, data.c_str()); + return this->write(cstr, data.length()); } void Serial::setRTS(bool level) { - ; + this->serial_port->set_option(RTSControl(level)); } void Serial::setDTR(bool level) { - ; + this->serial_port->set_option(DTRControl(level)); } bool Serial::getCTS() { + throw(boost::asio::error::operation_not_supported); return false; } bool Serial::getDSR() { + throw(boost::asio::error::operation_not_supported); return false; +} + +void Serial::setTimeoutMilliseconds(long timeout) { + if(timeout > 0.0) { + this->timeout = new boost::posix_time::milliseconds(timeout); + } else { + this->timeout = NULL; + } +} + +long Serial::getTimeoutMilliseconds() { + return this->timeout->total_milliseconds(); +} + +void Serial::setBaudrate(int baudrate) { + this->baudrate = new boost::asio::serial_port_base::baud_rate(baudrate); +} + +int Serial::getBaudrate() { + return this->baudrate->value(); +} + +void Serial::setBytesize(int bytesize) { + switch(bytesize) { + case FIVEBITS: + this->bytesize = new boost::asio::serial_port_base::character_size(5); + break; + case SIXBITS: + this->bytesize = new boost::asio::serial_port_base::character_size(6); + break; + case SEVENBITS: + this->bytesize = new boost::asio::serial_port_base::character_size(7); + break; + case EIGHTBITS: + this->bytesize = new boost::asio::serial_port_base::character_size(8); + break; + default: + throw(InvalidBytesizeException(bytesize)); + break; + } +} + +int Serial::getBytesize() { + return this->bytesize->value(); +} + +void Serial::setParity(int parity) { + switch(parity) { + case PARITY_NONE: + this->parity = new boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none); + break; + case PARITY_ODD: + this->parity = new boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::odd); + break; + case PARITY_EVEN: + this->parity = new boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::even); + break; + default: + throw(InvalidParityException(parity)); + break; + } +} + +int Serial::getParity() { + switch(this->parity->value()) { + case boost::asio::serial_port_base::parity::none: + return PARITY_NONE; + case boost::asio::serial_port_base::parity::odd: + return PARITY_ODD; + case boost::asio::serial_port_base::parity::even: + return PARITY_EVEN; + } + return -1; +} + +void Serial::setStopbits(int stopbits) { + switch(stopbits) { + case STOPBITS_ONE: + this->stopbits = new boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one); + break; + case STOPBITS_ONE_POINT_FIVE: + this->stopbits = new boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::onepointfive); + break; + case STOPBITS_TWO: + this->stopbits = new boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::two); + break; + default: + throw(InvalidStopbitsException(stopbits)); + break; + } +} + +int Serial::getStopbits() { + switch(this->parity->value()) { + case boost::asio::serial_port_base::stop_bits::one: + return STOPBITS_ONE; + case boost::asio::serial_port_base::stop_bits::onepointfive: + return STOPBITS_ONE_POINT_FIVE; + case boost::asio::serial_port_base::stop_bits::two: + return STOPBITS_TWO; + } + return -1; +} + +void Serial::setFlowcontrol(int flowcontrol) { + switch(flowcontrol) { + case FLOWCONTROL_NONE: + this->flowcontrol = new boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none); + break; + case FLOWCONTROL_SOFTWARE: + this->flowcontrol = new boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::software); + break; + case FLOWCONTROL_HARDWARE: + this->flowcontrol = new boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::hardware); + break; + default: + throw(InvalidFlowcontrolException(flowcontrol)); + break; + } +} + +int Serial::getFlowcontrol() { + switch(this->parity->value()) { + case boost::asio::serial_port_base::flow_control::none: + return FLOWCONTROL_NONE; + case boost::asio::serial_port_base::flow_control::software: + return FLOWCONTROL_SOFTWARE; + case boost::asio::serial_port_base::flow_control::hardware: + return FLOWCONTROL_HARDWARE; + } + return -1; } \ No newline at end of file diff --git a/serial/src/test_serial.cpp b/serial/src/test_serial.cpp index 2df4600..149ef1a 100644 --- a/serial/src/test_serial.cpp +++ b/serial/src/test_serial.cpp @@ -9,7 +9,7 @@ Serial *serial; std::string toHex(std::string input) { std::stringstream ss; - for(int i = 0; i != input.length(); i++) { + for(unsigned int i = 0; i != input.length(); i++) { char temp[4]; sprintf(temp, "%.2X", input[i]); ss << " "; @@ -31,19 +31,30 @@ int main(int argc, char **argv) ros::NodeHandle n; std::string port("/dev/tty.usbserial-A900cfJA"); + // std::string port("/dev/tty.usbmodemfa141"); - serial = new Serial(port); + serial = new Serial(port, 9600, 250); - ros::Rate loop_rate(10); + ros::Rate loop_rate(0.5); - while (ros::ok()) { + int count = 0; + while (ros::ok() and count != 30) { + // serial->write("Testing."); + // ROS_INFO("Out of write"); std::string result = serial->read(1); + std::cout << ">" << result << std::endl; + + + // ROS_INFO("Here."); + + // ROS_INFO(result.c_str()); + // ROS_INFO("%d,%s", result.length(), toHex(result).c_str()); - ROS_INFO("%d,%s", result.length(), toHex(result).c_str()); ros::spinOnce(); - loop_rate.sleep(); + // loop_rate.sleep(); + // count += 1; } return 0;