ROS Integration - Hanging on navdata_ready.wait()

[magicbycalvin]

Greetings,

I am writing a Robot Operating System (ROS) wrapper for this library and have been running into some issues. I suspect the issues I have are thread and/or network related. I wrote a simple example script to test whether I could control multiple AR Drones at once and that was successful. However, when I try to run similar code as a ROS node, I don't seem to be getting the results I am expecting. In addition to issues when running a ROS node, I also am sometimes unable to reconnect to the drone if I am testing code in the same Python interpreter. Perhaps I am not properly closing the connection?

Other than calling drone.close() when I am done, should I be doing anything else to close the connection and destruct the object?

Here is some sample code (excuse the mess, it is still in development). For the most part, you shouldn't need to know anything about ROS to understand the code. One important fact to note is that ROS will run the code below in its own instance of Python (as far as I know). So, when I am testing multiple drones, I simply have ROS spawn multiple nodes.

import sys

from geometry_msgs.msg import TwistStamped
import rospy
from std_msgs.msg import Empty

from pyardrone import ARDrone

class ARDroneCommander:
    def __init__(self, ip_addr):
        rospy.loginfo('Initializing AR Drone connection...')
        self.drone = ARDrone(host=ip_addr)
        self.drone.navdata_ready.wait()
        rospy.loginfo('AR Drone connection initialized.')

        self._cmd_sub = rospy.Subscriber('cmd_vel', TwistStamped, self.send_cmd)
        self._takeoff_sub = rospy.Subscriber('takeoff', Empty, lambda x: self.takeoff)
        self._land_sub = rospy.Subscriber('land', Empty, lambda x: self.land)
        self._reset_sub = rospy.Subscriber('reset', Empty, lambda x: self.reset)

        self.flying = False

    def __enter__(self):
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.land()

        rospy.loginfo('Closing AR Drone connection...')
        self.drone.close()
        rospy.loginfo('AR Drone connection closed.')

    def send_cmd(self, data):
        xvel = data.twist.linear.x
        yvel = data.twist.linear.y
        zvel = data.twist.linear.z
        zyaw = data.twist.angular.z

        self.drone._move(roll=yvel, pitch=-xvel, gaz=zvel, yaw=zyaw)

    def takeoff(self):
        self.reset()
        while not self.drone.state.fly_mask:
            self.drone.takeoff()
        self.flying = True

    def land(self):
        rospy.loginfo('Landing AR Drone...')
        while self.drone.state.fly_mask:
            self.drone.land()
        self.flying = False
        rospy.loginfo('AR Drone safely landed.')

    def reset(self):
        if self.drone.state.emergency_mask:
            self.drone.emergency()


if __name__ == '__main__':
    rospy.init_node('ardrone_ctrl', anonymous=True)

    ip_addr = sys.argv[1]

    with ARDroneCommander(ip_addr) as ardrone:
        while not rospy.is_shutdown():
            pass

For reference, I am also including the simple test code I wrote for controlling two drones:

import time
from pyardrone import ARDrone


if __name__ == '__main__':
    ar0 = ARDrone(host='192.168.0.10')
    ar1 = ARDrone(host='192.168.0.11')

    ar0.navdata_ready.wait()
    ar1.navdata_ready.wait()

    try:
        print('Taking off!')
        while not ar0.state.fly_mask and not ar1.state.fly_mask:
            ar0.takeoff()
            ar1.takeoff()

        print('Waiting 3 seconds...')
        time.sleep(3)

        print('Yawing...')
        ts = time.time()
        while time.time() - ts < 3.0:
            ar0._move(yaw=-1)
            ar1._move(roll=0.0, yaw=1)

    finally:
        print('Landing...')
        while ar0.state.fly_mask and ar1.state.fly_mask:
            ar0.land()
            ar1.land()

    print('DONE!')

Any help would be greatly appreciated! Please let me know if I can get any more information to aid in debugging this problem.

[afq984]

I also am sometimes unable to reconnect to the drone if I am testing code in the same Python interpreter.

This shouldn't happen. Maybe there's a bug in pyardrone where resources aren't cleaned up completely.

Unfortunately, I don't have the device anymore, but here are a few directions that I'd look into:

• Enable python dev mode. ResourceWarnings might show up when the interpreter is terminating with unclosed sockets.
• threading.enumerate to look for unclosed threads after drone.close()

[magicbycalvin]

Thanks for your reply. I will check those out this coming week and get back to you if I find anything. Should it be a simple fix, I'll make a pull request.