YOPO

Simulator/src/include/sensor_simulator.h

@@ -0,0 +1,144 @@
+#ifndef SENSOR_SIMULATOR_H
+#define SENSOR_SIMULATOR_H
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/common.h>
+#include <pcl/common/eigen.h>
+#include <pcl/octree/octree_search.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <opencv2/opencv.hpp>
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Image.h>
+#include <pcl_ros/point_cloud.h>
+#include <cv_bridge/cv_bridge.h>
+#include <iostream>
+#include <vector>
+#include <chrono>
+#include <omp.h>
+#include <yaml-cpp/yaml.h>
+
+class SensorSimulator {
+public:
+    SensorSimulator(ros::NodeHandle &nh) : nh_(nh) {
+        YAML::Node config = YAML::LoadFile(CONFIG_FILE_PATH);
+        // 读取camera参数
+        fx = config["camera"]["fx"].as<float>();
+        fy = config["camera"]["fy"].as<float>();
+        cx = config["camera"]["cx"].as<float>();
+        cy = config["camera"]["cy"].as<float>();
+        image_width = config["camera"]["image_width"].as<int>();
+        image_height = config["camera"]["image_height"].as<int>();
+        max_depth_dist = config["camera"]["max_depth_dist"].as<float>();
+        normalize_depth = config["camera"]["normalize_depth"].as<bool>();
+
+        // 读取lidar参数
+        vertical_lines = config["lidar"]["vertical_lines"].as<int>();
+        vertical_angle_start = config["lidar"]["vertical_angle_start"].as<float>();
+        vertical_angle_end = config["lidar"]["vertical_angle_end"].as<float>();
+        horizontal_num = config["lidar"]["horizontal_num"].as<int>();
+        horizontal_resolution = config["lidar"]["horizontal_resolution"].as<float>();
+        max_lidar_dist = config["lidar"]["max_lidar_dist"].as<float>();
+
+        render_lidar = config["render_lidar"].as<bool>();
+        render_depth = config["render_depth"].as<bool>();
+        float depth_fps = config["depth_fps"].as<float>();
+        float lidar_fps = config["lidar_fps"].as<float>();
+
+        std::string ply_file = config["ply_file"].as<std::string>();
+        std::string odom_topic = config["odom_topic"].as<std::string>();
+        std::string depth_topic = config["depth_topic"].as<std::string>();
+        std::string lidar_topic = config["lidar_topic"].as<std::string>();
+
+        float resolution = config["resolution"].as<float>();
+        int expand_y_times = config["expand_y_times"].as<int>();
+        int expand_x_times = config["expand_x_times"].as<int>();
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr orig_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+        printf("1.Reading Point Cloud... \n");
+
+        // if (pcl::io::loadPCDFile("/home/lu/用完删除/test/map.pcd", *cloud) == -1) {
+        //     PCL_ERROR("Couldn't read PLY file \n");
+        //     return;
+        // }
+
+        if (pcl::io::loadPLYFile(ply_file, *orig_cloud) == -1) {
+            PCL_ERROR("Couldn't read PLY file \n");
+            return;
+        }
+        
+        printf("2.Processing... \n");
+
+        cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
+        *cloud = *orig_cloud;
+        for (int i = 0; i < expand_x_times; ++i) {
+            expand_cloud(cloud, 0);
+        }
+        for (int i = 0; i < expand_y_times; ++i) {
+            expand_cloud(cloud, 1);
+        }
+
+        octree = pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr(
+            new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(resolution));
+        octree->setInputCloud(cloud);
+        octree->addPointsFromInputCloud();
+
+        image_pub_ = nh_.advertise<sensor_msgs::Image>(depth_topic, 1);
+        point_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(lidar_topic, 1);
+        odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorSimulator::odomCallback, this);
+        timer_depth_ = nh_.createTimer(ros::Duration(1 / depth_fps), &SensorSimulator::timerDepthCallback, this);
+        timer_lidar_ = nh_.createTimer(ros::Duration(1 / lidar_fps), &SensorSimulator::timerLidarCallback, this);
+        printf("3.Simulation Ready! \n");
+        ros::spin();
+    }
+
+    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
+
+    cv::Mat renderDepthImage();
+
+    pcl::PointCloud<pcl::PointXYZ> renderLidarPointcloud();
+
+    void timerDepthCallback(const ros::TimerEvent &);
+
+    void timerLidarCallback(const ros::TimerEvent &);
+
+    void expand_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr expanded_cloud, int direction = 0);
+
+private:
+    bool render_depth{false};
+    bool render_lidar{false};
+    bool odom_init{false};
+    Eigen::Quaternionf quat;
+    Eigen::Vector3f pos;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr octree;
+
+    // camera param
+    float fx = 80.0f; // focal length x
+    float fy = 80.0f; // focal length y
+    float cx = 80.0f; // principal point x (image center)
+    float cy = 45.0f; // principal point y (image center)
+    int image_width = 160;
+    int image_height = 90;
+    float max_depth_dist{20};
+    bool normalize_depth{false};
+
+    // lidar param
+    int vertical_lines = 16;            // 纵向16线
+    float vertical_angle_start = -15.0; // 起始垂直角度
+    float vertical_angle_end = 15.0;    // 结束垂直角度
+    int horizontal_num = 360;           // 水平360点
+    float horizontal_resolution = 1.0;  // 水平分辨率为1度
+    float max_lidar_dist{50};
+
+
+    ros::NodeHandle nh_;
+    ros::Publisher image_pub_, point_cloud_pub_;
+    ros::Subscriber odom_sub_;
+    ros::Timer timer_depth_, timer_lidar_;
+};
+
+
+#endif //SENSOR_SIMULATOR_H