mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-18 08:03:44 +08:00
[tools] Visualizer Rotating Wing (#3070)
* Visualizer Rotating Wing Rotwing Monitor * executable + tool * removed STL * Apply suggestions from code review
This commit is contained in:
Christophe De Wagter
committed by
GitHub
GitHub
parent
fd3479c2d7
commit
3da0992fd5
+36
@@ -0,0 +1,36 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# Copyright (C) 2023 TUDelft
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi. If not, see <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
import wx
|
||||
import rot_wing_viewer
|
||||
|
||||
class RotWingApp(wx.App):
|
||||
def OnInit(self):
|
||||
self.main = rot_wing_viewer.RotWingFrame()
|
||||
self.main.Show()
|
||||
self.SetTopWindow(self.main)
|
||||
return True
|
||||
|
||||
def main():
|
||||
application = RotWingApp(0)
|
||||
application.MainLoop()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,284 @@
|
||||
#
|
||||
# Copyright (C) 2023 TUDelft
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi. If not, see <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
import wx
|
||||
|
||||
import sys
|
||||
import os
|
||||
import math
|
||||
import datetime
|
||||
import numpy as np
|
||||
|
||||
|
||||
PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
|
||||
'../../../..')))
|
||||
|
||||
PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
|
||||
'../../../..')))
|
||||
|
||||
sys.path.append(PPRZ_HOME + "/var/lib/python")
|
||||
|
||||
from pprzlink.ivy import IvyMessagesInterface
|
||||
|
||||
WIDTH = 600
|
||||
BARH = 140
|
||||
|
||||
class EscMessage(object):
|
||||
def __init__(self, msg):
|
||||
self.id = int(msg['motor_id'])
|
||||
self.amp = float(msg['amps'])
|
||||
self.rpm = float(msg['rpm'])
|
||||
self.volt_b = float(msg['bat_volts'])
|
||||
self.volt_m = float(msg['motor_volts'])
|
||||
self.temperature = float(msg['temperature']) - 273.15
|
||||
self.temperature_dev = float(msg['temperature_dev']) - 273.15
|
||||
self.energy = float(msg['energy'])
|
||||
|
||||
def get_current(self):
|
||||
return str(round(self.amp ,1)) + "A"
|
||||
def get_current_perc(self):
|
||||
return self.amp / 30
|
||||
|
||||
def get_rpm(self):
|
||||
return str(round(self.rpm ,0)) + " rpm"
|
||||
def get_rpm_perc(self):
|
||||
return self.rpm / 5150
|
||||
def get_rpm_color(self):
|
||||
if self.rpm < 4000:
|
||||
return 1
|
||||
return 0.5
|
||||
|
||||
|
||||
def get_volt(self):
|
||||
if (self.id in [6,7,8,9,16,17,18,19]):
|
||||
return "Servo " + str(self.id) + " " +str(round(self.volt_m ,1)) + "V"
|
||||
else:
|
||||
return "Mot " + str(self.id) + " " +str(round(self.volt_m ,1)) + "V"
|
||||
def get_volt_perc(self):
|
||||
return self.volt_b / (6*4.2)
|
||||
|
||||
def get_temp(self):
|
||||
if self.temperature < -200:
|
||||
return "xxx"
|
||||
return str(round(self.temperature ,1)) + "C"
|
||||
def get_temp_perc(self):
|
||||
return self.temperature / 120.0
|
||||
|
||||
def get_temp_dev(self):
|
||||
if self.temperature_dev < -200:
|
||||
return "xxx"
|
||||
return str(round(self.temperature_dev, 1)) + "C"
|
||||
def get_temp_dev_perc(self):
|
||||
return self.temperature_dev / 120.0
|
||||
|
||||
def get_temp_color(self):
|
||||
if self.temperature < 0:
|
||||
return 0
|
||||
elif self.temperature < 60:
|
||||
return 1
|
||||
else:
|
||||
return 0.5
|
||||
|
||||
def get_temp_dev_color(self):
|
||||
if self.temperature_dev < 0:
|
||||
return 0
|
||||
elif self.temperature_dev < 100:
|
||||
return 1
|
||||
else:
|
||||
return 0.5
|
||||
|
||||
class INDIMessage(object):
|
||||
def __init__(self, msg):
|
||||
self.u = np.array(msg['u'], dtype=np.float)
|
||||
|
||||
def get_u(self, id):
|
||||
return str(round(self.u[id], 0)) + " PPRZ"
|
||||
|
||||
def get_u_perc(self, id):
|
||||
return self.u[id] / 9600.
|
||||
|
||||
def get_u_color(self, id):
|
||||
if self.u[id] < 9600 / 0.8:
|
||||
return 1
|
||||
else:
|
||||
return 0.5
|
||||
|
||||
class MotorList(object):
|
||||
def __init__(self):
|
||||
self.mot = []
|
||||
|
||||
def fill_from_esc_msg(self, esc):
|
||||
added = False
|
||||
for i in range(len(self.mot)):
|
||||
if self.mot[i].id == esc.id:
|
||||
self.mot[i] = esc
|
||||
added = True
|
||||
break
|
||||
if not added:
|
||||
self.mot.append(esc)
|
||||
|
||||
class RotWingFrame(wx.Frame):
|
||||
def message_recv(self, ac_id, msg):
|
||||
if msg.name == "ESC":
|
||||
self.esc = EscMessage(msg)
|
||||
self.motors.fill_from_esc_msg(self.esc)
|
||||
wx.CallAfter(self.update)
|
||||
|
||||
if msg.name == "STAB_ATTITUDE_FULL_INDI":
|
||||
self.indi = INDIMessage(msg)
|
||||
wx.CallAfter(self.update)
|
||||
|
||||
def update(self):
|
||||
self.Refresh()
|
||||
|
||||
def OnSize(self, event):
|
||||
self.w = event.GetSize().x
|
||||
self.h = event.GetSize().y
|
||||
self.cfg.Write("width", str(self.w))
|
||||
self.cfg.Write("height", str(self.h))
|
||||
self.Refresh()
|
||||
|
||||
def OnMove(self, event):
|
||||
self.x = event.GetPosition().x
|
||||
self.y = event.GetPosition().y
|
||||
self.cfg.Write("left", str(self.x))
|
||||
self.cfg.Write("top", str(self.y))
|
||||
|
||||
def StatusBox(self, dc, dx, dy, row, col, txt, percent, color):
|
||||
if percent < 0:
|
||||
percent = 0
|
||||
if percent > 1:
|
||||
percent = 1
|
||||
boxw = self.stat
|
||||
tdx = int(boxw * 10.0 / 300.0)
|
||||
tdy = int(boxw * 6.0 / 300.0)
|
||||
boxh = int(boxw * 40.0 / 300.0)
|
||||
boxw = self.stat - 2*tdx
|
||||
spacing = boxh+10
|
||||
|
||||
dc.SetPen(wx.Pen(wx.Colour(0,0,0)))
|
||||
dc.SetBrush(wx.Brush(wx.Colour(220,220,220)))
|
||||
dc.DrawRectangle(tdx + col * 200 + dx, int(row*spacing+tdx) +dy, int(boxw), boxh)
|
||||
dc.SetTextForeground(wx.Colour(0, 0, 0))
|
||||
if color < 0.2:
|
||||
dc.SetTextForeground(wx.Colour(255, 255, 255))
|
||||
dc.SetBrush(wx.Brush(wx.Colour(250,0,0)))
|
||||
elif color < 0.6:
|
||||
dc.SetBrush(wx.Brush(wx.Colour(250,180,0)))
|
||||
else:
|
||||
dc.SetBrush(wx.Brush(wx.Colour(0,250,0)))
|
||||
# dc.DrawLine(200,50,350,50)
|
||||
dc.DrawRectangle(tdx + col * 200 + dx, int(row*spacing+tdx+dy), int(boxw * percent), boxh)
|
||||
dc.DrawText(txt,18 + col * 200 + dx,int(row*spacing+tdy+tdx+dy))
|
||||
|
||||
def OnPaint(self, e):
|
||||
# Automatic Scaling
|
||||
w = self.w
|
||||
h = self.h - 25
|
||||
|
||||
dc = wx.PaintDC(self)
|
||||
brush = wx.Brush("white")
|
||||
dc.SetBackground(brush)
|
||||
dc.Clear()
|
||||
|
||||
# Background
|
||||
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
|
||||
|
||||
fontscale = int(w * 11.0 / 1500.0)
|
||||
if fontscale < 6:
|
||||
fontscale = 6
|
||||
font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
|
||||
dc.SetFont(font)
|
||||
|
||||
# Draw Drone
|
||||
dc.SetPen(wx.Pen(wx.Colour(0,0,0)))
|
||||
dc.SetBrush(wx.Brush(wx.Colour(240,240,220)))
|
||||
# Fuselage
|
||||
dc.DrawRoundedRectangle(int(0.425*w), int(0.05*h),int(0.15*w), int(0.4*h), int(0.05*w))
|
||||
# Front Wing
|
||||
#dc.DrawRectangle(int(0.05*w), int(0.25*h),int(0.9*w), int(0.15*h))
|
||||
# Back Wing
|
||||
#dc.DrawRectangle(int(0.01*w), int(0.65*h),int(0.98*w), int(0.15*h))
|
||||
|
||||
# Motors
|
||||
self.stat = int(0.10*w)
|
||||
dc.SetBrush(wx.Brush(wx.Colour(200,200,100)))
|
||||
w1 = 0.03
|
||||
w2 = 0.2
|
||||
w3 = 0.37
|
||||
w4 = 0.54
|
||||
dw = 0.11
|
||||
mw = 0.1
|
||||
mh = 0.15
|
||||
mm = [(0.5-0.5*mw,w1), (0.5+dw,w2), (0.5-0.5*mw,w3), (0.5-dw-mw,w2), (0.5-0.5*mw,w4)]
|
||||
for m in mm:
|
||||
dc.DrawRectangle(int(m[0]*w), int(m[1]*h),int(mw*w), int(mh*h))
|
||||
|
||||
for m in self.motors.mot:
|
||||
if m.id>4:
|
||||
continue
|
||||
mo_co = mm[m.id]
|
||||
#print(m.id, mo_co)
|
||||
dx = int(mo_co[0]*w)
|
||||
dy = int(mo_co[1]*h)
|
||||
self.StatusBox(dc, dx, dy, 0, 0, m.get_volt(), m.get_volt_perc(), 1)
|
||||
self.StatusBox(dc, dx, dy, 1, 0, m.get_current(), m.get_current_perc(), 1)
|
||||
self.StatusBox(dc, dx, dy, 2, 0, m.get_rpm(), m.get_rpm_perc(), m.get_rpm_color())
|
||||
self.StatusBox(dc, dx, dy, 3, 0, m.get_temp(), m.get_temp_perc(), m.get_temp_color())
|
||||
self.StatusBox(dc, dx, dy, 4, 0, m.get_temp_dev(), m.get_temp_dev_perc(), m.get_temp_dev_color())
|
||||
try:
|
||||
self.StatusBox(dc, dx, dy, 5, 0, self.indi.get_u(m.id), self.indi.get_u_perc(m.id), self.indi.get_u_color(m.id))
|
||||
except:
|
||||
pass
|
||||
|
||||
def __init__(self):
|
||||
|
||||
self.w = WIDTH
|
||||
self.h = WIDTH + BARH
|
||||
|
||||
self.cfg = wx.Config('rot_wing_conf')
|
||||
if self.cfg.Exists('width'):
|
||||
self.w = int(self.cfg.Read('width'))
|
||||
self.h = int(self.cfg.Read('height'))
|
||||
|
||||
wx.Frame.__init__(self, id=-1, parent=None, name=u'RotWingFrame',
|
||||
size=wx.Size(self.w, self.h), title=u'Rot Wing Monitoring')
|
||||
|
||||
if self.cfg.Exists('left'):
|
||||
self.x = int(self.cfg.Read('left'))
|
||||
self.y = int(self.cfg.Read('top'))
|
||||
self.SetPosition(wx.Point(self.x,self.y))
|
||||
|
||||
self.Bind(wx.EVT_PAINT, self.OnPaint)
|
||||
self.Bind(wx.EVT_SIZE, self.OnSize)
|
||||
self.Bind(wx.EVT_MOVE, self.OnMove)
|
||||
self.Bind(wx.EVT_CLOSE, self.OnClose)
|
||||
|
||||
#ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO)
|
||||
#self.SetIcon(ico)
|
||||
|
||||
self.motors = MotorList()
|
||||
|
||||
self.interface = IvyMessagesInterface("rotwingframe")
|
||||
self.interface.subscribe(self.message_recv)
|
||||
|
||||
def OnClose(self, event):
|
||||
self.interface.shutdown()
|
||||
self.Destroy()
|
||||
|
||||
@@ -0,0 +1,495 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# Copyright (C) 2023 TUDelft
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi. If not, see <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
import sys
|
||||
|
||||
import os
|
||||
import open3d as o3d
|
||||
import numpy as np
|
||||
import time
|
||||
import platform
|
||||
import random
|
||||
import threading
|
||||
import copy
|
||||
|
||||
isMacOS = (platform.system() == "Darwin")
|
||||
|
||||
PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
|
||||
'../../../..')))
|
||||
|
||||
PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
|
||||
'../../../..')))
|
||||
|
||||
sys.path.append(PPRZ_HOME + "/var/lib/python")
|
||||
|
||||
from pprzlink.ivy import IvyMessagesInterface
|
||||
|
||||
|
||||
class AHRSRefQuatMessage(object):
|
||||
def __init__(self, msg):
|
||||
self.body_qi = msg['body_qi']
|
||||
self.body_qx = msg['body_qx']
|
||||
self.body_qy = msg['body_qy']
|
||||
self.body_qz = msg['body_qz']
|
||||
|
||||
self.ref_qi = msg['ref_qi']
|
||||
self.ref_qx = msg['ref_qx']
|
||||
self.ref_qy = msg['ref_qy']
|
||||
self.ref_qz = msg['ref_qz']
|
||||
|
||||
def get_body_quaternion(self):
|
||||
body_qi = float(self.body_qi) / 2**15
|
||||
body_qx = float(self.body_qx) / 2**15
|
||||
body_qy = float(self.body_qy) / 2**15
|
||||
body_qz = float(self.body_qz) / 2**15
|
||||
return np.array([[body_qi], [body_qx], [body_qy], [body_qz]])
|
||||
|
||||
def get_ref_quaternion(self):
|
||||
ref_qi = float(self.ref_qi) / 2**15
|
||||
ref_qx = float(self.ref_qx) / 2**15
|
||||
ref_qy = float(self.ref_qy) / 2**15
|
||||
ref_qz = float(self.ref_qz) / 2**15
|
||||
return np.array([[ref_qi], [ref_qx], [ref_qy], [ref_qz]])
|
||||
|
||||
class RotWingControllerMessage(object):
|
||||
def __init__(self, msg):
|
||||
self.wing_angle_deg = msg['wing_angle_deg']
|
||||
self.wing_angle_deg_sp = msg['wing_angle_deg_sp']
|
||||
|
||||
def get_wing_angle(self):
|
||||
return np.deg2rad(float(self.wing_angle_deg))
|
||||
|
||||
def get_wing_angle_sp(self):
|
||||
return np.deg2rad(float(self.wing_angle_deg_sp))
|
||||
|
||||
class ActuatorsMessage(object):
|
||||
def __init__(self, msg):
|
||||
self.values = msg['values']
|
||||
|
||||
def get_actuator_values(self):
|
||||
return np.array(self.values).astype(dtype = float)
|
||||
|
||||
def get_actuator_value(self, idx):
|
||||
return float(self.values[idx])
|
||||
|
||||
class EscMessage(object):
|
||||
def __init__(self, msg):
|
||||
self.id = int(msg['motor_id'])
|
||||
self.amp = float(msg['amps'])
|
||||
self.rpm = float(msg['rpm'])
|
||||
self.volt_b = float(msg['bat_volts'])
|
||||
self.volt_m = float(msg['motor_volts'])
|
||||
self.temperature = float(msg['temperature']) - 273.15
|
||||
self.energy = float(msg['energy'])
|
||||
|
||||
def get_id(self):
|
||||
return self.id
|
||||
|
||||
def get_temp(self):
|
||||
return self.temperature
|
||||
|
||||
class Viewer3D(object):
|
||||
MENU_SPHERE = 1
|
||||
MENU_RANDOM = 2
|
||||
MENU_QUIT = 3
|
||||
|
||||
def __init__(self, title):
|
||||
self._id = 0
|
||||
|
||||
app = o3d.visualization.gui.Application.instance
|
||||
app.initialize()
|
||||
|
||||
self.w = app.create_window(title)
|
||||
|
||||
self.main_vis = o3d.visualization.gui.SceneWidget()
|
||||
self.main_vis.scene = o3d.visualization.rendering.Open3DScene(self.w.renderer)
|
||||
self.main_vis.scene.set_background(np.array([[0],[0],[0],[0]], dtype=np.float32))
|
||||
self.main_vis.scene.scene.set_sun_light(
|
||||
[1, 1, 1], # direction
|
||||
[1, 1, 1], # color
|
||||
100000) # intensity
|
||||
self.main_vis.scene.scene.enable_sun_light(True)
|
||||
|
||||
#self.main_vis.scene.view.set_post_processing(False)
|
||||
|
||||
self.mat = o3d.visualization.rendering.MaterialRecord()
|
||||
self.mat.shader = "defaultLit"
|
||||
self.mat.point_size = 50 * self.w.scaling
|
||||
|
||||
self.mat_ref = o3d.visualization.rendering.MaterialRecord()
|
||||
self.mat_ref.shader = 'defaultLitSSR'
|
||||
self.mat_ref.transmission = 1.0
|
||||
self.mat_ref.point_size = 50 * self.w.scaling
|
||||
|
||||
self.add_meshes()
|
||||
|
||||
bbox = self.main_vis.scene.bounding_box
|
||||
#bbox = o3d.geometry.AxisAlignedBoundingBox([-100, -100, -100],
|
||||
# [100, 100, 100])
|
||||
self.main_vis.setup_camera(60.0, bbox, bbox.get_center())
|
||||
self.main_vis.look_at(bbox.get_center(), np.array([[-2500],[0],[-500]]), np.array([[0],[0],[-1]]))
|
||||
self.w.add_child(self.main_vis)
|
||||
|
||||
# # The menu is global (because the macOS menu is global), so only create
|
||||
# # it once, no matter how many windows are created
|
||||
# if o3d.visualization.gui.Application.instance.menubar is None:
|
||||
# if isMacOS:
|
||||
# app_menu = o3d.visualization.gui.Menu()
|
||||
# app_menu.add_item("Quit", Viewer3D.MENU_QUIT)
|
||||
# debug_menu = o3d.visualization.gui.Menu()
|
||||
# debug_menu.add_item("Add Sphere", Viewer3D.MENU_SPHERE)
|
||||
# debug_menu.add_item("Add Random Spheres", Viewer3D.MENU_RANDOM)
|
||||
# if not isMacOS:
|
||||
# debug_menu.add_separator()
|
||||
# debug_menu.add_item("Quit", Viewer3D.MENU_QUIT)
|
||||
|
||||
# menu = o3d.visualization.gui.Menu()
|
||||
# if isMacOS:
|
||||
# # macOS will name the first menu item for the running application
|
||||
# # (in our case, probably "Python"), regardless of what we call
|
||||
# # it. This is the application menu, and it is where the
|
||||
# # About..., Preferences..., and Quit menu items typically go.
|
||||
# menu.add_menu("Example", app_menu)
|
||||
# menu.add_menu("Debug", debug_menu)
|
||||
# else:
|
||||
# menu.add_menu("Debug", debug_menu)
|
||||
# o3d.visualization.gui.Application.instance.menubar = menu
|
||||
|
||||
# # The menubar is global, but we need to connect the menu items to the
|
||||
# # window, so that the window can call the appropriate function when the
|
||||
# # menu item is activated.
|
||||
# self.w.set_on_menu_item_activated(Viewer3D.MENU_SPHERE,
|
||||
# self._on_menu_sphere)
|
||||
# self.w.set_on_menu_item_activated(Viewer3D.MENU_RANDOM,
|
||||
# self._on_menu_random)
|
||||
# self.w.set_on_menu_item_activated(Viewer3D.MENU_QUIT,
|
||||
# self._on_menu_quit)
|
||||
|
||||
test_label = self.main_vis.add_3d_label([0,0,-400], 'TEST')
|
||||
test_label.color = o3d.visualization.gui.Color(1.0, 1.0, 1.0, 1.0)
|
||||
|
||||
self.interface = IvyMessagesInterface("rotwingviewer")
|
||||
self.interface.subscribe(self.message_recv)
|
||||
|
||||
self.ahrs_ref_quat = None
|
||||
self.wing_rotation_controller = None
|
||||
self.actuators = None
|
||||
|
||||
self.T_rotation = np.matrix(np.eye(4))
|
||||
self.T_rotation_ref = np.matrix(np.eye(4))
|
||||
self.T_rotation_wing = np.matrix(np.eye(4))
|
||||
self.T_rotation_wing_ref = np.matrix(np.eye(4))
|
||||
self.T_rotating_wing_final = np.matrix(np.eye(4))
|
||||
self.T_rotating_wing_final_ref = np.matrix(np.eye(4))
|
||||
|
||||
def add_meshes(self):
|
||||
R_standard = o3d.geometry.get_rotation_matrix_from_zxy(np.array([[0],[np.deg2rad(-90)],[np.deg2rad(-90)]]))
|
||||
T_standard = np.array([[680],[0],[0]])
|
||||
|
||||
self.mesh_wing = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/WingQuad.ply")
|
||||
self.mesh_wing.compute_vertex_normals()
|
||||
self.mesh_wing.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_wing.translate(T_standard)
|
||||
self.mesh_fuselage = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/RWV4 NonMoving.ply")
|
||||
self.mesh_fuselage.compute_vertex_normals()
|
||||
self.mesh_fuselage.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_fuselage.translate(T_standard)
|
||||
self.mesh_vert_tail = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/RudderSolid.ply")
|
||||
self.mesh_vert_tail.compute_vertex_normals()
|
||||
self.mesh_vert_tail.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_vert_tail.translate(T_standard)
|
||||
self.mesh_hor_tail = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/HoriTail.ply")
|
||||
self.mesh_hor_tail.compute_vertex_normals()
|
||||
self.mesh_hor_tail.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_hor_tail.translate(T_standard)
|
||||
self.mesh_motor_F = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/15 inch prop Disc.ply")
|
||||
self.mesh_motor_F.compute_vertex_normals()
|
||||
self.mesh_motor_F.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_motor_F.translate(np.array([[-232],[0],[124]]))
|
||||
self.mesh_motor_F.translate(T_standard)
|
||||
self.mesh_motor_B = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/15 inch prop Disc.ply")
|
||||
self.mesh_motor_B.compute_vertex_normals()
|
||||
self.mesh_motor_B.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_motor_B.translate(np.array([[-1129],[0],[124]]))
|
||||
self.mesh_motor_B.translate(T_standard)
|
||||
self.mesh_motor_R = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/15 inch prop Disc.ply")
|
||||
self.mesh_motor_R.compute_vertex_normals()
|
||||
self.mesh_motor_R.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_motor_R.translate(np.array([[-1050],[0],[-152]]))
|
||||
self.mesh_motor_R.translate(T_standard)
|
||||
self.mesh_motor_L = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/15 inch prop Disc.ply")
|
||||
self.mesh_motor_L.compute_vertex_normals()
|
||||
self.mesh_motor_L.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
self.mesh_motor_L.translate(np.array([[-310],[0],[-152]]))
|
||||
self.mesh_motor_L.translate(T_standard)
|
||||
# self.mesh_motor_pusher = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/15 inch prop Disc.ply")
|
||||
# self.mesh_motor_pusher.compute_vertex_normals()
|
||||
# self.mesh_motor_pusher.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
# self.mesh_motor_pusher.translate(np.array([[-1050],[0],[-152]]))
|
||||
# self.mesh_motor_pusher.translate(T_standard)
|
||||
# self.mesh_rudder = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/Rudder.ply")
|
||||
# self.mesh_rudder.compute_vertex_normals()
|
||||
# self.mesh_rudder.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
# self.mesh_rudder.translate(np.array([[-806.6],[0],[-152]]))
|
||||
# self.mesh_rudder.translate(T_standard)
|
||||
# self.mesh_l_flap = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/LeftFlap.ply")
|
||||
# self.mesh_l_flap.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
# self.mesh_l_flap.compute_vertex_normals()
|
||||
# self.mesh_l_aileron = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/LeftAileron.ply")
|
||||
# self.mesh_l_aileron.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
# self.mesh_l_aileron.compute_vertex_normals()
|
||||
# self.mesh_r_flap = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/RightFlap.ply")
|
||||
# self.mesh_r_flap.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
# self.mesh_r_flap.compute_vertex_normals()
|
||||
# self.mesh_r_aileron = o3d.io.read_triangle_mesh("sw/ext/tudelft_gazebo_models/src/ply/RotatingWingV3/RightAileron.ply")
|
||||
# self.mesh_r_aileron.rotate(R_standard, np.array([[0],[0],[0]]))
|
||||
# self.mesh_r_aileron.compute_vertex_normals()
|
||||
|
||||
mesh_coordinate = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=np.array([0.0, 0.0, 0.0]))
|
||||
|
||||
self.main_vis.scene.add_geometry("wing", self.mesh_wing, self.mat)
|
||||
self.main_vis.scene.add_geometry("fuselage", self.mesh_fuselage, self.mat)
|
||||
self.main_vis.scene.add_geometry("coordinate", mesh_coordinate, self.mat)
|
||||
self.main_vis.scene.add_geometry("vert_tail", self.mesh_vert_tail, self.mat)
|
||||
self.main_vis.scene.add_geometry("ḧor_tail", self.mesh_hor_tail, self.mat)
|
||||
self.main_vis.scene.add_geometry("motor_F", self.mesh_motor_F, self.mat)
|
||||
self.main_vis.scene.add_geometry("motor_B", self.mesh_motor_B, self.mat)
|
||||
self.main_vis.scene.add_geometry("motor_R", self.mesh_motor_R, self.mat)
|
||||
self.main_vis.scene.add_geometry("motor_L", self.mesh_motor_L, self.mat)
|
||||
# self.main_vis.scene.add_geometry("rudder", self.mesh_rudder, self.mat)
|
||||
# self.main_vis.scene.add_geometry("l_flap", self.mesh_l_flap, self.mat)
|
||||
# self.main_vis.scene.add_geometry("l_aileron", self.mesh_l_aileron, self.mat)
|
||||
# self.main_vis.scene.add_geometry("r_flap", self.mesh_r_flap, self.mat)
|
||||
# self.main_vis.scene.add_geometry("r_aileron", self.mesh_r_aileron, self.mat)
|
||||
|
||||
# Add reference
|
||||
self.main_vis.scene.add_geometry("fuselage_ref", self.mesh_fuselage, self.mat_ref)
|
||||
self.main_vis.scene.add_geometry("wing_ref", self.mesh_wing, self.mat_ref)
|
||||
|
||||
def add_sphere(self):
|
||||
self._id += 1
|
||||
mat = o3d.visualization.rendering.MaterialRecord()
|
||||
mat.base_color = [
|
||||
random.random(),
|
||||
random.random(),
|
||||
random.random(), 1.0
|
||||
]
|
||||
mat.shader = "defaultLit"
|
||||
sphere = o3d.geometry.TriangleMesh.create_sphere(0.5)
|
||||
sphere.compute_vertex_normals()
|
||||
sphere.translate([
|
||||
10.0 * random.uniform(-1.0, 1.0), 10.0 * random.uniform(-1.0, 1.0),
|
||||
10.0 * random.uniform(-1.0, 1.0)
|
||||
])
|
||||
self.main_vis.scene.add_geometry("sphere" + str(self._id), sphere, mat)
|
||||
|
||||
def _on_menu_sphere(self):
|
||||
# GUI callbacks happen on the main thread, so we can do everything
|
||||
# normally here.
|
||||
self.add_sphere()
|
||||
|
||||
def _on_menu_random(self):
|
||||
# This adds spheres asynchronously. This pattern is useful if you have
|
||||
# data coming in from another source than user interaction.
|
||||
def thread_main():
|
||||
for _ in range(0, 20):
|
||||
# We can only modify GUI objects on the main thread, so we
|
||||
# need to post the function to call to the main thread.
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.add_sphere)
|
||||
time.sleep(1)
|
||||
|
||||
threading.Thread(target=thread_main).start()
|
||||
|
||||
def _on_menu_quit(self):
|
||||
o3d.visualization.gui.Application.instance.quit()
|
||||
|
||||
def update_actuator_color(self, name, min_cmd, max_cmd, cmd, transparant = True):
|
||||
cmd_fraction = (cmd - min_cmd) / (max_cmd - min_cmd)
|
||||
|
||||
# Color min cmd = Blue
|
||||
# Color neutral = Green
|
||||
# Color max cmd = red
|
||||
|
||||
color_rgb = np.ones(4)
|
||||
if cmd_fraction > 0.5:
|
||||
color_rgb[0] = (cmd_fraction - 0.5) * 2.
|
||||
color_rgb[1] = 1. - color_rgb[0]
|
||||
color_rgb[2] = 0.
|
||||
else:
|
||||
color_rgb[0] = 0.
|
||||
color_rgb[2] = (0.5 - cmd_fraction) * 2.
|
||||
color_rgb[1] = 1. - color_rgb[2]
|
||||
|
||||
mat = o3d.visualization.rendering.MaterialRecord()
|
||||
mat.base_color = color_rgb
|
||||
if transparant:
|
||||
mat.shader = 'defaultLitSSR'
|
||||
mat.transmission = 0.5
|
||||
else:
|
||||
mat.shader = "defaultLit"
|
||||
self.main_vis.scene.modify_geometry_material(name, mat)
|
||||
|
||||
def update_actuator_colors(self):
|
||||
# update motors
|
||||
self.update_actuator_color("motor_F", 1000, 8191, self.actuators.get_actuator_value(3))
|
||||
self.update_actuator_color("motor_R", 1000, 8191, self.actuators.get_actuator_value(4))
|
||||
self.update_actuator_color("motor_B", 1000, 8191, self.actuators.get_actuator_value(5))
|
||||
self.update_actuator_color("motor_L", 1000, 8191, self.actuators.get_actuator_value(6))
|
||||
# update elevator
|
||||
self.update_actuator_color("ḧor_tail", 8191, -1395, self.actuators.get_actuator_value(8), transparant=False)
|
||||
|
||||
def update_fuselage(self):
|
||||
self.main_vis.scene.set_geometry_transform("fuselage", self.T_rotation)
|
||||
self.main_vis.scene.set_geometry_transform("fuselage_ref", self.T_rotation_ref)
|
||||
|
||||
def update_vert_tail(self):
|
||||
self.main_vis.scene.set_geometry_transform("vert_tail", self.T_rotation)
|
||||
|
||||
def update_wing(self):
|
||||
if self.wing_rotation_controller == None:
|
||||
return
|
||||
|
||||
wing_rotation_matrix = np.matrix(o3d.geometry.get_rotation_matrix_from_axis_angle(np.array([[0],[0],[self.wing_rotation_controller.get_wing_angle() - np.pi/2]])))
|
||||
wing_rotation_matrix_ref = np.matrix(o3d.geometry.get_rotation_matrix_from_axis_angle(np.array([[0],[0],[self.wing_rotation_controller.get_wing_angle_sp() - np.pi/2]])))
|
||||
|
||||
self.T_rotation_wing = np.matrix(np.eye(4))
|
||||
self.T_rotation_wing[:3,:3] = wing_rotation_matrix
|
||||
self.T_rotating_wing_final = self.T_rotation * self.T_rotation_wing
|
||||
|
||||
self.T_rotation_wing_ref = np.matrix(np.eye(4))
|
||||
self.T_rotation_wing_ref[:3,:3] = wing_rotation_matrix_ref
|
||||
self.T_rotating_wing_final_ref = self.T_rotation * self.T_rotation_wing_ref
|
||||
|
||||
self.main_vis.scene.set_geometry_transform("wing", self.T_rotating_wing_final)
|
||||
self.main_vis.scene.set_geometry_transform("wing_ref", self.T_rotating_wing_final_ref)
|
||||
|
||||
def thread_main():
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_motor_R)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_motor_L)
|
||||
|
||||
threading.Thread(target=thread_main).start()
|
||||
|
||||
def update_elevator(self):
|
||||
cmd_elevator = self.actuators.get_actuator_value(8)
|
||||
min_angle = np.deg2rad(-36.6)
|
||||
max_angle = np.deg2rad(10.3)
|
||||
min_cmd = 8191.
|
||||
max_cmd = -1395.
|
||||
|
||||
# Calc angle from command
|
||||
angle_elevator = cmd_elevator * (max_angle - min_angle) / (max_cmd - min_cmd)
|
||||
|
||||
elevator_rotation_matrix = np.matrix(o3d.geometry.get_rotation_matrix_from_axis_angle(np.array([[0],[-angle_elevator],[0]])))
|
||||
|
||||
T_offset_hinge = np.matrix(np.eye(4))
|
||||
T_offset_hinge[:3,3] = np.matrix([[1486.6-680.],[8.3],[-32]])
|
||||
T_elevator = np.eye(4)
|
||||
T_elevator[:3,:3] = np.matrix(elevator_rotation_matrix)
|
||||
|
||||
T_elevator_final = self.T_rotation * np.linalg.inv(T_offset_hinge) * T_elevator * T_offset_hinge
|
||||
self.main_vis.scene.set_geometry_transform("ḧor_tail", T_elevator_final)
|
||||
|
||||
def update_motor_F(self):
|
||||
self.main_vis.scene.set_geometry_transform("motor_F", self.T_rotation)
|
||||
|
||||
def update_motor_R(self):
|
||||
if self.wing_rotation_controller == None:
|
||||
return
|
||||
|
||||
self.main_vis.scene.set_geometry_transform("motor_R", self.T_rotating_wing_final)
|
||||
|
||||
def update_motor_B(self):
|
||||
self.main_vis.scene.set_geometry_transform("motor_B", self.T_rotation)
|
||||
|
||||
def update_motor_L(self):
|
||||
if self.wing_rotation_controller == None:
|
||||
return
|
||||
|
||||
self.main_vis.scene.set_geometry_transform("motor_L", self.T_rotating_wing_final)
|
||||
|
||||
def update_actuators(self):
|
||||
if self.actuators == None:
|
||||
return
|
||||
|
||||
def thread_main():
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_elevator)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_motor_F)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_motor_B)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_motor_R)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_motor_L)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_actuator_colors)
|
||||
|
||||
threading.Thread(target=thread_main).start()
|
||||
|
||||
def update_orientation_from_quat(self):
|
||||
if self.ahrs_ref_quat == None:
|
||||
return
|
||||
|
||||
# Calculate drone rotation matrix
|
||||
rotation_matrix = np.matrix(o3d.geometry.get_rotation_matrix_from_quaternion(self.ahrs_ref_quat.get_body_quaternion()))
|
||||
rotation_matrix_ref = np.matrix(o3d.geometry.get_rotation_matrix_from_quaternion(self.ahrs_ref_quat.get_ref_quaternion()))
|
||||
|
||||
self.T_rotation = np.matrix(np.eye(4))
|
||||
self.T_rotation[:3,:3] = rotation_matrix
|
||||
|
||||
self.T_rotation_ref = np.matrix(np.eye(4))
|
||||
self.T_rotation_ref[:3,:3] = rotation_matrix_ref
|
||||
|
||||
def thread_main():
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_fuselage)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_vert_tail)
|
||||
o3d.visualization.gui.Application.instance.post_to_main_thread(
|
||||
self.w, self.update_wing)
|
||||
|
||||
threading.Thread(target=thread_main).start()
|
||||
|
||||
self.update_actuators()
|
||||
|
||||
def message_recv(self, ac_id, msg):
|
||||
if msg.name == "AHRS_REF_QUAT":
|
||||
self.ahrs_ref_quat = AHRSRefQuatMessage(msg)
|
||||
self.update_orientation_from_quat()
|
||||
|
||||
if msg.name == "ROT_WING_CONTROLLER":
|
||||
self.wing_rotation_controller = RotWingControllerMessage(msg)
|
||||
|
||||
if msg.name == "ACTUATORS":
|
||||
self.actuators = ActuatorsMessage(msg)
|
||||
self.update_actuators()
|
||||
|
||||
|
||||
def main():
|
||||
o3d.visualization.gui.Application.instance.initialize()
|
||||
Viewer3D("Rot wing visualizer")
|
||||
o3d.visualization.gui.Application.instance.run()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user