diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 13bd4ad..78eedd0 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,7 +1,7 @@ { "image": "incebellipipo/devcontainer:humble", "customizations": { - "terminalPrompt": "🐳 [container] ${containerName} ${folder}", + "terminalPrompt": "[container] ${containerName} ${folder}", "settings": { "terminal.integrated.shell.linux": "/bin/bash" }, diff --git a/.github/workflows/build-and-push-arm64.yml b/.github/workflows/build-and-push-arm64.yml deleted file mode 100644 index 6446d08..0000000 --- a/.github/workflows/build-and-push-arm64.yml +++ /dev/null @@ -1,51 +0,0 @@ -name: Docker Hub for ARM64 - -on: - push: - branches: - - 'master' - schedule: - - cron: "0 0 * * 0" - -jobs: - docker: - - # Disable this job for now, as we are not using arm64 images yet. - if: false - - strategy: - fail-fast: false - matrix: - configurations: - - ros_distro: 'humble' - - ros_distro: 'jazzy' - - runs-on: ubuntu-24.04-arm64 - # runs-on: ubuntu-22.04 - - steps: - - name: Checkout - uses: actions/checkout@v3 - - - name: Set up QEMU - uses: docker/setup-qemu-action@v3 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 - - - name: Login to Docker Hub - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - - name: Build and push - uses: docker/build-push-action@v6 - with: - context: . - file: dockerfile - build-args: | - ROS_DISTRO=${{ matrix.configurations.ros_distro }} - push: true - platforms: linux/arm64 - tags: incebellipipo/cybership:${{ matrix.configurations.ros_distro }} diff --git a/.github/workflows/build-and-push.yml b/.github/workflows/build-and-push.yml index 9b6369a..79f3c3e 100644 --- a/.github/workflows/build-and-push.yml +++ b/.github/workflows/build-and-push.yml @@ -13,17 +13,10 @@ jobs: fail-fast: false matrix: configurations: - - ros_distro: 'humble' - arch: 'amd64' + - ros_distro: 'kilted' - ros_distro: 'jazzy' - arch: 'amd64' - - ros_distro: 'humble' - arch: 'arm64' - - ros_distro: 'jazzy' - arch: 'arm64' - # runs-on: ubuntu-latest - runs-on: ubuntu-22.04 + runs-on: ubuntu-latest steps: - name: Checkout @@ -49,5 +42,5 @@ jobs: build-args: | ROS_DISTRO=${{ matrix.configurations.ros_distro }} push: true - platforms: linux/${{ matrix.configurations.arch }} + platforms: linux/amd64,linux/arm64 tags: incebellipipo/cybership:${{ matrix.configurations.ros_distro }} diff --git a/README.md b/README.md index c76dced..421bd63 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ # cybership_common -This repository is the home of the CyberShip Software Suite – a collection of ROS 2 packages that together provide the tools to simulate, visualize, and control autonomous maritime vessels. The suite covers everything from the digital twin (URDF models and visualization) to real-time sensor integration (IMU, motion capture, etc.) and advanced control (dynamic positioning and thrust allocation). +This repository is the home of the CyberShip Software Suite - a collection of ROS 2 packages that together provide the tools to simulate, visualize, and control autonomous maritime vessels. The suite covers everything from the digital twin (URDF models and visualization) to real-time sensor integration (IMU, motion capture, etc.) and advanced control (dynamic positioning and thrust allocation). -## What’s Included +## What's Included - **cybership_bringup**: Launch files to initialize all the necessary nodes for bringing up a vessel in a ROS 2 environment. This includes hardware drivers, localization, and sensor integration. - **cybership_simulator**: Simple Python scripts for vessel simulation using basic physics. Use these scripts to test vessel behavior in simulation. @@ -84,7 +84,7 @@ For Simulation: Use the cybership_simulator package. This runs simplified physics scripts: ### 3. Visualization -Visualize your vessel using the RViz visualization tools provided in the suite. The cybership_viz package sets up RViz with a preconfigured scene showing your vessel’s URDF model and sensor data overlays. To launch visualization: +Visualize your vessel using the RViz visualization tools provided in the suite. The cybership_viz package sets up RViz with a preconfigured scene showing your vessel's URDF model and sensor data overlays. To launch visualization: Additionally, the cybership_description package can publish the URDF model via: diff --git a/cybership_bringup/launch/all.launch.py b/cybership_bringup/launch/all.launch.py new file mode 100644 index 0000000..f280fec --- /dev/null +++ b/cybership_bringup/launch/all.launch.py @@ -0,0 +1,106 @@ +import launch +import launch.actions +import launch.conditions +import launch.launch_description_sources +import launch.substitutions +import launch_ros.substitutions + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + vessel_model = launch.substitutions.LaunchConfiguration("vessel_model") + vessel_name = vessel_model + + ld.add_action( + launch.actions.DeclareLaunchArgument( + "vessel_model", + description="Vessel model to bring up (drillship|enterprise|voyager)", + ) + ) + + include_drillship = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "drillship.launch.py", + ] + ) + ), + condition=launch.conditions.IfCondition( + launch.substitutions.PythonExpression(["'", vessel_model, "' == 'drillship'"]) + ), + ) + + include_enterprise = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "enterprise.launch.py", + ] + ) + ), + condition=launch.conditions.IfCondition( + launch.substitutions.PythonExpression(["'", vessel_model, "' == 'enterprise'"]) + ), + ) + + include_voyager = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "voyager.launch.py", + ] + ) + ), + condition=launch.conditions.IfCondition( + launch.substitutions.PythonExpression(["'", vessel_model, "' == 'voyager'"]) + ), + ) + + include_localization = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_bringup"), + "launch", + "localization.launch.py", + ] + ) + ), + launch_arguments={ + "vessel_model": vessel_model, + "vessel_name": vessel_name, + }.items(), + ) + + include_dp = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_dp"), + "launch", + "dp.launch.py", + ] + ) + ), + launch_arguments={ + "vessel_model": vessel_model, + "vessel_name": vessel_name, + }.items(), + ) + + ld.add_action(include_drillship) + ld.add_action(include_enterprise) + ld.add_action(include_voyager) + ld.add_action(include_localization) + ld.add_action(include_dp) + + return ld diff --git a/cybership_bringup/launch/drillship.launch.py b/cybership_bringup/launch/drillship.launch.py index 0dc6666..9d5aca5 100644 --- a/cybership_bringup/launch/drillship.launch.py +++ b/cybership_bringup/launch/drillship.launch.py @@ -5,42 +5,10 @@ import launch.launch_description_sources -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - param_file=""): - - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(param_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, param_file] - ) - ) - ) - ) - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) + +from cybership_utilities.launch import include_launch_action_with_config + def generate_launch_description(): @@ -72,13 +40,6 @@ def generate_launch_description(): ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'robot_localization.launch.py', 'robot_localization.yaml' - # ) - # ) - ld.add_action( include_launch_action_with_config( vessel_model, vessel_name, @@ -100,11 +61,4 @@ def generate_launch_description(): ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'urdf_description.launch.py', 'empty.yaml' - # ) - # ) - return ld diff --git a/cybership_bringup/launch/enterprise.launch.py b/cybership_bringup/launch/enterprise.launch.py index de6c384..aad8987 100644 --- a/cybership_bringup/launch/enterprise.launch.py +++ b/cybership_bringup/launch/enterprise.launch.py @@ -5,42 +5,8 @@ import launch.launch_description_sources -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - config_file=""): +from cybership_utilities.launch import include_launch_action_with_config - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(config_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) - ) - ) - ) - - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) def generate_launch_description(): @@ -65,13 +31,6 @@ def generate_launch_description(): ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'robot_localization.launch.py', 'robot_localization.yaml' - # ) - # ) - ld.add_action( include_launch_action_with_config( vessel_model, vessel_name, diff --git a/cybership_bringup/launch/localization.launch.py b/cybership_bringup/launch/localization.launch.py index 8ba0190..6b92208 100644 --- a/cybership_bringup/launch/localization.launch.py +++ b/cybership_bringup/launch/localization.launch.py @@ -3,64 +3,33 @@ import launch.actions import launch.substitutions import launch.launch_description_sources +import launch.launch_description +from cybership_utilities.launch import include_launch_action_with_config -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - config_file=""): - - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(config_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, config_file] - ) - ) - ) - ) - - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) - +from cybership_utilities.launch import anon +from cybership_utilities.launch import COMMON_ARGUMENTS as ARGUMENTS def generate_launch_description(): - vessel_name = 'voyager' - - vessel_model = 'voyager' ld = launch.LaunchDescription() + for argument in ARGUMENTS: + ld.add_action(argument) + ld.add_action( include_launch_action_with_config( - vessel_model, vessel_name, + launch.substitutions.LaunchConfiguration("vessel_model"), + launch.substitutions.LaunchConfiguration("vessel_name"), 'robot_localization.launch.py', 'robot_localization.yaml' ) ) ld.add_action( include_launch_action_with_config( - vessel_model, vessel_name, + launch.substitutions.LaunchConfiguration("vessel_model"), + launch.substitutions.LaunchConfiguration("vessel_name"), 'urdf_description.launch.py' ) ) diff --git a/cybership_bringup/launch/voyager.launch.py b/cybership_bringup/launch/voyager.launch.py index 7eb6b06..5c034dd 100644 --- a/cybership_bringup/launch/voyager.launch.py +++ b/cybership_bringup/launch/voyager.launch.py @@ -4,43 +4,8 @@ import launch.substitutions import launch.launch_description_sources +from cybership_utilities.launch import include_launch_action_with_config -def include_launch_action_with_config( - vessel_model, - vessel_name, - launch_file, - param_file=""): - - bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_bringup') - config_pkg_dir = launch_ros.substitutions.FindPackageShare( - 'cybership_config') - - launch_arguments = [ - ('vessel_name', vessel_name), - ('vessel_model', vessel_model) - ] - - if len(param_file) != 0: - launch_arguments.append( - ( - 'param_file', - launch.substitutions.PathJoinSubstitution( - launch.substitutions.PathJoinSubstitution( - [config_pkg_dir, 'config', vessel_model, param_file] - ) - ) - ) - ) - - return launch.actions.IncludeLaunchDescription( - launch.launch_description_sources.PythonLaunchDescriptionSource( - launch.substitutions.PathJoinSubstitution( - [bringup_pkg_dir, 'launch', 'include', launch_file] - ) - ), - launch_arguments=launch_arguments - ) def generate_launch_description(): @@ -78,12 +43,6 @@ def generate_launch_description(): 'force_multiplexer.launch.py', 'force_multiplexer.yaml' ) ) - # ld.add_action( - # include_launch_action_with_config( - # vessel_model, vessel_name, - # 'robot_localization.launch.py', 'robot_localization.yaml' - # ) - # ) ld.add_action( include_launch_action_with_config( diff --git a/cybership_config/README.md b/cybership_config/README.md index b962ed8..a7f1d62 100644 --- a/cybership_config/README.md +++ b/cybership_config/README.md @@ -12,17 +12,4 @@ This package contains: ## Dependencies The package has minimal dependencies: -- `ament_cmake` (build dependency) - -## Structure - -The package uses a simple structure: -``` -cybership_config/ -├── CMakeLists.txt # Build configuration -├── LICENSE # GPL-3.0 license file -├── package.xml # Package metadata -├── README.md # This file -└── config/ # Configuration files directory - └── [various config files] -``` \ No newline at end of file +- `ament_cmake` (build dependency) \ No newline at end of file diff --git a/cybership_config/config/drillship/azimuth_controller.yaml b/cybership_config/config/drillship/azimuth_controller.yaml index 19c8cd2..e91fa61 100644 --- a/cybership_config/config/drillship/azimuth_controller.yaml +++ b/cybership_config/config/drillship/azimuth_controller.yaml @@ -6,49 +6,49 @@ servos: servo_1: - frame_id: servo_1_link + frame_id: drillship/servo_1_link control_topic: hardware/joint/servo_1/angle device_id: 3 gain: 100.0 delta: 45.0 - offset: -2.6 + offset: 1.6 servo_2: - frame_id: servo_2_link + frame_id: drillship/servo_2_link control_topic: hardware/joint/servo_2/angle device_id: 1 gain: 100.0 delta: 45.0 - offset: -0.18 + offset: 2.2 servo_3: - frame_id: servo_3_link + frame_id: drillship/servo_3_link control_topic: hardware/joint/servo_3/angle device_id: 2 gain: 100.0 delta: 45.0 - offset: -2.6 + offset: -1.0 servo_4: - frame_id: servo_4_link + frame_id: drillship/servo_4_link control_topic: hardware/joint/servo_4/angle device_id: 6 gain: 100.0 delta: 45.0 - offset: -0.18 + offset: 2.4 servo_5: - frame_id: servo_5_link + frame_id: drillship/servo_5_link control_topic: hardware/joint/servo_5/angle device_id: 4 gain: 100.0 delta: 45.0 - offset: -2.6 + offset: -2.0 servo_6: - frame_id: servo_6_link - control_topic: hardware/joint/servo_6_/angle + frame_id: drillship/servo_6_link + control_topic: hardware/joint/servo_6/angle device_id: 5 gain: 100.0 delta: 45.0 - offset: -0.18 \ No newline at end of file + offset: -1.0 \ No newline at end of file diff --git a/cybership_config/config/drillship/controller_position.yaml b/cybership_config/config/drillship/controller_position.yaml new file mode 100644 index 0000000..ffdc751 --- /dev/null +++ b/cybership_config/config/drillship/controller_position.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + # Vessel physical dimensions (meters) + vessel: + length: 2.0 # Length of vessel (m) + beam: 0.5 # Width of vessel (m) + draft: 0.02 # Draft/depth of vessel (m) + + # Controller parameters + control: + # Position control gains + p_gain: + pos: 1.2 # Proportional gain for position + vel: 0.2 # Proportional gain for velocity + yaw: 1.0 # Proportional gain for yaw + + # Integral gains + i_gain: + pos: 0.05 # Integral gain for position + vel: 0.0 # Integral gain for velocity + yaw: 0.1 # Integral gain for yaw + + # Derivative gains + d_gain: + pos: 0.5 # Derivative gain for position + vel: 0.5 # Derivative gain for velocity + yaw: 2.5 # Derivative gain for yaw + + # Integral error limits + max_integral_error: + pos: 1.0 # Maximum position error integration + yaw: 1.4 # Maximum yaw error integration + + # Saturation parameters + saturation: + pos: 0.1 # Position control saturation + yaw: 0.1 # Yaw control saturation + + # Target reaching tolerances + tolerance: + pos: 0.25 # Position reaching tolerance (m) + yaw: 0.1 # Yaw angle reaching tolerance (rad) + + # Reference filter parameters + filter: + omega: [0.15, 0.15, 0.15] # Natural frequency for reference filter + delta: [0.8, 0.8, 0.8] # Damping ratio for reference filter + + # Performance metrics configuration + metrics: + window_size: 200 # Number of samples in moving window + interval: 1.0 # Time between metrics calculations (seconds) + + # General parameters + dt: 0.01 # Control loop time step (seconds) diff --git a/cybership_config/config/drillship/controller_velocity.yaml b/cybership_config/config/drillship/controller_velocity.yaml new file mode 100644 index 0000000..cebf2df --- /dev/null +++ b/cybership_config/config/drillship/controller_velocity.yaml @@ -0,0 +1,51 @@ +# Configuration for velocity_control.py node +# Node: /cybership/velocity_control_node +# This configuration supports the reference feedforward velocity controller + +/**: + ros__parameters: + # Vessel physical dimensions (meters) + # These parameters define the physical characteristics of the vessel + # and are used to calculate the vessel's dynamic model + vessel: + length: 1.0 # Length of vessel (L) in meters + beam: 0.3 # Width of vessel (B) in meters + draft: 0.05 # Draft (depth) of vessel (T) in meters + + # Control parameters for PID velocity controller + # The controller uses reference feedforward with PID feedback + control: + # Proportional gains for each degree of freedom (DOF) + # Higher values provide faster response but may cause oscillations + p_gain: + surge: 5.0 # Forward/backward motion (u) + sway: 5.0 # Left/right motion (v) + yaw: 5.0 # Rotational motion (r) + + # Integral gains for each DOF + # Helps eliminate steady-state errors + i_gain: + surge: 0.0 # Usually kept low or zero for velocity control + sway: 0.0 + yaw: 0.0 + + # Derivative gains for each DOF + # Provides damping and improves stability + d_gain: + surge: 0.3 # Helps reduce overshoot + sway: 0.3 + yaw: 0.3 + + # Control limits and filtering + i_max: 20.0 # Maximum integral contribution (anti-windup) + smooth_limit: true # Enable smooth limiting for control outputs + filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1, lower = more smoothing) + + # Performance metrics configuration + # Used for monitoring and debugging controller performance + metrics: + window_size: 50 # Number of samples in moving window for statistics + interval: 1.0 # Time between metrics calculations (seconds) + + # Timing parameters + dt: 0.1 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_config/config/drillship/imu_bno055.yaml b/cybership_config/config/drillship/imu_bno055.yaml index f30feaf..836789d 100644 --- a/cybership_config/config/drillship/imu_bno055.yaml +++ b/cybership_config/config/drillship/imu_bno055.yaml @@ -35,7 +35,7 @@ i2c_addr: 0x28 data_query_frequency: 100 calib_status_frequency: 0.1 - frame_id: "imu_link" + frame_id: "drillship/imu_link" operation_mode: 0x0C placement_axis_remap: "P2" acc_factor: 100.0 diff --git a/cybership_config/config/drillship/mocap_transformer.yaml b/cybership_config/config/drillship/mocap_transformer.yaml index 228394e..6827ce1 100644 --- a/cybership_config/config/drillship/mocap_transformer.yaml +++ b/cybership_config/config/drillship/mocap_transformer.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - world_frame: world_ned - base_frame: mocap_link + world_frame: world + base_frame: drillship/mocap_link rigid_body_name: "2" topic_name: measurement/pose publish_tf: false \ No newline at end of file diff --git a/cybership_config/config/drillship/robot_localization.yaml b/cybership_config/config/drillship/robot_localization.yaml index 24c819d..2b63e9b 100644 --- a/cybership_config/config/drillship/robot_localization.yaml +++ b/cybership_config/config/drillship/robot_localization.yaml @@ -1,4 +1,3 @@ -### ekf config file ### /**: ros__parameters: # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin @@ -68,7 +67,7 @@ # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: world # Defaults to "odom" if unspecified - base_link_frame: base_link # Defaults to "base_link" if unspecified + base_link_frame: drillship/base_link_ned # Defaults to "base_link" if unspecified world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, diff --git a/cybership_config/config/drillship/servo_driver.yaml b/cybership_config/config/drillship/servo_driver.yaml index 11442e8..dc5a5ee 100644 --- a/cybership_config/config/drillship/servo_driver.yaml +++ b/cybership_config/config/drillship/servo_driver.yaml @@ -6,24 +6,24 @@ esc_1: pwm_topic: hardware/prop/esc_1/pwm scaled_topic: hardware/prop/esc_1/scaled - channel: 0 + channel: 5 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 - min: -1.3 - max: 1.3 + min: -1.0 + max: 1.0 esc_2: pwm_topic: hardware/prop/esc_2/pwm scaled_topic: hardware/prop/esc_2/scaled - channel: 1 + channel: 7 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 @@ -32,11 +32,11 @@ esc_3: pwm_topic: hardware/prop/esc_3/pwm scaled_topic: hardware/prop/esc_3/scaled - channel: 3 + channel: 9 pulse_duration: - center: 1.5 - min: 1.25 - max: 1.75 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 @@ -45,24 +45,24 @@ esc_4: pwm_topic: hardware/prop/esc_4/pwm scaled_topic: hardware/prop/esc_4/scaled - channel: 5 + channel: 11 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 - min: -1.3 - max: 1.3 + min: -1.0 + max: 1.0 esc_5: pwm_topic: hardware/prop/esc_5/pwm scaled_topic: hardware/prop/esc_5/scaled - channel: 4 + channel: 13 pulse_duration: - center: 1.575 - max: 1.675 - min: 1.475 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 @@ -71,11 +71,11 @@ esc_6: pwm_topic: hardware/prop/esc_6/pwm scaled_topic: hardware/prop/esc_6/scaled - channel: 3 + channel: 15 pulse_duration: - center: 1.5 - min: 1.25 - max: 1.75 + center: 1.50001 + max: 1.7 + min: 1.5 scale: center: 0.0 min: -1.0 diff --git a/cybership_config/config/drillship/thruster_control.yaml b/cybership_config/config/drillship/thruster_control.yaml index 302621d..0c5c520 100644 --- a/cybership_config/config/drillship/thruster_control.yaml +++ b/cybership_config/config/drillship/thruster_control.yaml @@ -2,11 +2,11 @@ ros__parameters: thrusters: - azimuth_1: + bow_port: type: azimuth - force_topic: thruster/azimuth_1/command + force_topic: thruster/bow_port/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_1/scaled inverted: false @@ -14,11 +14,11 @@ topic: hardware/joint/servo_1/angle inverted: false - azimuth_2: + bow_center: type: azimuth - force_topic: thruster/azimuth_2/command + force_topic: thruster/bow_center/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_2/scaled inverted: false @@ -26,11 +26,11 @@ topic: hardware/joint/servo_2/angle inverted: false - azimuth_3: + bow_starboard: type: azimuth - force_topic: thruster/azimuth_3/command + force_topic: thruster/bow_starboard/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_3/scaled inverted: false @@ -38,38 +38,38 @@ topic: hardware/joint/servo_3/angle inverted: false - azimuth_4: + stern_port: type: azimuth - force_topic: thruster/azimuth_4/command + force_topic: thruster/stern_port/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_4/scaled inverted: false angle: - topic: hardware/joint/servo_4/angle + topic: hardware/joint/servo_5/angle inverted: false - azimuth_5: + stern_center: type: azimuth - force_topic: thruster/azimuth_5/command + force_topic: thruster/stern_center/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_5/scaled inverted: false angle: - topic: hardware/joint/servo_5/angle + topic: hardware/joint/servo_6/angle inverted: false - azimuth_6: + stern_starboard: type: azimuth - force_topic: thruster/azimuth_6/command + force_topic: thruster/stern_starboard/command force_max: 1.0 - force_min: -1.0 + force_min: 0.0 rpm: topic: hardware/prop/esc_6/scaled inverted: false angle: - topic: hardware/joint/servo_6/angle + topic: hardware/joint/servo_4/angle inverted: false diff --git a/cybership_config/config/enterprise/controller_position.yaml b/cybership_config/config/enterprise/controller_position.yaml new file mode 100644 index 0000000..4d7798e --- /dev/null +++ b/cybership_config/config/enterprise/controller_position.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + # Vessel physical dimensions (meters) + vessel: + length: 1.0 # Length of vessel (m) + beam: 0.3 # Width of vessel (m) + draft: 0.02 # Draft/depth of vessel (m) + + # Controller parameters + control: + # Position control gains + p_gain: + pos: 4.0 # Proportional gain for position + vel: 0.7 # Proportional gain for velocity + yaw: 1.3 # Proportional gain for yaw + + # Integral gains + i_gain: + pos: 0.2 # Integral gain for position + vel: 0.1 # Integral gain for velocity + yaw: 0.2 # Integral gain for yaw + + # Derivative gains + d_gain: + pos: 0.2 # Derivative gain for position + vel: 0.5 # Derivative gain for velocity + yaw: 1.0 # Derivative gain for yaw + + # Integral error limits + max_integral_error: + pos: 1.0 # Maximum position error integration + yaw: 1.4 # Maximum yaw error integration + + # Saturation parameters + saturation: + pos: 0.1 # Position control saturation + yaw: 0.1 # Yaw control saturation + + # Target reaching tolerances + tolerance: + pos: 0.25 # Position reaching tolerance (m) + yaw: 0.1 # Yaw angle reaching tolerance (rad) + + # Reference filter parameters + filter: + omega: [0.15, 0.15, 0.15] # Natural frequency for reference filter + delta: [0.8, 0.8, 0.8] # Damping ratio for reference filter + + # Performance metrics configuration + metrics: + window_size: 200 # Number of samples in moving window + interval: 1.0 # Time between metrics calculations (seconds) + + # General parameters + dt: 0.01 # Control loop time step (seconds) diff --git a/cybership_config/config/enterprise/controller_velocity.yaml b/cybership_config/config/enterprise/controller_velocity.yaml new file mode 100644 index 0000000..cebf2df --- /dev/null +++ b/cybership_config/config/enterprise/controller_velocity.yaml @@ -0,0 +1,51 @@ +# Configuration for velocity_control.py node +# Node: /cybership/velocity_control_node +# This configuration supports the reference feedforward velocity controller + +/**: + ros__parameters: + # Vessel physical dimensions (meters) + # These parameters define the physical characteristics of the vessel + # and are used to calculate the vessel's dynamic model + vessel: + length: 1.0 # Length of vessel (L) in meters + beam: 0.3 # Width of vessel (B) in meters + draft: 0.05 # Draft (depth) of vessel (T) in meters + + # Control parameters for PID velocity controller + # The controller uses reference feedforward with PID feedback + control: + # Proportional gains for each degree of freedom (DOF) + # Higher values provide faster response but may cause oscillations + p_gain: + surge: 5.0 # Forward/backward motion (u) + sway: 5.0 # Left/right motion (v) + yaw: 5.0 # Rotational motion (r) + + # Integral gains for each DOF + # Helps eliminate steady-state errors + i_gain: + surge: 0.0 # Usually kept low or zero for velocity control + sway: 0.0 + yaw: 0.0 + + # Derivative gains for each DOF + # Provides damping and improves stability + d_gain: + surge: 0.3 # Helps reduce overshoot + sway: 0.3 + yaw: 0.3 + + # Control limits and filtering + i_max: 20.0 # Maximum integral contribution (anti-windup) + smooth_limit: true # Enable smooth limiting for control outputs + filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1, lower = more smoothing) + + # Performance metrics configuration + # Used for monitoring and debugging controller performance + metrics: + window_size: 50 # Number of samples in moving window for statistics + interval: 1.0 # Time between metrics calculations (seconds) + + # Timing parameters + dt: 0.1 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_config/config/enterprise/robot_localization.yaml b/cybership_config/config/enterprise/robot_localization.yaml index f8191c4..ba645b6 100644 --- a/cybership_config/config/enterprise/robot_localization.yaml +++ b/cybership_config/config/enterprise/robot_localization.yaml @@ -4,7 +4,7 @@ # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin # computation until it receives at least one message from one of the inputs. It will then run continuously at the # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. - frequency: 30.0 + frequency: 10.0 # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the @@ -15,7 +15,7 @@ # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected # by, for example, an IMU. Defaults to false if unspecified. - two_d_mode: false + two_d_mode: true # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if @@ -33,7 +33,7 @@ # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious # effects on the performance of the node. Defaults to false if unspecified. - debug: false + debug: true # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. # debug_out_file: /path/to/debug/file.txt @@ -67,9 +67,9 @@ # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified - odom_frame: odom # Defaults to "odom" if unspecified - base_link_frame: base_link # Defaults to "base_link" if unspecified - world_frame: odom # Defaults to the value of odom_frame if unspecified + odom_frame: world # Defaults to "odom" if unspecified + base_link_frame: enterprise/base_link_ned # Defaults to "base_link" if unspecified + world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, @@ -140,15 +140,15 @@ # odom1_pose_rejection_threshold: 2.0 # odom1_twist_rejection_threshold: 0.2 - pose0: /enterprise/measurement/pose - pose0_config: [true, true, true, - true, true, true, + pose0: measurement/pose + pose0_config: [ true, true, true, + true, true, true, false, false, false, false, false, false, false, false, false] - pose0_differential: true + pose0_differential: false pose0_relative: false - pose0_queue_size: 5 + pose0_queue_size: 10 pose0_rejection_threshold: 2.0 # Note the difference in parameter name # twist0: example/twist @@ -160,16 +160,16 @@ # twist0_queue_size: 3 # twist0_rejection_threshold: 2.0 - # imu0: example/imu + # imu0: measurement/imu # imu0_config: [false, false, false, - # true, true, true, + # false, false, false, # false, false, false, # true, true, true, # true, true, true] # imu0_differential: false - # imu0_relative: true - # imu0_queue_size: 5 - # imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names + # imu0_relative: false + # imu0_queue_size: 10 + # # imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names # imu0_twist_rejection_threshold: 0.8 # # imu0_linear_acceleration_rejection_threshold: 0.8 # @@ -193,27 +193,27 @@ # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to # false. - stamped_control: false + # stamped_control: false # The last issued control command will be used in prediction for this period. Defaults to 0.2. - control_timeout: 0.2 + # control_timeout: 0.2 # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. - control_config: [true, false, false, false, false, true] + # control_config: [true, true, false, false, false, true] # Places limits on how large the acceleration term will be. Should match your robot's kinematics. - acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] + # acceleration_limits: [1.3, 1.3, 0.0, 0.0, 0.0, 3.4] # Acceleration and deceleration limits are not always the same for robots. - deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + # deceleration_limits: [1.3, 1.3, 0.0, 0.0, 0.0, 4.5] # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these # gains - acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] + # acceleration_gains: [0.8, 0.8, 0.0, 0.0, 0.0, 0.9] # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these # gains - deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + # deceleration_gains: [1.0, 1.0, 0.5, 0.0, 0.0, 1.0] # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each @@ -246,4 +246,4 @@ # question. Users should take care not to use large values for variables that will not be measured directly. The values # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below # if unspecified. In this example, we specify only the diagonal of the matrix. - initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] \ No newline at end of file + # initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9] \ No newline at end of file diff --git a/cybership_config/config/voyager/azimuth_controller.yaml b/cybership_config/config/voyager/azimuth_controller.yaml index ef7ebdc..e5c9a6b 100644 --- a/cybership_config/config/voyager/azimuth_controller.yaml +++ b/cybership_config/config/voyager/azimuth_controller.yaml @@ -6,7 +6,7 @@ servos: # Starboard servo_1: - frame_id: starboard_azimuth_link + frame_id: voyager/starboard_azimuth_link control_topic: hardware/joint/servo_1/angle device_id: 1 gain: 100.0 @@ -14,7 +14,7 @@ offset: -2.6 # Port servo_2: - frame_id: port_azimuth_link + frame_id: voyager/port_azimuth_link control_topic: hardware/joint/servo_2/angle device_id: 2 gain: 100.0 diff --git a/cybership_config/config/voyager/controller_position.yaml b/cybership_config/config/voyager/controller_position.yaml new file mode 100644 index 0000000..a81565d --- /dev/null +++ b/cybership_config/config/voyager/controller_position.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + # Vessel physical dimensions (meters) + vessel: + length: 1.0 # Length of vessel (m) + beam: 0.3 # Width of vessel (m) + draft: 0.02 # Draft/depth of vessel (m) + + # Controller parameters + control: + # Position control gains + p_gain: + pos: 1.6 # Proportional gain for position + vel: 0.7 # Proportional gain for velocity + yaw: 1.1 # Proportional gain for yaw + + # Integral gains + i_gain: + pos: 0.2 # Integral gain for position + vel: 0.1 # Integral gain for velocity + yaw: 0.2 # Integral gain for yaw + + # Derivative gains + d_gain: + pos: 0.5 # Derivative gain for position + vel: 0.5 # Derivative gain for velocity + yaw: 0.5 # Derivative gain for yaw + + # Integral error limits + max_integral_error: + pos: 1.0 # Maximum position error integration + yaw: 0.3 # Maximum yaw error integration + + # Saturation parameters + saturation: + pos: 0.1 # Position control saturation + yaw: 0.1 # Yaw control saturation + + # Target reaching tolerances + tolerance: + pos: 0.25 # Position reaching tolerance (m) + yaw: 0.1 # Yaw angle reaching tolerance (rad) + + # Reference filter parameters + filter: + omega: [0.25, 0.25, 0.60] # Natural frequency for reference filter + delta: [1.00, 1.00, 1.00] # Damping ratio for reference filter + + # Performance metrics configuration + metrics: + window_size: 200 # Number of samples in moving window + interval: 1.0 # Time between metrics calculations (seconds) + + # General parameters + dt: 0.1 # Control loop time step (seconds) diff --git a/cybership_config/config/voyager/controller_velocity.yaml b/cybership_config/config/voyager/controller_velocity.yaml new file mode 100644 index 0000000..29c6d9a --- /dev/null +++ b/cybership_config/config/voyager/controller_velocity.yaml @@ -0,0 +1,51 @@ +# Configuration for velocity_control.py node +# Node: /cybership/velocity_control_node +# This configuration supports the reference feedforward velocity controller + +/**: + ros__parameters: + # Vessel physical dimensions (meters) + # These parameters define the physical characteristics of the vessel + # and are used to calculate the vessel's dynamic model + vessel: + length: 1.0 # Length of vessel (L) in meters + beam: 0.3 # Width of vessel (B) in meters + draft: 0.05 # Draft (depth) of vessel (T) in meters + + # Control parameters for PID velocity controller + # The controller uses reference feedforward with PID feedback + control: + # Proportional gains for each degree of freedom (DOF) + # Higher values provide faster response but may cause oscillations + p_gain: + surge: 0.5 # Forward/backward motion (u) + sway: 0.3 # Left/right motion (v) + yaw: 0.7 # Rotational motion (r) + + # Integral gains for each DOF + # Helps eliminate steady-state errors + i_gain: + surge: 0.0 # Usually kept low or zero for velocity control + sway: 0.0 + yaw: 0.0 + + # Derivative gains for each DOF + # Provides damping and improves stability + d_gain: + surge: 0.3 # Helps reduce overshoot + sway: 0.3 + yaw: 0.3 + + # Control limits and filtering + i_max: 0.0 # Maximum integral contribution (anti-windup) + smooth_limit: true # Enable smooth limiting for control outputs + filter_alpha: 0.1 # Smoothing factor for desired velocity (0-1, lower = more smoothing) + + # Performance metrics configuration + # Used for monitoring and debugging controller performance + metrics: + window_size: 10 # Number of samples in moving window for statistics + interval: 5.0 # Time between metrics calculations (seconds) + + # Timing parameters + dt: 0.05 # Control loop time step (seconds) - affects timer frequency diff --git a/cybership_config/config/voyager/imu_bno055.yaml b/cybership_config/config/voyager/imu_bno055.yaml index f30feaf..fbf2039 100644 --- a/cybership_config/config/voyager/imu_bno055.yaml +++ b/cybership_config/config/voyager/imu_bno055.yaml @@ -35,7 +35,7 @@ i2c_addr: 0x28 data_query_frequency: 100 calib_status_frequency: 0.1 - frame_id: "imu_link" + frame_id: "voyager/imu_link" operation_mode: 0x0C placement_axis_remap: "P2" acc_factor: 100.0 diff --git a/cybership_config/config/voyager/mocap_transformer.yaml b/cybership_config/config/voyager/mocap_transformer.yaml index c35c0df..507b8dc 100644 --- a/cybership_config/config/voyager/mocap_transformer.yaml +++ b/cybership_config/config/voyager/mocap_transformer.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - world_frame: world_ned - base_frame: mocap_link - rigid_body_name: "1" + world_frame: world + base_frame: voyager/mocap_link + rigid_body_name: "0" topic_name: measurement/pose publish_tf: false diff --git a/cybership_config/config/voyager/robot_localization.yaml b/cybership_config/config/voyager/robot_localization.yaml index 24c819d..86d4941 100644 --- a/cybership_config/config/voyager/robot_localization.yaml +++ b/cybership_config/config/voyager/robot_localization.yaml @@ -1,4 +1,3 @@ -### ekf config file ### /**: ros__parameters: # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin @@ -68,7 +67,7 @@ # estimation node from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: world # Defaults to "odom" if unspecified - base_link_frame: base_link # Defaults to "base_link" if unspecified + base_link_frame: voyager/base_link_ned # Defaults to "base_link" if unspecified world_frame: world # Defaults to the value of odom_frame if unspecified # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, diff --git a/cybership_config/config/voyager/thruster_control.yaml b/cybership_config/config/voyager/thruster_control.yaml index 6b7d7f8..608861d 100644 --- a/cybership_config/config/voyager/thruster_control.yaml +++ b/cybership_config/config/voyager/thruster_control.yaml @@ -11,7 +11,7 @@ inverted: false angle: topic: hardware/joint/servo_2/angle - inverted: false + inverted: true azimuth_starboard: type: azimuth @@ -23,7 +23,7 @@ inverted: false angle: topic: hardware/joint/servo_1/angle - inverted: false + inverted: true tunnel: type: fixed @@ -32,4 +32,4 @@ force_topic: thruster/tunnel/command signal: topic: hardware/prop/esc_0/scaled - inverted: false \ No newline at end of file + inverted: true \ No newline at end of file diff --git a/cybership_controller/CMakeLists.txt b/cybership_controller/CMakeLists.txt new file mode 100644 index 0000000..f1111f6 --- /dev/null +++ b/cybership_controller/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.10) +project(cybership_controller) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(cybership_interfaces REQUIRED) + +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME} +) + +# Install LOSGuidance action server node +file(GLOB LOS_SCRIPTS "${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py") +install( + PROGRAMS + ${LOS_SCRIPTS} + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/cybership_controller/LICENSE b/cybership_controller/LICENSE new file mode 100644 index 0000000..f288702 --- /dev/null +++ b/cybership_controller/LICENSE @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/cybership_controller/README.md b/cybership_controller/README.md new file mode 100644 index 0000000..f674a07 --- /dev/null +++ b/cybership_controller/README.md @@ -0,0 +1,2 @@ +# Cybership Controller + diff --git a/cybership_controller/nodes/los_guidance_client.py b/cybership_controller/nodes/los_guidance_client.py new file mode 100755 index 0000000..27fe98a --- /dev/null +++ b/cybership_controller/nodes/los_guidance_client.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Client for LOS Guidance +""" +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.executors import MultiThreadedExecutor + +import numpy as np + +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped +from cybership_interfaces.action import LOSGuidance +from cybership_controller.guidance.ros_action_client import LOSGuidanceClient +import time + +def main(args=None): + rclpy.init(args=args) + client = LOSGuidanceClient() + # Example waypoints for testing + while rclpy.ok(): + example_waypoints = [ + (0.0, 0.0), + (np.random.normal(6.0, 0.5), 0.0), + (np.random.normal(6.0, 0.5),np.random.normal(6.0, 0.5)), + (0.0, np.random.normal(6.0, 0.5)), + ] + client.send_goal(example_waypoints) + time.sleep(5.0) + client.get_logger().info('Requesting goal cancellation...') + # Use MultiThreadedExecutor to handle feedback continuously + executor = MultiThreadedExecutor() + executor.add_node(client) + executor.spin() + executor.shutdown() + client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cybership_controller/nodes/los_guidance_server.py b/cybership_controller/nodes/los_guidance_server.py new file mode 100755 index 0000000..97d3ae0 --- /dev/null +++ b/cybership_controller/nodes/los_guidance_server.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Server for LOS Guidance using cybership_interfaces/LOSGuidance +""" +import math +import rclpy + +from cybership_controller.guidance.ros_action_server import LOSGuidanceROS +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor + + +def main(args=None): + rclpy.init(args=args) + node = LOSGuidanceROS() + executor = MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cybership_controller/package.xml b/cybership_controller/package.xml new file mode 100644 index 0000000..c56a997 --- /dev/null +++ b/cybership_controller/package.xml @@ -0,0 +1,25 @@ + + + + cybership_controller + 0.0.0 + TODO: Package description + cem + GPL-3.0-only + + ament_cmake + ament_cmake_python + + rclpy + geometry_msgs + nav_msgs + visualization_msgs + cybership_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/cybership_controller/requirements.txt b/cybership_controller/requirements.txt new file mode 100644 index 0000000..e92e81b --- /dev/null +++ b/cybership_controller/requirements.txt @@ -0,0 +1 @@ +skadipy @ git+https://github.com/incebellipipo/skadipy.git@master \ No newline at end of file diff --git a/cybership_dp/cybership_dp/velocity_control/__init__.py b/cybership_controller/src/cybership_controller/__init__.py similarity index 100% rename from cybership_dp/cybership_dp/velocity_control/__init__.py rename to cybership_controller/src/cybership_controller/__init__.py diff --git a/cybership_controller/src/cybership_controller/guidance/los.py b/cybership_controller/src/cybership_controller/guidance/los.py new file mode 100644 index 0000000..2bc2a4b --- /dev/null +++ b/cybership_controller/src/cybership_controller/guidance/los.py @@ -0,0 +1,182 @@ +import numpy as np +from scipy.interpolate import splprep, splev +from scipy.optimize import minimize_scalar + + +class LOSGuidance: + def __init__(self, pts, V_d, delta, window_len=None, mu=0.0): + """ + pts: (N,2) waypoints (NED) + V_d: desired speed [m/s] + delta: look-ahead distance [m] (arc-length) + window_len: local search window [m] around s_prev (default: 4*delta) + mu: small gradient gain for along-track stabilization [0..0.5], 0 to disable + """ + pts = np.asarray(pts) + self.tck, self.u_wp = splprep( + [pts[:, 0], pts[:, 1]], s=0, k=min(3, len(pts)-1)) + + # Precompute dense arc-length lookup: u_grid <-> s_grid + self.u_grid = np.linspace(0.0, 1.0, 2000) + xy = np.array(splev(self.u_grid, self.tck)).T + dxy = np.diff(xy, axis=0) + seg = np.hypot(dxy[:, 0], dxy[:, 1]) + s_grid = np.concatenate(([0.0], np.cumsum(seg))) + self.s_grid = s_grid + self.path_len = float(s_grid[-1]) + + self.V_d = float(V_d) + self.delta = float(delta) + self.window_len = 4.0 * \ + self.delta if window_len is None else float(window_len) + self.mu = float(mu) # small gradient to reduce along-track error + + # Persistent arc-length parameter s (initialize lazily on first call) + self.s = None + + # ---- helpers: arc-length <-> u ------------------------------------------------- + def _u_from_s(self, s): + s = np.clip(s, 0.0, self.path_len) + i = np.searchsorted(self.s_grid, s) + if i == 0: + return self.u_grid[0] + if i >= len(self.s_grid): + return self.u_grid[-1] + s0, s1 = self.s_grid[i-1], self.s_grid[i] + u0, u1 = self.u_grid[i-1], self.u_grid[i] + t = 0.0 if s1 == s0 else (s - s0)/(s1 - s0) + return u0 + t*(u1 - u0) + + def _pos_from_s(self, s): + u = self._u_from_s(s) + x, y = splev(u, self.tck) + return np.array([x, y]) + + def _tangent_from_s(self, s): + u = self._u_from_s(s) + dx, dy = splev(u, self.tck, der=1) + t = np.array([dx, dy]) + nrm = np.linalg.norm(t) + return t / (nrm + 1e-12), nrm # unit tangent, |p'(u)| (scaled) + + # ---- local projection around s_prev (bounded window + progress bias) ---------- + def _project_local(self, x, y, s_guess, ds_ff): + """ + Project (x,y) to arc-length s within a local window around s_guess. + Adds a quadratic bias toward s_guess + ds_ff to prefer forward progress. + """ + w = self.window_len + a = max(0.0, s_guess - 0.5*w) + b = min(self.path_len, s_guess + 0.5*w) + if b <= a: # fallback + a, b = 0.0, self.path_len + + target = np.array([x, y]) + s_target = s_guess + ds_ff + + def cost(s): + p = self._pos_from_s(s) + d = p - target + # distance term + progress bias term (lambda weights) + return d@d + 1e-3*(s - s_target)**2 + + res = minimize_scalar(cost, bounds=( + a, b), method='bounded', options={'xatol': 1e-6}) + return float(res.x) + + # ---- public API ---------------------------------------------------------------- + def reset_to_nearest(self, x, y): + """Call once before the loop to initialize s to the global nearest point.""" + target = np.array([x, y]) + + def cost_u(u): + px, py = splev(u, self.tck) + d = np.array([px, py]) - target + return d@d + res = minimize_scalar(cost_u, bounds=(0.0, 1.0), method='bounded') + u0 = float(res.x) + # convert to arc length + i = np.searchsorted(self.u_grid, u0) + if i == 0: + self.s = 0.0 + elif i >= len(self.u_grid): + self.s = self.path_len + else: + u0a, u0b = self.u_grid[i-1], self.u_grid[i] + s0a, s0b = self.s_grid[i-1], self.s_grid[i] + t = (u0 - u0a) / (u0b - u0a + 1e-12) + self.s = s0a + t*(s0b - s0a) + + def guidance(self, x, y, dt=None): + """ + Compute heading + NED velocity command. + If dt is given, advance by V_d*dt along the curve; otherwise use delta lookahead. + Returns (chi_d [rad], vel_cmd [2], extras dict) + """ + if self.s is None: + self.reset_to_nearest(x, y) + + # feedforward advance in arc length + ds_ff = (self.V_d * (dt if dt is not None else 0.0)) + + # optional gradient correction to reduce along-track error (small mu) + # ṡ_c = -mu * t_hat^T * e where e = p(s) - current position + # discretize as ds_grad ~ ṡ_c * dt + ds_grad = 0.0 + if dt is not None and self.mu > 0.0: + p_s = self._pos_from_s(self.s) + t_hat, _ = self._tangent_from_s(self.s) + e = p_s - np.array([x, y]) + ds_grad = - self.mu * float(t_hat.dot(e)) * dt + + # predict prior to projection + s_pred = np.clip(self.s + ds_ff + ds_grad, 0.0, self.path_len) + + # local projection (prevents jumping at crossings) + s0 = self._project_local(x, y, s_pred, ds_ff) + + # choose look-ahead by arc length + s_la = min(s0 + self.delta, self.path_len) + p_la = self._pos_from_s(s_la) + + # heading toward lookahead + dx, dy = p_la[0] - x, p_la[1] - y + chi_d = np.arctan2(dy, dx) + + # velocity command in NED + Vx = self.V_d * np.cos(chi_d) + Vy = self.V_d * np.sin(chi_d) + + # commit parameter for next call + self.s = s0 + + return chi_d, np.array([Vx, Vy]), {"s": self.s, "s_la": s_la, "path_len": self.path_len, "lookahead_point": p_la} + + +if __name__ == "__main__": + # Example waypoints in NED (north-east) + waypoints = np.array([ + [0.0, 0.0], + [10.0, 5.0], + [20.0, 0.0], + [30.0, -5.0] + ]) + + # Initialize LOS guidance + desired_speed = 1.5 # m/s + lookahead = 5.0 # meters + los = LOSGuidance(waypoints, V_d=desired_speed, delta=lookahead) + + # Simulate evaluation at several positions + test_positions = [ + (2.0, 0.5), + (8.0, 3.0), + (15.0, -1.0), + (25.0, -4.0) + ] + + print("LOS Guidance Commands:") + for pos in test_positions: + chi_d, vel_cmd = los.guidance(*pos) + print( + f"Position {pos} -> Heading: {np.degrees(chi_d):.1f}°, Vel_CMD: [{vel_cmd[0]:.2f}, {vel_cmd[1]:.2f}]") diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_client.py b/cybership_controller/src/cybership_controller/guidance/ros_action_client.py new file mode 100755 index 0000000..42bf305 --- /dev/null +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_client.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Client for LOS Guidance +""" +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.executors import MultiThreadedExecutor + +import numpy as np + +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped +from cybership_interfaces.action import LOSGuidance + +class LOSGuidanceClient(Node): + def __init__(self, **kwargs): + super().__init__('los_guidance_client', **kwargs) + self._action_client = ActionClient(self, LOSGuidance, 'los_guidance') + + def send_goal(self, waypoints): + goal_msg = LOSGuidance.Goal() + path_msg = Path() + path_msg.header.frame_id = 'world' + path_msg.header.stamp = self.get_clock().now().to_msg() + for x, y in waypoints: + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.header.stamp = path_msg.header.stamp + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = 0.0 + pose.pose.orientation.w = 1.0 + path_msg.poses.append(pose) + goal_msg.path = path_msg + + self.get_logger().info('Waiting for action server...') + if not self._action_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('Action server not available, shutting down') + rclpy.shutdown() + return + + self.get_logger().info('Sending goal...') + send_goal_future = self._action_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Goal rejected :(') + return + self.get_logger().info('Goal accepted :)') + result_future = goal_handle.get_result_async() + result_future.add_done_callback(self.get_result_callback) + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + heading_deg = feedback.heading * 180.0 / 3.141592653589793 + vel = feedback.vel_cmd + self.get_logger().info( + f'Feedback: Heading={heading_deg:.1f}°, Vel=[{vel.x:.2f}, {vel.y:.2f}, {vel.z:.2f}]') + + def get_result_callback(self, future): + result = future.result().result + success = result.success + self.get_logger().info(f'Result: success={success}') + rclpy.shutdown() + + def cancel_goal(self): + if self._action_client.server_is_ready(): + self.get_logger().info('Cancelling goal...') + cancel_future = self._action_client.cancel_all_goals_async() + cancel_future.add_done_callback(self.cancel_done) + + +def main(args=None): + rclpy.init(args=args) + client = LOSGuidanceClient() + # Example waypoints for testing + example_waypoints = [ + (0.0, 0.0), + (np.random.normal(6.0, 1.0), 0.0), + (np.random.normal(6.0, 1.0),np.random.normal(6.0, 1.0)), + (0.0, np.random.normal(6.0, 1.0)), + ] + client.send_goal(example_waypoints) + # Use MultiThreadedExecutor to handle feedback continuously + executor = MultiThreadedExecutor() + executor.add_node(client) + executor.spin() + executor.shutdown() + client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cybership_controller/src/cybership_controller/guidance/ros_action_server.py b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py new file mode 100755 index 0000000..606d8ff --- /dev/null +++ b/cybership_controller/src/cybership_controller/guidance/ros_action_server.py @@ -0,0 +1,303 @@ +#!/usr/bin/env python3 +""" +ROS2 Action Server for LOS Guidance using cybership_interfaces/LOSGuidance +""" +import math +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer, ActionClient +from rclpy.action import CancelResponse, GoalResponse +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.qos import QoSProfile, DurabilityPolicy + +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Twist, Vector3, Point +from visualization_msgs.msg import Marker +from tf_transformations import euler_from_quaternion + +from cybership_interfaces.action import LOSGuidance + +# Import underlying LOS guidance implementation +from cybership_controller.guidance.los import LOSGuidance as BaseLOSGuidance +import numpy as np +import typing +from scipy.interpolate import splev + + +class LOSGuidanceROS(Node): + def __init__(self): + super().__init__('los_guidance_server', namespace="cybership") + # Parameters + self.declare_parameter('desired_speed', 0.3) + self.declare_parameter('lookahead', 1.4) + # Heading control gain + self.declare_parameter('heading_gain', 1.0) + # Publishers and Subscribers + self._cmd_pub = self.create_publisher( + Twist, 'control/velocity/command', 10) + # Publish markers with transient local durability so RViZ receives past markers + self._marker_pub = self.create_publisher( + Marker, 'visualization_marker', 10) + self._odom_sub = self.create_subscription( + Odometry, 'measurement/odom', self.odom_callback, 10) + # Vehicle pose + self._position = None + self._yaw = None + # Action Server + self._action_server = ActionServer( + self, + LOSGuidance, + 'los_guidance', + execute_callback=self.execute_callback, + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback, + callback_group=ReentrantCallbackGroup()) + + self.goal_uuids: typing.List = [] + + self.get_logger().info('LOS Guidance Action Server started') + + def odom_callback(self, msg: Odometry): + # Update position and heading + self._position = (msg.pose.pose.position.x, msg.pose.pose.position.y) + q = msg.pose.pose.orientation + # Convert quaternion to yaw using tf_transformations + quat = [q.x, q.y, q.z, q.w] + _, _, yaw = euler_from_quaternion(quat) + self._yaw = yaw + + def goal_callback(self, goal_request: LOSGuidance.Goal) -> GoalResponse: + if len(goal_request.path.poses) < 2: + self.get_logger().warn("Path has fewer than 2 poses — rejecting goal.") + return GoalResponse.REJECT + + return GoalResponse.ACCEPT + + def cancel_callback(self, goal_handle) -> CancelResponse: + + return CancelResponse.ACCEPT + + def prepare_waypoints(self, path_msg: Path, extend: bool = True, n_intermediate: int = 2) -> np.ndarray: + waypoints = [(pose.pose.position.x, pose.pose.position.y) + for pose in path_msg.poses] + + if extend: + # Add intermediate points + extended_waypoints = [] + for i in range(len(waypoints) - 1): + p1 = waypoints[i] + p2 = waypoints[i + 1] + extended_waypoints.append(p1) + # Add intermediate points + for j in range(1, n_intermediate + 1): + mid_point = ( + p1[0] + (p2[0] - p1[0]) * j / (n_intermediate + 1), + p1[1] + (p2[1] - p1[1]) * j / (n_intermediate + 1) + ) + extended_waypoints.append(mid_point) + extended_waypoints.append(waypoints[-1]) + waypoints = extended_waypoints + + return np.array(waypoints) + + async def execute_callback(self, goal_handle: LOSGuidance.Goal) -> LOSGuidance.Result: + + if len(self.goal_uuids) > 0: + self.goal_uuids.pop(0) + + self.goal_uuids.append(tuple(goal_handle.goal_id.uuid)) + + path_msg: Path = goal_handle.request.path + waypoints = [] + waypoints.extend([(pose.pose.position.x, pose.pose.position.y) + for pose in path_msg.poses]) + if self._position is not None: + waypoints.append(self._position) + + waypoints = self.prepare_waypoints( + path_msg, extend=True, n_intermediate=2) + + # Initialize guidance + hz = 2 + rate = self.create_rate(hz) + V_d = float(self.get_parameter('desired_speed').value) + delta = float(self.get_parameter('lookahead').value) + # Heading control gain + k_h = float(self.get_parameter('heading_gain').value) + guidance = BaseLOSGuidance(waypoints, V_d=V_d, delta=delta) + last_wp = waypoints[-1] + # Visualize spline path + self.publish_spline_marker(guidance) + + dt = 1.0 / hz + + canceled = False + while rclpy.ok(): + self.get_logger().info(f"{self.goal_uuids}") + if tuple(goal_handle.goal_id.uuid) not in self.goal_uuids: + self.get_logger().info( + f"LOS guidance goal no longer current. {goal_handle.goal_id.uuid}") + canceled = True + break + + if goal_handle.is_cancel_requested: + goal_handle.canceled() + self.get_logger().info( + f"LOS guidance canceled. {goal_handle.goal_id.uuid}") + break + if not goal_handle.is_active: + self.get_logger().info( + f"LOS guidance no longer active. {goal_handle.goal_id.uuid}") + break + if self._position is None or self._yaw is None: + rate.sleep() + continue + x, y = self._position + chi_d, vel_cmd, extras = guidance.guidance(x, y, dt=dt) + twist = Twist() + # Compute forward velocity in body frame using current heading + v_forward = vel_cmd[0] * \ + math.cos(self._yaw) + vel_cmd[1] * math.sin(self._yaw) + # Use computed forward speed (ensuring non-negative speed) + twist.linear.x = max(v_forward, 0.0) + twist.linear.y = 0.0 + twist.linear.z = 0.0 + # Compute and normalize heading error + heading_error = math.atan2( + math.sin(chi_d - self._yaw), math.cos(chi_d - self._yaw)) + twist.angular.z = k_h * heading_error + self._cmd_pub.publish(twist) + # Feedback + feedback = LOSGuidance.Feedback() + feedback.heading = chi_d + feedback.vel_cmd = Vector3(x=vel_cmd[0], y=vel_cmd[1], z=0.0) + goal_handle.publish_feedback(feedback) + self.publish_path_marker(path_msg) + self.publish_arrow_marker(x, y, vel_cmd) + # Look-ahead visualization + # x_la, y_la, _, _ = guidance.lookahead_point(x, y) + self.publish_lookahead_marker( + extras["lookahead_point"][0], extras["lookahead_point"][1]) + # self.publish_lookahead_line(x, y, x_la, y_la) + # Check completion + if math.hypot(x - last_wp[0], y - last_wp[1]) < delta: + break + rate.sleep() + + if not canceled: + goal_handle.succeed() + self.get_logger().info("LOS guidance finished (result empty).") + return LOSGuidance.Result() + + def publish_path_marker(self, path_msg: Path): + marker = Marker() + marker.header = path_msg.header + marker.ns = 'los_path' + marker.id = 0 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + marker.scale.x = 0.05 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + marker.points = [Point(x=pose.pose.position.x, + y=pose.pose.position.y, + z=0.0) + for pose in path_msg.poses] + self._marker_pub.publish(marker) + + def publish_arrow_marker(self, x, y, vel_cmd): + marker = Marker() + marker.header.frame_id = 'world' + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = 'los_arrow' + marker.id = 1 + marker.type = Marker.ARROW + marker.action = Marker.ADD + marker.scale.x = 0.1 + marker.scale.y = 0.1 + marker.scale.z = 0.1 + # Use a copy of vel_cmd for scaling to avoid in-place modification + vel_arrow = vel_cmd * 3 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.5 + marker.color.a = 1.0 + start = Point(x=x, y=y, z=0.0) + end = Point(x=x + vel_arrow[0], y=y + vel_arrow[1], z=0.0) + marker.points = [start, end] + self._marker_pub.publish(marker) + + def publish_spline_marker(self, guidance): + marker = Marker() + marker.header.frame_id = 'world' + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = 'los_spline' + marker.id = 2 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + marker.scale.x = 0.05 + marker.color.r = 0.0 + marker.color.g = 0.0 + marker.color.b = 1.0 + marker.color.a = 1.0 + # Sample spline path points + ts = np.linspace(0, 1, 1000) + spline_pts = np.array(splev(ts, guidance.tck)).T + marker.points = [Point(x=pt[0], y=pt[1], z=0.0) for pt in spline_pts] + self._marker_pub.publish(marker) + + def publish_lookahead_marker(self, x_la, y_la): + m = Marker() + m.header.frame_id = 'world' + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'los_lookahead' + m.id = 3 + m.type = Marker.SPHERE + m.action = Marker.ADD + m.scale.x = 0.25 # diameter in meters + m.scale.y = 0.25 + m.scale.z = 0.25 + m.color.r = 1.0 + m.color.g = 0.85 + m.color.b = 0.0 + m.color.a = 1.0 # golden/yellow + m.pose.position.x = float(x_la) + m.pose.position.y = float(y_la) + m.pose.position.z = 0.0 + self._marker_pub.publish(m) + + def publish_lookahead_line(self, x, y, x_la, y_la): + m = Marker() + m.header.frame_id = 'world' + m.header.stamp = self.get_clock().now().to_msg() + m.ns = 'los_lookahead' + m.id = 4 + m.type = Marker.LINE_STRIP + m.action = Marker.ADD + m.scale.x = 0.03 + m.color.r = 1.0 + m.color.g = 0.85 + m.color.b = 0.0 + m.color.a = 0.9 + start = Point(x=float(x), y=float(y), z=0.0) + end = Point(x=float(x_la), y=float(y_la), z=0.0) + m.points = [start, end] + self._marker_pub.publish(m) + + +def main(args=None): + rclpy.init(args=args) + node = LOSGuidanceROS() + executor = MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cybership_controller/src/cybership_controller/position/reference_filter.py b/cybership_controller/src/cybership_controller/position/reference_filter.py new file mode 100644 index 0000000..12fe3f2 --- /dev/null +++ b/cybership_controller/src/cybership_controller/position/reference_filter.py @@ -0,0 +1,170 @@ +import numpy as np + + +def wrap_to_pi(angle): + r"""Wrap an angle to [-pi, pi]. + + .. math:: + \mathrm{result} = (\,\mathtt{angle} + \pi\,) \bmod (2\pi) - \pi + + Args: + angle (float): Angle in radians. + + Returns: + float: Angle wrapped to the interval [-pi, pi]. + """ + return (angle + np.pi) % (2 * np.pi) - np.pi + + +def Rz(psi): + r"""Compute a 3DOF rotation matrix about the z-axis. + + .. math:: + R_z(\psi)= \begin{pmatrix} + \cos(\psi) & -\sin(\psi) & 0\\[1mm] + \sin(\psi) & \cos(\psi) & 0\\[1mm] + 0 & 0 & 1 + \end{pmatrix} + + Args: + psi (float): Yaw angle in radians. + + Returns: + numpy.ndarray: A 3x3 rotation matrix. + """ + return np.array( + [[np.cos(psi), -np.sin(psi), 0], + [np.sin(psi), np.cos(psi), 0], + [0, 0, 1]] + ) + + +class ThirdOrderReferenceFilter: + """Third-order reference filter for guidance.""" + + def __init__(self, dt, omega=[0.2, 0.2, 0.2], delta=[1.0, 1.0, 1.0], initial_eta=None): + """Initialize the third-order reference filter. + + Args: + dt (float): Time step size. + omega (list of float, optional): Frequency parameters. Defaults to [0.2, 0.2, 0.2]. + delta (list of float, optional): Damping parameters. Defaults to [1.0, 1.0, 1.0]. + initial_eta (list of float, optional): Initial state [x, y, yaw]. Defaults to None. + """ + self._dt = dt + # Keep everything as a float array + self.eta_d = ( + np.zeros(3) if initial_eta is None else np.array( + initial_eta, dtype=float) + ) # [ x, y, yaw ] + self.eta_d_dot = np.zeros(3) + self.eta_d_ddot = np.zeros(3) + self._eta_r = self.eta_d.copy() # reference target (unwrapped) + + # State vector of 9 elements: [ eta, eta_dot, eta_ddot ] + self._x = np.concatenate([self.eta_d, self.eta_d_dot, self.eta_d_ddot]) + + # Gains + self._delta = np.eye(3) + self._w = np.diag(omega) + O3x3 = np.zeros((3, 3)) + self.Ad = np.block( + [ + [O3x3, np.eye(3), O3x3], + [O3x3, O3x3, np.eye(3)], + [ + -self._w**3, + -(2 * self._delta + np.eye(3)) @ self._w**2, + -(2 * self._delta + np.eye(3)) @ self._w, + ], + ] + ) + self.Bd = np.block([[O3x3], [O3x3], [self._w**3]]) + + def get_eta_d(self): + r"""Retrieve the desired pose. + + Returns: + numpy.ndarray: Desired pose as [x, y, yaw]. + """ + return self.eta_d + + def get_eta_d_dot(self): + r"""Retrieve the desired velocity in the inertial frame. + + Returns: + numpy.ndarray: Desired velocity. + """ + return self.eta_d_dot + + def get_eta_d_ddot(self): + r"""Retrieve the desired acceleration in the inertial frame. + + Returns: + numpy.ndarray: Desired acceleration. + """ + return self.eta_d_ddot + + def get_nu_d(self): + r"""Retrieve the desired velocity in the body frame. + + Returns: + numpy.ndarray: Body frame velocity (u, v, r). + """ + psi = self.eta_d[2] + return Rz(psi).T @ self.eta_d_dot + + def set_eta_r(self, eta_r): + r"""Set the reference pose with smooth yaw transition. + + Args: + eta_r (list or numpy.ndarray): Reference pose [x, y, yaw]. + """ + old_yaw = self._eta_r[2] + new_yaw = eta_r[2] + + # Wrap new yaw to [-pi, pi] + new_yaw = wrap_to_pi(new_yaw) + + # Get minimal difference + diff = wrap_to_pi(new_yaw - old_yaw) + continuous_yaw = old_yaw + diff # small step only + + self._eta_r = np.array([eta_r[0], eta_r[1], continuous_yaw]) + + def update(self): + r"""Integrate the filter for one time step. + + The state is updated as follows: + + .. math:: + \dot{x} = A_d x + B_d \eta_r + + .. math:: + x_{\mathrm{new}} = x + dt \cdot \dot{x} + + Updates the internal state based on the reference pose. + """ + x_dot = self.Ad @ self._x + self.Bd @ self._eta_r + self._x += self._dt * x_dot + + # Extract the updated states + self.eta_d = self._x[:3] + self.eta_d_dot = self._x[3:6] + self.eta_d_ddot = self._x[6:] + + +def saturate(x, z): + r"""Apply a smooth saturation to the input. + + .. math:: + \mathrm{saturated\_value} = \frac{x}{\lvert x \rvert + z} + + Args: + x (numpy.ndarray or float): Input value. + z (numpy.ndarray or float): Saturation parameter. + + Returns: + numpy.ndarray or float: Saturated value between -1 and 1. + """ + return x / (np.abs(x) + z) diff --git a/cybership_controller/src/cybership_controller/utils.py b/cybership_controller/src/cybership_controller/utils.py new file mode 100644 index 0000000..24cd8e1 --- /dev/null +++ b/cybership_controller/src/cybership_controller/utils.py @@ -0,0 +1,69 @@ + +import numpy as np + +class ExponentialSmoothing(): + """ + Exponential smoothing class + """ + + def __init__(self, r: float = 0.7) -> None: + """ + Exponential smoothing class constructor + + :param r: A weighting factor in the set [0-1] + """ + self.r = r + self.x = None + self.x_p = None + self.dx_p = None + + def __call__(self, x: np.ndarray) -> np.ndarray: + """ + Exponential smoothing function + + :param x: current value of x + :return: dx + """ + if self.x_p is None or self.dx_p is None: + self.x_p = x + self.dx_p = x + return x + else: + self.dx_p = ExponentialSmoothing.__func(self.r, x, self.x_p, self.dx_p) + self.x_p = x + return self.dx_p + + def reset(self, x: np.ndarray) -> None: + """ + Reset the exponential smoothing + + :param x: current value of x + :return: + """ + self.x_p = x + self.dx_p = x + + @staticmethod + def __func( + r: float, x: np.ndarray, x_p: np.ndarray = None, dx_p: np.ndarray = None + ) -> np.ndarray: + r""" + Exponential smoothing function + + .. math:: + \begin{aligned} + s_{0}&=x_{0}\\ + s_{t}&=\alpha x_{t}+(1-\alpha )s_{t-1},\quad t>0 + \end{aligned} + + + :param r: A weighting factor in the set [0-1] + :param x: current value of x + :param x_p: previous value of x + :param dx_p: last computed filtered x value + :return: + """ + if x_p is None or dx_p is None: + return x + else: + return (1 - r) * dx_p + r * (x - x_p) \ No newline at end of file diff --git a/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py b/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py new file mode 100644 index 0000000..d7cfb5c --- /dev/null +++ b/cybership_controller/src/cybership_controller/velocity/reference_feedforward.py @@ -0,0 +1,140 @@ +from typing import Optional, Dict, Any +import numpy as np +from cybership_controller.utils import ExponentialSmoothing + + +class RffVelocityController(): + """Inverse-dynamics velocity controller with PI action and acceleration limiting.""" + + def __init__(self, config: Optional[Dict[str, Any]] = None): + r"""Initialize the RffVelocityController. + + Args: + config (Optional[Dict[str, Any]]): Configuration parameters dictionary. + Defaults to None. Supported keys: + - "M" (array-like): Inertia matrix, defaults to 3x3 identity matrix + - "D" (array-like): Damping matrix, defaults to 3x3 zero matrix + - "Kp" (array-like): Proportional gain matrix, defaults to 3x3 identity matrix + - "Ki" (array-like): Integral gain matrix, defaults to 3x3 zero matrix + - "Kd" (array-like): Derivative gain matrix, defaults to 3x3 zero matrix + - "dt" (float): Time step size in seconds, defaults to 0.01 + - "a_max" (float): Hard acceleration limit, values <= 0 disable limit, defaults to -1.0 + - "I_max" (float): Integral wind-up cap, values <= 0 disable cap, defaults to -1.0 + - "smooth_limit" (bool): Enable smooth limiting behavior, defaults to False + + M = \\text{inertia matrix},\\quad D = \\text{damping matrix},\\quad K_p, K_i, K_d = \\text{gain matrices},\\\\ + dt = \\text{time step size}. + + + Equations: + .. math:: + M = \text{inertia matrix},\quad D = \text{damping matrix},\quad K_p, K_i, K_d = \text{gain matrices},\\ + dt = \text{time step size}. + """ + + self.config = config or {} + # Matrices and gains + self.M: np.ndarray = np.array( + self.config.get("M", np.eye(3)), dtype=float) + self.D: np.ndarray = np.array( + self.config.get("D", np.zeros((3, 3))), dtype=float) + self.Kp: np.ndarray = np.array( + self.config.get("Kp", np.eye(3)), dtype=float) + self.Ki: np.ndarray = np.array( + self.config.get("Ki", np.zeros((3, 3))), dtype=float) + self.Kd: np.ndarray = np.array( + self.config.get("Kd", np.zeros((3, 3))), dtype=float) + self.dt: float = float( + self.config.get("dt", 0.01)) # seconds + + # Pre-compute inverse inertia + self.M_inv = np.linalg.inv(self.M) + + # Limits and behaviours + # hard acc limit (<=0 => off) + self.a_max: float = float(self.config.get("a_max", -1.0)) + # integral wind-up cap (<=0 => off) + self.I_max: float = float(self.config.get("I_max", -1.0)) + self.smooth: bool = bool(self.config.get("smooth_limit", False)) + + # Internal state + # for desired-velocity derivative + self._prev_vd: Optional[np.ndarray] = np.zeros(3) # previous desired velocity + self._int_e: np.ndarray = np.zeros(3) # integral of error dt + + self._prev_tau = np.zeros(3) # previous tau command + + def update(self, current_velocity: np.ndarray, desired_velocity: np.ndarray, dt: float) -> np.ndarray: + r"""Compute the force/torque command tau. + + The controller computes tau based on the following equations: + + .. math:: + e = current\_velocity - desired\_velocity + + .. math:: + v_{d\_dot} = \text{smoothed derivative of desired\_velocity} + + .. math:: + e_{dot} = \text{smoothed derivative of } e,\quad \text{where } e = current\_velocity - desired\_velocity + + .. math:: + a_{des} = v_{d\_dot} - K_p \cdot e - K_i \cdot \int e\,dt - K_d \cdot e_{dot} + + .. math:: + \tau = M \cdot a_{des} + D \cdot desired\_velocity + + Args: + current_velocity (np.ndarray): Current measured velocity. + desired_velocity (np.ndarray): Desired velocity. + dt (float): Time step in seconds. + + Raises: + ValueError: If dt is not positive. + + Returns: + np.ndarray: Computed force/torque command tau. + """ + + if dt <= 0.0: + return self._prev_tau + + v = current_velocity.reshape(3) + vd = desired_velocity.reshape(3) + + if not hasattr(self, '_vd_dot_smoother'): + r = self.config.get("filter_alpha", 0.7) + self._vd_dot_smoother = ExponentialSmoothing(r=r) + + if not hasattr(self, '_e_dot_smoother'): + r = self.config.get("filter_alpha", 0.7) + self._e_dot_smoother = ExponentialSmoothing(r=r) + + raw_vd_dot = (vd - self._prev_vd) / \ + dt if self._prev_vd is not None else np.zeros(3) + vd_dot = self._vd_dot_smoother(raw_vd_dot) + self._prev_vd = vd.copy() + + e = v - vd + raw_e_dot = (v - self._prev_vd) / \ + dt if self._prev_vd is not None else np.zeros(3) + e_dot = self._e_dot_smoother(raw_e_dot) + + self._int_e = self._int_e + e * dt + self._int_e = np.clip(self._int_e, -self.I_max, self.I_max) + + a_des = vd_dot - self.Kp @ e - self.Ki @ self._int_e - self.Kd @ e_dot + + tau = self.M @ a_des + self.D @ vd + self._prev_tau = tau.copy() + return tau + + def reset(self): + """Reset the controller state.""" + self._prev_vd = np.zeros(3) + self._int_e = np.zeros(3) + self._prev_tau = np.zeros(3) + if hasattr(self, '_vd_dot_smoother'): + self._vd_dot_smoother.reset() + if hasattr(self, '_e_dot_smoother'): + self._e_dot_smoother.reset() \ No newline at end of file diff --git a/cybership_description/README.md b/cybership_description/README.md index 5273366..fc668f1 100644 --- a/cybership_description/README.md +++ b/cybership_description/README.md @@ -19,7 +19,12 @@ This package depends on: - `xacro`: For URDF preprocessing - `urdf`: For URDF parsing -## Usage +## Frames + +> [!NOTE] +> The devices on the vehicle is first defined in the ENU coordinate frame, and then transformed to the NED coordinate frame using `base_link_ned` frame. + +## Usage ### Visualizing the Model diff --git a/cybership_description/launch/description.launch.py b/cybership_description/launch/description.launch.py index 7dd221f..f94a5c4 100755 --- a/cybership_description/launch/description.launch.py +++ b/cybership_description/launch/description.launch.py @@ -31,7 +31,8 @@ def generate_launch_description(): {'robot_description': launch.substitutions.Command([ 'xacro', ' ', xacro_file, ' ', 'gazebo:=ignition', ' ', - 'namespace:=', launch.substitutions.LaunchConfiguration('vessel_name')])} + 'namespace:=', launch.substitutions.LaunchConfiguration('vessel_name')])}, + {'frame_prefix': [launch.substitutions.LaunchConfiguration('vessel_name'), '/']}, ], remappings=[ # ('/tf', 'tf'), diff --git a/cybership_description/meshes/drillship/model.stl b/cybership_description/meshes/drillship/model.stl new file mode 100644 index 0000000..bb59959 Binary files /dev/null and b/cybership_description/meshes/drillship/model.stl differ diff --git a/cybership_description/urdf/model/drillship/base.urdf.xacro b/cybership_description/urdf/model/drillship/base.urdf.xacro index 5588a08..19046ba 100644 --- a/cybership_description/urdf/model/drillship/base.urdf.xacro +++ b/cybership_description/urdf/model/drillship/base.urdf.xacro @@ -2,13 +2,33 @@ + + + + + + + + + + + + + - + + diff --git a/cybership_description/urdf/model/enterprise/base.urdf.xacro b/cybership_description/urdf/model/enterprise/base.urdf.xacro index a284fda..36ab5a6 100644 --- a/cybership_description/urdf/model/enterprise/base.urdf.xacro +++ b/cybership_description/urdf/model/enterprise/base.urdf.xacro @@ -1,6 +1,13 @@ + + - - !-- @@ -21,10 +17,18 @@ + + + + + + + - + + @@ -39,20 +43,20 @@ - + - + - + \ No newline at end of file diff --git a/cybership_dp/CMakeLists.txt b/cybership_dp/CMakeLists.txt index 9d53637..eca0167 100644 --- a/cybership_dp/CMakeLists.txt +++ b/cybership_dp/CMakeLists.txt @@ -3,22 +3,24 @@ project(cybership_dp) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) - install( - DIRECTORY launch config - DESTINATION share/${PROJECT_NAME} + DIRECTORY + launch + DESTINATION + share/${PROJECT_NAME} ) +# Find all python scripts under the "nodes" directory. +file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py) + install( PROGRAMS - nodes/force_controller.py - nodes/velocity_controller.py + ${NODE_SCRIPTS} DESTINATION lib/${PROJECT_NAME} ) + ament_python_install_package(${PROJECT_NAME}) ament_package() \ No newline at end of file diff --git a/cybership_dp/README.md b/cybership_dp/README.md index 4279d9d..cb89256 100644 --- a/cybership_dp/README.md +++ b/cybership_dp/README.md @@ -11,31 +11,6 @@ The `cybership_dp` package implements control algorithms for dynamic positioning Both controllers are implemented as ROS 2 nodes and can be configured for different vessel models. -## Structure - -``` -cybership_dp/ -├── CMakeLists.txt # Build configuration -├── package.xml # Package metadata and dependencies -├── requirements.txt # Python dependencies -├── config/ # Controller configurations -│ ├── force_controller.yaml -│ ├── velocity_controller.yaml -│ └── [vessel_models]/ # Vessel-specific configs -├── cybership_dp/ # Python module -│ ├── __init__.py -│ ├── drillship/ # Drillship-specific implementations -│ ├── enterprise/ # Enterprise-specific implementations -│ └── voyager/ # Voyager-specific implementations -├── launch/ # Launch files -│ ├── dp.launch.py # Combined DP system launch -│ ├── force_controller.launch.py -│ └── velocity_controller.launch.py -└── nodes/ # ROS executable nodes - ├── force_controller.py - └── velocity_controller.py -``` - ## Usage ### Launch Files diff --git a/cybership_dp/config/force_controller.yaml b/cybership_dp/config/force_controller.yaml deleted file mode 100644 index b8ec1e6..0000000 --- a/cybership_dp/config/force_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - frequency: 20.0 diff --git a/cybership_dp/config/velocity_controller.yaml b/cybership_dp/config/velocity_controller.yaml deleted file mode 100644 index a5d0037..0000000 --- a/cybership_dp/config/velocity_controller.yaml +++ /dev/null @@ -1,18 +0,0 @@ -/**: - ros__parameters: - frequency: 10.0 - surge: - p_gain: 5.0 - i_gain: 0.1 - i_limit: 0.1 - d_gain: 1.0 - sway: - p_gain: 5.0 - i_gain: 0.01 - i_limit: 0.1 - d_gain: 2.0 - yaw: - p_gain: 2.0 - i_gain: 0.1 - i_limit: 0.1 - d_gain: 1.0 diff --git a/cybership_dp/cybership_dp/drillship/force_controller.py b/cybership_dp/cybership_dp/drillship/force_controller.py index 1286381..75b436b 100644 --- a/cybership_dp/cybership_dp/drillship/force_controller.py +++ b/cybership_dp/cybership_dp/drillship/force_controller.py @@ -36,14 +36,14 @@ def __init__(self, *args, **kwargs): self.pubs["bow_starboard_azimuth"] = self.create_publisher( geometry_msgs.msg.Wrench, "thruster/bow_starboard/command", 10 ) - self.pubs["aft_port_azimuth"] = self.create_publisher( - geometry_msgs.msg.Wrench, "thruster/aft_port/command", 10 + self.pubs["stern_port_azimuth"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/stern_port/command", 10 ) - self.pubs["aft_center_azimuth"] = self.create_publisher( - geometry_msgs.msg.Wrench, "thruster/aft_center/command", 10 + self.pubs["stern_center_azimuth"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/stern_center/command", 10 ) - self.pubs["aft_starboard_azimuth"] = self.create_publisher( - geometry_msgs.msg.Wrench, "thruster/aft_starboard/command", 10 + self.pubs["stern_starboard_azimuth"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/stern_starboard/command", 10 ) self.declare_parameter("frequency", rclpy.Parameter.Type.DOUBLE) @@ -68,35 +68,35 @@ def timer_callback(self): msg.force.y = float(u0_f[1]) self.pubs["bow_port_azimuth"].publish(msg) # Bow center azimuth thruster - u1_f = self.actuators[1].force - msg = geometry_msgs.msg.Wrench() - msg.force.x = float(u1_f[0]) - msg.force.y = float(u1_f[1]) - self.pubs["bow_center_azimuth"].publish(msg) + # u1_f = self.actuators[1].force + # msg = geometry_msgs.msg.Wrench() + # msg.force.x = float(u1_f[0]) + # msg.force.y = float(u1_f[1]) + # self.pubs["bow_center_azimuth"].publish(msg) # Bow starboard azimuth thruster - u2_f = self.actuators[2].force + u2_f = self.actuators[1].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u2_f[0]) msg.force.y = float(u2_f[1]) self.pubs["bow_starboard_azimuth"].publish(msg) # Aft port azimuth thruster - u3_f = self.actuators[3].force + u3_f = self.actuators[2].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u3_f[0]) msg.force.y = float(u3_f[1]) - self.pubs["aft_port_azimuth"].publish(msg) + self.pubs["stern_port_azimuth"].publish(msg) # Aft center azimuth thruster - u4_f = self.actuators[4].force - msg = geometry_msgs.msg.Wrench() - msg.force.x = float(u4_f[0]) - msg.force.y = float(u4_f[1]) - self.pubs["aft_center_azimuth"].publish(msg) + # u4_f = self.actuators[4].force + # msg = geometry_msgs.msg.Wrench() + # msg.force.x = float(u4_f[0]) + # msg.force.y = float(u4_f[1]) + # self.pubs["stern_center_azimuth"].publish(msg) # Aft starboard azimuth thruster - u5_f = self.actuators[5].force + u5_f = self.actuators[3].force msg = geometry_msgs.msg.Wrench() msg.force.x = float(u5_f[0]) msg.force.y = float(u5_f[1]) - self.pubs["aft_starboard_azimuth"].publish(msg) + self.pubs["stern_starboard_azimuth"].publish(msg) def force_callback(self, msg: geometry_msgs.msg.Wrench): @@ -116,47 +116,65 @@ def force_callback(self, msg: geometry_msgs.msg.Wrench): def _initialize_thrusters(self): bow_port_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([0.9344, -0.11, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, - "reference_angle": 3 * np.pi / 4.0, + "reference_angle": 3* np.pi / 4.0, } ) bow_center_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([1.0678, 0.0, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, - "reference_angle": 4 * np.pi / 4.0, + "reference_angle": -np.pi, } ) bow_starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([0.9344, 0.11, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, - "reference_angle": 5 * np.pi / 4.0, + "reference_angle": -3*np.pi / 4.0, } ) - aft_port_azimuth = skadipy.actuator.Azimuth( + stern_port_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.9911, -0.1644, -0.1]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, "reference_angle": np.pi / 4.0, } ) - aft_center_azimuth = skadipy.actuator.Azimuth( + stern_center_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-1.1644, 0.0, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, "reference_angle": 0.0, } ) - aft_starboard_azimuth = skadipy.actuator.Azimuth( + stern_starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.9911, 0.1644, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), extra_attributes={ "rate_limit": 0.1, "saturation_limit": 0.7, @@ -167,11 +185,11 @@ def _initialize_thrusters(self): # Put all actuators in a list and create the allocator object self.actuators = [ bow_port_azimuth, - bow_center_azimuth, + # bow_center_azimuth, bow_starboard_azimuth, - aft_port_azimuth, - aft_center_azimuth, - aft_starboard_azimuth + stern_port_azimuth, + # stern_center_azimuth, + stern_starboard_azimuth ] def _initialize_allocator(self): @@ -183,8 +201,8 @@ def _initialize_allocator(self): self.allocator = skadipy.allocator.reference_filters.MinimumMagnitudeAndAzimuth( actuators=self.actuators, force_torque_components=dofs, - gamma=0.001, - mu=0.01, + gamma=0.01, + mu=0.1, rho=1, time_step=(1.0 /self.freq ), control_barrier_function=skadipy.safety.ControlBarrierFunctionType.SUMSQUARE, diff --git a/cybership_dp/cybership_dp/enterprise/force_controller.py b/cybership_dp/cybership_dp/enterprise/force_controller.py new file mode 100644 index 0000000..a98e4d3 --- /dev/null +++ b/cybership_dp/cybership_dp/enterprise/force_controller.py @@ -0,0 +1,158 @@ +import rclpy +import rclpy.node + +import geometry_msgs.msg + +import numpy as np +import skadipy +import skadipy.allocator.reference_filters + + +class ForceControllerROS(rclpy.node.Node): + def __init__(self, *args, **kwargs): + super().__init__( + node_name="enterprise_thrust_allocator", + allow_undeclared_parameters=True, + *args, + **kwargs, + ) + + self.actuators: list[skadipy.actuator.ActuatorBase] = [] + self.allocator: skadipy.allocator.AllocatorBase = None + + self.tau_cmd = np.zeros((6, 1), dtype=np.float32) + self.subs = {} + self.subs["force"] = self.create_subscription( + geometry_msgs.msg.Wrench, "control/force/command", self.force_callback, 10 + ) + + self.pubs = {} + self.pubs["port_thruster"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/port/command", 10 + ) + self.pubs["starboard_thruster"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/starboard/command", 10 + ) + self.pubs["tunnel_thruster"] = self.create_publisher( + geometry_msgs.msg.Wrench, "thruster/tunnel/command", 10 + ) + + self.declare_parameter("frequency", rclpy.Parameter.Type.DOUBLE) + self.freq = ( + self.get_parameter_or("frequency", 1.0).get_parameter_value().double_value + ) + + self._initialize_thrusters() + + self._initialize_allocator() + + self.create_timer(1.0 / self.freq, self.timer_callback) + + def timer_callback(self): + + self.allocator.allocate(tau=self.tau_cmd) + + # Tunnel thruster + u0_f = self.actuators[0].force + msg = geometry_msgs.msg.Wrench() + msg.force.x = float(u0_f[0]) + + self.pubs["tunnel_thruster"].publish(msg) + + # Port azimuth thruster + u1_f = self.actuators[1].force + msg = geometry_msgs.msg.Wrench() + msg.force.x = float(u1_f[0]) + msg.force.y = float(u1_f[1]) + + self.pubs["port_thruster"].publish(msg) + + # Starboard azimuth thruster + u2_f = self.actuators[2].force + msg = geometry_msgs.msg.Wrench() + msg.force.x = float(u2_f[0]) + msg.force.y = float(u2_f[1]) + + self.pubs["starboard_thruster"].publish(msg) + + + def force_callback(self, msg: geometry_msgs.msg.Wrench): + + self.tau_cmd = np.array( + [ + msg.force.x, + msg.force.y, + msg.force.z, + msg.torque.x, + msg.torque.y, + msg.torque.z, + ], + dtype=np.float32, + ).reshape((6, 1)) + + def _initialize_thrusters(self): + tunnel = skadipy.actuator.Fixed( + position=skadipy.toolbox.Point([0.3875, 0.0, 0.0]), + orientation=skadipy.toolbox.Quaternion( + axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 + ), + extra_attributes={ + "rate_limit": 0.1, + "saturation_limit": 0.7, + "name": "tunnel", + }, + ) + port_azimuth = skadipy.actuator.Azimuth( + position=skadipy.toolbox.Point([-0.4574, -0.055, 0.0]), + extra_attributes={ + "rate_limit": 0.1, + "saturation_limit": 1.0, + "reference_angle": np.pi / 2.0, + "name": "port_azimuth", + }, + ) + starboard_azimuth = skadipy.actuator.Azimuth( + position=skadipy.toolbox.Point([-0.4547, 0.055, 0.0]), + extra_attributes={ + "rate_limit": 0.1, + "saturation_limit": 1.0, + "reference_angle": -np.pi / 2.0, + "name": "starboard_azimuth", + }, + ) + + # Put all actuators in a list and create the allocator object + self.actuators = [ + tunnel, + port_azimuth, + starboard_azimuth, + ] + + def _initialize_allocator(self): + dofs = [ + skadipy.allocator.ForceTorqueComponent.X, + skadipy.allocator.ForceTorqueComponent.Y, + skadipy.allocator.ForceTorqueComponent.N, + ] + self.allocator = skadipy.allocator.reference_filters.MinimumMagnitudeAndAzimuth( + actuators=self.actuators, + force_torque_components=dofs, + gamma=0.001, + mu=0.01, + rho=1, + time_step=(1.0 /self.freq ), + control_barrier_function=skadipy.safety.ControlBarrierFunctionType.SUMSQUARE, + ) + self.allocator.compute_configuration_matrix() + + +def main(args=None): + rclpy.init(args=args) + force_controller = ForceControllerROS() + rclpy.spin(force_controller) + force_controller.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/cybership_dp/cybership_dp/position_control/.gitkeep b/cybership_dp/cybership_dp/position_control/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/cybership_dp/cybership_dp/velocity_control/backstepping.py b/cybership_dp/cybership_dp/velocity_control/backstepping.py deleted file mode 100644 index 443e1c8..0000000 --- a/cybership_dp/cybership_dp/velocity_control/backstepping.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np - -class BacksteppingController: - def __init__(self, mass, damping, k_gain): - """ - Initialize the backstepping controller for surge, sway, and yaw. - - Parameters: - mass (array-like): [m_surge, m_sway, m_yaw] parameters. - damping (array-like): [d_surge, d_sway, d_yaw] damping coefficients. - k_gain (array-like): [k_surge, k_sway, k_yaw] control gains. - """ - self.mass = np.array(mass) - self.damping = np.array(damping) - self.k_gain = np.array(k_gain) - - def update(self, v, v_des, dv_des): - """ - Compute the control input vector for surge, sway, and yaw. - - Parameters: - v (np.array): Current velocity vector [surge, sway, yaw]. - v_des (np.array): Desired velocity vector [surge, sway, yaw]. - dv_des (np.array): Time derivative of the desired velocity vector. - - Returns: - u (np.array): Control input vector [u_surge, u_sway, u_yaw]. - """ - # Tracking error for each channel - e = v - v_des - - # Backstepping control law for each channel: - # u = damping * v + mass * (dv_des - k_gain * e) - u = self.damping @ v + self.mass @ (dv_des - self.k_gain * e) - return u diff --git a/cybership_dp/cybership_dp/voyager/force_controller.py b/cybership_dp/cybership_dp/voyager/force_controller.py index d3ef67e..971c778 100644 --- a/cybership_dp/cybership_dp/voyager/force_controller.py +++ b/cybership_dp/cybership_dp/voyager/force_controller.py @@ -107,7 +107,7 @@ def _initialize_thrusters(self): extra_attributes={ "rate_limit": 0.1, "saturation_limit": 1.0, - "reference_angle": -np.pi / 2.0, + "reference_angle": np.pi / 2.0, "name": "port_azimuth", }, ) @@ -116,7 +116,7 @@ def _initialize_thrusters(self): extra_attributes={ "rate_limit": 0.1, "saturation_limit": 1.0, - "reference_angle": np.pi / 2.0, + "reference_angle": -np.pi / 2.0, "name": "starboard_azimuth", }, ) @@ -137,8 +137,8 @@ def _initialize_allocator(self): self.allocator = skadipy.allocator.reference_filters.MinimumMagnitudeAndAzimuth( actuators=self.actuators, force_torque_components=dofs, - gamma=0.001, - mu=0.01, + gamma=0.01, + mu=0.1, rho=1, time_step=(1.0 /self.freq ), control_barrier_function=skadipy.safety.ControlBarrierFunctionType.SUMSQUARE, diff --git a/cybership_dp/cybership_dp/voyager/velocity_controller.py b/cybership_dp/cybership_dp/voyager/velocity_controller.py deleted file mode 100644 index 2998eaa..0000000 --- a/cybership_dp/cybership_dp/voyager/velocity_controller.py +++ /dev/null @@ -1,306 +0,0 @@ -import rclpy -import rclpy.node - -import geometry_msgs.msg -import nav_msgs.msg -import rcl_interfaces.msg - -import numpy as np - -from cybership_dp.velocity_control.backstepping import BacksteppingController -import shoeboxpy.model6dof as box - - -class PIDVelocityControllerROS(rclpy.node.Node): - def __init__(self, *args, **kwargs): - super().__init__( - node_name="voyager_velocity_controller", - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=False, - ) - - # Declare and get parameters - self._declare_parameters() - self._update_parameters() - - # Initialize messages - self._vel_cmd_msg = geometry_msgs.msg.Twist() - self._odom_msg = nav_msgs.msg.Odometry() - - # Initialize variables - self._previous_error = np.zeros(3, dtype=np.float32) - self._k_pid = np.zeros((3, 3), dtype=np.float32) - - # Initialize subscribers and publishers - self._subs = {} - self._pubs = {} - self._subs["odometry"] = self.create_subscription( - nav_msgs.msg.Odometry, "measurement/odom", self._odom_callback, 10 - ) - self._subs["command"] = self.create_subscription( - geometry_msgs.msg.Twist, - "control/velocity/command", - self._vel_cmd_callback, - 10, - ) - self._pubs["force"] = self.create_publisher( - geometry_msgs.msg.Wrench, "control/force/command", 10 - ) - - # Initialize timer - self._dt = 1.0 / self._freq - self.create_timer(self._dt, self.timer_callback) - - def timer_callback(self): - - self._update_parameters() - - cmd_force = geometry_msgs.msg.Wrench() - - x = np.array( - [ - self._odom_msg.twist.twist.linear.x, - self._odom_msg.twist.twist.linear.y, - self._odom_msg.twist.twist.angular.z, - ], - dtype=np.float32, - ) - - u = np.array( - [ - self._vel_cmd_msg.linear.x, - self._vel_cmd_msg.linear.y, - self._vel_cmd_msg.angular.z, - ], - dtype=np.float32, - ) - - # Calculate error - e = u - x - - # Proportional term - self._k_pid[0] = self._K_P @ e - - # Integral term - self._k_pid[1] += self._K_I @ (e * self._dt) - - # Derivative term - self._k_pid[2] = self._K_D @ (e - self._previous_error) / self._dt - - # Calculate control force - cmd = np.array( - [ - self._k_pid[0][0] + self._k_pid[1][0] + self._k_pid[2][0], - self._k_pid[0][1] + self._k_pid[1][1] + self._k_pid[2][1], - self._k_pid[0][2] + self._k_pid[1][2] + self._k_pid[2][2], - ], - dtype=float, - ) - - cmd_force.force.x = cmd[0] - cmd_force.force.y = cmd[1] - cmd_force.torque.z = cmd[2] - - self._pubs["force"].publish(cmd_force) - self._previous_error = e - - def _vel_cmd_callback(self, msg: geometry_msgs.msg.Twist): - self._vel_cmd_msg = msg - - def _odom_callback(self, msg: geometry_msgs.msg.Wrench): - self._odom_msg = msg - - def _declare_parameters(self): - - self.declare_parameter("frequency", rclpy.Parameter.Type.DOUBLE) - - dofs = ["surge", "sway", "yaw"] - gains = ["p_gain", "i_gain", "i_limit", "d_gain"] - for dof in dofs: - for gain in gains: - self.declare_parameter( - f"{dof}.{gain}", - 0.0, - rcl_interfaces.msg.ParameterDescriptor( - description=f"{dof} {gain}", - type=rcl_interfaces.msg.ParameterType.PARAMETER_DOUBLE, - read_only=False, - ), - ) - - def _update_parameters(self): - self._freq = self.get_parameter("frequency").get_parameter_value().double_value - - self._surge_p_gain = ( - self.get_parameter("surge.p_gain").get_parameter_value().double_value - ) - self._surge_i_gain = ( - self.get_parameter("surge.i_gain").get_parameter_value().double_value - ) - self._surge_d_gain = ( - self.get_parameter("surge.d_gain").get_parameter_value().double_value - ) - self._surge_i_limit = ( - self.get_parameter("surge.i_limit").get_parameter_value().double_value - ) - - self._sway_p_gain = ( - self.get_parameter("sway.p_gain").get_parameter_value().double_value - ) - self._sway_i_gain = ( - self.get_parameter("sway.i_gain").get_parameter_value().double_value - ) - self._sway_d_gain = ( - self.get_parameter("sway.d_gain").get_parameter_value().double_value - ) - self._sway_i_limit = ( - self.get_parameter("sway.i_limit").get_parameter_value().double_value - ) - - self._yaw_p_gain = ( - self.get_parameter("yaw.p_gain").get_parameter_value().double_value - ) - self._yaw_i_gain = ( - self.get_parameter("yaw.i_gain").get_parameter_value().double_value - ) - self._yaw_d_gain = ( - self.get_parameter("yaw.d_gain").get_parameter_value().double_value - ) - self._yaw_i_limit = ( - self.get_parameter("yaw.i_limit").get_parameter_value().double_value - ) - - self._K_P = np.array( - [ - [self._surge_p_gain, 0, 0], - [0, self._sway_p_gain, 0], - [0, 0, self._yaw_p_gain], - ], - dtype=np.float32, - ) - - self._K_I = np.array( - [ - [self._surge_i_gain, 0, 0], - [0, self._sway_i_gain, 0], - [0, 0, self._yaw_i_gain], - ], - dtype=np.float32, - ) - - self._K_D = np.array( - [ - [self._surge_d_gain, 0, 0], - [0, self._sway_d_gain, 0], - [0, 0, self._yaw_d_gain], - ], - dtype=np.float32, - ) - - -class BacksteppingVelocityControllerROS(rclpy.node.Node): - - def __init__(self, *args, **kwargs): - super().__init__( - node_name="voyager_velocity_controller", - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=False, - ) - # Define the reference model matrices (example values) - self.vessel = box.Shoebox(L=1.0, B=0.3, T=0.05) - - # Create an instance of the controller - self.controller = BacksteppingController( - mass=self.vessel.M_eff, - damping=self.vessel.D, - k_gain=np.array([10.0, 15.0, 0.0, 0.0, 0.0, 2.0]), - ) - - self._freq = 10.0 - - # Initialize messages - self._vel_cmd_msg = geometry_msgs.msg.Twist() - self._odom_msg = nav_msgs.msg.Odometry() - - # Initialize subscribers and publishers - self._subs = {} - self._pubs = {} - self._subs["odometry"] = self.create_subscription( - nav_msgs.msg.Odometry, "measurement/odom", self._odom_callback, 10 - ) - self._subs["command"] = self.create_subscription( - geometry_msgs.msg.Twist, - "control/velocity/command", - self._vel_cmd_callback, - 10, - ) - self._pubs["force"] = self.create_publisher( - geometry_msgs.msg.Wrench, "control/force/command", 10 - ) - - # Initialize timer - self._dt = 1.0 / self._freq - self.create_timer(self._dt, self.timer_callback) - - # self.v_prev = np.zeros(6, dtype=np.float32) - # self.v_des_prev = np.zeros(6, dtype=np.float32) - # self.dv_des = np.zeros(6, dtype=np.float32) - - def timer_callback(self): - - v=np.array( - [ - self._odom_msg.twist.twist.linear.x, - self._odom_msg.twist.twist.linear.y, - 0.0, - 0.0, - 0.0, - self._odom_msg.twist.twist.angular.z, - ], - dtype=np.float32, - ) - v_des=np.array( - [ - self._vel_cmd_msg.linear.x, - self._vel_cmd_msg.linear.y, - 0.0, - 0.0, - 0.0, - self._vel_cmd_msg.angular.z, - ], - dtype=np.float32, - ) - dv_des = (v_des - v) / self._dt - tau = self.controller.update( - v=v, - v_des=v_des, - dv_des=dv_des, - ) - - # self.v_prev = v - # self.v_des_prev = v_des - - - cmd_force = geometry_msgs.msg.Wrench() - cmd_force.force.x = tau[0] - cmd_force.force.y = tau[1] - cmd_force.torque.z = tau[5] - - self._pubs["force"].publish(cmd_force) - - def _vel_cmd_callback(self, msg: geometry_msgs.msg.Twist): - self._vel_cmd_msg = msg - - def _odom_callback(self, msg: geometry_msgs.msg.Wrench): - self._odom_msg = msg - -def main(args=None): - rclpy.init(args=args) - velocity_controller = BacksteppingVelocityControllerROS() - rclpy.spin(velocity_controller) - velocity_controller.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/cybership_dp/launch/dp.launch.py b/cybership_dp/launch/dp.launch.py index ef147e3..16d4a21 100644 --- a/cybership_dp/launch/dp.launch.py +++ b/cybership_dp/launch/dp.launch.py @@ -18,23 +18,13 @@ def generate_launch_description(): [ launch_ros.substitutions.FindPackageShare("cybership_dp"), "launch", - "force_controller.launch.py", + "thrust_allocation.launch.py", ] ) ), launch_arguments=[ ("vessel_name", launch.substitutions.LaunchConfiguration("vessel_name")), ("vessel_model", launch.substitutions.LaunchConfiguration("vessel_model")), - ( - "param_file", - launch.substitutions.PathJoinSubstitution( - [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), - "config", - "force_controller.yaml", - ] - ), - ), ], ) @@ -51,20 +41,40 @@ def generate_launch_description(): launch_arguments=[ ("vessel_name", launch.substitutions.LaunchConfiguration("vessel_name")), ("vessel_model", launch.substitutions.LaunchConfiguration("vessel_model")), - ( - "param_file", - launch.substitutions.PathJoinSubstitution( - [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), - "config", - "velocity_controller.yaml", - ] - ), - ), ], ) + include_position_controller = launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_dp"), + "launch", + "position_controller.launch.py", + ] + ) + ), + launch_arguments=[ + ("vessel_name", launch.substitutions.LaunchConfiguration("vessel_name")), + ("vessel_model", launch.substitutions.LaunchConfiguration("vessel_model")), + ], + ) + + # Add service call to switch force multiplexer to velocity controller output + # This will switch the force_mux to use control/force/command/velocity + service_call_switch_mux = launch.actions.ExecuteProcess( + cmd=[ + 'ros2', 'service', 'call', + [launch.substitutions.LaunchConfiguration("vessel_name"), '/force_mux/select'], + 'topic_tools_interfaces/srv/MuxSelect', + '{topic: "control/force/command/position"}' + ], + output='screen', + ) + ld.add_action(service_call_switch_mux) + ld.add_action(include_force_controller) ld.add_action(include_velocity_controller) + ld.add_action(include_position_controller) return ld diff --git a/cybership_dp/launch/position_controller.launch.py b/cybership_dp/launch/position_controller.launch.py new file mode 100644 index 0000000..1fc2810 --- /dev/null +++ b/cybership_dp/launch/position_controller.launch.py @@ -0,0 +1,50 @@ +import launch +import launch.actions +import launch.substitutions +import launch_ros.actions +from cybership_utilities.launch import anon +from cybership_utilities.launch import COMMON_ARGUMENTS as ARGUMENTS + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + arg_param_file = launch.actions.DeclareLaunchArgument( + name="param", + default_value=launch.substitutions.PathJoinSubstitution( + [ + launch_ros.substitutions.FindPackageShare("cybership_config"), + "config", + launch.substitutions.LaunchConfiguration("vessel_model"), + "controller_position.yaml", + ] + ), + ) + ld.add_action(arg_param_file) + + # arguments = list(filter(lambda a: a.name not in ["param_file", "vessel_model"], ARGUMENTS)) + for arg in ARGUMENTS: + ld.add_action(arg) + + node_position_controller = launch_ros.actions.Node( + namespace=launch.substitutions.LaunchConfiguration("vessel_name"), + package="cybership_dp", + executable="position_control_node.py", + name=f"position_controller", + parameters=[ + launch.substitutions.LaunchConfiguration("param"), + ], + remappings=[ + ( + "control/force/command", + "control/force/command/position", + ), # Remap force command to position controller output + ], + output="screen", + respawn=True, + respawn_delay=5, + ) + + ld.add_action(node_position_controller) + + return ld diff --git a/cybership_dp/launch/force_controller.launch.py b/cybership_dp/launch/thrust_allocation.launch.py similarity index 56% rename from cybership_dp/launch/force_controller.launch.py rename to cybership_dp/launch/thrust_allocation.launch.py index 4178171..9569535 100644 --- a/cybership_dp/launch/force_controller.launch.py +++ b/cybership_dp/launch/thrust_allocation.launch.py @@ -9,34 +9,19 @@ def generate_launch_description(): ld = launch.LaunchDescription() - arg_param_file = launch.actions.DeclareLaunchArgument( - name="param_file", - default_value=launch.substitutions.PathJoinSubstitution( - [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), - "config", - "force_controller.yaml", - ] - ), - ) - ld.add_action(arg_param_file) - - arguments = list(filter(lambda a: a.name != "param_file", ARGUMENTS)) - for arg in arguments: - ld.add_action(arg) - node_force_controller = launch_ros.actions.Node( + node_thrust_allocation = launch_ros.actions.Node( namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="cybership_dp", - executable="force_controller.py", - name=f"force_controller_{anon()}", + executable="thrust_allocation_node.py", + name=f"thrust_allocation_node", parameters=[ - launch.substitutions.LaunchConfiguration("param_file"), { "vessel_model": launch.substitutions.LaunchConfiguration( "vessel_model" ), }, + {"frequency": 20.0, } ], arguments=[ "--vessel-model", @@ -44,6 +29,12 @@ def generate_launch_description(): "--vessel-name", launch.substitutions.LaunchConfiguration("vessel_name"), ], + remappings=[ + ( + "control/force/command", + "control/force/command/mux", + ), # Remap to the multiplexer output + ], output="screen", respawn=True, respawn_delay=5, @@ -54,18 +45,20 @@ def generate_launch_description(): namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="topic_tools", executable="mux", - name=f"force_mux_{anon()}", + name=f"force_mux", arguments=[ - "control/force/command", # Output topic - "control/force/mux", # Input topic to listen to - "--repeat-delay", "0.1" # Optional delay for repeated messages + "control/force/command/mux", # Output topic + "control/force/command", # Input topic to listen to + "control/force/command/manual", # Input topic to listen to + "control/force/command/velocity", # Input topic to listen to + "control/force/command/position", # Input topic to listen to ], output="screen", respawn=True, respawn_delay=5, ) - ld.add_action(node_force_controller) + ld.add_action(node_thrust_allocation) ld.add_action(node_topic_mux) return ld diff --git a/cybership_dp/launch/velocity_controller.launch.py b/cybership_dp/launch/velocity_controller.launch.py index 3518db4..4b43cf0 100644 --- a/cybership_dp/launch/velocity_controller.launch.py +++ b/cybership_dp/launch/velocity_controller.launch.py @@ -13,42 +13,60 @@ def generate_launch_description(): name="param_file", default_value=launch.substitutions.PathJoinSubstitution( [ - launch_ros.substitutions.FindPackageShare("cybership_dp"), + launch_ros.substitutions.FindPackageShare("cybership_config"), "config", - "velocity_controller.yaml", + launch.substitutions.LaunchConfiguration("vessel_model"), + "controller_velocity.yaml", ] ), ) ld.add_action(arg_param_file) - arguments = list(filter(lambda a: a.name != "param_file", ARGUMENTS)) + arguments = list(filter(lambda a: a.name not in ["param_file", "vessel_model"], ARGUMENTS)) for arg in arguments: ld.add_action(arg) - node_force_controller = launch_ros.actions.Node( + node_velocity_controller = launch_ros.actions.Node( namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="cybership_dp", - executable="velocity_controller.py", - name=f"velocity_controller_{anon()}", + executable="velocity_control_node.py", + name=f"velocity_controller", parameters=[ launch.substitutions.LaunchConfiguration("param_file"), - { - "vessel_model": launch.substitutions.LaunchConfiguration( - "vessel_model" - ), - }, ], - arguments=[ - "--vessel-model", - launch.substitutions.LaunchConfiguration("vessel_model"), - "--vessel-name", - launch.substitutions.LaunchConfiguration("vessel_name"), + remappings=[ + ( + "control/velocity/command", + "control/velocity/command/mux", + ), # Remap to the multiplexer output + ( + "control/force/command", + "control/force/command/velocity", + ), # Remap force command to velocity controller output ], output="screen", respawn=True, respawn_delay=5, ) - ld.add_action(node_force_controller) + ld.add_action(node_velocity_controller) + + # Add topic multiplexer node for velocity commands + node_topic_mux = launch_ros.actions.Node( + namespace=launch.substitutions.LaunchConfiguration("vessel_name"), + package="topic_tools", + executable="mux", + name=f"velocity_mux", + arguments=[ + "control/velocity/command/mux", # Output topic + "control/velocity/command", # Input topic to listen to + "control/velocity/command/los", # Input topic to listen to + "control/velocity/command/manual", # Input topic to listen to + ], + output="screen", + respawn=True, + respawn_delay=5, + ) + ld.add_action(node_topic_mux) return ld diff --git a/cybership_tests/cybership_tests/reference_filter_server.py b/cybership_dp/nodes/position_control_node.py similarity index 61% rename from cybership_tests/cybership_tests/reference_filter_server.py rename to cybership_dp/nodes/position_control_node.py index 95c8bc3..c70db0b 100755 --- a/cybership_tests/cybership_tests/reference_filter_server.py +++ b/cybership_dp/nodes/position_control_node.py @@ -15,6 +15,7 @@ import numpy as np import rclpy from rclpy.node import Node +from rclpy.parameter import Parameter from geometry_msgs.msg import Wrench, Pose2D, PoseStamped, TwistStamped from nav_msgs.msg import Odometry import math @@ -27,7 +28,8 @@ from shoeboxpy.model3dof import Shoebox from visualization_msgs.msg import Marker from std_msgs.msg import Float32MultiArray - +from std_srvs.srv import SetBool, Trigger, Empty +from cybership_controller.position.reference_filter import ThirdOrderReferenceFilter try: from cybership_interfaces.msg import PerformanceMetrics @@ -62,88 +64,6 @@ def Rz(psi): ) -class ThrdOrderRefFilter: - """Third-order reference filter for guidance.""" - - def __init__(self, dt, omega=[0.2, 0.2, 0.2], delta=[1.0, 1.0, 1.0], initial_eta=None): - self._dt = dt - # Keep everything as a float array - self.eta_d = ( - np.zeros(3) if initial_eta is None else np.array( - initial_eta, dtype=float) - ) # [ x, y, yaw ] - self.eta_d_dot = np.zeros(3) - self.eta_d_ddot = np.zeros(3) - self._eta_r = self.eta_d.copy() # reference target (unwrapped) - - # State vector of 9 elements: [ eta, eta_dot, eta_ddot ] - self._x = np.concatenate([self.eta_d, self.eta_d_dot, self.eta_d_ddot]) - - # Gains - self._delta = np.eye(3) - self._w = np.diag(omega) - O3x3 = np.zeros((3, 3)) - self.Ad = np.block( - [ - [O3x3, np.eye(3), O3x3], - [O3x3, O3x3, np.eye(3)], - [ - -self._w**3, - -(2 * self._delta + np.eye(3)) @ self._w**2, - -(2 * self._delta + np.eye(3)) @ self._w, - ], - ] - ) - self.Bd = np.block([[O3x3], [O3x3], [self._w**3]]) - - def get_eta_d(self): - """Get desired pose: [ x, y, yaw ].""" - return self.eta_d - - def get_eta_d_dot(self): - """Get desired velocity in the inertial frame.""" - return self.eta_d_dot - - def get_eta_d_ddot(self): - """Get desired acceleration in the inertial frame.""" - return self.eta_d_ddot - - def get_nu_d(self): - """Get desired velocity in *body* frame (u, v, r).""" - psi = self.eta_d[2] - return Rz(psi).T @ self.eta_d_dot - - def set_eta_r(self, eta_r): - """ - Set the reference pose. We ensure the yaw changes by at most ±π - so the filter sees only small changes in yaw. - """ - old_yaw = self._eta_r[2] - new_yaw = eta_r[2] - - # Wrap new yaw to [-pi, pi] - new_yaw = wrap_to_pi(new_yaw) - - # Get minimal difference - diff = wrap_to_pi(new_yaw - old_yaw) - continuous_yaw = old_yaw + diff # small step only - - self._eta_r = np.array([eta_r[0], eta_r[1], continuous_yaw]) - - def update(self): - """ - Integrate filter for one time step. We do NOT forcibly wrap - 'self.eta_d[2]' so the filter remains continuous in yaw. - """ - x_dot = self.Ad @ self._x + self.Bd @ self._eta_r - self._x += self._dt * x_dot - - # Extract the updated states - self.eta_d = self._x[:3] - self.eta_d_dot = self._x[3:6] - self.eta_d_ddot = self._x[6:] - - def saturate(x, z): """ Saturation function: returns x / (|x| + z) @@ -152,9 +72,68 @@ def saturate(x, z): return x / (np.abs(x) + z) -class GotoPointController(Node): +class PositionController(Node): def __init__(self): - super().__init__("goto_point_controller", namespace="voyager") + super().__init__("position_controller", namespace="cybership") + + # Declare parameters with default values + self.declare_parameters( + namespace='', + parameters=[ + # Controller gains + ('control.p_gain.pos', 4.0), + ('control.i_gain.pos', 0.2), + ('control.d_gain.pos', 0.2), + ('control.p_gain.vel', 0.7), + ('control.i_gain.vel', 0.1), + ('control.d_gain.vel', 0.5), + ('control.p_gain.yaw', 1.3), + ('control.i_gain.yaw', 0.2), + ('control.d_gain.yaw', 1.0), + + # Controller limits + ('control.max_integral_error.pos', 1.0), + ('control.max_integral_error.yaw', 1.4), + ('control.saturation.pos', 0.1), + ('control.saturation.yaw', 0.1), + + # Tolerances + ('control.tolerance.pos', 0.25), + ('control.tolerance.yaw', 0.1), + # Require being within tolerance for this long before success + ('control.success_hold_time', 1.0), + + # Vessel properties + ('vessel.length', 1.0), + ('vessel.beam', 0.3), + ('vessel.draft', 0.02), + + # Reference filter parameters + ('filter.omega', [0.15, 0.15, 0.15]), + ('filter.delta', [0.8, 0.8, 0.8]), + + # Performance metrics + ('metrics.window_size', 200), + ('metrics.interval', 1.0), + + # Time step + ('dt', 0.01), + ] + ) + + # Initialize the 3rd order reference filter. + # Here, we treat [x, y, yaw] as the 3D pose to be smoothed. + self.ref_filter = None + + self.start_time = None + + self.error_window = [] # For position error + self.error_yaw_window = [] # For yaw error + self.sample_count = 0 + self.last_metrics_time = 0.0 + + # Add parameter callback for runtime updates + self.add_on_set_parameters_callback(self.parameters_callback) # Publisher to send control commands (force and torque) self.control_pub = self.create_publisher( @@ -169,13 +148,11 @@ def __init__(self): self.debug_error_vel_pub = self.create_publisher( TwistStamped, "control/pose/debug/tracking_error_velocity", 10) - self.debug_metrics_pub = None if PerformanceMetrics is not None: self.debug_metrics_pub = self.create_publisher( PerformanceMetrics, "control/pose/debug/performance_metrics", 10) - # self.create_subscription(PoseStamped, "/goal_pose", self.goal_pose_callback, 10) self.create_subscription( Odometry, "measurement/odom", self.odom_callback, 10) @@ -183,7 +160,12 @@ def __init__(self): self.marker_pub = self.create_publisher( Marker, "visualization_marker", 10) - self.dt = 0.01 # seconds + # Get time step from parameters + self.dt = self.get_parameter('dt').value + + # Initialize controller parameters from ROS parameters + self.update_configuration() + # Latest odometry message storage self.latest_odom = None @@ -193,66 +175,20 @@ def __init__(self): self.target_y = None # target y position (meters) self.target_yaw = None # target yaw (radians) - # --- Controller gains (for the PD part) --- - # You can tune these gains as needed. - self.Kp_pos = 4.0 # proportional gain for position - self.Ki_pos = 0.2 # integral gain for position - self.Kd_pos = 0.2 # derivative gain for position - self.Kp_vel = 0.7 # proportional gain for velocity - self.Ki_vel = 0.1 # integral gain for velocity - self.Kd_vel = 0.5 # derivative gain for velocity - - self.Kp_yaw = 1.3 # proportional gain for yaw - self.Ki_yaw = 0.2 # integral gain for yaw - self.Kd_yaw = 1.0 # derivative gain for yaw - - # --- Performance metrics --- - # Performance metrics tracking with moving window - self.window_size = 200 # 2 seconds of data at 100Hz - self.error_window = [] # Store recent position errors - self.error_yaw_window = [] # Store recent yaw errors - self.start_time = None - self.sample_count = 0 - - # Time between metrics calculations (e.g., 1 second) - self.metrics_interval = 1.0 # seconds - self.last_metrics_time = 0.0 - # Track integral error - self.max_integral_error_pos = 1.0 - self.max_integral_error_yaw = 1.4 self.integral_error_pos = np.zeros(2) self.integral_error_yaw = 0.0 - # Saturation parameters (if needed) - self.saturation_pos = 0.1 - self.saturation_yaw = 0.1 - - # Tolerances for considering the target reached - self.pos_tol = 0.25 # meters - self.yaw_tol = 0.1 # radians - self.error_pos = np.zeros(2) self.error_yaw = 0.0 - # Initialize the 3rd order reference filter. - # Here, we treat [x, y, yaw] as the 3D pose to be smoothed. - initial_eta = [self.target_x, self.target_y, self.target_yaw] - self.ref_filter = None - self.get_logger().info( - "Goto Point Controller (Reference Filter Version) Initialized." + "Goto Point Controller (Reference Filter Version) Initialized with parameters." ) # Timer for periodic control updates self.timer = self.create_timer(self.dt, self.control_loop) - self.shoebox = Shoebox( - L=1.0, - B=0.3, - T=0.02, - ) - # --- Action Server using nav2_msgs/NavigateToPose --- self._action_server = ActionServer( self, @@ -262,34 +198,166 @@ def __init__(self): goal_callback=self.action_goal_callback, cancel_callback=self.action_cancel_callback, ) + # Service to enable/disable the controller + self.enabled = True + self.state_service = self.create_service( + SetBool, + f"{self.get_name()}/change_state", + self.change_state_callback + ) + + def update_configuration(self): + """Update controller configuration from parameters""" + # Get controller gains + self.Kp_pos = self.get_parameter('control.p_gain.pos').value + self.Ki_pos = self.get_parameter('control.i_gain.pos').value + self.Kd_pos = self.get_parameter('control.d_gain.pos').value + self.Kp_vel = self.get_parameter('control.p_gain.vel').value + self.Ki_vel = self.get_parameter('control.i_gain.vel').value + self.Kd_vel = self.get_parameter('control.d_gain.vel').value + self.Kp_yaw = self.get_parameter('control.p_gain.yaw').value + self.Ki_yaw = self.get_parameter('control.i_gain.yaw').value + self.Kd_yaw = self.get_parameter('control.d_gain.yaw').value + + # Get controller limits + self.max_integral_error_pos = self.get_parameter( + 'control.max_integral_error.pos').value + self.max_integral_error_yaw = self.get_parameter( + 'control.max_integral_error.yaw').value + self.saturation_pos = self.get_parameter( + 'control.saturation.pos').value + self.saturation_yaw = self.get_parameter( + 'control.saturation.yaw').value + + # Get tolerances + self.pos_tol = self.get_parameter('control.tolerance.pos').value + self.yaw_tol = self.get_parameter('control.tolerance.yaw').value + self.success_hold_time = self.get_parameter('control.success_hold_time').value + + # Get vessel properties + vessel_length = self.get_parameter('vessel.length').value + vessel_beam = self.get_parameter('vessel.beam').value + vessel_draft = self.get_parameter('vessel.draft').value + + # Create vessel model + self.shoebox = Shoebox( + L=vessel_length, + B=vessel_beam, + T=vessel_draft, + ) + + # Get performance metrics parameters + self.window_size = self.get_parameter('metrics.window_size').value + self.metrics_interval = self.get_parameter('metrics.interval').value + + # Reset reference filter if parameters change + if self.ref_filter is not None and self.latest_odom is not None: + # Get filter parameters + filter_omega = self.get_parameter('filter.omega').value + filter_delta = self.get_parameter('filter.delta').value + + # Save current state + current_eta = self.ref_filter.eta_d + + # Create new filter with updated parameters + self.ref_filter = ThirdOrderReferenceFilter( + dt=self.dt, + omega=filter_omega, + delta=filter_delta, + initial_eta=current_eta, + ) + self.ref_filter.eta_d = current_eta + + self.get_logger().info( + f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + + f"POS gains: P={self.Kp_pos}, I={self.Ki_pos}, D={self.Kd_pos}, " + + f"YAW gains: P={self.Kp_yaw}, I={self.Ki_yaw}, D={self.Kd_yaw}" + ) + + def parameters_callback(self, params): + """Handle parameter updates""" + update_needed = False + + for param in params: + if param.name.startswith(('control.', 'vessel.', 'filter.', 'metrics.')): + update_needed = True + + if param.name == 'dt': + self.dt = param.value + # Recreate the timer with new dt + self.timer.cancel() + self.timer = self.create_timer(self.dt, self.control_loop) + update_needed = True + + # Update configuration if parameters changed + if update_needed: + self.update_configuration() + + return True # Accept all parameter changes + + # def odom_callback(self, msg: Odometry): + # """ + # Callback to update the latest odometry measurement. + # """ + # if self.ref_filter is None: + # print("Setting target position from odometry.") + # self.target_x = msg.pose.pose.position.x + # self.target_y = msg.pose.pose.position.y + # _, _, self.target_yaw = R.from_quat( + # [ + # msg.pose.pose.orientation.x, + # msg.pose.pose.orientation.y, + # msg.pose.pose.orientation.z, + # msg.pose.pose.orientation.w, + # ] + # ).as_euler("xyz", degrees=False) + + # # Get filter parameters from ROS parameters + # filter_omega = self.get_parameter('filter.omega').value + # filter_delta = self.get_parameter('filter.delta').value + + # self.ref_filter = ThirdOrderReferenceFilter( + # dt=self.dt, + # omega=filter_omega, + # delta=filter_delta, + # initial_eta=[self.target_x, self.target_y, self.target_yaw], + # ) + # self.ref_filter.eta_d = np.array( + # [self.target_x, self.target_y, self.target_yaw] + # ) + + # self.latest_odom = msg def odom_callback(self, msg: Odometry): - """ - Callback to update the latest odometry measurement. - """ + current_x = msg.pose.pose.position.x + current_y = msg.pose.pose.position.y + _, _, current_yaw = R.from_quat([ + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + msg.pose.pose.orientation.w + ]).as_euler("xyz", degrees=False) + + # Always store latest odom + self.latest_odom = msg + if self.ref_filter is None: - print("Setting target position from odometry.") - self.target_x = msg.pose.pose.position.x - self.target_y = msg.pose.pose.position.y - _, _, self.target_yaw = R.from_quat( - [ - msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, - msg.pose.pose.orientation.w, - ] - ).as_euler("xyz", degrees=False) - self.ref_filter = ThrdOrderRefFilter( + # Initialize the filter to CURRENT pose (not the target) + filter_omega = self.get_parameter('filter.omega').value + filter_delta = self.get_parameter('filter.delta').value + self.ref_filter = ThirdOrderReferenceFilter( dt=self.dt, - omega=[0.15, 0.15, 0.15], - delta=[0.8, 0.8, 0.8], - initial_eta=[self.target_x, self.target_y, self.target_yaw], - ) - self.ref_filter.eta_d = np.array( - [self.target_x, self.target_y, self.target_yaw] + omega=filter_omega, + delta=filter_delta, + initial_eta=[current_x, current_y, current_yaw], ) + self.ref_filter.eta_d = np.array([current_x, current_y, current_yaw]) - self.latest_odom = msg + # Only seed a target from odom if no goal has ever been set + if self.target_x is None: + self.target_x = current_x + self.target_y = current_y + self.target_yaw = current_yaw def process_performance_metrics(self, desired_pose, desired_vel, error_pos, error_vel, error_yaw): """ @@ -378,12 +446,15 @@ def process_performance_metrics(self, desired_pose, desired_vel, error_pos, erro f"RMS: {metrics_msg.rms:.3f}m, Max: {metrics_msg.max:.3f}m" ) - def control_loop(self): """ - Control loop that computes and publishes the control command. + Periodic control loop to update reference filter and publish control commands. This version uses a 3rd order reference filter to generate a smooth desired trajectory. """ + # Skip control loop if controller is disabled + if not self.enabled: + return + if self.latest_odom is None: return @@ -438,14 +509,10 @@ def control_loop(self): self.integral_error_yaw += error_yaw * self.dt # Apply saturation to the integral error - self.integral_error_pos = np.clip(self.integral_error_pos, -self.max_integral_error_pos, self.max_integral_error_pos) - # self.integral_error_pos = self.max_integral_error_pos * saturate( - # self.integral_error_pos, self.saturation_pos - # ) - self.integral_error_yaw = np.clip(self.integral_error_yaw, -self.max_integral_error_yaw, self.max_integral_error_yaw) - # self.integral_error_yaw = self.max_integral_error_yaw * saturate( - # self.integral_error_yaw, self.saturation_yaw - # ) + self.integral_error_pos = np.clip( + self.integral_error_pos, -self.max_integral_error_pos, self.max_integral_error_pos) + self.integral_error_yaw = np.clip( + self.integral_error_yaw, -self.max_integral_error_yaw, self.max_integral_error_yaw) # Compute control commands. # For position, we use feedforward desired acceleration plus a PID correction. @@ -469,7 +536,8 @@ def control_loop(self): ) # Process and publish performance metrics - self.process_performance_metrics(desired_pose, desired_vel, error_pos, error_vel, error_yaw) + self.process_performance_metrics( + desired_pose, desired_vel, error_pos, error_vel, error_yaw) control_x, control_y, control_yaw = Rz( current_yaw).T @ np.array([world_x, world_y, world_yaw]) @@ -507,11 +575,44 @@ def action_cancel_callback(self, goal_handle): self.get_logger().info("Received cancel request for NavigateToPose") return CancelResponse.ACCEPT + def change_state_callback(self, request, response): + """Service callback to enable/disable the controller.""" + if request.data: + self.enabled = True + # Reset controller state + self.start_time = None + self.error_window.clear() + self.error_yaw_window.clear() + self.sample_count = 0 + self.last_metrics_time = 0.0 + self.integral_error_pos = np.zeros(2) + self.integral_error_yaw = 0.0 + self.error_pos = np.zeros(2) + self.error_yaw = 0.0 + self.ref_filter = None + + response.success = True + response.message = "Controller enabled and reset." + else: + self.enabled = False + # Publish zero forces and torques to reset the controller + zero_wrench = Wrench() + self.control_pub.publish(zero_wrench) + + response.success = True + response.message = "Controller disabled." + return response + def execute_callback(self, goal_handle): self.get_logger().info("Executing NavigateToPose goal...") # Extract target pose from the goal message target_pose: PoseStamped = goal_handle.request.pose + # Warn if frame differs from odom, since we compute distances in odom frame + if target_pose.header.frame_id and target_pose.header.frame_id not in ("odom", "/odom"): + self.get_logger().warn( + f"Goal frame_id '{target_pose.header.frame_id}' differs from 'odom'; no TF transform is applied." + ) self.target_x = target_pose.pose.position.x self.target_y = target_pose.pose.position.y @@ -533,14 +634,16 @@ def execute_callback(self, goal_handle): self.publish_target_pose_marker(target_pose) - # Loop until the robot reaches the target (within tolerance) or the goal is canceled. - while rclpy.ok(): + # Loop until the robot reaches the target (within tolerance for a hold time) or the goal is canceled. + within_since = None + while rclpy.ok(): # Loop until target reached or goal is canceled if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().info("NavigateToPose goal canceled") result = NavigateToPose.Result() return result + # Wait for odometry if self.latest_odom is None: time.sleep(1.0) continue @@ -576,11 +679,18 @@ def execute_callback(self, goal_handle): f"Distance remaining: {error_norm:.2f}, angle error: {error_yaw:.2f}" ) - # Check if target is reached (both position and yaw) + # Check if target is reached (both position and yaw) and held + now_t = self.get_clock().now().nanoseconds / 1e9 if error_norm < self.pos_tol and abs(error_yaw) < self.yaw_tol: - self.error_pos = np.array([np.inf, np.info]) - self.error_yaw = np.inf - break + if within_since is None: + within_since = now_t + if (now_t - within_since) >= self.success_hold_time: + # Mark large errors to avoid lingering metrics effects + self.error_pos = np.array([np.inf, np.inf]) + self.error_yaw = np.inf + break + else: + within_since = None time.sleep(1.0) # Publish feedback at ~1Hz @@ -620,7 +730,7 @@ def publish_target_pose_marker(self, pose_stamped: PoseStamped): def main(args=None): rclpy.init(args=args) - server_node = GotoPointController() + server_node = PositionController() client_node = NavigateToPoseClient() executor = ( MultiThreadedExecutor() diff --git a/cybership_dp/nodes/force_controller.py b/cybership_dp/nodes/thrust_allocation_node.py similarity index 100% rename from cybership_dp/nodes/force_controller.py rename to cybership_dp/nodes/thrust_allocation_node.py diff --git a/cybership_tests/cybership_tests/velocity_control.py b/cybership_dp/nodes/velocity_control_node.py similarity index 53% rename from cybership_tests/cybership_tests/velocity_control.py rename to cybership_dp/nodes/velocity_control_node.py index 9242891..4ffc2f5 100755 --- a/cybership_tests/cybership_tests/velocity_control.py +++ b/cybership_dp/nodes/velocity_control_node.py @@ -9,18 +9,16 @@ # --------------------------------------------------------------------------- import numpy as np -import math import rclpy from rclpy.node import Node -from geometry_msgs.msg import Twist, Wrench, PoseStamped, Pose2D +from rclpy.parameter import Parameter +from geometry_msgs.msg import Twist, Wrench from nav_msgs.msg import Odometry -from typing import Tuple import shoeboxpy.model3dof as box -from visualization_msgs.msg import Marker -from scipy.spatial.transform import Rotation as R -import numpy as np -from typing import Dict, Any, Optional +from std_srvs.srv import SetBool + +from cybership_controller.velocity.reference_feedforward import RffVelocityController as VelocityController # Add import for performance metrics try: @@ -28,173 +26,7 @@ except ImportError: print("cybership_msgs not found. Skipping performance metrics.") -import numpy as np - -class ExponentialSmoothing(): - """ - Exponential smoothing class - """ - - def __init__(self, r: float = 0.7) -> None: - """ - Exponential smoothing class constructor - - :param r: A weighting factor in the set [0-1] - """ - self.r = r - self.x = None - self.x_p = None - self.dx_p = None - - def __call__(self, x: np.ndarray) -> np.ndarray: - """ - Exponential smoothing function - - :param x: current value of x - :return: dx - """ - if self.x_p is None or self.dx_p is None: - self.x_p = x - self.dx_p = x - return x - else: - self.dx_p = ExponentialSmoothing.__func(self.r, x, self.x_p, self.dx_p) - self.x_p = x - return self.dx_p - def reset(self, x: np.ndarray) -> None: - """ - Reset the exponential smoothing - - :param x: current value of x - :return: - """ - self.x_p = x - self.dx_p = x - - @staticmethod - def __func( - r: float, x: np.ndarray, x_p: np.ndarray = None, dx_p: np.ndarray = None - ) -> np.ndarray: - r""" - Exponential smoothing function - - .. math:: - \begin{aligned} - s_{0}&=x_{0}\\ - s_{t}&=\alpha x_{t}+(1-\alpha )s_{t-1},\quad t>0 - \end{aligned} - - - :param r: A weighting factor in the set [0-1] - :param x: current value of x - :param x_p: previous value of x - :param dx_p: last computed filtered x value - :return: - """ - if x_p is None or dx_p is None: - return x - else: - return (1 - r) * dx_p + r * (x - x_p) - -def _saturate(vec: np.ndarray, limit: float) -> np.ndarray: - """Scale `vec` so its l2-norm does not exceed `limit`. If `limit` <= 0, return vec unchanged.""" - nrm = np.linalg.norm(vec) - return vec if nrm <= limit or limit <= 0.0 else vec * (limit / nrm) - -# ---------------------------------------------------------------------- # -# Controller implementation # -# ---------------------------------------------------------------------- # - -class AccelLimitedInverseDynamicsPI(): - """Inverse-dynamics velocity controller with PI action and acceleration limiting.""" - - def __init__(self, config: Optional[Dict[str, Any]] = None): - - self.config = config or {} - # Matrices and gains - self.M: np.ndarray = np.array(self.config.get("M", np.eye(3)), dtype=float) - self.D: np.ndarray = np.array(self.config.get("D", np.zeros((3, 3))), dtype=float) - self.Kp: np.ndarray = np.array(self.config.get("Kp", np.eye(3)), dtype=float) - self.Ki: np.ndarray = np.array(self.config.get("Ki", np.zeros((3, 3))), dtype=float) - self.Kd: np.ndarray = np.array(self.config.get("Kd", np.zeros((3, 3))), dtype=float) - self.dt: float = float(self.config.get("dt", 0.01)) # seconds - - # Pre-compute inverse inertia - self.M_inv = np.linalg.inv(self.M) - - # Limits and behaviours - self.a_max: float = float(self.config.get("a_max", -1.0)) # hard acc limit (<=0 => off) - self.I_max: float = float(self.config.get("I_max", -1.0)) # integral wind-up cap (<=0 => off) - self.smooth: bool = bool(self.config.get("smooth_limit", False)) - - # Internal state - self._prev_vd: Optional[np.ndarray] = None # for desired-velocity derivative - self._int_e: np.ndarray = np.zeros(3) # integral of error dt - - # ------------------------------------------------------------------ # - # Public API # - # ------------------------------------------------------------------ # - def update( - self, - current_velocity: np.ndarray, - desired_velocity: np.ndarray, - dt: float, - ) -> np.ndarray: - """Compute the force/torque command tau.""" - - if dt <= 0.0: - raise ValueError("dt must be positive") - - v = current_velocity.reshape(3) - vd = desired_velocity.reshape(3) - - if not hasattr(self, '_vd_dot_smoother'): - r = self.config.get("filter_alpha", 0.7) - self._vd_dot_smoother = ExponentialSmoothing(r=r) - - if not hasattr(self, '_e_dot_smoother'): - r = self.config.get("filter_alpha", 0.7) - self._e_dot_smoother = ExponentialSmoothing(r=r) - - raw_vd_dot = (vd - self._prev_vd) / dt if self._prev_vd is not None else np.zeros(3) - vd_dot = self._vd_dot_smoother(raw_vd_dot) - self._prev_vd = vd.copy() - - e = v - vd - raw_e_dot = (v - self._prev_vd) / dt if self._prev_vd is not None else np.zeros(3) - e_dot = self._e_dot_smoother(raw_e_dot) - - self._int_e = self._int_e + e * dt - self._int_e = np.clip(self._int_e, -self.I_max, self.I_max) - - a_des = vd_dot - self.Kp @ e - self.Ki @ self._int_e - self.Kd @ e_dot - - tau = self.M @ a_des + self.D @ vd - return tau - - def reset(self): - """Clear stored derivative and integral information.""" - self._prev_vd = None - self._int_e = np.zeros(3) - - # ------------------------------------------------------------------ # - # Private helpers # - # ------------------------------------------------------------------ # - def _limit_acceleration(self, a_des: np.ndarray) -> tuple[np.ndarray, bool]: - """Return (a_cmd, saturated_flag) after applying the configured limiter.""" - if self.a_max <= 0.0: - return a_des, False # unlimited - - if self.smooth: - # Smooth limiter using tanh - a_cmd = self.a_max * np.tanh(a_des / self.a_max) - saturated = not np.allclose(a_cmd, a_des) - return a_cmd, saturated - else: - a_cmd = _saturate(a_des, self.a_max) - saturated = not np.allclose(a_cmd, a_des) - return a_cmd, saturated class VelocityControlNode(Node): """ @@ -203,30 +35,51 @@ class VelocityControlNode(Node): """ def __init__(self): - super().__init__("velocity_control_node", namespace="voyager") + super().__init__("velocity_control_node", namespace="cybership") + + # Declare parameters with default values + self.declare_parameters( + namespace='', + parameters=[ + # Vessel dimensions + ('vessel.length', 1.0), + ('vessel.beam', 0.3), + ('vessel.draft', 0.05), + + # Control gains + ('control.p_gain.surge', 1.0), + ('control.p_gain.sway', 1.0), + ('control.p_gain.yaw', 1.0), + + ('control.i_gain.surge', 0.0), + ('control.i_gain.sway', 0.0), + ('control.i_gain.yaw', 0.0), + + ('control.d_gain.surge', 0.3), + ('control.d_gain.sway', 0.3), + ('control.d_gain.yaw', 0.3), + + ('control.i_max', 1.0), + ('control.smooth_limit', True), + ('control.filter_alpha', 0.1), + + # Performance metrics + ('metrics.window_size', 50), + ('metrics.interval', 1.0), + + # Time step + ('dt', 0.1), + ] + ) - # Create a velocity controller - self.dt = 0.05 # seconds - self.vessel = box.Shoebox(L=1.0, B=0.3, T=0.05) + # Add parameter callback for runtime updates + self.add_on_set_parameters_callback(self.parameters_callback) - # # Control gains for surge, sway, and yaw - self.k_p_gain = np.array([5.0, 5.0, 5.0]) - self.k_i_gain = np.array([0.0, 0.0, 0.0]) - self.k_d_gain = np.array([0.3, 0.3, 0.3]) + # Get current parameter values + self.dt = self.get_parameter('dt').value - self.controller = AccelLimitedInverseDynamicsPI( - config={ - "M": self.vessel.M_eff, - "D": self.vessel.D, - "Kp": np.diag(self.k_p_gain), - "Ki": np.diag(self.k_i_gain), - "Kd": np.diag(self.k_d_gain), - "I_max": 20.0, # Nms - "smooth_limit": True, - "filter_alpha": 0.1, # Smoothing factor for desired velocity - "dt": self.dt, - } - ) + # Initialize vessel and controller + self.update_configuration() self.nu_prev = np.zeros(3) # [u, v, r] previous velocities self.nu_cmd = np.zeros(3) # [u, v, r] desired velocities @@ -248,13 +101,13 @@ def __init__(self): Odometry, 'measurement/odom', self.odom_callback, - 10 + 1 ) # Publisher for control commands self.control_pub = self.create_publisher( Wrench, - 'control/force/mux/velocity', + 'control/force/command', 10 ) @@ -281,18 +134,126 @@ def __init__(self): # --- Performance metrics --- # Performance metrics tracking with moving window - self.window_size = 200 # 2 seconds of data at 100Hz + self.window_size = self.get_parameter('metrics.window_size').value self.error_window = [] # Store recent velocity errors self.start_time = None self.sample_count = 0 # Time between metrics calculations - self.metrics_interval = 1.0 # seconds + self.metrics_interval = self.get_parameter('metrics.interval').value self.last_metrics_time = 0.0 # Timer for control loop self.timer = self.create_timer(self.dt, self.control_loop) + # Service to enable/disable the velocity controller + self.enabled = True + self.state_service = self.create_service( + SetBool, + f"{self.get_name()}/change_state", + self.change_state_callback + ) + + self.get_logger().info("Velocity controller initialized with parameters") + + def update_configuration(self): + """Update vessel and controller configuration from parameters""" + # Get vessel dimensions + vessel_length = self.get_parameter('vessel.length').value + vessel_beam = self.get_parameter('vessel.beam').value + vessel_draft = self.get_parameter('vessel.draft').value + + # Create vessel model + self.vessel = box.Shoebox(L=vessel_length, B=vessel_beam, T=vessel_draft) + + # Get control gains + self.k_p_gain = np.array([ + self.get_parameter('control.p_gain.surge').value, + self.get_parameter('control.p_gain.sway').value, + self.get_parameter('control.p_gain.yaw').value + ]) + + self.k_i_gain = np.array([ + self.get_parameter('control.i_gain.surge').value, + self.get_parameter('control.i_gain.sway').value, + self.get_parameter('control.i_gain.yaw').value + ]) + + self.k_d_gain = np.array([ + self.get_parameter('control.d_gain.surge').value, + self.get_parameter('control.d_gain.sway').value, + self.get_parameter('control.d_gain.yaw').value + ]) + + # Create/update controller + self.controller = VelocityController( + config={ + "M": self.vessel.M_eff, + "D": self.vessel.D, + "Kp": np.diag(self.k_p_gain), + "Ki": np.diag(self.k_i_gain), + "Kd": np.diag(self.k_d_gain), + "I_max": self.get_parameter('control.i_max').value, + "smooth_limit": self.get_parameter('control.smooth_limit').value, + "filter_alpha": self.get_parameter('control.filter_alpha').value, + "dt": self.dt, + } + ) + + # Update metrics parameters + self.window_size = self.get_parameter('metrics.window_size').value + self.metrics_interval = self.get_parameter('metrics.interval').value + + self.get_logger().info(f"Updated configuration - vessel: [{vessel_length}, {vessel_beam}, {vessel_draft}], " + + f"P: {self.k_p_gain}, I: {self.k_i_gain}, D: {self.k_d_gain}") + + def parameters_callback(self, params): + """Handle parameter updates""" + update_needed = False + + # Log parameter changes + + for param in params: + if param.name in [ + 'vessel.length', 'vessel.beam', 'vessel.draft', + 'control.p_gain.surge', 'control.p_gain.sway', 'control.p_gain.yaw', + 'control.i_gain.surge', 'control.i_gain.sway', 'control.i_gain.yaw', + 'control.d_gain.surge', 'control.d_gain.sway', 'control.d_gain.yaw', + 'control.i_max', 'control.smooth_limit', 'control.filter_alpha', + 'metrics.window_size', 'metrics.interval' + ]: + update_needed = True + + if param.name == 'dt': + self.dt = param.value + # Recreate the timer with new dt + self.timer.cancel() + self.timer = self.create_timer(self.dt, self.control_loop) + update_needed = True + + # Update vessel and controller if parameters changed + if update_needed: + self.update_configuration() + + return True # Accept all parameter changes + + def change_state_callback(self, request, response): + """Service callback to enable/disable the velocity controller.""" + if request.data: + self.enabled = True + # Reset performance metrics state + self.start_time = None + self.error_window.clear() + self.sample_count = 0 + self.last_metrics_time = 0.0 + response.success = True + response.message = "Velocity controller enabled and reset." + else: + self.enabled = False + response.success = True + response.message = "Velocity controller disabled." + return response + def cmd_vel_callback(self, msg: Twist): """ Process incoming velocity commands. @@ -401,16 +362,14 @@ def control_loop(self): """ Periodic control loop to update reference filter and publish control commands. """ - # Update the reference filter - - now = self.get_clock().now().nanoseconds * 1e-9 - dt_rt = now - self._prev_t if hasattr(self, '_prev_t') else self.dt + # Skip control loop if controller is disabled + if not self.enabled: + return tau = self.controller.update( current_velocity=self.nu, desired_velocity=self.nu_cmd, - dt=dt_rt + dt=self.dt ) - self._prev_t = now # Calculate velocity error for metrics error_vel = self.nu_cmd - self.nu @@ -429,6 +388,25 @@ def control_loop(self): self.control_pub.publish(wrench_msg) + def change_state_callback(self, request, response): + """ + Callback for the change_state service to enable/disable the controller. + """ + self.enabled = request.data + + if self.enabled: + response.message = "Velocity controller enabled" + self.get_logger().info(response.message) + else: + # Publish zero forces and torques to reset the controller + zero_wrench = Wrench() + self.control_pub.publish(zero_wrench) + response.message = "Velocity controller disabled" + self.get_logger().info(response.message) + + response.success = True + return response + def main(args=None): """ diff --git a/cybership_dp/nodes/velocity_controller.py b/cybership_dp/nodes/velocity_controller.py index bd539d4..544d409 100755 --- a/cybership_dp/nodes/velocity_controller.py +++ b/cybership_dp/nodes/velocity_controller.py @@ -12,6 +12,7 @@ import os import sys import argparse +import warnings import numpy as np @@ -21,6 +22,18 @@ import cybership_dp.voyager import cybership_dp.enterprise +# DEPRECATION WARNING +warnings.warn( + "This velocity_controller.py file is deprecated and may be removed in future versions. " + "Please use the updated velocity control implementation.", + DeprecationWarning, + stacklevel=2 +) + +print("DEPRECATED: This velocity controller node is deprecated and may be removed in future versions.") +print(" Please migrate to the updated velocity control implementation.") +print(" For more information, consult the documentation or contact the development team.") +print() class VelocityControllerManager(): @@ -47,6 +60,15 @@ def initialize(self) -> rclpy.node.Node: def main(args=None): + # Print deprecation warning + print("=" * 80) + print("DEPRECATION WARNING") + print("This velocity_controller.py node is DEPRECATED!") + print("Please migrate to the updated velocity control implementation.") + print("This file may be removed in future versions.") + print("=" * 80) + print() + rclpy.init(args=args) manager = VelocityControllerManager() diff --git a/cybership_interfaces/CMakeLists.txt b/cybership_interfaces/CMakeLists.txt index e753157..77ce1bd 100644 --- a/cybership_interfaces/CMakeLists.txt +++ b/cybership_interfaces/CMakeLists.txt @@ -7,6 +7,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -16,12 +17,17 @@ set(msg_files set(srv_files srv/ResetSimulator.srv ) +set(action_files + action/LOSGuidance.action +) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} + ${action_files} DEPENDENCIES geometry_msgs + nav_msgs builtin_interfaces ) diff --git a/cybership_interfaces/action/LOSGuidance.action b/cybership_interfaces/action/LOSGuidance.action new file mode 100644 index 0000000..cb0c1a5 --- /dev/null +++ b/cybership_interfaces/action/LOSGuidance.action @@ -0,0 +1,11 @@ +# Goal definition: path to follow +nav_msgs/Path path +# Result: final reached? not used +bool success +--- +# Empty result currently +bool success +--- +# Feedback: current heading and velocity command +float64 heading +geometry_msgs/Vector3 vel_cmd diff --git a/cybership_interfaces/package.xml b/cybership_interfaces/package.xml index 8075157..ce0bc1f 100644 --- a/cybership_interfaces/package.xml +++ b/cybership_interfaces/package.xml @@ -14,6 +14,7 @@ rosidl_default_generators geometry_msgs builtin_interfaces + nav_msgs rosidl_default_runtime rosidl_interface_packages diff --git a/cybership_mocap/README.md b/cybership_mocap/README.md index e9e475b..a1f89bf 100644 --- a/cybership_mocap/README.md +++ b/cybership_mocap/README.md @@ -66,5 +66,5 @@ The node behavior can be configured via parameters in the `config.yaml` file: ### Published Transforms (if enabled) -- `mocap` → `cybership/base_link` - Transform from the world frame to the ship's base frame +- `mocap` -> `cybership/base_link` - Transform from the world frame to the ship's base frame diff --git a/cybership_observer/CMakeLists.txt b/cybership_observer/CMakeLists.txt index 77edb1d..4fce58b 100644 --- a/cybership_observer/CMakeLists.txt +++ b/cybership_observer/CMakeLists.txt @@ -15,25 +15,26 @@ find_package(tf2_ros REQUIRED) include_directories(include) -add_executable(ned_to_enu_transformer src/ned_to_enu_transformer.cpp) -ament_target_dependencies(ned_to_enu_transformer +add_executable(ned_to_enu_transformer_cpp src/ned_to_enu_transformer.cpp) +ament_target_dependencies(ned_to_enu_transformer_cpp rclcpp std_msgs geometry_msgs tf2_ros ) + +file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/nodes/*.py) + install( PROGRAMS - nodes/enu_to_ned_twist.py - nodes/enu_to_ned_odom.py - nodes/ned_to_enu_transformer.py + ${NODE_SCRIPTS} DESTINATION lib/${PROJECT_NAME} ) install( TARGETS - ned_to_enu_transformer + ned_to_enu_transformer_cpp DESTINATION lib/${PROJECT_NAME} ) diff --git a/cybership_observer/nodes/enu_to_ned_odom.py b/cybership_observer/nodes/enu_to_ned_odom.py index 708582a..d4c3dfe 100755 --- a/cybership_observer/nodes/enu_to_ned_odom.py +++ b/cybership_observer/nodes/enu_to_ned_odom.py @@ -2,90 +2,67 @@ import rclpy from rclpy.node import Node -from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry -import tf_transformations -import numpy as np +from geometry_msgs.msg import Pose, PoseWithCovariance, TwistWithCovariance, Quaternion -class NEDToENUPublisher(Node): +from tf_transformations import euler_from_quaternion, quaternion_from_euler +class OdomEnuToNedNode(Node): def __init__(self): - super().__init__('ned_to_enu_publisher') - - # Declare and get the frame_id parameter - self.declare_parameter('frame_id', 'world') - self.frame_id = self.get_parameter( - 'frame_id').get_parameter_value().string_value - - # Subscriber to the NED frame PoseWithCovarianceStamped messages - self.ned_sub = self.create_subscription( + super().__init__('odom_enu_to_ned') + self.sub = self.create_subscription( Odometry, 'in_odom', - self.ned_callback, - 10 - ) - - # Publisher for the transformed ENU frame PoseWithCovarianceStamped messages - self.enu_pub = self.create_publisher( - Odometry, - 'out_odom', + self.odom_callback, 10 ) + self.declare_parameter('frame_id', 'world') + self.pub = self.create_publisher(Odometry, 'out_odom', 10) - def ned_callback(self, msg): - # Transform the pose from NED to ENU frame - enu_msg = Odometry() - enu_msg.header = msg.header - enu_msg.header.frame_id = self.frame_id # Set the frame_id parameter - - enu_msg.child_frame_id = msg.child_frame_id + def odom_callback(self, msg): + ned_msg = Odometry() + ned_msg.header = msg.header + ned_msg.child_frame_id = "base_link_ned" # Position transformation - enu_msg.pose.pose.position.x = - msg.pose.pose.position.y - enu_msg.pose.pose.position.y = - msg.pose.pose.position.x - enu_msg.pose.pose.position.z = - msg.pose.pose.position.z - - enu_msg.twist.twist.linear.x = msg.twist.twist.linear.y - enu_msg.twist.twist.linear.y = msg.twist.twist.linear.x - enu_msg.twist.twist.linear.z = - msg.twist.twist.linear.z - - enu_msg.twist.twist.angular.x = msg.twist.twist.angular.x - enu_msg.twist.twist.angular.y = - msg.twist.twist.angular.y - enu_msg.twist.twist.angular.z = - msg.twist.twist.angular.z - - - # Orientation transformation (quaternion) - q_ned = [ - msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z, - msg.pose.pose.orientation.w - ] - - roll, pitch, yaw = tf_transformations.euler_from_quaternion( - q_ned, axes='sxyz') # Check the orientation in Euler angles - - q_enu = tf_transformations.quaternion_from_euler( - roll, pitch, -yaw - np.pi / 2.0, axes='sxyz') - - enu_msg.pose.pose.orientation.x = q_enu[0] - enu_msg.pose.pose.orientation.y = q_enu[1] - enu_msg.pose.pose.orientation.z = q_enu[2] - enu_msg.pose.pose.orientation.w = q_enu[3] - - # Copy the covariance matrix as it is (assuming it remains the same) - enu_msg.pose.covariance = msg.pose.covariance - - # Publish the transformed message - self.enu_pub.publish(enu_msg) - + ned_msg.pose.pose.position.x = msg.pose.pose.position.y + ned_msg.pose.pose.position.y = msg.pose.pose.position.x + ned_msg.pose.pose.position.z = -msg.pose.pose.position.z + + # Orientation transformation + # Convert quaternion to euler + q = msg.pose.pose.orientation + euler = euler_from_quaternion([q.x, q.y, q.z, q.w]) + + roll = euler[0] + pitch = -euler[1] + yaw = -euler[2] + + # Convert back to quaternion + new_q = quaternion_from_euler(roll, pitch, yaw) + ned_msg.pose.pose.orientation = Quaternion() + ned_msg.pose.pose.orientation.x = new_q[0] + ned_msg.pose.pose.orientation.y = new_q[1] + ned_msg.pose.pose.orientation.z = new_q[2] + ned_msg.pose.pose.orientation.w = new_q[3] + + # Velocity transformation + ned_msg.twist.twist.linear.x = msg.twist.twist.linear.y + ned_msg.twist.twist.linear.y = msg.twist.twist.linear.x + ned_msg.twist.twist.linear.z = -msg.twist.twist.linear.z + + ned_msg.twist.twist.angular.x = msg.twist.twist.angular.y + ned_msg.twist.twist.angular.y = msg.twist.twist.angular.x + ned_msg.twist.twist.angular.z = -msg.twist.twist.angular.z + + self.pub.publish(ned_msg) def main(args=None): rclpy.init(args=args) - node = NEDToENUPublisher() + node = OdomEnuToNedNode() rclpy.spin(node) + node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': main() diff --git a/cybership_dp/config/enterprise/.gitkeep b/cybership_observer/nodes/ned_to_enu_odom.py similarity index 100% rename from cybership_dp/config/enterprise/.gitkeep rename to cybership_observer/nodes/ned_to_enu_odom.py diff --git a/cybership_observer/package.xml b/cybership_observer/package.xml index 4600177..18e8043 100644 --- a/cybership_observer/package.xml +++ b/cybership_observer/package.xml @@ -11,6 +11,7 @@ std_msgs rclpy robot_localization + tf_transformations ament_cmake ament_lint_auto diff --git a/cybership_simulator/config/multi_vessel_simulation.yaml b/cybership_simulator/config/multi_vessel_simulation.yaml new file mode 100644 index 0000000..9dc2a0c --- /dev/null +++ b/cybership_simulator/config/multi_vessel_simulation.yaml @@ -0,0 +1,30 @@ + +/voyager/voyager_simulation: + ros__parameters: + initial_conditions: + position: + x: 5.0 + y: 1.0 + yaw: 3.14159 + velocity: + surge: 0.0 + sway: 0.0 + yaw_rate: 0.0 + tf_prefix: voyager + body_frame_id: base_link_ned + world_frame_id: world + +/drillship/drillship_simulation: + ros__parameters: + initial_conditions: + position: + x: -2.5 + y: 0.5 + yaw: 0.0 + velocity: + surge: 0.0 + sway: 0.0 + yaw_rate: 0.0 + tf_prefix: drillship + body_frame_id: base_link_ned + world_frame_id: world diff --git a/cybership_simulator/config/simulation.yaml b/cybership_simulator/config/simulation.yaml index eff9db8..f64acde 100644 --- a/cybership_simulator/config/simulation.yaml +++ b/cybership_simulator/config/simulation.yaml @@ -8,4 +8,4 @@ velocity: surge: 0.0 sway: 0.0 - yaw_rate: 0.0 + yaw_rate: 0.0 \ No newline at end of file diff --git a/cybership_simulator/launch/drillship.launch.py b/cybership_simulator/launch/drillship.launch.py index 3d73101..8aeb77a 100644 --- a/cybership_simulator/launch/drillship.launch.py +++ b/cybership_simulator/launch/drillship.launch.py @@ -45,7 +45,8 @@ def generate_launch_description(): "config", "simulation.yaml", ] - ) + ), + {"tf_prefix": launch.substitutions.LaunchConfiguration("vessel_name")}, ], name=f"sim_{anon()}", ) diff --git a/cybership_simulator/launch/enterprise.launch.py b/cybership_simulator/launch/enterprise.launch.py index 5d38ca4..8ebdd00 100644 --- a/cybership_simulator/launch/enterprise.launch.py +++ b/cybership_simulator/launch/enterprise.launch.py @@ -45,7 +45,8 @@ def generate_launch_description(): "config", "simulation.yaml", ] - ) + ), + {"tf_prefix": launch.substitutions.LaunchConfiguration("vessel_name")}, ], name=f"sim_{anon()}", ) diff --git a/cybership_simulator/launch/multi_vessel.launch.py b/cybership_simulator/launch/multi_vessel.launch.py new file mode 100644 index 0000000..4eb81c5 --- /dev/null +++ b/cybership_simulator/launch/multi_vessel.launch.py @@ -0,0 +1,100 @@ +import os + +import launch +import launch.actions +import launch.conditions +import launch.substitutions +import launch_ros +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description() -> LaunchDescription: + ld = launch.LaunchDescription() + + # Launch args + arg_use_gui = launch.actions.DeclareLaunchArgument( + 'use_gui', default_value='true', description='Launch visualization for both vessels' + ) + + ld.add_action(arg_use_gui) + + # Resolve parameter file path + sim_share = get_package_share_directory('cybership_simulator') + multi_yaml_path = os.path.join( + sim_share, 'config', 'multi_vessel_simulation.yaml') + + # Include robot descriptions for both vessels + description_launch_voyager = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + launch_ros.substitutions.FindPackageShare( + 'cybership_description'), + '/launch/description.launch.py', + ] + ), + launch_arguments=[ + ('vessel_model', 'voyager'), + ('vessel_name', 'voyager'), + ], + ) + + description_launch_drillship = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + launch_ros.substitutions.FindPackageShare( + 'cybership_description'), + '/launch/description.launch.py', + ] + ), + launch_arguments=[ + ('vessel_model', 'drillship'), + ('vessel_name', 'drillship'), + ], + ) + + # Simulator nodes (names fixed to match keys in YAML; topics live under per-vessel namespace) + voyager_node = Node( + namespace='voyager', + package='cybership_simulator', + executable='voyager.py', + name='voyager_simulation', + parameters=[multi_yaml_path] + ) + + drillship_node = Node( + namespace='drillship', + package='cybership_simulator', + executable='drillship.py', + name='drillship_simulation', + parameters=[multi_yaml_path] + ) + + # Optional visualization for both (conditioned by use_gui) + viz = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + launch_ros.substitutions.FindPackageShare('cybership_viz'), + '/launch/multi.launch.py', + ] + ) + ) + + group_gui = launch.actions.GroupAction( + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('use_gui', default='true')), + actions=[viz], + ) + + # Order: viz (optional) -> descriptions -> simulation nodes + ld.add_action(group_gui) + ld.add_action(description_launch_voyager) + ld.add_action(description_launch_drillship) + ld.add_action(voyager_node) + ld.add_action(drillship_node) + + return ld diff --git a/cybership_simulator/launch/voyager.launch.py b/cybership_simulator/launch/voyager.launch.py index 1c5d714..84acbb3 100644 --- a/cybership_simulator/launch/voyager.launch.py +++ b/cybership_simulator/launch/voyager.launch.py @@ -45,7 +45,8 @@ def generate_launch_description(): "config", "simulation.yaml", ] - ) + ), + {"tf_prefix": launch.substitutions.LaunchConfiguration("vessel_name")}, ], name=f"sim_{anon()}", ) diff --git a/cybership_simulator/src/cybership_simulator/base.py b/cybership_simulator/src/cybership_simulator/base.py index 0ce7a2a..ec3c606 100644 --- a/cybership_simulator/src/cybership_simulator/base.py +++ b/cybership_simulator/src/cybership_simulator/base.py @@ -46,15 +46,16 @@ def __init__(self, node_name="base_simulator"): # Thruster command vector (u) will be defined by the subclass since its dimension can vary. self.u = None + # Declare and read parameters for initial conditions + self._declare_parameters() + self._read_parameters() + self.add_on_set_parameters_callback(self._on_parameter_change) + # Create vessel and allocator (subclasses provide concrete implementations) self.vessel = self._create_vessel() self.allocator = self._init_allocator() self.allocator.compute_configuration_matrix() - # Declare and read parameters for initial conditions - self._declare_parameters() - self._read_parameters() - self.add_on_set_parameters_callback(self._on_parameter_change) # Common publishers for odometry and TF self.odom = Odometry() @@ -124,6 +125,10 @@ def _declare_parameters(self): ('yaw_rate', 0.0), ] ) + self.declare_parameter('world_frame_id', 'world') + self.declare_parameter('body_frame_id', 'base_link_ned') + self.declare_parameter('tf_prefix', '') + def _read_parameters(self): self.eta0[0] = self.get_parameter('initial_conditions.position.x').value @@ -134,6 +139,12 @@ def _read_parameters(self): self.nu0[1] = self.get_parameter('initial_conditions.velocity.sway').value self.nu0[5] = self.get_parameter('initial_conditions.velocity.yaw_rate').value + self.world_frame_id = self.get_parameter('world_frame_id').value + self.body_frame_id = self.get_parameter('body_frame_id').value + self.tf_prefix = self.get_parameter('tf_prefix').value + + self.get_logger().info(f"Initial conditions set: eta0={self.eta0.flatten()}, nu0={self.nu0.flatten()}") + def _on_parameter_change(self, params): successful = True new_eta0 = self.eta0.copy() @@ -195,7 +206,7 @@ def iterate(self): # # Transform to body fixed frame # F[0:3] = Rz.T @ F[0:3] - self.vessel.eta[2] = 0.0 + # self.vessel.eta[2] = 0.0 self.vessel.step(tau=tau, dt=self.dt) @@ -212,16 +223,16 @@ def publish_clock(self): """ self.clock_msg.clock.sec = int(self.sim_time) self.clock_msg.clock.nanosec = int((self.sim_time - int(self.sim_time)) * 1e9) - self.publisher_clock.publish(self.clock_msg) + # self.publisher_clock.publish(self.clock_msg) def publish_odom(self): eta, nu = self.vessel.get_states() rot = R.from_euler('xyz', eta[3:6], degrees=False) quat = rot.as_quat() # [x, y, z, w] - self.odom.header.frame_id = "world" self.odom.header.stamp = self.get_clock().now().to_msg() - self.odom.child_frame_id = "base_link" + self.odom.header.frame_id = self.world_frame_id + self.odom.child_frame_id = self._frame(self.body_frame_id) self.odom.pose.pose.position.x = eta[0].item() self.odom.pose.pose.position.y = eta[1].item() @@ -248,8 +259,8 @@ def publish_odom(self): def publish_tf(self): t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() - t.header.frame_id = "world" - t.child_frame_id = "base_link" + t.header.frame_id = self.world_frame_id + t.child_frame_id = self._frame(self.body_frame_id) t.transform.translation.x = self.odom.pose.pose.position.x t.transform.translation.y = self.odom.pose.pose.position.y @@ -261,3 +272,8 @@ def publish_tf(self): self.tf_broadcaster.sendTransform(t) + def _frame(self, link: str) -> str: + """Return namespaced frame id if body_frame_id is set, else the link itself.""" + if isinstance(self.tf_prefix, str) and self.tf_prefix.strip(): + return f"{self.tf_prefix.rstrip('/')}/{link}" + return link \ No newline at end of file diff --git a/cybership_simulator/src/cybership_simulator/drillship.py b/cybership_simulator/src/cybership_simulator/drillship.py index 7f5c598..c0ce97b 100755 --- a/cybership_simulator/src/cybership_simulator/drillship.py +++ b/cybership_simulator/src/cybership_simulator/drillship.py @@ -20,7 +20,7 @@ def __init__(self): def _create_vessel(self): return shoeboxpy.model6dof.Shoebox( L=3.0, B=0.4, T=0.10, GM_theta=0.02, GM_phi=0.02, - eta0=np.array([0.0, 0.0, 0.0, 0.2, 0.2, 0.0]), + eta0=self.eta0.flatten(), ) def _init_allocator(self): @@ -116,7 +116,7 @@ def cb_bow_port_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[0] = np.clip(msg.force.x, -1.0, 1.0) self.u[1] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_port_thruster_link" + issued.header.frame_id = self._frame("bow_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -126,7 +126,7 @@ def cb_bow_center_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[2] = np.clip(msg.force.x, -1.0, 1.0) self.u[3] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_center_thruster_link" + issued.header.frame_id = self._frame("bow_center_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -136,7 +136,7 @@ def cb_bow_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[4] = np.clip(msg.force.x, -1.0, 1.0) self.u[5] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_starboard_thruster_link" + issued.header.frame_id = self._frame("bow_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -146,7 +146,7 @@ def cb_aft_port_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[6] = np.clip(msg.force.x, -1.0, 1.0) self.u[7] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "aft_port_thruster_link" + issued.header.frame_id = self._frame("aft_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -156,7 +156,7 @@ def cb_aft_center_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[8] = np.clip(msg.force.x, -1.0, 1.0) self.u[9] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "aft_center_thruster_link" + issued.header.frame_id = self._frame("aft_center_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y @@ -166,7 +166,7 @@ def cb_aft_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[10] = np.clip(msg.force.x, -1.0, 1.0) self.u[11] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "aft_starboard_thruster_link" + issued.header.frame_id = self._frame("aft_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y diff --git a/cybership_simulator/src/cybership_simulator/enterprise.py b/cybership_simulator/src/cybership_simulator/enterprise.py index 7785ec1..826b59c 100755 --- a/cybership_simulator/src/cybership_simulator/enterprise.py +++ b/cybership_simulator/src/cybership_simulator/enterprise.py @@ -18,25 +18,37 @@ def __init__(self): super().__init__(node_name="enterprise_simulator") def _create_vessel(self): + return shoeboxpy.model6dof.Shoebox( L=1.0, B=0.3, T=0.08, GM_theta=0.02, GM_phi=0.02, - eta0=np.array([0.0, 0.0, 0.0, 0.2, 0.2, 0.0]), + eta0=self.eta0.flatten(), ) def _init_allocator(self): + + # Thruster definitions are in accordance with NED frame tunnel = skadipy.actuator.Fixed( position=skadipy.toolbox.Point([0.3875, 0.0, -0.01]), orientation=skadipy.toolbox.Quaternion( axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0 ) ) + + # X = -0.4574, Y = -0.055, Z = -0.1 port_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.4574, -0.055, -0.1]), ) + # X = -0.4547, Y = 0.055, Z = -0.1 starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.4547, 0.055, -0.1]), ) - actuators = [tunnel, port_azimuth, starboard_azimuth] + + # This order is important when unpacking the command vector + actuators = [ + tunnel, + port_azimuth, + starboard_azimuth + ] dofs = [ skadipy.allocator._base.ForceTorqueComponent.X, skadipy.allocator._base.ForceTorqueComponent.Y, @@ -81,30 +93,30 @@ def setup_thrusters(self): def cb_tunnel_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[0] = msg.force.x issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_tunnel_thruster_link" + issued.header.frame_id = self._frame("bow_tunnel_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x self.publisher_tunnel_thruster.publish(issued) - def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): + def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[1] = np.clip(msg.force.x, -1.0, 1.0) self.u[2] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_starboard_thruster_link" + issued.header.frame_id = self._frame("stern_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y - self.publisher_starboard_thruster.publish(issued) + self.publisher_port_thruster.publish(issued) - def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): + def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[3] = np.clip(msg.force.x, -1.0, 1.0) self.u[4] = np.clip(msg.force.y, -1.0, 1.0) issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_port_thruster_link" + issued.header.frame_id = self._frame("stern_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x issued.wrench.force.y = msg.force.y - self.publisher_port_thruster.publish(issued) + self.publisher_starboard_thruster.publish(issued) # ---------------------------------------------------------------------------- # Main entry point diff --git a/cybership_simulator/src/cybership_simulator/voyager.py b/cybership_simulator/src/cybership_simulator/voyager.py index b6702c7..bafd2d5 100755 --- a/cybership_simulator/src/cybership_simulator/voyager.py +++ b/cybership_simulator/src/cybership_simulator/voyager.py @@ -8,6 +8,7 @@ import rclpy import geometry_msgs.msg + class VoyagerSimulator(BaseSimulator): """ Concrete simulator for the Voyager vessel. @@ -20,7 +21,7 @@ def __init__(self): def _create_vessel(self): return shoeboxpy.model6dof.Shoebox( L=1.0, B=0.3, T=0.08, GM_theta=0.02, GM_phi=0.02, - eta0=np.array([0.0, 0.0, 0.0, 0.2, 0.2, 0.0]), + eta0=self.eta0.flatten(), ) def _init_allocator(self): @@ -36,7 +37,12 @@ def _init_allocator(self): starboard_azimuth = skadipy.actuator.Azimuth( position=skadipy.toolbox.Point([-0.4547, 0.055, -0.1]), ) - actuators = [tunnel, port_azimuth, starboard_azimuth] + # This order is important when unpacking the command vector + actuators = [ + tunnel, + port_azimuth, + starboard_azimuth + ] dofs = [ skadipy.allocator._base.ForceTorqueComponent.X, skadipy.allocator._base.ForceTorqueComponent.Y, @@ -80,51 +86,51 @@ def setup_thrusters(self): # Thruster command callbacks def cb_tunnel_thruster(self, msg: geometry_msgs.msg.Wrench): self.u[0] = msg.force.x - if np.linalg.norm(self.u[0]) < 0.05: self.u[0] = 0.0 - issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "bow_tunnel_thruster_link" + issued.header.frame_id = self._frame("bow_tunnel_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() issued.wrench.force.x = msg.force.x self.publisher_tunnel_thruster.publish(issued) - def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): - self.u[1] = np.clip(msg.force.x, -1.0, 1.0) - self.u[2] = np.clip(msg.force.y, -1.0, 1.0) - + def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): + fx = np.clip(msg.force.x, -1.0, 1.0) + fy = np.clip(msg.force.y, -1.0, 1.0) # Apply a deadband to the thruster commands - if np.linalg.norm(self.u[1:3]) < 0.1: - self.u[1] = 0.0 - self.u[2] = 0.0 - + if np.linalg.norm([fx, fy]) < 0.1: + fx = 0.0 + fy = 0.0 + self.u[1] = fx + self.u[2] = fy issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_starboard_thruster_link" + issued.header.frame_id = self._frame("stern_port_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() - issued.wrench.force.x = msg.force.x - issued.wrench.force.y = msg.force.y - self.publisher_starboard_thruster.publish(issued) - - def cb_port_thruster(self, msg: geometry_msgs.msg.Wrench): - self.u[3] = np.clip(msg.force.x, -1.0, 1.0) - self.u[4] = np.clip(msg.force.y, -1.0, 1.0) - - if np.linalg.norm(self.u[3:5]) < 0.1: - self.u[3] = 0.0 - self.u[4] = 0.0 + issued.wrench.force.x = fx + issued.wrench.force.y = fy + self.publisher_port_thruster.publish(issued) + def cb_starboard_thruster(self, msg: geometry_msgs.msg.Wrench): + fx = np.clip(msg.force.x, -1.0, 1.0) + fy = np.clip(msg.force.y, -1.0, 1.0) + # Apply a deadband to the thruster commands + if np.linalg.norm([fx, fy]) < 0.1: + fx = 0.0 + fy = 0.0 + self.u[3] = fx + self.u[4] = fy issued = geometry_msgs.msg.WrenchStamped() - issued.header.frame_id = "stern_port_thruster_link" + issued.header.frame_id = self._frame("stern_starboard_thruster_link") issued.header.stamp = self.get_clock().now().to_msg() - issued.wrench.force.x = msg.force.x - issued.wrench.force.y = msg.force.y - self.publisher_port_thruster.publish(issued) + issued.wrench.force.x = fx + issued.wrench.force.y = fy + self.publisher_starboard_thruster.publish(issued) # ---------------------------------------------------------------------------- # Main entry point # ---------------------------------------------------------------------------- + def main(args=None): rclpy.init(args=args) diff --git a/cybership_teleop/CMakeLists.txt b/cybership_teleop/CMakeLists.txt index 39e6265..7056823 100644 --- a/cybership_teleop/CMakeLists.txt +++ b/cybership_teleop/CMakeLists.txt @@ -7,21 +7,7 @@ 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 - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() install( DIRECTORY diff --git a/cybership_teleop/config/allocation/ps5.yaml b/cybership_teleop/config/allocation/ps5.yaml index 9990e72..4f8035f 100644 --- a/cybership_teleop/config/allocation/ps5.yaml +++ b/cybership_teleop/config/allocation/ps5.yaml @@ -8,15 +8,15 @@ axis_mappings: force-x: axis: 3 - scale: 1.0 + scale: 3.0 offset: 0.0 force-y: axis: 2 - scale: 1.0 + scale: -3.0 offset: 0.0 torque-z: axis: 0 - scale: 1.0 + scale: -1.0 offset: 0.0 enable: @@ -25,9 +25,22 @@ service_name: thruster/enable buttons: [0] - disable: type: service interface_type: std_srvs/srv/Empty service_name: thruster/disable buttons: [1] + + joy_force_mux: + type: service + interface_type: topic_tools_interfaces/srv/MuxSelect + service_name: force_mux/select + buttons: [2] + request: {"topic": "control/force/command"} + + position_force_mux: + type: service + interface_type: topic_tools_interfaces/srv/MuxSelect + service_name: force_mux/select + buttons: [3] + request: {"topic": "control/force/command/position"} diff --git a/cybership_teleop/config/simple/ps5.yaml b/cybership_teleop/config/simple/ps5.yaml index 84d9e05..d59d41e 100644 --- a/cybership_teleop/config/simple/ps5.yaml +++ b/cybership_teleop/config/simple/ps5.yaml @@ -12,7 +12,7 @@ offset: 0.0 force-y: axis: 0 - scale: 1.0 + scale: -1.0 offset: 0.0 starboard_thruster: @@ -27,7 +27,7 @@ offset: 0.0 force-y: axis: 3 - scale: 1.0 + scale: -1.0 offset: 0.0 enable: diff --git a/cybership_teleop/launch/allocation.launch.py b/cybership_teleop/launch/allocation.launch.py index 238d156..db9edc9 100644 --- a/cybership_teleop/launch/allocation.launch.py +++ b/cybership_teleop/launch/allocation.launch.py @@ -29,11 +29,20 @@ def generate_launch_description(): description="Joystick configuration file", ) + arg_joy_id = launch.actions.DeclareLaunchArgument( + "joy_id", + default_value="0", + description="Joystick id", + ) + node_joy = Node( namespace=launch.substitutions.LaunchConfiguration("vessel_name"), package="joy", executable="game_controller_node", name=f"vessel_joy_node_{anon()}", + parameters=[ + {"device_id": launch.substitutions.LaunchConfiguration("joy_id")} + ], output="screen", ) @@ -46,6 +55,7 @@ def generate_launch_description(): parameters=[launch.substitutions.LaunchConfiguration("joy_config")], ) + ld.add_action(arg_joy_id) ld.add_action(arg_joy_config) ld.add_action(node_joy) ld.add_action(node_joy_teleop) diff --git a/cybership_teleop/launch/joy.launch.py b/cybership_teleop/launch/joy.launch.py new file mode 100644 index 0000000..efa865e --- /dev/null +++ b/cybership_teleop/launch/joy.launch.py @@ -0,0 +1,45 @@ +import launch +import launch_ros + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +from cybership_utilities.launch import anon +from cybership_utilities.launch import COMMON_ARGUMENTS as ARGUMENTS + + +def generate_launch_description(): + + ld = LaunchDescription() + + for argument in ARGUMENTS: + ld.add_action(argument) + + + + arg_joy_id = launch.actions.DeclareLaunchArgument( + "joy_id", + default_value="0", + description="Joystick id", + ) + + node_joy = Node( + namespace=launch.substitutions.LaunchConfiguration("vessel_name"), + package="joy", + executable="game_controller_node", + name=f"vessel_joy_node_{anon()}", + parameters=[ + {"device_id": launch.substitutions.LaunchConfiguration("joy_id")}, + {"deadzone": 0.1}, + {"autorepeat_rate": 20.0}, + {"coalesce_interval": 0.01}, + ], + output="screen", + ) + + + ld.add_action(arg_joy_id) + ld.add_action(node_joy) + + return ld diff --git a/cybership_tests/CMakeLists.txt b/cybership_tests/CMakeLists.txt index 8ddca9c..76ed859 100644 --- a/cybership_tests/CMakeLists.txt +++ b/cybership_tests/CMakeLists.txt @@ -8,24 +8,16 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(joy REQUIRED) -find_package(sensor_msgs REQUIRED) find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - ament_python_install_package(${PROJECT_NAME}) +# Find all python scripts under the "nodes" directory. +file(GLOB NODE_SCRIPTS ${CMAKE_CURRENT_SOURCE_DIR}/cybership_tests/*.py) + install( PROGRAMS - ${PROJECT_NAME}/four_corner_line_of_sight.py - ${PROJECT_NAME}/four_corner_client.py - ${PROJECT_NAME}/pid_position_server.py - ${PROJECT_NAME}/go_to_client.py - ${PROJECT_NAME}/reference_filter_server.py + ${NODE_SCRIPTS} DESTINATION lib/${PROJECT_NAME} ) diff --git a/cybership_tests/cybership_tests/four_corner_line_of_sight.py b/cybership_tests/cybership_tests/four_corner_line_of_sight.py deleted file mode 100755 index b549716..0000000 --- a/cybership_tests/cybership_tests/four_corner_line_of_sight.py +++ /dev/null @@ -1,286 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import Wrench -from nav_msgs.msg import Odometry -import math -import numpy as np -from scipy.spatial.transform import Rotation as R - - -def wrap_to_pi(angle): - """ - Wrap an angle in radians to the interval [-pi, pi]. - """ - return (angle + math.pi) % (2 * math.pi) - math.pi - - -class SquareMoveController(Node): - def __init__(self): - super().__init__("square_move_controller", namespace="voyager") - - # Publisher to send control commands (force and torque) - self.control_pub = self.create_publisher(Wrench, "control/force/command", 10) - - # Subscriber to receive odometry measurements - self.create_subscription(Odometry, "measurement/odom", self.odom_callback, 10) - - # Timer for periodic control update (dt = 0.01 s) - self.dt = 0.01 - self.timer = self.create_timer(self.dt, self.control_loop) - - # Latest odometry message storage - self.latest_odom = None - - # Square path parameters (in meters) - # (Example: a 3×3 square centered at (0,0).) - self.side_length = 3.0 - self.corners = [ - (0.0, 0.0), - (self.side_length, 0.0), - (self.side_length, self.side_length), - (0.0, self.side_length), - (0.0, 0.0), # Close the loop - ] - # Shift corners so the square is centered at (-1.5, -1.5): - for i in range(len(self.corners)): - self.corners[i] = (self.corners[i][0] - 1.5, self.corners[i][1] - 1.5) - - # Desired headings at each corner (radians): - self.corner_headings = [ - 0.0, # corner 0 - 0.0, # corner 1 - math.pi / 4, # corner 2 - 0.0, # corner 3 - 0.0, - ] - - # --------------------------------------- - # Controller gains (PID-like) - # --------------------------------------- - # Proportional gains - self.kp_xy = 1.0 # cross-track & along-track - self.kp_yaw = 0.6 # heading - self.kp_speed = 0.6 # forward speed - - # Integral gains - self.ki_xy = 0.1 # cross-track integrator - self.ki_yaw = 0.0 # heading integrator (set to 0 if not desired) - self.ki_speed = 0.0 # forward speed integrator (set to 0 if not desired) - - # Desired forward speed along the path - self.desired_speed = 1.0 # m/s - - # Index to track current segment start corner - self.current_waypoint_index = 0 - - # Threshold to decide when we have "arrived" near the next corner - self.segment_arrival_threshold = 0.25 - - # --------------------------------------- - # Integral error states - # --------------------------------------- - # We'll track x- and y-components of cross-track error - self.cross_x_int = 0.2 - self.cross_y_int = 0.2 - - # Integral of yaw error - self.yaw_err_int = 0.0 - - # Integral of speed error - self.speed_err_int = 0.0 - - # Maximum allowed velocities (in m/s) for the vessel - self.max_surge_velocity = 0.05 - self.max_sway_velocity = 0.1 - - self.get_logger().info( - "Square Move Controller with cross-track integral initialized." - ) - - def reset_integrators(self): - """ - Resets integral states to zero. - Typically called when you switch to a new segment. - """ - self.cross_x_int = 0.0 - self.cross_y_int = 0.0 - self.yaw_err_int = 0.0 - - def odom_callback(self, msg: Odometry): - """ - Callback to update the latest odometry measurement. - """ - self.latest_odom = msg - - def control_loop(self): - """ - Control loop that computes and publishes the control command. - Now includes integral control for cross-track, heading, and (optionally) speed. - """ - if self.latest_odom is None: - return - - # Check if the loop is complete - if self.current_waypoint_index == len(self.corners) - 1: - self.get_logger().info("Completed square path.") - # Stop the control loop - self.destroy_timer(self.timer) - return - - # Extract current position - pos = self.latest_odom.pose.pose.position - current_x = pos.x - current_y = pos.y - - # Extract current yaw - orientation = self.latest_odom.pose.pose.orientation - rot = R.from_quat([orientation.x, orientation.y, orientation.z, orientation.w]) - _, _, current_yaw = rot.as_euler("xyz", degrees=False) - - # Determine current segment from corners[i] to corners[i+1] - p1_index = self.current_waypoint_index - p2_index = (p1_index + 1) % len(self.corners) - p1 = self.corners[p1_index] - p2 = self.corners[p2_index] - - # Segment vector - dx = p2[0] - p1[0] - dy = p2[1] - p1[1] - segment_length = math.sqrt(dx * dx + dy * dy) - - # Handle degenerate case - if segment_length < 1e-6: - # Move to next segment - self.current_waypoint_index = p2_index - self.reset_integrators() - return - - # Normalized direction along the segment - ux = dx / segment_length - uy = dy / segment_length - - # Vector from p1 to current position - ex = current_x - p1[0] - ey = current_y - p1[1] - - # Along-track distance via dot product - along_track_dist = ex * ux + ey * uy - - # Check if we're close enough to the end of the segment - if along_track_dist >= segment_length - self.segment_arrival_threshold: - self.current_waypoint_index = p2_index - self.reset_integrators() - return - - # Clamp if below 0 - if along_track_dist < 0.0: - along_track_dist = 0.0 - - # Nearest point on the segment - nearest_x = p1[0] + along_track_dist * ux - nearest_y = p1[1] + along_track_dist * uy - - # Cross-track error (vector from nearest point to current position) - cross_x = current_x - nearest_x - cross_y = current_y - nearest_y - - # --------------------------------------- - # Update integrators - # --------------------------------------- - # Integrate cross-track error in x and y - self.cross_x_int += cross_x * self.dt - self.cross_y_int += cross_y * self.dt - - # Heading - desired_yaw = self.corner_headings[p1_index] - yaw_error = wrap_to_pi(desired_yaw - current_yaw) - self.yaw_err_int += yaw_error * self.dt - - # Speed integrator - # velocity in global frame - vx = self.latest_odom.twist.twist.linear.x - vy = self.latest_odom.twist.twist.linear.y - # along-track velocity - v_along = vx * ux + vy * uy - speed_err = self.desired_speed - v_along - self.speed_err_int += speed_err * self.dt - - # --------------------------------------- - # Compute Control (P + I) - # --------------------------------------- - # Cross-track control: push back toward the path - # tau_ct = -(kp*error + ki*integral_of_error) - tau_ct_x = -(self.kp_xy * cross_x + self.ki_xy * self.cross_x_int) - tau_ct_y = -(self.kp_xy * cross_y + self.ki_xy * self.cross_y_int) - # Along-track control for speed - # (kp*speed_err + ki*integral_of_speed_err) in direction of segment - speed_control = self.kp_speed * speed_err + self.ki_speed * self.speed_err_int - tau_along_x = speed_control * ux - tau_along_y = speed_control * uy - - # Combine cross-track + along-track - tau_x = tau_ct_x + tau_along_x - tau_y = tau_ct_y + tau_along_y - - # Heading control - # tau_yaw = kp_yaw * yaw_err + ki_yaw * yaw_err_int - tau_yaw = self.kp_yaw * yaw_error + self.ki_yaw * self.yaw_err_int - - # --- Velocity Limiting --- - # Assume the odometry twist is expressed in the vessel's body frame. - current_surge_velocity = self.latest_odom.twist.twist.linear.x - current_sway_velocity = self.latest_odom.twist.twist.linear.y - - # Limit surge force - if abs(current_surge_velocity) > self.max_surge_velocity: - scaling_factor = self.max_surge_velocity / abs(current_surge_velocity) - self.get_logger().debug( - f"Surge velocity ({current_surge_velocity:.2f} m/s) exceeds max. Scaling force_x by {scaling_factor:.2f}" - ) - tau_x *= scaling_factor - - # Limit sway force - if abs(current_sway_velocity) > self.max_sway_velocity: - scaling_factor = self.max_sway_velocity / abs(current_sway_velocity) - self.get_logger().debug( - f"Sway velocity ({current_sway_velocity:.2f} m/s) exceeds max. Scaling force_y by {scaling_factor:.2f}" - ) - tau_y *= scaling_factor - - wrench_msg = Wrench() - wrench_msg.force.x = float(tau_x) - wrench_msg.force.y = float(tau_y) - wrench_msg.force.z = 0.0 - wrench_msg.torque.x = 0.0 - wrench_msg.torque.y = 0.0 - wrench_msg.torque.z = float(tau_yaw) - self.control_pub.publish(wrench_msg) - - cross_track_error_mag = math.sqrt(cross_x**2 + cross_y**2) - self.get_logger().info( - f"Pos=({current_x:.2f}, {current_y:.2f}), " - f"Yaw={math.degrees(current_yaw):.1f} deg, " - f"dYaw={(math.degrees(yaw_error)):.1f} deg, " - f"SegDist={along_track_dist:.2f}/{segment_length:.2f}, " - f"XTE={cross_track_error_mag:.3f}, " - f"Fx={tau_x:.2f}, Fy={tau_y:.2f}, Tz={tau_yaw:.2f}" - f"SurgeVel={current_surge_velocity:.2f}, SwayVel={current_sway_velocity:.2f}, " - ) - - -def main(args=None): - rclpy.init(args=args) - node = SquareMoveController() - try: - rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Square Move Controller stopped by user.") - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/cybership_tests/cybership_tests/mission.py b/cybership_tests/cybership_tests/mission.py new file mode 100755 index 0000000..2a07fcb --- /dev/null +++ b/cybership_tests/cybership_tests/mission.py @@ -0,0 +1,715 @@ +#!/usr/bin/env python3 + + +import rclpy +import rclpy.client +from rclpy.node import Node +from std_srvs.srv import SetBool +from topic_tools_interfaces.srv import MuxSelect, MuxAdd +from geometry_msgs.msg import PoseStamped, Twist, Wrench +from std_msgs.msg import Bool +from nav_msgs.msg import Odometry +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose +from tf_transformations import quaternion_from_euler +import numpy as np +from typing import Any, Dict, Optional, List +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from abc import ABC, abstractmethod +import time + + +# Topic and service name constants +TOPIC_FORCE_CONSTANT: str = "control/force/command/constant" +TOPIC_VELOCITY_COMMAND: str = "control/velocity/command" +TOPIC_ODOM: str = "measurement/odom" +TOPIC_POSE_GOAL: str = "navigate_to_pose" +TOPIC_FORCE_POSITION: str = "control/force/command/position" +TOPIC_FORCE_VELOCITY: str = "control/force/command/velocity" +TOPIC_FORCE_BASE: str = "control/force/command" + +SERVICE_MUX_ADD: str = "force_mux/add" +SERVICE_MUX_SELECT: str = "force_mux/select" +SERVICE_POSE_ENABLE: str = "position_controller/change_state" +SERVICE_VEL_ENABLE: str = "velocity_controller/change_state" + +TOPIC_EXPERIMENT_ODOM: str = "experiment/odom" +TOPIC_EXPERIMENT_FORCE: str = "experiment/force" +TOPIC_EXPERIMENT_FLAG: str = "experiment/flag" + + +class ActionRunner(Node): + """ + ActionRunner executes a predefined sequence of navigation, velocity, force, and wait actions + on a ROS2 node, monitoring odometry for abort conditions. + + Attributes: + repeat (int): Number of times to repeat the full action sequence. + current_odom (Odometry): Latest odometry message for pose feedback. + """ + + def __init__(self): + """ + Initialize the ActionRunner node, including publishers, subscribers, action and service clients. + + Sets up: + - Mux for force control topics + - Publishers for velocity (Twist) and force (Wrench) commands + - Subscriber for odometry (Odometry) + - ActionClient for NavigateToPose + - Service clients for enabling/disabling pose and velocity controllers + """ + super().__init__("action_runner", namespace="voyager") + # Private executor for service calls + # self._executor.add_node(self) + # Define the list of mission actions + self.add_mux(TOPIC_FORCE_CONSTANT) + + self.repeat = 500 + + self.vel_pub = self.create_publisher(Twist, TOPIC_VELOCITY_COMMAND, 10) + self.force_pub = self.create_publisher(Wrench, TOPIC_FORCE_CONSTANT, 10) + + self.experiment_odom_pub = self.create_publisher( + Odometry, TOPIC_EXPERIMENT_ODOM, 10 + ) + self.experiment_force_pub = self.create_publisher( + Wrench, TOPIC_EXPERIMENT_FORCE, 10 + ) + self.experiment_flag_pub = self.create_publisher( + Bool, TOPIC_EXPERIMENT_FLAG, 10 + ) + + self.current_odom = None + self.create_subscription(Odometry, TOPIC_ODOM, self.odom_callback, 10) + + # Action client for pose navigation (relative to node namespace) + self.pose_action_client = ActionClient(self, NavigateToPose, TOPIC_POSE_GOAL) + self.get_logger().info(f"Waiting for {self.pose_action_client._action_name} action server...") + self.pose_action_client.wait_for_server() + + self.pose_enable_client = self.create_client(SetBool, SERVICE_POSE_ENABLE) + self.get_logger().info(f"Waiting for server {self.pose_enable_client.service_name}...") + self.pose_enable_client.wait_for_service() + + self.vel_enable_client = self.create_client(SetBool, SERVICE_VEL_ENABLE) + self.get_logger().info(f"Waiting for server {self.vel_enable_client.service_name}...") + self.vel_enable_client.wait_for_service() + + self.rng = np.random.default_rng() + + self.wait_until_ready() + + # Experiment publishing state + self.experiment_enabled: bool = False + self._last_force: Optional[Wrench] = None + self.create_timer(0.1, self._experiment_publisher) + + + # Default abort lands back at origin after timeout + self.default_abort_action = { + "name": "default_abort", + "action": self.run_pose_action, + "parameters": { + "x": 0.0, + "y": 0.0, + "yaw": 0.0, + "duration": 60.0, + "grace": 15.0, + }, + } + + self.actions = [ + { + "name": "pose_neg_diag", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": -2.0, + "yaw": np.pi / 4, + "duration": 60.0, + }, + }, + { + "name": "velocity_random_1", + "action": self.run_velocity_action, + "parameters": { + # Sample linear and angular components each publish + "linear_x": lambda: self.rng.uniform(0.4, 0.8), + "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + "angular_z": lambda: self.rng.uniform(-0.01, 0.01), + "duration": 30.0, + "margin": 0.1, # Allow some margin for early success + }, + "abort_action": { + "name": "velocity_random_1_abort", + "action": self.run_pose_action, + "parameters": { + "x": 2.0, + "y": 2.0, + "yaw": 5 * np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, + }, + }, + { + "name": "force_random_1", + "action": self.run_force_action, + "parameters": { + "force_x": lambda: self.rng.uniform(0.2, 0.8), + "force_y": lambda: self.rng.uniform(-0.5, 0.5), + "torque_z": lambda: self.rng.uniform(-0.1, 0.1), + "duration": 30.0, + "experiment": True, # Enable experiment flag + "continuous_sampling": True, # Sample force continuously + "frequency": 1.0, # Hz + }, + "abort_action": { + "name": "force_random_1_abort", + "action": self.run_pose_action, + "parameters": { + "x": 2.0, + "y": 2.0, + "yaw": 5 * np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, + }, + }, + { + "name": "pose_pos_diag", + "action": self.run_pose_action, + "parameters": { + "x": 2.0, + "y": 2.0, + "yaw": 5 * np.pi / 4, + "duration": 60.0, + }, + }, + { + "name": "velocity_random_2", + "action": self.run_velocity_action, + "parameters": { + # Sample linear and angular components each publish + "linear_x": lambda: self.rng.uniform(0.2, 0.8), + "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + "angular_z": lambda: self.rng.uniform(-0.01, 0.01), + "duration": 30.0, + "margin": 0.1, # Allow some margin for early success + }, + "abort_action": { + "name": "velocity_random_2_abort", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": -2.0, + "yaw": np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, + }, + }, + { + "name": "force_random_2", + "action": self.run_force_action, + "parameters": { + "force_x": lambda: self.rng.uniform(0.2, 0.8), + "force_y": lambda: self.rng.uniform(-0.5, 0.5), + "torque_z": lambda: self.rng.uniform(-0.1, 0.1), + "duration": 30.0, + "experiment": True, # Enable experiment flag + "continuous_sampling": True, # Sample force continuously + "frequency": 1.0, # Hz + }, + "abort_action": { + "name": "force_random_2_abort", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": -2.0, + "yaw": np.pi / 4, + "duration": 60.0, + "grace": 15.0, + }, + }, + }, + { + "name": "wait", + "action": self.run_wait_action, + "parameters": {"duration": 10.0}, + }, + ] + + def wait_until_ready(self) -> None: + """ + Block until the first odometry message is received. + + Ensures the vehicle has a valid initial pose before performing actions. + """ + + while rclpy.ok() and self.current_odom is None: + self.get_logger().info("Waiting for initial odometry data...") + # process callbacks without blocking executor + rclpy.spin_once(self, timeout_sec=0.1) + time.sleep(0.5) + self.get_logger().info("Initial odometry data received.") + + def _experiment_publisher(self) -> None: + """ + Background thread publishes experiment flag, and when enabled, odometry and force data. + """ + flag_msg = Bool() + flag_msg.data = self.experiment_enabled + self.experiment_flag_pub.publish(flag_msg) + if self.experiment_enabled: + if self.current_odom: + self.experiment_odom_pub.publish(self.current_odom) + if self._last_force: + self.experiment_force_pub.publish(self._last_force) + + + def add_mux(self, topic: str) -> None: + """ + Add a topic to the force multiplexer using MuxAdd service. + + Args: + topic (str): ROS topic name to add to the mux. + """ + req = MuxAdd.Request() + mux_add_client = self.create_client(MuxAdd, SERVICE_MUX_ADD) + req.topic = topic + future = mux_add_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + # MuxAdd returns a result with a success flag + if future.result().success: + self.get_logger().info(f"Mux added topic {topic}") + else: + self.get_logger().error(f"Failed to add mux topic {topic}") + + def abort_condition(self) -> bool: + """ + Determine if the mission should abort based on odometry limits. + + Returns: + bool: True if |x| or |y| position exceeds 2.5 meters, False otherwise. + """ + return ( + abs(self.current_odom.pose.pose.position.x) > 2.5 + or abs(self.current_odom.pose.pose.position.y) > 2.5 + ) + + def call_mux(self, topic: str) -> None: + """ + Select the active force control topic on the mux. + + Args: + topic (str): ROS topic name to select for force commands. + """ + mux_client = self.create_client(MuxSelect, SERVICE_MUX_SELECT) + mux_client.wait_for_service() + self.get_logger().info(f'Calling mux select for topic {topic}') + req = MuxSelect.Request() + req.topic = topic + future = mux_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result(): + self.get_logger().info(f"Mux switched to {topic}") + else: + self.get_logger().error(f"Failed to switch mux to {topic}") + + def set_enable(self, client: rclpy.client.Client, enable: bool) -> None: + """ + Enable or disable a controller via SetBool service. + + Args: + client (rclpy.client.Client): Service client for SetBool. + enable (bool): True to enable, False to disable. + """ + self.get_logger().info(f"Setting {client.service_name} enable={enable}") + req = SetBool.Request() + req.data = enable + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result().success: + self.get_logger().info(f'Service {client.service_name} set enable={enable}') + else: + self.get_logger().error(f'Failed to set {client.service_name} enable={enable}') + + def _pose_feedback_callback(self, feedback_msg: Any) -> None: + """ + Callback to log remaining distance from a NavigateToPose action. + + Args: + feedback_msg: Action feedback message containing distance_remaining (float, meters). + """ + fb = feedback_msg.feedback + # nav2 NavigateToPose feedback has distance_remaining + dist = getattr(fb, "distance_remaining", None) + if dist is not None: + self.get_logger().info( + f" NavigateToPose feedback: distance_remaining={dist:.2f}" + ) + else: + self.get_logger().info(" NavigateToPose feedback received") + + def run_pose_action(self, **params: Any) -> None: + """ + Navigate to a target pose and monitor for timeouts or abort conditions. + + Sends a NavigateToPose goal and cancels if duration elapsed or abort condition met. + + Args: + x (float): Target x-position in meters. + y (float): Target y-position in meters. + yaw (float): Target yaw angle in radians. + duration (float): Max navigation time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + # Activate pose controller via mux & service + self.call_mux(TOPIC_FORCE_POSITION) + + self.set_enable(self.pose_enable_client, True) + + # Extract parameters + x = resolved_params.get("x", 0.0) + y = resolved_params.get("y", 0.0) + yaw = resolved_params.get("yaw", 0.0) + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + self.get_logger().info( + f"Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), " + f"duration={duration:.1f}s, grace={grace:.1f}s" + ) + + # Build goal message + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "world" + msg.pose.position.x = x + msg.pose.position.y = y + q = quaternion_from_euler(0.0, 0.0, yaw) + msg.pose.orientation.x = q[0] + msg.pose.orientation.y = q[1] + msg.pose.orientation.z = q[2] + msg.pose.orientation.w = q[3] + goal = NavigateToPose.Goal() + goal.pose = msg + + # Send goal with feedback callback + goal_fut = self.pose_action_client.send_goal_async( + goal, feedback_callback=self._pose_feedback_callback + ) + rclpy.spin_until_future_complete(self, goal_fut) + handle = goal_fut.result() + if not handle.accepted: + self.get_logger().error("NavigateToPose goal rejected") + self.set_enable(self.pose_enable_client, False) + return + + self.get_logger().info("NavigateToPose goal accepted, monitoring execution") + + # Wait for result, checking abort & timeout + result_fut = handle.get_result_async() + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + while rclpy.ok(): + elapsed = (self.get_clock().now().nanoseconds / 1e9) - start + + if elapsed >= duration: + self.get_logger().info( + "NavigateToPose action completed due to duration" + ) + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + break + + if result_fut.done(): + self.get_logger().info("NavigateToPose action completed") + break + + if elapsed >= grace and self.abort_condition(): + + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + raise RuntimeError('Abort condition met during pose action, aborting') + + rclpy.spin_once(self, timeout_sec=0.5) + + # Deactivate pose controller + self.set_enable(self.pose_enable_client, False) + + def run_velocity_action(self, **params: Any) -> None: + """ + Publish constant velocity commands for a given duration, with optional early stop. + + Args: + linear_x (float): Desired linear velocity in x (m/s). + linear_y (float): Desired linear velocity in y (m/s). + angular_z (float): Desired angular velocity around z (rad/s). + duration (float): Total time to publish commands (seconds). + margin (float, optional): Early stop threshold for actual vs commanded velocity (m/s). + grace (float): Grace period before starting abort checks (seconds). + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + # Activate velocity controller + self.call_mux(TOPIC_FORCE_VELOCITY) + self.set_enable(self.vel_enable_client, True) + + # Prepare and log command + msg = Twist() + lx = resolved_params.get("linear_x", 0.0) + ly = resolved_params.get("linear_y", 0.0) + az = resolved_params.get("angular_z", 0.0) + margin = resolved_params.get("margin", None) + self.get_logger().info( + " " + f"Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})" + + (f", margin={margin:.4f}" if margin is not None else "") + ) + + # Extract duration, grace period and start time + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + # Loop at ~10 Hz, processing callbacks without blocking + period = 0.1 + + + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + # Publish the commanded velocity + msg.linear.x = lx + msg.linear.y = ly + msg.angular.z = az + self.vel_pub.publish(msg) + + # Feedback: log current odometry state + if self.current_odom: + pos = self.current_odom.pose.pose.position + lvel = self.current_odom.twist.twist.linear + avel = self.current_odom.twist.twist.angular + self.get_logger().info( + " " + f"Velocity feedback: " + f"vel=(vx={lvel.x:.2f}, vy={lvel.y:.2f}, vr={avel.z:.2f}), " + f"cmd=(u={lx:.2f}, v={ly:.2f}, r={az:.2f})" + ) + + # margin-based early success + if margin is not None: + act = self.current_odom.twist.twist + if abs(act.linear.x - lx) < margin and abs(act.linear.y - ly) < margin: + self.get_logger().info("Velocity within margin, stopping early") + break + + # Abort check after grace period + if self.current_odom and elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during velocity action, aborting" + ) + time.sleep(0.1) + rclpy.spin_once(self, timeout_sec=period) + + # Deactivate velocity controller + self.set_enable(self.vel_enable_client, False) + + def run_wait_action(self, **params: Any) -> None: + """ + Wait for a specified duration while processing ROS callbacks. + + This loop periodically checks for an abort condition after an optional grace period + and logs odometry feedback during the wait. + + Args: + duration (float): Total wait time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + + self.get_logger().info(f"Waiting for {duration:.4f} seconds") + elapsed = 0.0 + # Loop at ~2 Hz, processing callbacks without blocking + period = 0.5 + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError("Abort condition met during wait action, aborting") + + # allow callbacks and enforce loop timing + rclpy.spin_once(self, timeout_sec=period) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + def run_force_action(self, **params: Any) -> None: + """ + Apply a constant force to the vehicle for a specified duration. + + This loop publishes a Wrench message at fixed intervals and checks for abort + conditions after an optional grace period. Odometry feedback is logged each cycle. + + Args: + force_x (float): Force in x-direction in Newtons. + force_y (float): Force in y-direction in Newtons. + torque_z (float): Torque about z-axis in Newton-meters. + duration (float): Total time to apply force in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + + resolved_params = {k: v() if callable(v) else v for k, v in params.items()} + + self.call_mux(TOPIC_FORCE_CONSTANT) + + msg = Wrench() + + duration = params.get("duration", 0.0) + grace = params.get("grace", 0.0) + + continuous_sampling = params.get("continuous_sampling", False) + frequency = params.get("frequency", 1.0) # Hz + + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + + # Loop at specified frequency (or 2 Hz), processing callbacks without blocking + period = 1.0 / (frequency if continuous_sampling else 2.0) + + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + msg.force.x = resolved_params.get("force_x", 0.0) + msg.force.y = resolved_params.get("force_y", 0.0) + msg.torque.z = resolved_params.get("torque_z", 0.0) + + if continuous_sampling: + # Sample force parameters at each iteration + msg.force.x = params.get("force_x", 0.0)() + msg.force.y = params.get("force_y", 0.0)() + msg.torque.z = params.get("torque_z", 0.0)() + + # record last force for experiment publishing + self._last_force = msg + self.force_pub.publish(msg) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Force feedback: (fx={msg.force.x:.2f}, " + f"fy={msg.force.y:.2f}, tz={msg.torque.z:.2f})," + f" pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError("Abort condition met during force action, aborting") + + time.sleep(period) # Sleep for the period to control frequency + # allow callbacks and enforce publish rate + rclpy.spin_once(self) + + def execute_action(self, action: Dict[str, Any]) -> None: + """ + Execute one mission action and resolve its parameters. + + Normalizes callable parameters, disables other controllers, and invokes the action. + + Args: + action (dict): Action descriptor with keys 'action' (callable) and 'parameters' (dict). + """ + if not isinstance(action, dict): + self.get_logger().error(f'Invalid action format: {action}') + return + + runner = action.get('action') + params = action.get('parameters', {}) + + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) + + if not callable(runner): + self.get_logger().warn(f'Unknown action: {runner}') + return + + self.get_logger().info(f"Executing action: {action['name']}") + + # Handle experiment flag without resolving other parameters + self.experiment_enabled = bool(params.get("experiment", False)) + runner(**params) + + # Disable experiment after action completes + self.experiment_enabled = False + + def execute_actions(self) -> None: + """ + Loop through the action sequence `repeat` times, handling aborts. + + For each action, calls execute_action and on RuntimeError runs an abort_action. + """ + for _ in range(self.repeat): + + for action in self.actions: + + try: + self.execute_action(action) + except RuntimeError as e: + abort_action = action.get('abort_action', self.default_abort_action) + self.get_logger().warn(f'Error executing action {action["name"]}: {e}') + self.get_logger().warn(f'Executing abort action: {abort_action["name"]}') + try: + self.execute_action(abort_action) + except RuntimeError as abort_e: + self.get_logger().error(f'Abort action failed too!: {abort_e}') + self.set_enable(self.pose_enable_client, False, 'pose') + self.set_enable(self.vel_enable_client, False, 'velocity') + self.call_mux(TOPIC_FORCE_BASE) + exit(1) + + + def odom_callback(self, msg): + """ + Callback for Odometry subscription; stores the latest message. + + Args: + msg (Odometry): Received odometry message containing pose and twist. + """ + self.current_odom = msg + + +def main(args: Optional[List[str]] = None) -> None: + rclpy.init(args=args) + node = ActionRunner() + + executor = SingleThreadedExecutor() + executor.add_node(node) + + node.execute_actions() + executor.spin() + executor.shutdown() + +if __name__ == "__main__": + main() diff --git a/cybership_tests/cybership_tests/mission_1.py b/cybership_tests/cybership_tests/mission_1.py new file mode 100755 index 0000000..cfa963a --- /dev/null +++ b/cybership_tests/cybership_tests/mission_1.py @@ -0,0 +1,804 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + + +import rclpy +import rclpy.client +from rclpy.node import Node +from std_srvs.srv import SetBool +from topic_tools_interfaces.srv import MuxSelect, MuxAdd +from geometry_msgs.msg import PoseStamped, Twist, Wrench +from std_msgs.msg import Bool +from nav_msgs.msg import Odometry +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose +from tf_transformations import quaternion_from_euler +import numpy as np +from typing import Any, Dict, Optional, List +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from abc import ABC, abstractmethod +import time + + +# Topic and service name constants +TOPIC_FORCE_CONSTANT: str = "control/force/command/constant" +TOPIC_VELOCITY_COMMAND: str = "control/velocity/command" +TOPIC_ODOM: str = "measurement/odom" +TOPIC_POSE_GOAL: str = "navigate_to_pose" +TOPIC_FORCE_POSITION: str = "control/force/command/position" +TOPIC_FORCE_VELOCITY: str = "control/force/command/velocity" +TOPIC_FORCE_BASE: str = "control/force/command" + +SERVICE_MUX_ADD: str = "force_mux/add" +SERVICE_MUX_SELECT: str = "force_mux/select" +SERVICE_POSE_ENABLE: str = "position_controller/change_state" +SERVICE_VEL_ENABLE: str = "velocity_controller/change_state" + +TOPIC_EXPERIMENT_ODOM: str = "experiment/odom" +TOPIC_EXPERIMENT_FORCE: str = "experiment/force" +TOPIC_EXPERIMENT_FLAG: str = "experiment/flag" + + +class ActionRunner(Node): + def __init__(self): + super().__init__("action_runner", namespace="voyager") + # Private executor for service calls + # self._executor.add_node(self) + # Define the list of mission actions + self.repeat = 500 + + self.mux_add_client = self.create_client(MuxAdd, SERVICE_MUX_ADD) + + self.mux_select_client = self.create_client( + MuxSelect, SERVICE_MUX_SELECT) + + self.pose_action_client = ActionClient( + self, NavigateToPose, TOPIC_POSE_GOAL) + + self.pose_enable_client = self.create_client( + SetBool, SERVICE_POSE_ENABLE) + + self.vel_enable_client = self.create_client( + SetBool, SERVICE_VEL_ENABLE) + + self.vel_pub = self.create_publisher(Twist, TOPIC_VELOCITY_COMMAND, 10) + self.force_pub = self.create_publisher( + Wrench, TOPIC_FORCE_CONSTANT, 10) + + self.experiment_odom_pub = self.create_publisher( + Odometry, TOPIC_EXPERIMENT_ODOM, 10 + ) + self.experiment_force_pub = self.create_publisher( + Wrench, TOPIC_EXPERIMENT_FORCE, 10 + ) + self.experiment_flag_pub = self.create_publisher( + Bool, TOPIC_EXPERIMENT_FLAG, 10 + ) + + self.current_odom = None + self.create_subscription(Odometry, TOPIC_ODOM, self.odom_callback, 10) + + self.rng = np.random.default_rng() + + # Experiment publishing state + self.experiment_enabled: bool = False + self._last_force: Optional[Wrench] = None + self.create_timer(0.05, self._experiment_publisher) + + # Default abort lands back at origin after timeout + self.default_abort_action = { + "name": "default_abort", + "action": self.run_pose_action, + "parameters": { + "x": -2.0, + "y": 0.0, + "yaw": 0.0, + "duration": 50.0, + "grace": 50.0, + }, + } + + self.actions = [ + # i want boat in same starting position each time. + { + "name": "pose_neg_diag", + "action": self.run_pose_action, + "parameters": { + "x": -3.0, + "y": 0.5, + "yaw": 0.0, + "duration": 60.0, + }, + }, + # command constant forces + # { + # "name": "command_constant_force", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0.7, 1.3), + # "force_y": lambda: self.rng.uniform(-0.0, 0.0), + # "torque_z": lambda: self.rng.uniform(-0.0, 0.0), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 0.25, # Hz + # } + # }, + # { + # "name": "pose_pos_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": 3.0, + # "y": 0.0, + # "yaw": np.pi, + # "duration": 60.0, + # }, + # }, + # command constant forces + { + "name": "command_constant_force", + "action": self.run_force_action, + "parameters": { + "force_x": lambda: self.rng.uniform(0.7, 1.0), + "force_y": lambda: self.rng.uniform(-0.05, 0.05), + "torque_z": lambda: self.rng.uniform(-0.05, 0.05), + "duration": 20.0, + "experiment": True, # Enable experiment flag + "continuous_sampling": False, # Sample force continuously + "frequency": 0.25, # Hz + } + } + # { + # "name": "pose_neg_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # }, + # }, + # { + # "name": "velocity_random_1", + # "action": self.run_velocity_action, + # "parameters": { + # # Sample linear and angular components each publish + # "linear_x": lambda: self.rng.uniform(0, 0.3), + # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), + # "duration": 15.0, + # "margin": 0.01, # Allow some margin for early success + # }, + # "abort_action": { + # "name": "velocity_random_1_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "force_random_1", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0.5, 1.5), + # "force_y": lambda: self.rng.uniform(-1.5, 1.5), + # "torque_z": lambda: self.rng.uniform(-1.5, 1.5), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 1.0, # Hz + # }, + # "abort_action": { + # "name": "force_random_1_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "pose_pos_diag", + # "action": self.run_pose_action, + # "parameters": { + # "x": 2.0, + # "y": 2.0, + # "yaw": 5 * np.pi / 4, + # "duration": 60.0, + # }, + # }, + # { + # "name": "velocity_random_2", + # "action": self.run_velocity_action, + # "parameters": { + # # Sample linear and angular components each publish + # "linear_x": lambda: self.rng.uniform(0, 0.3), + # "linear_y": lambda: self.rng.uniform(-0.1, 0.1), + # "angular_z": lambda: self.rng.uniform(-0.1, 0.1), + # "duration": 15.0, + # "margin": 0.01, # Allow some margin for early success + # }, + # "abort_action": { + # "name": "velocity_random_2_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "force_random_2", + # "action": self.run_force_action, + # "parameters": { + # "force_x": lambda: self.rng.uniform(0, 0.2), + # "force_y": lambda: self.rng.uniform(0, 0.05), + # "torque_z": lambda: self.rng.uniform(0, 0.05), + # "duration": 20.0, + # "experiment": True, # Enable experiment flag + # "continuous_sampling": True, # Sample force continuously + # "frequency": 1.0, # Hz + # }, + # "abort_action": { + # "name": "force_random_2_abort", + # "action": self.run_pose_action, + # "parameters": { + # "x": -2.0, + # "y": -2.0, + # "yaw": np.pi / 4, + # "duration": 60.0, + # "grace": 15.0, + # }, + # }, + # }, + # { + # "name": "wait", + # "action": self.run_wait_action, + # "parameters": {"duration": 10.0}, + # }, + ] + + def call_init_services(self) -> None: + # Private executor for service calls + # self._executor.add_node(self) + # Define the list of mission actions + + # Action client for pose navigation (relative to node namespace) + self.get_logger().info(f"Waiting for pose action server...") + self.pose_action_client.wait_for_server() + + self.get_logger().info(f"Waiting for pose enable server ...") + self.pose_enable_client.wait_for_service() + + self.get_logger().info(f"Waiting for vel enables server ...") + self.vel_enable_client.wait_for_service() + + self.get_logger().info(f"Waiting for Force mux server...") + self.mux_add_client.wait_for_service() + + self.get_logger().info(f"Waiting for Force mux server...") + self.mux_select_client.wait_for_service() + + self.add_mux(TOPIC_FORCE_CONSTANT) + + self.wait_until_ready() + + def wait_until_ready(self) -> None: + """ + Block until the first odometry message is received. + + Ensures the vehicle has a valid initial pose before performing actions. + """ + + while rclpy.ok() and self.current_odom is None: + self.get_logger().info("Waiting for initial odometry data...") + # process callbacks without blocking executor + rclpy.spin_once(self, timeout_sec=0.1) + time.sleep(0.5) + self.get_logger().info("Initial odometry data received.") + + def _experiment_publisher(self) -> None: + """ + Background thread publishes experiment flag, and when enabled, odometry and force data. + """ + flag_msg = Bool() + flag_msg.data = self.experiment_enabled + self.experiment_flag_pub.publish(flag_msg) + if self.experiment_enabled: + if self.current_odom: + self.experiment_odom_pub.publish(self.current_odom) + if self._last_force: + self.experiment_force_pub.publish(self._last_force) + + def add_mux(self, topic: str) -> None: + """ + Add a topic to the force multiplexer using MuxAdd service. + + Args: + topic (str): ROS topic name to add to the mux. + """ + req = MuxAdd.Request() + req.topic = topic + future = self.mux_add_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + # MuxAdd returns a result with a success flag + if future.result().success: + self.get_logger().info(f"Mux added topic {topic}") + else: + self.get_logger().error(f"Failed to add mux topic {topic}") + + def abort_condition(self) -> bool: + """ + Determine if the mission should abort based on odometry limits. + + Returns: + bool: True if |x| or |y| position exceeds 2.5 meters, False otherwise. + """ + + return ( + not( + (-5.0 < self.current_odom.pose.pose.position.x < 4.0) + and (-3.0 < self.current_odom.pose.pose.position.y < 2.0) + ) + ) + + def call_mux(self, topic: str) -> None: + """ + Select the active force control topic on the mux. + + Args: + topic (str): ROS topic name to select for force commands. + """ + self.get_logger().info(f'Calling mux select for topic {topic}') + req = MuxSelect.Request() + req.topic = topic + future = self.mux_select_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result(): + self.get_logger().info(f"Mux switched to {topic}") + else: + self.get_logger().error(f"Failed to switch mux to {topic}") + + def set_enable(self, client: rclpy.client.Client, enable: bool) -> None: + """ + Enable or disable a controller via SetBool service. + + Args: + client (rclpy.client.Client): Service client for SetBool. + enable (bool): True to enable, False to disable. + """ + self.get_logger().info(f"Setting {client} enable={enable}") + req = SetBool.Request() + req.data = enable + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result().success: + self.get_logger().info(f'Service {client} set enable={enable}') + else: + self.get_logger().error(f'Failed to set {client} enable={enable}') + + def _pose_feedback_callback(self, feedback_msg: Any) -> None: + """ + Callback to log remaining distance from a NavigateToPose action. + + Args: + feedback_msg: Action feedback message containing distance_remaining (float, meters). + """ + fb = feedback_msg.feedback + # nav2 NavigateToPose feedback has distance_remaining + dist = getattr(fb, "distance_remaining", None) + if dist is not None: + self.get_logger().info( + f" NavigateToPose feedback: distance_remaining={dist:.2f}" + ) + else: + self.get_logger().info(" NavigateToPose feedback received") + + def run_pose_action(self, **params: Any) -> None: + """ + Navigate to a target pose and monitor for timeouts or abort conditions. + + Sends a NavigateToPose goal and cancels if duration elapsed or abort condition met. + + Args: + x (float): Target x-position in meters. + y (float): Target y-position in meters. + yaw (float): Target yaw angle in radians. + duration (float): Max navigation time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + # Activate pose controller via mux & service + self.call_mux(TOPIC_FORCE_POSITION) + + self.set_enable(self.pose_enable_client, True) + + # Extract parameters + x = resolved_params.get("x", 0.0) + y = resolved_params.get("y", 0.0) + yaw = resolved_params.get("yaw", 0.0) + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + self.get_logger().info( + f"Pose action start (x={x:.2f}, y={y:.2f}, yaw={yaw:.2f}), " + f"duration={duration:.1f}s, grace={grace:.1f}s" + ) + + # Build goal message + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + # Use the same frame as odometry to avoid frame mismatch in the server's + # distance_remaining calculation. If your stack uses TF, you can send in + # any frame and transform on the server side instead. + msg.header.frame_id = "odom" + msg.pose.position.x = x + msg.pose.position.y = y + q = quaternion_from_euler(0.0, 0.0, yaw) + msg.pose.orientation.x = q[0] + msg.pose.orientation.y = q[1] + msg.pose.orientation.z = q[2] + msg.pose.orientation.w = q[3] + goal = NavigateToPose.Goal() + goal.pose = msg + + # Send goal with feedback callback + goal_fut = self.pose_action_client.send_goal_async( + goal, feedback_callback=self._pose_feedback_callback + ) + rclpy.spin_until_future_complete(self, goal_fut) + handle = goal_fut.result() + if not handle.accepted: + self.get_logger().error("NavigateToPose goal rejected") + self.set_enable(self.pose_enable_client, False) + return + + self.get_logger().info("NavigateToPose goal accepted, monitoring execution") + + # Wait for result, checking abort & timeout + result_fut = handle.get_result_async() + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + while rclpy.ok(): + elapsed = (self.get_clock().now().nanoseconds / 1e9) - start + + if elapsed >= duration: + self.get_logger().info( + "NavigateToPose action completed due to duration" + ) + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + break + + if result_fut.done(): + self.get_logger().info("NavigateToPose action completed") + break + + if elapsed >= grace and self.abort_condition(): + + cancel_fut = handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_fut) + self.set_enable(self.pose_enable_client, False) + raise RuntimeError( + 'Abort condition met during pose action, aborting') + + rclpy.spin_once(self, timeout_sec=0.5) + + # Deactivate pose controller + self.set_enable(self.pose_enable_client, False) + + def run_velocity_action(self, **params: Any) -> None: + """ + Publish constant velocity commands for a given duration, with optional early stop. + + Args: + linear_x (float): Desired linear velocity in x (m/s). + linear_y (float): Desired linear velocity in y (m/s). + angular_z (float): Desired angular velocity around z (rad/s). + duration (float): Total time to publish commands (seconds). + margin (float, optional): Early stop threshold for actual vs commanded velocity (m/s). + grace (float): Grace period before starting abort checks (seconds). + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + # Activate velocity controller + self.call_mux(TOPIC_FORCE_VELOCITY) + self.set_enable(self.vel_enable_client, True) + + # Prepare and log command + msg = Twist() + lx = resolved_params.get("linear_x", 0.0) + ly = resolved_params.get("linear_y", 0.0) + az = resolved_params.get("angular_z", 0.0) + margin = resolved_params.get("margin", None) + self.get_logger().info( + " " + f"Velocity action (u={lx:.4f}, v={ly:.4f}, r={az:.4f})" + + (f", margin={margin:.4f}" if margin is not None else "") + ) + + # Extract duration, grace period and start time + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + # Loop at ~10 Hz, processing callbacks without blocking + period = 0.1 + + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + # Publish the commanded velocity + msg.linear.x = lx + msg.linear.y = ly + msg.angular.z = az + self.vel_pub.publish(msg) + + # Feedback: log current odometry state + if self.current_odom: + pos = self.current_odom.pose.pose.position + lvel = self.current_odom.twist.twist.linear + avel = self.current_odom.twist.twist.angular + self.get_logger().info( + " " + f"Velocity feedback: " + f"vel=(vx={lvel.x:.2f}, vy={lvel.y:.2f}, vr={avel.z:.2f}), " + f"cmd=(u={lx:.2f}, v={ly:.2f}, r={az:.2f})" + ) + + # margin-based early success + if margin is not None: + act = self.current_odom.twist.twist + if abs(act.linear.x - lx) < margin and abs(act.linear.y - ly) < margin: + self.get_logger().info("Velocity within margin, stopping early") + break + + # Abort check after grace period + if self.current_odom and elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during velocity action, aborting" + ) + time.sleep(0.1) + rclpy.spin_once(self, timeout_sec=period) + + # Deactivate velocity controller + self.set_enable(self.vel_enable_client, False) + + def run_wait_action(self, **params: Any) -> None: + """ + Wait for a specified duration while processing ROS callbacks. + + This loop periodically checks for an abort condition after an optional grace period + and logs odometry feedback during the wait. + + Args: + duration (float): Total wait time in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + duration = resolved_params.get("duration", 0.0) + grace = resolved_params.get("grace", 0.0) + start = self.get_clock().now().nanoseconds / 1e9 + + self.get_logger().info(f"Waiting for {duration:.4f} seconds") + elapsed = 0.0 + # Loop at ~2 Hz, processing callbacks without blocking + period = 0.5 + while rclpy.ok() and elapsed < duration: + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during wait action, aborting") + + # allow callbacks and enforce loop timing + rclpy.spin_once(self, timeout_sec=period) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Wait feedback: pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + def run_force_action(self, **params: Any) -> None: + """ + Apply a constant force to the vehicle for a specified duration. + + This loop publishes a Wrench message at fixed intervals and checks for abort + conditions after an optional grace period. Odometry feedback is logged each cycle. + + Args: + force_x (float): Force in x-direction in Newtons. + force_y (float): Force in y-direction in Newtons. + torque_z (float): Torque about z-axis in Newton-meters. + duration (float): Total time to apply force in seconds. + grace (float): Grace period before starting abort checks in seconds. + + Raises: + RuntimeError: If abort_condition() returns True after grace period. + """ + + resolved_params = {k: v() if callable( + v) else v for k, v in params.items()} + + self.call_mux(TOPIC_FORCE_CONSTANT) + + msg = Wrench() + + duration = params.get("duration", 0.0) + grace = params.get("grace", 0.0) + + continuous_sampling = params.get("continuous_sampling", False) + frequency = params.get("frequency", 1.0) # Hz + + start = self.get_clock().now().nanoseconds / 1e9 + elapsed = 0.0 + + # Loop at specified frequency (or 2 Hz), processing callbacks without blocking + period = 1.0 / (frequency if continuous_sampling else 2.0) + + last_update_elapsed = 0.0 + while rclpy.ok() and elapsed < duration: + + elapsed = self.get_clock().now().nanoseconds / 1e9 - start + + # allow callbacks and enforce publish rate + rclpy.spin_once(self, timeout_sec=0.5) + + # if np.mod(elapsed, period) > 0.01: + # # Skip this iteration if not at the right frequency + # continue + + if abs(last_update_elapsed - elapsed) < period: + # Skip if not enough time has passed since last update + continue + + last_update_elapsed = elapsed + + msg.force.x = resolved_params.get("force_x", 0.0) + msg.force.y = resolved_params.get("force_y", 0.0) + msg.torque.z = resolved_params.get("torque_z", 0.0) + + if continuous_sampling: + # Sample force parameters at each iteration + msg.force.x = params.get("force_x", 0.0)() + msg.force.y = params.get("force_y", 0.0)() + msg.torque.z = params.get("torque_z", 0.0)() + + # record last force for experiment publishing + self._last_force = msg + self.force_pub.publish(msg) + + if self.current_odom: + pos = self.current_odom.pose.pose.position + self.get_logger().info( + f"Force feedback: (fx={msg.force.x:.2f}, " + f"fy={msg.force.y:.2f}, tz={msg.torque.z:.2f})," + f" pos=({pos.x:.2f}, {pos.y:.2f}), " + f"elapsed={elapsed:.2f}s" + ) + + if elapsed >= grace and self.abort_condition(): + raise RuntimeError( + "Abort condition met during force action, aborting") + + # time.sleep(period) # Sleep for the period to control frequency + + def execute_action(self, action: Dict[str, Any]) -> None: + """ + Execute one mission action and resolve its parameters. + + Normalizes callable parameters, disables other controllers, and invokes the action. + + Args: + action (dict): Action descriptor with keys 'action' (callable) and 'parameters' (dict). + """ + if not isinstance(action, dict): + self.get_logger().error(f'Invalid action format: {action}') + return + + runner = action.get('action') + params = action.get('parameters', {}) + + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) + + if not callable(runner): + self.get_logger().warn(f'Unknown action: {runner}') + return + + self.get_logger().info(f"Executing action: {action['name']}") + + # Handle experiment flag without resolving other parameters + self.experiment_enabled = bool(params.get("experiment", False)) + runner(**params) + + # Disable experiment after action completes + self.experiment_enabled = False + + def execute_actions(self) -> None: + """ + Loop through the action sequence `repeat` times, handling aborts. + + For each action, calls execute_action and on RuntimeError runs an abort_action. + """ + print("executing actions...") + for _ in range(self.repeat): + + for action in self.actions: + + try: + self.execute_action(action) + except RuntimeError as e: + abort_action = action.get( + 'abort_action', self.default_abort_action) + self.get_logger().warn( + f'Error executing action {action["name"]}: {e}') + self.get_logger().warn( + f'Executing abort action: {abort_action["name"]}') + try: + self.execute_action(abort_action) + except RuntimeError as abort_e: + self.get_logger().error( + f'Abort action failed too!: {abort_e}') + self.set_enable(self.pose_enable_client, False) + self.set_enable(self.vel_enable_client, False) + self.call_mux(TOPIC_FORCE_BASE) + exit(1) + + def odom_callback(self, msg): + """ + Callback for Odometry subscription; stores the latest message. + + Args: + msg (Odometry): Received odometry message containing pose and twist. + """ + self.current_odom = msg + + # if self.experiment_enabled: + # # Publish odometry for experiment if enabled + # self.experiment_odom_pub.publish(msg) + # self.experiment_flag_pub.publish(Bool(data=True)) + # self.experiment_force_pub.publish(self._last_force) + + +def main(args: Optional[List[str]] = None) -> None: + + rclpy.init(args=args) + node = ActionRunner() + + node.call_init_services() + node.wait_until_ready() + + executor = MultiThreadedExecutor() + executor.add_node(node) + + node.execute_actions() + executor.spin() + executor.shutdown() + + +if __name__ == "__main__": + main() diff --git a/cybership_dp/config/voyager/.gitkeep b/cybership_tests/cybership_tests/mission_action.py similarity index 100% rename from cybership_dp/config/voyager/.gitkeep rename to cybership_tests/cybership_tests/mission_action.py diff --git a/cybership_dp/cybership_dp/force_control/.gitkeep b/cybership_tests/cybership_tests/mission_manager.py similarity index 100% rename from cybership_dp/cybership_dp/force_control/.gitkeep rename to cybership_tests/cybership_tests/mission_manager.py diff --git a/cybership_thrusters/README.md b/cybership_thrusters/README.md index c90b18a..0723069 100644 --- a/cybership_thrusters/README.md +++ b/cybership_thrusters/README.md @@ -78,6 +78,28 @@ The ROS2 parameters for configuring each thruster are defined under the `thruste - **`signal_inverted`**: A boolean flag indicating whether the output signal should be inverted. +## Safety Watchdog + +- All thruster controllers implement a safety watchdog. If no command message is received on the configured `force_topic` for more than `2.0` seconds, the thruster automatically publishes zero commands on its output topics (e.g., `signal`, `rpm`, `angle`, `arm_x`, `arm_y`). +- The timeout is configurable per thruster via `thrusters..safety_timeout` (seconds). Default: `2.0`. + +Example YAML: + +```yaml +thrusters: + thruster1: + type: "fixed" + force_topic: "/thruster1/force" + signal_topic: "/thruster1/signal" + force_max: 5.0 + force_min: -5.0 + safety_timeout: 2.0 # seconds +``` + +Behavior notes: +- When a thruster is disabled via service, it immediately publishes zero commands as well. +- The watchdog runs at 10 Hz and only publishes when a timeout occurs. + ## Usage The node will read parameters from the ROS2 parameter server and create instances of thrusters based on their configurations. @@ -119,7 +141,7 @@ thrusters: signal_inverted: true ``` -You can load this configuration via a YAML file and pass it as an argument to your node or use ROS2’s parameter mechanisms to set these values dynamically. +You can load this configuration via a YAML file and pass it as an argument to your node or use ROS2's parameter mechanisms to set these values dynamically. ## Additional Information diff --git a/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp b/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp index 348f7c0..383b868 100644 --- a/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp +++ b/cybership_thrusters/include/cybership_thrusters/cybership_thrusters.hpp @@ -102,6 +102,13 @@ class VoithSchneider : public ThrusterBase { void f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg); + // Safety watchdog + rclcpp::TimerBase::SharedPtr m_watchdog_timer; + rclcpp::Time m_last_cmd_time; + double m_safety_timeout_sec = 2.0; + void f_watchdog_check(); + void f_publish_zero(); + public: VoithSchneider(rclcpp::Node::SharedPtr node, std::string thruster_name); @@ -138,6 +145,13 @@ class Fixed : public ThrusterBase { void f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg); + // Safety watchdog + rclcpp::TimerBase::SharedPtr m_watchdog_timer; + rclcpp::Time m_last_cmd_time; + double m_safety_timeout_sec = 2.0; + void f_watchdog_check(); + void f_publish_zero(); + rclcpp::Service::SharedPtr m_enable_service; rclcpp::Service::SharedPtr m_disable_service; @@ -200,6 +214,13 @@ class Azimuth : public ThrusterBase { void f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg); + // Safety watchdog + rclcpp::TimerBase::SharedPtr m_watchdog_timer; + rclcpp::Time m_last_cmd_time; + double m_safety_timeout_sec = 2.0; + void f_watchdog_check(); + void f_publish_zero(); + public: Azimuth(rclcpp::Node::SharedPtr node, std::string thruster_name); diff --git a/cybership_thrusters/package.xml b/cybership_thrusters/package.xml index ba07fe9..eb07405 100644 --- a/cybership_thrusters/package.xml +++ b/cybership_thrusters/package.xml @@ -12,6 +12,7 @@ geometry_msgs std_msgs std_srvs + rclcpp ament_lint_auto ament_lint_common diff --git a/cybership_thrusters/src/azimuth.cpp b/cybership_thrusters/src/azimuth.cpp index 19709c4..81e93b3 100644 --- a/cybership_thrusters/src/azimuth.cpp +++ b/cybership_thrusters/src/azimuth.cpp @@ -25,6 +25,11 @@ Azimuth::Azimuth(rclcpp::Node::SharedPtr node, std::string name) : ThrusterBase( m_disable_service = m_node->create_service("thruster/disable", std::bind(&Azimuth::f_disable_callback, this, std::placeholders::_1, std::placeholders::_2)); + // Safety watchdog setup + m_node->get_parameter_or("thrusters." + m_config.name + ".safety_timeout", m_safety_timeout_sec, 2.0); + m_last_cmd_time = m_node->now(); + m_watchdog_timer = m_node->create_wall_timer(std::chrono::milliseconds(500), std::bind(&Azimuth::f_watchdog_check, this)); + } void Azimuth::f_enable_callback(const std::shared_ptr request, @@ -41,10 +46,12 @@ void Azimuth::f_disable_callback(const std::shared_ptrnow(); if (!m_enabled) { std_msgs::msg::Float32 zero_msg; @@ -62,6 +69,10 @@ void Azimuth::f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg) auto angle = std::atan2(msg->force.y, msg->force.x); auto rpm = std::sqrt(std::pow(msg->force.y,2) + std::pow(msg->force.x,2)); + if (m_config.angle_inverted) { + angle = -angle; + } + angle_msg.data = angle; rpm_msg.data = rpm; @@ -73,6 +84,23 @@ void Azimuth::f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg) } +void Azimuth::f_watchdog_check() +{ + auto now = m_node->now(); + auto elapsed = (now - m_last_cmd_time).seconds(); + if (elapsed > m_safety_timeout_sec) { + f_publish_zero(); + } +} + +void Azimuth::f_publish_zero() +{ + std_msgs::msg::Float32 zero_msg; + zero_msg.data = 0.0f; + m_angle_pub->publish(zero_msg); + m_rpm_pub->publish(zero_msg); +} + void Azimuth::Config::declare(rclcpp::Node::SharedPtr node) { // node->declare_parameter("thrusters." + name +".force_topic", rclcpp::PARAMETER_STRING); diff --git a/cybership_thrusters/src/fixed.cpp b/cybership_thrusters/src/fixed.cpp index 18b4c77..6e7bdeb 100644 --- a/cybership_thrusters/src/fixed.cpp +++ b/cybership_thrusters/src/fixed.cpp @@ -22,6 +22,11 @@ Fixed::Fixed(rclcpp::Node::SharedPtr node, std::string name) : ThrusterBase(node m_disable_service = m_node->create_service("thruster/disable", std::bind(&Fixed::f_disable_callback, this, std::placeholders::_1, std::placeholders::_2)); + // Safety watchdog setup + m_node->get_parameter_or("thrusters." + m_config.name + ".safety_timeout", m_safety_timeout_sec, 2.0); + m_last_cmd_time = m_node->now(); + m_watchdog_timer = m_node->create_wall_timer(std::chrono::milliseconds(500), std::bind(&Fixed::f_watchdog_check, this)); + } void Fixed::f_enable_callback(const std::shared_ptr request, @@ -38,10 +43,12 @@ void Fixed::f_disable_callback(const std::shared_ptrnow(); if (!m_enabled) { std_msgs::msg::Float32 zero_msg; @@ -79,6 +86,22 @@ void Fixed::f_force_callback(const geometry_msgs::msg::Wrench::SharedPtr msg) } +void Fixed::f_watchdog_check() +{ + auto now = m_node->now(); + auto elapsed = (now - m_last_cmd_time).seconds(); + if (elapsed > m_safety_timeout_sec) { + f_publish_zero(); + } +} + +void Fixed::f_publish_zero() +{ + std_msgs::msg::Float32 zero_msg; + zero_msg.data = 0.0f; + m_signal_pub->publish(zero_msg); +} + void Fixed::Config::declare(rclcpp::Node::SharedPtr node) { diff --git a/cybership_thrusters/src/voith_schneider.cpp b/cybership_thrusters/src/voith_schneider.cpp index 4118466..9b3b66b 100644 --- a/cybership_thrusters/src/voith_schneider.cpp +++ b/cybership_thrusters/src/voith_schneider.cpp @@ -27,6 +27,11 @@ VoithSchneider::VoithSchneider(rclcpp::Node::SharedPtr node, std::string name) : m_disable_service = m_node->create_service("thruster/disable", std::bind(&VoithSchneider::f_disable_callback, this, std::placeholders::_1, std::placeholders::_2)); + // Safety watchdog setup + m_node->get_parameter_or("thrusters." + m_config.name + ".safety_timeout", m_safety_timeout_sec, 2.0); + m_last_cmd_time = m_node->now(); + m_watchdog_timer = m_node->create_wall_timer(std::chrono::milliseconds(500), std::bind(&VoithSchneider::f_watchdog_check, this)); + } void VoithSchneider::f_enable_callback(const std::shared_ptr request, @@ -43,10 +48,12 @@ void VoithSchneider::f_disable_callback(const std::shared_ptrnow(); if (!m_enabled) { std_msgs::msg::Float32 zero_msg; @@ -103,6 +110,24 @@ void VoithSchneider::f_force_callback(const geometry_msgs::msg::Wrench::SharedPt } +void VoithSchneider::f_watchdog_check() +{ + auto now = m_node->now(); + auto elapsed = (now - m_last_cmd_time).seconds(); + if (elapsed > m_safety_timeout_sec) { + f_publish_zero(); + } +} + +void VoithSchneider::f_publish_zero() +{ + std_msgs::msg::Float32 zero_msg; + zero_msg.data = 0.0f; + m_arm_x_pub->publish(zero_msg); + m_arm_y_pub->publish(zero_msg); + m_rpm_pub->publish(zero_msg); +} + void VoithSchneider::Config::declare(rclcpp::Node::SharedPtr node) { // node->declare_parameter("thrusters." + name +".force_topic", rclcpp::PARAMETER_STRING); diff --git a/cybership_utilities/CMakeLists.txt b/cybership_utilities/CMakeLists.txt index 76f588e..01693b1 100644 --- a/cybership_utilities/CMakeLists.txt +++ b/cybership_utilities/CMakeLists.txt @@ -8,12 +8,20 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) -find_package(joy REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(rclpy REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) ament_python_install_package(${PROJECT_NAME}) +# Install Python executables +install( + DIRECTORY + launch html + DESTINATION + share/${PROJECT_NAME} +) + +install(PROGRAMS + nodes/force_mux_gui.py + DESTINATION lib/${PROJECT_NAME} +) + ament_package() \ No newline at end of file diff --git a/cybership_utilities/README.md b/cybership_utilities/README.md index 9d1d8b4..26617cd 100644 --- a/cybership_utilities/README.md +++ b/cybership_utilities/README.md @@ -20,4 +20,14 @@ You can use the provided utility functions and launch arguments in your ROS 2 la from cybership_utilities import launch ``` -Then use `sanitize_hostname_for_ros2` or `anon` in your launch configurations, and include `COMMON_ARGUMENTS` to standardize launch argument handling. \ No newline at end of file +Then use `sanitize_hostname_for_ros2` or `anon` in your launch configurations, and include `COMMON_ARGUMENTS` to standardize launch argument handling. + +### Force Mux GUI + +The script `nodes/force_mux_gui.py` provides a small PyQt GUI to inspect and switch the `topic_tools` force mux source for a selected vehicle namespace. + +- Lists available input topics via `MuxList` +- Switches active topic via `MuxSelect` +- Namespace can be typed (e.g., `/enterprise`) or set with `VEHICLE_NS` env var. + +Run it with your preferred Python, ensuring your ROS 2 environment is sourced so service types are discoverable. \ No newline at end of file diff --git a/cybership_utilities/cybership_utilities/launch.py b/cybership_utilities/cybership_utilities/launch.py index aecc9fd..9f2572d 100644 --- a/cybership_utilities/cybership_utilities/launch.py +++ b/cybership_utilities/cybership_utilities/launch.py @@ -71,3 +71,38 @@ def anon(host: bool = True) -> str: description="Use simulation clock if true", ), ] + +def include_launch_action_with_config( + vessel_model, + vessel_name, + launch_file, + config_file=""): + + bringup_pkg_dir = launch_ros.substitutions.FindPackageShare( + 'cybership_bringup') + config_pkg_dir = launch_ros.substitutions.FindPackageShare( + 'cybership_config') + + launch_arguments = [ + ('vessel_name', vessel_name), + ('vessel_model', vessel_model) + ] + + if len(config_file) != 0: + launch_arguments.append( + ( + 'param_file', + launch.substitutions.PathJoinSubstitution( + [config_pkg_dir, 'config', vessel_model, config_file] + ) + ) + ) + + return launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + launch.substitutions.PathJoinSubstitution( + [bringup_pkg_dir, 'launch', 'include', launch_file] + ) + ), + launch_arguments=launch_arguments + ) \ No newline at end of file diff --git a/cybership_utilities/html/index.html b/cybership_utilities/html/index.html new file mode 100644 index 0000000..3384a0e --- /dev/null +++ b/cybership_utilities/html/index.html @@ -0,0 +1,337 @@ + + +Mux UI (foxglove_bridge + @foxglove/cdr) + + +

ROS 2 Mux (Web UI)

+
+ + + + disconnected +
+ +
+ Namespace + +
+ +
+ Thrusters +
+ + + +
+
Calls <ns>/thrusters/activate and <ns>/thrusters/deactivate (std_srvs/Empty)
+ +
+ +
+ Force Mux +
+ + +
+
+ + + + +
+
+ +
+ Velocity Mux +
+ + +
+
+ + + + +
+
+ + +

+
+
\ No newline at end of file
diff --git a/cybership_utilities/launch/ui.launch.py b/cybership_utilities/launch/ui.launch.py
new file mode 100644
index 0000000..a69e04c
--- /dev/null
+++ b/cybership_utilities/launch/ui.launch.py
@@ -0,0 +1,49 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description() -> LaunchDescription:
+    http_port = LaunchConfiguration("http_port")
+    ws_port = LaunchConfiguration("ws_port")
+
+    html_dir = PathJoinSubstitution([FindPackageShare("cybership_utilities"), "html"])
+
+    # Simple static file server for the Web UI (serves index2.html, etc.)
+    http_server = ExecuteProcess(
+        cmd=["python3", "-m", "http.server", http_port],
+        cwd=html_dir,
+        output="screen",
+    )
+
+    # Foxglove WebSocket bridge (ROS 2) for service/topic access from the Web UI
+    foxglove = Node(
+        package="foxglove_bridge",
+        executable="foxglove_bridge",
+        name="foxglove_bridge",
+        output="screen",
+        arguments=["--port", ws_port],
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "http_port", default_value="8001", description="Port for the static HTTP server"
+            ),
+            DeclareLaunchArgument(
+                "ws_port", default_value="8765", description="WebSocket port for foxglove_bridge"
+            ),
+            LogInfo(
+                msg=[
+                    TextSubstitution(text="Serving UI at http://localhost:"),
+                    http_port,
+                    TextSubstitution(text="/index.html | Foxglove WS ws://localhost:"),
+                    ws_port,
+                ]
+            ),
+            http_server,
+            foxglove,
+        ]
+    )
diff --git a/cybership_utilities/nodes/force_mux_gui.py b/cybership_utilities/nodes/force_mux_gui.py
new file mode 100755
index 0000000..5484a97
--- /dev/null
+++ b/cybership_utilities/nodes/force_mux_gui.py
@@ -0,0 +1,368 @@
+#!/usr/bin/env python3
+
+"""
+PyQt GUI to control topic_tools multiplexers for force or velocity commands.
+
+Features
+- Enter/select a vehicle namespace (e.g., /enterprise)
+- Discover mux services and list input topics via MuxList
+- Switch the active source using MuxSelect
+"""
+
+import os
+import sys
+from typing import Optional, List
+
+from PyQt5 import QtWidgets, QtCore
+
+import rclpy
+from rclpy.node import Node
+
+try:
+    from topic_tools_interfaces.srv import MuxList, MuxSelect
+    from std_srvs.srv import Empty
+except Exception:
+    print("Missing dependency topic_tools_interfaces. Please build/install it.")
+    raise
+
+
+DEFAULT_MUX_NAME = os.environ.get("MUX_NAME", "force_mux")
+
+
+class ForceMuxClient(Node):
+    def __init__(self, namespace: str, mux_name: str = DEFAULT_MUX_NAME) -> None:
+        super().__init__("force_mux_client", namespace=namespace)
+        self.mux_name = mux_name
+        self._list_cli = self.create_client(MuxList, f"{mux_name}/list")
+        self._select_cli = self.create_client(MuxSelect, f"{mux_name}/select")
+
+    def discover_mux_bases(self) -> List[str]:
+        bases: List[str] = []
+        ns_prefix = self.get_namespace().rstrip("/")
+        for name, types in self.get_service_names_and_types():
+            flat_types: List[str] = []
+            for t in types:
+                if isinstance(t, str):
+                    flat_types.append(t)
+                else:
+                    flat_types.extend(t)
+            if not any("topic_tools_interfaces/srv/MuxList" in t for t in flat_types):
+                continue
+            if ns_prefix and not name.startswith(ns_prefix + "/"):
+                continue
+            if not name.endswith("/list"):
+                continue
+            base = name.split("/")[-2]
+            bases.append(base)
+        return sorted(set(bases))
+
+    def wait_for_services(self, timeout_sec: float = 2.0) -> bool:
+        ok1 = self._list_cli.wait_for_service(timeout_sec=timeout_sec)
+        ok2 = self._select_cli.wait_for_service(timeout_sec=timeout_sec)
+        return bool(ok1 and ok2)
+
+    def list_topics(self):
+        req = MuxList.Request()
+        return self._list_cli.call_async(req)
+
+    def select_topic(self, topic: str):
+        req = MuxSelect.Request()
+        req.topic = topic
+        return self._select_cli.call_async(req)
+
+
+class MuxPanel(QtWidgets.QGroupBox):
+    def __init__(self, kind: str, get_namespace, ensure_rcl) -> None:
+        title = "Force Mux" if kind == "force" else "Velocity Mux"
+        super().__init__(title)
+        self.kind = kind  # 'force' or 'velocity'
+        self._get_namespace = get_namespace
+        self._ensure_rcl = ensure_rcl
+        self._node = None  # type: Optional[ForceMuxClient]
+
+        self.topic_combo = QtWidgets.QComboBox()
+        self.topic_combo.setEditable(False)
+        self.find_btn = QtWidgets.QPushButton("Find")
+        self.refresh_btn = QtWidgets.QPushButton("Refresh")
+        self.select_btn = QtWidgets.QPushButton("Select")
+        self.status_lbl = QtWidgets.QLabel("Idle")
+
+        row = QtWidgets.QHBoxLayout()
+        row.addWidget(self.topic_combo, 1)
+        row.addWidget(self.find_btn)
+        row.addWidget(self.refresh_btn)
+        row.addWidget(self.select_btn)
+
+        lay = QtWidgets.QVBoxLayout(self)
+        lay.addLayout(row)
+        lay.addWidget(self.status_lbl)
+
+        self.find_btn.clicked.connect(self.on_find)
+        self.refresh_btn.clicked.connect(self.on_refresh)
+        self.select_btn.clicked.connect(self.on_select)
+
+    def _prefix(self) -> str:
+        return "force_mux" if self.kind == "force" else "velocity_mux"
+
+    def _desired_default_mux_name(self) -> str:
+        # Allow per-kind override, else global, else default
+        per_kind = os.environ.get("FORCE_MUX_NAME" if self.kind == "force" else "VELOCITY_MUX_NAME")
+        if per_kind:
+            return per_kind
+        env_override = os.environ.get("MUX_NAME")
+        if env_override:
+            return env_override
+        return self._prefix()
+
+    def _namespace(self) -> str:
+        ns = (self._get_namespace() or "").strip()
+        if ns and not ns.startswith("/"):
+            ns = "/" + ns
+        return ns
+
+    def _recreate_node(self, mux_name: Optional[str] = None) -> Optional[ForceMuxClient]:
+        ns = self._namespace()
+        if self._node is not None:
+            try:
+                self._node.destroy_node()
+            except Exception:
+                pass
+            self._node = None
+        self._ensure_rcl()
+        try:
+            self._node = ForceMuxClient(namespace=ns, mux_name=mux_name or self._desired_default_mux_name())
+        except Exception as e:
+            self.status_lbl.setText(f"Node error: {e}")
+            self._node = None
+        return self._node
+
+    def spin_once(self) -> None:
+        if self._node is not None and rclpy.ok():
+            rclpy.spin_once(self._node, timeout_sec=0.0)
+
+    def on_find(self) -> None:
+        node = self._recreate_node()
+        if node is None:
+            return
+        self.status_lbl.setText("Discovering mux…")
+        bases = node.discover_mux_bases()
+        if not bases:
+            self.status_lbl.setText("No mux found")
+            return
+        preferred = [b for b in bases if b.startswith(self._prefix())]
+        chosen = preferred[0] if preferred else bases[0]
+        # Bind to chosen mux
+        self._recreate_node(mux_name=chosen)
+        self.status_lbl.setText(f"Using '{chosen}'")
+
+    def on_refresh(self) -> None:
+        node = self._recreate_node()
+        if node is None:
+            return
+        self.status_lbl.setText("Waiting for services…")
+        if not node.wait_for_services(timeout_sec=2.0):
+            bases = node.discover_mux_bases()
+            if not bases:
+                self.status_lbl.setText("Mux services unavailable")
+                return
+            chosen = (next((b for b in bases if b.startswith(self._prefix())), None) or bases[0])
+            node = self._recreate_node(mux_name=chosen)
+            if node is None or not node.wait_for_services(timeout_sec=2.0):
+                self.status_lbl.setText("Mux services unavailable")
+                return
+            self.status_lbl.setText(f"Connected '{chosen}'")
+
+        self.status_lbl.setText("Listing…")
+        fut = node.list_topics()
+
+        def on_done():
+            if fut.cancelled() or fut.exception() is not None:
+                self.status_lbl.setText(f"List failed: {fut.exception()}")
+                return
+            resp = fut.result()
+            topics = list(getattr(resp, "topics", []))
+            self.topic_combo.clear()
+            if topics:
+                self.topic_combo.addItems(topics)
+                self.status_lbl.setText(f"{len(topics)} topic(s)")
+            else:
+                self.status_lbl.setText("No topics in mux")
+
+        fut.add_done_callback(lambda _: QtCore.QTimer.singleShot(0, on_done))
+
+    def on_select(self) -> None:
+        if self._node is None:
+            self.status_lbl.setText("Find/Refresh first")
+            return
+        topic = self.topic_combo.currentText().strip()
+        if not topic:
+            self.status_lbl.setText("Pick a topic")
+            return
+        self.status_lbl.setText(f"Selecting {topic}…")
+        fut = self._node.select_topic(topic)
+
+        def on_done():
+            if fut.cancelled() or fut.exception() is not None:
+                self.status_lbl.setText(f"Select failed: {fut.exception()}")
+                return
+            resp = fut.result()
+            ok = bool(getattr(resp, "success", False))
+            self.status_lbl.setText("Selected" if ok else "Select rejected")
+
+        fut.add_done_callback(lambda _: QtCore.QTimer.singleShot(0, on_done))
+
+
+class BothMuxGUI(QtWidgets.QWidget):
+    def __init__(self) -> None:
+        super().__init__()
+        self.setWindowTitle("Mux Controller (Force + Velocity)")
+        self.setMinimumWidth(640)
+
+        # rcl state
+        self._rcl_inited = False
+
+        # Top controls
+        self.ns_edit = QtWidgets.QLineEdit()
+        self.ns_edit.setPlaceholderText("/vehicle_namespace (e.g., /enterprise)")
+        self.ns_edit.setText(os.environ.get("VEHICLE_NS", ""))
+
+        form = QtWidgets.QFormLayout()
+        form.addRow("Namespace", self.ns_edit)
+
+        # Panels (vertical stack)
+        self.force_panel = MuxPanel("force", get_namespace=lambda: self.ns_edit.text(), ensure_rcl=self._ensure_rcl)
+        self.velocity_panel = MuxPanel("velocity", get_namespace=lambda: self.ns_edit.text(), ensure_rcl=self._ensure_rcl)
+
+        panels = QtWidgets.QVBoxLayout()
+        panels.addWidget(self.force_panel)
+        panels.addWidget(self.velocity_panel)
+
+        # Thruster control
+        thruster_box = QtWidgets.QGroupBox("Thruster")
+        self.thr_activate_btn = QtWidgets.QPushButton("Activate")
+        self.thr_disable_btn = QtWidgets.QPushButton("Deactivate")
+        self.thr_status_lbl = QtWidgets.QLabel("Idle")
+        thr_buttons = QtWidgets.QHBoxLayout()
+        thr_buttons.addWidget(self.thr_activate_btn)
+        thr_buttons.addWidget(self.thr_disable_btn)
+        thr_lay = QtWidgets.QVBoxLayout(thruster_box)
+        thr_lay.addLayout(thr_buttons)
+        thr_lay.addWidget(self.thr_status_lbl)
+
+        root = QtWidgets.QVBoxLayout(self)
+        root.addLayout(form)
+        root.addWidget(thruster_box)
+        root.addLayout(panels)
+
+        # Timer to spin both nodes
+        self._spin_timer = QtCore.QTimer(self)
+        self._spin_timer.timeout.connect(self._spin_once)
+        self._spin_timer.start(20)
+
+        # Thruster node/state
+        self._thruster_node = None
+        self._thruster_ns = None
+        self.thr_activate_btn.clicked.connect(self.on_thr_activate)
+        self.thr_disable_btn.clicked.connect(self.on_thr_disable)
+
+    def _ensure_rcl(self) -> None:
+        if not self._rcl_inited:
+            rclpy.init(args=None)
+            self._rcl_inited = True
+
+    def _spin_once(self) -> None:
+        self.force_panel.spin_once()
+        self.velocity_panel.spin_once()
+        # Spin thruster node too
+        if self._thruster_node is not None and rclpy.ok():
+            rclpy.spin_once(self._thruster_node, timeout_sec=0.0)
+
+    def _namespace(self) -> str:
+        ns = (self.ns_edit.text() or "").strip()
+        if ns and not ns.startswith("/"):
+            ns = "/" + ns
+        return ns
+
+    def _ensure_thruster_node(self):
+        # Create once per namespace; reuse to avoid handle invalidation
+        ns = self._namespace()
+        if self._thruster_node is None or self._thruster_ns != ns:
+            # Dispose old (if any) before replacing
+            if self._thruster_node is not None:
+                try:
+                    self._thruster_node.destroy_node()
+                except Exception:
+                    pass
+                self._thruster_node = None
+            self._ensure_rcl()
+            self._thruster_node = rclpy.create_node("thruster_client", namespace=ns)
+            self._thruster_ns = ns
+        return self._thruster_node
+
+    def _discover_thruster_services(self, name_candidates, node) -> list:
+        found = []
+        empty_type_names = {"std_srvs/srv/Empty", "std_srvs/srv/Empty.srv"}
+        services = node.get_service_names_and_types()
+        ns = self._namespace() or "/"
+        ns_prefix = ns.rstrip("/") + "/" if ns != "/" else "/"
+        for cand in name_candidates:
+            for name, types in services:
+                flat_types = []
+                for t in types:
+                    flat_types.append(t if isinstance(t, str) else t[0])
+                if not any(t in empty_type_names for t in flat_types):
+                    continue
+                if not name.startswith(ns_prefix):
+                    continue
+                if name.endswith("/" + cand):
+                    found.append(name)
+        # Unique preserve order
+        seen = set()
+        ordered = []
+        for n in found:
+            if n not in seen:
+                seen.add(n)
+                ordered.append(n)
+        return ordered
+
+    def _call_empty_service(self, name_candidates) -> bool:
+        node = self._ensure_thruster_node()
+        # Try discovered fully-qualified matches first (using same node)
+        fqns = self._discover_thruster_services(name_candidates, node)
+        try_list = fqns + name_candidates
+        for svc_name in try_list:
+            cli = node.create_client(Empty, svc_name)
+            if not cli.wait_for_service(timeout_sec=1.5):
+                continue
+            fut = cli.call_async(Empty.Request())
+            for _ in range(30):
+                rclpy.spin_once(node, timeout_sec=0.02)
+                if fut.done():
+                    break
+            if fut.done() and fut.exception() is None:
+                return True
+        return False
+
+    def on_thr_activate(self) -> None:
+        self.thr_status_lbl.setText("Activating…")
+        ok = self._call_empty_service(["thruster/activate", "thruster/enable"])  # fallback to enable
+        self.thr_status_lbl.setText("Activated" if ok else "Activate failed")
+
+    def on_thr_disable(self) -> None:
+        self.thr_status_lbl.setText("Disabling…")
+        ok = self._call_empty_service(["thruster/disable"])  # strict name per request
+        self.thr_status_lbl.setText("Disabled" if ok else "Disable failed")
+
+
+def main() -> None:
+    app = QtWidgets.QApplication(sys.argv)
+    gui = BothMuxGUI()
+    gui.show()
+    rc = app.exec_()
+    if rclpy.ok():
+        rclpy.shutdown()
+    sys.exit(rc)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/cybership_utilities/package.xml b/cybership_utilities/package.xml
index df00ab2..6b1badc 100644
--- a/cybership_utilities/package.xml
+++ b/cybership_utilities/package.xml
@@ -9,12 +9,8 @@
 
   ament_cmake
 
-  joy
-  sensor_msgs
   rclpy
-  geometry_msgs
-  tf2_ros
-  ros_base
+  topic_tools_interfaces
   ament_cmake_python
 
   
diff --git a/cybership_utilities/requirements.txt b/cybership_utilities/requirements.txt
index e69de29..dcd21e8 100644
--- a/cybership_utilities/requirements.txt
+++ b/cybership_utilities/requirements.txt
@@ -0,0 +1 @@
+PyQt5>=5.15
diff --git a/cybership_utilities/scripts/install_discovery_service.sh b/cybership_utilities/scripts/install_discovery_service.sh
new file mode 100755
index 0000000..06cf3e5
--- /dev/null
+++ b/cybership_utilities/scripts/install_discovery_service.sh
@@ -0,0 +1,71 @@
+#!/usr/bin/env bash
+
+set -Eeuo pipefail
+
+SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" >/dev/null 2>&1 && pwd)
+SERVICE_SRC="$SCRIPT_DIR/../service/fastdds_discovery.service"
+SYSTEMD_USER_DIR="${XDG_CONFIG_HOME:-$HOME/.config}/systemd/user"
+SERVICE_DEST="$SYSTEMD_USER_DIR/fastdds_discovery.service"
+FASTDDS_ENV_FILE="$HOME/.config/fastdds"
+ROSPROFILE_FILE="$HOME/.rosrc"
+
+echo "Installing Fast DDS Discovery Server user service..."
+
+if [[ ! -f "$SERVICE_SRC" ]]; then
+	echo "Error: Service file not found at $SERVICE_SRC" >&2
+	exit 1
+fi
+
+# Optional sanity checks and friendly warnings
+if ! command -v fastdds >/dev/null 2>&1; then
+	echo "Warning: 'fastdds' command is not in PATH. The service may fail to start until it's installed." >&2
+fi
+
+if [[ ! -f "$FASTDDS_ENV_FILE" ]]; then
+	echo "Creating default Fast DDS env file at $FASTDDS_ENV_FILE"
+	mkdir -p "$HOME/.config"
+	cat > "$FASTDDS_ENV_FILE" <<'EOF'
+# Fast DDS Discovery Server environment
+# Required: Unique discovery server ID (0-255). Choose per host.
+export FASTDDS_DISCOVERY_ID="1"
+
+# Optional: Discovery server port (defaults to 11811 if unset)
+export FASTDDS_DISCOVERY_PORT="11811"
+EOF
+else
+	echo "Found existing env file at $FASTDDS_ENV_FILE"
+fi
+
+# Ensure ~/.rosrc exists to satisfy ConditionPathExists from the unit
+if [[ ! -f "$ROSPROFILE_FILE" ]]; then
+	echo "Creating $ROSPROFILE_FILE (empty placeholder for service condition)"
+	: > "$ROSPROFILE_FILE"
+fi
+
+mkdir -p "$SYSTEMD_USER_DIR"
+install -m 0644 "$SERVICE_SRC" "$SERVICE_DEST"
+echo "Installed unit to $SERVICE_DEST"
+
+# Enable lingering so the user service can run without a login session
+echo "Enabling lingering for user '$USER' (so the service runs at boot)..."
+if ! loginctl enable-linger "$USER" >/dev/null 2>&1; then
+	if command -v sudo >/dev/null 2>&1; then
+		echo "Retrying with sudo to enable lingering..."
+		sudo loginctl enable-linger "$USER"
+	else
+		echo "Warning: Could not enable lingering automatically. Run this manually:" >&2
+		echo "  sudo loginctl enable-linger $USER" >&2
+	fi
+fi
+
+# Reload, enable and start the user service
+echo "Reloading systemd user units..."
+systemctl --user daemon-reload
+
+echo "Enabling and starting 'fastdds_discovery.service'..."
+systemctl --user enable --now fastdds_discovery.service
+
+echo "Service status (short):"
+systemctl --user --no-pager --full status fastdds_discovery.service || true
+
+echo "Done. The Fast DDS Discovery Server should now be active."
diff --git a/cybership_utilities/service/fastdds_discovery.service b/cybership_utilities/service/fastdds_discovery.service
new file mode 100644
index 0000000..3307f7f
--- /dev/null
+++ b/cybership_utilities/service/fastdds_discovery.service
@@ -0,0 +1,14 @@
+[Unit]
+Description=Fast DDS Discovery Server (reads ~/.config/fastdds for configuration)
+After=network-online.target
+Wants=network-online.target
+ConditionPathExists=%h/.config/fastdds
+
+[Service]
+Type=simple
+ExecStart=/bin/bash -lc 'source "$HOME/.config/fastdds"; : "${FASTDDS_DISCOVERY_ID:?Set FASTDDS_DISCOVERY_ID in ~/.config/fastdds}"; exec fastdds discovery -i "$FASTDDS_DISCOVERY_ID" -p "${FASTDDS_DISCOVERY_PORT:-11811}"'
+Restart=always
+RestartSec=5
+
+[Install]
+WantedBy=default.target
\ No newline at end of file
diff --git a/cybership_viz/CMakeLists.txt b/cybership_viz/CMakeLists.txt
index cee71cb..40dcd98 100644
--- a/cybership_viz/CMakeLists.txt
+++ b/cybership_viz/CMakeLists.txt
@@ -1,27 +1,7 @@
 cmake_minimum_required(VERSION 3.10)
 project(cybership_viz)
 
-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
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
 
 install(
   DIRECTORY launch config
diff --git a/cybership_viz/config/drillship.rviz b/cybership_viz/config/drillship.rviz
index 74f0a34..6fb8622 100644
--- a/cybership_viz/config/drillship.rviz
+++ b/cybership_viz/config/drillship.rviz
@@ -6,7 +6,13 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /TF1
+        - /TF1/Frames1
+        - /TF1/Tree1
         - /Odometry1/Shape1
+        - /RobotModel1
+        - /RobotModel1/Status1
+        - /RobotModel1/Description Topic1
       Splitter Ratio: 0.5596868991851807
     Tree Height: 1132
   - Class: rviz_common/Selection
@@ -50,26 +56,30 @@ Visualization Manager:
       Value: true
     - Class: rviz_default_plugins/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        aft_center_thruster_link:
+        drillship/aft_center_thruster_link:
           Value: true
-        aft_port_thruster_link:
+        drillship/aft_port_thruster_link:
           Value: true
-        aft_starboard_thruster_link:
+        drillship/aft_starboard_thruster_link:
           Value: true
-        base_link:
+        drillship/base_link:
           Value: true
-        bow_center_thruster_link:
+        drillship/base_link_ned:
           Value: true
-        bow_port_thruster_link:
+        drillship/bow_center_thruster_link:
           Value: true
-        bow_starboard_thruster_link:
+        drillship/bow_port_thruster_link:
           Value: true
-        imu_link:
+        drillship/bow_starboard_thruster_link:
           Value: true
-        mocap_link:
+        drillship/imu_link:
+          Value: true
+        drillship/mocap_link:
           Value: true
         world:
           Value: true
@@ -80,22 +90,23 @@ Visualization Manager:
       Show Names: true
       Tree:
         world:
-          base_link:
-            aft_center_thruster_link:
-              {}
-            aft_port_thruster_link:
-              {}
-            aft_starboard_thruster_link:
-              {}
-            bow_center_thruster_link:
-              {}
-            bow_port_thruster_link:
-              {}
-            bow_starboard_thruster_link:
-              {}
-            imu_link:
-              {}
-            mocap_link:
+          drillship/base_link_ned:
+            drillship/base_link:
+              drillship/aft_center_thruster_link:
+                {}
+              drillship/aft_port_thruster_link:
+                {}
+              drillship/aft_starboard_thruster_link:
+                {}
+              drillship/bow_center_thruster_link:
+                {}
+              drillship/bow_port_thruster_link:
+                {}
+              drillship/bow_starboard_thruster_link:
+                {}
+              drillship/imu_link:
+                {}
+            drillship/mocap_link:
               {}
       Update Interval: 0
       Value: true
@@ -287,6 +298,73 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: measurement/odom
       Value: true
+    - Alpha: 0.5
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: true
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /drillship/robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        aft_center_thruster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        aft_port_thruster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        aft_starboard_thruster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link_ned:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        bow_center_thruster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        bow_port_thruster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        bow_starboard_thruster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        mocap_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: drillship
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -333,7 +411,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 8.380566596984863
+      Distance: 4.215280055999756
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -345,21 +423,21 @@ Visualization Manager:
         Z: 0
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
+      Invert Z Axis: true
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6697965264320374
+      Pitch: 0.7303979992866516
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 2.2503936290740967
+      Yaw: 0.4803977608680725
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 1423
   Hide Left Dock: false
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000201000004f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004f5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008330000003efc0100000002fb0000000800540069006d00650100000000000008330000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000062c000004f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000304000004f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004f5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000004f5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004f5000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008330000003efc0100000002fb0000000800540069006d00650100000000000008330000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000423000004f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -367,7 +445,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: true
+    collapsed: false
   Width: 2099
-  X: 4319
-  Y: 224
+  X: 4255
+  Y: 160
diff --git a/cybership_viz/config/multi.rviz b/cybership_viz/config/multi.rviz
new file mode 100644
index 0000000..e2e07a3
--- /dev/null
+++ b/cybership_viz/config/multi.rviz
@@ -0,0 +1,624 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1/Frames1
+        - /TF1/Tree1
+      Splitter Ratio: 0.5
+    Tree Height: 1228
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: 
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        drillship/aft_center_thruster_link:
+          Value: true
+        drillship/aft_port_thruster_link:
+          Value: true
+        drillship/aft_starboard_thruster_link:
+          Value: true
+        drillship/base_link:
+          Value: true
+        drillship/base_link_ned:
+          Value: true
+        drillship/bow_center_thruster_link:
+          Value: true
+        drillship/bow_port_thruster_link:
+          Value: true
+        drillship/bow_starboard_thruster_link:
+          Value: true
+        drillship/imu_link:
+          Value: true
+        drillship/mocap_link:
+          Value: true
+        voyager/base_link:
+          Value: true
+        voyager/base_link_ned:
+          Value: true
+        voyager/bow_tunnel_thruster_link:
+          Value: true
+        voyager/imu_link:
+          Value: true
+        voyager/mocap_link:
+          Value: true
+        voyager/stern_port_thruster_link:
+          Value: true
+        voyager/stern_starboard_thruster_link:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          drillship/base_link_ned:
+            drillship/base_link:
+              drillship/aft_center_thruster_link:
+                {}
+              drillship/aft_port_thruster_link:
+                {}
+              drillship/aft_starboard_thruster_link:
+                {}
+              drillship/bow_center_thruster_link:
+                {}
+              drillship/bow_port_thruster_link:
+                {}
+              drillship/bow_starboard_thruster_link:
+                {}
+              drillship/imu_link:
+                {}
+            drillship/mocap_link:
+              {}
+          voyager/base_link_ned:
+            voyager/base_link:
+              voyager/bow_tunnel_thruster_link:
+                {}
+              voyager/imu_link:
+                {}
+              voyager/stern_port_thruster_link:
+                {}
+              voyager/stern_starboard_thruster_link:
+                {}
+            voyager/mocap_link:
+              {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        los_arrow: true
+        los_lookahead: true
+        los_path: true
+        los_spline: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /voyager/visualization_marker
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /voyager/visualization_marker_array
+      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /voyager/thruster/port/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /voyager/thruster/starboard/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /voyager/thruster/tunnel/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 100
+          Name: Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 0.25
+            Axes Radius: 0.02500000037252903
+            Color: 255; 255; 0
+            Head Length: 0.10000000149011612
+            Head Radius: 0.10000000149011612
+            Shaft Length: 0.5
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /voyager/measurement/odom
+          Value: true
+        - Alpha: 0.4000000059604645
+          Class: rviz_default_plugins/RobotModel
+          Collision Enabled: false
+          Description File: ""
+          Description Source: Topic
+          Description Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /voyager/robot_description
+          Enabled: true
+          Links:
+            All Links Enabled: true
+            Expand Joint Details: false
+            Expand Link Details: false
+            Expand Tree: false
+            Link Tree Style: Links in Alphabetic Order
+            base_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            base_link_ned:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            bow_tunnel_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            imu_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            mocap_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            stern_port_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            stern_starboard_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+          Mass Properties:
+            Inertia: false
+            Mass: false
+          Name: RobotModel
+          TF Prefix: voyager
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+      Enabled: true
+      Name: Voyager
+    - Class: rviz_common/Group
+      Displays:
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/thruster/aft_center/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/thruster/aft_port/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/thruster/aft_starboard/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/thruster/bow_center/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/thruster/bow_port/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Accept NaN Values: false
+          Alpha: 1
+          Arrow Width: 0.5
+          Class: rviz_default_plugins/Wrench
+          Enabled: true
+          Force Arrow Scale: 2
+          Force Color: 204; 51; 51
+          History Length: 1
+          Name: Wrench
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/thruster/bow_starboard/issued
+          Torque Arrow Scale: 2
+          Torque Color: 204; 204; 51
+          Value: true
+        - Alpha: 0.4000000059604645
+          Class: rviz_default_plugins/RobotModel
+          Collision Enabled: false
+          Description File: ""
+          Description Source: Topic
+          Description Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/robot_description
+          Enabled: true
+          Links:
+            All Links Enabled: true
+            Expand Joint Details: false
+            Expand Link Details: false
+            Expand Tree: false
+            Link Tree Style: Links in Alphabetic Order
+            aft_center_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            aft_port_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            aft_starboard_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            base_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+              Value: true
+            base_link_ned:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            bow_center_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            bow_port_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            bow_starboard_thruster_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            imu_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+            mocap_link:
+              Alpha: 1
+              Show Axes: false
+              Show Trail: false
+          Mass Properties:
+            Inertia: false
+            Mass: false
+          Name: RobotModel
+          TF Prefix: drillship
+          Update Interval: 0
+          Value: true
+          Visual Enabled: true
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 100
+          Name: Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 0.25
+            Axes Radius: 0.02500000037252903
+            Color: 255; 255; 0
+            Head Length: 0.10000000149011612
+            Head Radius: 0.10000000149011612
+            Shaft Length: 0.5
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /drillship/measurement/odom
+          Value: true
+      Enabled: true
+      Name: Drillship
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 16.33730697631836
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: true
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7853981852531433
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 0.7853981852531433
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1519
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000024500000555fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000555000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000555fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000555000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007fd0000003efc0100000002fb0000000800540069006d00650100000000000007fd0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005b20000055500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 2045
+  X: 1313
+  Y: 217
diff --git a/cybership_viz/config/voyager.rviz b/cybership_viz/config/voyager.rviz
index 976167b..b272b25 100644
--- a/cybership_viz/config/voyager.rviz
+++ b/cybership_viz/config/voyager.rviz
@@ -6,12 +6,15 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /Odometry1
         - /Odometry1/Shape1
-        - /Wrench4/Topic1
-        - /RobotModel1/Mass Properties1
-        - /RobotModel1/Description Topic1
+        - /Marker1
+        - /MarkerArray1
+        - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 555
+    Tree Height: 1764
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -53,20 +56,44 @@ Visualization Manager:
       Value: true
     - Class: rviz_default_plugins/TF
       Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        base_link:
+        drillship/aft_center_thruster_link:
           Value: true
-        bow_tunnel_thruster_link:
+        drillship/aft_port_thruster_link:
           Value: true
-        imu_link:
+        drillship/aft_starboard_thruster_link:
           Value: true
-        mocap_link:
+        drillship/base_link:
           Value: true
-        stern_port_thruster_link:
+        drillship/base_link_ned:
           Value: true
-        stern_starboard_thruster_link:
+        drillship/bow_center_thruster_link:
+          Value: true
+        drillship/bow_port_thruster_link:
+          Value: true
+        drillship/bow_starboard_thruster_link:
+          Value: true
+        drillship/imu_link:
+          Value: true
+        drillship/mocap_link:
+          Value: true
+        voyager/base_link:
+          Value: true
+        voyager/base_link_ned:
+          Value: true
+        voyager/bow_tunnel_thruster_link:
+          Value: true
+        voyager/imu_link:
+          Value: true
+        voyager/mocap_link:
+          Value: true
+        voyager/stern_port_thruster_link:
+          Value: true
+        voyager/stern_starboard_thruster_link:
           Value: true
         world:
           Value: true
@@ -77,76 +104,20 @@ Visualization Manager:
       Show Names: false
       Tree:
         world:
-          base_link:
-            bow_tunnel_thruster_link:
-              {}
-            imu_link:
-              {}
-            mocap_link:
-              {}
-            stern_port_thruster_link:
-              {}
-            stern_starboard_thruster_link:
+          voyager/base_link_ned:
+            voyager/base_link:
+              voyager/bow_tunnel_thruster_link:
+                {}
+              voyager/imu_link:
+                {}
+              voyager/stern_port_thruster_link:
+                {}
+              voyager/stern_starboard_thruster_link:
+                {}
+            voyager/mocap_link:
               {}
       Update Interval: 0
       Value: true
-    - Accept NaN Values: false
-      Alpha: 1
-      Arrow Width: 0.5
-      Class: rviz_default_plugins/Wrench
-      Enabled: true
-      Force Arrow Scale: 2
-      Force Color: 85; 255; 255
-      History Length: 1
-      Name: Wrench
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: thruster/port/issued
-      Torque Arrow Scale: 2
-      Torque Color: 204; 204; 51
-      Value: true
-    - Accept NaN Values: false
-      Alpha: 1
-      Arrow Width: 0.5
-      Class: rviz_default_plugins/Wrench
-      Enabled: true
-      Force Arrow Scale: 2
-      Force Color: 85; 255; 255
-      History Length: 1
-      Name: Wrench
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: thruster/starboard/issued
-      Torque Arrow Scale: 2
-      Torque Color: 204; 204; 51
-      Value: true
-    - Accept NaN Values: false
-      Alpha: 1
-      Arrow Width: 0.5
-      Class: rviz_default_plugins/Wrench
-      Enabled: true
-      Force Arrow Scale: 2
-      Force Color: 85; 255; 255
-      History Length: 1
-      Name: Wrench
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: thruster/tunnel/issued
-      Torque Arrow Scale: 2
-      Torque Color: 204; 204; 51
-      Value: true
     - Alpha: 1
       Axes Length: 1
       Axes Radius: 0.10000000149011612
@@ -221,26 +192,32 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: measurement/odom
       Value: true
-    - Accept NaN Values: false
-      Alpha: 1
-      Arrow Width: 0.5
-      Class: rviz_default_plugins/Wrench
+    - Class: rviz_default_plugins/Marker
       Enabled: true
-      Force Arrow Scale: 2
-      Force Color: 0; 170; 255
-      History Length: 1
-      Name: Wrench
+      Name: Marker
+      Namespaces:
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: allocated
-      Torque Arrow Scale: 2
-      Torque Color: 204; 204; 51
+        Value: /voyager/visualization_marker
       Value: true
-    - Alpha: 1
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /voyager/visualization_marker_array
+      Value: true
+    - Alpha: 0.4000000059604645
       Class: rviz_default_plugins/RobotModel
       Collision Enabled: false
       Description File: ""
@@ -263,6 +240,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        base_link_ned:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         bow_tunnel_thruster_link:
           Alpha: 1
           Show Axes: false
@@ -287,7 +268,7 @@ Visualization Manager:
         Inertia: false
         Mass: false
       Name: RobotModel
-      TF Prefix: ""
+      TF Prefix: voyager
       Update Interval: 0
       Value: true
       Visual Enabled: true
@@ -349,7 +330,7 @@ Visualization Manager:
         Z: 0
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
+      Invert Z Axis: true
       Name: Current View
       Near Clip Distance: 0.009999999776482582
       Pitch: 0.7853981852531433
@@ -360,10 +341,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 2055
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000018e000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000031c000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000018e0000076dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000076d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000076dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000076d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ef40000003efc0100000002fb0000000800540069006d0065010000000000000ef40000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000d600000076d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -372,6 +353,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1200
-  X: 2146
-  Y: 537
+  Width: 3828
+  X: -32
+  Y: -32
diff --git a/cybership_viz/launch/multi.launch.py b/cybership_viz/launch/multi.launch.py
new file mode 100644
index 0000000..076781c
--- /dev/null
+++ b/cybership_viz/launch/multi.launch.py
@@ -0,0 +1,41 @@
+import launch
+import launch_ros
+
+import cybership_utilities.launch
+
+# import os
+# from ament_index_python.packages import get_package_share_directory
+# from launch import LaunchDescription
+# from launch.actions import *
+# from launch.substitutions import LaunchConfiguration
+# from launch_ros.actions import Node
+# from launch.launch_description_sources import *
+
+
+def generate_launch_description():
+
+    ld = launch.LaunchDescription()
+
+    for argument in cybership_utilities.launch.COMMON_ARGUMENTS:
+        ld.add_action(argument)
+
+    subsitution_rviz_config_path = launch.substitutions.PathJoinSubstitution(
+        [
+            launch_ros.substitutions.FindPackageShare("cybership_viz"),
+            "config",
+            "multi.rviz",
+        ]
+    )
+
+    node_rviz = launch_ros.actions.Node(
+        package="rviz2",
+        executable="rviz2",
+        name=f"rviz_{cybership_utilities.launch.anon()}",
+        namespace=launch.substitutions.LaunchConfiguration("vessel_name"),
+        output="screen",
+        arguments=["-d", subsitution_rviz_config_path],
+    )
+
+    ld.add_action(node_rviz)
+
+    return ld
diff --git a/dockerfile b/dockerfile
index 7bb0319..ceeef84 100644
--- a/dockerfile
+++ b/dockerfile
@@ -2,7 +2,7 @@ ARG ROS_DISTRO=jazzy
 ARG UID=1000
 ARG GID=1000
 
-FROM ros:$ROS_DISTRO
+FROM ros:${ROS_DISTRO}-ros-base
 
 ARG ROS_DISTRO
 ARG UID
@@ -12,45 +12,46 @@ ENV ROS_DISTRO=${ROS_DISTRO}
 ENV DEBIAN_FRONTEND=noninteractive
 ENV DEBIAN_FRONTEND=teletype
 
-RUN apt-get update && apt-get upgrade -y && apt-get install -y sudo
+RUN sudo apt-get update && \
+    sudo apt-get install -y git ros-dev-tools python3-venv python3-pip && \
+    rosdep update
 
-RUN uid=${UID} && \
+RUN \
+    # Remove existing user with the specified UID if it exists
+    uid=${UID} && \
     if getent passwd $uid > /dev/null; then \
     username=$(getent passwd $uid | cut -d: -f1); \
     userdel -r $username; \
-    fi
-
-RUN groupadd --gid "${GID}" ros_user \
+    fi \
+    && \
+    # Add a new user with the specified UID and GID
+    groupadd --gid "${GID}" ros_user \
     && useradd --uid "${UID}" --gid "${GID}" -m ros_user \
     && usermod -aG sudo ros_user \
     && echo "ros_user ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
 
-RUN mkdir -p /ros/src
-
-WORKDIR /ros
-
-COPY . src/cybership_common
-COPY .git/ src/cybership_common/.git/
+RUN mkdir -p /ros/src && chown -R ros_user:ros_user /ros
 
-RUN chown -R ros_user:ros_user /ros
+COPY . /ros/src/cybership
+COPY .git/ /ros/src/cybership/.git/
 
 USER ros_user
 
-RUN sudo apt-get update && \
-    sudo apt-get install -y git ros-dev-tools && \
-    sudo apt-cache search ${ROS_DISTRO}-rmw \
-    | grep -v -E "dbgsym|connextdds" \
-    | awk '{print $1}' \
-    | xargs sudo apt-get install -y
-
-RUN cd src/cybership_common && git submodule update --init --recursive
+RUN sudo chown -R ros_user:ros_user /ros && \
+    git -C /ros/src/cybership submodule update --init --recursive  && \
+    rosdep update && \
+    rosdep install --from-paths /ros/src --ignore-src -r -y
 
-RUN rosdep update && \
-    rosdep install --from-paths src --ignore-src -r -y
+RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \
+    python3 -m venv /ros/venv --system-site-packages --symlinks && \
+    touch /ros/venv/COLCON_IGNORE && \
+    . /ros/venv/bin/activate && \
+    find /ros/src -name "requirements*txt" -exec pip install -r {} \;
 
-RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.sh && colcon build"
+RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.sh && cd /ros && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release && \
+    echo 'source /ros/install/setup.sh' >> /home/ros_user/.bashrc && \
+    echo 'source /ros/venv/bin/activate' >> /home/ros_user/.bashrc"
 
 SHELL ["/bin/bash"]
-
-ENTRYPOINT ["/bin/bash", "-c", "source /ros/install/setup.sh && exec \"${@}\"", "--"]
+ENTRYPOINT ["/bin/bash", "-c", "source /ros/install/setup.sh && source /ros/venv/bin/activate && exec \"${@}\"", "--"]
 CMD ["bash"]