Use tf instead of /pid_input topic

[fwalch]

@noamyogev84 @fselius @efiShtain

After last meeting we discussed how/where the "PID loop" is supposed to run. We talked about that maybe we can just run it in a callback from /pid_input, as the PID values need to be updated each time we get new input. One problem with this is that there won't always be continuous input to /pid_input, e.g. if the CV node loses sight of the target.

However, we can use tf to run a continuous loop and keep a fixed rate. So instead of listening to /pid_input and receive a callback on each input, the navigation node can poll the last known delta from tf. That way, it can also know if the CV node lost sight of the target, because then the tf data will have an old timestamp.

This is how I thought it could work:

import tf
import rospy

class Navigation:
    def __init__(self):
        self.t = tf.TransformListener()
        self.r = rospy.Rate(25)  # 25 Hz

        # Register service callback
        # ...

    # Callback for /set_pid_mode service
    def on_switch_mode(self, mode):
        self._pid_mode = mode

    def execute(self):
        timeout = rospy.Duration(0.01)
        last_lookup = rospy.Time.now()
        while not rospy.is_shutdown():
            # Look up the latest values
            try:
                # http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28Python%29
                now = rospy.Time.now()
                self.t.waitForTransform('/base_link', '/waypoint', now, timeout)
                translation, rotation = self.t.lookupTransform('/base_link', '/waypoint', now)
                last_lookup = now
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                # Did not receive a new delta in 'timeout' time.
                # Stop drone? Maybe check when last_lookup was?
                # ...
                pass

            # Update PID
            if self._pid_mode == 'slow':
                # ...
                pass
            elif self._pid_mode == 'fast':
                # ...
                pass

            # Send PID values to MavROS
            # ...

            # Will sleep as needed to keep the 25 Hz. See http://wiki.ros.org/rospy/Overview/Time#Sleeping_and_Rates
            r.sleep()

if __name__ == '__main__':
    rospy.init('navigation')
    n = Navigation()
    n.execute()

What do you think? Could that work?

[fwalch]

This is just an idea I had, not sure if it makes sense. I don't want to complicate things for you or push you to make changes; for the CV module it doesn't make a difference whether we publish to a topic or use tf.

Just FYI, if we use tf, we can use something like the following to publish test transforms (10 Hz in this example):

rosrun tf static_transform_publisher 0 1 1 0 0 0 1 base_link waypoint 10

[fselius]

@noamyogev84 @efiShtain , your call. Try to lower latency as much as possible though.