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