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")
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 []
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
14 changes: 13 additions & 1 deletion 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 @@ -66,6 +66,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 @@ -130,6 +131,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 @@ -227,6 +234,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 @@ -265,8 +273,11 @@ def generate_launch_description():
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "mj_world", "map"],
)

# Static TF between map and odom frame
# 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_transform_publisher",
Expand Down Expand Up @@ -346,6 +357,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 @@ -19,6 +19,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