Skip to content

Commit

Permalink
removed prefix in enums in messages and changed to use existing funct…
Browse files Browse the repository at this point in the history 
…ions for string and quaternion convert
  • Loading branch information
@Crowdedlight @vooon
Crowdedlight authored and vooon committed Jun 6, 2024
1 parent 9635084 commit 5646ba4
Show file tree
Hide file tree
Showing 5 changed files with 50 additions and 68 deletions.
28 changes: 5 additions & 23 deletions mavros_extras/src/plugins/gimbal_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
*
Expand All @@ -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;
Expand Down
28 changes: 14 additions & 14 deletions mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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.
24 changes: 12 additions & 12 deletions mavros_msgs/msg/GimbalDeviceInformation.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
10 changes: 5 additions & 5 deletions mavros_msgs/msg/GimbalDeviceSetAttitude.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
28 changes: 14 additions & 14 deletions mavros_msgs/msg/GimbalManagerInformation.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 5646ba4

Please sign in to comment.