feat: 初始化 Go2 导航代码与配置

Livox_automatic_calibration-master/include/point_cloud_localization/PointCloudLocalization.h

@@ -0,0 +1,76 @@
+#ifndef POINT_CLOUD_LOCALIZATION_H
+#define POINT_CLOUD_LOCALIZATION_H
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+//#include <ros/ros.h>
+#include <geometry_utils/Transform3.h>
+
+//#include <pcl_ros/point_cloud.h>
+//#include <tf2_ros/transform_broadcaster.h>
+#include"common.h"
+
+class PointCloudLocalization {
+ public:
+	 typedef pcl::PointCloud<pcl::POINT_TYPE> PointCloud;
+
+  PointCloudLocalization();
+  ~PointCloudLocalization();
+
+  bool Initialize();
+
+  bool TransformPointsToFixedFrame(const PointCloud& points,
+                                   PointCloud* points_transformed) const;
+
+  bool TransformPointsToSensorFrame(const PointCloud& points,
+                                    PointCloud* points_transformed) const;
+
+  bool MotionUpdate(const geometry_utils::Transform3& incremental_odom);
+
+  bool MeasurementUpdate(const PointCloud::Ptr& query,
+                         const PointCloud::Ptr& reference,
+                         PointCloud* aligned_query);
+
+  const geometry_utils::Transform3& GetIncrementalEstimate() const;
+  const geometry_utils::Transform3& GetIntegratedEstimate() const;
+
+  void SetIntegratedEstimate(
+      const geometry_utils::Transform3& integrated_estimate);
+
+ private:
+  // Node initialization.
+  bool LoadParameters();
+
+  
+  // The node's name.
+  std::string name_;
+
+  // Pose estimate.
+  geometry_utils::Transform3 incremental_estimate_;
+  geometry_utils::Transform3 integrated_estimate_;
+
+  
+
+  // Coordinate frames.
+  std::string fixed_frame_id_;
+  std::string base_frame_id_;
+
+  // Parameters for filtering and ICP.
+  struct Parameters {
+    // Stop ICP if the transformation from the last iteration was this small.
+    double tf_epsilon;
+
+    // During ICP, two points won't be considered a correspondence if they are
+    // at least this far from one another.
+    double corr_dist;
+
+    // Iterate ICP this many times.
+    unsigned int iterations;
+  } params_;
+
+
+  bool transform_thresholding_;
+  double max_translation_;
+  double max_rotation_;
+};
+
+#endif