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)))