lib_utils.cpp

Go to the documentation of this file.

#include "toposens_echo_driver/lib_utils.h"

#include <ros/ros.h>

namespace toposens_echo_driver
{
void InitCanInterface(const std::string& can_device, const int data_rate)
{
  InitInterface(const_cast<char*>(can_device.c_str()), data_rate, IF_CAN);
}

void DeinitCanInterface() { DeinitInterface(IF_CAN); }

void InitUARTInterface(const std::string& uart_device, const int data_rate)
{
  InitInterface(const_cast<char*>(uart_device.c_str()), data_rate, IF_UART);
}

void DeinitUARTInterface() { DeinitInterface(IF_UART); }

auto RequestSessionData() -> Sensor_Session_t*
{
  auto* const known_sensors = GetKnownSensors();
  // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic)
  SetTargetSensor(known_sensors[0].InterfaceSensorId_u16);

  RequestMeasurement();
  uint16_t sender = WaitForSessionEnd();
  Sensor_Session_t* session_data = GetSessionData(sender);

  return session_data;
}

auto SetTargetSensor(uint16_t TargetSensor_u16) -> bool
{
  // TODO(tobi) Add GetTargetSensor to toposens_can_lib and compare if already set correctly

  auto* const known_sensors = GetKnownSensors();
  const auto number_of_sensors = GetNumberOfKnownSensors();
  for (auto i = 0; i < number_of_sensors; ++i)
  {
    if (known_sensors[i].InterfaceSensorId_u16 == TargetSensor_u16)
    {
      ::SetTargetSensor(TargetSensor_u16);
      return true;
    }
  }

  ROS_ERROR("Requested target sensor ID (%d) is not known on bus!", TargetSensor_u16);
  return false;
}

void LogSettings()
{
  std::stringstream ss;
  ss << "Current sensor settings:\n";

  const auto FixedFrameRate_b = GetParameterADCUseFixedFrameRate_b();
  ss << "FixedFrameRate_b: " << FixedFrameRate_b;
  ss << "GetParameterADCFixedFrameRate_u8: " << std::to_string(GetParameterADCFixedFrameRate_u8());
  ss << "\nGetParameterTransducerVolume_u8: " << std::to_string(GetParameterTransducerVolume_u8());
  ss << "\nGetParameterTransducerNumOfPulses_u8: "
     << std::to_string(GetParameterTransducerNumOfPulses_u8());
  ss << "\nGetParameterSignalProcessingTemperature_f: "
     << std::to_string(GetParameterSignalProcessingTemperature_f());
  ss << "\nGetParameterSignalProcessingHumidity_u8: "
     << std::to_string(GetParameterSignalProcessingHumidity_u8());
  ss << "\nGetParameterSystemNodeID_u16: " << std::to_string(GetParameterSystemNodeID_u16());
  ss << "\nGetParameterSystemMCUTemperature_f: "
     << std::to_string(GetParameterSystemMCUTemperature_f());
  ss << "\nGetParameterSystemLogLevel_t: " << std::to_string(GetParameterSystemLogLevel_t());
  ss << "\nGetParameterSystemResetReason_t: " << std::to_string(GetParameterSystemResetReason_t());
  ss << "\nGetParameterSystemSensorState_t: " << std::to_string(GetParameterSystemSensorState_t());
  ss << "\nGetParameterSystemSensorMode_t: " << std::to_string(GetParameterSystemSensorMode_t());

  constexpr auto max_id_index = 2;
  for (auto i = 0; i <= max_id_index; ++i)
  {
    const auto UniqueID_t = GetParameterSystemUniqueID_t(i);
    ss << "\nUniqueID_cp[" << i << "]: ";
    for (auto j = 0; j < UID_SIZE; ++j)
    {
      // NOLINTNEXTLINE (cppcoreguidelines-pro-bounds-constant-array-index)
      ss << toHexString(UniqueID_t.values[j]);
      if (j < (UID_SIZE - 1))
      {
        ss << ".";
      }
    }
  }

  ROS_INFO("%s", ss.str().c_str());
}

void LogKnownSensors(const double log_interval)
{
  static ros::Time last_log_timestamp;
  const auto logging_interval{ros::Duration(log_interval)};

  if ((ros::Time::now() - last_log_timestamp) > logging_interval)
  {
    const auto number_of_sensors_on_bus = GetNumberOfKnownSensors();
    auto* known_sensors = GetKnownSensors();

    std::stringstream ss;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic)
    ss << "[" << std::to_string(known_sensors[0].InterfaceSensorId_u16);
    for (auto i = 1; i < number_of_sensors_on_bus; ++i)
    {
      ss << ", ";
      // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic)
      ss << std::to_string(known_sensors[i].InterfaceSensorId_u16);
    }
    ss << "]";

    ROS_INFO("%u known sensors on bus. IDs: %s", number_of_sensors_on_bus, ss.str().c_str());

    last_log_timestamp = ros::Time::now();
  }
}

void LogVersions()
{
  Version_t version;
  std::stringstream ss;
  ss << "Versions:\n";

  auto to_string = [](Version_t version) {
    return std::to_string(version.major) + "." + std::to_string(version.minor) + "." +
           std::to_string(version.hotfix);
  };

  version = RequestVersion_t(VERSION_BYTE_BOOTLOADER);
  ss << "Bootloader: " << to_string(version) << ",\n";
  version = RequestVersion_t(VERSION_BYTE_APP);
  ss << "App: " << to_string(version) << ",\n";
  version = RequestVersion_t(VERSION_BYTE_HW);
  ss << "Hardware: " << to_string(version) << ",\n";
  version = RequestVersion_t(VERSION_BYTE_SIG_PRO_LIB);
  ss << "Sig Pro Lib: " << to_string(version) << ",\n";
  version = RequestVersion_t(VERSION_BYTE_COMMS_LIB);
  ss << "Comms Lib: " << to_string(version);

  ROS_INFO("%s", ss.str().c_str());
}

void LogSessionData(const Sensor_Session_t* session)
{
  if (session == nullptr)
  {
    ROS_WARN("LogSessionData: Received request to log session == nullptr!");
  }

  auto to_string = [](bool b) { return b ? "true" : "false"; };

  ROS_DEBUG(
      "LogSessionData:\nSessionActive_b:\t%s,\nsenderId_u16:\t\t%d,\nNumberOfPoints_u8:"
      "\t%"
      "d\nNoiseLevel_u16:\t\t%d\nNearFieldPoint_b:\t%"
      "s",
      // NOLINTNEXTLINE(clang-analyzer-core.NullDereference) session has been checked for nullptr
      to_string(session->SessionState), session->SenderId_u16, session->NumberOfPoints_u8,
      session->NoiseLevel_u16, to_string(session->NearFieldPoint_b));

  // 1D Points
  std::stringstream ss;
  // NOLINTNEXTLINE(clang-analyzer-core.NullDereference) session has been checked for nullptr
  ss << "1D Points [VectorLength, Intensity], " << std::to_string(session->NumberOf1DPoints)
     << " total:";
  for (auto i = 0; i < session->NumberOf1DPoints; ++i)
  {
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    ss << "\n\t" << i << ": [" << std::to_string(session->Point1D_tp[i].VectorLength_u16)
       << ", "
       // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
       << std::to_string(session->Point1D_tp[i].Intensity_u8) << "]";
  }
  ROS_DEBUG("%s", ss.str().c_str());

  // 3D Points
  ss.str(std::string());
  ss << "3D Points [x, y, z, intensity], " << std::to_string(session->NumberOf3DPoints)
     << " total:";
  for (auto i = 0; i < session->NumberOf3DPoints; ++i)
  {
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    ss << "\n\t" << i << ": [" << std::to_string(session->Point3D_tp[i].X_i16)
       << ", "
       // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
       << std::to_string(session->Point3D_tp[i].Y_i16)
       << ", "
       // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
       << std::to_string(session->Point3D_tp[i].Z_i16)
       << ", "
       // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
       << std::to_string(session->Point3D_tp[i].Intensity_u8) << "]";
  }
  ROS_DEBUG("%s", ss.str().c_str());
}

void ConfigureSensorLogMessages(void (*Callback)(uint16_t Sender_u16, uint8_t* ReceivedPayload_pu8),
                                const LogLevel_t log_level = LOG_LEVEL_INFO)
{
  RegisterLogMsgCallback(Callback);
  if (SetParameterSystemLogLevel(log_level))
  {
    ROS_DEBUG("Successfully configured sensor log level (%d)!", log_level);
  }
  else
  {
    ROS_WARN("Failed to configure sensor log level (%d)!", log_level);
  }
}

void LogMsgCallback(uint16_t SenderId_u16, uint8_t* ReceivedPayload_pu8)
{
  char message_buffer[MAX_PARSED_LOG_MESSAGE_LEN];
  ParseLogMessageToText(message_buffer, SenderId_u16, ReceivedPayload_pu8);

  std::stringstream ss;
  ss << "Log-Msg Callback from Sender " << SenderId_u16 << ": " << message_buffer;

  const auto log_msg = ss.str();

  switch (ReceivedPayload_pu8[LOGGING_SEVERITY_BYTE])
  {
    case CONTROL_BYTE_LOG_DEBUG:
      ROS_DEBUG_STREAM(log_msg.c_str());
      break;
    case CONTROL_BYTE_LOG_INFO:
      ROS_INFO_STREAM(log_msg.c_str());
      break;
    case CONTROL_BYTE_LOG_WARN:
      ROS_WARN_STREAM(log_msg.c_str());
      break;
    case CONTROL_BYTE_LOG_ERROR:
      ROS_ERROR_STREAM(log_msg.c_str());
      break;
    default:
      break;
  }
}

}  // namespace toposens_echo_driver