From 255e0e705d087382da22077e2da309522fce4f53 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Sat, 22 May 2021 12:46:47 -0700 Subject: [PATCH 1/3] resolved inbound serial comunication problems --- vesc_driver/src/vesc_driver.cpp | 5 +++- vesc_driver/src/vesc_interface.cpp | 38 +++++++++++++++++++++++------- 2 files changed, 34 insertions(+), 9 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 07d5760..d5dc0cf 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,7 +65,10 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - std::string port = declare_parameter("port", ""); + // default to /tmp/ttyV0 for debug purpose + // use socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' + // to sniff comunication data to and from vesc + std::string port = declare_parameter("port", "/tmp/ttyV0"); // attempt to connect to the serial port try { diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 0b0c20d..b56cd5d 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -67,12 +67,18 @@ class VescInterface::Impl void VescInterface::Impl::rxThread() { + //the size varies dynamically and start from 0. capacity is 4096 fixed. Buffer buffer; buffer.reserve(4096); + //buffer with fixed size used to read from serial + Buffer bufferRx(4096); + while (rx_thread_run_) { int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + //std::cout << "loop " << bytes_needed < lock(serial_mutex_); + // removed temporarily because it causes a deadlock + //std::lock_guard lock(serial_mutex_); + boost::system::error_code ec; const size_t bytes_read = boost::asio::read( serial_port_, - boost::asio::buffer(buffer, buffer.size()), - boost::asio::transfer_exactly(bytes_to_read)); + boost::asio::buffer(bufferRx, bufferRx.size()), + boost::asio::transfer_exactly(bytes_to_read), + ec + ); + + if (ec.value() > 0 ){ + std::ostringstream ss; + ss << "Serial port comunication error "<< ec << ec < 0 && 0 == bytes_read && !buffer.empty()) { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); @@ -201,7 +223,7 @@ void VescInterface::connect(const std::string & port) boost::asio::serial_port::stop_bits::one)); } catch (const std::exception & e) { std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); + ss << "Failed to open the serial port "<< port <<" to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } @@ -221,21 +243,21 @@ void VescInterface::disconnect() impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); - std::lock_guard lock(impl_->serial_mutex_); + //std::lock_guard lock(impl_->serial_mutex_); impl_->serial_port_.close(); } } bool VescInterface::isConnected() const { - std::lock_guard lock(impl_->serial_mutex_); + //std::lock_guard lock(impl_->serial_mutex_); return impl_->serial_port_.is_open(); } void VescInterface::send(const VescPacket & packet) { try { - std::lock_guard lock(impl_->serial_mutex_); + //std::lock_guard lock(impl_->serial_mutex_); size_t written = impl_->serial_port_.write_some( boost::asio::buffer(packet.frame())); if (written != packet.frame().size()) { From d9a8c9be43080c648e2e3f2b89f2340f4b26d1fd Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Mon, 24 May 2021 00:37:10 -0700 Subject: [PATCH 2/3] fix test results --- vesc_driver/src/vesc_driver.cpp | 8 ++++-- vesc_driver/src/vesc_interface.cpp | 44 +++++++++++++++--------------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index d5dc0cf..96ef1a1 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,9 +65,11 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - // default to /tmp/ttyV0 for debug purpose - // use socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' - // to sniff comunication data to and from vesc + /* default to /tmp/ttyV0 for debug purpose + * use : + * socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' + * to sniff comunication data to and from vesc + */ std::string port = declare_parameter("port", "/tmp/ttyV0"); // attempt to connect to the serial port diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index b56cd5d..21fec38 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -67,18 +67,16 @@ class VescInterface::Impl void VescInterface::Impl::rxThread() { - //the size varies dynamically and start from 0. capacity is 4096 fixed. + // the size varies dynamically and start from 0. capacity is 4096 fixed. Buffer buffer; buffer.reserve(4096); - //buffer with fixed size used to read from serial + // buffer with fixed size used to read from serial Buffer bufferRx(4096); while (rx_thread_run_) { int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - //std::cout << "loop " << bytes_needed < lock(serial_mutex_); + // std::lock_guard lock(serial_mutex_); boost::system::error_code ec; const size_t bytes_read = boost::asio::read( @@ -145,19 +143,21 @@ void VescInterface::Impl::rxThread() boost::asio::buffer(bufferRx, bufferRx.size()), boost::asio::transfer_exactly(bytes_to_read), ec - ); - - if (ec.value() > 0 ){ - std::ostringstream ss; - ss << "Serial port comunication error "<< ec << ec < 0) { + std::ostringstream ss; + + ss << "Serial port comunication error " << ec << ec << std::endl; + ss << "failed " << ec.failed() << std::endl; + ss << ec.value() << std::endl; + ss << ec.category().name() << std::endl; + ss << "try to read the bytes received " << std::endl; + + error_handler_(ss.str()); + } + + std::copy(bufferRx.begin(), bufferRx.begin() + bytes_read, std::back_inserter(buffer)); if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); @@ -223,7 +223,7 @@ void VescInterface::connect(const std::string & port) boost::asio::serial_port::stop_bits::one)); } catch (const std::exception & e) { std::stringstream ss; - ss << "Failed to open the serial port "<< port <<" to the VESC. " << e.what(); + ss << "Failed to open the serial port " << port << " to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } @@ -243,21 +243,21 @@ void VescInterface::disconnect() impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); - //std::lock_guard lock(impl_->serial_mutex_); + // std::lock_guard lock(impl_->serial_mutex_); impl_->serial_port_.close(); } } bool VescInterface::isConnected() const { - //std::lock_guard lock(impl_->serial_mutex_); + // std::lock_guard lock(impl_->serial_mutex_); return impl_->serial_port_.is_open(); } void VescInterface::send(const VescPacket & packet) { try { - //std::lock_guard lock(impl_->serial_mutex_); + // std::lock_guard lock(impl_->serial_mutex_); size_t written = impl_->serial_port_.write_some( boost::asio::buffer(packet.frame())); if (written != packet.frame().size()) { From 21d75d331d4c727e35bd59c36e33b7b6d3d47c9c Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Fri, 4 Jun 2021 00:05:05 +0200 Subject: [PATCH 3/3] removed commented-out code --- vesc_driver/src/vesc_interface.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 21fec38..728c820 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -134,8 +134,6 @@ void VescInterface::Impl::rxThread() int bytes_to_read = std::min(bytes_needed, 4096); { - // removed temporarily because it causes a deadlock - // std::lock_guard lock(serial_mutex_); boost::system::error_code ec; const size_t bytes_read = boost::asio::read( @@ -148,7 +146,7 @@ void VescInterface::Impl::rxThread() if (ec.value() > 0) { std::ostringstream ss; - ss << "Serial port comunication error " << ec << ec << std::endl; + ss << "Serial port comunication error " << std::endl; ss << "failed " << ec.failed() << std::endl; ss << ec.value() << std::endl; ss << ec.category().name() << std::endl; @@ -242,8 +240,6 @@ void VescInterface::disconnect() // bring down read thread impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); - - // std::lock_guard lock(impl_->serial_mutex_); impl_->serial_port_.close(); } }