123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106 |
- #ifndef SLAM_SCAN_MATCH_H_
- #define SLAM_SCAN_MATCH_H_
- // C++ 标准库头文件
- #include <iostream>
- #include <memory>
- #include <new>
- #include <queue>
- #include <thread>
- #include <mutex>
- // C++ 第三方库头文件
- #include <glog/logging.h>
- // ROS 相关头文件
- #include <ros/ros.h>
- #include <ros/time.h>
- #include <ros/duration.h>
- #include <pcl_ros/point_cloud.h>
- #include <tf_conversions/tf_eigen.h>
- #include <tf/transform_listener.h>
- #include <tf/transform_broadcaster.h>
- #include <std_msgs/Time.h>
- #include <nav_msgs/Odometry.h>
- #include <sensor_msgs/PointCloud2.h>
- #include <geometry_msgs/TransformStamped.h>
- #include <geometry_msgs/PoseWithCovarianceStamped.h>
- #include <nodelet/nodelet.h>
- #include <pluginlib/class_list_macros.h>
- #include <laser_geometry/laser_geometry.h>
- // PCL 库头文件
- #include <pcl/filters/voxel_grid.h>
- #include <pcl/filters/passthrough.h>
- #include <pcl/filters/approximate_voxel_grid.h>
- // fast_gicp 相关头文件
- #include <fast_gicp/gicp/fast_gicp.hpp>
- // 项目内部头文件
- #include "odom_laser/registrations.hpp"
- #include "odom_laser/ScanMatchingStatus.h"
- #include "odom_laser/ros_utils.hpp"
- #undef new
- typedef pcl::PointXYZI PointT;
- struct MatchPCL
- {
- ros::Time time;
- pcl::PointCloud<PointT> cloud;
- };
- namespace laser_odom_ns
- {
- class Laser_Odom
- {
- private:
- static Laser_Odom *laser_odom;
- ros::NodeHandle nh;
- ros::Publisher odom_pub; // 里程计发布
- ros::Publisher path_pub;
- ros::Publisher transformed_cloud_pub;
- sensor_msgs::PointCloud2 cloud2; // 点云数据类型转换的中间变量
- pcl::PointCloud<PointT>::Ptr cloud; // 点云数据
- pcl::PointCloud<PointT>::Ptr trans_cloud; // 转换后的点云数据
- pcl::PointCloud<PointT>::ConstPtr keyframe; // 关键帧
- pcl::Filter<PointT>::Ptr downsample_filter;
- fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
- ros::Time prev_time;
- ros::Time keyframe_stamp; // 关键帧的时间戳
- Eigen::Matrix4f prev_trans; // 地图起点到当前帧的仿射变换
- Eigen::Matrix4f keyframe_pose; // 地图起点到当前关键帧的仿射变换
- Eigen::Matrix4f odom;
- double downsample_resolution;
- double keyframe_delta_trans;
- double keyframe_delta_angle;
- double keyframe_delta_time;
- laser_geometry::LaserProjection projector;
- std::mutex mutex_;
- std::queue<MatchPCL> clouds_;
- void procThread();
- void point_cloud_callback(const sensor_msgs::LaserScan::ConstPtr &laser_scan_msg);
- void publish_odometry(const ros::Time &stamp, const std::string &base_frame_id, const Eigen::Matrix4f &pose);
- void publish_keyfrrame(const ros::Time &stamp, const std::string &base_frame_id, const Eigen::Matrix4f &pose); // 发布里程计数据
- pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr &cloud);
- Eigen::Matrix4f matching(const ros::Time &stamp, const pcl::PointCloud<PointT>::ConstPtr &cloud);
- geometry_msgs::TransformStamped matrix2transform(const ros::Time &stamp, const Eigen::Matrix4f &pose, const std::string &frame_id, const std::string &child_frame_id);
- public:
- Laser_Odom();
- ~Laser_Odom();
- void init();
- };
- }; // namespace laser_odom_ns
- #endif // SLAM_SCAN_MATCH_H_
|