removed prefix in enums in messages and changed to use existing functions for string and quaternion convert

Author: Crowdedlight

mavros_extras/src/plugins/gimbal_control.cpp

@@ -257,12 +257,8 @@ class GimbalControlPlugin : public plugin::Plugin
     gimbal_attitude_msg.target_system = mo.target_system;
     gimbal_attitude_msg.target_component = mo.target_component;
     gimbal_attitude_msg.flags = mo.flags;
-    geometry_msgs::msg::Quaternion q;
-    q.w = mo.q[0];
-    q.x = mo.q[1];
-    q.y = mo.q[2];
-    q.z = mo.q[3];
-    gimbal_attitude_msg.q = q;
+    auto q = mavros::ftf::mavlink_to_quaternion(mo.q);
+    gimbal_attitude_msg.q = tf2::toMsg(q);
     gimbal_attitude_msg.angular_velocity_x = mo.angular_velocity_x;
     gimbal_attitude_msg.angular_velocity_y = mo.angular_velocity_y;
     gimbal_attitude_msg.angular_velocity_z = mo.angular_velocity_z;
@@ -298,20 +294,6 @@ class GimbalControlPlugin : public plugin::Plugin
     gimbal_manager_status_pub->publish(gimbal_manager_status_msg);
   }
 
-  /**
-   * @brief Helper function for converting char arrays from GimbalDeviceInformation msg to string
-   * 
-   * @param a - array of chars from GimbalDeviceInformation Mavlink msg
-   * @param size - The size of the char array
-   */
-  std::string convertToString(std::array<char, 32> a, int size) {
-    std::string s = "";
-    for (int i=0; i<size; i++) {
-      s += a[i];
-    }
-    return s;
-  }
-
   /**
    * @brief Publish gimbal device information - Note: this message is only published on request by default (see device_get_info_cb)
    *
@@ -326,9 +308,9 @@ class GimbalControlPlugin : public plugin::Plugin
   {
     mavros_msgs::msg::GimbalDeviceInformation gimbal_device_information_msg;
     gimbal_device_information_msg.header = uas->synchronized_header(frame_id, di.time_boot_ms);
-    gimbal_device_information_msg.vendor_name = convertToString(di.vendor_name, sizeof(di.vendor_name));
-    gimbal_device_information_msg.model_name = convertToString(di.model_name, sizeof(di.model_name));
-    gimbal_device_information_msg.custom_name = convertToString(di.custom_name, sizeof(di.custom_name));
+    gimbal_device_information_msg.vendor_name = mavlink::to_string(di.vendor_name);
+    gimbal_device_information_msg.model_name = mavlink::to_string(di.model_name);
+    gimbal_device_information_msg.custom_name = mavlink::to_string(di.custom_name);
     gimbal_device_information_msg.firmware_version = di.firmware_version;
     gimbal_device_information_msg.hardware_version = di.hardware_version;
     gimbal_device_information_msg.uid = di.uid;

mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg

@@ -8,11 +8,11 @@ uint8 target_component          # Component ID
 
 uint16 flags                    # Current gimbal flags set (bitwise) - See GIMBAL_DEVICE_FLAGS
 #GIMBAL_DEVICE_FLAGS
-uint16 GIMBAL_DEVICE_FLAGS_RETRACT = 1      # Set to retracted safe position (no stabilization), takes presedence over all other flags.
-uint16 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2      # Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (pitch=yaw=0) but may be any orientation.
-uint16 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4    # Lock roll angle to absolute angle relative to horizon (not relative to drone). This is generally the default with a stabilizing gimbal.
-uint16 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8   # Lock pitch angle to absolute angle relative to horizon (not relative to drone). This is generally the default.
-uint16 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16    # Lock yaw angle to absolute angle relative to North (not relative to drone). If this flag is set, the quaternion is in the Earth frame with the x-axis pointing North (yaw absolute). If this flag is not set, the quaternion frame is in the Earth frame rotated so that the x-axis is pointing forward (yaw relative to vehicle).
+uint16 FLAGS_RETRACT = 1      # Set to retracted safe position (no stabilization), takes presedence over all other flags.
+uint16 FLAGS_NEUTRAL = 2      # Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (pitch=yaw=0) but may be any orientation.
+uint16 FLAGS_ROLL_LOCK = 4    # Lock roll angle to absolute angle relative to horizon (not relative to drone). This is generally the default with a stabilizing gimbal.
+uint16 FLAGS_PITCH_LOCK = 8   # Lock pitch angle to absolute angle relative to horizon (not relative to drone). This is generally the default.
+uint16 FLAGS_YAW_LOCK = 16    # Lock yaw angle to absolute angle relative to North (not relative to drone). If this flag is set, the quaternion is in the Earth frame with the x-axis pointing North (yaw absolute). If this flag is not set, the quaternion frame is in the Earth frame rotated so that the x-axis is pointing forward (yaw relative to vehicle).
 
 geometry_msgs/Quaternion q      # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
 float32 angular_velocity_x      # X component of angular velocity (NaN if unknown)
@@ -21,12 +21,12 @@ float32 angular_velocity_z      # Z component of angular velocity (NaN if unknow
 
 uint32 failure_flags            # Failure flags (0 for no failure) (bitwise) - See GIMBAL_DEVICE_ERROR_FLAGS
 #GIMBAL_DEVICE_ERROR_FLAGS
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_AT_ROLL_LIMIT = 1          # Gimbal device is limited by hardware roll limit.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_AT_PITCH_LIMIT = 2         # Gimbal device is limited by hardware pitch limit.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_AT_YAW_LIMIT = 4           # Gimbal device is limited by hardware yaw limit.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR = 8          # There is an error with the gimbal encoders.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR = 16           # There is an error with the gimbal power source.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR = 32           # There is an error with the gimbal motor's.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR = 64        # There is an error with the gimbal's software.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR = 128          # There is an error with the gimbal's communication.
-uint32 GIMBAL_DEVICE_ERROR_FLAGS_CALIBRATION_RUNNING = 256  # Gimbal is currently calibrating.
+uint32 ERROR_FLAGS_AT_ROLL_LIMIT = 1          # Gimbal device is limited by hardware roll limit.
+uint32 ERROR_FLAGS_AT_PITCH_LIMIT = 2         # Gimbal device is limited by hardware pitch limit.
+uint32 ERROR_FLAGS_AT_YAW_LIMIT = 4           # Gimbal device is limited by hardware yaw limit.
+uint32 ERROR_FLAGS_ENCODER_ERROR = 8          # There is an error with the gimbal encoders.
+uint32 ERROR_FLAGS_POWER_ERROR = 16           # There is an error with the gimbal power source.
+uint32 ERROR_FLAGS_MOTOR_ERROR = 32           # There is an error with the gimbal motor's.
+uint32 ERROR_FLAGS_SOFTWARE_ERROR = 64        # There is an error with the gimbal's software.
+uint32 ERROR_FLAGS_COMMS_ERROR = 128          # There is an error with the gimbal's communication.
+uint32 ERROR_FLAGS_CALIBRATION_RUNNING = 256  # Gimbal is currently calibrating.

mavros_msgs/msg/GimbalDeviceInformation.msg

@@ -12,18 +12,18 @@ uint64 uid # UID of gimbal hardware (0 if unknown).
 
 uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_DEVICE_CAP_FLAGS
 #GIMBAL_DEVICE_CAP_FLAGS
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1 # Gimbal device supports a retracted position
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2 # Gimbal device supports a horizontal, forward looking position, stabilized
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS = 4 # Gimbal device supports rotating around roll axis.
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Gimbal device supports to follow a roll angle relative to the vehicle
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK = 16 # Gimbal device supports locking to an roll angle (generally that's the default with roll stabilized)
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS = 32 # Gimbal device supports rotating around pitch axis.
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Gimbal device supports to follow a pitch angle relative to the vehicle
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK = 128 # Gimbal device supports locking to an pitch angle (generally that's the default with pitch stabilized)
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256 # Gimbal device supports rotating around yaw axis.
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default)
-uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024 # Gimbal device supports locking to an absolute heading (often this is an option available)
-uint32 GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Gimbal device supports yawing/panning infinetely (e.g. using slip disk).
+uint32 CAP_FLAGS_HAS_RETRACT = 1 # Gimbal device supports a retracted position
+uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Gimbal device supports a horizontal, forward looking position, stabilized
+uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Gimbal device supports rotating around roll axis.
+uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Gimbal device supports to follow a roll angle relative to the vehicle
+uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Gimbal device supports locking to an roll angle (generally that's the default with roll stabilized)
+uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Gimbal device supports rotating around pitch axis.
+uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Gimbal device supports to follow a pitch angle relative to the vehicle
+uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Gimbal device supports locking to an pitch angle (generally that's the default with pitch stabilized)
+uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Gimbal device supports rotating around yaw axis.
+uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default)
+uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Gimbal device supports locking to an absolute heading (often this is an option available)
+uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Gimbal device supports yawing/panning infinetely (e.g. using slip disk).
 
 uint16 custom_cap_flags # Bitmap for use for gimbal-specific capability flags.
 float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)

mavros_msgs/msg/GimbalDeviceSetAttitude.msg

@@ -6,11 +6,11 @@ uint8 target_component      # Component ID
 
 uint16 flags                # Low level gimbal flags (bitwise) - See GIMBAL_DEVICE_FLAGS
 #GIMBAL_DEVICE_FLAGS
-uint16 GIMBAL_DEVICE_FLAGS_RETRACT = 1     # Based on GIMBAL_DEVICE_FLAGS_RETRACT
-uint16 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2     # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
-uint16 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4   # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
-uint16 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8  # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
-uint16 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16   # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+uint16 FLAGS_RETRACT = 1     # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint16 FLAGS_NEUTRAL = 2     # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint16 FLAGS_ROLL_LOCK = 4   # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint16 FLAGS_PITCH_LOCK = 8  # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint16 FLAGS_YAW_LOCK = 16   # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
 
 geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
 float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored.

mavros_msgs/msg/GimbalManagerInformation.msg

@@ -5,20 +5,20 @@ std_msgs/Header header
 
 uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_MANAGER_CAP_FLAGS
 #GIMBAL_MANAGER_CAP_FLAGS
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT = 1 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS = 4 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK = 16 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS = 32 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK = 128 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS = 256 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK = 1024 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 # Gimbal manager supports to point to a local position.
-uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 # Gimbal manager supports to point to a global latitude, longitude, altitude position.
+uint32 CAP_FLAGS_HAS_RETRACT = 1 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT.
+uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL.
+uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS.
+uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW.
+uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK.
+uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS.
+uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW.
+uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK.
+uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS.
+uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW.
+uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK.
+uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW.
+uint32 CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 # Gimbal manager supports to point to a local position.
+uint32 CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 # Gimbal manager supports to point to a global latitude, longitude, altitude position.
 
 uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for.
 float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)