Improve Nav2 integration: EKF covariance, IMU remap, optional bond

- fillPoseCovariance: use EKF P matrix when available, fall back to
  fitness-based estimate for smoother/standard paths
- Add /imu topic remapping to base launch file
- Add optional Nav2 bond connection (requires nav2_util + bondcpp,
  auto-detected via CMake find_package QUIET)
- Bond starts on activate, stops on deactivate

Nav2 checklist:
  [x] Lifecycle node (configure/activate/deactivate)
  [x] /initialpose subscription
  [x] PoseWithCovarianceStamped with covariance
  [x] map->odom TF (enable_map_odom_tf: true)
  [x] Nav2 launch file with proper remappings
  [x] Optional bond for lifecycle manager health monitoring
  [x] IMU topic remapping

Author: rsasaki0109

CMakeLists.txt

@@ -26,6 +26,8 @@ find_package(pcl_conversions REQUIRED)
 find_package(PCL REQUIRED)
 find_package(ndt_omp_ros2 REQUIRED)
 find_package(small_gicp QUIET CONFIG)
+find_package(nav2_util QUIET)
+find_package(bondcpp QUIET)
 find_package(OpenMP)
 
 if (OPENMP_FOUND)
@@ -57,6 +59,12 @@ if(small_gicp_FOUND)
   target_link_libraries(lidar_localization_component small_gicp::small_gicp)
 endif()
 
+if(nav2_util_FOUND AND bondcpp_FOUND)
+  target_compile_definitions(lidar_localization_component PRIVATE LIDAR_LOCALIZATION_HAVE_NAV2_BOND=1)
+  ament_target_dependencies(lidar_localization_component nav2_util bondcpp)
+  message(STATUS "Nav2 bond support enabled")
+endif()
+
 
 
 include_directories(

include/lidar_localization/lidar_localization_component.hpp

@@ -51,6 +51,10 @@
 #include "lidar_localization/twist_gtsam_smoother.hpp"
 #include "lidar_localization/imu_gtsam_smoother.hpp"
 
+#ifdef LIDAR_LOCALIZATION_HAVE_NAV2_BOND
+#include "bondcpp/bond.hpp"
+#endif
+
 using namespace std::chrono_literals;
 
 class PCLLocalization : public rclcpp_lifecycle::LifecycleNode
@@ -135,6 +139,12 @@ class PCLLocalization : public rclcpp_lifecycle::LifecycleNode
   double last_imu_stamp_{0.0};
   double last_scan_stamp_for_imu_{0.0};
 
+#ifdef LIDAR_LOCALIZATION_HAVE_NAV2_BOND
+  // Nav2 lifecycle manager bond
+  std::unique_ptr<bond::Bond> bond_;
+  bool use_bond_{false};
+#endif
+
   // parameters
   std::string global_frame_id_;
   std::string odom_frame_id_;

launch/lidar_localization.launch.py

@@ -46,14 +46,17 @@ def generate_launch_description():
     twist_topic = launch.substitutions.LaunchConfiguration(
         'twist_topic',
         default='/twist')
+    imu_topic = launch.substitutions.LaunchConfiguration(
+        'imu_topic',
+        default='/imu')
 
     lidar_localization = launch_ros.actions.LifecycleNode(
         name='lidar_localization',
         namespace='',
         package='lidar_localization_ros2',
         executable='lidar_localization_node',
         parameters=[localization_param_dir],
-        remappings=[('/cloud', cloud_topic), ('/twist', twist_topic)],
+        remappings=[('/cloud', cloud_topic), ('/twist', twist_topic), ('/imu', imu_topic)],
         output='screen')
 
     to_inactive = launch.actions.EmitEvent(

src/lidar_localization_component.cpp

@@ -272,6 +272,17 @@ CallbackReturn PCLLocalization::on_activate(const rclcpp_lifecycle::State &)
     map_recieved_ = true;
   }
 
+  // Start Nav2 bond if available
+#ifdef LIDAR_LOCALIZATION_HAVE_NAV2_BOND
+  if (use_bond_) {
+    bond_ = std::make_unique<bond::Bond>("bond", get_name(), shared_from_this());
+    bond_->setHeartbeatPeriod(0.1);
+    bond_->setHeartbeatTimeout(4.0);
+    bond_->start();
+    RCLCPP_INFO(get_logger(), "Nav2 bond started");
+  }
+#endif
+
   RCLCPP_INFO(get_logger(), "Activating end");
   return CallbackReturn::SUCCESS;
 }
@@ -280,6 +291,13 @@ CallbackReturn PCLLocalization::on_deactivate(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(get_logger(), "Deactivating");
 
+#ifdef LIDAR_LOCALIZATION_HAVE_NAV2_BOND
+  if (bond_) {
+    bond_.reset();
+    RCLCPP_INFO(get_logger(), "Nav2 bond stopped");
+  }
+#endif
+
   pose_pub_->on_deactivate();
   path_pub_->on_deactivate();
   status_pub_->on_deactivate();
@@ -1322,25 +1340,36 @@ void PCLLocalization::advancePredictionWithoutMeasurement(double stamp_sec)
 
 void PCLLocalization::fillPoseCovariance(double fitness_score)
 {
-  // Set covariance from fitness score for Nav2 compatibility
-  // Nav2 PoseWithCovarianceStamped.pose.covariance is a 6x6 row-major array [36]
+  // Nav2 PoseWithCovarianceStamped.pose.covariance: 6x6 row-major [36]
   // Indices: [0]=xx, [7]=yy, [14]=zz, [21]=roll, [28]=pitch, [35]=yaw
   auto & cov = corrent_pose_with_cov_stamped_ptr_->pose.covariance;
   std::fill(cov.begin(), cov.end(), 0.0);
 
-  // Base uncertainty from fitness score (higher fitness = more uncertainty)
-  double scale = std::max(1.0, fitness_score / 1.0);
-  double pos_var = 0.01 * scale;   // ~0.1m std at fitness=1.0
-  double z_var = 0.05 * scale;
-  double rp_var = 0.001 * scale;   // ~1.8deg std at fitness=1.0
-  double yaw_var = 0.0005 * scale; // ~1.3deg std at fitness=1.0
-
-  cov[0] = pos_var;   // x
-  cov[7] = pos_var;   // y
-  cov[14] = z_var;    // z
-  cov[21] = rp_var;   // roll
-  cov[28] = rp_var;   // pitch
-  cov[35] = yaw_var;  // yaw
+  // Use EKF covariance matrix if available (most accurate)
+  if (use_twist_ekf_ && twist_ekf_.isInitialized()) {
+    const auto & P = twist_ekf_.covariance();
+    // EKF state: [px,py,pz,vx,vy,vz,yaw,gyro_bias,speed_bias]
+    cov[0] = P(0, 0);    // xx
+    cov[1] = P(0, 1);    // xy
+    cov[6] = P(1, 0);    // yx
+    cov[7] = P(1, 1);    // yy
+    cov[14] = P(2, 2);   // zz
+    cov[35] = P(6, 6);   // yaw-yaw
+    // roll/pitch not estimated by EKF — use fitness-based estimate
+    double scale = std::max(1.0, fitness_score);
+    cov[21] = 0.001 * scale;
+    cov[28] = 0.001 * scale;
+    return;
+  }
+
+  // Fitness-based covariance for smoother / standard paths
+  double scale = std::max(1.0, fitness_score);
+  cov[0] = 0.01 * scale;    // x
+  cov[7] = 0.01 * scale;    // y
+  cov[14] = 0.05 * scale;   // z
+  cov[21] = 0.001 * scale;  // roll
+  cov[28] = 0.001 * scale;  // pitch
+  cov[35] = 0.0005 * scale; // yaw
 }
 
 void PCLLocalization::publishAlignmentStatus(