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

TSC21 · 2017-05-02T11:28:02Z

Sync between thrust and pose msg
Sync between thrust and twist msg
Sync between thrust and transform

…pics to be sent together

TSC21 · 2017-05-02T11:28:50Z

mavros/src/plugins/setpoint_accel.cpp

-				Eigen::Vector3d::Zero(),
-				accel,
-				0.0, 0.0);
+					utils::enum_value(MAV_FRAME::LOCAL_NED),


I applied the uncrustify to the wrong file initially so that's why this changed.

TSC21 · 2017-05-02T11:29:35Z

mavros/src/plugins/setpoint_attitude.cpp

 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
-#include <std_msgs/Float64.h>
+#include <mavros_msgs/Float32Stamped.h>


This was needed as Float64 doesn't have a stamp so to sync.

Since I'm changing this to a custom msg, maybe I should change to Thrust.msg only

TSC21 · 2017-05-02T11:55:01Z

Seems like APM is also able to handle RPY together with throttle. Will add it then

TSC21 · 2017-05-02T12:17:01Z

@rmackay9 FYI

…rust)

TSC21 · 2017-05-11T22:13:28Z

ping @vooon

…criber

TSC21 · 2017-06-11T02:15:41Z

@vooon please review. thanks

TSC21 · 2017-06-15T12:08:47Z

@vooon this is tested. friendly ping!

vooon · 2017-06-15T14:17:24Z

mavros/include/mavros/setpoint_mixin.h

+	 * @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 &))


I don't like that change. Should be better way.

This change took me half day to figure a way to work lol If you have a different proposal that works the same way or better I accept it

Besides I'm doing some updates on mavteleop that require that this PR is merged to work properly. Also, this change is required to setpoint_attitude to work as it should, and I don't think this has to be delayed more

The only optimization I can think of is generalize the type of message we are syncing the tf listener with, instead of with the thrust only. That way we have a generalized way of syncing different types of topics with the transform.

TSC21 · 2017-06-15T15:53:38Z

@vooon I think right now this is the best that we can get right (a generic function template that syncs any topic with a transform listener). Besides now the templates have the same name but different classes/typenames, which in terms of code structure is better on the plugin side (is not a template friend class but a friend class with function templates). And this is working. So unless you have a better proposal, I think this is good to go.

…d only

TSC21 · 2017-06-22T19:17:41Z

@vooon ok better now I kept the template for the class and only added the template for the tf2_start in case of sync needed. Can you review and merge of ok, please? Thanks

vooon

I do not see why you need two threads in TF2ListenerMixin.
Other looks ok, but also please update config yaml in launch.

vooon · 2017-06-23T13:23:57Z

mavros/include/mavros/setpoint_mixin.h

+	std::thread tf_thread_1;
+	std::thread tf_thread_2;
+	std::string tf_thd_name_1;
+	std::string tf_thd_name_2;


Why you need two threads? Only one tf_start() should be used.

If you are using one thread for tf2_start in one plugin and another tf2_start for another plugin which is using sync (the case of setpoint_ attitude), shouldn't two different threads be used?

Each plugin instance (object) have their std::thread object.
It is not shared between plugins, only inside class instance.

Ok will use one only then.

vooon · 2017-06-23T13:26:56Z

mavros/src/plugins/setpoint_attitude.cpp

@@ -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;


Use using instead typedef.

using SyncPoseThrust = mesage_filters::sync_polices::ApproximateTime<geometry_msgs::PoseStamped, mavros_msgs::Thrust>;

Also may be good to move out of method, e.g. place in std_plugins::.

Makes sense

vooon · 2017-06-23T13:28:23Z

mavros/src/plugins/setpoint_attitude.cpp

 		// 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
+		message_filters::Subscriber<mavros_msgs::Thrust> thrust_sub(sp_nh, "thrust", 10);


Why subscriber here? How it can survive when initialize() returns?

Oh yeah mistake. I will put on public and then initialize here, as before.

vooon · 2017-06-23T13:29:21Z

mavros/src/plugins/setpoint_attitude.cpp

+						"Listen to desired attitude transform "
+						<< tf_frame_id << " -> " << tf_child_frame_id);
+
+			tf2_start<mavros_msgs::Thrust>("AttitudeSpTFSync", &SetpointAttitudePlugin::transform_cb);


Message type should be deduced from callback type automagically.

so I should remove <mavros_msgs::Thrust>?

Yes, and in theory is should still works :)

It didn't work

TSC21 · 2017-06-23T13:47:16Z

but also please update config yaml in launch.

Ok sure.

vooon · 2017-06-23T13:53:16Z

mavros/include/mavros/setpoint_mixin.h

+								_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0));
+
+						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));


That looks weird. Creating new filter on each TF rx not good. And _topic_sub not initialized, there no topic name.
Do you test that?

I had _top_sub initialized before in a proper way. I don't know why I changed it. I would roll back and initialize it on plugin and use it here on the template by applying static_cast<D *>(this)->. Sorry for that

Creating new filter on each TF rx not good.

Yeah I will put it outside the while loop.

TSC21 · 2017-06-24T15:13:26Z

@vooon done. Do you have a suggestion on how #700 (diff) could work?

vooon · 2017-06-25T15:30:19Z

mavros/src/plugins/setpoint_attitude.cpp

@@ -106,7 +106,10 @@ class SetpointAttitudePlugin : public plugin::PluginBase,
 	std::string tf_frame_id;
 	std::string tf_child_frame_id;

+	message_filters::Subscriber<mavros_msgs::Thrust> sub;


s/sub/thd_sub/

What do you mean?

General "sub" bad for later support, thats why i want more specific name like thd_sub.
And to solve problem with tf_start(), just pass by reference.

Or move whole tf_start() code out of mixin, since it looks specific to that plugin.
Or you plan to use that variation somewhere else?

vooon · 2017-06-28T16:14:56Z

mavros/include/mavros/setpoint_mixin.h

+			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<T> &_topic_sub = static_cast<D *>(this)->sub;


Pass it via args!

vooon · 2017-06-28T16:17:08Z

mavros/include/mavros/setpoint_mixin.h

+	{
+		tf_thd_name = _thd_name;
+
+		tf_thread = std::thread([this, cbp]() {


void tf2_start(const char *_thd_name, message_fileters::Subscriber<T> &tsub, ...) ... th_therad = std::thread([this, cbp, tsub]() { ...

…g it

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

plugins: setpoint_attitude: add sync between throttle and attitude to…

1f01ab8

…pics to be sent together

TSC21 commented May 2, 2017

View reviewed changes

TSC21 changed the title ~~[WIP] Plugins: setpoint_attitude: add sync between throttle and attitude~~ [WIP] Plugins: setpoint_attitude: add sync between thrust and attitude May 2, 2017

TSC21 added 2 commits May 2, 2017 14:38

plugins: typo correction: replace throttle with thrust

f84b046

plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg

175479e

TSC21 requested a review from vooon May 2, 2017 12:15

TSC21 self-assigned this May 2, 2017

TSC21 added APM enhancement plugin PX4 labels May 2, 2017

plugins: setpoint_attitude: add sync between twist and thrust (RPY+Th…

35d31e3

…rust)

TSC21 force-pushed the attitude_plug_msg_sync branch from ba54714 to 35d31e3 Compare May 2, 2017 12:19

TSC21 added PX4 and removed PX4 labels May 2, 2017

TSC21 modified the milestone: Version 0.20 May 3, 2017

TSC21 mentioned this pull request Jun 3, 2017

[setpoint_attitude] Don't work with APM #396

Closed

TSC21 added 2 commits June 10, 2017 10:50

setpoint_attitude: update the logic of thrust normalization verification

008c99e

setpoint_attitude: implement sync between tf listener and thrust subs…

4b45304

…criber

vooon reviewed Jun 15, 2017

View reviewed changes

TSC21 force-pushed the attitude_plug_msg_sync branch 2 times, most recently from 8bf9304 to 18ffd0a Compare June 15, 2017 15:46

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

b958de3

TSC21 force-pushed the attitude_plug_msg_sync branch from 18ffd0a to b958de3 Compare June 15, 2017 15:48

TSC21 closed this Jun 15, 2017

TSC21 reopened this Jun 15, 2017

TSC21 closed this Jun 17, 2017

TSC21 reopened this Jun 17, 2017

TF2ListenerMixin: keep class template, use template for tf sync metho…

98e6535

…d only

TSC21 closed this Jun 23, 2017

TSC21 reopened this Jun 23, 2017

vooon requested changes Jun 23, 2017

View reviewed changes

vooon reviewed Jun 23, 2017

View reviewed changes

TSC21 added 2 commits June 24, 2017 14:55

TF2ListenerMixin: fix and improve sync tf2_start method

f50453c

general update to yaml config files and parameters

2927dc9

vooon reviewed Jun 25, 2017

View reviewed changes

setpoint_attitude: add note on Thrust sub name

c4d9757

vooon requested changes Jun 28, 2017

View reviewed changes

setpoint_attitude: TF sync: pass subscriber pointer instead of bindin…

7fd9231

…g it

vooon merged commit df9f380 into mavlink:master Jun 28, 2017

TSC21 deleted the attitude_plug_msg_sync branch June 28, 2017 17:10

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

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

Conversation

TSC21 commented May 2, 2017 • edited

TSC21 May 2, 2017 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented May 2, 2017

TSC21 commented May 2, 2017

TSC21 commented May 11, 2017

TSC21 commented Jun 11, 2017

TSC21 commented Jun 15, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented Jun 15, 2017 • edited

TSC21 commented Jun 22, 2017

vooon left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented Jun 23, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented Jun 24, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented May 2, 2017 •

edited

TSC21 May 2, 2017 •

edited

TSC21 commented Jun 15, 2017 •

edited