Enable DDS

docker/Dockerfile

@@ -42,8 +42,10 @@ ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute
 ENV QT_X11_NO_MITSHM=1
 
 # Install some ardupilot and ardupilot_gazebo prereqs
+# openjdk-17-jdk is required for Micro-XRCE-DDS-Gen
 RUN apt-get update \
   && apt-get -y --quiet --no-install-recommends install \
+    openjdk-17-jdk \
     python3-pip \
     python3-wxgtk4.0 \
     rapidjson-dev \
@@ -77,37 +79,30 @@ RUN groupadd --gid $USER_GID $USERNAME \
 USER $USERNAME
 ENV USER=$USERNAME
 
-# Clone ArduSub code
-WORKDIR /home/$USERNAME
-RUN git clone https://github.com/clydemcqueen/ardupilot.git --recurse-submodules \
-  && cd ardupilot \
-  && git checkout 9eb5dc4
-
-# Install ArduSub prereqs (this also appends to .bashrc)
-WORKDIR /home/$USERNAME/ardupilot
-ENV SKIP_AP_EXT_ENV=1 SKIP_AP_GRAPHIC_ENV=1 SKIP_AP_COV_ENV=1 SKIP_AP_GIT_CHECK=1
-RUN Tools/environment_install/install-prereqs-ubuntu.sh -y
-
-# Build ArduSub
-# Note: waf will capture all of the environment variables in ardupilot/.lock-waf_linux_build.
-# Any change to enviroment variables will cause a re-build.
-# To avoid this call sim_vehicle.py with the "--no-rebuild" option.
-WORKDIR /home/$USERNAME/ardupilot
-RUN modules/waf/waf-light configure --board sitl \
-  && modules/waf/waf-light build --target bin/ardusub
-
 # Build for Gazebo Harmonic, define this before building ardupilot_gazebo
 ENV GZ_VERSION=harmonic
 
 # Create colcon workspace and the mount point for orca5
 WORKDIR /home/$USERNAME
 RUN mkdir -p colcon_ws/src/orca5
 
-# Get workspace repos
+# Install MicroXRCEDDSGen, required for ardupilot_sitl build
+WORKDIR /home/$USERNAME/colcon_ws
+RUN git clone --recurse-submodules https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git
+WORKDIR /home/$USERNAME/colcon_ws/Micro-XRCE-DDS-Gen
+RUN JAVA_HOME=/usr/lib/jvm/java-17-openjdk-amd64 ./gradlew assemble
+ENV PATH="/home/$USERNAME/colcon_ws/Micro-XRCE-DDS-Gen/scripts:${PATH}"
+
+# Get workspace repos, this includes ardupilot and micro_ros_agent
 WORKDIR /home/$USERNAME/colcon_ws/src
 COPY --chown=$USER_UID:$USER_GID workspace.repos orca5
 RUN vcs import --recursive < orca5/workspace.repos
 
+# Install ArduSub prereqs (this appends to .bashrc)
+WORKDIR /home/$USERNAME/colcon_ws/src/ardupilot
+ENV SKIP_AP_EXT_ENV=1 SKIP_AP_GRAPHIC_ENV=1 SKIP_AP_COV_ENV=1 SKIP_AP_GIT_CHECK=1
+RUN Tools/environment_install/install-prereqs-ubuntu.sh -y
+
 # Run rosdep over workspace repos
 # Fun fact: rosdep will automatically elevate permissions, but apt-get will not, so it requires sudo
 WORKDIR /home/$USERNAME/colcon_ws

orca_bridge/scripts/geometry.py

@@ -14,12 +14,14 @@
 class Pose:
     """Simple pose object, using 2 tuples"""
 
-    # Static transform camera_sensor (OpenCV) -> camera_link (FLU)
+    # Static transform camera_sensor (OpenCV) -> camera_link (FLU), and inverse
     T_OPENCV_FLU = None
-
-    # Static transform camera_link (FLU) -> camera_sensor (OpenCV)
     T_FLU_OPENCV = None
 
+    # Static transform "standard" to "frame", and inverse
+    T_STANDARD_FRAME = None
+    T_FRAME_STANDARD = None
+
     def __init__(self, p: tuple[float, float, float] = (0., 0., 0.), q: tuple[float, float, float, float] = (1., 0., 0., 0.)):
         self.p: tuple[float, float, float] = p
         self.q: tuple[float, float, float, float] = q
@@ -157,3 +159,6 @@ def __str__(self):
 Pose.T_OPENCV_FLU = Pose()
 Pose.T_OPENCV_FLU.set_euler(0, -math.pi / 2, math.pi / 2)
 Pose.T_FLU_OPENCV = Pose.T_OPENCV_FLU.inverse()
+Pose.T_FRAME_STANDARD = Pose()
+Pose.T_FRAME_STANDARD.set_euler(0, 0, math.pi / 2)
+Pose.T_STANDARD_FRAME = Pose.T_FRAME_STANDARD.inverse()

orca_bridge/scripts/slam.py

@@ -138,7 +138,7 @@ def update(self, msg: orb_slam3_msgs.msg.SlamStatus, sub: sub.Sub, logger):
 
         if self.current_map is None or msg.map_id not in self.maps:
             logger.info(f'Create map {msg.map_id}')
-            t_map_base = sub.t_map_base_ned.ned_to_enu_frame()  # Use axes-only conversion
+            t_map_base = sub.t_map_base.mult(geometry.Pose.T_STANDARD_FRAME)  # Back out the 90d NED->ENU rotation
             self.maps[msg.map_id] = SlamMap(msg.map_id, sub.sonar_rf_distance, slam_rf_distance, t_map_base)
             self.current_map = self.maps[msg.map_id]
         else:

orca_bridge/scripts/slam_bridge.py

@@ -14,6 +14,7 @@
 import pymavlink.mavutil
 import rclpy
 import rclpy.node
+import rclpy.qos
 import rclpy.serialization
 import rclpy.time
 import sensor_msgs.msg
@@ -87,8 +88,11 @@ def __init__(self):
         self.map_sub = self.create_subscription(sensor_msgs.msg.PointCloud2, 'map_points', self.map_callback, 10)
         self.slam_sub = self.create_subscription(orb_slam3_msgs.msg.SlamStatus, 'slam_status', self.slam_callback, 10)
 
+        # /ap/... topics are published best-effort
+        best_effort = rclpy.qos.QoSProfile(reliability=rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT, depth=10)
+        self.ap_pose_sub = self.create_subscription(geometry_msgs.msg.PoseStamped, 'ap/pose/filtered', self.ap_pose_callback, best_effort)
+
         # Publishers
-        self.ekf_pose_pub = self.create_publisher(geometry_msgs.msg.PoseStamped, 'ekf_pose', 10)
         self.ekf_status_pub = self.create_publisher(orca_msgs.msg.FilterStatus, 'ekf_status', 10)
         self.scaled_map_pub = self.create_publisher(sensor_msgs.msg.PointCloud2, 'map_points/scaled', 10)
         self.rf_scale_pub = self.create_publisher(geometry_msgs.msg.PointStamped, 'rf_scale', 10)
@@ -267,10 +271,20 @@ def map_callback(self, msg: sensor_msgs.msg.PointCloud2):
             return
 
         self.scaled_map_pub.publish(slam.scale_cloud(msg, self.maps.current_map.scale))
-
+
+    def ap_pose_callback(self, msg: geometry_msgs.msg.PoseStamped):
+        self.sub.t_map_base = geometry.Pose.from_pose_msg(msg.pose)
+
+        # Publish map -> base_link, note that this is from the EKF, not the SLAM pose
+        tf_msg = geometry_msgs.msg.TransformStamped()
+        tf_msg.header.stamp = msg.header.stamp
+        tf_msg.header.frame_id = 'map'
+        tf_msg.child_frame_id = 'base_link'
+        tf_msg.transform = self.sub.t_map_base.to_transform_msg()
+        self.tf_broadcaster.sendTransform(tf_msg)
+
     def timer_callback(self):
         """Update ArduSub state and publish the results."""
-
         now = self.get_clock().now()
         now_stamp = now.to_msg()
         now_s = time_to_s(now)
@@ -291,22 +305,6 @@ def timer_callback(self):
         ekf_status_msg.airspeed_variance = self.sub.ekf_status_report.airspeed_variance
         self.ekf_status_pub.publish(ekf_status_msg)
 
-        # Publish the ROV pose as determined by the ArduSub EKF
-        t_map_base_from_ekf = self.sub.t_map_base_ned.ned_to_enu_standard()  # Swaps axes and applies 90d yaw rotation
-        ekf_pose_stamped_msg = geometry_msgs.msg.PoseStamped()
-        ekf_pose_stamped_msg.header.stamp = now_stamp
-        ekf_pose_stamped_msg.header.frame_id = 'map'
-        ekf_pose_stamped_msg.pose = t_map_base_from_ekf.to_pose_msg()
-        self.ekf_pose_pub.publish(ekf_pose_stamped_msg)
-
-        # Publish map -> base_link, note that this is from the EKF, not the SLAM pose
-        tf_msg = geometry_msgs.msg.TransformStamped()
-        tf_msg.header.stamp = now_stamp
-        tf_msg.header.frame_id = 'map'
-        tf_msg.child_frame_id = 'base_link'
-        tf_msg.transform = t_map_base_from_ekf.to_transform_msg()
-        self.tf_broadcaster.sendTransform(tf_msg)
-
 
 def main(args=None):
     rclpy.init(args=args)

orca_bridge/scripts/sub.py

@@ -11,18 +11,15 @@ class Sub:
 
     # Listen to these MAVLink messages
     MAVLINK_MSGS = [
-        'ATTITUDE',  # Orientation, always available
         'EKF_STATUS_REPORT',  # Filter status
-        'GLOBAL_POSITION_INT',  # Altitude above home, use if filter_status == ALT_ONLY
-        'LOCAL_POSITION_NED',  # Position, use if filter_status == HORIZ_REL
         'RANGEFINDER',  # Sonar rangefinder
     ]
 
     def __init__(self):
         self.last_heartbeat_s = 0.0
         self.ekf_status_report = orca_msgs.msg.FilterStatus()  # Empty message
         self.ekf_status_time: float | None = None
-        self.t_map_base_ned = geometry.Pose()
+        self.t_map_base = geometry.Pose()
         self.sonar_rf_distance = None
 
     def ekf_const_pos(self):
@@ -47,19 +44,12 @@ def update(self, conn, now_s: float, logger):
                 break
 
             msg_type = msg.get_type()
-            if msg_type == 'ATTITUDE':
-                self.t_map_base_ned.set_euler(msg.roll, msg.pitch, msg.yaw)
-            elif msg_type == 'EKF_STATUS_REPORT':
+            if msg_type == 'EKF_STATUS_REPORT':
                 old_flags = self.ekf_status_report.flags
                 self.ekf_status_report = msg
                 self.ekf_status_time = now_s
                 if old_flags != self.ekf_status_report.flags:
                     logger.info(f'EKF status change {old_flags} => {self.ekf_status_report.flags}, const_pos={self.ekf_const_pos()}, horiz_rel={self.ekf_horiz_rel()}')
-            elif msg_type == 'GLOBAL_POSITION_INT':
-                if self.ekf_const_pos():
-                    self.t_map_base_ned.set_altitude(-msg.relative_alt / 1e3)  # Height above home, flip sign for NED
-            elif msg_type == 'LOCAL_POSITION_NED':
-                self.t_map_base_ned.set_position(msg.x, msg.y, msg.z)
             elif msg_type == 'RANGEFINDER':
                 # Hack to support testing
                 self.sonar_rf_distance = msg.distance if msg.distance > 0.0 else 1.0

orca_bridge/scripts/test_pose.py

@@ -281,6 +281,17 @@ def test_large_position_values(self):
         pose_back = pose_enu.enu_to_ned_frame()
         self.assertPoseAlmostEqual(pose_ned, pose_back)
 
+    def test_standard_frame_static_transforms(self):
+        """Test T_STANDARD_FRAME and T_FRAME_STANDARD static transforms."""
+        pose_enu_standard = Pose(p=(1.0, 2.0, 3.0), q=(1.0, 0.0, 0.0, 0.0))
+
+        pose_enu_frame = pose_enu_standard.enu_to_ned_standard().ned_to_enu_frame()
+        pose_enu_frame_method2 = pose_enu_standard.mult(Pose.T_STANDARD_FRAME)
+        self.assertPoseAlmostEqual(pose_enu_frame, pose_enu_frame_method2)
+
+        pose_enu_standard = pose_enu_frame.enu_to_ned_frame().ned_to_enu_standard()
+        pose_enu_standard_method2 = pose_enu_frame.mult(Pose.T_FRAME_STANDARD)
+        self.assertPoseAlmostEqual(pose_enu_standard, pose_enu_standard_method2)
 
 if __name__ == '__main__':
     unittest.main()

orca_bringup/launch/bench.launch.py

@@ -88,9 +88,9 @@ def generate_launch_description():
             cmd=[
                 'ros2', 'bag', 'record',
                 '--include-hidden-topics',
+                '/ap/pose/filtered',
                 '/camera_info',
                 '/camera_pose',
-                '/ekf_pose',
                 '/ekf_status',
                 '/rf_scale',
                 '/rosout',

orca_bringup/launch/sim.launch.py

@@ -20,6 +20,10 @@ def generate_launch_description():
     orca_bringup_dir = get_package_share_directory('orca_bringup')
     sub_parm_file = os.path.join(orca_bringup_dir, 'config', 'sub.parm')
 
+    ardupilot_sitl_dir = get_package_share_directory('ardupilot_sitl')
+    udp_dds_parm_file = os.path.join(ardupilot_sitl_dir, 'config', 'default_params', 'dds_udp.parm')
+    ardusub_param_files = f'{sub_parm_file},{udp_dds_parm_file}'
+
     nodes = [
         DeclareLaunchArgument(
             'ardusub',
@@ -113,9 +117,9 @@ def generate_launch_description():
             cmd=[
                 'ros2', 'bag', 'record',
                 '--include-hidden-topics',
+                '/ap/pose/filtered',
                 '/camera_info',
                 '/camera_pose',
-                '/ekf_pose',
                 '/ekf_status',
                 '/model/orca5/odometry',
                 '/rf_scale',
@@ -153,11 +157,20 @@ def generate_launch_description():
             }.items(),
         ),
 
-        # Launch ArduSub w/ SIM_JSON. Make sure ardusub is on the $PATH. To use the heavy (6dof) model: specify
-        # vectored_6dof as the model, AND the default params must set magic ArduSub parameter FRAME_CONFIG to 2.0.
-        # Yaw is provided by Gazebo, so the start yaw value is ignored.
+        # Launch the Micro ROS agent
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(ardupilot_sitl_dir, 'launch', 'micro_ros_agent.launch.py')),
+            launch_arguments={
+                "transport": "udp4",
+            }.items(),
+            condition=IfCondition(LaunchConfiguration('ardusub')),
+        ),
+
+        # Launch ArduSub w/ SIM_JSON. To use the heavy (6dof) model: specify vectored_6dof as the model, AND the default
+        # params must set magic ArduSub parameter FRAME_CONFIG to 2.0. Yaw is provided by Gazebo, so the initial yaw
+        # value is ignored.
         ExecuteProcess(
-            cmd=['ardusub', '-S', '-w', '-M', 'JSON', '--defaults', sub_parm_file,
+            cmd=['ardusub', '-S', '-w', '-M', 'JSON', '--defaults', ardusub_param_files,
                  '-I0', '--home', '47.6302,-122.3982391,-0.1,0'],
             output='screen',
             condition=IfCondition(LaunchConfiguration('ardusub')),

workspace.repos

@@ -9,3 +9,13 @@ repositories:
     type: git
     url: https://github.com/clydemcqueen/orb_slam3_ros.git
     version: ce6ec19
+
+  micro_ros_agent:
+    type: git
+    url: https://github.com/micro-ROS/micro-ROS-Agent.git
+    version: 52abdf5
+
+  ardupilot:
+    type: git
+    url: https://github.com/clydemcqueen/ardupilot.git
+    version: 9eb5dc4