diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index d8d1e568d..4a6e24c35 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -98,6 +98,7 @@ add_library(mavros_extras_plugins SHARED src/plugins/esc_status.cpp src/plugins/esc_telemetry.cpp src/plugins/fake_gps.cpp + src/plugins/gimbal_control.cpp src/plugins/gps_input.cpp src/plugins/gps_rtk.cpp src/plugins/gps_status.cpp diff --git a/mavros_extras/README.md b/mavros_extras/README.md index 5be647f58..bbcc86ffa 100644 --- a/mavros_extras/README.md +++ b/mavros_extras/README.md @@ -34,6 +34,28 @@ fake\_gps Sends fake GPS from local position estimation source data (motion capture, vision) to FCU. +gimbal\_control +---- + +Adds support for Mavlink Gimbal Protocol v2. To publish to tf, set parameter +tf_send=True. The implementation of this plugin has been tested with a +Freefly Astro with the mapping payload as well as with Auterion Sim. The +plugin was built following the specifications available at +https://mavlink.io/en/services/gimbal_v2.html with some adaptation to better +suit ROS2 and support tf publishing with child frame labels specified by the +gimbal_device_id field of gimbal_attitude_msg. This should enable support for +multiple gimbal devices on the target platform publishing to different leaves +of the tf tree. The assumed frame for each gimbal device is base_link_frd. +When taking control of the gimbal with the GimbalManagerConfigure, sysid_primary +can be set to -2, and default gimbal_device_id is 154, though 0 can be used for all +gimbal devices. After taking control of the gimbal, you can set RoI's or manually +set the gimbal's orientation using the service calls provided. The topic publishers +for gimbal control have not been successfully validated, though this is possibly due +to the implimentation on the Freefly Astro or with Auterion's simulator. Feel +free to reach out to mark.beaty@adinkratech.com with any questions or feedback on +this plugin! + + gps\_input ----------- diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index 30e522c40..6a55fcde2 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -69,6 +69,12 @@ Sends fake GPS from local position estimation source data (motion capture, vision) to FCU - processed in HIL mode or out of it if parameter MAV_USEHILGPS is set on PX4 Pro Autopilot Firmware; Ardupilot Firmware already supports it without a flag set. + + + @brief Gimbal Control Plugin +@plugin gimbal_control + +Adds support for Mavlink Gimbal Protocol v2. @brief GPS_INPUT GPS plugin. diff --git a/mavros_extras/src/plugins/gimbal_control.cpp b/mavros_extras/src/plugins/gimbal_control.cpp new file mode 100644 index 000000000..28811d3a7 --- /dev/null +++ b/mavros_extras/src/plugins/gimbal_control.cpp @@ -0,0 +1,708 @@ +/* + * Copyright 2023 Adinkra Inc. + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ +/** + * @brief Gimbal Control Plugin + * @file gimbal_control.cpp + * @author Mark Beaty + * + * @addtogroup plugin + * @{ + */ + +#include + +#include + +#include "rcpputils/asserts.hpp" +#include "mavros/mavros_uas.hpp" +#include "mavros/plugin.hpp" +#include "mavros/plugin_filter.hpp" + +#include "mavros_msgs/srv/command_long.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" + +#include "mavros_msgs/msg/gimbal_device_attitude_status.hpp" +#include "mavros_msgs/msg/gimbal_device_set_attitude.hpp" +#include "mavros_msgs/msg/gimbal_device_information.hpp" +#include "mavros_msgs/msg/gimbal_manager_set_attitude.hpp" +#include "mavros_msgs/msg/gimbal_manager_set_pitchyaw.hpp" +#include "mavros_msgs/msg/gimbal_manager_status.hpp" +#include "mavros_msgs/msg/gimbal_manager_information.hpp" + +#include "mavros_msgs/srv/gimbal_get_information.hpp" +#include "mavros_msgs/srv/gimbal_manager_configure.hpp" +#include "mavros_msgs/srv/gimbal_manager_pitchyaw.hpp" +#include "mavros_msgs/srv/gimbal_manager_set_roi.hpp" +#include "mavros_msgs/srv/gimbal_manager_camera_track.hpp" + +namespace mavros +{ +namespace extra_plugins +{ +using namespace std::placeholders; // NOLINT + +// Mavlink enumerations +using mavlink::common::GIMBAL_MANAGER_FLAGS; +using mavlink::common::GIMBAL_MANAGER_CAP_FLAGS; +using mavlink::common::GIMBAL_DEVICE_CAP_FLAGS; +using mavlink::common::GIMBAL_DEVICE_FLAGS; +using mavlink::common::GIMBAL_DEVICE_ERROR_FLAGS; +using mavlink::common::MAV_CMD; +using utils::enum_value; + +/** + * @brief Gimbal Control Plugin + * @plugin gimbal_control + * + * Adds support for Mavlink Gimbal Protocol v2. + * Also publishes gimbal pose to TF when parameter tf_send==true + */ +class GimbalControlPlugin : public plugin::Plugin +{ +public: + explicit GimbalControlPlugin(plugin::UASPtr uas_) + : Plugin(uas_, "gimbal_control"), + tf_send(false) + { + /** + * Sample service calls for interfacing with a Gimbal Protocol v2 gimbal: + * This service must be called first to take control of the gimbal: + * ros2 service call /mavros/gimbal_control/manager/configure mavros_msgs/srv/GimbalManagerConfigure "{sysid_primary: -2, compid_primary: 191, sysid_secondary: -1, compid_secondary: -1, gimbal_device_id: 0}" + * Set pitch and yaw: + * ros2 service call /mavros/gimbal_control/manager/pitchyaw mavros_msgs/srv/GimbalManagerPitchyaw "{pitch: -45, yaw: 90, pitch_rate: -0.2, yaw_rate: -0.2, flags: 0, gimbal_device_id: 0}" + * Set region of intrest for tracking: + * ros2 service call /mavros/gimbal_control/manager/set_roi mavros_msgs/srv/GimbalManagerSetRoi "{mode: 0, gimbal_device_id: 0, latitude: x, longitude: y, altitude: z}" + */ + // Callback group for supporting nested service calls + cb_group = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + // Parameter for msg header frame + enable_node_watch_parameters(); + node_declare_and_watch_parameter( + "frame_id", "base_link_frd", [&](const rclcpp::Parameter & p) { + frame_id = p.as_string(); + }); + + // Important tf subsection + // Report the transform from base_link to gimbal here. + node_declare_and_watch_parameter( + "tf.send", false, [&](const rclcpp::Parameter & p) { + tf_send = p.as_bool(); + }); + node_declare_and_watch_parameter( + "tf.frame_id", "base_link_frd", [&](const rclcpp::Parameter & p) { + tf_frame_id = p.as_string(); + }); + + // Subscribers + // --Not successfully validated-- + set_device_attitude_sub = node->create_subscription( + "~/device/set_attitude", 10, std::bind( + &GimbalControlPlugin::device_set_attitude_cb, this, + _1)); + + // --Not successfully validated-- + set_manager_attitude_sub = node->create_subscription( + "~/manager/set_attitude", 10, std::bind( + &GimbalControlPlugin::manager_set_attitude_cb, this, + _1)); + + // --Not successfully validated-- + set_manager_pitchyaw_sub = node->create_subscription( + "~/manager/set_pitchyaw", 10, std::bind( + &GimbalControlPlugin::manager_set_pitchyaw_cb, this, + _1)); + + // --Not successfully validated-- also note that the message is the same as pitchyaw and will likely change + set_manager_manual_control_sub = node->create_subscription( + "~/manager/set_manual_control", 10, std::bind( + &GimbalControlPlugin::manager_set_manual_control_cb, this, + _1)); + + + // Publishers + gimbal_attitude_status_pub = node->create_publisher( + "~/device/attitude_status", + 10); + + gimbal_manager_status_pub = node->create_publisher( + "~/manager/status", + 10); + + gimbal_manager_info_pub = node->create_publisher( + "~/manager/info", + 10); + + // --Not successfully validated-- + gimbal_device_info_pub = node->create_publisher( + "~/device/info", + 10); + + + // Services + // --Not successfully validated-- + gimbal_device_info_srv = node->create_service( + "~/device/get_info", std::bind( + &GimbalControlPlugin::device_get_info_cb, + this, _1, _2)); + + gimbal_manager_info_srv = node->create_service( + "~/manager/get_info", std::bind( + &GimbalControlPlugin::manager_get_info_cb, + this, _1, _2)); + + gimbal_manager_configure_srv = node->create_service( + "~/manager/configure", std::bind( + &GimbalControlPlugin::manager_configure_cb, + this, _1, _2)); + + gimbal_manager_pitchyaw_srv = node->create_service( + "~/manager/pitchyaw", std::bind( + &GimbalControlPlugin::manager_pitchyaw_cb, + this, _1, _2)); + + gimbal_manager_set_roi_srv = node->create_service( + "~/manager/set_roi", std::bind( + &GimbalControlPlugin::manager_set_roi_cb, + this, _1, _2)); + + // --Not successfully validated-- + gimbal_manager_camera_track = node->create_service( + "~/manager/camera_track", std::bind( + &GimbalControlPlugin::manager_camera_track, + this, _1, _2)); + + // Client used by all services for sending mavros/cmd/command service calls, on a separate callback group to support nested service calls + cmdClient = node->create_client("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"); + } + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&GimbalControlPlugin::handle_gimbal_attitude_status), + make_handler(&GimbalControlPlugin::handle_manager_status), + make_handler(&GimbalControlPlugin::handle_device_information), + make_handler(&GimbalControlPlugin::handle_manager_information), + }; + } + +private: + // Callback Group + rclcpp::CallbackGroup::SharedPtr cb_group; + // Subscribers + rclcpp::Subscription::SharedPtr set_device_attitude_sub; + rclcpp::Subscription::SharedPtr set_manager_attitude_sub; + rclcpp::Subscription::SharedPtr set_manager_pitchyaw_sub; + rclcpp::Subscription::SharedPtr set_manager_manual_control_sub; + + // Publishers + rclcpp::Publisher::SharedPtr gimbal_attitude_status_pub; + rclcpp::Publisher::SharedPtr gimbal_manager_status_pub; + rclcpp::Publisher::SharedPtr gimbal_manager_info_pub; + rclcpp::Publisher::SharedPtr gimbal_device_info_pub; + + // Services + rclcpp::Service::SharedPtr gimbal_device_info_srv; + rclcpp::Service::SharedPtr gimbal_manager_info_srv; + rclcpp::Service::SharedPtr gimbal_manager_configure_srv; + rclcpp::Service::SharedPtr gimbal_manager_pitchyaw_srv; + rclcpp::Service::SharedPtr gimbal_manager_set_roi_srv; + rclcpp::Service::SharedPtr gimbal_manager_camera_track; + + // Clients + rclcpp::Client::SharedPtr cmdClient; + + std::string frame_id; // origin frame for topic headers + std::string tf_frame_id; // origin frame for TF + std::atomic tf_send; // parameter for enabling TF publishing + + // Transform Publisher + void publish_tf(mavros_msgs::msg::GimbalDeviceAttitudeStatus & gimbal_attitude_msg) + { + if (tf_send) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = gimbal_attitude_msg.header.stamp; + transform.header.frame_id = tf_frame_id; + // TF child_frame_id with format "gimbal_" where the component_id comes from the gimbal_attitude_msg + transform.child_frame_id = "gimbal_" + std::to_string(gimbal_attitude_msg.target_component); + transform.transform.rotation = gimbal_attitude_msg.q; + uas->tf2_broadcaster.sendTransform(transform); + } + } + + // Mavlink subscriber callbacks + /** + * @brief Publish the gimbal orientation + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS + * @param msg - the mavlink message + * @param mo - received GimbalDeviceAttitudeStatus msg + */ + void handle_gimbal_attitude_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GIMBAL_DEVICE_ATTITUDE_STATUS & mo, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + mavros_msgs::msg::GimbalDeviceAttitudeStatus gimbal_attitude_msg; + gimbal_attitude_msg.header = uas->synchronized_header(frame_id, mo.time_boot_ms); + 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; + 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; + gimbal_attitude_msg.failure_flags = mo.failure_flags; + + gimbal_attitude_status_pub->publish(gimbal_attitude_msg); + + // publish tf + publish_tf(gimbal_attitude_msg); + } + + /** + * @brief Publish gimbal manager status + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS + * @param msg - the mavlink message + * @param ms - received ManagerStatus msg + */ + void handle_manager_status( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GIMBAL_MANAGER_STATUS & ms, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + mavros_msgs::msg::GimbalManagerStatus gimbal_manager_status_msg; + gimbal_manager_status_msg.header = uas->synchronized_header(frame_id, ms.time_boot_ms); + gimbal_manager_status_msg.flags = ms.flags; + gimbal_manager_status_msg.gimbal_device_id = ms.gimbal_device_id; + gimbal_manager_status_msg.sysid_primary = ms.primary_control_sysid; + gimbal_manager_status_msg.compid_primary = ms.primary_control_compid; + gimbal_manager_status_msg.sysid_secondary = ms.secondary_control_sysid; + gimbal_manager_status_msg.compid_secondary = ms.secondary_control_compid; + + 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 a, int size) { + std::string s = ""; + for (int i=0; isynchronized_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.firmware_version = di.firmware_version; + gimbal_device_information_msg.hardware_version = di.hardware_version; + gimbal_device_information_msg.uid = di.uid; + gimbal_device_information_msg.cap_flags = di.cap_flags; + gimbal_device_information_msg.custom_cap_flags = di.custom_cap_flags; + gimbal_device_information_msg.roll_min = di.roll_min; + gimbal_device_information_msg.roll_max = di.roll_max; + gimbal_device_information_msg.pitch_min = di.pitch_min; + gimbal_device_information_msg.pitch_max = di.pitch_max; + gimbal_device_information_msg.yaw_min = di.yaw_min; + gimbal_device_information_msg.yaw_max = di.yaw_max; + + gimbal_device_info_pub->publish(gimbal_device_information_msg); + } + + /** + * @brief Publish gimbal manager information - Note: this message is only published on request by default (see manager_get_info_cb) + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS + * @param msg - the mavlink message + * @param mi - received GimbalManagerInformation msg + */ + void handle_manager_information( + const mavlink::mavlink_message_t * msg [[maybe_unused]], + mavlink::common::msg::GIMBAL_MANAGER_INFORMATION & mi, + plugin::filter::SystemAndOk filter [[maybe_unused]]) + { + mavros_msgs::msg::GimbalManagerInformation gimbal_manager_information_msg; + gimbal_manager_information_msg.header = uas->synchronized_header(frame_id, mi.time_boot_ms); + gimbal_manager_information_msg.cap_flags = mi.cap_flags; + gimbal_manager_information_msg.gimbal_device_id = mi.gimbal_device_id; + gimbal_manager_information_msg.roll_min = mi.roll_min; + gimbal_manager_information_msg.roll_max = mi.roll_max; + gimbal_manager_information_msg.pitch_min = mi.pitch_min; + gimbal_manager_information_msg.pitch_max = mi.pitch_max; + gimbal_manager_information_msg.yaw_min = mi.yaw_min; + gimbal_manager_information_msg.yaw_max = mi.yaw_max; + + gimbal_manager_info_pub->publish(gimbal_manager_information_msg); + } + + // ROS subscriber callbacks + /** + * @brief Send attitude control commands to gimbal device + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE + * @param req - received GimbalControl msg + */ + void device_set_attitude_cb(const mavros_msgs::msg::GimbalDeviceSetAttitude::SharedPtr req) + { + mavlink::common::msg::GIMBAL_DEVICE_SET_ATTITUDE msg; + std::array new_q; + new_q[0] = req->q.w; + new_q[1] = req->q.x; + new_q[2] = req->q.y; + new_q[3] = req->q.z; + uas->msg_set_target(msg); + msg.target_system = req->target_system; + msg.target_component = req->target_component; + msg.flags = req->flags; + msg.q = new_q; + msg.angular_velocity_x = req->angular_velocity_x; + msg.angular_velocity_y = req->angular_velocity_y; + msg.angular_velocity_z = req->angular_velocity_z; + + uas->send_message(msg); + } + + /** + * @brief Send attitude control commands to gimbal manager + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_ATTITUDE + * @param req - received GimbalControl msg + */ + void manager_set_attitude_cb(const mavros_msgs::msg::GimbalManagerSetAttitude::SharedPtr req) + { + mavlink::common::msg::GIMBAL_MANAGER_SET_ATTITUDE msg {}; + std::array new_q; + new_q[0] = req->q.w; + new_q[1] = req->q.x; + new_q[2] = req->q.y; + new_q[3] = req->q.z; + uas->msg_set_target(msg); + msg.target_system = req->target_system; + msg.target_component = req->target_component; + msg.flags = req->flags; + msg.gimbal_device_id = req->gimbal_device_id; + msg.q = new_q; + msg.angular_velocity_x = req->angular_velocity_x; + msg.angular_velocity_y = req->angular_velocity_y; + msg.angular_velocity_z = req->angular_velocity_z; + + uas->send_message(msg); + } + + /** + * @brief Send pitchyaw control commands to gimbal manager + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_PITCHYAW + * @param req - received GimbalControl msg + */ + void manager_set_pitchyaw_cb(const mavros_msgs::msg::GimbalManagerSetPitchyaw::SharedPtr req) + { + mavlink::common::msg::GIMBAL_MANAGER_SET_PITCHYAW msg {}; + uas->msg_set_target(msg); + msg.target_system = req->target_system; + msg.target_component = req->target_component; + msg.flags = req->flags; + msg.gimbal_device_id = req->gimbal_device_id; + msg.pitch = req->pitch; + msg.yaw = req->yaw; + msg.pitch_rate = req->pitch_rate; + msg.yaw_rate = req->yaw_rate; + + uas->send_message(msg); + } + + /** + * @brief Send manual control commands to gimbal manager + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_MANUAL_CONTROL + * Note that message contents is identical to that of pitchyaw, so the message type is re-used here + * @param req - received GimbalControl msg + */ + void manager_set_manual_control_cb(const mavros_msgs::msg::GimbalManagerSetPitchyaw::SharedPtr req) + { + mavlink::common::msg::GIMBAL_MANAGER_SET_PITCHYAW msg {}; + uas->msg_set_target(msg); + msg.target_system = req->target_system; + msg.target_component = req->target_component; + msg.flags = req->flags; + msg.gimbal_device_id = req->gimbal_device_id; + msg.pitch = req->pitch; + msg.yaw = req->yaw; + msg.pitch_rate = req->pitch_rate; + msg.yaw_rate = req->yaw_rate; + + uas->send_message(msg); + } + + // Service Callbacks + /** + * @brief Request GIMBAL_DEVICE_INFORMATION msg be broadcast through Mavlink (see handle_device_information) + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION + * @param req - received GimbalGetInformation msg + */ + void device_get_info_cb( + const mavros_msgs::srv::GimbalGetInformation::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::GimbalGetInformation::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + try { + auto cmdrq = std::make_shared(); + cmdrq->command = enum_value(MAV_CMD::REQUEST_MESSAGE); + cmdrq->param1 = mavlink::common::msg::GIMBAL_DEVICE_INFORMATION::MSG_ID; + auto future = cmdClient->async_send_request(cmdrq); + auto response = future.get(); + res->success = response->success; + res->result = response->result; + } catch (std::exception & ex) { + RCLCPP_ERROR(get_logger(), "GimbalControl: %s", ex.what()); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !res->success, "GimbalControl: plugin service call failed!"); + } + + void device_get_info_response(rclcpp::Client::SharedFuture response) { + auto result = response.get(); + + } + + /** + * @brief Request GIMBAL_MANAGER_INFORMATION msg be broadcast through Mavlink (see handle_manager_information) + * + * Message specification: https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION + * @param req - received GimbalControl msg + */ + void manager_get_info_cb( + const mavros_msgs::srv::GimbalGetInformation::Request::SharedPtr req [[maybe_unused]], + mavros_msgs::srv::GimbalGetInformation::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + try { + auto cmdrq = std::make_shared(); + cmdrq->command = enum_value(MAV_CMD::REQUEST_MESSAGE); + cmdrq->param1 = mavlink::common::msg::GIMBAL_MANAGER_INFORMATION::MSG_ID; + auto future = cmdClient->async_send_request(cmdrq); + auto response = future.get(); + res->success = response->success; + res->result = response->result; + } catch (std::exception & ex) { + RCLCPP_ERROR(get_logger(), "GimbalControl: %s", ex.what()); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !res->success, "GimbalControl: plugin service call failed!"); + } + + /** + * @brief Configure gimbal manager + * + * Message specification: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE + * @param req - received GimbalControl msg + */ + void manager_configure_cb( + mavros_msgs::srv::GimbalManagerConfigure::Request::SharedPtr req, + mavros_msgs::srv::GimbalManagerConfigure::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + + try { + auto cmdrq = std::make_shared(); + cmdrq->command = enum_value(MAV_CMD::DO_GIMBAL_MANAGER_CONFIGURE); + cmdrq->param1 = req->sysid_primary; + cmdrq->param2 = req->compid_primary; + cmdrq->param3 = req->sysid_secondary; + cmdrq->param4 = req->compid_secondary; + cmdrq->param7 = req->gimbal_device_id; + auto future = cmdClient->async_send_request(cmdrq); + auto response = future.get(); + res->success = response->success; + res->result = response->result; + } catch (std::exception & ex) { + RCLCPP_ERROR(get_logger(), "GimbalControl: %s", ex.what()); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !res->success, "GimbalControl: plugin service call failed!"); + } + + /** + * @brief Send pitch/yaw command to gimbal manager + * + * Message specification: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW + * @param req - received GimbalControl msg + */ + void manager_pitchyaw_cb( + mavros_msgs::srv::GimbalManagerPitchyaw::Request::SharedPtr req, + mavros_msgs::srv::GimbalManagerPitchyaw::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + + try { + auto cmdrq = std::make_shared(); + cmdrq->command = enum_value(MAV_CMD::DO_GIMBAL_MANAGER_PITCHYAW); + cmdrq->param1 = req->pitch; + cmdrq->param2 = req->yaw; + cmdrq->param3 = req->pitch_rate; + cmdrq->param4 = req->yaw_rate; + cmdrq->param5 = req->flags; + cmdrq->param7 = req->gimbal_device_id; + auto future = cmdClient->async_send_request(cmdrq); + auto response = future.get(); + res->success = response->success; + res->result = response->result; + } catch (std::exception & ex) { + RCLCPP_ERROR(get_logger(), "GimbalControl: %s", ex.what()); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !res->success, "GimbalControl: plugin service call failed!"); + } + + /** + * @brief Set Gimbal ROI mode and parameters + * + * Message specifications: + * https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_LOCATION + * https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET + * https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_SYSID + * https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE + * @param req - received GimbalControl msg + */ + void manager_set_roi_cb( + mavros_msgs::srv::GimbalManagerSetRoi::Request::SharedPtr req, + mavros_msgs::srv::GimbalManagerSetRoi::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + + try { + auto cmdrq = std::make_shared(); + if (req->mode == req->ROI_MODE_LOCATION) { + cmdrq->command = enum_value(MAV_CMD::DO_SET_ROI_LOCATION); + cmdrq->param1 = req->gimbal_device_id; + cmdrq->param5 = req->latitude; + cmdrq->param6 = req->longitude; + cmdrq->param7 = req->altitude; + } + else if (req->mode == req->ROI_MODE_WP_NEXT_OFFSET) { + cmdrq->command = enum_value(MAV_CMD::DO_SET_ROI_WPNEXT_OFFSET); + cmdrq->param1 = req->gimbal_device_id; + cmdrq->param5 = req->pitch_offset; + cmdrq->param6 = req->roll_offset; + cmdrq->param7 = req->yaw_offset; + } + else if (req->mode == req->ROI_MODE_SYSID) { + cmdrq->command = enum_value(MAV_CMD::DO_SET_ROI_SYSID); + cmdrq->param1 = req->sysid; + cmdrq->param2 = req->gimbal_device_id; + } + else if (req->mode == req->ROI_MODE_NONE) { + cmdrq->command = enum_value(MAV_CMD::DO_SET_ROI_NONE); + cmdrq->param1 = req->gimbal_device_id; + } + else { + res->success = false; + res->result = 2; // MAV_RESULT_DENIED - Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work. + return; + } + + // RCLCPP_DEBUG(get_logger(), "GimbalManagerSetRoi for gimbal id: %u ", req->gimbal_device_id); + auto future = cmdClient->async_send_request(cmdrq); + auto response = future.get(); + res->success = response->success; + res->result = response->result; + } catch (std::exception & ex) { + RCLCPP_ERROR(get_logger(), "GimbalManagerSetRoi: %s", ex.what()); + } + + RCLCPP_ERROR_EXPRESSION( + get_logger(), !res->success, "GimbalManager - set roi: plugin service call failed!"); + } + + /** + * @brief Set camera tracking mode and parameters + * + * Message specifications: + * https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT + * https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE + * https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_STOP_TRACKING + * @param req - received GimbalControl msg + */ + void manager_camera_track( + mavros_msgs::srv::GimbalManagerCameraTrack::Request::SharedPtr req, + mavros_msgs::srv::GimbalManagerCameraTrack::Response::SharedPtr res) + { + using mavlink::common::MAV_CMD; + + try { + auto cmdrq = std::make_shared(); + if (req->mode == req->CAMERA_TRACK_MODE_POINT) { + cmdrq->command = enum_value(MAV_CMD::CAMERA_TRACK_POINT); + cmdrq->param1 = req->x; + cmdrq->param2 = req->y; + cmdrq->param3 = req->radius; + } + else if (req->mode == req->CAMERA_TRACK_MODE_RECTANGLE) { + cmdrq->command = enum_value(MAV_CMD::CAMERA_TRACK_RECTANGLE); + cmdrq->param1 = req->top_left_x; + cmdrq->param2 = req->top_left_y; + cmdrq->param3 = req->bottom_right_x; + cmdrq->param4 = req->bottom_right_y; + } + else if (req->mode == req->CAMERA_TRACK_MODE_STOP_TRACKING) { + cmdrq->command = enum_value(MAV_CMD::CAMERA_STOP_TRACKING); + } + else { + res->success = false; + res->result = 2; // MAV_RESULT_DENIED - Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work. + return; + } + + auto future = cmdClient->async_send_request(cmdrq); + auto response = future.get(); + res->success = response->success; + res->result = response->result; + } catch (std::exception & ex) { + RCLCPP_ERROR(get_logger(), "GimbalManagerCameraTrack: %s", ex.what()); + } + + RCLCPP_ERROR_EXPRESSION( + get_logger(), !res->success, "GimbalManager - camera track: plugin service call failed!"); + } +}; +} // namespace extra_plugins +} // namespace mavros + +#include // NOLINT +MAVROS_PLUGIN_REGISTER(mavros::extra_plugins::GimbalControlPlugin) diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index dfbd4e3ac..7177ad501 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -50,6 +50,13 @@ set(msg_files msg/EstimatorStatus.msg msg/ExtendedState.msg msg/FileEntry.msg + msg/GimbalDeviceAttitudeStatus.msg + msg/GimbalDeviceInformation.msg + msg/GimbalDeviceSetAttitude.msg + msg/GimbalManagerInformation.msg + msg/GimbalManagerSetAttitude.msg + msg/GimbalManagerSetPitchyaw.msg + msg/GimbalManagerStatus.msg msg/GPSINPUT.msg msg/GPSRAW.msg msg/GPSRTK.msg @@ -124,6 +131,11 @@ set(srv_files srv/FileRename.srv srv/FileTruncate.srv srv/FileWrite.srv + srv/GimbalGetInformation.srv + srv/GimbalManagerCameraTrack.srv + srv/GimbalManagerConfigure.srv + srv/GimbalManagerPitchyaw.srv + srv/GimbalManagerSetRoi.srv srv/LogRequestData.srv srv/LogRequestEnd.srv srv/LogRequestList.srv diff --git a/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg b/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg new file mode 100644 index 000000000..64ccc7475 --- /dev/null +++ b/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg @@ -0,0 +1,32 @@ +# MAVLink message: GIMBAL_DEVICE_ATTITUDE_STATUS +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS + +std_msgs/Header header + +uint8 target_system # System ID +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). + +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) +float32 angular_velocity_y # Y component of angular velocity (NaN if unknown) +float32 angular_velocity_z # Z component of angular velocity (NaN if unknown) + +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. diff --git a/mavros_msgs/msg/GimbalDeviceInformation.msg b/mavros_msgs/msg/GimbalDeviceInformation.msg new file mode 100644 index 000000000..d2e7c52d2 --- /dev/null +++ b/mavros_msgs/msg/GimbalDeviceInformation.msg @@ -0,0 +1,34 @@ +# MAVLink message: GIMBAL_DEVICE_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION + +std_msgs/Header header + +string vendor_name # Name of the gimbal vendor. +string model_name # Name of the gimbal model. +string custom_name # Custom name of the gimbal given to it by the user. +uint32 firmware_version # Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). +uint32 hardware_version # Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). +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). + +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) +float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +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) \ No newline at end of file diff --git a/mavros_msgs/msg/GimbalDeviceSetAttitude.msg b/mavros_msgs/msg/GimbalDeviceSetAttitude.msg new file mode 100644 index 000000000..ebf35ef7f --- /dev/null +++ b/mavros_msgs/msg/GimbalDeviceSetAttitude.msg @@ -0,0 +1,18 @@ +# MAVLink message: GIMBAL_DEVICE_SET_ATTITUDE +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE + +uint8 target_system # System ID +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 + +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. +float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored. +float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored. \ No newline at end of file diff --git a/mavros_msgs/msg/GimbalManagerInformation.msg b/mavros_msgs/msg/GimbalManagerInformation.msg new file mode 100644 index 000000000..d2090837a --- /dev/null +++ b/mavros_msgs/msg/GimbalManagerInformation.msg @@ -0,0 +1,29 @@ +# MAVLink message: GIMBAL_MANAGER_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION + +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. + +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) +float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +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) \ No newline at end of file diff --git a/mavros_msgs/msg/GimbalManagerSetAttitude.msg b/mavros_msgs/msg/GimbalManagerSetAttitude.msg new file mode 100644 index 000000000..8bbec371b --- /dev/null +++ b/mavros_msgs/msg/GimbalManagerSetAttitude.msg @@ -0,0 +1,24 @@ +# MAVLink message: GIMBAL_MANAGER_SET_ATTITUDE +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_ATTITUDE + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint32 flags # High level gimbal manager flags to use (bitwise) - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +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. +float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored. +float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored. \ No newline at end of file diff --git a/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg b/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg new file mode 100644 index 000000000..e87874e1a --- /dev/null +++ b/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg @@ -0,0 +1,27 @@ +# MAVLink message: GIMBAL_MANAGER_SET_PITCHYAW +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_PITCHYAW +# Note that this message structure is identical also to GIMBAL_MANAGER_SET_MANUAL_CONTROL and is +# reused as such by the plugin +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_MANUAL_CONTROL + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +float32 pitch # Pitch angle (positive: up, negative: down, NaN to be ignored). +float32 yaw # Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). +float32 pitch_rate # Pitch angular rate (positive: up, negative: down, NaN to be ignored). +float32 yaw_rate # Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). \ No newline at end of file diff --git a/mavros_msgs/msg/GimbalManagerStatus.msg b/mavros_msgs/msg/GimbalManagerStatus.msg new file mode 100644 index 000000000..8df8b3e9c --- /dev/null +++ b/mavros_msgs/msg/GimbalManagerStatus.msg @@ -0,0 +1,19 @@ +# MAVLink message: GIMBAL_MANAGER_STATUS +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS + +std_msgs/Header header + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for. + +uint8 sysid_primary # System ID of MAVLink component with primary control, 0 for none. +uint8 compid_primary # Component ID of MAVLink component with primary control, 0 for none. +uint8 sysid_secondary # System ID of MAVLink component with secondary control, 0 for none. +uint8 compid_secondary # Component ID of MAVLink component with secondary control, 0 for none. \ No newline at end of file diff --git a/mavros_msgs/srv/GimbalGetInformation.srv b/mavros_msgs/srv/GimbalGetInformation.srv new file mode 100644 index 000000000..c91bda21a --- /dev/null +++ b/mavros_msgs/srv/GimbalGetInformation.srv @@ -0,0 +1,10 @@ +# MAVLink command: MAV_CMD_REQUEST_MESSAGE +# https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_MESSAGE +# Specifically used to request Information messages from Gimbal Device and Gimbal Manager +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/mavros_msgs/srv/GimbalManagerCameraTrack.srv b/mavros_msgs/srv/GimbalManagerCameraTrack.srv new file mode 100644 index 000000000..ef46dc948 --- /dev/null +++ b/mavros_msgs/srv/GimbalManagerCameraTrack.srv @@ -0,0 +1,28 @@ +# MAVLink commands: CAMERA_TRACK_POINT, CAMERA_TRACK_RECTANGLE, CAMERA_STOP_TRACKING +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_STOP_TRACKING + +uint8 mode # enumerator to indicate camera track mode setting - see CAMERA_TRACK_MODE +#CAMERA_TRACK_MODE +uint8 CAMERA_TRACK_MODE_POINT = 0 # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. [CAMERA_TRACK_POINT] +uint8 CAMERA_TRACK_MODE_RECTANGLE = 1 # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. [CAMERA_TRACK_RECTANGLE] +uint8 CAMERA_TRACK_MODE_STOP_TRACKING = 2 # Stops ongoing tracking. [CAMERA_STOP_TRACKING] + +#For CAMERA_TRACK_POINT +float32 x # Point to track x value (normalized 0..1, 0 is left, 1 is right). +float32 y # Point to track y value (normalized 0..1, 0 is top, 1 is bottom). +float32 radius # Point radius (normalized 0..1, 0 is image left, 1 is image right). + +#For CAMERA_TRACK_RECTANGLE +float32 top_left_x # Top left corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). +float32 top_left_y # Top left corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). +float32 bottom_right_x # Bottom right corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). +float32 bottom_right_y # Bottom right corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). + +#CAMERA_STOP_TRACKING doesn't take extra parameters + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/mavros_msgs/srv/GimbalManagerConfigure.srv b/mavros_msgs/srv/GimbalManagerConfigure.srv new file mode 100644 index 000000000..c0a2176e4 --- /dev/null +++ b/mavros_msgs/srv/GimbalManagerConfigure.srv @@ -0,0 +1,32 @@ +# MAVLink command: DO_GIMBAL_MANAGER_CONFIGURE +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE +# Note: default MAV_COMP_ID_ONBOARD_COMPUTER = 191, see MAV_COMPONENT documentation +# https://mavlink.io/en/messages/common.html#MAV_COMPONENT + +int16 sysid_primary # Sysid for primary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 compid_primary # Compid for primary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 sysid_secondary # Sysid for secondary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 compid_secondary # Compid for secondary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). + # Note: Default Mavlink gimbal device ids: 154, 171-175 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/mavros_msgs/srv/GimbalManagerPitchyaw.srv b/mavros_msgs/srv/GimbalManagerPitchyaw.srv new file mode 100644 index 000000000..6e3c14ee1 --- /dev/null +++ b/mavros_msgs/srv/GimbalManagerPitchyaw.srv @@ -0,0 +1,27 @@ +# MAVLink commands: DO_GIMBAL_MANAGER_PITCHYAW +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW + + +float32 pitch # Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode). (-180 to 180 deg) +float32 yaw # Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode). (-180 to 180 deg) +float32 pitch_rate # Pitch rate (positive to pitch up). (deg/s) +float32 yaw_rate # Yaw rate (positive to yaw to the right). (deg/s) + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/mavros_msgs/srv/GimbalManagerSetRoi.srv b/mavros_msgs/srv/GimbalManagerSetRoi.srv new file mode 100644 index 000000000..02904d8a8 --- /dev/null +++ b/mavros_msgs/srv/GimbalManagerSetRoi.srv @@ -0,0 +1,38 @@ +# MAVLink commands: DO_SET_ROI_LOCATION, DO_SET_ROI_WPNEXT_OFFSET, DO_SET_ROI_SYSID, DO_SET_ROI_NONE +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_LOCATION +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_SYSID +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE + +uint8 mode # enumerator to indicate ROI mode setting - see ROI_MODE +#ROI_MODE +uint8 ROI_MODE_LOCATION = 0 # Sets the region of interest (ROI) to a location. [DO_SET_ROI_LOCATION] +uint8 ROI_MODE_WP_NEXT_OFFSET = 1 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. [DO_SET_ROI_WPNEXT_OFFSET] +uint8 ROI_MODE_SYSID = 2 # Mount tracks system with specified system ID [DO_SET_ROI_SYSID] +uint8 ROI_MODE_NONE = 3 # Cancels any previous ROI setting and returns vehicle to defaults [DO_SET_ROI_NONE] + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +#For ROI_MODE_LOCATION +float32 latitude +float32 longitude +float32 altitude # Meters + +#For ROI_MODE_WP_NEXT_OFFSET +float32 pitch_offset # Pitch offset from next waypoint, positive pitching up +float32 roll_offset # Roll offset from next waypoint, positive rolling to the right +float32 yaw_offset # Yaw offset from next waypoint, positive yawing to the right + +#For ROI_MODE_SYSID +uint8 sysid # System ID to track (min: 1, max: 255) + +#ROI_MODE_NONE doesn't take extra parameters + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file