47 lines
1.2 KiB
C++
47 lines
1.2 KiB
C++
#pragma once
|
|
|
|
#include "Eigen/Geometry"
|
|
#include "geometry_msgs/msg/point_stamped.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "sensor_msgs/msg/laser_scan.hpp"
|
|
#include "tf2_ros/buffer.hpp"
|
|
#include "tf2_ros/transform_listener.hpp"
|
|
#include "toid_msgs/msg/rival.hpp"
|
|
#include "visualization_msgs/msg/marker.hpp"
|
|
#include "vector"
|
|
|
|
namespace toid
|
|
{
|
|
|
|
class ToidRivalDetect : public rclcpp::Node
|
|
{
|
|
using LaserScan = sensor_msgs::msg::LaserScan;
|
|
using Rival = toid_msgs::msg::Rival;
|
|
using PointStamped = geometry_msgs::msg::PointStamped;
|
|
using Marker = visualization_msgs::msg::Marker;
|
|
|
|
public:
|
|
ToidRivalDetect();
|
|
private:
|
|
|
|
void process_scan(LaserScan::ConstSharedPtr msg);
|
|
|
|
rclcpp::Subscription<LaserScan>::SharedPtr scan_sub_;
|
|
rclcpp::Publisher<Rival>::SharedPtr rival_pub_;
|
|
rclcpp::Publisher<Marker>::SharedPtr marker_pub_;
|
|
|
|
std::shared_ptr<tf2_ros::Buffer> tf_buf_;
|
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
|
|
|
std::string global_frame_ = "map";
|
|
double map_width_ = 3.0;
|
|
double map_height_ = 2.0;
|
|
bool visualize_ = false;
|
|
bool closest_ = false;
|
|
|
|
rclcpp::Logger logger_ = get_logger();
|
|
rclcpp::Clock::SharedPtr clock_;
|
|
|
|
};
|
|
|
|
} // namespace toid
|