git update

src/ground_slam/include/visualization.h

@@ -0,0 +1,65 @@
+#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_