ros_utils.cpp

Go to the documentation of this file.

#include "toposens_echo_driver/ros_utils.h"

#include "toposens_msgs/TsScan.h"

namespace toposens_echo_driver
{
[[nodiscard]] auto RosParameters::to_string() const -> std::string
{
  std::stringstream ss;

  ss << "com_interface: " << com_interface << std::endl;
  ss << "can_device: " << can_device << std::endl;
  ss << "uart_device: " << uart_device << std::endl;
  ss << "scans_topic: " << scans_topic << std::endl;
  ss << "sensor_mode: " << sensor_mode << std::endl;
  ss << "frame_id: " << frame_id << std::endl;
  ss << "target_frame: " << target_frame << std::endl;
  ss << "loop_rate_hz: " << std::fixed << std::to_string(loop_rate_hz) << std::endl;
  ss << "temperature: " << std::to_string(temperature) << std::endl;
  ss << "transducer_volume: " << std::to_string(transducer_volume) << std::endl;
  ss << "transducer_num_pulses: " << std::to_string(transducer_num_pulses);

  return ss.str();
}

RosParameters::RosParameters(const ros::NodeHandle private_nh) { load(private_nh); }

void RosParameters::load(const ros::NodeHandle private_nh)
{
  bool success = true;

  success &= private_nh.getParam("com_interface", com_interface);
  success &= private_nh.getParam("can_device", can_device);
  success &= private_nh.getParam("uart_device", uart_device);
  success &= private_nh.getParam("scans_topic", scans_topic);
  success &= private_nh.getParam("sensor_mode", sensor_mode);
  success &= private_nh.getParam("frame_id", frame_id);
  success &= private_nh.getParam("target_frame", target_frame);
  success &= private_nh.getParam("loop_rate", loop_rate_hz);
  success &= private_nh.getParam("signal_processing/temperature", temperature);
  success &= private_nh.getParam("transducer/volume", transducer_volume);
  success &= private_nh.getParam("transducer/num_pulses", transducer_num_pulses);

  if (success)
  {
    ROS_DEBUG("Successfully loaded parameters:\n%s", to_string().c_str());
  }
  else
  {
    ROS_WARN("One or more parameters could not be loaded properly!\nParameters:\n%s",
             to_string().c_str());
  }
}

auto to_TsScan(const Sensor_Session_t* session_data, const RosParameters& parameters)
    -> toposens_msgs::TsScan
{
  toposens_msgs::TsScan msg;

  // Information is sent in mm from sensor, ROS works in m, though
  constexpr auto scaling_factor = double{1000.0};

  for (auto i = 0; i < session_data->NumberOf3DPoints; ++i)
  {
    toposens_msgs::TsPoint point;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    point.location.x = session_data->Point3D_tp[i].Z_i16 / scaling_factor;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    point.location.y = -session_data->Point3D_tp[i].X_i16 / scaling_factor;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    point.location.z = -session_data->Point3D_tp[i].Y_i16 / scaling_factor;
    // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-constant-array-index)
    point.intensity = session_data->Point3D_tp[i].Intensity_u8;

    msg.points.push_back(point);
  }

  msg.noisy = session_data->NoiseLevel_u16;

  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = parameters.target_frame;

  return msg;
}

auto getStaticTransformMsg(const ros::Time& t, const std::vector<float>& trans,
                           const tf::Quaternion& q, const std::string& from, const std::string& to)
    -> geometry_msgs::TransformStamped
{
  geometry_msgs::TransformStamped msg;

  msg.header.stamp = t;
  msg.header.frame_id = from;
  msg.child_frame_id = to;
  msg.transform.translation.x = trans.at(0);
  msg.transform.translation.y = trans.at(1);
  msg.transform.translation.z = trans.at(2);
  msg.transform.rotation.x = q.getX();
  msg.transform.rotation.y = q.getY();
  msg.transform.rotation.z = q.getZ();
  msg.transform.rotation.w = q.getW();

  return msg;
}

}  // namespace toposens_echo_driver