YOPO

Simulator/src/include/cuda_toolkit/matrix.cuh

@@ -0,0 +1,103 @@
+// This file is part of REMODE - REgularized MOnocular Depth Estimation.
+//
+// Copyright (C) 2014 Matia Pizzoli <matia dot pizzoli at gmail dot com>
+// Robotics and Perception Group, University of Zurich, Switzerland
+// http://rpg.ifi.uzh.ch
+//
+// REMODE is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// REMODE is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <cuda_runtime.h>
+#include <ostream>
+#include <iomanip>
+
+namespace cudaMat {
+template<typename Type, unsigned R, unsigned C>
+struct Matrix
+{
+  __host__ __device__ __forceinline__
+  Type operator()(int row, int col) const
+  {
+    return data[row*C+col];
+  }
+
+  __host__ __device__ __forceinline__
+  Type & operator()(int row, int col)
+  {
+    return data[row*C+col];
+  }
+
+  __host__ __device__ __forceinline__
+  Type operator[](int ind) const
+  {
+    return data[ind];
+  }
+
+  __host__ __device__ __forceinline__
+  Type & operator[](int ind)
+  {
+    return data[ind];
+  }
+
+  __host__
+  friend std::ostream & operator<<(std::ostream &out, const Matrix<Type, R, C> &m)
+  {
+    for(size_t row=0; row<R; ++row)
+    {
+      for(size_t col=0; col<C; ++col)
+      {
+        out << std::setprecision(9) << m(row, col) << " ";
+      }
+      out << std::endl;
+    }
+    return out;
+  }
+
+  Type data[R*C];
+};
+
+template<typename Type, unsigned R, unsigned CR, unsigned C>
+__host__ __device__ __forceinline__
+Matrix<Type, R, C> operator*(const Matrix<Type, R, CR> & lhs,
+                             const Matrix<Type, CR, C> & rhs)
+{
+  Matrix<Type, R, C> result;
+  for(size_t row=0; row<R; ++row)
+  {
+    for(size_t col=0; col<C; ++col)
+    {
+      result(row, col) = 0;
+      for(size_t i=0; i<CR; ++i)
+      {
+        result(row, col) += lhs(row,i) * rhs(i,col);
+      }
+    }
+  }
+  return result;
+}
+
+template<typename Type>
+__host__ __device__ __forceinline__
+Matrix<Type, 2, 2> inv(const Matrix<Type, 2, 2> & in)
+{
+  Matrix<Type, 2, 2> out;
+  float det = in[0]*in[3] - in[1]*in[2];
+  out[0] =  in[3] / det;
+  out[1] = -in[1] / det;
+  out[2] = -in[2] / det;
+  out[3] =  in[0] / det;
+  return out;
+}
+
+}
+

Simulator/src/include/cuda_toolkit/se3.cuh

@@ -0,0 +1,202 @@
+// This file is part of REMODE - REgularized MOnocular Depth Estimation.
+//
+// Copyright (C) 2014 Matia Pizzoli <matia dot pizzoli at gmail dot com>
+// Robotics and Perception Group, University of Zurich, Switzerland
+// http://rpg.ifi.uzh.ch
+//
+// REMODE is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// REMODE is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include "./helper_math.h"
+#include "./matrix.cuh"
+
+namespace cudaMat {
+template<typename Type>
+struct SE3
+{
+  __host__ __device__ __forceinline__
+  SE3()
+  {
+    data(0, 0) = (Type)1;
+    data(0, 1) = 0;
+    data(0, 2) = 0;
+    data(1, 0) = 0;
+    data(1, 1) = (Type)1;
+    data(1, 2) = 0;
+    data(2, 0) = 0;
+    data(2, 1) = 0;
+    data(2, 2) = (Type)1;
+
+    data(0, 3) = 0;
+    data(1, 3) = 0;
+    data(2, 3) = 0;
+  }
+
+  /// Constructor from a normalized quaternion and a translation vector
+  __host__ __device__ __forceinline__
+  SE3(Type qw, Type qx, Type qy, Type qz, Type tx, Type ty, Type tz)
+  {
+    const Type x  = 2*qx;
+    const Type y  = 2*qy;
+    const Type z  = 2*qz;
+    const Type wx = x*qw;
+    const Type wy = y*qw;
+    const Type wz = z*qw;
+    const Type xx = x*qx;
+    const Type xy = y*qx;
+    const Type xz = z*qx;
+    const Type yy = y*qy;
+    const Type yz = z*qy;
+    const Type zz = z*qz;
+
+    data(0, 0) = 1-(yy+zz);
+    data(0, 1) = xy-wz;
+    data(0, 2) = xz+wy;
+    data(1, 0) = xy+wz;
+    data(1, 1) = 1-(xx+zz);
+    data(1, 2) = yz-wx;
+    data(2, 0) = xz-wy;
+    data(2, 1) = yz+wx;
+    data(2, 2) = 1-(xx+yy);
+
+    data(0, 3) = tx;
+    data(1, 3) = ty;
+    data(2, 3) = tz;
+  }
+
+  /// Construct from C arrays
+  /// r is rotation matrix row major
+  /// t is the translation vector (x y z)
+  __host__ __device__ __forceinline__
+  SE3(Type *r, Type *t)
+  {
+    data[0]=r[0]; data[1]=r[1]; data[2] =r[2]; data[3] =t[0];
+    data[4]=r[3]; data[5]=r[4]; data[6] =r[5]; data[7] =t[1];
+    data[8]=r[6]; data[9]=r[7]; data[10]=r[8]; data[11]=t[2];
+  }
+
+  __host__ __device__ __forceinline__
+  SE3<Type> inv() const
+  {
+    SE3<Type> result;
+    result.data[0]  = data[0];
+    result.data[1]  = data[4];
+    result.data[2]  = data[8];
+    result.data[4]  = data[1];
+    result.data[5]  = data[5];
+    result.data[6]  = data[9];
+    result.data[8]  = data[2];
+    result.data[9]  = data[6];
+    result.data[10] = data[10];
+    result.data[3]  = -data[0]*data[3] -data[4]*data[7] -data[8] *data[11];
+    result.data[7]  = -data[1]*data[3] -data[5]*data[7] -data[9] *data[11];
+    result.data[11] = -data[2]*data[3] -data[6]*data[7] -data[10]*data[11];
+    return result;
+  }
+
+  __host__ __device__ __forceinline__
+  Type operator()(int r, int c) const
+  {
+    return data(r, c);
+  }
+
+  __host__ __device__ __forceinline__
+  Type & operator()(int r, int c)
+  {
+    return data(r, c);
+  }
+
+  __host__ __device__ __forceinline__
+  float3 rotate(const float3 &p) const
+  {
+    return make_float3(data(0,0)*p.x + data(0,1)*p.y + data(0,2)*p.z,
+                       data(1,0)*p.x + data(1,1)*p.y + data(1,2)*p.z,
+                       data(2,0)*p.x + data(2,1)*p.y + data(2,2)*p.z);
+  }
+
+  __host__ __device__ __forceinline__
+  float rotate_1_row(const float3 &p) const
+  {
+    return (data(0,0)*p.x + data(0,1)*p.y + data(0,2)*p.z);
+  }
+  __host__ __device__ __forceinline__
+  float rotate_2_row(const float3 &p) const
+  {
+    return (data(1,0)*p.x + data(1,1)*p.y + data(1,2)*p.z);
+  }
+  __host__ __device__ __forceinline__
+  float rotate_3_row(const float3 &p) const
+  {
+    return (data(2,0)*p.x + data(2,1)*p.y + data(2,2)*p.z);
+  }
+
+  __host__ __device__ __forceinline__
+  float3 translate(const float3 &p) const
+  {
+    return make_float3(p.x + data(0,3),
+                       p.y + data(1,3),
+                       p.z + data(2,3));
+  }
+
+  __host__ __device__ __forceinline__
+  float3 getTranslation() const
+  {
+    return make_float3(data(0,3),
+                       data(1,3),
+                       data(2,3));
+  }
+
+  __host__ __device__ __forceinline__
+  float3 getinvTranslation() const
+  {
+    SE3<Type> inv_ = this->inv();
+    return inv_.getTranslation();
+  }
+  __host__
+  friend std::ostream & operator<<(std::ostream &out, const SE3 &m)
+  {
+    out << m.data;
+    return out;
+  }
+
+  Matrix<Type, 3, 4> data;
+};
+
+template<typename Type>
+__host__ __device__ __forceinline__
+SE3<Type> operator*(const SE3<Type> &lhs, const SE3<Type> &rhs)
+{
+  SE3<Type> result;
+  result.data[0]  = lhs.data[0]*rhs.data[0] + lhs.data[1]*rhs.data[4] + lhs.data[2]*rhs.data[8];
+  result.data[1]  = lhs.data[0]*rhs.data[1] + lhs.data[1]*rhs.data[5] + lhs.data[2]*rhs.data[9];
+  result.data[2]  = lhs.data[0]*rhs.data[2] + lhs.data[1]*rhs.data[6] + lhs.data[2]*rhs.data[10];
+  result.data[3]  = lhs.data[3] + lhs.data[0]*rhs.data[3] + lhs.data[1]*rhs.data[7] + lhs.data[2]*rhs.data[11];
+  result.data[4]  = lhs.data[4]*rhs.data[0] + lhs.data[5]*rhs.data[4] + lhs.data[6]*rhs.data[8];
+  result.data[5]  = lhs.data[4]*rhs.data[1] + lhs.data[5]*rhs.data[5] + lhs.data[6]*rhs.data[9];
+  result.data[6]  = lhs.data[4]*rhs.data[2] + lhs.data[5]*rhs.data[6] + lhs.data[6]*rhs.data[10];
+  result.data[7]  = lhs.data[7] + lhs.data[4]*rhs.data[3] + lhs.data[5]*rhs.data[7] + lhs.data[6]*rhs.data[11];
+  result.data[8]  = lhs.data[8]*rhs.data[0] + lhs.data[9]*rhs.data[4] + lhs.data[10]*rhs.data[8];
+  result.data[9]  = lhs.data[8]*rhs.data[1] + lhs.data[9]*rhs.data[5] + lhs.data[10]*rhs.data[9];
+  result.data[10] = lhs.data[8]*rhs.data[2] + lhs.data[9]*rhs.data[6] + lhs.data[10]*rhs.data[10];
+  result.data[11] = lhs.data[11] + lhs.data[8]*rhs.data[3] + lhs.data[9]*rhs.data[7] + lhs.data[10]*rhs.data[11];
+  return result;
+}
+
+__host__ __device__ __forceinline__
+float3 operator*(const SE3<float> &se3, const float3 &p)
+{
+  return se3.translate(se3.rotate(p));
+}
+}
+
+

Simulator/src/include/maps.hpp

@@ -0,0 +1,106 @@
+#ifndef MAPS_HPP
+#define MAPS_HPP
+#include <yaml-cpp/yaml.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/common/transforms.h>
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <algorithm>
+#include <iostream>
+#include <random>
+#include <vector>
+#include <Eigen/Core>
+#include "perlinnoise.hpp"
+
+namespace mocka {
+
+class Maps {
+public:
+  typedef struct BasicInfo {
+    int sizeX;
+    int sizeY;
+    int sizeZ;
+    int seed;
+    double scale;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+  } BasicInfo;
+
+  BasicInfo getInfo() const;
+  void setInfo(const BasicInfo &value);
+  void setParam(const YAML::Node& config);
+  Maps() {}
+  void generate(int type);
+
+private:
+  BasicInfo info;
+  // perlin3D
+  double complexity;
+  double fill;
+  int    fractal;
+  double attenuation;
+  // randomMap
+  double _w_l, _w_h;
+  int    _ObsNum;
+  // maze2D
+  double width;
+  int    addWallX;
+  int    addWallY;
+  // tree
+  std::string tree_file;
+  double tree_dist;
+  // room
+  int room_number;
+  int max_windows;
+  int add_ceiling;
+  double window_size_min, window_size_max;
+
+  std::uniform_real_distribution<double> dis_window_x, dis_window_z, dis_window_size;
+  std::default_random_engine window_eng;
+
+  void perlin3D();
+  void maze2D();
+  void randomMapGenerate();
+  void Maze3DGen();
+  void recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi &maze);
+  void recursizeDivisionMaze(Eigen::MatrixXi &maze);
+  void optimizeMap();
+
+  void forest();
+  void generatePoissonPoints(float map_width, float map_height, float dist, std::vector<Eigen::Vector2f> &positions);
+  void scaleAndTranslateCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float scale_factor, Eigen::Vector2f position, Eigen::Matrix3f &rotation);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr generateGround(const pcl::PointCloud<pcl::PointXYZ>::Ptr &forest_cloud, float grid_size, float hight = 0.0);
+
+  void room();
+  void transformPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud,
+                           const Eigen::Matrix3f &rotation, const Eigen::Vector3f &translation);
+  void generateWallWithWindows(pcl::PointCloud<pcl::PointXYZ>::Ptr wall, float L, float W, float H, int num_windows);
+};
+
+class MazePoint {
+private:
+  pcl::PointXYZ point;
+  double dist1;
+  double dist2;
+  int point1;
+  int point2;
+  bool isdoor;
+
+public:
+  pcl::PointXYZ getPoint();
+  int getPoint1();
+  int getPoint2();
+  double getDist1();
+  double getDist2();
+  void setPoint(pcl::PointXYZ p);
+  void setPoint1(int p);
+  void setPoint2(int p);
+  void setDist1(double set);
+  void setDist2(double set);
+};
+
+} // namespace mocka
+
+#endif // MAPS_HPP

Simulator/src/include/perlinnoise.hpp

@@ -0,0 +1,33 @@
+#ifndef PERLINNOISE_HPP
+#define PERLINNOISE_HPP
+
+#include <vector>
+
+// THIS CLASS IS A TRANSLATION TO C++11 FROM THE REFERENCE
+// JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see
+// http://mrl.nyu.edu/~perlin/noise/)
+// THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN
+
+// I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT
+// PRESENT IN THE ORIGINAL IMPLEMENTATION)
+
+class PerlinNoise
+{
+  // The permutation vector
+  std::vector<int> p;
+
+public:
+  // Initialize with the reference values for the permutation vector
+  PerlinNoise();
+  // Generate a new permutation vector based on the value of seed
+  PerlinNoise(unsigned int seed);
+  // Get a noise value, for 2D images z can have any value
+  double noise(double x, double y, double z);
+
+private:
+  double fade(double t);
+  double lerp(double t, double a, double b);
+  double grad(int hash, double x, double y, double z);
+};
+
+#endif // PERLINNOISE_HPP

Simulator/src/include/sensor_simulator.cuh

@@ -0,0 +1,84 @@
+#ifndef CUDA_UTILS_CUH
+#define CUDA_UTILS_CUH
+
+#include <cuda_runtime.h>
+#include "cuda_toolkit/se3.cuh"
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <pcl/common/common.h> // For pcl::getMinMax3D
+#include <pcl/point_cloud.h>   // For pcl::PointCloud
+#include <pcl/point_types.h>   // For pcl::PointXYZ
+#include <chrono>
+
+namespace raycast
+{
+    struct Vector3f
+    {
+        float x, y, z;
+        __device__ __host__ Vector3f() : x(0.0f), y(0.0f), z(0.0f) {}
+        __device__ __host__ Vector3f(float x_val, float y_val, float z_val)
+            : x(x_val), y(y_val), z(z_val) {}
+    };
+
+    struct Vector3i
+    {
+        int x, y, z;
+        __device__ __host__ Vector3i() : x(0), y(0), z(0) {}
+        __device__ __host__ Vector3i(int x_val, int y_val, int z_val)
+            : x(x_val), y(y_val), z(z_val) {}
+    };
+
+    struct CameraParams
+    {
+        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};
+    };
+
+    struct LidarParams
+    {
+        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};
+    };
+
+    class GridMap
+    {
+        public:
+            GridMap(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float resolution, int occupy_threshold);
+            ~GridMap() {};
+            void freeGridMap() {cudaFree(map_cuda_);}
+            __host__ __device__ Vector3i Pos2Vox(const Vector3f &pos);
+            __host__ __device__ Vector3f Vox2Pos(const Vector3i &vox);
+            __host__ __device__ int Vox2Idx(const Vector3i &vox);
+            __host__ __device__ Vector3i Idx2Vox(int idx);
+            __device__ int symmetricIndex(int index, int length);
+            __device__ int mapQuery(const Vector3f &pos);
+
+            float raycast_step_; // raycast step
+        private:
+            // map param
+            int *map_cuda_;
+            float resolution_;                                           // grid resolution
+            float origin_x_, origin_y_, origin_z_;                       // origin coordinates
+            int grid_size_x_, grid_size_y_, grid_size_z_, grid_size_yz_; // grid sizes
+            int occupy_threshold_;                                        // occupancy threshold
+    };
+
+    __global__ void cameraRaycastKernel(float *depth_values, GridMap grid_map, CameraParams camera_param, cudaMat::SE3<float> T_wc);
+    __global__ void lidarRaycastKernel(Vector3f* point_values, GridMap grid_map, LidarParams lidar_param, cudaMat::SE3<float> T_wc);
+
+    void renderDepthImage(GridMap *grid_map, CameraParams *camera_param, cudaMat::SE3<float>& T_wc, cv::Mat &depth_image);
+    void renderLidarPointcloud(GridMap *grid_map, LidarParams *lidar_param, cudaMat::SE3<float>& T_wc, pcl::PointCloud<pcl::PointXYZ>& lidar_points);
+}
+#endif // CUDA_UTILS_CUH

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