echo_driver.cpp

Go to the documentation of this file.

#include "toposens_echo_driver/echo_driver.h"

#include <ros/ros.h>

#include "toposens_echo_driver/adc_dump.h"
#include "toposens_echo_driver/lib_utils.h"
#include "toposens_msgs/TsScan.h"

namespace toposens_echo_driver
{
EchoOneDriver::EchoOneDriver(ros::NodeHandle nh, RosParameters parameters)
    : nh_(nh), parameters_(std::move(parameters))
{
  ROS_INFO("Instantiating EchoOneDriver!");
  initialize();
}

EchoOneDriver::~EchoOneDriver()
{
  if (parameters_.com_interface == "CAN")
  {
    DeinitCanInterface();
  }
  else if (parameters_.com_interface == "UART")
  {
    DeinitUARTInterface();
  }
}

void EchoOneDriver::initialize()
{
  ROS_DEBUG("EchoOneDriver: Initializing!");

  // Prepare sensor communication
  if (parameters_.com_interface == "CAN")
  {
    InitCanInterface(parameters_.can_device);
  }
  else if (parameters_.com_interface == "UART")
  {
    InitUARTInterface(parameters_.uart_device);
  }
  else
  {
    ROS_ERROR("Unsupported communication interface %s ", parameters_.com_interface.c_str());
    throw std::invalid_argument("Unsupported communication interface");
  }

  LogSettings();
  LogVersions();
  ConfigureSensorLogMessages(LogMsgCallback, LOG_LEVEL_INFO);

  // Prepare Single Measurement mode
  if (SetParameterSystemSensorMode(SENSOR_MODE_SINGLE_SHOT_TRANSMIT_LISTEN))
  {
    ROS_INFO("Successfully configured sensor mode SENSOR_MODE_SINGLE_SHOT_TRANSMIT_LISTEN!");
  }
  else
  {
    ROS_WARN("Start SENSOR_MODE_SINGLE_SHOT_TRANSMIT_LISTEN did not work!");
  }

  // Prepare publishers
  constexpr auto queue_size = 100;
  ts_scan_publisher_ = nh_.advertise<toposens_msgs::TsScan>(parameters_.scans_topic, queue_size);

  configureDynamicReconfigureServer();

  adc_dump_service_ = nh_.advertiseService("request_adc_dump", &requestAdcDump);
  RegisterADCDumpStartRequestCallback(requestAdcDumpCallback);
  RegisterADCDumpSessionEndCallback(adcDumpEndCallback);
}

void EchoOneDriver::measure()
{
  if (GetNumberOfKnownSensors() < 1)
  {
    ROS_ERROR("No sensors on bus, cannot measure!");
    return;
  }

  const auto* session_data = RequestSessionData();

  const auto msg = to_TsScan(session_data, parameters_);
  ts_scan_publisher_.publish(msg);
}

void EchoOneDriver::publishStaticTransforms()
{
  const auto ts = ros::Time::now();

  const auto base_frame = parameters_.frame_id;
  const auto target_frame = parameters_.target_frame;
  const auto target_frame_optical_frame = target_frame + "_optical_frame";

  auto zero_transform = {0.0F, 0.0F, 0.0F};

  tf::Quaternion q;
  q.setRPY(0, 0, 0);
  tf::Quaternion q_optical;
  q_optical.setRPY(-M_PI / 2.0, 0.0, -M_PI / 2.0);

  const auto tf_msg = getStaticTransformMsg(ts, zero_transform, q, base_frame, target_frame);
  const auto tf_optical_msg = getStaticTransformMsg(ts, zero_transform, q_optical, target_frame,
                                                    target_frame_optical_frame);

  static_tf_broadcaster_.sendTransform(tf_msg);
  static_tf_broadcaster_.sendTransform(tf_optical_msg);
}

void EchoOneDriver::configureDynamicReconfigureServer()
{
  // Set up dynamic reconfigure server
  // Set values from config file (otherwise the dynamic reconfigure values would be set in
  // rqt_reconfigure initially)
  ROS_DEBUG("Preparing dynamic reconfigure server...");

  dynamic_reconfigure_server_ = std::make_unique<EchoOneDriverConfigServer>(
      dynamic_reconfigure_server_mutex_, ros::NodeHandle(nh_, "EchoOneDriver"));

  EchoOneDriverConfig initial_config;
  initial_config.transducer_volume = parameters_.transducer_volume;
  initial_config.transducer_num_pulses = parameters_.transducer_num_pulses;
  initial_config.temperature = parameters_.temperature;
  dynamic_reconfigure_server_->updateConfig(initial_config);

  dynamic_reconfigure_server_->setCallback(
      [this](auto& config, auto level) { return reconfigure(config, level); });
}

void EchoOneDriver::reconfigure(const EchoOneDriverConfig& config, const uint32_t level)
{
  auto log_reconfigure_failure = [](const std::string& parameter_name) {
    ROS_WARN("Reconfiguring of parameter %s failed!", parameter_name.c_str());
  };

  switch (level)
  {
    case 1: {
      if (SetParameterTransducerVolume(static_cast<uint8_t>(config.transducer_volume)))
      {
        ROS_INFO("Successfully reconfigured %s to %d", parameters_.transducer_volume_str.c_str(),
                 config.transducer_volume);
      }
      else
      {
        log_reconfigure_failure(parameters_.transducer_volume_str);
      }
      break;
    }

    case 2: {
      if (SetParameterTransducerNumOfPulses(static_cast<uint8_t>(config.transducer_num_pulses)))
      {
        ROS_INFO("Successfully reconfigured %s to %d",
                 parameters_.transducer_num_pulses_str.c_str(), config.transducer_num_pulses);
      }
      else
      {
        log_reconfigure_failure(parameters_.transducer_num_pulses_str);
      }
      break;
    }

    // NOLINTNEXTLINE (cppcoreguidelines-avoid-magic-numbers)
    case 11: {
      if (SetParameterSignalProcessingTemperature(static_cast<float>(config.temperature)))
      {
        ROS_INFO("Successfully reconfigured %s to %f", parameters_.temperature_str.c_str(),
                 config.temperature);
      }
      else
      {
        log_reconfigure_failure(parameters_.temperature_str);
      }
      break;
    }

    default:
      ROS_WARN("Unknown reconfigure level: %d!", level);
      break;
  }
}

}  // namespace toposens_echo_driver