added qos relay for the IMU

Author: suchkristenwow

src/hangar_sim/config/control/picknik_ur.ros2_control.yaml

@@ -287,9 +287,9 @@ imu_sensor_broadcaster:
     # Orientation covariance (rad^2)
     static_covariance_orientation: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001]
     # Angular velocity covariance (rad/s)^2
-    static_covariance_angular_velocity: [100.0, 0.0, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0, 100.0]
+    static_covariance_angular_velocity: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
     # Linear acceleration covariance (m/s^2)^2
-    static_covariance_linear_acceleration: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+    static_covariance_linear_acceleration: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
 
 
 velocity_force_controller:

src/hangar_sim/config/fuse/fuse.yaml

@@ -15,12 +15,10 @@ state_estimator:
     mobile_base_motion:
       # Process noise for state variables
       # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
-      # Ground robot: z, roll, pitch (and their velocities) should be small
-      # since the robot stays on the ground plane and level.
-      #trying this with the actual wheel odom 
-      process_noise_diagonal: [0.25, 0.25, 0.001, 0.001, 0.001, 0.25, 0.1, 0.1, 0.001, 0.001, 0.001, 0.1, 0.001, 0.001, 0.001] 
-      #this one works with the ground truth odom 
-      #process_noise_diagonal: [2.0, 2.0, 0.1, 0.1, 0.1, 2.0, 2.0, 1.0, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.001]
+      # z, roll, pitch and their derivatives are near-zero for a ground robot.
+      #process_noise_diagonal: [0.25, 0.25, 0.00001, 0.00001, 0.00001, 0.25
+      #   -, 0.1, 0.1, 0.00001, 0.00001, 0.0001, 0.1, 0.0001, 0.0001, 0.00001] 
+      process_noise_diagonal: [1.0, 1.0, 0.0001, 0.0001, 0.0001, 0.5, 0.5, 0.5, 0.0001, 0.0001, 0.0001, 0.25, 0.00001, 0.00001, 0.00001]
 
     sensor_models:
       initial_localization_sensor:
@@ -38,9 +36,11 @@ state_estimator:
       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]
+      # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
+      # z and its derivatives start with very tight sigma — ground robot stays on the plane.
+      initial_sigma: [0.1, 0.1, 0.001, 0.01, 0.01, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.1, 0.001, 0.001, 0.001]
 
-    # Wheel odometry — differential mode
+    # Wheel odometry — provides translation (x, y) and yaw
     # Using differential mode because platform_velocity_controller/odom is
     # wheel-integrated odometry that drifts over time. Differential mode fuses
     # only the relative change between consecutive messages, preventing
@@ -56,24 +56,20 @@ state_estimator:
         a: 1.0
       position_dimensions: ['x', 'y', 'z']
       orientation_dimensions: ['yaw']
-      linear_velocity_dimensions: ['x']
+      linear_velocity_dimensions: ['x', 'y']
       angular_velocity_dimensions: ['yaw']
       differential: true
       independent: false
       use_twist_covariance: true
 
-    # IMU sensor
-    # - Roll/pitch orientation from gravity vector (absolute, keeps robot level)
+    # IMU sensor — provides orientation and yaw rate
+    # - Roll/pitch/yaw orientation (absolute, covariance 0.001 vs wheel odom 0.5)
     # - Yaw angular velocity from gyroscope
-    # - Z linear acceleration (gravity-compensated, constrains vertical dynamics)
     imu_sensor:
-      topic: /imu_sensor_broadcaster/imu
-      orientation_dimensions: ['roll', 'pitch']
+      topic: /imu_sensor_broadcaster/imu_reliable
+      orientation_dimensions: ['roll', 'pitch', 'yaw']
       angular_velocity_dimensions: ['yaw']
-      linear_acceleration_dimensions: ['z']
-      remove_gravitational_acceleration: true
       queue_size: 10
-      throttle_period: 0.1
 
     # Publish filtered odometry
     publishers:

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

@@ -268,11 +268,11 @@ def generate_launch_description():
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"],
     )
 
-    # QoS relay to bridge BEST_EFFORT odom to RELIABLE for fuse
-    odom_qos_relay = Node(
+    # QoS relay to bridge BEST_EFFORT odom and IMU to RELIABLE for fuse
+    sensor_qos_relay = Node(
         package="hangar_sim",
         executable="odom_qos_relay.py",
-        name="odom_qos_relay",
+        name="sensor_qos_relay",
         output="log",
     )
 
@@ -304,7 +304,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_qos_relay)
+    ld.add_action(sensor_qos_relay)
 
     # Fuse state estimator for mobile base localization
     hangar_sim_pkg = FindPackageShare("hangar_sim")

src/hangar_sim/script/odom_qos_relay.py

@@ -28,12 +28,13 @@
 # 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."""
+"""Relay node to bridge BEST_EFFORT sensor topics to RELIABLE QoS for fuse."""
 
 import rclpy
 from rclpy.node import Node
 
 from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Imu
 
 from rclpy.qos import (
     QoSProfile,
@@ -43,11 +44,11 @@
 )
 
 
-class OdomQoSRelay(Node):
-    """Relay node that subscribes to odometry with BEST_EFFORT and republishes with RELIABLE QoS."""
+class SensorQoSRelay(Node):
+    """Relay node that subscribes to sensor topics with BEST_EFFORT and republishes with RELIABLE QoS."""
 
     def __init__(self):
-        super().__init__("odom_qos_relay")
+        super().__init__("sensor_qos_relay")
 
         # Subscribe with BEST_EFFORT to match mujoco_system publisher
         qos_sub = QoSProfile(
@@ -65,18 +66,28 @@ def __init__(self):
             depth=10,
         )
 
-        self.sub = self.create_subscription(
+        # Odometry relay
+        self.odom_sub = self.create_subscription(
             Odometry, "/odom", self.odom_callback, qos_sub
         )
-        self.pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub)
+        self.odom_pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub)
+
+        # IMU relay
+        self.imu_sub = self.create_subscription(
+            Imu, "/imu_sensor_broadcaster/imu", self.imu_callback, qos_sub
+        )
+        self.imu_pub = self.create_publisher(Imu, "/imu_sensor_broadcaster/imu_reliable", qos_pub)
 
     def odom_callback(self, msg):
-        self.pub.publish(msg)
+        self.odom_pub.publish(msg)
+
+    def imu_callback(self, msg):
+        self.imu_pub.publish(msg)
 
 
 def main(args=None):
     rclpy.init(args=args)
-    node = OdomQoSRelay()
+    node = SensorQoSRelay()
     rclpy.spin(node)
     node.destroy_node()
     rclpy.shutdown()