Merge pull request mit-racecar#11 from anscipione/Ros2_bug_fixing_ser…

…ial_comunication

resolved inbound serial comunication problems

vesc_driver/src/vesc_driver.cpp

@@ -65,7 +65,12 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options)
   fw_version_minor_(-1)
 {
   // get vesc serial port address
-  std::string port = declare_parameter<std::string>("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<std::string>("port", "/tmp/ttyV0");
 
   // attempt to connect to the serial port
   try {

vesc_driver/src/vesc_interface.cpp

@@ -67,9 +67,13 @@ 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;
     if (!buffer.empty()) {
@@ -127,15 +131,31 @@ void VescInterface::Impl::rxThread()
     }
 
     // attempt to read at least bytes_needed bytes from the serial port
-    int bytes_to_read = std::max(bytes_needed, 4096);
+    int bytes_to_read = std::min(bytes_needed, 4096);
 
     {
-      std::lock_guard<std::mutex> 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 " << 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.");
@@ -201,7 +221,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());
   }
 
@@ -220,22 +240,20 @@ void VescInterface::disconnect()
     // bring down read thread
     impl_->rx_thread_run_ = false;
     impl_->rx_thread_->join();
-
-    std::lock_guard<std::mutex> lock(impl_->serial_mutex_);
     impl_->serial_port_.close();
   }
 }
 
 bool VescInterface::isConnected() const
 {
-  std::lock_guard<std::mutex> lock(impl_->serial_mutex_);
+  // std::lock_guard<std::mutex> lock(impl_->serial_mutex_);
   return impl_->serial_port_.is_open();
 }
 
 void VescInterface::send(const VescPacket & packet)
 {
   try {
-    std::lock_guard<std::mutex> lock(impl_->serial_mutex_);
+    // std::lock_guard<std::mutex> lock(impl_->serial_mutex_);
     size_t written = impl_->serial_port_.write_some(
       boost::asio::buffer(packet.frame()));
     if (written != packet.frame().size()) {