ROS Integration - Hanging on navdata_ready.wait() #22
Description
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.