adding mount orientation to mount_control plugin

mavlink · Sep 9, 2019 · d82cf35 · d82cf35
1 parent 2bae233
commit d82cf35
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/mavros_extras/src/plugins/mount_control.cpp b/mavros_extras/src/plugins/mount_control.cpp
@@ -17,6 +17,7 @@
 #include <mavros/mavros_plugin.h>
 
 #include <mavros_msgs/MountControl.h>
+#include <geometry_msgs/Quaternion.h>
 
 namespace mavros {
 namespace extra_plugins {
@@ -43,7 +44,7 @@ class MountControlPlugin : public plugin::PluginBase {
 		PluginBase::initialize(uas_);
 
 		command_sub = mount_nh.subscribe("command", 10, &MountControlPlugin::command_cb, this);
-		mount_orientation_pub = gp_nh.advertise<geometry_msgs::Quaternion>("orientation", 10);
+		mount_orientation_pub = mount_nh.advertise<geometry_msgs::Quaternion>("orientation", 10);
 	}
 
 	Subscriptions get_subscriptions()
@@ -62,11 +63,13 @@ class MountControlPlugin : public plugin::PluginBase {
 	 * @brief Publish the mount orientation
 	 *
 	 * Message specification: https://mavlink.io/en/messages/common.html#MOUNT_ORIENTATION
+	 * @param msg   the mavlink message
+	 * @param mo	received MountOrientation msg
 	 */
-	void handle_mount_orientation(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MOUNT_ORIENTATION &o)
+	void handle_mount_orientation(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MOUNT_ORIENTATION &mo)
 	{
-		auto q = ftf::quaternion_from_rpy(o.roll, o.pitch, o.yaw);
-		auto quaternion_msg = boost::make_shared<geometry_msgs::Quaternion>();
+		auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(mo.roll, mo.pitch, mo.yaw) * M_PI / 180.0);
+		geometry_msgs::Quaternion quaternion_msg;
 		tf::quaternionEigenToMsg(q, quaternion_msg);
 		mount_orientation_pub.publish(quaternion_msg);
 	}