145 lines
5.4 KiB
C++
145 lines
5.4 KiB
C++
#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
|