RokubiminiSerialCommunication.cpp

Go to the documentation of this file.

#include <rokubimini_serial/RokubiminiSerialCommunication.hpp>
#include <ros/ros.h>

namespace rokubimini
{
namespace serial
{
RokubiminiSerialCommandCommSetup::RokubiminiSerialCommandCommSetup(
    const configuration::SensorConfiguration& sensorConfiguration, const uint8_t& dataFormat, const uint8_t& baudRate)
  : sensorConfiguration_(sensorConfiguration), dataFormat_(dataFormat), baudRate_(baudRate)
{
}

bool RokubiminiSerialCommandCommSetup::formatCommand(std::string& formattedString)
{
  char buffer[100];
  int bytes_count = sprintf(buffer, formatString_.c_str(), sensorConfiguration_.getTemperatureCompensationActive(),
                            sensorConfiguration_.getCalibrationMatrixActive(), dataFormat_, baudRate_);

  if (bytes_count < 0)
  {
    ROS_ERROR("Failed to format communication setup command");
    return false;
  }
  formattedString.clear();
  formattedString.assign(buffer, bytes_count);
  return true;
}

RokubiminiSerialCommandFilter::RokubiminiSerialCommandFilter(const configuration::ForceTorqueFilter& filter)
  : filter_(filter)
{
}

bool RokubiminiSerialCommandFilter::formatCommand(std::string& formattedString)
{
  char buffer[100];
  int bytes_count = sprintf(buffer, formatString_.c_str(), filter_.getSincFilterSize(), filter_.getChopEnable(),
                            filter_.getFastEnable(), filter_.getSkipEnable());

  if (bytes_count < 0)
  {
    ROS_ERROR("Failed to format filters command");
    return false;
  }
  formattedString.clear();
  formattedString.assign(buffer, bytes_count);
  return true;
}

RokubiminiSerialCommandOffset::RokubiminiSerialCommandOffset(const Eigen::Matrix<double, 6, 1>& forceTorqueOffset)
  : forceTorqueOffset_(forceTorqueOffset)
{
}

bool RokubiminiSerialCommandOffset::formatCommand(std::string& formattedString)
{
  char buffer[100];
  int bytes_count =
      sprintf(buffer, formatString_.c_str(), forceTorqueOffset_(0, 0), forceTorqueOffset_(1, 0),
              forceTorqueOffset_(2, 0), forceTorqueOffset_(3, 0), forceTorqueOffset_(4, 0), forceTorqueOffset_(5, 0));

  if (bytes_count < 0)
  {
    ROS_ERROR("Failed to format offset command");
    return false;
  }
  formattedString.clear();
  formattedString.assign(buffer, bytes_count);
  return true;
}

RokubiminiSerialCommandSensorCalibrationRow::RokubiminiSerialCommandSensorCalibrationRow(
    const double& sensorCalibration1, const double& sensorCalibration2, const double& sensorCalibration3,
    const double& sensorCalibration4, const double& sensorCalibration5, const double& sensorCalibration6,
    const uint32_t& row)
  : sensorCalibration1_(sensorCalibration1)
  , sensorCalibration2_(sensorCalibration2)
  , sensorCalibration3_(sensorCalibration3)
  , sensorCalibration4_(sensorCalibration4)
  , sensorCalibration5_(sensorCalibration5)
  , sensorCalibration6_(sensorCalibration6)
  , row_(row)
{
}

bool RokubiminiSerialCommandSensorCalibrationRow::formatCommand(std::string& formattedString)
{
  char buffer[100];
  int bytes_count = sprintf(buffer, formatString_.c_str(), row_, sensorCalibration1_, sensorCalibration2_,
                            sensorCalibration3_, sensorCalibration4_, sensorCalibration5_, sensorCalibration6_);

  if (bytes_count < 0)
  {
    ROS_ERROR("Failed to format offset command");
    return false;
  }
  formattedString.clear();
  formattedString.assign(buffer, bytes_count);
  return true;
}

bool RokubiminiSerialCommandHardReset::formatCommand(std::string& formattedString)
{
  formattedString.clear();
  formattedString.assign(formatString_);
  return true;
}

bool RokubiminiSerialCommandSoftReset::formatCommand(std::string& formattedString)
{
  formattedString.clear();
  formattedString.assign(formatString_);
  return true;
}

bool RokubiminiSerialCommandConfig::formatCommand(std::string& formattedString)
{
  formattedString.clear();
  formattedString.assign(formatString_);
  return true;
}

bool RokubiminiSerialCommandRun::formatCommand(std::string& formattedString)
{
  formattedString.clear();
  formattedString.assign(formatString_);
  return true;
}

bool RokubiminiSerialCommandSave::formatCommand(std::string& formattedString)
{
  formattedString.clear();
  formattedString.assign(formatString_);
  return true;
}

bool RokubiminiSerialCommandLoad::formatCommand(std::string& formattedString)
{
  formattedString.clear();
  formattedString.assign(formatString_);
  return true;
}

bool RokubiminiSerialCommandPrint::formatCommand(std::string& formattedString)
{
  formattedString.clear();
  formattedString.assign(formatString_);
  return true;
}

bool RokubiminiSerialResponseRegexCommSetup::matchInString(const std::string& str)
{
  bool success = false;
  std::smatch matches;
  success = regex_search(str, matches, stringRegex_);
  if (matches.empty() || !matches.ready())
  {
    ROS_ERROR("No matches were found");
    return false;
  }
  ROS_INFO("Match size is: %lu", matches.size());
  for (unsigned i = 0; i < matches.size(); ++i)
  {
    ROS_INFO_STREAM("match " << i << ": " << matches[i]);
    ROS_INFO_STREAM(" (with a length of " << matches[i].length() << ")");
  }
  std::string s = matches[0];
  char temp_comp, calibration, data_format, baud_rate;
  ROS_DEBUG_STREAM("String is: " << s << " and format is: " << format_);
  int bytes_count = sscanf(s.c_str(), format_.c_str(), &temp_comp, &calibration, &data_format, &baud_rate);
  if (bytes_count < 4)
  {
    ROS_ERROR("Failed to format communication setup fields");
    return false;
  }
  // use character arithmetic for extracting integer from ASCII codes.
  tempComp_ = temp_comp - '0';
  calibration_ = calibration - '0';
  dataFormat_ = data_format - '0';
  baudRate_ = baud_rate - '0';
  ROS_INFO("Baud rate is: %u, data format is: %u, temp comp: %u, calibration: %u", getBaudRate(), getDataFormat(),
           getTempComp(), getCalibration());
  return success;
}

}  // namespace serial
}  // namespace rokubimini