[WIP] Plugins: setpoint_attitude: add sync between thrust and attitude (

mavlink#700)

* plugins: setpoint_attitude: add sync between throttle and attitude topics to be sent together

* plugins: typo correction: replace throttle with thrust

* plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg

* plugins: setpoint_attitude: add sync between twist and thrust (RPY+Thrust)

* setpoint_attitude: update the logic of thrust normalization verification

* setpoint_attitude: implement sync between tf listener and thrust subscriber

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

* TF2ListenerMixin: keep class template, use template for tf sync method only

* TF2ListenerMixin: fix and improve sync tf2_start method

* general update to yaml config files and parameters

* setpoint_attitude: add note on Thrust sub name

* setpoint_attitude: TF sync: pass subscriber pointer instead of binding it

mavros/include/mavros/setpoint_mixin.h

@@ -22,6 +22,10 @@
 
 #include <geometry_msgs/TransformStamped.h>
 
+#include "tf2_ros/message_filter.h"
+#include <message_filters/subscriber.h>
+
+
 namespace mavros {
 namespace plugin {
 /**
@@ -122,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,
@@ -200,6 +204,46 @@ class TF2ListenerMixin {
 			}
 		});
 	}
+
+	/**
+	 * @brief start tf listener syncronized with another topic
+	 *
+	 * @param _thd_name  listener thread name
+	 * @param cbp        plugin callback function
+	 */
+	template <class T>
+	void tf2_start(const char *_thd_name, message_filters::Subscriber<T> &topic_sub, void (D::*cbp)(const geometry_msgs::TransformStamped &, const typename T::ConstPtr &))
+	{
+		tf_thd_name = _thd_name;
+
+		tf_thread = std::thread([this, cbp, &topic_sub]() {
+			mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
+
+			mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
+			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;
+
+			tf2_ros::MessageFilter<T> tf2_filter(topic_sub, m_uas_->tf2_buffer, _frame_id, 10, _sp_nh);
+
+			ros::Rate rate(static_cast<D *>(this)->tf_rate);
+			while (ros::ok()) {
+				// Wait up to 3s for transform
+				if (m_uas_->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) {
+					try {
+						auto transform = m_uas_->tf2_buffer.lookupTransform(
+								_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0));
+
+						tf2_filter.registerCallback(boost::bind(cbp, static_cast<D *>(this), transform, _1));
+					}
+					catch (tf2::LookupException &ex) {
+						ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what());
+					}
+				}
+				rate.sleep();
+			}
+		});
+	}
 };
 }	// namespace plugin
 }	// namespace mavros

mavros/launch/apm_config.yaml

@@ -86,19 +86,20 @@ setpoint_accel:
 
 # setpoint_attitude
 setpoint_attitude:
-  reverse_throttle: false   # allow reversed throttle
+  reverse_thrust: false     # allow reversed thrust
+  use_quaternion: false     # enable PoseStamped topic subscriber
   tf:
     listen: false           # enable tf listener (disable topic subscribers)
-    frame_id: "local_origin"
-    child_frame_id: "attitude"
-    rate_limit: 10.0
+    frame_id: "map"
+    child_frame_id: "target_attitude"
+    rate_limit: 50.0
 
 # setpoint_position
 setpoint_position:
   tf:
     listen: false           # enable tf listener (disable topic subscribers)
-    frame_id: "local_origin"
-    child_frame_id: "setpoint"
+    frame_id: "map"
+    child_frame_id: "target_position"
     rate_limit: 50.0
 
 # setpoint_velocity
@@ -174,8 +175,8 @@ px4flow:
 vision_pose:
   tf:
     listen: false           # enable tf listener (disable topic subscribers)
-    frame_id: "local_origin"
-    child_frame_id: "vision"
+    frame_id: "map"
+    child_frame_id: "vision_estimate"
     rate_limit: 10.0
 
 # vision_speed_estimate
@@ -186,4 +187,7 @@ vision_speed:
 vibration:
   frame_id: "vibration"
 
+# adsb
+# None
+
 # vim:set ts=2 sw=2 et:

mavros/launch/px4_config.yaml

@@ -88,19 +88,20 @@ setpoint_accel:
 
 # setpoint_attitude
 setpoint_attitude:
-  reverse_throttle: false   # allow reversed throttle
+  reverse_thrust: false     # allow reversed thrust
+  use_quaternion: false     # enable PoseStamped topic subscriber
   tf:
     listen: false           # enable tf listener (disable topic subscribers)
-    frame_id: "local_origin"
-    child_frame_id: "attitude"
-    rate_limit: 10.0
+    frame_id: "map"
+    child_frame_id: "target_attitude"
+    rate_limit: 50.0
 
 # setpoint_position
 setpoint_position:
   tf:
     listen: false           # enable tf listener (disable topic subscribers)
-    frame_id: "local_origin"
-    child_frame_id: "setpoint"
+    frame_id: "map"
+    child_frame_id: "target_position"
     rate_limit: 50.0
 
 # setpoint_velocity
@@ -188,8 +189,8 @@ px4flow:
 vision_pose:
   tf:
     listen: false           # enable tf listener (disable topic subscribers)
-    frame_id: "local_origin"
-    child_frame_id: "vision"
+    frame_id: "map"
+    child_frame_id: "vision_estimate"
     rate_limit: 10.0
 
 # vision_speed_estimate

mavros/src/plugins/setpoint_accel.cpp

@@ -81,12 +81,12 @@ class SetpointAccelerationPlugin : public plugin::PluginBase,
 		auto accel = ftf::transform_frame_enu_ned(accel_enu);
 
 		set_position_target_local_ned(stamp.toNSec() / 1000000,
-				utils::enum_value(MAV_FRAME::LOCAL_NED),
-				ignore_all_except_a_xyz,
-				Eigen::Vector3d::Zero(),
-				Eigen::Vector3d::Zero(),
-				accel,
-				0.0, 0.0);
+					utils::enum_value(MAV_FRAME::LOCAL_NED),
+					ignore_all_except_a_xyz,
+					Eigen::Vector3d::Zero(),
+					Eigen::Vector3d::Zero(),
+					accel,
+					0.0, 0.0);
 	}
 
 	/* -*- callbacks -*- */