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

mavros/include/mavros/setpoint_mixin.h

@@ -21,6 +21,11 @@
 #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>
+
 
 namespace mavros {
 namespace plugin {
@@ -162,8 +167,10 @@ class SetAttitudeTargetMixin {
 template <class D>
 class TF2ListenerMixin {
 public:
-	std::thread tf_thread;
-	std::string tf_thd_name;
+	std::thread tf_thread_1;
+	std::thread tf_thread_2;
+	std::string tf_thd_name_1;
+	std::string tf_thd_name_2;
 
 	/**
 	 * @brief start tf listener
@@ -173,11 +180,11 @@ class TF2ListenerMixin {
 	 */
 	void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &) )
 	{
-		tf_thd_name = _thd_name;
+		tf_thd_name_1 = _thd_name;
 		auto tf_transform_cb = std::bind(cbp, static_cast<D *>(this), std::placeholders::_1);
 
-		tf_thread = std::thread([this, tf_transform_cb]() {
-			mavconn::utils::set_this_thread_name("%s", tf_thd_name.c_str());
+		tf_thread_1 = std::thread([this, tf_transform_cb]() {
+			mavconn::utils::set_this_thread_name("%s", tf_thd_name_1.c_str());
 
 			mavros::UAS *m_uas_ = static_cast<D *>(this)->m_uas;
 			std::string &_frame_id = static_cast<D *>(this)->tf_frame_id;
@@ -193,7 +200,46 @@ class TF2ListenerMixin {
 						tf_transform_cb(transform);
 					}
 					catch (tf2::LookupException &ex) {
-						ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what());
+						ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name_1.c_str(), ex.what());
+					}
+				}
+				rate.sleep();
+			}
+		});
+	}
+
+	/**
+	 * @brief start tf listener syncronized with mavros_msgs::Thrust publisher 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 &))
+	{
+		tf_thd_name_2 = _thd_name;
+
+		tf_thread_2 = std::thread([this, cbp]() {
+			mavconn::utils::set_this_thread_name("%s", tf_thd_name_2.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;
+			message_filters::Subscriber<mavros_msgs::Thrust> &_thrust_sub = static_cast<D *>(this)->thrust_sub;
+
+			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_ros::MessageFilter<mavros_msgs::Thrust> tf2_filter(_thrust_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) {
+						ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name_2.c_str(), ex.what());
 					}
 				}
 				rate.sleep();

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 -*- */

mavros/src/plugins/setpoint_attitude.cpp

@@ -19,9 +19,13 @@
 #include <mavros/setpoint_mixin.h>
 #include <eigen_conversions/eigen_msg.h>
 
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
-#include <std_msgs/Float64.h>
+#include <mavros_msgs/Thrust.h>
 
 namespace mavros {
 namespace std_plugins {
@@ -37,7 +41,8 @@ class SetpointAttitudePlugin : public plugin::PluginBase,
 	SetpointAttitudePlugin() : PluginBase(),
 		sp_nh("~setpoint_attitude"),
 		tf_rate(10.0),
-		reverse_throttle(false)
+		use_quaternion(false),
+		reverse_thrust(false)
 	{ }
 
 	void initialize(UAS &uas_)
@@ -46,26 +51,46 @@ class SetpointAttitudePlugin : public plugin::PluginBase,
 
 		bool tf_listen;
 
+		typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PoseStamped, mavros_msgs::Thrust> SyncPoseThrust;
+		typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::TwistStamped, mavros_msgs::Thrust> SyncTwistThrust;
+
 		// main params
-		sp_nh.param("reverse_throttle", reverse_throttle, false);
+		sp_nh.param("use_quaternion", use_quaternion, false);
+		sp_nh.param("reverse_thrust", reverse_thrust, false);
 		// tf params
 		sp_nh.param("tf/listen", tf_listen, false);
 		sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
 		sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "aircraft");
 		sp_nh.param("tf/rate_limit", tf_rate, 10.0);
 
+		// thrust msg subscriber to sync
+		thrust_sub.subscribe(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_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb);
+						"Listen to desired attitude transform "
+						<< tf_frame_id << " -> " << tf_child_frame_id);
+
+			tf2_sync_start("AttitudeSpTFSync", &SetpointAttitudePlugin::transform_cb);
+		}
+		else if (use_quaternion) {
+			/**
+			 * @brief Use message_filters to sync attitude and thrust msg coming from different topics
+			 */
+			message_filters::Subscriber<geometry_msgs::PoseStamped> pose_sub(sp_nh, "attitude", 1);
+
+			/**
+			 * @brief Matches messages, even if they have different time stamps,
+			 * by using an adaptative algorithm <http://wiki.ros.org/message_filters/ApproximateTime>
+			 */
+			message_filters::Synchronizer<SyncPoseThrust> sync(SyncPoseThrust(10), pose_sub, thrust_sub);
+			sync.registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_pose_cb, this, _1, _2));
 		}
 		else {
-			twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this);
-			pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this);
+			message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub(sp_nh, "cmd_vel", 1);
+			message_filters::Synchronizer<SyncTwistThrust> sync(SyncTwistThrust(10), twist_sub, thrust_sub);
+			sync.registerCallback(boost::bind(&SetpointAttitudePlugin::attitude_twist_cb, this, _1, _2));
 		}
-
-		throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this);
 	}
 
 	Subscriptions get_subscriptions()
@@ -78,125 +103,108 @@ class SetpointAttitudePlugin : public plugin::PluginBase,
 	friend class TF2ListenerMixin;
 	ros::NodeHandle sp_nh;
 
-	ros::Subscriber twist_sub;
-	ros::Subscriber pose_sub;
-	ros::Subscriber throttle_sub;
-
 	std::string tf_frame_id;
 	std::string tf_child_frame_id;
+
+	message_filters::Subscriber<mavros_msgs::Thrust> thrust_sub;
+
 	double tf_rate;
-	bool reverse_throttle;
+	bool use_quaternion;
+	bool reverse_thrust;
+	float normalized_thrust;
+
+	/**
+	 * @brief Function to verify if the thrust values are normalized;
+	 * considers also the reversed trust values
+	 */
+	inline bool is_normalized(float thrust){
+		if (reverse_thrust) {
+			if (thrust < -1.0) {
+				ROS_WARN_NAMED("attitude", "Not normalized reversed thrust! Thd(%f) < Min(%f)", thrust, -1.0);
+				return false;
+			}
+		}
+		else {
+			if (thrust < 0.0) {
+				ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) < Min(%f)", thrust, 0.0);
+				return false;
+			}
+		}
+
+		if (thrust > 1.0) {
+			ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) > Max(%f)", thrust, 1.0);
+			return false;
+		}
+		return true;
+	}
 
 	/* -*- mid-level helpers -*- */
 
 	/**
-	 * @brief Send attitude setpoint to FCU attitude controller
-	 *
-	 * @note ENU frame.
+	 * @brief Send attitude setpoint and thrust to FCU attitude controller
 	 */
-	void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr)
+	void send_attitude_quaternion(const ros::Time &stamp, const Eigen::Affine3d &tr, const float thrust)
 	{
-		/* Thrust + RPY, also bits numbering started from 1 in docs
+		/**
+		 * @note RPY, also bits numbering started from 1 in docs
 		 */
-		const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
+		const uint8_t ignore_all_except_q_and_thrust = (7 << 0);
 
 		auto q = ftf::transform_orientation_enu_ned(
 					ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))
 					);
 
 		set_attitude_target(stamp.toNSec() / 1000000,
-				ignore_all_except_q,
-				q,
-				Eigen::Vector3d::Zero(),
-				0.0);
+					ignore_all_except_q_and_thrust,
+					q,
+					Eigen::Vector3d::Zero(),
+					thrust);
 	}
 
 	/**
-	 * @brief Send angular velocity setpoint to FCU attitude controller
-	 *
-	 * @note ENU frame.
+	 * @brief Send angular velocity setpoint and thrust to FCU attitude controller
 	 */
-	void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel)
+	void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel, const float thrust)
 	{
-		/* Q + Thrust, also bits noumbering started from 1 in docs
+		/**
+		 * @note Q, also bits noumbering started from 1 in docs
 		 */
-		const uint8_t ignore_all_except_rpy = (1 << 7) | (1 << 6);
+		const uint8_t ignore_all_except_rpy = (1 << 7);
 
 		auto av = ftf::transform_frame_baselink_aircraft(ang_vel);
 
 		set_attitude_target(stamp.toNSec() / 1000000,
-				ignore_all_except_rpy,
-				Eigen::Quaterniond::Identity(),
-				av,
-				0.0);
-	}
-
-	/**
-	 * @brief Send throttle to FCU attitude controller
-	 */
-	void send_attitude_throttle(const float throttle)
-	{
-		// Q + RPY
-		const uint8_t ignore_all_except_throttle = (1 << 7) | (7 << 0);
-
-		set_attitude_target(ros::Time::now().toNSec() / 1000000,
-				ignore_all_except_throttle,
-				Eigen::Quaterniond::Identity(),
-				Eigen::Vector3d::Zero(),
-				throttle);
+					ignore_all_except_rpy,
+					Eigen::Quaterniond::Identity(),
+					av,
+					thrust);
 	}
 
 	/* -*- callbacks -*- */
 
-	void transform_cb(const geometry_msgs::TransformStamped &transform) {
+	void transform_cb(const geometry_msgs::TransformStamped &transform, const mavros_msgs::Thrust::ConstPtr &thrust_msg) {
 		Eigen::Affine3d tr;
 		tf::transformMsgToEigen(transform.transform, tr);
 
-		send_attitude_target(transform.header.stamp, tr);
+		send_attitude_quaternion(transform.header.stamp, tr, thrust_msg->thrust);
 	}
 
-	void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) {
+	void attitude_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose_msg, const mavros_msgs::Thrust::ConstPtr &thrust_msg) {
 		Eigen::Affine3d tr;
-		tf::poseMsgToEigen(req->pose, tr);
+		tf::poseMsgToEigen(pose_msg->pose, tr);
 
-		send_attitude_target(req->header.stamp, tr);
+		if (is_normalized(thrust_msg->thrust))
+			send_attitude_quaternion(pose_msg->header.stamp, tr, thrust_msg->thrust);
 	}
 
-	void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
+	void attitude_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req, const mavros_msgs::Thrust::ConstPtr &thrust_msg) {
 		Eigen::Vector3d ang_vel;
 		tf::vectorMsgToEigen(req->twist.angular, ang_vel);
 
-		send_attitude_ang_velocity(req->header.stamp, ang_vel);
+		if (is_normalized(thrust_msg->thrust))
+			send_attitude_ang_velocity(req->header.stamp, ang_vel, thrust_msg->thrust);
 	}
 
-	inline bool is_normalized(float throttle, const float min, const float max)
-	{
-		if (throttle < min) {
-			ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) < Min(%f)", throttle, min);
-			return false;
-		}
-		else if (throttle > max) {
-			ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) > Max(%f)", throttle, max);
-			return false;
-		}
-
-		return true;
-	}
-
-	void throttle_cb(const std_msgs::Float64::ConstPtr &req)
-	{
-		float throttle_normalized = req->data;
-
-		/**
-		 * && are lazy, is_normalized() should be called only if reverse_throttle are true.
-		 */
-		if (reverse_throttle && !is_normalized(throttle_normalized, -1.0, 1.0))
-			return;
-		else if (!is_normalized(throttle_normalized, 0.0, 1.0))
-			return;
-
-		send_attitude_throttle(throttle_normalized);
-	}
 };
 }	// namespace std_plugins
 }	// namespace mavros

mavros_msgs/CMakeLists.txt

@@ -31,6 +31,7 @@ add_message_files(
   RCOut.msg
   RadioStatus.msg
   State.msg
+  Thrust.msg
   VFR_HUD.msg
   Vibration.msg
   Waypoint.msg
@@ -85,4 +86,3 @@ catkin_package(
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h")
-

mavros_msgs/msg/Thrust.msg

@@ -0,0 +1,5 @@
+# Thrust to send to the FCU
+
+std_msgs/Header header
+
+float32 thrust