diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
index 995815a5f..80975c333 100644
--- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
+++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
@@ -44,8 +44,8 @@ platform_velocity_controller:
rear_right_wheel_command_joint_name: "rear_right_wheel"
rear_left_wheel_command_joint_name: "rear_left_wheel"
- kinematics.wheels_radius: 0.05
- kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.4704
+ kinematics.wheels_radius: 0.0759
+ kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59
wheel_separation_multiplier: 1.0
wheel_radius_multiplier: 1.0
@@ -53,8 +53,8 @@ platform_velocity_controller:
reference_timeout: 0.1
use_stamped_vel: False
- pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
- twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
+ pose_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 0.5]
+ twist_covariance_diagonal: [0.1, 0.1, 0.1, 1000000.0, 1000000.0, 0.5]
cmd_vel_timeout: 0.5
@@ -105,8 +105,8 @@ platform_velocity_controller_nav2:
rear_right_wheel_command_joint_name: "rear_right_wheel"
rear_left_wheel_command_joint_name: "rear_left_wheel"
- kinematics.wheels_radius: 0.05
- kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.4704
+ kinematics.wheels_radius: 0.0759
+ kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59
wheel_separation_multiplier: 1.0
wheel_radius_multiplier: 1.0
@@ -282,7 +282,14 @@ force_torque_sensor_broadcaster:
imu_sensor_broadcaster:
ros__parameters:
sensor_name: imu_site
- frame_id: imu_link
+ frame_id: ridgeback_base_link
+ # Static covariance values (row-major 3x3 matrices)
+ # 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: [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: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
velocity_force_controller:
diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml
index db8a0d8a1..fbc429d41 100644
--- a/src/hangar_sim/config/fuse/fuse.yaml
+++ b/src/hangar_sim/config/fuse/fuse.yaml
@@ -1,5 +1,5 @@
# Mobile base localization configuration using fuse
-# Using 3D odometry model for wheel odometry-based pose estimation
+# Ground vehicle state estimation: wheel odometry + IMU
state_estimator:
ros__parameters:
# Fixed-lag smoother configuration
@@ -15,7 +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
- 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]
+ # 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:
@@ -25,32 +28,48 @@ state_estimator:
wheel_odom_sensor:
type: fuse_models::Odometry3D
motion_models: [mobile_base_motion]
+ imu_sensor:
+ type: fuse_models::Imu3D
+ 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]
+ # 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 — 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
+ # accumulated drift from corrupting the estimate.
wheel_odom_sensor:
- # Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE)
- topic: /odom_reliable
+ topic: /platform_velocity_controller/odom
queue_size: 10
- # 2D position from wheels (no z)
- position_dimensions: ['x', 'y']
- # Heading only
+ pose_loss:
+ type: fuse_loss::HuberLoss
+ a: 1.0
+ linear_velocity_loss:
+ type: fuse_loss::HuberLoss
+ a: 1.0
+ position_dimensions: ['x', 'y', 'z']
orientation_dimensions: ['yaw']
- # Forward velocity
- linear_velocity_dimensions: ['x']
- # Turn rate
+ linear_velocity_dimensions: ['x', 'y']
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]
+ independent: false
+ use_twist_covariance: true
+
+ # 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
+ imu_sensor:
+ topic: /imu_sensor_broadcaster/imu_reliable
+ orientation_dimensions: ['roll', 'pitch', 'yaw']
+ angular_velocity_dimensions: ['yaw']
+ queue_size: 10
# Publish filtered odometry
publishers:
@@ -59,8 +78,8 @@ state_estimator:
filtered_publisher:
topic: 'odom_filtered'
- base_link_frame_id: 'base_footprint'
- base_link_output_frame_id: 'base_footprint'
+ base_link_frame_id: 'ridgeback_base_link'
+ base_link_output_frame_id: 'ridgeback_base_link'
odom_frame_id: 'odom'
map_frame_id: 'map'
world_frame_id: 'odom'
diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml
index f06fcf854..158b7346a 100644
--- a/src/hangar_sim/description/front_left_wheel_link.xml
+++ b/src/hangar_sim/description/front_left_wheel_link.xml
@@ -2,356 +2,232 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/hangar_sim/description/front_right_wheel_link.xml b/src/hangar_sim/description/front_right_wheel_link.xml
index 38c6acce2..647208f44 100644
--- a/src/hangar_sim/description/front_right_wheel_link.xml
+++ b/src/hangar_sim/description/front_right_wheel_link.xml
@@ -2,356 +2,232 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
index a5c4d2e66..745bf9545 100644
--- a/src/hangar_sim/description/hangar_scene.xml
+++ b/src/hangar_sim/description/hangar_scene.xml
@@ -5,7 +5,13 @@
-
+
@@ -14,6 +20,18 @@
+
+
diff --git a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
index 85c866dd2..dd0b93e96 100644
--- a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
+++ b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
@@ -16,6 +16,18 @@
${mujoco_viewer}
default
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/hangar_sim/description/rear_left_wheel_link.xml b/src/hangar_sim/description/rear_left_wheel_link.xml
index 2484daab0..dad1830c9 100644
--- a/src/hangar_sim/description/rear_left_wheel_link.xml
+++ b/src/hangar_sim/description/rear_left_wheel_link.xml
@@ -2,356 +2,232 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/hangar_sim/description/rear_right_wheel_link.xml b/src/hangar_sim/description/rear_right_wheel_link.xml
index c044b53c6..aa1936161 100644
--- a/src/hangar_sim/description/rear_right_wheel_link.xml
+++ b/src/hangar_sim/description/rear_right_wheel_link.xml
@@ -2,356 +2,232 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index 8eb82040d..a222eda4d 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -194,7 +194,7 @@
-
+
@@ -203,9 +203,14 @@
-
-
-
+
+
+
diff --git a/src/hangar_sim/launch/agent_bridge.launch.xml b/src/hangar_sim/launch/agent_bridge.launch.xml
index 3cc960c03..b2aa75434 100644
--- a/src/hangar_sim/launch/agent_bridge.launch.xml
+++ b/src/hangar_sim/launch/agent_bridge.launch.xml
@@ -3,5 +3,4 @@
-
diff --git a/src/hangar_sim/launch/fuse.launch.py b/src/hangar_sim/launch/fuse.launch.py
deleted file mode 100644
index 089f93f53..000000000
--- a/src/hangar_sim/launch/fuse.launch.py
+++ /dev/null
@@ -1,38 +0,0 @@
-#! /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"])
- ],
- ),
- ]
- )
diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
index 92d81f6c8..c4961a50a 100644
--- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
+++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
@@ -268,34 +268,14 @@ def generate_launch_description():
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"],
)
- # Publish odometry as joint state messages
- odom_to_joint_state_repub = Node(
- package="hangar_sim",
- executable="odometry_joint_state_publisher.py",
- name="odometry_joint_state_publisher",
- output="log",
- )
-
- # 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",
)
- # 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()
@@ -324,8 +304,19 @@ 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(sensor_qos_relay)
+
+ # 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",
+ )
ld.add_action(fuse_state_estimator)
return ld
diff --git a/src/hangar_sim/script/odom_qos_relay.py b/src/hangar_sim/script/odom_qos_relay.py
index 11c588922..e0bbdada0 100755
--- a/src/hangar_sim/script/odom_qos_relay.py
+++ b/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,30 @@ 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()