Merge pull request #1606 from BV-OpenSource/master

Add Mount angles message for communications with ardupilotmega.
mavlink · Aug 30, 2021 · ccda446 · ccda446
2 parents c006917 + 93c2d8d
commit ccda446
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 2 deletions.
diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt
@@ -80,6 +80,7 @@ add_library(mavros_extras
   src/plugins/log_transfer.cpp
   src/plugins/mag_calibration_status.cpp
   src/plugins/mocap_pose_estimate.cpp
+  src/plugins/mount_control.cpp
   src/plugins/obstacle_distance.cpp
   src/plugins/odom.cpp
   src/plugins/play_tune.cpp
@@ -90,7 +91,6 @@ add_library(mavros_extras
   src/plugins/vision_pose_estimate.cpp
   src/plugins/vision_speed_estimate.cpp
   src/plugins/wheel_odometry.cpp
-  src/plugins/mount_control.cpp
 )
 add_dependencies(mavros_extras
   ${catkin_EXPORTED_TARGETS}

diff --git a/mavros_extras/src/plugins/mount_control.cpp b/mavros_extras/src/plugins/mount_control.cpp
@@ -19,6 +19,7 @@
 #include <mavros_msgs/CommandLong.h>
 #include <mavros_msgs/MountControl.h>
 #include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Vector3Stamped.h>
 #include <mavros_msgs/MountConfigure.h>
 
 namespace mavros {
@@ -47,13 +48,15 @@ class MountControlPlugin : public plugin::PluginBase {
 
 		command_sub = mount_nh.subscribe("command", 10, &MountControlPlugin::command_cb, this);
 		mount_orientation_pub = mount_nh.advertise<geometry_msgs::Quaternion>("orientation", 10);
+		mount_status_pub = mount_nh.advertise<geometry_msgs::Vector3Stamped>("status", 10);
 		configure_srv = mount_nh.advertiseService("configure", &MountControlPlugin::mount_configure_cb, this);
 	}
 
 	Subscriptions get_subscriptions() override
 	{
 		return {
-			make_handler(&MountControlPlugin::handle_mount_orientation)
+			make_handler(&MountControlPlugin::handle_mount_orientation),
+			make_handler(&MountControlPlugin::handle_mount_status)
 		};
 	}
 
@@ -62,6 +65,7 @@ class MountControlPlugin : public plugin::PluginBase {
 	ros::NodeHandle mount_nh;
 	ros::Subscriber command_sub;
 	ros::Publisher mount_orientation_pub;
+	ros::Publisher mount_status_pub;
 	ros::ServiceServer configure_srv;
 
 	/**
@@ -79,6 +83,32 @@ class MountControlPlugin : public plugin::PluginBase {
 		mount_orientation_pub.publish(quaternion_msg);
 	}
 
+	/**
+	 * @brief Publish the mount status
+	 *
+	 * @param msg   the mavlink message
+	 * @param ms	received MountStatus msg
+	 */
+	void handle_mount_status(const mavlink::mavlink_message_t *, mavlink::ardupilotmega::msg::MOUNT_STATUS &ms)
+	{
+		geometry_msgs::Vector3Stamped publish_msg;
+
+		publish_msg.header.stamp = ros::Time::now();
+
+		publish_msg.header.frame_id = std::to_string(ms.target_component);
+
+		auto vec = Eigen::Vector3d(ms.pointing_b, ms.pointing_a, ms.pointing_c) * M_PI / 18000.0;
+		tf::vectorEigenToMsg(vec, publish_msg.vector);
+
+		mount_status_pub.publish(publish_msg);
+
+		// pointing_X is cdeg
+		auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(ms.pointing_b, ms.pointing_a, ms.pointing_c) * M_PI / 18000.0);
+		geometry_msgs::Quaternion quaternion_msg;
+		tf::quaternionEigenToMsg(q, quaternion_msg);
+		mount_orientation_pub.publish(quaternion_msg);
+	}
+
 	/**
 	 * @brief Send mount control commands to vehicle
 	 *