From 5c17fcd90ebec37a987e319c668347a2bcce9df1 Mon Sep 17 00:00:00 2001 From: Tiffany Cappellari <156013635+tcappellari-bdai@users.noreply.github.com> Date: Thu, 19 Dec 2024 16:05:27 +0000 Subject: [PATCH] [SW-1106] using custom launch actions from synchros2 (#537) Co-authored-by: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> --- spot_driver/launch/point_cloud_xyz.launch.py | 2 ++ .../launch/point_cloud_xyzrgb.launch.py | 2 ++ spot_driver/launch/rviz.launch.py | 2 ++ spot_driver/launch/spot_driver.launch.py | 12 +++++------ .../launch/spot_image_publishers.launch.py | 2 ++ .../spot_driver/launch/spot_launch_helpers.py | 21 ++++++++----------- .../launch/spot_ros2_control.launch.py | 17 +++++++-------- 7 files changed, 31 insertions(+), 27 deletions(-) diff --git a/spot_driver/launch/point_cloud_xyz.launch.py b/spot_driver/launch/point_cloud_xyz.launch.py index 57f2173e9..813ac687b 100644 --- a/spot_driver/launch/point_cloud_xyz.launch.py +++ b/spot_driver/launch/point_cloud_xyz.launch.py @@ -36,6 +36,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from synchros2.launch.actions import update_sigterm_sigkill_timeout def generate_launch_description() -> LaunchDescription: @@ -73,4 +74,5 @@ def generate_launch_description() -> LaunchDescription: ), ] ) + update_sigterm_sigkill_timeout(ld) return ld diff --git a/spot_driver/launch/point_cloud_xyzrgb.launch.py b/spot_driver/launch/point_cloud_xyzrgb.launch.py index 8fbf2f725..32794ecae 100644 --- a/spot_driver/launch/point_cloud_xyzrgb.launch.py +++ b/spot_driver/launch/point_cloud_xyzrgb.launch.py @@ -36,6 +36,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from synchros2.launch.actions import update_sigterm_sigkill_timeout def generate_launch_description() -> LaunchDescription: @@ -76,4 +77,5 @@ def generate_launch_description() -> LaunchDescription: ), ] ) + update_sigterm_sigkill_timeout(ld) return ld diff --git a/spot_driver/launch/rviz.launch.py b/spot_driver/launch/rviz.launch.py index 8d2b92a94..51dda9849 100644 --- a/spot_driver/launch/rviz.launch.py +++ b/spot_driver/launch/rviz.launch.py @@ -13,6 +13,7 @@ PathJoinSubstitution, ) from launch_ros.substitutions import FindPackageShare +from synchros2.launch.actions import update_sigterm_sigkill_timeout THIS_PACKAGE = "spot_driver" @@ -43,6 +44,7 @@ def create_rviz_config(robot_name: str) -> None: def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: + update_sigterm_sigkill_timeout(ld) rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context) spot_name = LaunchConfiguration("spot_name").perform(context) diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index 5580e32da..42250bc3f 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -9,6 +9,7 @@ from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from synchros2.launch.actions import DeclareBooleanLaunchArgument, update_sigterm_sigkill_timeout from spot_driver.launch.spot_launch_helpers import IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, spot_has_arm @@ -16,6 +17,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: + update_sigterm_sigkill_timeout(ld) config_file = LaunchConfiguration("config_file") launch_rviz = LaunchConfiguration("launch_rviz") rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context) @@ -164,10 +166,9 @@ def generate_launch_description() -> LaunchDescription: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_rviz", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description="Choose whether to launch RViz", ) ) @@ -179,10 +180,9 @@ def generate_launch_description() -> LaunchDescription: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_image_publishers", - default_value="True", - choices=["True", "true", "False", "false"], + default_value=True, description="Choose whether to launch the image publishing nodes from Spot.", ) ) diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index 46b822d7f..6c81eb203 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -9,6 +9,7 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from synchros2.launch.actions import update_sigterm_sigkill_timeout from spot_driver.launch.spot_launch_helpers import ( DepthRegisteredMode, @@ -91,6 +92,7 @@ def create_point_cloud_nodelets( def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: + update_sigterm_sigkill_timeout(ld) config_file = LaunchConfiguration("config_file") spot_name = LaunchConfiguration("spot_name").perform(context) depth_registered_mode_config = LaunchConfiguration("depth_registered_mode") diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index 4febb6b5a..a94a4ee05 100644 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -7,6 +7,7 @@ import yaml from launch.actions import DeclareLaunchArgument +from synchros2.launch.actions import DeclareBooleanLaunchArgument from spot_wrapper.wrapper import SpotWrapper @@ -67,10 +68,9 @@ def declare_image_publisher_args() -> List[DeclareLaunchArgument]: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "publish_point_clouds", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description=( "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that" " the depth_register_mode launch argument is set to a value that is not `disable`." @@ -78,26 +78,23 @@ def declare_image_publisher_args() -> List[DeclareLaunchArgument]: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "uncompress_images", - default_value="True", - choices=["True", "true", "False", "false"], + default_value=True, description="Choose whether to publish uncompressed images from Spot.", ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "publish_compressed_images", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description="Choose whether to publish compressed images from Spot.", ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "stitch_front_images", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description=( "Choose whether to publish a stitched image constructed from Spot's front left and right cameras." ), diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index b5352b78b..923ab2767 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/spot_ros2_control/launch/spot_ros2_control.launch.py @@ -16,6 +16,7 @@ ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from synchros2.launch.actions import DeclareBooleanLaunchArgument, update_sigterm_sigkill_timeout from spot_driver.launch.spot_launch_helpers import ( IMAGE_PUBLISHER_ARGS, @@ -276,16 +277,14 @@ def generate_launch_description(): default_value="forward_position_controller", description="Robot controller to start. Must match an entry in controllers_config.", ), - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "mock_arm", - default_value="false", - choices=["True", "true", "False", "false"], + default_value=False, description="If in hardware_interface:=mock mode, whether or not the mocked robot has an arm.", ), - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_rviz", - default_value="true", - choices=["True", "true", "False", "false"], + default_value=True, description="Flag to enable rviz.", ), DeclareLaunchArgument( @@ -293,15 +292,15 @@ def generate_launch_description(): default_value="", description="Name of the Spot that will be used as a namespace.", ), - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_image_publishers", - default_value="true", - choices=["True", "true", "False", "false"], + default_value=True, description="Choose whether to launch the image publishers.", ), ] + declare_image_publisher_args() ) # Add nodes to launch description + update_sigterm_sigkill_timeout(ld) ld.add_action(OpaqueFunction(function=launch_setup, args=[ld])) return ld