Enable cmd_vel/cmd_accel control

README.md

@@ -25,6 +25,21 @@ Then, to launch the world in Gazebo
 
 `ros2 launch simulator robosub.launch`
 
+To launch world + tardigrade, use
+
+`ros2 launch simulator tardigrade_singlefile.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.
+
+To launch keyboard commands and tardigrade, launch two terminals:
+
+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
 
 To use the thrusters on the AUV, you must run the thruster_translate node. This node

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)

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

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

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

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'),
+            ],
+        ),
+    ])

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,
+    ])