From 0f8548e88d6dca6bcb8e562ed794852e55518e36 Mon Sep 17 00:00:00 2001 From: David Jablonski Date: Wed, 14 Aug 2019 14:51:44 +0200 Subject: [PATCH] adding mount orientation to mount_control plugin --- mavros_extras/src/plugins/mount_control.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/mavros_extras/src/plugins/mount_control.cpp b/mavros_extras/src/plugins/mount_control.cpp index a96f1d3c2..e18e12be0 100644 --- a/mavros_extras/src/plugins/mount_control.cpp +++ b/mavros_extras/src/plugins/mount_control.cpp @@ -17,6 +17,7 @@ #include #include +#include namespace mavros { namespace extra_plugins { @@ -43,16 +44,34 @@ class MountControlPlugin : public plugin::PluginBase { PluginBase::initialize(uas_); command_sub = mount_nh.subscribe("command", 10, &MountControlPlugin::command_cb, this); + mount_orientation_pub = mount_nh.advertise("orientation", 10); } Subscriptions get_subscriptions() { - return { /* Rx disabled */ }; + return { + make_handler(&MountControlPlugin::handle_mount_orientation) + }; } private: ros::NodeHandle mount_nh; ros::Subscriber command_sub; + ros::Publisher mount_orientation_pub; + + /** + * @brief Publish the mount orientation + * + * Message specification: https://mavlink.io/en/messages/common.html#MOUNT_ORIENTATION + * @param o received MountOrientation msg + */ + void handle_mount_orientation(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MOUNT_ORIENTATION &o) + { + auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(o.roll, o.pitch, o.yaw) * M_PI / 180.0); + geometry_msgs::Quaternion quaternion_msg; + tf::quaternionEigenToMsg(q, quaternion_msg); + mount_orientation_pub.publish(quaternion_msg); + } /** * @brief Send mount control commands to vehicle