fix(ardupilot_node, pose_estimation_node): Fix gimbal quaternion handling when gimbal pitch is lower than -90 degrees

Fixes naive interpretation of Euler roll and pitch whan handling cases where the gimbal pitch goes lower than -90 degrees.

[skip ci]

Author: hmakelin

gisnav/nodes/ardupilot_node.py

@@ -57,6 +57,7 @@ def _vehicle_nav_sat_fix_callback(self, msg: NavSatFix) -> None:
         self._vehicle_nav_sat_fix = msg
         self.publish_vehicle_geopose()
         self.publish_vehicle_altitude()
+        self.publish_gimbal_quaternion()  # TODO: temporarily assuming static camera so publishing gimbal quat here
 
     def _vehicle_pose_stamped_callback(self, msg: PoseStamped) -> None:
         """Handles latest :class:`mavros_msgs.msg.PoseStamped` message
@@ -102,16 +103,15 @@ def vehicle_geopose(self) -> Optional[GeoPoseStamped]:
             latitude, longitude = self._vehicle_nav_sat_fix.latitude, self._vehicle_nav_sat_fix.longitude
             altitude = self._vehicle_nav_sat_fix.altitude
 
-            # Attitude
-            attitude_ned_q = np.array([self._vehicle_pose_stamped.pose.orientation.y,
-                                       self._vehicle_pose_stamped.pose.orientation.x,
-                                       -self._vehicle_pose_stamped.pose.orientation.z,
-                                       self._vehicle_pose_stamped.pose.orientation.w])
-
             # Convert ENU->NED + re-center yaw
-            attitude_ned = Rotation.from_quat(attitude_ned_q) * Rotation.from_rotvec(np.array([0, 0, np.pi / 2]))
-            #attitude_ned = Attitude(attitude_ned.as_quat(), extrinsic=True)
-            orientation = messaging.as_ros_quaternion(attitude_ned.as_quat())
+            enu_to_ned = Rotation.from_euler('XYZ', np.array([np.pi, 0, np.pi / 2]))
+            attitude_ned = Rotation.from_quat(messaging.as_np_quaternion(self._vehicle_pose_stamped.pose.orientation)) \
+                * enu_to_ned.inv()
+            rpy = attitude_ned.as_euler('XYZ', degrees=True)
+            rpy[0] = (rpy[0] + 180) % 360
+            attitude_ned = Rotation.from_euler('XYZ', rpy, degrees=True)
+            attitude_ned = attitude_ned.as_quat()
+            orientation = messaging.as_ros_quaternion(attitude_ned)
 
             return GeoPoseStamped(header=messaging.create_header('base_link'),
                                   pose=GeoPose(
@@ -165,11 +165,11 @@ def gimbal_quaternion(self) -> Optional[Quaternion]:
         """
         # TODO: assumes static nadir facing camera, do proper implementation
         if self.vehicle_geopose is not None:
-            vehicle_attitude = Attitude(q=messaging.as_np_quaternion(self.vehicle_geopose.pose.orientation))
-            vehicle_attitude = vehicle_attitude.as_rotation()
-            gimbal_attitude = vehicle_attitude * Rotation.from_euler('XYZ', [0, -np.pi / 2, 0])
-            gimbal_attitude = messaging.as_ros_quaternion(gimbal_attitude.as_quat())
-            return gimbal_attitude
+            vehicle_attitude = Rotation.from_quat(messaging.as_np_quaternion(self.vehicle_geopose.pose.orientation))
+            gimbal_rpy = vehicle_attitude.as_euler('xyz', degrees=True)
+            gimbal_rpy[1] -= 90
+            gimbal_attitude = Rotation.from_euler('xyz', gimbal_rpy, degrees=True)
+            return messaging.as_ros_quaternion(gimbal_attitude.as_quat())
         else:
             self.get_logger().warn('Vehicle GeoPose unknown, cannot determine gimbal quaternion for static camera.')
             return None

gisnav/nodes/bbox_node.py

@@ -235,7 +235,7 @@ def _guess_fov_center(self, xy: GeoPt) -> Optional[GeoPt]:
         :return: Center of the projected FOV, or None if not available
         """
         if self._gimbal_quaternion is None:
-            self.get_logger().warn('Gimbal set attitude not available, cannot project gimbal FOV.')
+            self.get_logger().warn('Gimbal quaternion not available, cannot project gimbal FOV.')
             return None
         else:
             gimbal_attitude = Attitude(q=messaging.as_np_quaternion(self._gimbal_quaternion))

gisnav/nodes/pose_estimation_node.py

@@ -193,9 +193,19 @@ def _contextual_map_data(self) -> Optional[ContextualMapData]:
         # Get cropped and rotated map
         if self._gimbal_quaternion is not None:
             gimbal_attitude = Attitude(q=messaging.as_np_quaternion(self._gimbal_quaternion), extrinsic=True)
+            roll = gimbal_attitude.roll
             camera_yaw = gimbal_attitude.yaw
+            if abs(roll) > np.pi / 2:
+                # Assume camera pitch (NED frame) is lower than -90 degrees
+                # A naive interpretation of the Euler angles would assume the camera (and the vehicle) is upside down
+                # cause the map being rotated incorrectly by 180 degrees compared to what is needed for pose estimation
+                # This might happen e.g. if gimbal has been pointing straight down nadir and vehicle body has suddenly
+                # gained additional negative pitch before gimbal has had time to stabilize
+                self.get_logger().debug(f'Gimbal absolute Euler roll angle over 90 degrees: assuming gimbal is not '
+                                        f'upside down and that gimbal absolute pitch is over 90 degrees instead.')
+                camera_yaw = (camera_yaw + np.pi / 2) % (2 * np.pi)
             assert_type(camera_yaw, float)
-            assert -np.pi <= camera_yaw <= np.pi, f'Unexpected gimbal yaw value: {camera_yaw} ([-pi, pi] expected).'
+            assert -2 * np.pi <= camera_yaw <= 2 * np.pi, f'Unexpected gimbal yaw value: {camera_yaw} ([-2*pi, 2*pi] expected).'
         else:
             self.get_logger().warn(f'Camera yaw unknown, cannot estimate pose.')
             return None
@@ -269,7 +279,7 @@ def image_callback(self, msg: Image) -> None:
                 pose = Pose(*pose)
                 self._post_process_pose(pose, image_pair)
             else:
-                self.get_logger().warn(f'Worker did not return a pose, skipping this frame.')
+                self.get_logger().warn(f'Could not estimate a pose, skipping this frame.')
                 return None
 
     def _orthoimage_3d_callback(self, msg: OrthoImage3D) -> None: