ts_echo_driver_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>

#include "toposens_echo_driver/echo_driver.h"
#include "toposens_echo_driver/lib_utils.h"
#include "toposens_echo_driver/ros_utils.h"

auto main(int argc, char** argv) -> int
{
  ros::init(argc, argv, "toposens_echo_driver_node");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");
  toposens_echo_driver::RosParameters params(private_nh);

  if (params.sensor_mode != "single_shot")
  {
    ROS_ERROR("Given sensor mode %s not supported yet, aborting launching of node!",
              params.sensor_mode.c_str());
    return 0;
  }

  ROS_INFO("Launching ECHO ONE Driver node in %s mode!", params.sensor_mode.c_str());

  toposens_echo_driver::EchoOneDriver driver(nh, params);

  ros::Rate rate(params.loop_rate_hz);
  while (ros::ok())
  {
    ros::spinOnce();

    constexpr auto log_interval = double{5};
    toposens_echo_driver::LogKnownSensors(log_interval);
    driver.measure();
    driver.publishStaticTransforms();

    rate.sleep();
  }

  return 0;
}