Publishing transforms both directions is incorrect

[tfoote]

The comment that you need to publish the transform the other way is incorrect.

mavros/mavros/src/lib/uas_data.cpp

Line 57 in bdc5c73

// for being able to apply the transforms at the data coming from the FCU, the opposite transform is also required

Only one of these should be used:

mavros/mavros/src/lib/uas_data.cpp

Lines 56 to 58 in 366a9a7

	publish_static_transform("fcu", "fcu_frd", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)));
	// for being able to apply the transforms at the data coming from the FCU, the opposite transform is also required
	publish_static_transform("fcu_frd", "fcu", Eigen::Affine3d(ftf::quaternion_from_rpy(-M_PI, 0, 0)));

They will fight with each other and cause continuous reparenting such that at any given time the last one recieved will win. Likely the first version tested caused a disconnected tree by inadvertent reparenting and adding the second call masked it out.

And while I'm looking at this code. It's cleaner and more efficient to publish a vector of static transforms to take advantage of latching rather than publishing a sequence of individual transforms.

[mhkabir]

We brought this up here before: #1252 (comment)

@TSC21 FYI

[mhkabir]

@jkflying @baumanta Can you guys try to test if the timestamp issue exists with the duplicate publisher removed?

[baumanta]

@tfoote you were right with your you comment on issue #388 . The tf loop in mavros seems to be responsible for getting always the tf at ros::Time::now().

I put a tf listener in the avoidance code which retrieved the transforms at the timestamp of the last pointcloud and at ros::Time::now() and printed both transforms. As the drone was moving I would expect the transforms to be slightly different. Using Mavros 0.29.2. both transforms were always identical even thought the timestamps were not. Using Mavros PR #1252 the transforms were different.

[mhkabir]

OK, I suspect why the odom plugin code wasn't working is due to an obscure limitation of latched topics - you cannot publish different messages to a latched topic from the same process.

See ros/ros_comm#146 and https://answers.ros.org/question/261815/how-can-i-access-all-static-tf2-transforms/.

The workaround for this is to construct a vector of all the transforms and publish them in one shot (pass to specialized TF2 function http://docs.ros.org/melodic/api/tf2_ros/html/c++/classtf2__ros_1_1StaticTransformBroadcaster.html#a83d0664cb0cc85688b60a2236e9d81ff)

[TSC21]

Fixed #1300.