Add Fuse Config to Hangar Sim

src/hangar_sim/CMakeLists.txt

@@ -24,6 +24,7 @@ pluginlib_export_plugin_description_file(
 
 install(PROGRAMS
   script/odometry_joint_state_publisher.py
+  script/odom_qos_relay.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/hangar_sim/config/fuse/fuse.yaml

@@ -0,0 +1,69 @@
+# Mobile base localization configuration using fuse
+# Using 3D odometry model for wheel odometry-based pose estimation
+state_estimator:
+  ros__parameters:
+    # Fixed-lag smoother configuration
+    optimization_frequency: 20.0
+    transaction_timeout: 0.01
+    lag_duration: 0.5
+
+    # Motion model for mobile base (3D omnidirectional)
+    motion_models:
+      mobile_base_motion:
+        type: fuse_models::Omnidirectional3D
+
+    mobile_base_motion:
+      # Process noise for state variables
+      # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
+      process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0, 1.0, 1.0]
+
+    sensor_models:
+      initial_localization_sensor:
+        type: fuse_models::Omnidirectional3DIgnition
+        motion_models: [mobile_base_motion]
+        ignition: true
+      wheel_odom_sensor:
+        type: fuse_models::Odometry3D
+        motion_models: [mobile_base_motion]
+
+    initial_localization_sensor:
+      publish_on_startup: true
+      # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
+      initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+
+    wheel_odom_sensor:
+      # Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE)
+      topic: /odom_reliable
+      queue_size: 10
+      # 2D position from wheels (no z)
+      position_dimensions: ['x', 'y']
+      # Heading only
+      orientation_dimensions: ['yaw']
+      # Forward velocity
+      linear_velocity_dimensions: ['x']
+      # Turn rate
+      angular_velocity_dimensions: ['yaw']
+      # Use relative constraints (pose changes rather than absolute poses)
+      differential: true
+      # Covariances - use large values for unused dimensions
+      # Order: x, y, z, roll, pitch, yaw
+      pose_covariance_diagonal: [0.05, 0.05, 1e9, 1e9, 1e9, 0.05]
+      # Order: vx, vy, vz, vroll, vpitch, vyaw
+      twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05]
+
+    # Publish filtered odometry
+    publishers:
+      filtered_publisher:
+        type: fuse_models::Odometry3DPublisher
+
+    filtered_publisher:
+      topic: 'odom_filtered'
+      base_link_frame_id: 'base_footprint'
+      base_link_output_frame_id: 'base_footprint'
+      odom_frame_id: 'odom'
+      map_frame_id: 'map'
+      world_frame_id: 'odom'
+      publish_tf: true
+      publish_frequency: 10.0
+      predict_to_current_time: true

src/hangar_sim/launch/agent_bridge.launch.xml

@@ -3,10 +3,5 @@
   <include
     file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
   />
-  <!--  <node-->
-  <!--    pkg="hangar_sim"-->
-  <!--    exec="odometry_joint_state_publisher.py"-->
-  <!--    name="odometry_joint_state_publisher"-->
-  <!--    output="both"-->
-  <!--  />-->
+  <include file="$(find-pkg-share hangar_sim)/launch/fuse.launch.py" />
 </launch>

src/hangar_sim/launch/fuse.launch.py

@@ -0,0 +1,38 @@
+#! /usr/bin/env python3
+
+# Copyright 2024 PickNik Robotics
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    pkg_dir = FindPackageShare("hangar_sim")
+
+    return LaunchDescription(
+        [
+            # Run the fuse fixed-lag smoother for mobile base state estimation
+            Node(
+                package="fuse_optimizers",
+                executable="fixed_lag_smoother_node",
+                name="state_estimator",
+                parameters=[
+                    PathJoinSubstitution([pkg_dir, "config", "fuse", "fuse.yaml"])
+                ],
+            ),
+        ]
+    )

src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py

@@ -39,10 +39,15 @@
 )
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
 from launch_ros.actions import Node
 from launch_ros.actions import PushRosNamespace
 from launch_ros.descriptions import ParameterFile
+from launch_ros.substitutions import FindPackageShare
 from nav2_common.launch import RewrittenYaml, ReplaceString
 
 
@@ -279,6 +284,26 @@ def generate_launch_description():
         output="log",
     )
 
+    # QoS relay to bridge BEST_EFFORT odom to RELIABLE for fuse
+    odom_qos_relay = Node(
+        package="hangar_sim",
+        executable="odom_qos_relay.py",
+        name="odom_qos_relay",
+        output="log",
+    )
+
+    # Fuse state estimator for mobile base localization
+    hangar_sim_pkg = FindPackageShare("hangar_sim")
+    fuse_state_estimator = Node(
+        package="fuse_optimizers",
+        executable="fixed_lag_smoother_node",
+        name="state_estimator",
+        parameters=[
+            PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"])
+        ],
+        output="screen",
+    )
+
     # Create the launch description and populate
     ld = LaunchDescription()
 
@@ -310,5 +335,7 @@ def generate_launch_description():
     ld.add_action(static_tf_world_to_map)
     ld.add_action(static_tf_map_to_odom)
     ld.add_action(odom_to_joint_state_repub)
+    ld.add_action(odom_qos_relay)
+    ld.add_action(fuse_state_estimator)
 
     return ld

src/hangar_sim/script/odom_qos_relay.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 PickNik Inc.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the PickNik Inc. nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""Relay node to bridge BEST_EFFORT odometry to RELIABLE QoS for fuse."""
+
+import rclpy
+from rclpy.node import Node
+
+from nav_msgs.msg import Odometry
+
+from rclpy.qos import (
+    QoSProfile,
+    QoSReliabilityPolicy,
+    QoSDurabilityPolicy,
+    QoSHistoryPolicy,
+)
+
+
+class OdomQoSRelay(Node):
+    """Relay node that subscribes to odometry with BEST_EFFORT and republishes with RELIABLE QoS."""
+
+    def __init__(self):
+        super().__init__("odom_qos_relay")
+
+        # Subscribe with BEST_EFFORT to match mujoco_system publisher
+        qos_sub = QoSProfile(
+            reliability=QoSReliabilityPolicy.BEST_EFFORT,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+
+        # Publish with RELIABLE for fuse state_estimator
+        qos_pub = QoSProfile(
+            reliability=QoSReliabilityPolicy.RELIABLE,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+
+        self.sub = self.create_subscription(
+            Odometry, "/odom", self.odom_callback, qos_sub
+        )
+        self.pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub)
+
+    def odom_callback(self, msg):
+        self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomQoSRelay()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()