new default drone for tests and setup

.github/workflows/ros-node-tests.yml

@@ -16,7 +16,6 @@ jobs:
       setup_script: "tests/setup.sh"
       test_scripts: '[
         "tests/ros_node_tests/dp_node_test.sh",
-        "tests/ros_node_tests/lqr_autopilot_node_test.sh",
         "tests/ros_node_tests/reference_filter_node_test.sh",
         "tests/ros_node_tests/los_node_test.sh",
         ]'

.github/workflows/simulator-test.yml

@@ -17,6 +17,5 @@ jobs:
       pre_test_script: "scripts/ci_install_dependencies.sh"
       test_scripts: '[
         "tests/simulator_tests/waypoint_navigation/simulator_test.sh",
-        "tests/simulator_tests/los_test/simulator_test.sh",
         "tests/simulator_tests/waypoint_manager_test/simulator_test.sh",
         ]'

auv_setup/auv_setup/launch_arg_common.py

@@ -2,7 +2,7 @@
 from launch.substitutions import LaunchConfiguration
 
 
-def declare_drone_and_namespace_args(default_drone="orca"):
+def declare_drone_and_namespace_args(default_drone="nautilus"):
     return [
         DeclareLaunchArgument(
             "drone",

auv_setup/config/robots/moby.yaml → auv_setup/config/robots/nautilus.yaml

@@ -1,4 +1,4 @@
-# This file defines parameters specific to Moby.
+# This file defines parameters specific to Nautilus.
 # When looking at the AUV from above, the thruster placement is:
 #
 #        front
@@ -101,15 +101,16 @@
       temperature: "temperature"
       joy: "joy"
       pose: "pose"
-      odom: "odom"
       twist: "twist"
+      odom: "odom"
       operation_mode: "operation_mode"
       killswitch: "killswitch"
       reference_pose: "reference_pose"
+      landmarks: "landmarks"
+      waypoint: "waypoint"
       guidance:
         los: "guidance/los"
         dp: "guidance/dp"
-      waypoint: "waypoint"
       dvl_twist: "dvl/twist"
       dvl_altitude: "dvl/altitude"
       imu: "imu/data_raw"
@@ -121,10 +122,14 @@
     action_servers:
       reference_filter: "reference_filter"
       los: "los_guidance"
+      landmark_polling: "landmark_polling"
+      landmark_convergence: "landmark_convergence"
+      waypoint_manager: "waypoint_manager"
 
     services:
       set_operation_mode: "set_operation_mode"
       set_killswitch: "set_killswitch"
       toggle_killswitch: "toggle_killswitch"
       get_operation_mode: "get_operation_mode"
+      waypoint_addition: "waypoint_addition"
       start_mission: "start_mission"

auv_setup/description/moby.urdf.xacro → auv_setup/description/nautilus.urdf.xacro

@@ -71,7 +71,7 @@
 
   <link name="arm"></link>
 
-  <joint name="moby/arm_joint" type="revolute">
+  <joint name="nautilus/arm_joint" type="revolute">
     <parent link="shoulder"/>
     <child link="arm"/>
     <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
@@ -82,7 +82,7 @@
   <link name="finger1"></link>
 
 
-  <joint name="moby/finger_joint1" type="prismatic">
+  <joint name="nautilus/finger_joint1" type="prismatic">
     <parent link="arm"/>
     <child link="finger1"/>
     <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
@@ -92,7 +92,7 @@
 
   <link name="finger2"></link>
 
-  <joint name="moby/finger_joint2" type="prismatic">
+  <joint name="nautilus/finger_joint2" type="prismatic">
     <parent link="arm"/>
     <child link="finger2"/>
     <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>

auv_setup/launch/autopilot.launch.py

@@ -3,50 +3,63 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
-    DeclareLaunchArgument,
-    IncludeLaunchDescription,
     OpaqueFunction,
 )
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+from auv_setup.launch_arg_common import (
+    declare_drone_and_namespace_args,
+    resolve_drone_and_namespace,
+)
 
 
 def launch_setup(context, *args, **kwargs):
-    drone = LaunchConfiguration("drone").perform(context)
-
-    los_guidance_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(
-                get_package_share_directory("los_guidance"),
-                "launch",
-                "los_guidance.launch.py",
-            )
-        ),
-        launch_arguments={"drone": drone}.items(),
+    drone, namespace = resolve_drone_and_namespace(context)
+
+    velocity_lqr_config = os.path.join(
+        get_package_share_directory("velocity_controller_lqr"),
+        "config",
+        "param_velocity_controller_lqr.yaml",
+    )
+
+    los_config = os.path.join(
+        get_package_share_directory("los_guidance"),
+        "config",
+        "guidance_params.yaml",
+    )
+
+    drone_params = os.path.join(
+        get_package_share_directory("auv_setup"),
+        "config",
+        "robots",
+        f"{drone}.yaml",
+    )
+
+    los_node = Node(
+        package="los_guidance",
+        executable="los_guidance_node",
+        name="los_guidance_node",
+        namespace=namespace,
+        parameters=[drone_params, los_config],
+        output="screen",
     )
 
-    autopilot_controller_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(
-                get_package_share_directory("velocity_controller_lqr"),
-                "launch",
-                "velocity_controller_lqr.launch.py",
-            )
-        ),
-        launch_arguments={"drone": drone}.items(),
+    lqr_node = Node(
+        package="velocity_controller_lqr",
+        executable="velocity_controller_lqr_node.py",
+        name="velocity_controller_lqr_node",
+        namespace=namespace,
+        output="screen",
+        parameters=[drone_params, velocity_lqr_config],
     )
 
-    return [los_guidance_launch, autopilot_controller_launch]
+    return [los_node, lqr_node]
 
 
-def generate_launch_description() -> LaunchDescription:
+def generate_launch_description():
     return LaunchDescription(
-        [
-            DeclareLaunchArgument(
-                "drone",
-                default_value="orca",
-                description="Drone name / namespace",
-            ),
+        declare_drone_and_namespace_args()
+        + [
             OpaqueFunction(function=launch_setup),
         ]
     )

guidance/los_guidance/launch/los_guidance.launch.py

@@ -14,7 +14,7 @@
 def launch_setup(context, *args, **kwargs):
     drone, namespace = resolve_drone_and_namespace(context)
 
-    adapt_params = os.path.join(
+    los_config = os.path.join(
         get_package_share_directory("los_guidance"),
         "config",
         "guidance_params.yaml",
@@ -33,7 +33,7 @@ def launch_setup(context, *args, **kwargs):
             executable="los_guidance_node",
             name="los_guidance_node",
             namespace=namespace,
-            parameters=[drone_params, adapt_params],
+            parameters=[drone_params, los_config],
             output="screen",
         )
     ]

motion/thrust_allocator_auv/tests/CMakeLists.txt

@@ -13,7 +13,7 @@ add_executable(
 
 target_compile_definitions(thrust_allocator_auv_test PRIVATE
   # YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../../../auv_setup/config/robots/orca.yaml"
-  YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../../../auv_setup/config/robots/moby.yaml"
+  YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../../../auv_setup/config/robots/nautilus.yaml"
 )
 
 target_link_libraries(

motion/thrust_allocator_auv/tests/thrust_allocator_tests.cpp

@@ -34,7 +34,7 @@ void print_generalized_force(const Vector6d& tau) {
     spdlog::info("Yaw moment: {}", tau[5]);
 }
 
-//  When looking at the AUV from above (MOBY)
+//  When looking at the AUV from above (Nautilus)
 //
 //         front
 //        |======|
@@ -628,7 +628,7 @@ TEST_F(QPAllocatorTests, Q12_QP_SurgePlusAndYawPlus_UsesHorizontalThrusters) {
 
     const double eps_active = 0.1;  // must be "meaningfully used"
 
-    // For moby geometry, surge+yaw reinforces T4 and T7 while T0 and T3
+    // For nautilus geometry, surge+yaw reinforces T4 and T7 while T0 and T3
     // cancel each other out
     EXPECT_GT(std::abs(u[4]), eps_active);
     EXPECT_GT(std::abs(u[7]), eps_active);

tests/ros_node_tests/dp_node_test.sh

@@ -2,6 +2,9 @@
 set -e
 set -o pipefail
 
+# Drone name
+DRONE_ARG=${1:-"nautilus"}
+
 # Load ROS 2 environment
 echo "Setting up ROS 2 environment..."
 . /opt/ros/humble/setup.sh
@@ -34,12 +37,12 @@ fi
 
 # Set operation mode
 echo "Turning off killswitch and setting operation mode to autonomous mode"
-ros2 service call /orca/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}"
-ros2 service call /orca/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}"
+ros2 service call /$DRONE_ARG/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}"
+ros2 service call /$DRONE_ARG/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}"
 
 # Check if controller correctly publishes tau
 echo "Waiting for wrench data..."
-timeout 10s ros2 topic echo /orca/wrench_input --once
+timeout 10s ros2 topic echo /$DRONE_ARG/wrench_input --once
 echo "Got wrench data"
 
 # Terminate processes

tests/ros_node_tests/los_node_test.sh

@@ -2,6 +2,9 @@
 set -e
 set -o pipefail
 
+# Drone name
+DRONE_ARG=${1:-"nautilus"}
+
 # Load ROS 2 environment
 echo "Setting up ROS 2 environment..."
 . /opt/ros/humble/setup.sh
@@ -28,10 +31,10 @@ fi
 
 # Send action goal
 echo "Sending goal..."
-ros2 action send_goal /orca/los_guidance vortex_msgs/action/LOSGuidance "{goal: {point: {x: 20.0, y: 20.0, z: 5.0}}}" &
+ros2 action send_goal /$DRONE_ARG/los_guidance vortex_msgs/action/LOSGuidance "{goal: {point: {x: 20.0, y: 20.0, z: 5.0}}}" &
 # Check if node correctly publishes guidance
 echo "Waiting for guidance data..."
-timeout 10s ros2 topic echo /orca/guidance/los --once
+timeout 10s ros2 topic echo /$DRONE_ARG/guidance/los --once
 echo "Got guidance data"
 
 # Terminate processes

tests/ros_node_tests/lqr_autopilot_node_test.sh

@@ -2,6 +2,9 @@
 set -e
 set -o pipefail
 
+# Drone name
+DRONE_ARG=${1:-"nautilus"}
+
 # Load ROS 2 environment
 echo "Setting up ROS 2 environment..."
 . /opt/ros/humble/setup.sh
@@ -35,13 +38,13 @@ fi
 
 # Set operation mode
 echo "Turning off killswitch and setting operation mode to autonomous mode"
-ros2 service call /orca/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}"
-ros2 service call /orca/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}"
+ros2 service call /$DRONE_ARG/set_killswitch vortex_msgs/srv/SetKillswitch "{killswitch_on: false}"
+ros2 service call /$DRONE_ARG/set_operation_mode vortex_msgs/srv/SetOperationMode "{requested_operation_mode: {operation_mode: 1}}"
 
-sleep 2
+sleep 5
 # Check if controller correctly publishes tau
 echo "Waiting for wrench data..."
-timeout 20s ros2 topic echo /orca/wrench_input --once --qos-reliability best_effort
+timeout 20s ros2 topic echo /$DRONE_ARG/wrench_input --once --qos-reliability best_effort
 echo "Got wrench data"
 
 # Terminate processes

tests/ros_node_tests/reference_filter_node_test.sh

@@ -2,6 +2,9 @@
 set -e
 set -o pipefail
 
+# Drone name
+DRONE_ARG=${1:-"nautilus"}
+
 # Load ROS 2 environment
 echo "Setting up ROS 2 environment..."
 . /opt/ros/humble/setup.sh
@@ -28,14 +31,14 @@ fi
 
 # Send action goal
 echo "Sending goal..."
-ros2 action send_goal /orca/reference_filter vortex_msgs/action/ReferenceFilterWaypoint \
+ros2 action send_goal /$DRONE_ARG/reference_filter vortex_msgs/action/ReferenceFilterWaypoint \
     "{waypoint: {pose: {position: {x: 1.0,y: 0.0,z: 0.0}, orientation:{x: 0,y: 0,z: 0,w: 1}}, waypoint_mode: {mode: 0}}}" &
 
 sleep 2
 
 # Check if controller correctly publishes guidance
 echo "Waiting for guidance data..."
-timeout 10s ros2 topic echo /orca/guidance/dp --once
+timeout 10s ros2 topic echo /$DRONE_ARG/guidance/dp --once
 echo "Got guidance data"
 
 # Terminate processes

tests/simulator_tests/los_test/send_goal.py

@@ -8,7 +8,7 @@ class LOSGuidanceClient(Node):
     def __init__(self):
         super().__init__('los_guidance_client')
         # Create the action client
-        self._action_client = ActionClient(self, LOSGuidance, '/orca/los_guidance')
+        self._action_client = ActionClient(self, LOSGuidance, '/nautilus/los_guidance')
         self.send_goal()
 
     def send_goal(self):