Initial import of FaRui Campus ADS v3.2

master/src/Localization/lio_twist_fusion/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.14)
+project(lio_twist_fusion)
+
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/lio_twist_converter.cpp
+  src/twist_fusion_selector.cpp
+)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+ament_target_dependencies(${PROJECT_NAME}
+  geometry_msgs
+  nav_msgs
+  rclcpp
+  rclcpp_components
+  tf2
+  tf2_geometry_msgs
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "lio_twist_converter::LioTwistConverter"
+  EXECUTABLE lio_twist_converter_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "twist_fusion_selector::TwistFusionSelector"
+  EXECUTABLE twist_fusion_selector_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+install(
+  DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

master/src/Localization/lio_twist_fusion/config/lio_twist_converter.param.yaml

@@ -0,0 +1,13 @@
+/**:
+  ros__parameters:
+    output_frame_id: "base_link"
+    min_dt_sec: 0.02
+    max_dt_sec: 0.5
+    max_abs_linear_x_mps: 30.0
+    max_abs_angular_z_rps: 3.0
+    linear_x_scale: 1.0
+    angular_z_scale: 1.0
+    covariance:
+      linear_x: 0.25
+      angular_z: 0.01
+      unused_axis: 100000.0

master/src/Localization/lio_twist_fusion/config/twist_fusion_selector.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    output_frame_id: "base_link"
+    selection_mode: "auto"  # auto, wheel, lio, blend
+    wheel_timeout_sec: 0.3
+    lio_timeout_sec: 0.3
+    turn_on_angular_velocity_rps: 0.25
+    turn_off_angular_velocity_rps: 0.15
+    max_abs_linear_x_mps: 30.0
+    max_abs_angular_z_rps: 3.0
+    max_lio_wheel_linear_x_diff_mps: 5.0
+    max_lio_wheel_angular_z_diff_rps: 1.0
+    require_lio_wheel_consistency: false
+    disagreement_covariance_scale: 4.0

master/src/Localization/lio_twist_fusion/launch/lio_twist_converter.launch.xml

@@ -0,0 +1,11 @@
+<launch>
+  <arg name="input_odometry_topic" default="/Odometry"/>
+  <arg name="output_twist_with_covariance_topic" default="/localization/fast_lio/twist_with_covariance"/>
+  <arg name="param_file" default="$(find-pkg-share lio_twist_fusion)/config/lio_twist_converter.param.yaml"/>
+
+  <node pkg="lio_twist_fusion" exec="lio_twist_converter_node" output="both">
+    <param from="$(var param_file)"/>
+    <remap from="input/odometry" to="$(var input_odometry_topic)"/>
+    <remap from="output/twist_with_covariance" to="$(var output_twist_with_covariance_topic)"/>
+  </node>
+</launch>

master/src/Localization/lio_twist_fusion/launch/lio_twist_fusion.launch.xml

@@ -0,0 +1,21 @@
+<launch>
+  <arg name="input_odometry_topic" default="/Odometry"/>
+  <arg name="input_wheel_twist_with_covariance_topic" default="/localization/twist_estimator/twist_with_covariance"/>
+  <arg name="lio_twist_with_covariance_topic" default="/localization/fast_lio/twist_with_covariance"/>
+  <arg name="output_twist_with_covariance_topic" default="/localization/twist_estimator/fused_twist_with_covariance"/>
+  <arg name="lio_twist_converter_param_file" default="$(find-pkg-share lio_twist_fusion)/config/lio_twist_converter.param.yaml"/>
+  <arg name="twist_fusion_selector_param_file" default="$(find-pkg-share lio_twist_fusion)/config/twist_fusion_selector.param.yaml"/>
+
+  <node pkg="lio_twist_fusion" exec="lio_twist_converter_node" output="both">
+    <param from="$(var lio_twist_converter_param_file)"/>
+    <remap from="input/odometry" to="$(var input_odometry_topic)"/>
+    <remap from="output/twist_with_covariance" to="$(var lio_twist_with_covariance_topic)"/>
+  </node>
+
+  <node pkg="lio_twist_fusion" exec="twist_fusion_selector_node" output="both">
+    <param from="$(var twist_fusion_selector_param_file)"/>
+    <remap from="input/wheel_twist_with_covariance" to="$(var input_wheel_twist_with_covariance_topic)"/>
+    <remap from="input/lio_twist_with_covariance" to="$(var lio_twist_with_covariance_topic)"/>
+    <remap from="output/twist_with_covariance" to="$(var output_twist_with_covariance_topic)"/>
+  </node>
+</launch>

master/src/Localization/lio_twist_fusion/launch/twist_fusion_selector.launch.xml

@@ -0,0 +1,13 @@
+<launch>
+  <arg name="input_wheel_twist_with_covariance_topic" default="/localization/twist_estimator/twist_with_covariance"/>
+  <arg name="input_lio_twist_with_covariance_topic" default="/localization/fast_lio/twist_with_covariance"/>
+  <arg name="output_twist_with_covariance_topic" default="/localization/twist_estimator/fused_twist_with_covariance"/>
+  <arg name="param_file" default="$(find-pkg-share lio_twist_fusion)/config/twist_fusion_selector.param.yaml"/>
+
+  <node pkg="lio_twist_fusion" exec="twist_fusion_selector_node" output="both">
+    <param from="$(var param_file)"/>
+    <remap from="input/wheel_twist_with_covariance" to="$(var input_wheel_twist_with_covariance_topic)"/>
+    <remap from="input/lio_twist_with_covariance" to="$(var input_lio_twist_with_covariance_topic)"/>
+    <remap from="output/twist_with_covariance" to="$(var output_twist_with_covariance_topic)"/>
+  </node>
+</launch>

master/src/Localization/lio_twist_fusion/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lio_twist_fusion</name>
+  <version>0.1.0</version>
+  <description>Convert and select LiDAR-inertial and wheel/gyro twist measurements for EKF input.</description>
+  <maintainer email="autoware@example.com">Autoware</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/Localization/lio_twist_fusion/src/lio_twist_converter.cpp

@@ -0,0 +1,162 @@
+// Copyright 2026 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <array>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <string>
+#include <functional>
+
+namespace lio_twist_converter
+{
+namespace
+{
+constexpr std::size_t covariance_index(const std::size_t row, const std::size_t col)
+{
+  return row * 6 + col;
+}
+
+bool is_finite(const double value)
+{
+  return std::isfinite(value);
+}
+}  // namespace
+
+class LioTwistConverter : public rclcpp::Node
+{
+public:
+  explicit LioTwistConverter(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+  : Node("lio_twist_converter", options)
+  {
+    output_frame_id_ = declare_parameter<std::string>("output_frame_id", "base_link");
+    min_dt_sec_ = declare_parameter<double>("min_dt_sec", 0.02);
+    max_dt_sec_ = declare_parameter<double>("max_dt_sec", 0.5);
+    max_abs_linear_x_mps_ = declare_parameter<double>("max_abs_linear_x_mps", 30.0);
+    max_abs_angular_z_rps_ = declare_parameter<double>("max_abs_angular_z_rps", 3.0);
+    linear_x_scale_ = declare_parameter<double>("linear_x_scale", 1.0);
+    angular_z_scale_ = declare_parameter<double>("angular_z_scale", 1.0);
+    linear_x_covariance_ = declare_parameter<double>("covariance.linear_x", 0.25);
+    angular_z_covariance_ = declare_parameter<double>("covariance.angular_z", 0.01);
+    unused_axis_covariance_ = declare_parameter<double>("covariance.unused_axis", 100000.0);
+
+    odometry_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+      "input/odometry", rclcpp::SensorDataQoS(),
+      std::bind(&LioTwistConverter::on_odometry, this, std::placeholders::_1));
+    twist_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
+      "output/twist_with_covariance", 10);
+  }
+
+private:
+  void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
+  {
+    if (!previous_odom_) {
+      previous_odom_ = msg;
+      return;
+    }
+
+    const double dt =
+      (rclcpp::Time(msg->header.stamp) - rclcpp::Time(previous_odom_->header.stamp)).seconds();
+    if (dt < min_dt_sec_ || dt > max_dt_sec_) {
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 1000, "Reject LIO odometry increment because dt is %.3f sec",
+        dt);
+      previous_odom_ = msg;
+      return;
+    }
+
+    tf2::Transform previous_pose;
+    tf2::Transform current_pose;
+    tf2::fromMsg(previous_odom_->pose.pose, previous_pose);
+    tf2::fromMsg(msg->pose.pose, current_pose);
+
+    const tf2::Transform relative_pose = previous_pose.inverse() * current_pose;
+    const tf2::Vector3 relative_translation = relative_pose.getOrigin();
+
+    double roll = 0.0;
+    double pitch = 0.0;
+    double yaw = 0.0;
+    tf2::Matrix3x3(relative_pose.getRotation()).getRPY(roll, pitch, yaw);
+
+    geometry_msgs::msg::TwistWithCovarianceStamped twist_msg;
+    twist_msg.header.stamp = msg->header.stamp;
+    twist_msg.header.frame_id = output_frame_id_;
+    twist_msg.twist.twist.linear.x = linear_x_scale_ * relative_translation.x() / dt;
+    twist_msg.twist.twist.linear.y = relative_translation.y() / dt;
+    twist_msg.twist.twist.linear.z = relative_translation.z() / dt;
+    twist_msg.twist.twist.angular.x = roll / dt;
+    twist_msg.twist.twist.angular.y = pitch / dt;
+    twist_msg.twist.twist.angular.z = angular_z_scale_ * yaw / dt;
+    fill_covariance(twist_msg.twist.covariance);
+
+    if (!is_valid_twist(twist_msg)) {
+      RCLCPP_WARN_THROTTLE(
+        get_logger(), *get_clock(), 1000,
+        "Reject LIO twist. vx=%.3f m/s, wz=%.3f rad/s",
+        twist_msg.twist.twist.linear.x, twist_msg.twist.twist.angular.z);
+      previous_odom_ = msg;
+      return;
+    }
+
+    twist_pub_->publish(twist_msg);
+    previous_odom_ = msg;
+  }
+
+  void fill_covariance(std::array<double, 36> & covariance) const
+  {
+    covariance.fill(0.0);
+    covariance[covariance_index(0, 0)] = linear_x_covariance_;
+    covariance[covariance_index(1, 1)] = unused_axis_covariance_;
+    covariance[covariance_index(2, 2)] = unused_axis_covariance_;
+    covariance[covariance_index(3, 3)] = unused_axis_covariance_;
+    covariance[covariance_index(4, 4)] = unused_axis_covariance_;
+    covariance[covariance_index(5, 5)] = angular_z_covariance_;
+  }
+
+  bool is_valid_twist(const geometry_msgs::msg::TwistWithCovarianceStamped & msg) const
+  {
+    const auto & twist = msg.twist.twist;
+    return is_finite(twist.linear.x) && is_finite(twist.angular.z) &&
+           std::abs(twist.linear.x) <= max_abs_linear_x_mps_ &&
+           std::abs(twist.angular.z) <= max_abs_angular_z_rps_;
+  }
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_pub_;
+  nav_msgs::msg::Odometry::ConstSharedPtr previous_odom_;
+
+  std::string output_frame_id_;
+  double min_dt_sec_{0.02};
+  double max_dt_sec_{0.5};
+  double max_abs_linear_x_mps_{30.0};
+  double max_abs_angular_z_rps_{3.0};
+  double linear_x_scale_{1.0};
+  double angular_z_scale_{1.0};
+  double linear_x_covariance_{0.25};
+  double angular_z_covariance_{0.01};
+  double unused_axis_covariance_{100000.0};
+};
+
+}  // namespace lio_twist_converter
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(lio_twist_converter::LioTwistConverter)

master/src/Localization/lio_twist_fusion/src/twist_fusion_selector.cpp

@@ -0,0 +1,296 @@
+// Copyright 2026 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <string>
+
+namespace twist_fusion_selector
+{
+namespace
+{
+using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
+
+constexpr std::size_t covariance_index(const std::size_t row, const std::size_t col)
+{
+  return row * 6 + col;
+}
+
+double square(const double value)
+{
+  return value * value;
+}
+}  // namespace
+
+class TwistFusionSelector : public rclcpp::Node
+{
+public:
+  explicit TwistFusionSelector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+  : Node("twist_fusion_selector", options)
+  {
+    output_frame_id_ = declare_parameter<std::string>("output_frame_id", "base_link");
+    selection_mode_ = declare_parameter<std::string>("selection_mode", "auto");
+    wheel_timeout_sec_ = declare_parameter<double>("wheel_timeout_sec", 0.3);
+    lio_timeout_sec_ = declare_parameter<double>("lio_timeout_sec", 0.3);
+    turn_on_angular_velocity_rps_ =
+      declare_parameter<double>("turn_on_angular_velocity_rps", 0.25);
+    turn_off_angular_velocity_rps_ =
+      declare_parameter<double>("turn_off_angular_velocity_rps", 0.15);
+    max_abs_linear_x_mps_ = declare_parameter<double>("max_abs_linear_x_mps", 30.0);
+    max_abs_angular_z_rps_ = declare_parameter<double>("max_abs_angular_z_rps", 3.0);
+    max_lio_wheel_linear_x_diff_mps_ =
+      declare_parameter<double>("max_lio_wheel_linear_x_diff_mps", 5.0);
+    max_lio_wheel_angular_z_diff_rps_ =
+      declare_parameter<double>("max_lio_wheel_angular_z_diff_rps", 1.0);
+    require_lio_wheel_consistency_ =
+      declare_parameter<bool>("require_lio_wheel_consistency", false);
+    disagreement_covariance_scale_ =
+      declare_parameter<double>("disagreement_covariance_scale", 4.0);
+
+    wheel_twist_sub_ = create_subscription<TwistWithCovarianceStamped>(
+      "input/wheel_twist_with_covariance", 10,
+      std::bind(&TwistFusionSelector::on_wheel_twist, this, std::placeholders::_1));
+    lio_twist_sub_ = create_subscription<TwistWithCovarianceStamped>(
+      "input/lio_twist_with_covariance", 10,
+      std::bind(&TwistFusionSelector::on_lio_twist, this, std::placeholders::_1));
+    twist_pub_ =
+      create_publisher<TwistWithCovarianceStamped>("output/twist_with_covariance", 10);
+  }
+
+private:
+  void on_wheel_twist(const TwistWithCovarianceStamped::ConstSharedPtr msg)
+  {
+    latest_wheel_twist_ = msg;
+
+    if (selection_mode_ == "wheel" || !should_use_lio()) {
+      publish_wheel_or_lio_fallback();
+    }
+  }
+
+  void on_lio_twist(const TwistWithCovarianceStamped::ConstSharedPtr msg)
+  {
+    latest_lio_twist_ = msg;
+
+    if (selection_mode_ == "lio" || selection_mode_ == "blend" || should_use_lio()) {
+      publish_selected();
+    }
+  }
+
+  void publish_selected()
+  {
+    if (selection_mode_ == "wheel") {
+      publish_wheel_or_lio_fallback();
+      return;
+    }
+
+    if (selection_mode_ == "lio") {
+      publish_lio_or_wheel_fallback();
+      return;
+    }
+
+    if (selection_mode_ == "blend") {
+      if (is_fresh(latest_lio_twist_, lio_timeout_sec_) &&
+          is_fresh(latest_wheel_twist_, wheel_timeout_sec_)) {
+        publish(blend(*latest_wheel_twist_, *latest_lio_twist_));
+      } else {
+        publish_lio_or_wheel_fallback();
+      }
+      return;
+    }
+
+    if (should_use_lio()) {
+      TwistWithCovarianceStamped selected = *latest_lio_twist_;
+      if (is_lio_wheel_disagreeing()) {
+        inflate_observed_covariance(selected, disagreement_covariance_scale_);
+      }
+      publish(selected);
+      return;
+    }
+
+    publish_wheel_or_lio_fallback();
+  }
+
+  void publish_wheel_or_lio_fallback()
+  {
+    if (is_fresh(latest_wheel_twist_, wheel_timeout_sec_)) {
+      publish(*latest_wheel_twist_);
+    } else if (is_fresh(latest_lio_twist_, lio_timeout_sec_)) {
+      publish(*latest_lio_twist_);
+    }
+  }
+
+  void publish_lio_or_wheel_fallback()
+  {
+    if (is_fresh(latest_lio_twist_, lio_timeout_sec_)) {
+      publish(*latest_lio_twist_);
+    } else if (is_fresh(latest_wheel_twist_, wheel_timeout_sec_)) {
+      publish(*latest_wheel_twist_);
+    }
+  }
+
+  void publish(TwistWithCovarianceStamped msg)
+  {
+    msg.header.frame_id = output_frame_id_;
+    twist_pub_->publish(msg);
+  }
+
+  bool should_use_lio()
+  {
+    if (!is_fresh(latest_lio_twist_, lio_timeout_sec_) || !is_sane(*latest_lio_twist_)) {
+      using_lio_ = false;
+      return false;
+    }
+
+    if (require_lio_wheel_consistency_ && is_lio_wheel_disagreeing()) {
+      using_lio_ = false;
+      return false;
+    }
+
+    const double lio_abs_wz = std::abs(latest_lio_twist_->twist.twist.angular.z);
+    const double wheel_abs_wz =
+      latest_wheel_twist_ ? std::abs(latest_wheel_twist_->twist.twist.angular.z) : 0.0;
+    const double abs_wz = std::max(lio_abs_wz, wheel_abs_wz);
+
+    if (using_lio_) {
+      using_lio_ = abs_wz > turn_off_angular_velocity_rps_;
+    } else {
+      using_lio_ = abs_wz > turn_on_angular_velocity_rps_;
+    }
+
+    return using_lio_;
+  }
+
+  bool is_fresh(
+    const TwistWithCovarianceStamped::ConstSharedPtr & msg, const double timeout_sec) const
+  {
+    if (!msg) {
+      return false;
+    }
+    return std::abs((now() - rclcpp::Time(msg->header.stamp)).seconds()) <= timeout_sec;
+  }
+
+  bool is_sane(const TwistWithCovarianceStamped & msg) const
+  {
+    const auto & twist = msg.twist.twist;
+    return std::isfinite(twist.linear.x) && std::isfinite(twist.angular.z) &&
+           std::abs(twist.linear.x) <= max_abs_linear_x_mps_ &&
+           std::abs(twist.angular.z) <= max_abs_angular_z_rps_;
+  }
+
+  bool is_lio_wheel_disagreeing() const
+  {
+    if (
+      !is_fresh(latest_lio_twist_, lio_timeout_sec_) ||
+      !is_fresh(latest_wheel_twist_, wheel_timeout_sec_)) {
+      return false;
+    }
+
+    const double vx_diff = std::abs(
+      latest_lio_twist_->twist.twist.linear.x - latest_wheel_twist_->twist.twist.linear.x);
+    const double wz_diff = std::abs(
+      latest_lio_twist_->twist.twist.angular.z - latest_wheel_twist_->twist.twist.angular.z);
+
+    return vx_diff > max_lio_wheel_linear_x_diff_mps_ ||
+           wz_diff > max_lio_wheel_angular_z_diff_rps_;
+  }
+
+  TwistWithCovarianceStamped blend(
+    const TwistWithCovarianceStamped & wheel, const TwistWithCovarianceStamped & lio) const
+  {
+    TwistWithCovarianceStamped out = lio;
+    out.header.stamp = lio.header.stamp;
+
+    const double wheel_vx_var = positive_or_default(wheel.twist.covariance[covariance_index(0, 0)]);
+    const double lio_vx_var = positive_or_default(lio.twist.covariance[covariance_index(0, 0)]);
+    const double wheel_wz_var = positive_or_default(wheel.twist.covariance[covariance_index(5, 5)]);
+    const double lio_wz_var = positive_or_default(lio.twist.covariance[covariance_index(5, 5)]);
+
+    out.twist.twist.linear.x = fuse_scalar(
+      wheel.twist.twist.linear.x, wheel_vx_var, lio.twist.twist.linear.x, lio_vx_var);
+    out.twist.twist.angular.z = fuse_scalar(
+      wheel.twist.twist.angular.z, wheel_wz_var, lio.twist.twist.angular.z, lio_wz_var);
+    out.twist.covariance[covariance_index(0, 0)] = fused_variance(wheel_vx_var, lio_vx_var);
+    out.twist.covariance[covariance_index(5, 5)] = fused_variance(wheel_wz_var, lio_wz_var);
+
+    if (is_lio_wheel_disagreeing()) {
+      inflate_observed_covariance(out, disagreement_covariance_scale_);
+    }
+
+    return out;
+  }
+
+  static double positive_or_default(const double variance)
+  {
+    constexpr double min_variance = 1.0e-6;
+    constexpr double default_variance = 1.0;
+    if (!std::isfinite(variance) || variance < min_variance) {
+      return default_variance;
+    }
+    return variance;
+  }
+
+  static double fuse_scalar(
+    const double a, const double a_variance, const double b, const double b_variance)
+  {
+    const double a_weight = 1.0 / a_variance;
+    const double b_weight = 1.0 / b_variance;
+    return (a_weight * a + b_weight * b) / (a_weight + b_weight);
+  }
+
+  static double fused_variance(const double a_variance, const double b_variance)
+  {
+    return 1.0 / (1.0 / a_variance + 1.0 / b_variance);
+  }
+
+  static void inflate_observed_covariance(TwistWithCovarianceStamped & msg, const double scale)
+  {
+    const double safe_scale = std::max(scale, 1.0);
+    msg.twist.covariance[covariance_index(0, 0)] =
+      std::max(msg.twist.covariance[covariance_index(0, 0)], 1.0e-6) * safe_scale;
+    msg.twist.covariance[covariance_index(5, 5)] =
+      std::max(msg.twist.covariance[covariance_index(5, 5)], 1.0e-6) * safe_scale;
+  }
+
+  rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr wheel_twist_sub_;
+  rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr lio_twist_sub_;
+  rclcpp::Publisher<TwistWithCovarianceStamped>::SharedPtr twist_pub_;
+  TwistWithCovarianceStamped::ConstSharedPtr latest_wheel_twist_;
+  TwistWithCovarianceStamped::ConstSharedPtr latest_lio_twist_;
+
+  std::string output_frame_id_;
+  std::string selection_mode_;
+  double wheel_timeout_sec_{0.3};
+  double lio_timeout_sec_{0.3};
+  double turn_on_angular_velocity_rps_{0.25};
+  double turn_off_angular_velocity_rps_{0.15};
+  double max_abs_linear_x_mps_{30.0};
+  double max_abs_angular_z_rps_{3.0};
+  double max_lio_wheel_linear_x_diff_mps_{5.0};
+  double max_lio_wheel_angular_z_diff_rps_{1.0};
+  bool require_lio_wheel_consistency_{false};
+  bool using_lio_{false};
+  double disagreement_covariance_scale_{4.0};
+};
+
+}  // namespace twist_fusion_selector
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(twist_fusion_selector::TwistFusionSelector)