Skip to content

Commit

Permalink
Refactor sim_time and composable node usage in Collision Monitor (#3604)
Browse files Browse the repository at this point in the history
* Refactor sim_time and composable node usage in Collision Monitor launch script

* Added namespace support

* Fix comment
  • Loading branch information
AlexeyMerzlyakov authored and SteveMacenski committed Jun 9, 2023
1 parent 8edb367 commit eb4a87a
Show file tree
Hide file tree
Showing 2 changed files with 79 additions and 26 deletions.
103 changes: 78 additions & 25 deletions nav2_collision_monitor/launch/collision_monitor_node.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,14 @@
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.substitutions import NotEqualsSubstitution
from launch_ros.actions import LoadComposableNodes, SetParameter
from launch_ros.actions import Node
from launch_ros.actions import PushRosNamespace
from launch_ros.descriptions import ComposableNode
from nav2_common.launch import RewrittenYaml


Expand All @@ -32,12 +37,17 @@ def generate_launch_description():
# Constant parameters
lifecycle_nodes = ['collision_monitor']
autostart = True
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]

# Launch arguments
# 1. Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
use_composition = LaunchConfiguration('use_composition')
container_name = LaunchConfiguration('container_name')
container_name_full = (namespace, '/', container_name)

# 2. Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
Expand All @@ -55,43 +65,86 @@ def generate_launch_description():
default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')

# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time}
declare_use_composition_cmd = DeclareLaunchArgument(
'use_composition', default_value='True',
description='Use composed bringup if True')

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

configured_params = RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
param_rewrites={},
convert_types=True)

# Nodes launching commands
start_lifecycle_manager_cmd = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager',
output='screen',
emulate_tty=True, # https://github.com/ros2/launch/issues/188
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
{'node_names': lifecycle_nodes}])

start_collision_monitor_cmd = Node(
package='nav2_collision_monitor',
executable='collision_monitor',
output='screen',
emulate_tty=True, # https://github.com/ros2/launch/issues/188
parameters=[configured_params])
# Declare node launching commands
load_nodes = GroupAction(
condition=IfCondition(PythonExpression(['not ', use_composition])),
actions=[
SetParameter('use_sim_time', use_sim_time),
PushRosNamespace(
condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
namespace=namespace),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_collision_monitor',
output='screen',
emulate_tty=True, # https://github.com/ros2/launch/issues/188
parameters=[{'autostart': autostart},
{'node_names': lifecycle_nodes}],
remappings=remappings),
Node(
package='nav2_collision_monitor',
executable='collision_monitor',
output='screen',
emulate_tty=True, # https://github.com/ros2/launch/issues/188
parameters=[configured_params],
remappings=remappings)
]
)

load_composable_nodes = GroupAction(
condition=IfCondition(use_composition),
actions=[
SetParameter('use_sim_time', use_sim_time),
PushRosNamespace(
condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
namespace=namespace),
LoadComposableNodes(
target_container=container_name_full,
composable_node_descriptions=[
ComposableNode(
package='nav2_lifecycle_manager',
plugin='nav2_lifecycle_manager::LifecycleManager',
name='lifecycle_manager_collision_monitor',
parameters=[{'autostart': autostart},
{'node_names': lifecycle_nodes}],
remappings=remappings),
ComposableNode(
package='nav2_collision_monitor',
plugin='nav2_collision_monitor::CollisionMonitor',
name='collision_monitor',
parameters=[configured_params],
remappings=remappings)
],
)
]
)

ld = LaunchDescription()

# Launch arguments
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_use_composition_cmd)
ld.add_action(declare_container_name_cmd)

# Node launching commands
ld.add_action(start_lifecycle_manager_cmd)
ld.add_action(start_collision_monitor_cmd)
ld.add_action(load_nodes)
ld.add_action(load_composable_nodes)

return ld
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ collision_monitor:
observation_sources: ["scan"]
scan:
type: "scan"
topic: "/scan"
topic: "scan"
pointcloud:
type: "pointcloud"
topic: "/intel_realsense_r200_depth/points"
Expand Down

0 comments on commit eb4a87a

Please sign in to comment.