Develop

devel/core_algorithm/fusion/CMakeLists.txt

@@ -52,8 +52,9 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
-install(
-  DIRECTORY launch
+install(DIRECTORY 
+  launch
+  param
   DESTINATION share/${PROJECT_NAME}
 )
 

devel/core_algorithm/fusion/include/fusion/ekf.hpp

@@ -17,7 +17,7 @@ class EKF{
 
     void Xprediction(Eigen::Vector3d, Eigen::Matrix3d);
     void Vprediction(Eigen::Vector3d, Eigen::Matrix3d, double);
-    void correction(Eigen::Vector3d, Eigen::Matrix3d);
+    void correction(Eigen::Vector3d, Eigen::Matrix3d, Eigen::Matrix3d);
 
     Eigen::Vector3d getPose();
     Eigen::Matrix3d getCov();

devel/core_algorithm/fusion/include/fusion/fusion_node.hpp

@@ -52,14 +52,21 @@ class FusionNode : public rclcpp::Node{
     Eigen::Matrix3d local_cov;
     Eigen::Matrix3d cbcam_cov;
     Eigen::Matrix3d obcam_cov;
-    Eigen::Matrix3d init_rot;
+    Eigen::Matrix3d global_h;
+    Eigen::Matrix3d cbcam_h;
+    Eigen::Matrix3d obcam_h;
 
     bool if_align;
+    bool spin_control;
 
     int pose_modes[4];
 
+    double spin_threshold;
+
     double max_dt_align;
     double min_dt_align;
+
+    double lidar_yaw;
 
     double prev_pred_time;
 

devel/core_algorithm/fusion/launch/fusion.launch.xml

@@ -5,7 +5,7 @@
     <arg name="robot_frame_id" default="base_footprint"/>
 
     <node pkg="fusion" exec="fusion_node" name="fusion_node" output="screen">
-        <param from="/home/user/localization-ws/src/core_algorithm/fusion/param/fusion.yaml"/>
+        <param from="$(find-pkg-share fusion)/param/fusion.yaml"/>
         <param name="global_frame_id" value="$(var global_frame_id)"/>
         <param name="robot_parent_frame_id" value="$(var robot_parent_frame_id)"/>
         <param name="robot_frame_id" value="$(var robot_frame_id)"/>