ROS2-SLAM-Fundamental/map_merge/launch/from_map_server.launch.py

142 lines
4.6 KiB
Python

# showcases map_merge with static maps served by map_server
# you can run this with test maps provided in m-explore-extra repo
# https://github.com/hrnr/m-explore-extra
# roslaunch multirobot_map_merge from_map_server.launch map1:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2011-08-09-12-22-52.yaml map2:=PATH_TO_m-explore-extra/map_merge/gmapping_maps/2012-01-28-11-12-01.yaml rviz:=True
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node, PushRosNamespace
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
EmitEvent,
GroupAction,
RegisterEventHandler,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
ld = LaunchDescription()
config = os.path.join(
get_package_share_directory("multirobot_map_merge"), "config", "params.yaml"
)
use_sim_time = LaunchConfiguration("use_sim_time")
namespace = LaunchConfiguration("namespace")
known_init_poses = LaunchConfiguration("known_init_poses")
rviz = LaunchConfiguration("rviz")
declare_use_sim_time_argument = DeclareLaunchArgument(
"use_sim_time", default_value="true", description="Use simulation/Gazebo clock"
)
declare_namespace_argument = DeclareLaunchArgument(
"namespace",
default_value="/",
description="Namespace for the explore node",
)
declare_known_init_poses_argument = DeclareLaunchArgument(
"known_init_poses",
default_value="False",
description="Known initial poses of the robots. If so don't forget to declare them in the params.yaml file",
)
declare_rviz_argument = DeclareLaunchArgument(
"rviz",
default_value="False",
description="Run rviz2",
)
num_maps = 2
group_actions = []
for i in range(num_maps):
map_argument_name = f"map{i+1}"
map_yaml_file = LaunchConfiguration(map_argument_name)
declare_map_argument = DeclareLaunchArgument(
map_argument_name,
default_value=f"{map_argument_name}.yaml",
description="Full path to map yaml file to load",
)
map_server = Node(
package="nav2_map_server",
executable="map_server",
name="map_server",
namespace=f"robot{i+1}",
output="screen",
parameters=[
{"yaml_filename": map_yaml_file},
{"use_sim_time": use_sim_time},
],
)
map_server_manager = Node(
package="nav2_lifecycle_manager",
executable="lifecycle_manager",
name="lifecycle_manager_localization",
output="screen",
namespace=f"robot{i+1}",
parameters=[
{"use_sim_time": use_sim_time},
{"autostart": True},
{"node_names": ["map_server"]},
],
)
group_action = GroupAction(
[
PushRosNamespace(namespace=namespace),
map_server_manager,
map_server,
declare_map_argument,
]
)
group_actions.append(group_action)
node = Node(
package="multirobot_map_merge",
name="map_merge",
namespace=namespace,
executable="map_merge",
parameters=[
config,
{"use_sim_time": use_sim_time},
{"known_init_poses": known_init_poses},
],
output="screen",
)
rviz_config_file = os.path.join(
get_package_share_directory("multirobot_map_merge"), "launch", "map_merge.rviz"
)
start_rviz_cmd = Node(
condition=IfCondition(rviz),
package="rviz2",
executable="rviz2",
arguments=["-d", rviz_config_file],
output="screen",
)
exit_event_handler = RegisterEventHandler(
condition=IfCondition(rviz),
event_handler=OnProcessExit(
target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason="rviz exited")),
),
)
ld.add_action(declare_use_sim_time_argument)
ld.add_action(declare_known_init_poses_argument)
ld.add_action(declare_namespace_argument)
ld.add_action(declare_rviz_argument)
for group_action in group_actions:
ld.add_action(group_action)
ld.add_action(node)
ld.add_action(start_rviz_cmd)
ld.add_action(exit_event_handler)
return ld