Skip to content

Commit

Permalink
launch2/turbopi_ros.launch.py: Launch delayed v4l2_camera_node
Browse files Browse the repository at this point in the history
- Starts after joint state broadcaster, not required but something to use
  to delay start
  • Loading branch information
wltjr committed May 16, 2024
1 parent a8ab163 commit 03934b4
Showing 1 changed file with 15 additions and 0 deletions.
15 changes: 15 additions & 0 deletions launch2/turbopi_ros.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ def launch_setup(context: LaunchContext):

pkg_path = os.path.join(get_package_share_directory(pkg_name))
sim = eval(context.perform_substitution(LaunchConfiguration('sim')).title())
camera_params_file = os.path.join(pkg_path, 'config', 'camera.yaml')
controller_params = os.path.join(pkg_path, 'config', 'turbopi_controllers.yaml')
xacro_file = os.path.join(pkg_path,'description',filename)

Expand Down Expand Up @@ -68,6 +69,12 @@ def launch_setup(context: LaunchContext):
arguments=["position_controllers", "-c", "/controller_manager"],
)

v4l2_camera_node = Node(
package='v4l2_camera',
executable='v4l2_camera_node',
parameters=[camera_params_file],
)

delayed_joint_broad_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=controller_manager,
Expand All @@ -89,12 +96,20 @@ def launch_setup(context: LaunchContext):
)
)

delayed_v4l2_camera_node = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_broad_spawner,
on_exit=[v4l2_camera_node],
)
)

nodes = [
controller_manager,
node_robot_state_publisher,
delayed_joint_broad_spawner,
delayed_diff_drive_spawner,
delayed_position_spawner,
delayed_v4l2_camera_node,
]

return nodes
Expand Down

0 comments on commit 03934b4

Please sign in to comment.