Skip to content

Commit 4e92bcc

Browse files
suchkristenwowsuchkristenwow
committed
initial commit
1 parent 2a0bd41 commit 4e92bcc

File tree

6 files changed

+219
-7
lines changed

6 files changed

+219
-7
lines changed

‎src/hangar_sim/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ pluginlib_export_plugin_description_file(
2424

2525
install(PROGRAMS
2626
script/odometry_joint_state_publisher.py
27+
script/odom_qos_relay.py
2728
DESTINATION lib/${PROJECT_NAME}
2829
)
2930

‎src/hangar_sim/config/fuse/fuse.yaml‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
# Mobile base localization configuration using fuse
2+
# Using 3D odometry model for wheel odometry-based pose estimation
3+
state_estimator:
4+
ros__parameters:
5+
# Fixed-lag smoother configuration
6+
optimization_frequency: 20.0
7+
transaction_timeout: 0.01
8+
lag_duration: 0.5
9+
10+
# Motion model for mobile base (3D omnidirectional)
11+
motion_models:
12+
mobile_base_motion:
13+
type: fuse_models::Omnidirectional3D
14+
15+
mobile_base_motion:
16+
# Process noise for state variables
17+
# Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
18+
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]
19+
20+
sensor_models:
21+
initial_localization_sensor:
22+
type: fuse_models::Omnidirectional3DIgnition
23+
motion_models: [mobile_base_motion]
24+
ignition: true
25+
wheel_odom_sensor:
26+
type: fuse_models::Odometry3D
27+
motion_models: [mobile_base_motion]
28+
29+
initial_localization_sensor:
30+
publish_on_startup: true
31+
# Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
32+
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]
33+
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]
34+
35+
wheel_odom_sensor:
36+
# Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE)
37+
topic: /odom_reliable
38+
queue_size: 10
39+
# 2D position from wheels (no z)
40+
position_dimensions: ['x', 'y']
41+
# Heading only
42+
orientation_dimensions: ['yaw']
43+
# Forward velocity
44+
linear_velocity_dimensions: ['x']
45+
# Turn rate
46+
angular_velocity_dimensions: ['yaw']
47+
# Use relative constraints (pose changes rather than absolute poses)
48+
differential: true
49+
# Covariances - use large values for unused dimensions
50+
# Order: x, y, z, roll, pitch, yaw
51+
pose_covariance_diagonal: [0.05, 0.05, 1e9, 1e9, 1e9, 0.05]
52+
# Order: vx, vy, vz, vroll, vpitch, vyaw
53+
twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05]
54+
55+
# Publish filtered odometry
56+
publishers:
57+
filtered_publisher:
58+
type: fuse_models::Odometry3DPublisher
59+
60+
filtered_publisher:
61+
topic: 'odom_filtered'
62+
base_link_frame_id: 'base_footprint'
63+
base_link_output_frame_id: 'base_footprint'
64+
odom_frame_id: 'odom'
65+
map_frame_id: 'map'
66+
world_frame_id: 'odom'
67+
publish_tf: true
68+
publish_frequency: 10.0
69+
predict_to_current_time: true

‎src/hangar_sim/launch/agent_bridge.launch.xml‎

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,5 @@
33
<include
44
file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
55
/>
6-
<!-- <node-->
7-
<!-- pkg="hangar_sim"-->
8-
<!-- exec="odometry_joint_state_publisher.py"-->
9-
<!-- name="odometry_joint_state_publisher"-->
10-
<!-- output="both"-->
11-
<!-- />-->
6+
<include file="$(find-pkg-share hangar_sim)/launch/fuse.launch.py" />
127
</launch>

‎src/hangar_sim/launch/fuse.launch.py‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
#! /usr/bin/env python3
2+
3+
# Copyright 2024 PickNik Robotics
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
17+
from launch import LaunchDescription
18+
from launch.substitutions import PathJoinSubstitution
19+
from launch_ros.actions import Node
20+
from launch_ros.substitutions import FindPackageShare
21+
22+
23+
def generate_launch_description():
24+
pkg_dir = FindPackageShare("hangar_sim")
25+
26+
return LaunchDescription(
27+
[
28+
# Run the fuse fixed-lag smoother for mobile base state estimation
29+
Node(
30+
package="fuse_optimizers",
31+
executable="fixed_lag_smoother_node",
32+
name="state_estimator",
33+
parameters=[
34+
PathJoinSubstitution([pkg_dir, "config", "fuse", "fuse.yaml"])
35+
],
36+
),
37+
]
38+
)

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

Lines changed: 24 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,10 +39,11 @@
3939
)
4040
from launch.conditions import IfCondition
4141
from launch.launch_description_sources import PythonLaunchDescriptionSource
42-
from launch.substitutions import LaunchConfiguration, PythonExpression
42+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
4343
from launch_ros.actions import Node
4444
from launch_ros.actions import PushRosNamespace
4545
from launch_ros.descriptions import ParameterFile
46+
from launch_ros.substitutions import FindPackageShare
4647
from nav2_common.launch import RewrittenYaml, ReplaceString
4748

4849

@@ -279,6 +280,26 @@ def generate_launch_description():
279280
output="log",
280281
)
281282

283+
# QoS relay to bridge BEST_EFFORT odom to RELIABLE for fuse
284+
odom_qos_relay = Node(
285+
package="hangar_sim",
286+
executable="odom_qos_relay.py",
287+
name="odom_qos_relay",
288+
output="log",
289+
)
290+
291+
# Fuse state estimator for mobile base localization
292+
hangar_sim_pkg = FindPackageShare("hangar_sim")
293+
fuse_state_estimator = Node(
294+
package="fuse_optimizers",
295+
executable="fixed_lag_smoother_node",
296+
name="state_estimator",
297+
parameters=[
298+
PathJoinSubstitution([hangar_sim_pkg, "config", "fuse", "fuse.yaml"])
299+
],
300+
output="screen",
301+
)
302+
282303
# Create the launch description and populate
283304
ld = LaunchDescription()
284305

@@ -310,5 +331,7 @@ def generate_launch_description():
310331
ld.add_action(static_tf_world_to_map)
311332
ld.add_action(static_tf_map_to_odom)
312333
ld.add_action(odom_to_joint_state_repub)
334+
ld.add_action(odom_qos_relay)
335+
ld.add_action(fuse_state_estimator)
313336

314337
return ld

‎src/hangar_sim/script/odom_qos_relay.py‎

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
#!/usr/bin/env python3
2+
3+
# Copyright 2024 PickNik Inc.
4+
#
5+
# Redistribution and use in source and binary forms, with or without
6+
# modification, are permitted provided that the following conditions are met:
7+
#
8+
# * Redistributions of source code must retain the above copyright
9+
# notice, this list of conditions and the following disclaimer.
10+
#
11+
# * Redistributions in binary form must reproduce the above copyright
12+
# notice, this list of conditions and the following disclaimer in the
13+
# documentation and/or other materials provided with the distribution.
14+
#
15+
# * Neither the name of the PickNik Inc. nor the names of its
16+
# contributors may be used to endorse or promote products derived from
17+
# this software without specific prior written permission.
18+
#
19+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29+
# POSSIBILITY OF SUCH DAMAGE.
30+
31+
"""Relay node to bridge BEST_EFFORT odometry to RELIABLE QoS for fuse."""
32+
33+
import rclpy
34+
from rclpy.node import Node
35+
36+
from nav_msgs.msg import Odometry
37+
38+
from rclpy.qos import (
39+
QoSProfile,
40+
QoSReliabilityPolicy,
41+
QoSDurabilityPolicy,
42+
QoSHistoryPolicy,
43+
)
44+
45+
46+
class OdomQoSRelay(Node):
47+
"""Relay node that subscribes to odometry with BEST_EFFORT and republishes with RELIABLE QoS."""
48+
49+
def __init__(self):
50+
super().__init__("odom_qos_relay")
51+
52+
# Subscribe with BEST_EFFORT to match mujoco_system publisher
53+
qos_sub = QoSProfile(
54+
reliability=QoSReliabilityPolicy.BEST_EFFORT,
55+
durability=QoSDurabilityPolicy.VOLATILE,
56+
history=QoSHistoryPolicy.KEEP_LAST,
57+
depth=10,
58+
)
59+
60+
# Publish with RELIABLE for fuse state_estimator
61+
qos_pub = QoSProfile(
62+
reliability=QoSReliabilityPolicy.RELIABLE,
63+
durability=QoSDurabilityPolicy.VOLATILE,
64+
history=QoSHistoryPolicy.KEEP_LAST,
65+
depth=10,
66+
)
67+
68+
self.sub = self.create_subscription(
69+
Odometry, "/odom", self.odom_callback, qos_sub
70+
)
71+
self.pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub)
72+
73+
def odom_callback(self, msg):
74+
self.pub.publish(msg)
75+
76+
77+
def main(args=None):
78+
rclpy.init(args=args)
79+
node = OdomQoSRelay()
80+
rclpy.spin(node)
81+
node.destroy_node()
82+
rclpy.shutdown()
83+
84+
85+
if __name__ == "__main__":
86+
main()

0 commit comments

Comments
 (0)