sensor.cpp

Go to the documentation of this file.

#include "toposens_driver/sensor.h"

namespace toposens_driver
{
Sensor::Sensor(ros::NodeHandle nh, ros::NodeHandle private_nh)
{
  std::string port;
  private_nh.getParam("port", port);
  private_nh.getParam("frame_id", _frame_id);

  // Set up serial connection to sensor
  _serial = std::make_unique<Serial>(port);

  // Set up dynamic reconfigure to change sensor settings
  _srv = std::make_unique<Cfg>(mutex, private_nh);
  Cfg::CallbackType f = boost::bind(&Sensor::_reconfig, this, _1, _2);
  _srv->setCallback(f);

  // Publishing topic for TsScans
  _pub = nh.advertise<toposens_msgs::TsScan>(kScansTopic, kQueueSize);
  ROS_INFO("Publishing toposens data to /%s", kScansTopic);

  setMode(ScanContinuously);
  _displayFirmwareVersion();
  //_synchronizeParameterValues();
}

Sensor::Sensor(ros::NodeHandle nh, ros::NodeHandle private_nh, std::string port,
               std::string frame_id)
{
  _frame_id = frame_id;

  // Set up serial connection to sensor
  _serial = std::make_unique<Serial>(port);

  // Set up dynamic reconfigure to change sensor settings
  _srv = std::make_unique<Cfg>(mutex, private_nh);
  Cfg::CallbackType f = boost::bind(&Sensor::_reconfig, this, _1, _2);
  _srv->setCallback(f);

  // Publishing topic for TsScans
  _pub = nh.advertise<toposens_msgs::TsScan>(kScansTopic, kQueueSize);
  ROS_INFO("Publishing toposens data to /%s", kScansTopic);

  _displayFirmwareVersion();
  //_synchronizeParameterValues();
}

void Sensor::setMode(ScanMode scan_mode)
{
  Command cSMode(TsParam::ScanMode, scan_mode);
  _serial->sendCmd(cSMode, _buffer);
  if (!_evaluateAck(cSMode, _buffer.str())) ROS_WARN("Sending scan mode command failed");
}

void Sensor::_displayFirmwareVersion()
{
  Command cFirmwareVers(TsService::FirmwareVersion);
  _serial->sendCmd(cFirmwareVers, _buffer);

  std::string data = _buffer.str();
  size_t frame_start = data.find('S');

  int ack = (data[frame_start + 6] - '0');
  if (data[frame_start + 5] == '-') ack *= -1;

  // The retrieving firmware version number from acknowledgement message.
  try
  {
    if (ack == 7)
    {
      auto i = data.begin() + 8;
      ROS_INFO("Firmware version: %d", (int)_toNum(i));
    }
    else
      throw "Invalid acknowledgement error";
  }
  catch (...)
  {
    ROS_INFO("Firmware version could not be retrieved");
  }
}

bool Sensor::poll(void)
{
  _scan.header.stamp = ros::Time::now();
  _scan.header.frame_id = _frame_id;
  _scan.points.clear();

  _serial->getFrame(_buffer);
  Sensor::_parse(_buffer.str());

  _pub.publish(_scan);

  _buffer.str(std::string());
  _buffer.clear();

  if (_scan.points.empty())
    return false;
  else
    return true;
}

void Sensor::shutdown()
{
  _serial.reset();
  _srv.reset();
}

void Sensor::_init(void)
{
  bool success = true;

  Command cPulses(TsParam::NumberOfPulses, _cfg.num_pulses);
  _serial->sendCmd(cPulses, _buffer);
  if (!_evaluateAck(cPulses, _buffer.str())) success = false;

  Command cDetect(TsParam::PeakDetectionWindow, _cfg.peak_detection_window);
  _serial->sendCmd(cDetect, _buffer);
  if (!_evaluateAck(cDetect, _buffer.str())) success = false;

  Command cReject(TsParam::EchoRejectionThreshold, _cfg.echo_rejection_threshold);
  _serial->sendCmd(cReject, _buffer);
  if (!_evaluateAck(cReject, _buffer.str())) success = false;

  Command cNoise(TsParam::NoiseIndicatorThreshold, _cfg.noise_indicator_threshold);
  _serial->sendCmd(cNoise, _buffer);
  if (!_evaluateAck(cNoise, _buffer.str())) success = false;

  Command cTemp(TsParam::ExternalTemperature, _cfg.use_external_temperature
                                                  ? _cfg.external_temperature
                                                  : USE_INTERNAL_TEMPERATURE);
  _serial->sendCmd(cTemp, _buffer);
  if (!_evaluateAck(cTemp, _buffer.str())) success = false;

  if (success)
    ROS_INFO("Sensor settings initialized");
  else
    ROS_WARN("One or more settings failed to initialize");
}

void Sensor::_reconfig(TsDriverConfig &cfg, uint32_t level)
{
  if (level == 0b00000000) return;

  _cfg = cfg;
  if (level == -1) return Sensor::_init();

  if ((int)level >= 0b10000000)
  {
    ROS_INFO("Update skipped: Parameter not recognized");
    return;
  }

  // Decode level Bit mask
  if (level & 0b00000010)
  {
    Command cmd = Command(TsParam::EchoRejectionThreshold, _cfg.echo_rejection_threshold);
    _serial->sendCmd(cmd, _buffer);

    if (_evaluateAck(cmd, _buffer.str()))
      ROS_INFO("Sensor setting updated");
    else
      ROS_WARN("Settings update failed");
  }

  if (level & 0b00000100)
  {
    Command cmd = Command(TsParam::NoiseIndicatorThreshold, _cfg.noise_indicator_threshold);
    _serial->sendCmd(cmd, _buffer);

    if (_evaluateAck(cmd, _buffer.str()))
      ROS_INFO("Sensor setting updated");
    else
      ROS_WARN("Settings update failed");
  }

  if (level & 0b00001000)
  {
    Command cmd = Command(TsParam::NumberOfPulses, _cfg.num_pulses);
    _serial->sendCmd(cmd, _buffer);

    if (_evaluateAck(cmd, _buffer.str()))
      ROS_INFO("Sensor setting updated");
    else
      ROS_WARN("Settings update failed");
  }

  if (level & 0b00010000)
  {
    Command cmd = Command(TsParam::PeakDetectionWindow, _cfg.peak_detection_window);
    _serial->sendCmd(cmd, _buffer);

    if (_evaluateAck(cmd, _buffer.str()))
      ROS_INFO("Sensor setting updated");
    else
      ROS_WARN("Settings update failed");
  }

  if (level & 0b00100001)
  {
    // Sends externally provided temperature to Ts sensor, or value indicating
    // to read temperature from internal temperature sensor.
    Command cmd = Command(TsParam::ExternalTemperature, _cfg.use_external_temperature
                                                            ? _cfg.external_temperature
                                                            : USE_INTERNAL_TEMPERATURE);
    _serial->sendCmd(cmd, _buffer);

    if (_evaluateAck(cmd, _buffer.str()))
      ROS_INFO("Sensor setting updated");
    else
      ROS_WARN("Settings update failed");
  }
}

void Sensor::_parse(const std::string &frame)
{
  auto i = frame.begin();

  while (*i != 'S')
    if (++i == frame.end()) return;
  _scan.noisy = (*(++i) == '1');

  for (i; i < frame.end(); ++i)
  {
    // Find next X-tag in the frame
    while (*i != 'X')
      if (++i == frame.end()) return;

    try
    {
      toposens_msgs::TsPoint pt;
      pt.location.x = _toNum(++i) / 1000.0;

      if (*(++i) == 'Y')
        pt.location.y = _toNum(++i) / 1000.0;
      else
        throw std::invalid_argument("Expected Y-tag not found");

      if (*(++i) == 'Z')
        pt.location.z = _toNum(++i) / 1000.0;
      else
        throw std::invalid_argument("Expected Z-tag not found");

      if (*(++i) == 'V')
        pt.intensity = _toNum(++i) / 100.0;
      else
        throw std::invalid_argument("Expected V-tag not found");

      if (pt.intensity > 0) _scan.points.push_back(pt);
    }
    catch (const std::exception &e)
    {
      ROS_INFO("Skipped invalid point in stream");
      ROS_DEBUG("Error: %s in message %s", e.what(), frame.c_str());
    }
  }
}

Command *Sensor::_parseAck(const std::string &data)
{
  size_t frame_start = data.find('S');
  int ack = (data[frame_start + 6] - '0');
  if (data[frame_start + 5] == '-') ack *= -1;

  float val = std::atof(&data[frame_start + 8]);
  if ((TsParam)(1 << ack) == TsParam::ExternalTemperature) val /= 10;

  return ack > 0 ? new Command((TsParam)(1 << ack), val) : nullptr;
}

bool Sensor::_evaluateAck(Command &tx_cmd, const std::string &data)
{
  Command *rx_cmd = _parseAck(data);
  if (rx_cmd == nullptr) return false;

  // Compare parameter levels of command and acknowledgement frames.
  if (tx_cmd.getParam() == rx_cmd->getParam())
  {
    // Compare 5 value bytes of command and acknowledgement frames.
    if (strncmp(&rx_cmd->getBytes()[6], &tx_cmd.getBytes()[6], 5) == 0)
    {
      if (tx_cmd.getParam() != TsParam::ScanMode)
      {
        ROS_INFO_STREAM("TS parameter: " << tx_cmd.getParamName() << " updated to "
                                         << (tx_cmd.getParam() == TsParam::ExternalTemperature
                                                 ? std::atof(&rx_cmd->getBytes()[6]) / 10
                                                 : std::atof(&rx_cmd->getBytes()[6])));
      }
      return true;
    }
    else if (strcmp(tx_cmd.getBytes(), "CsTemp-1000\r") == 0)
    {
      ROS_INFO_STREAM("TS parameter: " << tx_cmd.getParamName()
                                       << " set to use internal temperature sensor.");
      return true;
    }
    else
    {
      ROS_WARN_STREAM("TS parameter: " << tx_cmd.getParamName() << " clipped to "
                                       << std::atof(&rx_cmd->getBytes()[6]));
    }
  }
  else
  {
    ROS_WARN_STREAM("TS parameter: " << tx_cmd.getParamName() << " value update failed!");
  }

  return false;
}

void Sensor::_updateConfig(TsParam param, int value)
{
  if (param == TsParam::NumberOfPulses)
    _cfg.num_pulses = value;
  else if (param == TsParam::PeakDetectionWindow)
    _cfg.peak_detection_window = value;
  else if (param == TsParam::EchoRejectionThreshold)
    _cfg.echo_rejection_threshold = value;
  else if (param == TsParam::ExternalTemperature)
    _cfg.external_temperature = value;
  else if (param == TsParam::NoiseIndicatorThreshold)
    _cfg.noise_indicator_threshold = value;

  _srv->updateConfig(_cfg);
  return;
}

void Sensor::_synchronizeParameterValues()
{
  // Query current parameter configurations from TS sensor
  Command cmd = Command(TsService::FirmwareConfiguration);
  _serial->sendCmd(cmd, _buffer);
  std::string data;
  std::size_t frame_start;
  int ack = 0;

  while (true)
  {
    _buffer.str(std::string());
    _buffer.clear();

    _serial->getFrame(_buffer);

    data = _buffer.str().c_str();

    if (_serial->isAcknowledgementFrame(_buffer.str()))
    {
      // Parse acknowledgement message
      Command *rx_cmd = _parseAck(data);
      _updateConfig(rx_cmd->getParam(), rx_cmd->getValue());
    }
    else
    {
      // All acknowledgement messages processed.
      return;
    }
  }
}

float Sensor::_toNum(auto &i)
{
  // Size of X, Y, Z, V data is always 5 bytes
  int abs = 0, factor = 1, length = 5;

  // Throw exception if first character is not a number or "-"
  if (*i == '-')
    factor = -1;
  else if (*i != '0')
    throw std::invalid_argument("Invalid value char");

  while (--length)
  {
    int d = *(++i) - '0';
    if (d >= 0 && d <= 9)
      abs = abs * 10 + d;
    else
      throw std::bad_cast();
  }

  return (float)(factor * abs);
}

}  // namespace toposens_driver