Controls, Menus, and Teleoperation

The operator-facing launch tasks are:

  • interactive: RViz menus, interactive markers, planning, replay, waypoint execution, grasper commands, and optional manual joystick command input.

  • manual: PS4 joystick direct command teleoperation.

  • direct_thrusters: keyboard PWM testing for individual thruster channels.

Interactive Task

The interactive task starts interactive_controller and exposes an RViz interactive-marker menu.

Launch:

ros2 launch ros2_control_blue_reach_5 robot_system_multi_interface.launch.py task:=interactive

Select the active robot once from Robots. Tool menus such as Path Planner, Cmd Replay, Grasper, Waypoints, and Plan & Execute then operate on that selected robot.

Main menu groups:

  • Plan & Execute: activate the currently selected feedback controller if it is inactive, then plan and execute the selected target or waypoint mission.

  • Robots: select active robot when multiple simulated robots exist.

  • Waypoints: add, delete, clear, or stop vehicle waypoint missions.

  • Path Planner: select the active robot’s planner backend, currently Bitstar, RRTstar, or RRTConnect.

  • Dynamic Obstacles: add a generated spherical obstacle ahead of the selected robot’s active path, clear dynamic obstacles, enable/disable dynamic replanning, and print replanning status. Add Path Obstacle requires an active planned path and enables dynamic replanning if it is not already active. Clear Obstacles removes all dynamic obstacles from the simulator.

  • Cmd Replay: select the active robot’s replay profile, reset/play, and stop replay. See Command Replay and Experiments for profile format and experiment logging.

  • Dynamics Profile: apply a named robot dynamics parameter profile during simulation.

  • Data Recording: start or stop MCAP recording for the configured robot, reference, performance, camera, and dynamic obstacle topics.

  • Grasper: open/close the active robot’s grasper through feedback controllers only.

  • Reset Manager: simulator state reset and release controls. Reset clears the selected robot’s waypoint/path state and the global dynamic obstacle set. Requests for hardware namespaces are rejected with a log message.

  • Robot Control: select controller, control space, and IK settings for the active robot.

Feedback Control and Replay

Feedback control and replay are separate operating modes:

  • PID and InvDyn are closed-loop feedback controllers.

  • CmdReplay is the CSV replay controller. A profile decides whether each subsystem replays commands, tracks references, holds its initial state, or publishes zero commands.

  • Plan & Execute belongs to feedback-controller operation.

  • Replay playback is started from Cmd Replay after selecting CmdReplay and a replay profile.

  • Grasper commands belong to feedback-controller operation.

At startup the default PID controller is selected but inactive. It does not publish closed-loop commands until the user explicitly activates a behavior. Pressing Plan & Execute activates the selected feedback controller on demand.

Controller notes:

  • PID combines feedback terms with hydrostatic restoring compensation on the vehicle side. The manipulator side is joint-space feedback with configured command limits.

  • InvDyn implements inverse-dynamics control in the computed-torque sense: desired state and desired acceleration are mapped through an estimated model to actuation, with feedback terms correcting model and state-estimation errors. The manipulator model comes from Floating-KinDyn-Graph; the vehicle model comes from diff_uv.

Waypoint Menu

The waypoint menu operates on vehicle waypoint missions:

  • Add: append the current target pose as a vehicle waypoint.

  • Delete: remove a selected waypoint.

  • Clear: remove all waypoints and clear visualization.

  • Stop: stop execution while keeping the waypoint list.

Joystick in the Interactive Task

In the interactive task, the robot starts a joystick reader when the matching /dev/input/jsN device exists.

Share switches between:

  • direct joystick command publishing, and

  • the selected feedback controller.

This is manual joystick command input inside the interactive task. It is separate from launching the standalone manual task.

Manual Task

Launch:

ros2 launch ros2_control_blue_reach_5 robot_system_multi_interface.launch.py task:=manual

The manual task starts the PS4 joystick teleoperation node. Joystick input is published directly as commands.

Vehicle controls:

  • Left stick vertical: surge.

  • Left stick horizontal: sway.

  • L2 / R2: heave down/up.

  • L1 / R1: roll.

  • Right stick vertical: pitch.

  • Right stick horizontal: yaw.

Manipulator controls:

  • D-pad left/right: axis_e negative/positive.

  • D-pad up/down: axis_d positive/negative.

  • Triangle / X: axis_c positive/negative.

  • Square / Circle: axis_b positive/negative.

  • Right-stick press / left-stick press: grasper axis_a positive/negative.

Options mode:

  • Options toggles between manipulator joint mode and auxiliary mode.

  • In auxiliary mode, D-pad left/right publishes light commands.

  • In auxiliary mode, D-pad up/down publishes camera mount pitch commands.

Direct Thruster Keyboard Task

Launch:

ros2 launch ros2_control_blue_reach_5 robot_system_multi_interface.launch.py \
    use_vehicle_hardware:=true \
    use_manipulator_hardware:=false \
    sim_robot_count:=0 \
    task:=direct_thrusters

Keyboard channel mapping:

u -> channel 0
i -> channel 1
o -> channel 2
p -> channel 3
h -> channel 4
j -> channel 5
k -> channel 6
l -> channel 7

Neutral PWM is 1500. Pressed keys publish active PWM 1700 for the mapped channel. Releasing a key returns the channel to neutral.