RokubiminiSerialImpl.cpp

Go to the documentation of this file.

/*
 * RokubiminiSerialImpl.hpp
 *  Created on: April, 2020
 *  Author(s):  Mike Karamousadakis, Ilias Patsiaouras
 */

#include <stdint.h>
#include <stdlib.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>
#include <cerrno>
#include <functional>
#include <thread>
#include <sys/wait.h>
#include <asm/ioctls.h>
#include <linux/serial.h>
#include <sys/ioctl.h>
#include <iostream>
#include <fstream>
#include <string>

#include "ros/ros.h"

#include <rokubimini_serial/RokubiminiSerialImpl.hpp>

namespace rokubimini
{
namespace serial
{
RokubiminiSerialImpl::RokubiminiSerialImpl(const std::string& name, const std::string& port,
                                           const std::uint32_t& baudRate)
  : name_(name)
  , port_(port)
  , baudRate_(baudRate)
  , usbFileDescriptor_{ -1 }
  , frameSync_{ false }
  , usbStreamIn_{ nullptr }
  , usbStreamOut_{ nullptr }
  , timeStampSecs_(0.0)
  , runInThreadedMode_(true)
  , useDeviceTimeStamps_(false)
  , connectionThread_()
  , pollingThread_()
  , connectionState_(ConnectionState::DISCONNECTED)
  , errorState_(ErrorState::NO_ERROR)
  , modeState_{ ModeState::RUN_MODE }
  , isRunning_{ true }
  , pollingSyncErrorCounter_(0)
  , frameReceivedCounter_(0)
  , frameSuccessCounter_(0)
  , frameCrcErrorCounter_(0)
  , frameSyncErrorCounter_(0)
  , maxFrameSyncErrorCounts_(10000)
{
  ROS_DEBUG("[%s] Device connected to serial port: %s with baudrate: %u \n", name_.c_str(), port_.c_str(), baudRate_);
  ros::Time::init();
}

bool RokubiminiSerialImpl::init()
{
  ROS_DEBUG("[%s] Attempting to start-up device.", name_.c_str());

  // Connect to serial port
  if (connect())
  {
    // Sleep for 3.0 seconds for the sensor to boot and start sending data.
    std::this_thread::sleep_for(std::chrono::microseconds(3000000));
    ROS_INFO("[%s] Start-up successful.", name_.c_str());
  }
  else
  {
    ROS_ERROR("[%s] Could not establish connection with device. Start-up failed.", name_.c_str());
    return false;
  }
  if (!setInitMode())
  {
    ROS_ERROR("[%s] Could not bring device to INIT mode.", name_.c_str());
    return false;
  }
  return true;
}

bool RokubiminiSerialImpl::startPollingThread()
{
  // Start the polling thread if driver is to run in threaded mode and
  // if the polling thread is not already running
  if (runInThreadedMode_ && !pollingThread_.joinable())
  {
    ROS_INFO("[%s] Launching polling thread.", name_.c_str());
    pollingThread_ = boost::thread{ &RokubiminiSerialImpl::pollingWorker, this };
  }
  return true;
}

bool RokubiminiSerialImpl::startup()
{
  return true;
}

void RokubiminiSerialImpl::shutdown()
{
  ROS_INFO("[%s] Driver will attempt to shut-down", name_.c_str());

  // Signal for termination (all background thread)
  isRunning_ = false;

  // Shutdown the connection thread if running
  if (runInThreadedMode_ && connectionThread_.joinable())
  {
    connectionThread_.join();
  }

  // Shutdown the polling thread if running
  if (runInThreadedMode_ && pollingThread_.joinable())
  {
    pollingThread_.join();
  }

  // Close the serial-port file descriptior
  if (usbFileDescriptor_ != -1)
  {
    ROS_INFO("[%s] Closing Serial Communication", name_.c_str());
    close(usbFileDescriptor_);
  }
  usbStreamIn_.close();
  usbStreamOut_.close();
  if (((usbStreamIn_.fail() | usbStreamOut_.fail()) != 0))
  {
    ROS_ERROR("[%s] Failed to close file streams.", name_.c_str());
  }

  ROS_INFO("[%s] Shut-down successful", name_.c_str());
}

bool RokubiminiSerialImpl::connect()
{
  // Abort attempt if the driver is already in the process of connecting
  if (connectionState_ == ConnectionState::ISCONNECTING)
  {
    return false;
  }
  else
  {
    connectionState_ = ConnectionState::ISCONNECTING;
  }

  // Close the file descriptor if it was already open
  if (usbFileDescriptor_ != -1)
  {
    close(usbFileDescriptor_);
    usbFileDescriptor_ = -1;
  }

  // Reset error counters and error state
  frameReceivedCounter_ = 0;
  frameSuccessCounter_ = 0;
  frameCrcErrorCounter_ = 0;
  frameSyncErrorCounter_ = 0;
  errorState_ = ErrorState::NO_ERROR;

  // Check if the driver is set to run in threaded mode
  if (!runInThreadedMode_)
  {
    return initSensorCommunication(false);
  }
  else
  {
    // Start the connection thread
    connectionThread_ = boost::thread{ &RokubiminiSerialImpl::connectionWorker, this };
  }

  return true;
}

bool RokubiminiSerialImpl::connect(const std::string& port)
{
  port_ = port;
  return connect();
}

bool RokubiminiSerialImpl::readDevice(RxFrame& frame)
{
  while (errorState_ == ErrorState::NO_ERROR && isRunning_ && modeState_ == ModeState::RUN_MODE)
  {
    /* read the next available byte and check if is the header
     * make sure to unget it after. emulating peek. Using static
     * variable for the sync flag will preserve status of sync
     * between calls */
    while (!frameSync_)
    {
      uint8_t possible_header;
      /* read bytes 1 by 1 to find the header */
      usbStreamIn_.read((char*)&possible_header, sizeof(possible_header));
      if (possible_header == frameHeader)
      {
        /* read the remaining frame to make check also CRC */
        usbStreamIn_.read((char*)&frame.bytes[sizeof(frame.header)], sizeof(frame) - sizeof(frame.header));
        if (frame.crc16_ccitt == calcCrc16X25(frame.data.bytes, sizeof(frame.data)))
        {
          ROS_INFO("[%s] Frame synced with 0x%X header", name_.c_str(), frameHeader);
          frameSync_ = true;
        }
        else
        {
          /* if there is a frame that included the header 0xAA in
           * a fixed position. Could be the above checking mechanism
           * will get stuck because will find the wrong value as header
           * then will remove from the buffer n bytes where n the size
           * of the frame and then will find again exactly the same
           * situation the wrong header. So we read on extra byte to make
           * sure next time will start from the position that is size of frame
           * plus 1. It works */
          char dummy;
          // usbStreamIn_.read((char*)&dummy, sizeof(dummy));
          usbStreamIn_.read((char*)&dummy, sizeof(dummy));
          ROS_WARN("[%s] Skipping incomplete frame", name_.c_str());
        }
      }
    }
    /* Read the sensor measurements frame assuming that is alligned with the RX buffer */
    try
    {
      usbStreamIn_.read((char*)frame.bytes, sizeof(frame));
    }
    catch (std::exception& e)
    {
      ROS_ERROR("[%s] Error while reading a packet, %s", name_.c_str(), e.what());
      errorState_ = ErrorState::PACKET_READING_ERROR;
      return false;
    }

    /* Check if the frame is still aligned, otherwise exit */
    if (frame.header != frameHeader)
    {
      frameSync_ = false;
      /* keep some statistics */
      if (++frameSyncErrorCounter_ >= maxFrameSyncErrorCounts_)
      {
        ROS_WARN("[%s] Reached max syncing errors. Disconnecting sensor.", name_.c_str());
        errorState_ = ErrorState::SYNC_ERROR;
        connectionState_ = ConnectionState::DISCONNECTED;
      }
      return false;
    }
    // Read and check CRC 16-bit
    try
    {
      uint16_t crc_received = frame.crc16_ccitt;
      uint16_t crc_computed = calcCrc16X25(frame.data.bytes, sizeof(frame.data));
      if (crc_received != crc_computed)
      {
        frameCrcErrorCounter_++;
        ROS_WARN("[%s] CRC missmatch received: 0x%04x calculated: 0x%04x", name_.c_str(), crc_received, crc_computed);
        return false;
      }
    }
    catch (std::exception& e)
    {
      ROS_ERROR("[%s] Error while reading a packet, %s", name_.c_str(), e.what());
      errorState_ = ErrorState::PACKET_READING_ERROR;
      return false;
    }

    if (frame.data.status.app_took_too_long)
    {
      ROS_WARN("[%s] Warning force sensor is skipping measurements, Increase sinc length", name_.c_str());
    }
    if (frame.data.status.overrange)
    {
      ROS_WARN("[%s] Warning measuring range exceeded", name_.c_str());
    }
    if (frame.data.status.invalid_measurements)
    {
      ROS_ERROR("[%s] Warning force torque measurements are invalid, Permanent damage may occur", name_.c_str());
    }
    if (frame.data.status.raw_measurements)
    {
      ROS_WARN_THROTTLE(1.0, "[%s] Warning raw force torque measurements from gages", name_.c_str());
    }
    frameSuccessCounter_++;
    return true;
  }
  // This point should not be reached
  return false;
}

bool RokubiminiSerialImpl::isConnected() const
{
  return (connectionState_ == ConnectionState::CONNECTED);
}

bool RokubiminiSerialImpl::isConnecting() const
{
  return (connectionState_ == ConnectionState::ISCONNECTING);
}

bool RokubiminiSerialImpl::hasError() const
{
  return (errorState_ != ErrorState::NO_ERROR);
}

bool RokubiminiSerialImpl::isInConfigMode() const
{
  return (modeState_ == ModeState::CONFIG_MODE);
}

ConnectionState RokubiminiSerialImpl::getConnectionState() const
{
  return connectionState_;
}

ErrorState RokubiminiSerialImpl::getErrorState() const
{
  return errorState_;
}

std::string RokubiminiSerialImpl::getErrorString() const
{
  switch (errorState_)
  {
    case ErrorState::NO_ERROR:
      return std::string{ "No Error" };

    case ErrorState::OFFSET_ERROR:
      return std::string{ "Offset Error" };

    case ErrorState::CALIBRATION_ERROR:
      return std::string{ "Calibration Error" };

    case ErrorState::PACKET_READING_ERROR:
      return std::string{ "Packet Reading Error" };

    case ErrorState::SYNC_ERROR:
      return std::string{ "Sync Error" };

    default:
      return std::string{ "Undefined" };
  }

  return std::string{ "No Error" };
}

bool RokubiminiSerialImpl::initSensorCommunication(bool keepConnecting)
{
  ROS_INFO("[%s] Initializing serial-port at: %s", name_.c_str(), port_.c_str());

  // Block, continuously trying to connect to the serial-port, until
  // signaled to stop or connection is established
  bool connected;
  do
  {
    connected = initSerialPort(port_);
    std::this_thread::sleep_for(std::chrono::microseconds(100000));
  } while (isRunning_ && keepConnecting && !connected);

  if (!isRunning_)
  {
    // connection process was cancelled from somewhere else
    ROS_ERROR("[%s] Sensor is disconnected.", name_.c_str());
    connectionState_ = ConnectionState::DISCONNECTED;
    return false;
  }

  connectionState_ = ConnectionState::CONNECTED;
  return true;
}

uint32_t RokubiminiSerialImpl::getBaudRateDefinition(const uint32_t& baudRate)
{
  switch (baudRate)
  {
    case 50:
      return B50;
    case 75:
      return B75;
    case 110:
      return B110;
    case 134:
      return B134;
    case 150:
      return B150;
    case 200:
      return B200;
    case 300:
      return B300;
    case 600:
      return B600;
    case 1200:
      return B1200;
    case 1800:
      return B1800;
    case 2400:
      return B2400;
    case 4800:
      return B4800;
    case 9600:
      return B9600;
    case 19200:
      return B19200;
    case 38400:
      return B38400;
    case 57600:
      return B57600;
    case 115200:
      return B115200;
    case 230400:
      return B230400;
    case 460800:
      return B460800;
    case 500000:
      return B500000;
    case 576000:
      return B576000;
    case 921600:
      return B921600;
    case 1000000:
      return B1000000;
    case 1152000:
      return B1152000;
    case 1500000:
      return B1500000;
    case 2000000:
      return B2000000;
    case 2500000:
      return B2500000;
    case 3000000:
      return B3000000;
    case 3500000:
      return B3500000;
    case 4000000:
      return B4000000;
    default:
      ROS_ERROR("[%s] Baud rate %u is not supported\n", name_.c_str(), baudRate);
      return B0;
  }
}
bool RokubiminiSerialImpl::initSerialPort(const std::string& port)
{
  // int flags;
  termios newtio;
  struct serial_struct ser_info;

  // make the fstreams unbuffered
  usbStreamIn_.rdbuf()->pubsetbuf(nullptr, 0);
  usbStreamOut_.rdbuf()->pubsetbuf(nullptr, 0);

  // Open the Serial Port
  usbFileDescriptor_ = open(port.c_str(), O_RDWR | O_NOCTTY);
  if (usbFileDescriptor_ < 0)
  {
    ROS_ERROR("[%s] Failed to open serial-port: '%s'", name_.c_str(), port.c_str());
    return false;
  }

  // Get the Serial Descriptor Flags
  if (tcgetattr(usbFileDescriptor_, &newtio) < 0)
  {
    ROS_ERROR("[%s] Failed to get connection attributes.", name_.c_str());
    return false;
  }

  // Set the Serial Speed
  uint32_t baud_rate_definition = getBaudRateDefinition(baudRate_);
  if (baud_rate_definition != 0)
  {
    cfsetispeed(&newtio, baud_rate_definition);
    cfsetospeed(&newtio, baud_rate_definition);
  }
  else
  {
    return false;
  }
  cfmakeraw(&newtio);

  // Set the Serial Connection Attributes (SCA)
  if (tcsetattr(usbFileDescriptor_, TCSAFLUSH, &newtio) < 0)
  {
    ROS_ERROR("[%s] Failed to set connection attributes.", name_.c_str());
    return false;
  }

  // Flush the input and output streams
  if (tcflush(usbFileDescriptor_, TCIOFLUSH) < 0)
  {
    ROS_ERROR("[%s] Failed to flush the input and output streams.", name_.c_str());
    return false;
  }

  // Enable linux FTDI low latency mode
  ioctl(usbFileDescriptor_, TIOCGSERIAL, &ser_info);
  ser_info.flags |= ASYNC_LOW_LATENCY;
  ioctl(usbFileDescriptor_, TIOCSSERIAL, &ser_info);

  // Get the Serial Descriptor Flags
  if (fcntl(usbFileDescriptor_, F_GETFL) < 0)
  {
    ROS_ERROR("[%s] Failed to set the descriptor flags.", name_.c_str());
    return false;
  }

  // create a std stream to read and write from/to device
  usbStreamIn_.open(port.c_str(), std::ifstream::in);
  usbStreamOut_.open(port.c_str(), std::ofstream::out);
  if (((usbStreamIn_.fail() | usbStreamOut_.fail()) != 0))
  {
    ROS_ERROR("[%s] Failed to open file streams.", name_.c_str());
    return false;
  }

  // wait another second to get measurements
  std::this_thread::sleep_for(std::chrono::seconds(1));
  return true;
}

inline uint16_t RokubiminiSerialImpl::calcCrc16X25(uint8_t* data, int len)
{
  uint16_t crc = 0xFFFF;
  while (len--)
    crc = crcCcittUpdate(crc, *data++);
  return ~crc;
}

uint16_t RokubiminiSerialImpl::crcCcittUpdate(uint16_t crc, uint8_t data)
{
#define lo8(x) ((x)&0xFF)
#define hi8(x) (((x) >> 8) & 0xFF)
  data ^= lo8(crc);
  data ^= data << 4;

  return ((((uint16_t)data << 8) | hi8(crc)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3));
}

void RokubiminiSerialImpl::connectionWorker()
{
  initSensorCommunication(true);
}

void RokubiminiSerialImpl::pollingWorker()
{
  // Configure the scheduling policy for the pollin gthread
  int priority = 99;
  sched_param params;
  params.sched_priority = priority;
  if (sched_setscheduler(getpid(), SCHED_FIFO, &params) != 0)
  {
    ROS_WARN("[%s] Failed to set thread priority to %d: %s.", name_.c_str(), priority, std::strerror(errno));
  }

  // Wait for connection to be established before starting to poll
  usleep(100000);
  if (!isConnected())
  {
    while (!isConnected() && isRunning_)
    {
      usleep(1000);
      ROS_WARN_THROTTLE(4.0, "[%s] Polling thread waiting for connection. ", name_.c_str());
    }
  }
  double time_stamp = 0.0;

  // Variables for timing
  timespec t_current, t_previous, t_cycle, t_loop;
  double time_delta;
  bool run_processing = true;
  unsigned long loop_counter = 0;
  pollingSyncErrorCounter_ = 0;

  // create timespec for the cycle time by breaking the time step into integral and fractional part with modf()
  double int_part;
  t_cycle.tv_nsec = NSEC_PER_SEC * modf(pollingThreadTimeStep_, &int_part);
  t_cycle.tv_sec = static_cast<uint64_t>(int_part);
  clock_gettime(CLOCK_MONOTONIC_RAW, &t_current);

  // Start sensor polling
  while (isRunning_ && modeState_ == ModeState::RUN_MODE)
  {
    t_previous = t_current;
    clock_gettime(CLOCK_MONOTONIC_RAW, &t_current);
    time_delta = diffSec(t_previous, t_current);
    if (time_delta > pollingThreadTimeStep_)
    {
      pollingSyncErrorCounter_++;
      ROS_WARN("[%s] Polling is out of sync! TimeDelta=%f", name_.c_str(), time_delta);
    }
    // Poll the serial device, skipping processing in current iteration if no valid data has been received
    RxFrame frame;

    std::unique_lock<std::recursive_mutex> lock(serialMutex_);
    (this->readDevice(frame)) ? (run_processing = true) : (run_processing = false);
    lock.unlock();
    // TODO: check if PID of sent data and PID configured match
    // We have to check the connection again because readDevice() may have signalled to disconnect
    // if too many communication errors occurred
    if (connectionState_ == ConnectionState::CONNECTED && run_processing)
    {
      /* Set the time-stamp */
      if (useDeviceTimeStamps_)
      {
        time_stamp = (double)frame.data.timestamp * 1e-6;
      }
      else
      {
        time_stamp = timespecToSec(t_current);
      }
      // Store the final measurement into the primary buffer
      {
        std::lock_guard<std::recursive_mutex> lock(readingMutex_);
        frame_ = frame;
        timeStampSecs_ = time_stamp;
      }
    }
    t_loop = timespecAdd(t_current, t_cycle);
    // Sleep to implement cyclic polling
    clock_nanosleep(CLOCK_MONOTONIC_RAW, TIMER_ABSTIME, &t_loop, nullptr);
    loop_counter++;
  }

  // Show error statistics if errors have been detected
  if ((pollingSyncErrorCounter_ > 0) || (frameSyncErrorCounter_ > 0) || (frameCrcErrorCounter_ > 0))
  {
    ROS_WARN("[%s] Sensor polling thread error report:", name_.c_str());
    ROS_WARN("[%s]   Total iterations:    %lu", name_.c_str(), loop_counter);
    ROS_WARN("[%s]   Frames received:     %lu, (%f%%)", name_.c_str(), frameReceivedCounter_,
             100.0 * ((double)frameReceivedCounter_ / (double)loop_counter));
    ROS_WARN("[%s]   Correct frames:      %lu, (%f%%)", name_.c_str(), frameSuccessCounter_,
             100.0 * ((double)frameSuccessCounter_ / (double)frameReceivedCounter_));
    ROS_WARN("[%s]   Polling sync errors: %lu, (%f%%)", name_.c_str(), pollingSyncErrorCounter_,
             100.0 * ((double)pollingSyncErrorCounter_ / (double)loop_counter));
    ROS_WARN("[%s]   Frame CRC errors:    %lu, (%f%%)", name_.c_str(), frameCrcErrorCounter_,
             100.0 * ((double)frameCrcErrorCounter_ / (double)frameReceivedCounter_));
    ROS_WARN("[%s]   Frame sync errors:   %d", name_.c_str(), frameSyncErrorCounter_);
  }
}

void RokubiminiSerialImpl::getReading(rokubimini::Reading& reading)
{
  if (isConnected())
  {
    {
      std::lock_guard<std::recursive_mutex> lock(readingMutex_);

      const auto& stamp = ros::Time::now();

      serialImplReading_.getWrench().header.stamp = stamp;
      serialImplReading_.getWrench().header.frame_id = name_ + "_wrench";
      serialImplReading_.getWrench().wrench.force.x = frame_.data.forces[0];
      serialImplReading_.getWrench().wrench.force.y = frame_.data.forces[1];
      serialImplReading_.getWrench().wrench.force.z = frame_.data.forces[2];
      serialImplReading_.getWrench().wrench.torque.x = frame_.data.forces[3];
      serialImplReading_.getWrench().wrench.torque.y = frame_.data.forces[4];
      serialImplReading_.getWrench().wrench.torque.z = frame_.data.forces[5];

      serialImplReading_.getTemperature().header.stamp = stamp;
      serialImplReading_.getTemperature().header.frame_id = name_ + "_temp";
      serialImplReading_.getTemperature().temperature = frame_.data.temperature;
      serialImplReading_.getTemperature().variance = 0;  // unknown variance
      reading = serialImplReading_;
    }
  }
}

bool RokubiminiSerialImpl::setConfigMode()
{
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  auto config_command = RokubiminiSerialCommandConfig();
  std::string str;
  if (!config_command.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the config command", name_.c_str());
    return false;
  }
  bool success = writeSerial(str);
  if (!success)
  {
    lock.unlock();
    return false;
  }
  else
  {
    modeState_ = ModeState::CONFIG_MODE;
    lock.unlock();
    // required delay for the sensor to load all the defaults
    std::this_thread::sleep_for(std::chrono::microseconds(1500000));
    if (runInThreadedMode_ && pollingThread_.joinable())
    {
      pollingThread_.join();
    }
    // Reset error counters and error state
    frameReceivedCounter_ = 0;
    frameSuccessCounter_ = 0;
    frameCrcErrorCounter_ = 0;
    frameSyncErrorCounter_ = 0;
    errorState_ = ErrorState::NO_ERROR;
    return true;
  }
}

bool RokubiminiSerialImpl::setRunMode()
{
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  auto run_command = RokubiminiSerialCommandRun();
  std::string str;
  if (!run_command.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the run command", name_.c_str());
    return false;
  }
  bool success = writeSerial(str);
  lock.unlock();
  if (!success)
  {
    return false;
  }

  modeState_ = ModeState::RUN_MODE;
  // Start a new polling thread.
  return startPollingThread();
}

bool RokubiminiSerialImpl::setHardwareReset()
{
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  auto hard_reset_command = RokubiminiSerialCommandHardReset();
  std::string str;
  if (!hard_reset_command.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the hardware reset command", name_.c_str());
    return false;
  }
  bool success = writeSerial(str);
  lock.unlock();
  return success;
}

bool RokubiminiSerialImpl::setInitMode()
{
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  auto soft_reset_command = RokubiminiSerialCommandSoftReset();
  std::string str;
  if (!soft_reset_command.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the software reset command", name_.c_str());
    return false;
  }
  bool success = writeSerial(str);
  if (!success)
  {
    lock.unlock();
    return false;
  }
  else
  {
    modeState_ = ModeState::CONFIG_MODE;
    lock.unlock();

    std::this_thread::sleep_for(std::chrono::microseconds(500000));
    if (runInThreadedMode_ && pollingThread_.joinable())
    {
      pollingThread_.join();
    }
    // Reset error counters and error state
    frameReceivedCounter_ = 0;
    frameSuccessCounter_ = 0;
    frameCrcErrorCounter_ = 0;
    frameSyncErrorCounter_ = 0;
    errorState_ = ErrorState::NO_ERROR;
    return true;
  }
}

bool RokubiminiSerialImpl::setForceTorqueFilter(const configuration::ForceTorqueFilter& filter)
{
  if (!isInConfigMode())
  {
    return false;
  }

  ROS_DEBUG("[%s] Setting force/torque filter", name_.c_str());
  ROS_DEBUG("[%s] \tsize: %u", name_.c_str(), filter.getSincFilterSize());
  ROS_DEBUG("[%s] \tchop: %u", name_.c_str(), filter.getChopEnable());
  ROS_DEBUG("[%s] \tfast: %u", name_.c_str(), filter.getFastEnable());
  ROS_DEBUG("[%s] \tskip: %u", name_.c_str(), filter.getSkipEnable());

  auto filter_command = RokubiminiSerialCommandFilter(filter);
  std::string str;
  if (!filter_command.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the filter command", name_.c_str());
    return false;
  }
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  bool success = writeSerial(str);
  lock.unlock();
  return success;
}

bool RokubiminiSerialImpl::setForceTorqueOffset(const Eigen::Matrix<double, 6, 1>& forceTorqueOffset)
{
  if (!isInConfigMode())
  {
    return false;
  }

  ROS_DEBUG_STREAM("[" << name_.c_str() << "] Setting Force/Torque offset: " << forceTorqueOffset << std::endl);
  auto offset_command = RokubiminiSerialCommandOffset(forceTorqueOffset);
  std::string str;
  if (!offset_command.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the force torque offset command", name_.c_str());
    return false;
  }
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  bool success = writeSerial(str);
  lock.unlock();
  return success;
}

bool RokubiminiSerialImpl::setSensorCalibration(const calibration::SensorCalibration& sensorCalibration)
{
  if (!isInConfigMode())
  {
    return false;
  }
  char buffer[100];
  bool success = true;
  for (uint32_t row = 0; row < 6; row++)
  {
    auto sensor_calibration_row_command = RokubiminiSerialCommandSensorCalibrationRow(
        sensorCalibration.getCalibrationMatrix()(row, 0), sensorCalibration.getCalibrationMatrix()(row, 1),
        sensorCalibration.getCalibrationMatrix()(row, 2), sensorCalibration.getCalibrationMatrix()(row, 3),
        sensorCalibration.getCalibrationMatrix()(row, 4), sensorCalibration.getCalibrationMatrix()(row, 5), row);
    std::string str;
    if (!sensor_calibration_row_command.formatCommand(str))
    {
      ROS_ERROR("[%s] Could not format the calibration matrix command", name_.c_str());
      return false;
    }
    ROS_DEBUG_STREAM("[" << name_.c_str() << "] Calibration matrix line is: " << str << ". Size is " << str.size());
    std::unique_lock<std::recursive_mutex> lock(serialMutex_);
    success &= writeSerial(str);
    lock.unlock();
    memset(buffer, 0, sizeof(buffer));
    // Sleep for 10.0 milliseconds before sending the next chunk of data.
    std::this_thread::sleep_for(std::chrono::microseconds(10000));
  }
  return success;
}

bool RokubiminiSerialImpl::setSensorConfiguration(const configuration::SensorConfiguration& sensorConfiguration)
{
  if (!isInConfigMode())
  {
    return false;
  }
  ROS_DEBUG("[%s] Setting sensor configuration", name_.c_str());

  uint32_t baud_rate = baudRate_;
  uint8_t data_format = 0;
  return setCommunicationSetup(sensorConfiguration, data_format, baud_rate);
}
bool RokubiminiSerialImpl::setCommunicationSetup(const configuration::SensorConfiguration& sensorConfiguration,
                                                 const uint8_t& dataFormat, const uint32_t& baudRate)
{
  ROS_DEBUG("[%s] Setting communication setup", name_.c_str());
  uint8_t baud_rate;
  switch (baudRate)
  {
    case 9600:
      baud_rate = 0;
      break;
    case 57600:
      baud_rate = 1;
      break;
    case 115200:
      baud_rate = 2;
      break;
    case 230400:
      baud_rate = 3;
      break;
    case 460800:
      baud_rate = 4;
      break;
    default:
      ROS_ERROR("[%s] The baud rate %u is not supported\n", name_.c_str(), baudRate);
      return false;
  }
  auto comm_setup = RokubiminiSerialCommandCommSetup(sensorConfiguration, dataFormat, baud_rate);
  std::string str;
  if (!comm_setup.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the communication setup command", name_.c_str());
    return false;
  }
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  bool success = writeSerial(str);
  lock.unlock();
  return success;
}

bool RokubiminiSerialImpl::saveConfigParameter()
{
  if (!isInConfigMode())
  {
    return false;
  }
  auto save_config = RokubiminiSerialCommandSave();
  std::string str;
  if (!save_config.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the save config command", name_.c_str());
    return false;
  }
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  bool success = writeSerial(str);
  lock.unlock();
  return success;
}
bool RokubiminiSerialImpl::loadConfig()
{
  if (!isInConfigMode())
  {
    return false;
  }
  auto load_config = RokubiminiSerialCommandLoad();
  std::string str;
  if (!load_config.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the load config command", name_.c_str());
    return false;
  }
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  bool success = writeSerial(str);
  lock.unlock();
  return success;
}
bool RokubiminiSerialImpl::printUserConfig()
{
  if (!isInConfigMode())
  {
    return false;
  }
  auto print_config = RokubiminiSerialCommandPrint();
  std::string str;
  if (!print_config.formatCommand(str))
  {
    ROS_ERROR("[%s] Could not format the print config command", name_.c_str());
    return false;
  }
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  bool success = writeSerial(str);
  lock.unlock();
  if (!success)
  {
    return false;
  }
  uint8_t c;
  uint64_t timespec_diff_ns;
  timespec tnow, t_loop;
  clock_gettime(CLOCK_MONOTONIC_RAW, &tnow);
  t_loop = tnow;
  lock.lock();
  ROS_INFO("[%s] Printing user configuration:", name_.c_str());
  do
  {
    /* run the next readsome() only if there are available bytes in the input buffer.*/
    if (usbStreamIn_.rdbuf()->in_avail() > 0)
    {
      /* read bytes 1 by 1 and print them.*/
      usbStreamIn_.readsome((char*)&c, 1);
      printf("%c", c);
    }
    clock_gettime(CLOCK_MONOTONIC_RAW, &t_loop);
    timespec_diff_ns = (t_loop.tv_sec - tnow.tv_sec) * 1e9 + (t_loop.tv_nsec - tnow.tv_nsec);
  } while (timespec_diff_ns < 1e9);  // run the loop for 1sec.
  lock.unlock();
  return true;
}

bool RokubiminiSerialImpl::writeSerial(const std::string& str)
{
  try
  {
    // check that the string size doesn't exceed the serial input buffer's size (64) of the device.
    if (str.size() > 64)
    {
      ROS_WARN("[%s] String's length exceeds permittable limit (64)", name_.c_str());
      return false;
    }
    ROS_DEBUG("[%s] Number of chars: %zu", name_.c_str(), str.size());
    ROS_DEBUG("[%s] String chars: %s", name_.c_str(), str.c_str());
    if (usbStreamIn_.is_open() && usbStreamOut_.is_open())
    {
      usbStreamIn_.sync();
      char cstr[str.size() + 1];
      strcpy(cstr, str.c_str());
      for (uint8_t i = 0; i < str.size(); i++)
      {
        usbStreamOut_.put(cstr[i]);
        std::this_thread::sleep_for(std::chrono::microseconds(5000));
        usbStreamOut_.flush();
      }
      if ((usbStreamOut_.fail() | usbStreamIn_.fail()) != 0)
      {
        ROS_WARN("[%s] Serial Write or Read failed", name_.c_str());
        return false;
      }
    }
    else
    {
      ROS_WARN("[%s] Streams are not yet open.", name_.c_str());
      return false;
    }
  }
  catch (const std::exception& e)
  {
    ROS_ERROR("[%s] %s", name_.c_str(), e.what());
    return false;
  }
  return true;
}

bool RokubiminiSerialImpl::firmwareUpdate(const std::string& filePath)
{
  pid_t pid;
  int status;
  ROS_INFO("Flashing firmware...");
  std::unique_lock<std::recursive_mutex> lock(serialMutex_);
  setHardwareReset();
  pid = fork();

  if (pid == -1)
  {
    // pid == -1 means error occured
    ROS_ERROR("Could not fork, error occured");
    lock.unlock();
    return false;
  }
  else if (pid == 0)
  {
    ROS_DEBUG("Child process, pid = %u\n", getpid());
    // pid == 0 means child process created
    char path_argument[filePath.size() + 15];
    int ret = sprintf(path_argument, "-Uflash:w:%s:i", filePath.c_str());
    if (ret < 0)
    {
      ROS_ERROR("[%s] sprintf failed to write data", name_.c_str());
      lock.unlock();
      return false;
    }
    char port_argument[port_.size() + 2];
    ret = sprintf(port_argument, "-P%s", port_.c_str());
    if (ret < 0)
    {
      ROS_ERROR("[%s] sprintf failed to write data", name_.c_str());
      lock.unlock();
      return false;
    }
    // the argv list first argument should point to
    // filename associated with file being executed
    // the array pointer must be terminated by a NULL
    // pointer
    const char* argv_list[9] = { "avrdude",  "-v", "-patmega328p", "-carduino", port_argument,
                                 "-b230400", "-D", path_argument,  nullptr };
    ROS_DEBUG("Complete command for avrdude:");
    for (std::size_t i = 0; i < sizeof(argv_list); i++)
    {
      ROS_DEBUG("%s", argv_list[i]);
    }
    ROS_DEBUG("\n");
    // the execv() only returns if error occurred (then the return value is -1)
    ret = execvp(argv_list[0], const_cast<char* const*>(argv_list));
    if (ret < 0)
    {
      ROS_ERROR("[%s] Execution of avrdude failed: %s", name_.c_str(), strerror(errno));
      kill(getpid(), SIGKILL);
    }
    exit(0);
  }
  else
  {
    // a positive number is returned which is the pid of child process
    // the parent process calls waitpid() on the child.
    // waitpid() system call suspends execution of
    // calling process until a child specified by pid
    // argument has changed state
    // see wait() man page for all the flags or options
    // used here
    if (waitpid(pid, &status, WUNTRACED) > 0)
    {
      if (WIFEXITED(status) && !WEXITSTATUS(status))
      {
        ROS_INFO("[%s] avrdude returned successfully", name_.c_str());
        // stop polling and prepare to shutdown
        isRunning_ = false;
      }
      else if (WIFEXITED(status) && WEXITSTATUS(status))
      {
        if (WEXITSTATUS(status) == 127)
        {
          // execvp failed
          ROS_ERROR("[%s] execvp failed", name_.c_str());
          lock.unlock();
          return false;
        }
        else
        {
          ROS_ERROR("[%s] program terminated normally,"
                    " but returned a non-zero status",
                    name_.c_str());
          // stop polling and prepare to shutdown
          isRunning_ = false;
          lock.unlock();
          return false;
        }
      }
      else
      {
        ROS_ERROR("[%s] program didn't terminate normally", name_.c_str());
        lock.unlock();
        return false;
      }
    }
    else
    {
      // waitpid() failed
      ROS_ERROR("[%s] waitpid() failed", name_.c_str());
      lock.unlock();
      return false;
    }
  }
  lock.unlock();
  return true;
}
}  // namespace serial
}  // namespace rokubimini