def run ( self ):
# sherlock logger
# l_log = logging.getLogger ( "ARDroneNetworkProcess::run" )
# l_log.setLevel ( w_logLvl )
# l_log.debug ( ">>" )
# create video socket
l_vid_socket = socket.socket ( socket.AF_INET, socket.SOCK_DGRAM )
# assert ( l_vid_socket )
# config socket
l_vid_socket.setblocking ( 0 )
l_vid_socket.bind (( "", arDefs.ARDRONE_VIDEO_PORT ))
l_vid_socket.sendto ( "\x01\x00\x00\x00", ( arDefs.ARDRONE_IP_ADDRESS, arDefs.ARDRONE_VIDEO_PORT ))
# create navData socket
l_nav_socket = socket.socket ( socket.AF_INET, socket.SOCK_DGRAM )
# assert ( l_nav_socket )
# config socket
l_nav_socket.setblocking ( 0 )
l_nav_socket.bind (( "", arDefs.ARDRONE_NAVDATA_PORT ))
l_nav_socket.sendto ( "\x01\x00\x00\x00", ( arDefs.ARDRONE_IP_ADDRESS, arDefs.ARDRONE_NAVDATA_PORT ))
lv_stopping = False
while ( not lv_stopping ):
l_inputReady, l_outputReady, l_exceptReady = select.select ( [ l_nav_socket, l_vid_socket, self._com_pipe ], [], [] )
for lo_srce in l_inputReady:
# video socket ?
if ( l_vid_socket == lo_srce ):
while ( True ):
try:
# receive video data
l_data = l_vid_socket.recv ( 65535 )
except IOError:
# we consumed every packet from the socket and continue with the last one
break
# decode video data
l_w, l_h, l_image, l_t = arVideo.read_picture ( l_data )
# send over pipe
self._vid_pipe.send ( l_image )
# navdata socket ?
elif ( l_nav_socket == lo_srce ):
while ( True ):
try:
# receive navdata
lo_data = l_nav_socket.recv ( 65535 )
except IOError:
# we consumed every packet from the socket and continue with the last one
break
# decode navdata
l_navdata, lv_has_information = arNavData.decode_navdata ( lo_data )
# l_log.debug ( "l_navdata: " + str ( l_navdata ))
#if ( lv_has_information ):
#self._drone.set_navdata ( l_navdata )
# send over pipe
self._nav_pipe.send ( l_navdata )
# command pipe ?
elif ( self._com_pipe == lo_srce ):
# receive command
_ = self._com_pipe.recv ()
lv_stopping = True
break
# close sockets
l_vid_socket.close ()
l_nav_socket.close ()
def run ( self ):
# sherlock logger
# l_log = logging.getLogger ( "ARDrone2NetworkProcess::run" )
# l_log.setLevel ( w_logLvl )
# l_log.debug ( ">>" )
# -----------------------------------------------------------------------------------------
# ARDrone2NetworkProcess::run::_connect
# -----------------------------------------------------------------------------------------
def _connect ():
# sherlock logger
# l_log = logging.getLogger ( "ARDrone2NetworkProcess::run::_connect" )
# l_log.setLevel ( w_logLvl )
# l_log.debug ( ">>" )
# l_log.warn ( "Connection to AR.Drone on way ..." )
l_nav_socket = socket.socket ( socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP )
l_nav_socket.setblocking ( 0 )
l_nav_socket.bind (( "", arDefs.ARDRONE_NAVDATA_PORT ))
l_nav_socket.sendto ( "\x01\x00\x00\x00", ( arDefs.ARDRONE_IP_ADDRESS, arDefs.ARDRONE_NAVDATA_PORT ))
l_control_socket = socket.socket ( socket.AF_INET, socket.SOCK_STREAM )
l_control_socket.connect (( arDefs.ARDRONE_IP_ADDRESS, arDefs.ARDRONE_CONTROL_PORT ))
l_control_socket.setblocking ( 0 )
# l_log.warn ( "Connection ok" )
# sherlock logger
# l_log.debug ( "<<" )
return l_nav_socket, l_control_socket
# -----------------------------------------------------------------------------------------
# ARDrone2NetworkProcess::run::_disconnect
# -----------------------------------------------------------------------------------------
def _disconnect ( nav_socket, control_socket ):
# sherlock logger
# l_log = logging.getLogger ( "ARDrone2NetworkProcess::run::_disconnect" )
# l_log.setLevel ( w_logLvl )
# l_log.debug ( ">>" )
# l_log.warn ( "Deconnection AR.Drone" )
# close sockets
nav_socket.close ()
control_socket.close ()
# sherlock logger
# l_log.debug ( "<<" )
# -----------------------------------------------------------------------------------------
# run
# -----------------------------------------------------------------------------------------
l_nav_socket, l_control_socket = _connect ()
lv_stopping = False
li_connection_lost = 1
lv_reconnection_needed = False
while ( not lv_stopping ):
if ( lv_reconnection_needed ):
_disconnect ( l_nav_socket, l_control_socket )
l_nav_socket, l_control_socket = _connect ()
lv_reconnection_needed = False
l_inputReady, l_outputReady, l_exceptReady = select.select ( [ l_nav_socket, self._com_pipe, l_control_socket ], [], [], 1. )
if ( 0 == len ( l_inputReady )):
li_connection_lost += 1
lv_reconnection_needed = True
for lo_srce in l_inputReady:
if ( l_nav_socket == lo_srce ):
while ( True ):
try:
lo_data = l_nav_socket.recv ( 500 )
except IOError:
# we consumed every packet from the socket and continue with the last one
break
l_navdata, lv_has_information = arNavData.decode_navdata ( lo_data )
if ( lv_has_information ):
self._drone.set_navdata ( l_navdata )
elif ( self._com_pipe == lo_srce ):
_ = self._com_pipe.recv ()
lv_stopping = True
break
elif ( l_control_socket == lo_srce ):
lv_reconnection_needed = False
while ( not lv_reconnection_needed ):
try:
data = l_control_socket.recv ( 65536 )
if ( 0 == len ( data )):
# l_log.warning ( "Received an empty packet on control socket" )
lv_reconnection_needed = True
else:
# l_log.warning ( "Control Socket says : %s", data )
pass
except IOError:
break
_disconnect ( l_nav_socket, l_control_socket )
