forked from zbw/yiliao2026
66 lines
2.0 KiB
C++
66 lines
2.0 KiB
C++
#ifndef VISUALIZATION_H_
|
|
#define VISUALIZATION_H_
|
|
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <functional>
|
|
#include <map>
|
|
#include <memory>
|
|
#include <Eigen/Core>
|
|
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <nav_msgs/msg/path.hpp>
|
|
#include <geometry_msgs/msg/quaternion.hpp>
|
|
#include <geometry_msgs/msg/pose_stamped.hpp>
|
|
#include <nav_msgs/msg/occupancy_grid.hpp>
|
|
#include <nav_msgs/msg/odometry.hpp>
|
|
#include <sensor_msgs/msg/image.hpp>
|
|
#include <sensor_msgs/msg/point_cloud2.hpp>
|
|
#include <cv_bridge/cv_bridge.h>
|
|
|
|
#include "read_configs.h"
|
|
#include "map_stitcher.h"
|
|
#include "map_builder.h"
|
|
|
|
class Visualizer {
|
|
public:
|
|
enum class TrajectoryType {
|
|
Frame = 0,
|
|
KCC = 1,
|
|
Odom = 2,
|
|
};
|
|
|
|
Visualizer(rclcpp::Node::SharedPtr node, VisualizationConfig& config);
|
|
void AddNewPoseToPath(
|
|
Eigen::Vector3d& pose, double time_double,
|
|
nav_msgs::msg::Path& path, std::string& frame_id);
|
|
void UpdateOdomPose(Eigen::Vector3d& pose, double time_double);
|
|
void UpdateKccPose(Eigen::Vector3d& pose, double time_double);
|
|
void UpdateFramePose(Aligned<std::vector, Eigen::Vector3d>& frame_poses,
|
|
std::vector<double>& timestamps);
|
|
void ConvertMapToOccupancyMsgs(OccupancyData& map,
|
|
nav_msgs::msg::OccupancyGrid& msgs);
|
|
void UpdateMap(MapBuilder& map_builder);
|
|
void PublishImage(cv::Mat& image, double time_double);
|
|
void GetTrajectoryTxt(std::vector<std::vector<std::string>>& lines,
|
|
TrajectoryType trajectory_type);
|
|
|
|
private:
|
|
rclcpp::Node::SharedPtr node_;
|
|
std::string frame_id_;
|
|
|
|
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr kcc_pose_pub_;
|
|
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr frame_pose_pub_;
|
|
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
|
|
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
|
|
|
|
nav_msgs::msg::Path odom_pose_msgs_;
|
|
nav_msgs::msg::Path kcc_pose_msgs_;
|
|
nav_msgs::msg::Path frame_pose_msgs_;
|
|
nav_msgs::msg::OccupancyGrid occupancy_map_msgs_;
|
|
};
|
|
|
|
#endif // VISUALIZATION_H_
|