Skip to content

feat: hangar_sim, add beluga_amcl localization with hardcoded initial… #605

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
bkanator wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

feat: hangar_sim, add beluga_amcl localization with hardcoded initial… #605

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
244 changes: 136 additions & 108 deletions src/hangar_sim/launch/sim/localization_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,47 +26,42 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

# Non-composable launch mode is not supported — hangar_sim always uses use_composition:=True.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.actions import (
DeclareLaunchArgument,
ExecuteProcess,
OpaqueFunction,
SetEnvironmentVariable,
TimerAction,
)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import LoadComposableNodes
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode, ParameterFile
from nav2_common.launch import RewrittenYaml


# This file is adapted from: https://github.com/ros-navigation/navigation2/blob/humble/nav2_bringup/launch/localization_launch.py
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory("nav2_bringup")

namespace = LaunchConfiguration("namespace")
map_yaml_file = LaunchConfiguration("map")
use_sim_time = LaunchConfiguration("use_sim_time")
autostart = LaunchConfiguration("autostart")
params_file = LaunchConfiguration("params_file")
use_composition = LaunchConfiguration("use_composition")
container_name = LaunchConfiguration("container_name")
container_name_full = (namespace, "/", container_name)
use_respawn = LaunchConfiguration("use_respawn")
log_level = LaunchConfiguration("log_level")

@coderabbitai coderabbitai Bot Jun 4, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟡 Minor | ⚡ Quick win

Remove the unused log_level LaunchConfiguration assignment.

Line 60 is assigned but never used, matching the reported F841 and likely failing lint CI.

Suggested fix 
-    log_level = LaunchConfiguration("log_level")
📝 Committable suggestion

‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.

Suggested change
log_level = LaunchConfiguration("log_level")
🧰 Tools
🪛 Flake8 (7.3.0)

[error] 60-60: local variable 'log_level' is assigned to but never used

(F841)

🤖 Prompt for AI Agents 
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.

In `@src/hangar_sim/launch/sim/localization_launch.py` at line 60, Remove the
unused LaunchConfiguration assignment by deleting the log_level =
LaunchConfiguration("log_level") line (remove the symbol log_level and its
LaunchConfiguration call) so the unused variable no longer triggers F841; ensure
no other references to log_level remain and run lint to confirm the warning is
resolved.

localization = LaunchConfiguration("localization")

lifecycle_nodes = ["map_server"]

# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]

# Create our own temporary YAML files that include substitutions
param_substitutions = {"use_sim_time": use_sim_time, "yaml_filename": map_yaml_file}

configured_params = ParameterFile(
Expand All @@ -83,95 +78,57 @@ def generate_launch_description():
"RCUTILS_LOGGING_BUFFERED_STREAM", "1"
)

declare_namespace_cmd = DeclareLaunchArgument(
"namespace", default_value="", description="Top-level namespace"
)

declare_map_yaml_cmd = DeclareLaunchArgument(
"map", description="Full path to map yaml file to load"
)

declare_use_sim_time_cmd = DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Use simulation (Gazebo) clock if true",
)

declare_params_file_cmd = DeclareLaunchArgument(
"params_file",
default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
description="Full path to the ROS2 parameters file to use for all launched nodes",
)

declare_autostart_cmd = DeclareLaunchArgument(
"autostart",
default_value="true",
description="Automatically startup the nav2 stack",
)

declare_use_composition_cmd = DeclareLaunchArgument(
"use_composition",
default_value="False",
description="Use composed bringup if True",
)

declare_container_name_cmd = DeclareLaunchArgument(
"container_name",
default_value="nav2_container",
description="the name of container that nodes will load in if use composition",
)

declare_use_respawn_cmd = DeclareLaunchArgument(
"use_respawn",
default_value="False",
description="Whether to respawn if a node crashes. Applied when composition is disabled.",
)

declare_log_level_cmd = DeclareLaunchArgument(
"log_level", default_value="info", description="log level"
)

load_nodes = GroupAction(
condition=IfCondition(PythonExpression(["not ", use_composition])),
actions=[
Node(
def check_map_exists(context):
if context.perform_substitution(localization) != "True":
return []
Comment on lines +82 to +83

@coderabbitai coderabbitai Bot Jun 4, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟡 Minor | ⚡ Quick win

Normalize the localization value before map-check gating.

Line 82 does an exact "True" comparison, so localization:=true skips validation while localization can still run. Make the gate boolean-safe.

Suggested fix 
     def check_map_exists(context):
-        if context.perform_substitution(localization) != "True":
+        localization_enabled = (
+            context.perform_substitution(localization).strip().lower()
+            in ("true", "1", "yes", "on")
+        )
+        if not localization_enabled:
             return []
🤖 Prompt for AI Agents 
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.

In `@src/hangar_sim/launch/sim/localization_launch.py` around lines 82 - 83, The
gate compares context.perform_substitution(localization) to the exact string
"True", which fails for "true"/" TRUE"/etc.; normalize the substituted
localization value before comparing (e.g., call .strip().lower() on the result)
so the check uses a boolean-safe comparison in the if statement that currently
references localization; update that condition in localization_launch.py to
compare the normalized string to "true".

map_file = context.perform_substitution(map_yaml_file)
if not os.path.exists(map_file):
raise RuntimeError(
f"Map file not found: {map_file}\n"
"Run SLAM first (slam:=True) to build a map, or provide a map path via map:=<path>."
)
return []

map_check = OpaqueFunction(function=check_map_exists)

# Load map_server and amcl into the shared nav2_container when localization is enabled.
load_localization_nodes = LoadComposableNodes(
condition=IfCondition(localization),
target_container=container_name_full,
composable_node_descriptions=[
ComposableNode(
package="nav2_map_server",
executable="map_server",
plugin="nav2_map_server::MapServer",
name="map_server",
output="screen",
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
arguments=["--ros-args", "--log-level", log_level],
remappings=remappings,
),
# Node(
# package='nav2_amcl',
# executable='amcl',
# name='amcl',
# output='screen',
# respawn=use_respawn,
# respawn_delay=2.0,
# parameters=[configured_params],
# arguments=['--ros-args', '--log-level', log_level],
# remappings=remappings),
Node(
ComposableNode(
package="beluga_amcl",
plugin="beluga_amcl::AmclNode",
name="amcl",
parameters=[configured_params],
remappings=remappings,
),
ComposableNode(
package="nav2_lifecycle_manager",
executable="lifecycle_manager",
plugin="nav2_lifecycle_manager::LifecycleManager",
name="lifecycle_manager_localization",
output="screen",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
{"use_sim_time": use_sim_time},
{"autostart": autostart},
{"node_names": lifecycle_nodes},
{
"use_sim_time": use_sim_time,
"autostart": autostart,
"node_names": ["map_server", "amcl"],
}
],
),
],
)

load_composable_nodes = LoadComposableNodes(
condition=IfCondition(use_composition),
# Load map_server only (no AMCL) when localization is disabled.
# A static map->odom TF is expected from the parent launch in this case.
load_map_server_only = LoadComposableNodes(
condition=IfCondition(PythonExpression(["not ", localization])),
target_container=container_name_full,
composable_node_descriptions=[
ComposableNode(
Expand All @@ -189,32 +146,103 @@ def generate_launch_description():
{
"use_sim_time": use_sim_time,
"autostart": autostart,
"node_names": lifecycle_nodes,
"node_names": ["map_server"],
}
],
),
],
)

# Create the launch description and populate
# Publish the robot's known sim spawn pose to /initialpose so beluga_amcl
# can initialize its particle filter without a manual 2D pose estimate.
# The 3-second delay gives map_server and amcl time to activate first.
# Covariance values match nav2_amcl defaults (±0.5 m, ±~15 deg).
initial_pose_pub = TimerAction(
condition=IfCondition(localization),
period=3.0,
actions=[
ExecuteProcess(
cmd=[
"ros2",
"topic",
"pub",
"--once",
"/initialpose",
"geometry_msgs/msg/PoseWithCovarianceStamped",
(
'{"header": {"frame_id": "map"}, '
'"pose": {"pose": {'
'"position": {"x": 0.0, "y": 0.0, "z": 0.0}, '
'"orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}}, '
'"covariance": [0.25, 0, 0, 0, 0, 0, '
"0, 0.25, 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.0685]}}"
),
],
output="log",
)
],
)

ld = LaunchDescription()

# Set environment variables
ld.add_action(stdout_linebuf_envvar)

# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_composition_cmd)
ld.add_action(declare_container_name_cmd)
ld.add_action(declare_use_respawn_cmd)
ld.add_action(declare_log_level_cmd)

# Add the actions to launch all of the localiztion nodes
ld.add_action(load_nodes)
ld.add_action(load_composable_nodes)
ld.add_action(
DeclareLaunchArgument(
"namespace", default_value="", description="Top-level namespace"
)
)
ld.add_action(
DeclareLaunchArgument("map", description="Full path to map yaml file to load")
)
ld.add_action(
DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Use simulation clock if true",
)
)
ld.add_action(
DeclareLaunchArgument(
"params_file",
default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"),
description="Full path to the ROS2 parameters file to use for all launched nodes",
)
)
ld.add_action(
DeclareLaunchArgument(
"autostart",
default_value="true",
description="Automatically startup the nav2 stack",
)
)
ld.add_action(
DeclareLaunchArgument(
"container_name",
default_value="nav2_container",
description="Name of the component container to load nodes into",
)
)
ld.add_action(
DeclareLaunchArgument(
"log_level", default_value="info", description="log level"
)
)
ld.add_action(
DeclareLaunchArgument(
"localization",
default_value="True",
description="Run beluga_amcl for map-based localization. Set False to use a static map->odom TF instead.",
)
)

ld.add_action(map_check)
ld.add_action(load_localization_nodes)
ld.add_action(load_map_server_only)
ld.add_action(initial_pose_pub)

return ld
17 changes: 13 additions & 4 deletions src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,7 @@ def generate_launch_description():
namespace = LaunchConfiguration("namespace")
use_namespace = LaunchConfiguration("use_namespace")
slam = LaunchConfiguration("slam")
localization = LaunchConfiguration("localization")
map_yaml_file = LaunchConfiguration("map")
use_sim_time = LaunchConfiguration("use_sim_time")
params_file = LaunchConfiguration("params_file")
Expand Down Expand Up @@ -180,6 +181,12 @@ def generate_launch_description():
"slam", default_value="False", description="Whether run a SLAM"
)

declare_localization_cmd = DeclareLaunchArgument(
"localization",
default_value="True",
description="Run beluga_amcl for map-based localization. Set False to use a static map->odom TF instead.",
)

declare_map_yaml_cmd = DeclareLaunchArgument(
"map",
default_value=os.path.join(config_dir, "maps", "hangar_map.yaml"),
Expand Down Expand Up @@ -277,6 +284,7 @@ def generate_launch_description():
"use_composition": use_composition,
"use_respawn": use_respawn,
"container_name": "nav2_container",
"localization": localization,
}.items(),
),
IncludeLaunchDescription(
Expand Down Expand Up @@ -316,16 +324,16 @@ def generate_launch_description():
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "mj_world", "map"],
)

# Static map→odom bootstrap: anchors 'odom' in the TF tree before MuJoCo finishes loading so
# bt_navigator can initialize without an "unconnected trees" TF error. Not used in slam mode
# because slam_toolbox owns and publishes map→odom dynamically.
# Static map->odom TF fallback: only used when neither SLAM nor AMCL is publishing it.
static_tf_map_to_odom = Node(
condition=IfCondition(
PythonExpression(["not ", slam, " and not ", localization])
),
package="tf2_ros",
executable="static_transform_publisher",
name="static_tf_map_to_odom",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"],
condition=IfCondition(PythonExpression(["not ", slam])),
)

# Static TF connecting MoveIt's planning root ('world') to the simulation root ('mj_world').
Expand Down Expand Up @@ -411,6 +419,7 @@ def generate_launch_description():
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_slam_cmd)
ld.add_action(declare_localization_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
Expand Down
1 change: 1 addition & 0 deletions src/hangar_sim/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
<exec_depend>moveit_ros_perception</exec_depend>
<exec_depend>moveit_studio_agent</exec_depend>
<exec_depend>moveit_pro_behavior</exec_depend>
<exec_depend>beluga_amcl</exec_depend>
<exec_depend>laser_filters</exec_depend>
<exec_depend>nav2_bringup</exec_depend>
<exec_depend>picknik_accessories</exec_depend>
Expand Down
Loading
Loading