Skip to content
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.

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

[SW-1106] using custom launch actions from synchros2 #537

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 2 additions & 0 deletions spot_driver/launch/point_cloud_xyz.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -73,4 +74,5 @@ def generate_launch_description() -> LaunchDescription:
),
]
)
update_sigterm_sigkill_timeout(ld)
return ld
2 changes: 2 additions & 0 deletions spot_driver/launch/point_cloud_xyzrgb.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -76,4 +77,5 @@ def generate_launch_description() -> LaunchDescription:
),
]
)
update_sigterm_sigkill_timeout(ld)
return ld
2 changes: 2 additions & 0 deletions spot_driver/launch/rviz.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
PathJoinSubstitution,
)
from launch_ros.substitutions import FindPackageShare
from synchros2.launch.actions import update_sigterm_sigkill_timeout

THIS_PACKAGE = "spot_driver"

Expand Down Expand Up @@ -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)

Expand Down
12 changes: 6 additions & 6 deletions spot_driver/launch/spot_driver.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,15 @@
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

THIS_PACKAGE = "spot_driver"


def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
update_sigterm_sigkill_timeout(ld)
khughes-bdai marked this conversation as resolved.
Show resolved Hide resolved
config_file = LaunchConfiguration("config_file")
launch_rviz = LaunchConfiguration("launch_rviz")
rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context)
Expand Down Expand Up @@ -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",
)
)
Expand All @@ -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.",
)
)
Expand Down
2 changes: 2 additions & 0 deletions spot_driver/launch/spot_image_publishers.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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")
Expand Down
21 changes: 9 additions & 12 deletions spot_driver/spot_driver/launch/spot_launch_helpers.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

import yaml
from launch.actions import DeclareLaunchArgument
from synchros2.launch.actions import DeclareBooleanLaunchArgument

from spot_wrapper.wrapper import SpotWrapper

Expand Down Expand Up @@ -67,37 +68,33 @@ 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`."
),
)
)
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."
),
Expand Down
17 changes: 8 additions & 9 deletions spot_ros2_control/launch/spot_ros2_control.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -276,32 +277,30 @@ 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(
"spot_name",
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
Loading