Tf broadcaster dynamically changing values

Question

Hi guys,

my robot's sensor angle can be changed during its run so the transformations have to be dynamically changed. For now I have a node which reads pointclouds from the sensor, detects the angle of the sensor and publishes this information on a topic. Then I have a node which publishes transformations (a simple tf broadcaster):

ros::Rate rate(1000);
while(nodeHandler.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, detectedAngle, 0, 1), tf::Vector3(0.12, 0.0, 0.28)),
        ros::Time::now(),"base_footprint", "camera_link"));
    rate.sleep();
  }

I need the broadcaster node to read the message from the angle topic and use it in a detectedAngle variable, so it's always up to date. Now I'm not sure how the code should look like. I probably cannot use the typical subscriber syntax with ros::spin() as I have this cycle in the tf broadcaster. I could write a simple subscriber to the angle topic and publish the transformation in its callback function but I'm not sure if the transformation frequency would be enough (it would be the frequency of the sensor), or would be? Isn't there any tool in ros which could be used here? I imagine something like: the tf broadcaster node publishes transformation with some rate (e.g. 100Hz) and if some message come to the angle topic, the broadcaster updates the detectedAngle variable with the new value and continues publishing transformations.

---

Originally posted by sykatch on ROS Answers with karma: 109 on 2017-08-21

Post score: 0

---

Original comments

Comment by gvdhoorn on 2017-08-21:
I deleted my previous answer as I'd missed the:

For now I have a node which reads pointclouds from the sensor, detects the angle of the sensor and publishes this information on a topic.

bit.

Comment by gvdhoorn on 2017-08-21:
Is detectedAngle a 1D value? If it's only a yaw fi, you could model it as a joint and then still use urdf + robot_state_publisher to get 'dynamically updated TFs'.

Comment by gvdhoorn on 2017-08-21:
just noticed this:

tf::Transform(tf::Quaternion(0, detectedAngle, 0, 1), ...)

is this correct? a Quaternion is not an RPY triple, so if detectedAngle is supposed to be a pitch value (fi), then the resulting Quaternion will most likely never match it.

Comment by sykatch on 2017-08-23:
I was not sure if it is correct, I didn't run it yet. I just put it there as I thought these are the same values as the arguments for static_transform_publisher. And to answer if detectedAngle is a 1D value, it is one float number representing the angle.

Comment by sykatch on 2017-08-29:
Finally I used urdf, state publisher and joint publisher and its working. I'd mark your answer as correct but its deleted. Thank you anyway for showing me the right direction.

Answer 1

It sounds like the tf broadcast should go right into the original node that is acquiring both the angle and points.

Otherwise the node publishing detected angle needs to send a timestamp along with it, and that time re-used in the tf broadcaster callback instead of ros::Time::now(). The point cloud also needs to be timestamped correctly, ideally there is a clock on the sensor and you are not using the time the node received the data from the sensor (unless you can show the delay is fixed and you can measure it and subtract it out, or it is negligibly small).

(and also use an euler to quaternion function as hinted at above).

---

Originally posted by lucasw with karma: 8729 on 2017-08-21

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by sykatch on 2017-08-23:
If I put the tf broadcast to the original node, wouldn't the frequency of the published transforms be too low? Or is it enough to publish transforms with the sensor frame rate?

Comment by lucasw on 2017-08-23:
Isn't the frequency of the detected angle the same as the point cloud update rate? And they are acquired at the same time or nearly the same time? If that is true then it doesn't matter what detected angle is at times inbetween clouds.

Comment by lucasw on 2017-08-23:
Even if the detected angle was a lower frequency or highly offset from the point cloud, if detected angle is changing smoothly enough then the linear interpolation of tf ought to be good enough to provide a good estimate of what the detected angle was at an in-between time when the points were made.