From acb3678d8de95b6f90a0d3152559b862254ddbed Mon Sep 17 00:00:00 2001 From: Maltomatic Date: Sun, 15 Mar 2026 03:44:16 -0700 Subject: [PATCH 1/5] restore cmakelists for simulator/src --- src/simulator/CMakeLists.txt | 45 ++++++++++++++++++++ src/simulator/CMakeLists.txt:Zone.Identifier | 0 2 files changed, 45 insertions(+) create mode 100644 src/simulator/CMakeLists.txt create mode 100644 src/simulator/CMakeLists.txt:Zone.Identifier diff --git a/src/simulator/CMakeLists.txt b/src/simulator/CMakeLists.txt new file mode 100644 index 0000000..4defa8e --- /dev/null +++ b/src/simulator/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(simulator) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(PROGRAMS + scripts/thruster_translate.py + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch worlds models + DESTINATION share/${PROJECT_NAME} + PATTERN "*~" + EXCLUDE) + +ament_package() diff --git a/src/simulator/CMakeLists.txt:Zone.Identifier b/src/simulator/CMakeLists.txt:Zone.Identifier new file mode 100644 index 0000000..e69de29 From d1d530da56feb277e9d31664e641ae6b86b35ed9 Mon Sep 17 00:00:00 2001 From: Maltomatic Date: Tue, 17 Mar 2026 16:15:17 -0700 Subject: [PATCH 2/5] CMakelists patch & upload; build/launch passes --- README.md | 15 ++ src/simulator/CMakeLists.txt | 2 +- .../launch/tardigrade_cmd_vel.launch.py | 155 ++++++++++++++++++ src/simulator/package.xml | 8 + 4 files changed, 179 insertions(+), 1 deletion(-) create mode 100644 src/simulator/launch/tardigrade_cmd_vel.launch.py diff --git a/README.md b/README.md index 340fafb..0126c3f 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,21 @@ Then, to launch the world in Gazebo `ros2 launch simulator robosub.launch` +## One-command cmd_vel bring-up for Tardigrade + +To launch the world, spawn Tardigrade, start thrust allocation, start the +cascaded PID control chain, and start keyboard teleop publishing to +`/tardigrade/cmd_vel`: + +`ros2 launch simulator tardigrade_cmd_vel.launch.py` + +Useful launch options: + +- Use joystick teleop instead of keyboard: + `ros2 launch simulator tardigrade_cmd_vel.launch.py use_keyboard_teleop:=false use_joystick_teleop:=true joy_id:=0` +- Reuse a saved TAM after first run: + `ros2 launch simulator tardigrade_cmd_vel.launch.py reset_tam:=false` + ## Working with thrusters To use the thrusters on the AUV, you must run the thruster_translate node. This node diff --git a/src/simulator/CMakeLists.txt b/src/simulator/CMakeLists.txt index 4defa8e..2b63d60 100644 --- a/src/simulator/CMakeLists.txt +++ b/src/simulator/CMakeLists.txt @@ -37,7 +37,7 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY launch worlds models +install(DIRECTORY launch worlds models robots urdf meshes DESTINATION share/${PROJECT_NAME} PATTERN "*~" EXCLUDE) diff --git a/src/simulator/launch/tardigrade_cmd_vel.launch.py b/src/simulator/launch/tardigrade_cmd_vel.launch.py new file mode 100644 index 0000000..af76aab --- /dev/null +++ b/src/simulator/launch/tardigrade_cmd_vel.launch.py @@ -0,0 +1,155 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration as Lc +from launch_ros.actions import Node, PushRosNamespace + +from ament_index_python.packages import get_package_share_directory + +import os + + +def generate_launch_description(): + namespace = Lc('namespace') + use_sim_time = Lc('use_sim_time') + use_keyboard_teleop = Lc('use_keyboard_teleop') + use_joystick_teleop = Lc('use_joystick_teleop') + + simulator_share = get_package_share_directory('simulator') + thruster_manager_share = get_package_share_directory('uuv_thruster_manager') + cascaded_pid_share = get_package_share_directory('uuv_control_cascaded_pid') + teleop_share = get_package_share_directory('uuv_teleop') + + inertial_file = os.path.join( + cascaded_pid_share, + 'config', + 'tardigrade', + 'inertial.yaml') + + vel_pid_file = os.path.join( + cascaded_pid_share, + 'config', + 'tardigrade', + 'vel_pid_control.yaml') + + world_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource( + os.path.join(simulator_share, 'launch', 'robosub.launch')), + launch_arguments=[ + ('gui', Lc('gui')), + ('paused', Lc('paused')), + ('use_sim_time', use_sim_time), + ('set_timeout', Lc('set_timeout')), + ('timeout', Lc('timeout')), + ]) + + spawn_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(simulator_share, 'launch', 'spawn_tardigrade.launch.py')), + launch_arguments=[ + ('debug', Lc('debug')), + ('namespace', namespace), + ('x', Lc('x')), + ('y', Lc('y')), + ('z', Lc('z')), + ('roll', Lc('roll')), + ('pitch', Lc('pitch')), + ('yaw', Lc('yaw')), + ('use_ned_frame', Lc('use_ned_frame')), + ]) + + thruster_manager_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource( + os.path.join(thruster_manager_share, 'launch', 'thruster_manager.launch')), + launch_arguments=[ + ('model_name', 'tardigrade'), + ('uuv_name', namespace), + ('reset_tam', Lc('reset_tam')), + ]) + + control_group = GroupAction([ + PushRosNamespace(namespace), + Node( + package='uuv_control_cascaded_pid', + executable='acceleration_control.py', + name='acceleration_control', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, inertial_file], + ), + Node( + package='uuv_control_cascaded_pid', + executable='velocity_control.py', + name='velocity_control', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, vel_pid_file], + remappings=[ + ('odom', ['/', namespace, '/pose_gt']), + ('cmd_accel', ['/', namespace, '/cmd_accel']), + ], + ), + ]) + + keyboard_teleop_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource( + os.path.join(teleop_share, 'launch', 'uuv_keyboard_teleop.launch')), + launch_arguments=[ + ('uuv_name', namespace), + ('output_topic', 'cmd_vel'), + ('message_type', 'twist'), + ], + condition=IfCondition(use_keyboard_teleop)) + + joystick_teleop_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource( + os.path.join(teleop_share, 'launch', 'uuv_teleop.launch')), + launch_arguments=[ + ('uuv_name', namespace), + ('joy_id', Lc('joy_id')), + ('output_topic', 'cmd_vel'), + ('message_type', 'twist'), + ('axis_yaw', Lc('axis_yaw')), + ('axis_x', Lc('axis_x')), + ('axis_y', Lc('axis_y')), + ('axis_z', Lc('axis_z')), + ], + condition=IfCondition(use_joystick_teleop)) + + return LaunchDescription([ + DeclareLaunchArgument('namespace', default_value='tardigrade'), + DeclareLaunchArgument('debug', default_value='0'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('use_ned_frame', default_value='false'), + + DeclareLaunchArgument('x', default_value='15'), + DeclareLaunchArgument('y', default_value='-15'), + DeclareLaunchArgument('z', default_value='-3'), + DeclareLaunchArgument('roll', default_value='0.0'), + DeclareLaunchArgument('pitch', default_value='0.0'), + DeclareLaunchArgument('yaw', default_value='0.0'), + + DeclareLaunchArgument('gui', default_value='true'), + DeclareLaunchArgument('paused', default_value='false'), + DeclareLaunchArgument('set_timeout', default_value='false'), + DeclareLaunchArgument('timeout', default_value='0.0'), + + DeclareLaunchArgument('reset_tam', default_value='true'), + + DeclareLaunchArgument('use_keyboard_teleop', default_value='true'), + DeclareLaunchArgument('use_joystick_teleop', default_value='false'), + DeclareLaunchArgument('joy_id', default_value='0'), + DeclareLaunchArgument('axis_yaw', default_value='0'), + DeclareLaunchArgument('axis_x', default_value='4'), + DeclareLaunchArgument('axis_y', default_value='3'), + DeclareLaunchArgument('axis_z', default_value='1'), + + world_launch, + spawn_launch, + thruster_manager_launch, + control_group, + keyboard_teleop_launch, + joystick_teleop_launch, + ]) diff --git a/src/simulator/package.xml b/src/simulator/package.xml index 03214df..cb2cd24 100644 --- a/src/simulator/package.xml +++ b/src/simulator/package.xml @@ -11,6 +11,14 @@ gazebo_ros gazebo_msgs + launch + launch_ros + xacro + ament_index_python + plankton_utils + uuv_thruster_manager + uuv_control_cascaded_pid + uuv_teleop launch_xml From 1d135b178ea7cb1b72eeaac9e0b9d7912f6eb833 Mon Sep 17 00:00:00 2001 From: Maltomatic Date: Sat, 4 Apr 2026 16:10:39 -0700 Subject: [PATCH 3/5] keyboard teleop node working, thrusters thrusting --- src/simulator/CMakeLists.txt | 3 +- src/simulator/config/tardigrade/inertial.yaml | 11 + .../config/tardigrade/thruster_manager.yaml | 14 + .../config/tardigrade/vel_pid_control.yaml | 13 + .../launch/tardigrade_cmd_vel.launch.py | 48 ++++ .../tardigrade_control_interface.launch.py | 91 +++++++ .../launch/tardigrade_singlefile.launch.py | 244 ++++++++++++++++++ src/simulator/package.xml | 5 + src/simulator/scripts/key_ctrl.py | 132 ++++++++++ 9 files changed, 560 insertions(+), 1 deletion(-) create mode 100644 src/simulator/config/tardigrade/inertial.yaml create mode 100644 src/simulator/config/tardigrade/thruster_manager.yaml create mode 100644 src/simulator/config/tardigrade/vel_pid_control.yaml create mode 100644 src/simulator/launch/tardigrade_cmd_vel.launch.py create mode 100644 src/simulator/launch/tardigrade_control_interface.launch.py create mode 100644 src/simulator/launch/tardigrade_singlefile.launch.py create mode 100755 src/simulator/scripts/key_ctrl.py diff --git a/src/simulator/CMakeLists.txt b/src/simulator/CMakeLists.txt index 0ce0a0c..5597c34 100644 --- a/src/simulator/CMakeLists.txt +++ b/src/simulator/CMakeLists.txt @@ -33,11 +33,12 @@ if(BUILD_TESTING) endif() install(PROGRAMS + scripts/key_ctrl.py scripts/thruster_translate.py DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY launch worlds models robots +install(DIRECTORY launch worlds models robots config DESTINATION share/${PROJECT_NAME} PATTERN "*~" EXCLUDE) diff --git a/src/simulator/config/tardigrade/inertial.yaml b/src/simulator/config/tardigrade/inertial.yaml new file mode 100644 index 0000000..9b70840 --- /dev/null +++ b/src/simulator/config/tardigrade/inertial.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + pid: + mass: 1862.87 + inertial: + ixx: 525.39 + ixy: 1.44 + ixz: 33.41 + iyy: 794.20 + iyz: 2.6 + izz: 691.23 diff --git a/src/simulator/config/tardigrade/thruster_manager.yaml b/src/simulator/config/tardigrade/thruster_manager.yaml new file mode 100644 index 0000000..1b0343f --- /dev/null +++ b/src/simulator/config/tardigrade/thruster_manager.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + thruster_manager: + tf_prefix: tardigrade + base_link: base_link + thruster_topic_prefix: thrusters/ + thruster_topic_suffix: /input + thruster_frame_base: thruster_ + max_thrust: 2000.0 + timeout: -1.0 + update_rate: 50.0 + conversion_fcn: proportional + conversion_fcn_params: + gain: 0.00031 diff --git a/src/simulator/config/tardigrade/vel_pid_control.yaml b/src/simulator/config/tardigrade/vel_pid_control.yaml new file mode 100644 index 0000000..0738a3d --- /dev/null +++ b/src/simulator/config/tardigrade/vel_pid_control.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + linear_d: 0.0 + linear_i: 2.0 + linear_p: 10.0 + linear_sat: 20.0 + + angular_d: 1.0 + angular_i: 2.0 + angular_p: 10.0 + angular_sat: 5.0 + + odom_vel_in_world: true diff --git a/src/simulator/launch/tardigrade_cmd_vel.launch.py b/src/simulator/launch/tardigrade_cmd_vel.launch.py new file mode 100644 index 0000000..800b077 --- /dev/null +++ b/src/simulator/launch/tardigrade_cmd_vel.launch.py @@ -0,0 +1,48 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration as Lc + +from ament_index_python.packages import get_package_share_directory + +import os + + +def generate_launch_description(): + singlefile_launch = os.path.join( + get_package_share_directory('simulator'), + 'launch', + 'tardigrade_singlefile.launch.py' + ) + + return LaunchDescription([ + DeclareLaunchArgument('namespace', default_value='tardigrade'), + DeclareLaunchArgument('x', default_value='15'), + DeclareLaunchArgument('y', default_value='-15'), + DeclareLaunchArgument('z', default_value='-3'), + DeclareLaunchArgument('roll', default_value='0.0'), + DeclareLaunchArgument('pitch', default_value='0.0'), + DeclareLaunchArgument('yaw', default_value='0.0'), + DeclareLaunchArgument('gui', default_value='true'), + DeclareLaunchArgument('paused', default_value='false'), + DeclareLaunchArgument('headless', default_value='false'), + DeclareLaunchArgument('verbose', default_value='true'), + IncludeLaunchDescription( + AnyLaunchDescriptionSource(singlefile_launch), + launch_arguments=[ + ('namespace', Lc('namespace')), + ('x', Lc('x')), + ('y', Lc('y')), + ('z', Lc('z')), + ('roll', Lc('roll')), + ('pitch', Lc('pitch')), + ('yaw', Lc('yaw')), + ('gui', Lc('gui')), + ('paused', Lc('paused')), + ('headless', Lc('headless')), + ('verbose', Lc('verbose')), + ('enable_control_interface', 'true'), + ('control_interface', 'cmd_vel'), + ], + ), + ]) diff --git a/src/simulator/launch/tardigrade_control_interface.launch.py b/src/simulator/launch/tardigrade_control_interface.launch.py new file mode 100644 index 0000000..5d8c243 --- /dev/null +++ b/src/simulator/launch/tardigrade_control_interface.launch.py @@ -0,0 +1,91 @@ +# Copyright (c) 2026 +# All rights reserved. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration as Lc +from launch.substitutions import PythonExpression +from launch_ros.actions import Node, PushRosNamespace + +from ament_index_python.packages import get_package_share_directory + +import os + + +def generate_launch_description(): + namespace = Lc('namespace') + control_interface = Lc('control_interface') + + simulator_share = get_package_share_directory('simulator') + + thruster_manager_launch = os.path.join( + get_package_share_directory('uuv_thruster_manager'), + 'launch', + 'thruster_manager.launch' + ) + + inertial_config = os.path.join( + simulator_share, + 'config', + 'tardigrade', + 'inertial.yaml' + ) + + velocity_config = os.path.join( + simulator_share, + 'config', + 'tardigrade', + 'vel_pid_control.yaml' + ) + + thruster_manager_config = os.path.join( + simulator_share, + 'config', + 'tardigrade', + 'thruster_manager.yaml' + ) + + thruster_allocator = IncludeLaunchDescription( + AnyLaunchDescriptionSource(thruster_manager_launch), + launch_arguments=[ + ('uuv_name', namespace), + ('model_name', 'tardigrade'), + ('config_file', thruster_manager_config), + ('output_dir', os.path.join(simulator_share, 'config', 'tardigrade')), + # Always recompute TAM from current spawned model to avoid stale matrix files. + ('reset_tam', 'true'), + ], + ) + + controller_group = GroupAction([ + PushRosNamespace(namespace), + + Node( + package='uuv_control_cascaded_pid', + executable='acceleration_control.py', + name='acceleration_control', + output='screen', + parameters=[inertial_config], + ), + + Node( + condition=IfCondition(PythonExpression(["'", control_interface, "' == 'cmd_vel'"])), + package='uuv_control_cascaded_pid', + executable='velocity_control.py', + name='velocity_control', + output='screen', + remappings=[ + ('odom', [ '/', namespace, '/pose_gt' ]), + ], + parameters=[velocity_config], + ), + ]) + + return LaunchDescription([ + DeclareLaunchArgument('namespace', default_value='tardigrade'), + DeclareLaunchArgument('control_interface', default_value='cmd_vel'), + thruster_allocator, + controller_group, + ]) diff --git a/src/simulator/launch/tardigrade_singlefile.launch.py b/src/simulator/launch/tardigrade_singlefile.launch.py new file mode 100644 index 0000000..761c913 --- /dev/null +++ b/src/simulator/launch/tardigrade_singlefile.launch.py @@ -0,0 +1,244 @@ +# Copyright (c) 2020 The Plankton Authors. +# All rights reserved. +# +# This source code is derived from UUV Simulator +# (https://github.com/uuvsimulator/uuv_simulator) +# Copyright (c) 2016-2019 The UUV Simulator Authors +# licensed under the Apache license, Version 2.0 +# cf. 3rd-party-licenses.txt file in the root directory of this source tree. +# +from launch import LaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch_ros.actions import Node, PushRosNamespace +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import OpaqueFunction +from launch.actions import IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration as Lc + +import launch_testing.actions + +from ament_index_python.packages import get_package_share_directory + +import os +import pathlib +import xacro + + +def to_bool(value: str): + if isinstance(value, bool): + return value + if not isinstance(value, str): + raise ValueError('String to bool, invalid type ' + str(value)) + + valid = {'true':True, '1':True, + 'false':False, '0':False} + + if value.lower() in valid: + return valid[value] + + raise ValueError('String to bool, invalid value: %s' % value) + + +# ============================================================================= +def launch_setup(context, *args, **kwargs): + # Perform substitutions + debug = Lc('debug').perform(context) + namespace = Lc('namespace').perform(context) + use_sim_time = to_bool(Lc('use_sim_time').perform(context)) + gui = to_bool(Lc('gui').perform(context)) + paused = to_bool(Lc('paused').perform(context)) + headless = to_bool(Lc('headless').perform(context)) + verbose = to_bool(Lc('verbose').perform(context)) + x = Lc('x').perform(context) + y = Lc('y').perform(context) + z = Lc('z').perform(context) + roll = Lc('roll').perform(context) + pitch = Lc('pitch').perform(context) + yaw = Lc('yaw').perform(context) + use_world_ned = Lc('use_ned_frame').perform(context) + enable_control_interface = to_bool(Lc('enable_control_interface').perform(context)) + control_interface = Lc('control_interface').perform(context) + + global_sim_time = Node( + name='plankton_global_sim_time', + package='plankton_utils', + executable='plankton_global_sim_time', + output='screen', + parameters=[{'use_sim_time': use_sim_time}] + ) + + gazebo_launch = os.path.join( + get_package_share_directory('gazebo_ros'), + 'launch', + 'gazebo.launch.py' + ) + + world_file = os.path.join( + get_package_share_directory('simulator'), + 'worlds', + 'transdec.world' + ) + + gazebo_args = [ + ('world', world_file), + ('pause', str(paused).lower()), + ('use_sim_time', str(use_sim_time).lower()), + ('gui', str(gui).lower()), + ('headless', str(headless).lower()), + ('debug', 'false'), + ('verbose', str(verbose).lower()), + ('extra_gazebo_args', '-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo'), + ] + + gazebo_world = IncludeLaunchDescription( + AnyLaunchDescriptionSource(gazebo_launch), + launch_arguments=gazebo_args, + ) + + control_launch_file = os.path.join( + get_package_share_directory('simulator'), + 'launch', + 'tardigrade_control_interface.launch.py' + ) + + if not pathlib.Path(control_launch_file).exists(): + exc = 'Launch file ' + control_launch_file + ' does not exist' + raise Exception(exc) + + control_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource(control_launch_file), + launch_arguments=[ + ('namespace', namespace), + ('control_interface', control_interface), + ], + ) + + # Xacro + xacro_file = os.path.join( + get_package_share_directory('simulator'), + 'robots', + 'tardigrade_default.xacro' + ) + + # Build the directories, check for existence + path = os.path.join( + get_package_share_directory('simulator'), + 'robots', + 'generated', + namespace, + ) + + if not pathlib.Path(path).exists(): + try: + # Create directory if required and sub-directory + os.makedirs(path) + except OSError: + print ("Creation of the directory %s failed" % path) + + output = os.path.join( + path, + 'robot_description' + ) + + if not pathlib.Path(xacro_file).exists(): + exc = 'Launch file ' + xacro_file + ' does not exist' + raise Exception(exc) + + mapping = {} + if to_bool(use_world_ned): + mappings={'debug': debug, 'namespace': namespace, 'inertial_reference_frame':'world_ned'} + else: + mappings={'debug': debug, 'namespace': namespace, 'inertial_reference_frame':'world'} + + doc = xacro.process(xacro_file, mappings=mappings) + + with open(output, 'w') as file_out: + file_out.write(doc) + + # URDF spawner + args=('-gazebo_namespace /gazebo ' + '-x %s -y %s -z %s -R %s -P %s -Y %s -entity %s -file %s' + %(x, y, z, roll, pitch, yaw, namespace, output)).split() + + # Urdf spawner. NB: node namespace does not prefix the topic, + # as using a leading / + urdf_spawner = Node( + name = 'urdf_spawner', + package='gazebo_ros', + executable='spawn_entity.py', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + # TODO To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}] + arguments=args + ) + + # A joint state publisher plugin already is started with the model, no need to use the default joint state publisher + + args = (output).split() + robot_state_publisher = Node( + name = 'robot_state_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + # TODO To replace in foxy with parameters=[{'robot_description', Command('ros2 run xacro...')}] + arguments=args, + output = 'screen', + parameters=[{'use_sim_time': use_sim_time}] # Use subst here + ) + + # Message to tf + message_to_tf_launch = os.path.join( + get_package_share_directory('uuv_assistants'), + 'launch', + 'message_to_tf.launch' + ) + + if not pathlib.Path(message_to_tf_launch).exists(): + exc = 'Launch file ' + message_to_tf_launch + ' does not exist' + raise Exception(exc) + + launch_args = [('namespace', namespace), ('world_frame', 'world'), + ('child_frame_id', '/' + namespace + '/base_link')] + message_to_tf_launch = IncludeLaunchDescription( + AnyLaunchDescriptionSource(message_to_tf_launch), launch_arguments=launch_args) + + + group = GroupAction([ + PushRosNamespace(namespace), + urdf_spawner, + robot_state_publisher, + ]) + + + actions = [global_sim_time, gazebo_world, group, message_to_tf_launch] + if enable_control_interface: + actions.append(control_launch) + + return actions + +# ============================================================================= +def generate_launch_description(): + # TODO Try LaunchContext ? + return LaunchDescription([ + DeclareLaunchArgument('debug', default_value='0'), + + DeclareLaunchArgument('x', default_value='15'), + DeclareLaunchArgument('y', default_value='-15'), + DeclareLaunchArgument('z', default_value='-3'), + DeclareLaunchArgument('roll', default_value='0.0'), + DeclareLaunchArgument('pitch', default_value='0.0'), + DeclareLaunchArgument('yaw', default_value='0.0'), + + DeclareLaunchArgument('mode', default_value='default'), + DeclareLaunchArgument('namespace', default_value='tardigrade'), + DeclareLaunchArgument('use_ned_frame', default_value='false'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('gui', default_value='true'), + DeclareLaunchArgument('paused', default_value='false'), + DeclareLaunchArgument('headless', default_value='false'), + DeclareLaunchArgument('verbose', default_value='true'), + DeclareLaunchArgument('enable_control_interface', default_value='true'), + DeclareLaunchArgument('control_interface', default_value='cmd_vel'), + + OpaqueFunction(function = launch_setup) + ]) \ No newline at end of file diff --git a/src/simulator/package.xml b/src/simulator/package.xml index 03214df..35581b0 100644 --- a/src/simulator/package.xml +++ b/src/simulator/package.xml @@ -11,6 +11,11 @@ gazebo_ros gazebo_msgs + geometry_msgs + rclpy + uuv_control_cascaded_pid + uuv_thruster_manager + plankton_utils launch_xml diff --git a/src/simulator/scripts/key_ctrl.py b/src/simulator/scripts/key_ctrl.py new file mode 100755 index 0000000..8777aed --- /dev/null +++ b/src/simulator/scripts/key_ctrl.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 + +import select +import sys +import termios +import tty + +import rclpy +from geometry_msgs.msg import Accel, Twist +from rclpy.node import Node + + +class KeyCtrl(Node): + def __init__(self): + super().__init__('key_ctrl') + + self.declare_parameter('namespace', 'tardigrade') + self.declare_parameter('interface', 'cmd_vel') + self.declare_parameter('publish_rate', 20.0) + self.declare_parameter('linear_step', 0.4) + self.declare_parameter('vertical_step', 0.25) + self.declare_parameter('yaw_step', 0.5) + + namespace = self.get_parameter('namespace').value + interface = self.get_parameter('interface').value + self.linear_step = float(self.get_parameter('linear_step').value) + self.vertical_step = float(self.get_parameter('vertical_step').value) + self.yaw_step = float(self.get_parameter('yaw_step').value) + + if interface not in ('cmd_vel', 'cmd_accel'): + raise ValueError("Parameter 'interface' must be 'cmd_vel' or 'cmd_accel'") + + self.interface = interface + self.topic = f'/{namespace}/{interface}' + + if self.interface == 'cmd_vel': + self.publisher = self.create_publisher(Twist, self.topic, 10) + else: + self.publisher = self.create_publisher(Accel, self.topic, 10) + + # self.settings = termios.tcgetattr(sys.stdin) + self.get_logger().info('Publishing %s commands on %s' % (self.interface, self.topic)) + self.get_logger().info( + 'Controls: W/S forward/back, A/D left/right, Q/E yaw left/right, Z/C dive/rise, SPACE stop, Ctrl-C exit' + ) + + # period = 1.0 / max(float(self.get_parameter('publish_rate').value), 1.0) + # self.timer = self.create_timer(period, self._tick) + + # def _read_key(self): + # tty.setraw(sys.stdin.fileno()) + # rlist, _, _ = select.select([sys.stdin], [], [], 0.0) + # key = sys.stdin.read(1) if rlist else '' + # termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) + # return key + + def _build_message(self, key): + lin_x = 0.0 + lin_y = 0.0 + lin_z = 0.0 + yaw_z = 0.0 + + if key == 'w': + lin_x = self.linear_step + elif key == 's': + lin_x = -self.linear_step + elif key == 'a': + lin_y = self.linear_step + elif key == 'd': + lin_y = -self.linear_step + elif key == 'q': + yaw_z = self.yaw_step + elif key == 'e': + yaw_z = -self.yaw_step + elif key == 'z': + lin_z = -self.vertical_step + elif key == 'c': + lin_z = self.vertical_step + else: + return Twist() if self.interface == 'cmd_vel' else Accel() + + if self.interface == 'cmd_vel': + msg = Twist() + else: + msg = Accel() + + msg.linear.x = lin_x + msg.linear.y = lin_y + msg.linear.z = lin_z + msg.angular.z = yaw_z + + return msg + + # def _tick(self): + # key = self._read_key() + + # if key == '\x03': + # self._publish_zero() + # raise KeyboardInterrupt + + # msg = self._build_message(key) + # self.publisher.publish(msg) + + def _publish_zero(self): + if self.interface == 'cmd_vel': + msg = Twist() + else: + msg = Accel() + self.publisher.publish(msg) + + + +def main(args=None): + rclpy.init(args=args) + + node = KeyCtrl() + try: + while(1): + key = input().strip().lower() + cmd = node._build_message(key) + node.publisher.publish(cmd) + except KeyboardInterrupt: + node.get_logger().info('Stopping key_ctrl') + finally: + node._publish_zero() + # termios.tcsetattr(sys.stdin, termios.TCSADRAIN, node.settings) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 8ef778141a8e1983c9d119ff025994424eb01018 Mon Sep 17 00:00:00 2001 From: Maltomatic Date: Sat, 4 Apr 2026 16:11:56 -0700 Subject: [PATCH 4/5] CRLF --> LF whitespace changes in order to build --- src/Plankton | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Plankton b/src/Plankton index 5e279bc..b4525c6 160000 --- a/src/Plankton +++ b/src/Plankton @@ -1 +1 @@ -Subproject commit 5e279bcab1aec2b7949f5e3111cb421b9e967204 +Subproject commit b4525c6ad802929077e61c84592fa6a3866492fe From 3b4c24e371679fd9ef820c577505ea53744dcb6b Mon Sep 17 00:00:00 2001 From: Maltomatic Date: Sat, 4 Apr 2026 16:24:15 -0700 Subject: [PATCH 5/5] updated readme for keyboard ops --- README.md | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 0126c3f..c8726f6 100644 --- a/README.md +++ b/README.md @@ -25,20 +25,20 @@ Then, to launch the world in Gazebo `ros2 launch simulator robosub.launch` -## One-command cmd_vel bring-up for Tardigrade +To launch world + tardigrade, use -To launch the world, spawn Tardigrade, start thrust allocation, start the -cascaded PID control chain, and start keyboard teleop publishing to -`/tardigrade/cmd_vel`: +`ros2 launch simulator tardigrade_singlefile.launch.py` -`ros2 launch simulator tardigrade_cmd_vel.launch.py` +## Controls +`Accel` and `Twist` options are available for control, through `tardigrade/cmd_accel` and `tardigrade/cmd_vel` respectively. You may use `key_ctrl.py` for keyboard teleoperation to test things out. -Useful launch options: +To launch keyboard commands and tardigrade, launch two terminals: -- Use joystick teleop instead of keyboard: - `ros2 launch simulator tardigrade_cmd_vel.launch.py use_keyboard_teleop:=false use_joystick_teleop:=true joy_id:=0` -- Reuse a saved TAM after first run: - `ros2 launch simulator tardigrade_cmd_vel.launch.py reset_tam:=false` +Terminal 1: Gazebo + Tardigrade model: +`ros2 launch simulator tardigrade_singlefile.launch.py` + +Terminal 2: keyboard teleoperation: +`ros2 run simulator key_ctrl.py` (defaults to cmd_vel) or `ros2 run simulator key_ctrl.py --ros-args -p interface:=cmd_accel` to use Accel instead of Twist messages. ## Working with thrusters