odom_laser.h 3.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106
  1. #ifndef SLAM_SCAN_MATCH_H_
  2. #define SLAM_SCAN_MATCH_H_
  3. // C++ 标准库头文件
  4. #include <iostream>
  5. #include <memory>
  6. #include <new>
  7. #include <queue>
  8. #include <thread>
  9. #include <mutex>
  10. // C++ 第三方库头文件
  11. #include <glog/logging.h>
  12. // ROS 相关头文件
  13. #include <ros/ros.h>
  14. #include <ros/time.h>
  15. #include <ros/duration.h>
  16. #include <pcl_ros/point_cloud.h>
  17. #include <tf_conversions/tf_eigen.h>
  18. #include <tf/transform_listener.h>
  19. #include <tf/transform_broadcaster.h>
  20. #include <std_msgs/Time.h>
  21. #include <nav_msgs/Odometry.h>
  22. #include <sensor_msgs/PointCloud2.h>
  23. #include <geometry_msgs/TransformStamped.h>
  24. #include <geometry_msgs/PoseWithCovarianceStamped.h>
  25. #include <nodelet/nodelet.h>
  26. #include <pluginlib/class_list_macros.h>
  27. #include <laser_geometry/laser_geometry.h>
  28. // PCL 库头文件
  29. #include <pcl/filters/voxel_grid.h>
  30. #include <pcl/filters/passthrough.h>
  31. #include <pcl/filters/approximate_voxel_grid.h>
  32. // fast_gicp 相关头文件
  33. #include <fast_gicp/gicp/fast_gicp.hpp>
  34. // 项目内部头文件
  35. #include "odom_laser/registrations.hpp"
  36. #include "odom_laser/ScanMatchingStatus.h"
  37. #include "odom_laser/ros_utils.hpp"
  38. #undef new
  39. typedef pcl::PointXYZI PointT;
  40. struct MatchPCL
  41. {
  42. ros::Time time;
  43. pcl::PointCloud<PointT> cloud;
  44. };
  45. namespace laser_odom_ns
  46. {
  47. class Laser_Odom
  48. {
  49. private:
  50. static Laser_Odom *laser_odom;
  51. ros::NodeHandle nh;
  52. ros::Publisher odom_pub; // 里程计发布
  53. ros::Publisher path_pub;
  54. ros::Publisher transformed_cloud_pub;
  55. sensor_msgs::PointCloud2 cloud2; // 点云数据类型转换的中间变量
  56. pcl::PointCloud<PointT>::Ptr cloud; // 点云数据
  57. pcl::PointCloud<PointT>::Ptr trans_cloud; // 转换后的点云数据
  58. pcl::PointCloud<PointT>::ConstPtr keyframe; // 关键帧
  59. pcl::Filter<PointT>::Ptr downsample_filter;
  60. fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
  61. ros::Time prev_time;
  62. ros::Time keyframe_stamp; // 关键帧的时间戳
  63. Eigen::Matrix4f prev_trans; // 地图起点到当前帧的仿射变换
  64. Eigen::Matrix4f keyframe_pose; // 地图起点到当前关键帧的仿射变换
  65. Eigen::Matrix4f odom;
  66. double downsample_resolution;
  67. double keyframe_delta_trans;
  68. double keyframe_delta_angle;
  69. double keyframe_delta_time;
  70. laser_geometry::LaserProjection projector;
  71. std::mutex mutex_;
  72. std::queue<MatchPCL> clouds_;
  73. void procThread();
  74. void point_cloud_callback(const sensor_msgs::LaserScan::ConstPtr &laser_scan_msg);
  75. void publish_odometry(const ros::Time &stamp, const std::string &base_frame_id, const Eigen::Matrix4f &pose);
  76. void publish_keyfrrame(const ros::Time &stamp, const std::string &base_frame_id, const Eigen::Matrix4f &pose); // 发布里程计数据
  77. pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr &cloud);
  78. Eigen::Matrix4f matching(const ros::Time &stamp, const pcl::PointCloud<PointT>::ConstPtr &cloud);
  79. geometry_msgs::TransformStamped matrix2transform(const ros::Time &stamp, const Eigen::Matrix4f &pose, const std::string &frame_id, const std::string &child_frame_id);
  80. public:
  81. Laser_Odom();
  82. ~Laser_Odom();
  83. void init();
  84. };
  85. }; // namespace laser_odom_ns
  86. #endif // SLAM_SCAN_MATCH_H_