-
Notifications
You must be signed in to change notification settings - Fork 986
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Gimbal protocol v2 plugin #1825
base: ros2
Are you sure you want to change the base?
Conversation
Added all functionality to support a plugin to enable compatibility with MAVLink Gimbal Protocol v2
Added functionality that was overlooked for camera tracking if supported, added copyright info, added custom exception thrown when mode enumerator is not understood
Replaced with service call failure with MAV_RESULT_DENIED result value (2)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Overall looks good. Just a few things to fix.
<description>@brief Gimbal Control Plugin | ||
@plugin gimbal_control | ||
|
||
Adds support for Mavlink Gimbal Protocol v2.</description> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think you haven't used cog.py
to re-generate that file?
cmdClient = node->create_client<mavros_msgs::srv::CommandLong>("cmd/command", rmw_qos_profile_services_default, cb_group); | ||
while (!cmdClient->wait_for_service(std::chrono::seconds(5))) { | ||
RCLCPP_ERROR(node->get_logger(), "GimbalControl: mavros/cmd/command service not available after waiting"); | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Locking in the constructor might be a bad idea.
Maybe better to check for that service later?
q.w = mo.q[0]; | ||
q.x = mo.q[1]; | ||
q.y = mo.q[2]; | ||
q.z = mo.q[3]; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
mavros/mavros/include/mavros/frame_tf.hpp
Lines 427 to 430 in 10569e6
inline Eigen::Quaterniond mavlink_to_quaternion(const std::array<float, 4> & q) | |
{ | |
return Eigen::Quaterniond(q[0], q[1], q[2], q[3]); | |
} |
* @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) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
*/ | ||
std::string convertToString(std::array<char, 32> a, int size) { | ||
std::string s = ""; | ||
for (int i=0; i<size; i++) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
BTW you shouldn't use sizeof with std::array. Use std::array<>::size() method.
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). |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'd remove GIMBAL_DEVICE
prefix...
float32 pitch_min # Minimum pitch angle (positive: up, negative: down) | ||
float32 pitch_max # Maximum pitch angle (positive: up, negative: down) | ||
float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left) | ||
float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Just to think about, what if we define min and max Vector3 instead of separate components. But i'm ok to leave it close to MAVLink.
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ideally that should be generated like for MAV_CMD.
Hi, any updates? |
I am also interested if there are plans on getting this merged in or if something else is blocking it? Edit: If it would help getting it merged I could try to address the comments left on this PR? Not sure what the best approach is? |
@Crowdedlight if you fix the issues above, yes, it can be merged. After fixing merge conflicts... Probably it's easier for you just to start a new PR. You can use commits from this branch, then apply fixes. |
This plugin has been tested with ROS2 Foxy and Humble and everything appears to be working correctly. Certain services and topics haven't been observed as working due to limitations of my testing hardware (Freefly Astro) and software (Auterion Simulator), but everything follows the documentation currently available for MAVLink's Gimbal Protocol v2. I also plan on adding another branch on my fork with the plugin separate from Mavros to enable use as a standalone plugin prior to the PR being approved/incorporated in a release.