Program Listing for File LMS1xx_node.hpp

Return to documentation for file (include/LMS1xx/LMS1xx_node.hpp)

#pragma once

#include "LMS1xx/LMS1xx.h"

#include <chrono>
#include <memory>
#include <string>

#include "laser_geometry/laser_geometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/header.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"


namespace LMS1xx_node
{

enum LMS1xxNodeState
{
  CONNECTING = 0,
  TIMEOUT = 1,
  MEASURING = 2,
  ERROR = 3
};

static constexpr auto MAX_CONNECT_ATTEMPTS = 100;

class LMS1xx_node : public rclcpp::Node
{
  public:
  explicit LMS1xx_node();

  private:
  // Node state
  LMS1xxNodeState state_;
  rclcpp::TimerBase::SharedPtr spin_timer_;
  rclcpp::TimerBase::SharedPtr timeout_timer_;
  bool timed_out_;

  // laser data
  LMS1xx laser_;
  scanCfg cfg_;
  scanOutputRange outputRange_;
  scanDataCfg dataCfg_;

  // parameters
  std::string host_;
  std::string frame_id_;
  int port_;
  bool inf_range_;
  int reconnect_timeout_{0};
  sensor_msgs::msg::LaserScan scan_msg_;

  // publishers
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr laserscan_pub_;

  bool connect_lidar();

  void construct_scan();

  bool get_measurements();

  void start_measurements();

  void stop_measurements();

  void publish_scan(scanData& data);

  void publish_cloud();

  void spin_once();
};

}  // namespace LMS1xx_node