TF sync listener: generalize topic type that can be syncronized with TF2

mavros/include/mavros/setpoint_mixin.h

@@ -21,7 +21,6 @@
 #include <mavros/mavros_plugin.h>
 
 #include <geometry_msgs/TransformStamped.h>
-#include <mavros_msgs/Thrust.h>
 
 #include "tf2_ros/message_filter.h"
 #include <message_filters/subscriber.h>
@@ -127,7 +126,7 @@ class SetPositionTargetGlobalIntMixin {
 template <class D>
 class SetAttitudeTargetMixin {
 public:
-	//! Message sepecification: @p http://mavlink.org/messages/common#SET_ATTITIDE_TARGET
+	//! Message sepecification: @p http://mavlink.org/messages/common#SET_ATTITUDE_TARGET
 	void set_attitude_target(uint32_t time_boot_ms,
 			uint8_t type_mask,
 			Eigen::Quaterniond orientation,
@@ -164,7 +163,6 @@ class SetAttitudeTargetMixin {
  * It requires tf_frame_id, tf_child_frame_id strings
  * tf_rate double and uas object pointer
  */
-template <class D>
 class TF2ListenerMixin {
 public:
 	std::thread tf_thread_1;
@@ -178,6 +176,7 @@ class TF2ListenerMixin {
 	 * @param _thd_name  listener thread name
 	 * @param cbp        plugin callback function
 	 */
+	template <class D>
 	void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &) )
 	{
 		tf_thd_name_1 = _thd_name;
@@ -209,12 +208,13 @@ class TF2ListenerMixin {
 	}
 
 	/**
-	 * @brief start tf listener syncronized with mavros_msgs::Thrust publisher topic
+	 * @brief start tf listener syncronized with another topic
 	 *
 	 * @param _thd_name  listener thread name
 	 * @param cbp        plugin callback function
 	 */
-	void tf2_sync_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &, const mavros_msgs::Thrust::ConstPtr &))
+	template <class D, class T>
+	void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &, const typename T::ConstPtr &))
 	{
 		tf_thd_name_2 = _thd_name;
 
@@ -225,7 +225,8 @@ class TF2ListenerMixin {
 			ros::NodeHandle &_sp_nh = static_cast<D *>(this)->sp_nh;
 			std::string &_frame_id = static_cast<D *>(this)->tf_frame_id;
 			std::string &_child_frame_id = static_cast<D *>(this)->tf_child_frame_id;
-			message_filters::Subscriber<mavros_msgs::Thrust> &_thrust_sub = static_cast<D *>(this)->thrust_sub;
+
+			message_filters::Subscriber<T> _topic_sub;
 
 			ros::Rate rate(static_cast<D *>(this)->tf_rate);
 			while (ros::ok()) {
@@ -235,7 +236,7 @@ class TF2ListenerMixin {
 						auto transform = m_uas_->tf2_buffer.lookupTransform(
 								_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0));
 
-						tf2_ros::MessageFilter<mavros_msgs::Thrust> tf2_filter(_thrust_sub, m_uas_->tf2_buffer, _frame_id, 10, _sp_nh);
+						tf2_ros::MessageFilter<T> tf2_filter(_topic_sub, m_uas_->tf2_buffer, _frame_id, 10, _sp_nh);
 						tf2_filter.registerCallback(boost::bind(cbp, static_cast<D *>(this), transform, _1));
 					}
 					catch (tf2::LookupException &ex) {

mavros/src/plugins/setpoint_attitude.cpp

@@ -36,7 +36,7 @@ namespace std_plugins {
  */
 class SetpointAttitudePlugin : public plugin::PluginBase,
 	private plugin::SetAttitudeTargetMixin<SetpointAttitudePlugin>,
-	private plugin::TF2ListenerMixin<SetpointAttitudePlugin> {
+	private plugin::TF2ListenerMixin {
 public:
 	SetpointAttitudePlugin() : PluginBase(),
 		sp_nh("~setpoint_attitude"),
@@ -64,14 +64,14 @@ class SetpointAttitudePlugin : public plugin::PluginBase,
 		sp_nh.param("tf/rate_limit", tf_rate, 10.0);
 
 		// thrust msg subscriber to sync
-		thrust_sub.subscribe(sp_nh, "thrust", 10);
+		message_filters::Subscriber<mavros_msgs::Thrust> thrust_sub(sp_nh, "thrust", 10);
 
 		if (tf_listen) {
 			ROS_INFO_STREAM_NAMED("attitude",
 						"Listen to desired attitude transform "
 						<< tf_frame_id << " -> " << tf_child_frame_id);
 
-			tf2_sync_start("AttitudeSpTFSync", &SetpointAttitudePlugin::transform_cb);
+			tf2_start<SetpointAttitudePlugin, mavros_msgs::Thrust>("AttitudeSpTFSync", &SetpointAttitudePlugin::transform_cb);
 		}
 		else if (use_quaternion) {
 			/**
@@ -106,8 +106,6 @@ class SetpointAttitudePlugin : public plugin::PluginBase,
 	std::string tf_frame_id;
 	std::string tf_child_frame_id;
 
-	message_filters::Subscriber<mavros_msgs::Thrust> thrust_sub;
-
 	double tf_rate;
 	bool use_quaternion;
 	bool reverse_thrust;

mavros/src/plugins/setpoint_position.cpp

@@ -29,7 +29,7 @@ namespace std_plugins {
  */
 class SetpointPositionPlugin : public plugin::PluginBase,
 	private plugin::SetPositionTargetLocalNEDMixin<SetpointPositionPlugin>,
-	private plugin::TF2ListenerMixin<SetpointPositionPlugin> {
+	private plugin::TF2ListenerMixin {
 public:
 	SetpointPositionPlugin() : PluginBase(),
 		sp_nh("~setpoint_position"),
@@ -51,7 +51,7 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 		if (tf_listen) {
 			ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform "
 					<< tf_frame_id << " -> " << tf_child_frame_id);
-			tf2_start("PositionSpTF", &SetpointPositionPlugin::transform_cb);
+			tf2_start<SetpointPositionPlugin>("PositionSpTF", &SetpointPositionPlugin::transform_cb);
 		}
 		else {
 			setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this);

mavros_extras/src/plugins/vision_pose_estimate.cpp

@@ -32,7 +32,7 @@ namespace extra_plugins{
  *
  */
 class VisionPoseEstimatePlugin : public plugin::PluginBase,
-	private plugin::TF2ListenerMixin<VisionPoseEstimatePlugin> {
+	private plugin::TF2ListenerMixin {
 public:
 	VisionPoseEstimatePlugin() : PluginBase(),
 		sp_nh("~vision_pose"),
@@ -54,7 +54,7 @@ class VisionPoseEstimatePlugin : public plugin::PluginBase,
 		if (tf_listen) {
 			ROS_INFO_STREAM_NAMED("vision_pose", "Listen to vision transform " << tf_frame_id
 						<< " -> " << tf_child_frame_id);
-			tf2_start("VisionPoseTF", &VisionPoseEstimatePlugin::transform_cb);
+			tf2_start<VisionPoseEstimatePlugin>("VisionPoseTF", &VisionPoseEstimatePlugin::transform_cb);
 		}
 		else {
 			vision_sub = sp_nh.subscribe("pose", 10, &VisionPoseEstimatePlugin::vision_cb, this);