class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.auto2_index = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.auto2_index = settings.name_lookup["auto2"].index
except Exception as e:
print(e)
print("auto2 setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("guided mode example")
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg["ac_id"] = self.ac_id
msg["index"] = self.auto2_index
msg["value"] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg["ac_id"] = self.ac_id
msg["index"] = self.auto2_index
msg["value"] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x00
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x01
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x03
msg["x"] = forward
msg["y"] = right
msg["z"] = down
msg["yaw"] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x70
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = yaw
print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(msg)
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.auto2_index = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.auto2_index = settings.name_lookup['auto2'].index
except Exception as e:
print(e)
print("auto2 setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("guided mode example")
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x00
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x0D
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x0E
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_ned_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x60
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x62
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("gb2ivy")
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def bind_flight_param(self, send_cb):
def bat_cb(ac_id, msg):
bat = float(msg['bat'])
# should not be more that 18 characters
send_cb('bat: ' + str(bat) + ' V')
self._interface.subscribe(bat_cb,
regex=('(^ground ENGINE_STATUS ' +
str(self.ac_id) + ' .*)'))
def set_guided_mode(self):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName(
'Guided') # AP_MODE_GUIDED
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'GUIDED') # AP_MODE_GUIDED
except ValueError:
msg['value'] = 19 # fallback to fixed index
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'NAV') # AP_MODE_NAV
except ValueError:
msg['value'] = 13 # fallback to fixed index
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0xE2
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def command_callback(self, command):
"""
convert incoming command into velocity
"""
right = 0.0
forward = 0.0
down = 0.0
yaw = 0.0
command = int(command)
if command & J_RIGHT:
right += 2.0
if command & J_LEFT:
right -= 2.0
if command & J_UP:
forward += 2.0
if command & J_DOWN:
forward -= 2.0
if command & J_A:
down -= 1.0
if command & J_B:
down += 1.0
if command & J_START:
yaw += radians(20)
if command & J_SELECT:
yaw -= radians(20)
self.move_at_body_vel(forward, right, down, yaw)
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'].index
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("gb2ivy")
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def bind_flight_param(self, send_cb):
def bat_cb(ac_id, msg):
bat = float(msg['bat'])
# should not be more that 18 characters
send_cb('bat: '+str(bat)+' V')
self._interface.subscribe(bat_cb, regex=('(^ground ENGINE_STATUS '+str(self.ac_id)+' .*)'))
def set_guided_mode(self):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0xE2
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def command_callback(self, command):
"""
convert incoming command into velocity
"""
right = 0.0
forward = 0.0
down = 0.0
yaw = 0.0
command = int(command)
if command & J_RIGHT:
right += 2.0
if command & J_LEFT:
right -= 2.0
if command & J_UP:
forward += 2.0
if command & J_DOWN:
forward -= 2.0
if command & J_A:
down -= 1.0
if command & J_B:
down += 1.0
if command & J_START:
yaw += radians(20)
if command & J_SELECT:
yaw -= radians(20)
self.move_at_body_vel(forward, right, down, yaw)
class RtpViewer:
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
# Start an 'infinite' loop
while True:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved or 'q' is pressed
if not ret or cv2.waitKey(1) & 0xFF == ord('q'):
break
# Run the computer vision function
self.cv()
def cv(self):
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'], (0, 255, 0), 2)
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
class RtpViewer:
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
# Start an 'infinite' loop
while True:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved or 'q' is pressed
if not ret or cv2.waitKey(1) & 0xFF == ord('q'):
break
# Run the computer vision function
self.cv()
def cv(self):
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'],
(0, 255, 0), 2)
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
class Guidance(object):
def __init__(self, ac_id):
self.ac_id = ac_id
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("sim_rc_4ch")
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def set_guided_mode(self):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName(
'Guided') # AP_MODE_GUIDED
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'GUIDED') # AP_MODE_GUIDED
except ValueError:
msg['value'] = 19 # fallback to fixed index
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'NAV') # AP_MODE_NAV
except ValueError:
msg['value'] = 13 # fallback to fixed index
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def rc_4_channel_output(self,
mode=2,
throttle=0.0,
roll=0.0,
pitch=0.0,
yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "RC_4CH")
msg['ac_id'] = self.ac_id
msg['mode'] = mode
msg['throttle'] = throttle
msg['roll'] = roll
msg['pitch'] = pitch
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
class RtpViewer:
running = False
scale = 1
rotate = 0
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
self.running = True
# Start an 'infinite' loop
while self.running:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved
if not ret:
break
# Run the computer vision function
self.cv()
# Process key input
self.on_key(cv2.waitKey(1) & 0xFF)
def cv(self):
# Rotate the image by increments of 90
if self.rotate % 2:
self.frame = cv2.transpose(self.frame)
if self.rotate > 0:
self.frame = cv2.flip(self.frame, [1, -1, 0][self.rotate - 1])
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'],
(0, 255, 0), 2)
if self.scale != 1:
h, w = self.frame.shape[:2]
self.frame = cv2.resize(self.frame,
(int(self.scale * w), int(self.scale * h)))
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_key(self, key):
if key == ord('q'):
self.running = False
if key == ord('r'):
self.rotate = (self.rotate + 1) % 4
self.mouse['start'] = None
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN and self.rotate == 0:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
class RtpViewer:
running = False
scale = 1
rotate = 0
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
self.running = True
# Start an 'infinite' loop
while self.running:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved
if not ret:
break
# Run the computer vision function
self.cv()
# Process key input
self.on_key(cv2.waitKey(1) & 0xFF)
def cv(self):
# Rotate the image by increments of 90
if self.rotate % 2:
self.frame = cv2.transpose(self.frame)
if self.rotate > 0:
self.frame = cv2.flip(self.frame, [1, -1, 0][self.rotate - 1])
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'], (0, 255, 0), 2)
if self.scale != 1:
h, w = self.frame.shape[:2]
self.frame = cv2.resize(self.frame, (int(self.scale * w), int(self.scale * h)))
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_key(self, key):
if key == ord('q'):
self.running = False
if key == ord('r'):
self.rotate = (self.rotate + 1) % 4
self.mouse['start'] = None
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN and self.rotate == 0:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
class DroneControler(object):
def __init__(self, ac_id, plateform_id, tag_id):
# Start a ivy messages listener named PIR DRONE
self._interface = IvyMessagesInterface("PIR", ivy_bus="192.168.1:2010")
self.drone = FC_Rotorcraft(ac_id, tag_id, self._interface)
self.plateform = Platform(plateform_id)
# bind to GROUND_REF message
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id == self.drone.id:
# X and V in NED
self.drone.X_opti[0] = float(msg['pos'][1])
self.drone.X_opti[1] = float(msg['pos'][0])
self.drone.X_opti[2] = -float(msg['pos'][2])
self.drone.V_opti[0] = float(msg['speed'][1])
self.drone.V_opti[1] = float(msg['speed'][0])
self.drone.V_opti[2] = -float(msg['speed'][2])
self.drone.quaternions[0] = float(msg['quat'][0])
self.drone.quaternions[1] = float(msg['quat'][1])
self.drone.quaternions[2] = float(msg['quat'][2])
self.drone.quaternions[3] = float(msg['quat'][3])
self.drone.timeLastUpdate = time.time()
self.drone.initialized = True
if ac_id == self.plateform.id:
# X and V in NED
self.plateform.X[0] = float(msg['pos'][1])
self.plateform.X[1] = float(msg['pos'][0])
self.plateform.X[2] = -float(msg['pos'][2])
self.plateform.V[0] = float(msg['speed'][1])
self.plateform.V[1] = float(msg['speed'][0])
self.plateform.V[2] = -float(msg['speed'][2])
self.plateform.quaternions[0] = float(msg['quat'][0])
self.plateform.quaternions[1] = float(msg['quat'][1])
self.plateform.quaternions[2] = float(msg['quat'][2])
self.plateform.quaternions[3] = float(msg['quat'][3])
self.plateform.timeLastUpdate = time.time()
self.plateform.initialized = True
self._interface.subscribe(ground_ref_cb,
PprzMessage("ground", "GROUND_REF"))
# Recuperation du x et y via l'UWB
def get_xy_uwb(uwb_id, msg):
# X and V in NED
#msg est un tableau contenant : d1corr, d2corr, d3corr, d1vrai, d2vrai, d3vrai, x, y, z, vx, vy, vz
x = float(msg['values'][6])
y = float(msg['values'][7])
z = self.drone.X_opti[3]
psi = cr.getEulerAngles(
self.drone.quaternions)[2] - cr.getEulerAngles(
self.plateform.quaternions)[2]
#self.drone.V_uwb[0] = float(msg['values'][9])
#self.drone.V_uwb[1] = float(msg['values'][10])
self.drone.X_uwb = cr.uwb2pseudoBody(x, y, z)
self._interface.subscribe(get_xy_uwb,
PprzMessage("telemetry", "PAYLOAD_FLOAT"))
def stop(self):
# Stop IVY interface
print("Stopping Ivy interface")
if self._interface is not None:
self._interface.shutdown()
def envoi_cmd(self, phi, theta, psi, throttle):
msg = PprzMessage("datalink", "JOYSTICK_RAW")
msg['ac_id'] = self.drone.id
msg['roll'] = phi
msg['pitch'] = theta
msg['yaw'] = psi
msg['throttle'] = throttle
self._interface.send_raw_datalink(msg)
def envoi_cmd_guidedSetPointNED(self):
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.drone.id
msg['flags'] = 0b100011 # x and y (position) as offset coordinate in pseudoBody frame
phi, theta, psi = cr.getEulerAngles(self.drone.quaternions)
Xtag_pseudoBody = cr.cam2pseudoBody(self.drone.cam.tag.X, phi, theta)
msg['x'] = Xtag_pseudoBody[0] / 2.3
msg['y'] = Xtag_pseudoBody[1] / 2.3
msg['z'] = -1.3 # COmmande en NED so the z axis is oriented facing down
msg['yaw'] = 0
print(msg)
self._interface.send_raw_datalink(msg)
def gen_commande(self, x, y, z, psi):
"""Entree : coordonnes x,y,z du drone dans le repere pseudo drone
Sortie : Commande phi, theta, psi,V_z"""
pidx = pid.PID(Px, Ix, Dx)
pidy = pid.PID(Py, Iy, Dy)
phi = pidx.update(x)
theta = pidy.update(y)
psi = getEulerAngles(self.drone.quaternions)[2] - getEulerAngles(
self.plateform.quaternions)[2]
V_z = pidz.update(z)
return phi, theta, psi, V_z
def update(self):
[self.drone.U[0], self.drone.U[1], self.drone.U[2],
self.drone.U[3]] = self.gen_commande(self.drone.X[0], self.drone.X[1],
self.drone.X[2])
self.envoi_cmd(phi, theta, psi, throttle)
def run(self):
while (True):
self.update()
class Guidance(object):
def __init__(self, verbose=False, interface=None, quad_ids=None):
self.verbose = verbose
self._interface = interface
self.auto2_index = None
self.ac_id = quad_ids[0]
self.ids = quad_ids
self.ap_mode = None
self.rotorcrafts = [Rotorcraft(i) for i in quad_ids]
try:
settings = PaparazziACSettings(
self.ac_id) # target and follower should be checked, FIX ME !
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface(
"Deep Guidance is on the way...")
# bind to INS message
def ins_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "INS":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
rc.X[0] = float(msg['ins_x']) * i2p
rc.X[1] = float(msg['ins_y']) * i2p
rc.X[2] = float(msg['ins_z']) * i2p
rc.V[0] = float(msg['ins_xd']) * i2v
rc.V[1] = float(msg['ins_yd']) * i2v
rc.V[2] = float(msg['ins_zd']) * i2v
rc.timeout = 0
rc.initialized = True
# self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
#################################################################
def rotorcraft_fp_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "ROTORCRAFT_FP":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
i2w = 1. / 2**12 # integer to angle
rc.X[0] = float(msg['north']) * i2p
rc.X[1] = float(msg['east']) * i2p
rc.X[2] = float(msg['up']) * i2p
rc.V[0] = float(msg['vnorth']) * i2v
rc.V[1] = float(msg['veast']) * i2v
rc.V[2] = float(msg['vup']) * i2v
rc.W[2] = float(msg['psi']) * i2w
rc.timeout = 0
rc.initialized = True
# Un-comment this if the quadrotors are providing state information to use_deep_guidance.py
self._interface.subscribe(rotorcraft_fp_cb,
PprzMessage("telemetry", "ROTORCRAFT_FP"))
# bind to GROUND_REF message : ENAC Voliere is sending LTP_ENU
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id in self.ids:
rc = self.rotorcrafts[self.ids.index(ac_id)]
# X and V in NED
rc.X[0] = float(msg['pos'][1])
rc.X[1] = float(msg['pos'][0])
rc.X[2] = float(msg['pos'][2])
rc.V[0] = float(msg['speed'][1])
rc.V[1] = float(msg['speed'][0])
rc.V[2] = float(msg['speed'][2])
rc.timeout = 0
rc.initialized = True
# Un-comment this if optitrack is being used for state information for use_deep_guidance.py **For use only in the Voliere**
#self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF"))
################################################################
# <message name="ROTORCRAFT_FP" id="147">
# <field name="east" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="north" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="up" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="veast" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="vnorth" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="vup" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="carrot_east" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="carrot_north" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="carrot_up" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="carrot_psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
# <field name="thrust" type="int32"/>
# <field name="flight_time" type="uint16" unit="s"/>
# </message>
# <message name="INS" id="198">
# <field name="ins_x" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="ins_y" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="ins_z" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
# <field name="ins_xd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="ins_yd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="ins_zd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019"/>
# <field name="ins_xdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
# <field name="ins_ydd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
# <field name="ins_zdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
# </message>
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def set_guided_mode(self, quad_id=None):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = quad_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName(
'Guided') # AP_MODE_GUIDED
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'GUIDED') # AP_MODE_GUIDED
except ValueError:
msg['value'] = 19 # fallback to fixed index
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self, quad_id=None):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = quad_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'NAV') # AP_MODE_NAV
except ValueError:
msg['value'] = 13 # fallback to fixed index
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0, quad_id=None):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = quad_id
msg['flags'] = 0x00
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x0D
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x0E
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_ned_vel(self,
north=0.0,
east=0.0,
down=0.0,
yaw=0.0,
quad_id=None):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = quad_id
msg['flags'] = 0xE0
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
#print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x62
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def accelerate(self, north=0.0, east=0.0, down=0.0, quad_id=2):
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = quad_id
msg['flag'] = 0 # full 3D
msg['ux'] = north
msg['uy'] = east
msg['uz'] = down
self._interface.send(msg)
# <message name="GUIDED_SETPOINT_NED" id="40" link="forwarded">
# <description>
# Set vehicle position or velocity in NED.
# Frame can be specified with the bits 0-3
# Velocity of position setpoint can be specified with the bits 5-7
# Flags field definition:
# - bit 0: x,y as offset coordinates
# - bit 1: x,y in body coordinates
# - bit 2: z as offset coordinates
# - bit 3: yaw as offset coordinates
# - bit 4: free
# - bit 5: x,y as vel
# - bit 6: z as vel
# - bit 7: yaw as rate
# </description>
# <field name="ac_id" type="uint8"/>
# <field name="flags" type="uint8">bits 0-3: frame, bits 5-7: use as velocity</field>
# <field name="x" type="float" unit="m">X position/velocity in NED</field>
# <field name="y" type="float" unit="m">Y position/velocity in NED</field>
# <field name="z" type="float" unit="m">Z position/velocity in NED (negative altitude)</field>
# <field name="yaw" type="float" unit="rad" alt_unit="deg">yaw/rate setpoint</field>
# </message>
# class IvyRequester(object):
# def __init__(self, interface=None):
# self._interface = interface
# if interface is None:
# self._interface = IvyMessagesInterface("ivy requester")
# self.ac_list = []
# def __del__(self):
# self.shutdown()
# def shutdown(self):
# if self._interface is not None:
# print("Shutting down ivy interface...")
# self._interface.shutdown()
# self._interface = None
# def get_aircrafts(self):
# def aircrafts_cb(ac_id, msg):
# self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
# print("aircrafts: {}".format(self.ac_list))
# self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)")
# sender = 'get_aircrafts'
# request_id = '42_1' # fake request id, should be PID_index
# self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id))
# # hack: sleep briefly to wait for answer
# sleep(0.1)
# return self.ac_list
class PaparazziUAV(object):
BIND_REGEX = "((^[0-9]* NPS_RATE_ATTITUDE .*)|" \
"(^[0-9]* NPS_SPEED_POS .*)|" \
"(^[0-9]* NPS_WIND .*)|" \
"(^[0-9]* BAT .*))"
def __init__(self, ac_id, ground_alt=0):
"""Init PaparazziUAV
Arguments:
ac_id: aircraft id.
ground_alt: ground altitude above sea level.
"""
self.ac_id = ac_id
self.ground_alt = ground_alt
self.east = .0
self.north = .0
self.up = .0
self.phi = .0 # in rad
self.psi = .0 # in rad
self.theta = .0 # in rad
self.voltage = 0. # in V
self.current = 0. # in A
self.power = 0. # in W
self.energy = 0. # in mAh
self.measured_wind_x = 0.
self.measured_wind_y = 0.
self.measured_wind_z = 0.
self.path_queue = Queue.Queue() # format: (time, (x, y, z))
self._last_log = 0
self._log_interval = 1
# initiate ivy message interface to catch all messages
self.ivy_mes = IvyMessagesInterface()
self.ivy_mes.subscribe(self.ivy_callback, PaparazziUAV.BIND_REGEX)
rospy.init_node('paparazziuav')
self.ros_pub_state = rospy.Publisher('uav_state',
UAVState,
queue_size=1)
self.ros_pub_measured_wind = rospy.Publisher('measured_wind',
WindSample,
queue_size=1)
self.ros_pub_energy_consumption = rospy.Publisher('energy_consumption',
EnergyConsumption,
queue_size=1)
rospy.Subscriber("/tick", Clock, self.on_tick_recv, queue_size=10)
rospy.Subscriber('expected_uav_state_sequence', UAVStateSequence,
self.on_exp_uav_state_seq_recv)
# TODO: Handle connection and disconnection of simulator.
# With this architecture publish_state will send values even when the
# simulator was disconnected.
def on_tick_recv(self, tick):
"""On new time slot beginning.
Arguments:
tick: time-slot time.
"""
self.tick_time = tick.clock.to_sec()
self.execute_path(method="SEGMENT")
self.publish_state()
self.publish_energy_consumption()
self.publish_wind_sample()
def ivy_callback(self, ac_id, msg):
"""Handle state callback from ivy bus.
Arguments:
ac_id: aircraft id.
msg: PprzMessage.
"""
if ac_id != self.ac_id:
return
if msg.msg_class == "telemetry":
if msg.name == "NPS_RATE_ATTITUDE":
self.phi = float(msg.phi)
self.psi = float(msg.psi)
self.theta = float(msg.theta)
elif msg.name == "NPS_SPEED_POS":
# NPS_SPEED_POS is sent in NED coordinates and we use ENU
self.east = float(msg.ltpp_y)
self.north = float(msg.ltpp_x)
self.up = -float(msg.ltpp_z)
elif msg.name == "NPS_WIND":
# TODO Check if wind needs also to be converted to ENU
self.measured_wind_x = float(msg.vy)
self.measured_wind_y = float(msg.vx)
self.measured_wind_z = -float(msg.vz)
elif msg.name == "BAT":
self.voltage = int(msg.voltage) / 10.
self.current = int(msg.amps) / 100.
self.power = self.voltage * self.current
self.energy = float(msg.energy)
def publish_state(self):
"""Publish state into ROS topic."""
# Increment time in 1 to be consistent with simuav behavior which sends
# the state at the *end* of the cycle, so tick_time + 1
h = Header(stamp=rospy.Time(self.tick_time + 1))
position = Point(self.east, self.north, self.up)
attitude = Attitude(self.phi, self.psi, self.theta)
battery_level = 100000 # np.NaN
uav_state = UAVState(h, self.ac_id, position, attitude, battery_level)
rospy.loginfo("[pprzuav] [ac_id={}]\n{}".format(self.ac_id, uav_state))
self.ros_pub_state.publish(uav_state)
def publish_energy_consumption(self):
"""Publish power information into ROS topic."""
# Increment time in 1 to be consistent with simuav behavior which sends
# the state at the *end* of the cycle, so tick_time + 1
h = Header(stamp=rospy.Time(self.tick_time + 1))
energy_con = EnergyConsumption(h, self.ac_id, self.voltage,
self.current, self.power, self.energy)
rospy.logdebug("[pprzuav] [ac_id={}] state:\n{}".format(
self.ac_id, energy_con))
self.ros_pub_energy_consumption.publish(energy_con)
def publish_wind_sample(self):
"""Publish wind sample into ROS topic."""
h = Header(stamp=rospy.Time(self.tick_time))
position = Point(float(self.east), float(self.north), float(self.up))
wind = Wind(float(self.measured_wind_x), float(self.measured_wind_y),
float(self.measured_wind_z))
wind_sample = WindSample(h, self.ac_id, position, wind)
rospy.loginfo("[pprzuav] [ac_id={}] wind:\n{}".format(
self.ac_id, wind_sample))
self.ros_pub_measured_wind.publish(wind_sample)
def execute_path(self, method="GOTO_WP"):
"""Execute a path in paparazzi using different methods.
Arguments:
method (str): type of message to use. Default "GOTO_WP".
"GOTO_WP": MISSION_GOTO_WP message
"SEGMENT": MISSION_SEGMENT message
"""
if method == "GOTO_WP":
self._execute_path_goto_wp()
elif method == "SEGMENT":
self._execute_path_segment()
def _execute_path_segment(self):
"""Execute path as segments."""
def get_until(queue, time):
"""Generator returning all elements in queue until time.
Arguments:
time: time.
"""
try:
e = queue.get(False)
while np.around(e[0] - time, decimals=DECIMAL_DIGITS) <= 0:
yield e
e = queue.get(False)
else:
with queue.mutex:
queue.queue.appendleft(e)
except Queue.Empty:
return
# straight line by default
def_s = (self.tick_time, (self.east, self.north, self.up))
def_d = (self.east + 15 * np.cos(self.psi),
self.north + 15 * np.sin(self.psi))
mode = 0 # first command is appended
# mode = 3 # first command replaces all others
s = None
for p in get_until(self.path_queue, np.inf):
if not s:
s = p
else:
self.mission_segment((s[1][0], s[1][1]), (p[1][0], p[1][1]),
self.ground_alt + p[1][2],
insert_mode=mode)
msg = "".join([
"[ac_id={}] Sending segment {} -> {} ",
" with alt. {} expected for t={}"
])
msg = msg.format(self.ac_id, s[1][:2], p[1][:2], p[1][2],
(s[0], p[0]))
rospy.loginfo(msg)
s = p
mode = 0 # the following are appended
time.sleep(0.1)
def _execute_path_goto_wp(self):
"""Execute path as waypoints."""
# default destination
dest = (0, (0, 0, self.ground_alt + 200)) # (time, (east, north, up))
try:
p = self.path_queue.get(False)
while np.around(p[0], decimals=DECIMAL_DIGITS) < self.tick_time:
p = self.path_queue.get(False)
if np.isclose(p[0], self.tick_time, atol=ABS_TOL):
rospy.loginfo('[ac_id={}] Got wp {} for time t={}'.format(
self.ac_id, p[1], p[0]))
dest = p
try:
way_p = dest[1]
self.mission_goto_wp(way_p[0],
way_p[1],
self.ground_alt + way_p[2],
insert_mode=0)
while True:
dest = self.path_queue.get(False)
rospy.loginfo("dest {} ".format(str(dest)) +
"t={}".format(self.tick_time))
way_p = dest[1]
self.mission_goto_wp(way_p[0],
way_p[1],
self.ground_alt + way_p[2],
insert_mode=0)
except Queue.Empty:
rospy.loginfo("All sent")
else:
# Put again the element on the queue and warn
with self.path_queue.mutex:
self.path_queue.queue.appendleft(p)
rospy.logwarn("[ac_id={}] No position ".format(self.ac_id) +
"defined for t={}. ".format(self.tick_time) +
"Fallback to default")
# keep default destination
except Queue.Empty:
rospy.logwarn("[ac_id={}] ".format(self.ac_id) +
"Empty path queue at t={}. ".format(self.tick_time) +
"Fallback to default")
# keep default destination
#
# way_p = dest[1]
# self.ac_goto_wp(way_p[0], way_p[1], way_p[2], 5)
def mission_goto_wp(self, east, north, up, duration=0, insert_mode=0):
"""Send a MISSION_GOTO_WP command to paparazzi.
Arguments:
east: position east
north: position north
up: position up (in meters above sea level)
duration: duration of mission before being discarded. Default is 0,
meaning no specified duration.
insert_mode: default APPEND
0: APPEND
1: PREPEND
2: REPLACE_CURRENT
3: REPLACE_ALL
"""
msg = PprzMessage("datalink", "MISSION_GOTO_WP")
msg['ac_id'] = self.ac_id
# APPEND, PREPEND, REPLACE_CURRENT, REPLACE_ALL
msg['insert'] = insert_mode
# ENU to NED coordinates
msg['wp_east'] = east
msg['wp_north'] = north
msg['wp_alt'] = up
msg['duration'] = duration
self.ivy_mes.send_raw_datalink(msg)
def mission_segment(self,
s_pos,
d_pos,
altitude,
duration=0,
insert_mode=0):
"""Send a MISSION_SEGMENT command to paparazzi.
Arguments:
s_pos: source position as tuple (east, north)
d_pos: destination position as tuple (east, north)
altitude: altitude of the segment (in meters above sea level)
duration: duration of mission (in seconds) before being discarded.
Default is 0, meaning no specified duration.
insert_mode: default APPEND
0: APPEND
1: PREPEND
2: REPLACE_CURRENT
3: REPLACE_ALL
"""
msg = PprzMessage("datalink", "MISSION_SEGMENT")
msg['ac_id'] = self.ac_id
# APPEND, PREPEND, REPLACE_CURRENT, REPLACE_ALL
msg['insert'] = insert_mode
msg['segment_east_1'] = s_pos[0]
msg['segment_north_1'] = s_pos[1]
msg['segment_east_2'] = d_pos[0]
msg['segment_north_2'] = d_pos[1]
msg['segment_alt'] = altitude
msg['duration'] = duration
self.ivy_mes.send_raw_datalink(msg)
def on_exp_uav_state_seq_recv(self, exp_state_seq):
"""Handle aircraft input reception."""
rospy.loginfo("[ac_id={}] Received UAV state sequence:\n{}".format(
exp_state_seq.ac_id, exp_state_seq))
# Fill queue
for i in exp_state_seq.states:
self.path_queue.put((i.header.stamp.to_sec(),
(i.position.x, i.position.y, i.position.z)))
