From c0741e01ab1d0947762b571891ead80cefc3c483 Mon Sep 17 00:00:00 2001 From: Tom van Dijk Date: Wed, 30 May 2018 17:34:03 +0200 Subject: [PATCH] MAVLab course 2018 (#2268) * Merge cyberzoo model (squashed) Squashed commit of the following: commit 4ea70542059cb5d242f8e13ba43f132be6ebb710 Author: Tom van Dijk Date: Tue Feb 6 11:30:28 2018 +0100 Remove origin marker from worlds commit c008c9658a8071b8160bac276325fafcf6336a3e Author: Tom van Dijk Date: Fri Feb 2 17:25:11 2018 +0100 Compress textures Reduced texture dimensions. Size of model folder is now approx. 20 MB instead of 60+ MB. commit 5b63fc19882c52789fa908e1b343a0e3f8823ee6 Author: Tom van Dijk Date: Fri Feb 2 17:03:26 2018 +0100 Minor tweaks to models commit ed003351d18f056df853149cc59163aa18e2e009 Author: Tom van Dijk Date: Fri Feb 2 15:26:28 2018 +0100 Add cyberzoo props commit 751b94364a4e5e391e55251bf9edcdd0f65eea9f Author: Tom van Dijk Date: Thu Feb 1 15:03:30 2018 +0100 Restore cyberzoo floor collision box commit 1d4fd2d92f9343eaf0874bbef7e127ba891335c1 Author: Tom van Dijk Date: Thu Feb 1 14:59:44 2018 +0100 Add cyberzoo surroundings to main model in separate layers commit 9df9f2832c696980dc07eebaf497b46549f84f3f Author: Tom van Dijk Date: Thu Feb 1 14:48:06 2018 +0100 Separate cyberzoo model into layers Makes it easier to hide parts of the model in the GUI. commit f8c4221e9836cf05b9d6b702665efb594abb6ec0 Author: Tom van Dijk Date: Thu Feb 1 11:47:52 2018 +0100 Add daylight in windows on outer walls commit b0e6a0c572174b3e6207f671ee925155edbd15d1 Author: Tom van Dijk Date: Mon Jan 29 16:25:08 2018 +0100 Remove old cyberzoo model, fix dependencies commit dd5400077171c2c5d5a60b4bbb119ebd3cb71d5c Author: Tom van Dijk Date: Mon Jan 29 15:04:43 2018 +0100 Add cyberzoo world with orange poles for orange_avoider commit a34e2850827aebec9cc8328fed9abc141ce392d6 Author: Tom van Dijk Date: Mon Jan 29 14:30:47 2018 +0100 Set world coordinates and rotation, fix naming conflict commit 1a99ebb7bd28c58820f202e2fa8bff45f20f2ece Author: Tom van Dijk Date: Thu Jan 18 11:58:06 2018 +0100 Create cyberzoo world To-do: set origin coordinates and rotate environment commit ebf7eaf9a712f57e85218a8a256cd85fb2884a2a Author: Tom van Dijk Date: Thu Jan 18 11:41:37 2018 +0100 Minor fixes to surroundings commit c57312c557d5f4cc457f378952f773a980fec244 Author: Tom van Dijk Date: Thu Jan 18 11:15:09 2018 +0100 Add updated cyberzoo surroundings and outside walls commit 58914e14ae4c67345ae931e5bb90ab543b4636bf Author: Tom van Dijk Date: Wed Jan 17 17:24:57 2018 +0100 Add first draft of cyberzoo surroundings commit 40712f5f3beaf7dd2442a2c5c6feef03a25e9851 Author: Tom van Dijk Date: Tue Dec 12 13:45:25 2017 +0100 Fix cyberzoo origin commit e47f76cee5eaffa7d7ff19a368880ad0c64c441a Author: Tom van Dijk Date: Tue Dec 12 13:34:11 2017 +0100 Fix orange_pole2 smoothing errors commit b1e08695e78c3ed9c33301c90f97db66af699396 Author: Tom van Dijk Date: Tue Dec 12 13:09:09 2017 +0100 Add orange pole and traffic mat objects commit 8bd3d214e627da4f6afe16783466479bf43e8922 Author: Tom van Dijk Date: Fri Dec 8 14:48:53 2017 +0100 Fix optitrack camera color commit 59e7b67fef6abf0dfa9379592700fb83e0075904 Author: Tom van Dijk Date: Fri Dec 8 14:43:39 2017 +0100 Fix lighting colors commit 810e6124c1060e3ed43ee7c0ce6db7222b5148b9 Author: Tom van Dijk Date: Fri Dec 8 14:20:47 2017 +0100 Fix optitrack cameras commit e6e17c2677ef4d1fc9a12d9e139e50a35ee78ad1 Author: Tom van Dijk Date: Fri Dec 8 14:10:55 2017 +0100 Import improved model commit e9ed496af9bd0166fcbe31b792768c30d446c1f3 Author: Tom van Dijk Date: Thu Nov 30 16:38:22 2017 +0100 Fix ambient lighting of cyberzoo_solid commit 0e1c2600d60170a8a03b8351318f11932c3b14e3 Author: Tom van Dijk Date: Wed Nov 29 14:38:05 2017 +0100 First steps towards detailed cyberzoo model. Got transparency working. * Fix altitude overshoot in simulation - Bypass INS - Set nominal hover throttle to 0.51 (verified in simulation and on real drone). * Fix INS horizontal offset - Set flight plan origin to match gazebo spherical coordinates, prevents https://github.com/paparazzi/paparazzi/issues/2134#issuecomment-342493105 * Increase link lost timeout to 5s Otherwise gazebo will continuously lose link. Might need to lower this for real-world flights... * Fix image.c compilation warnings * Move dummy waypoint to 0, 0 Should prevent "waypoint 'dummy' too far from HOME" warnings when max_dist_from_home is small (e.g. cyberzoo flightplans) and when HOME is set appropriately close to the flight plan origin. * Rename 2017 -> 2018 Also removed simulation aircraft as it is no longer necessary. * Move modules to firmware section * Restore original nominal throttle Nominal throttle of 0.51 was measured without bumpers! 0.68 is correct with bumpers attached. Instead, increased the weight of the gazebo model. * Clean up control panel - Removed Bebop video stream since it did not work - Removed the Gazebo tool. When pointed to /usr/bin/gazebo it does actually start, but the GAZEBO_MODEL_PATH appears to be ignored this way. Also added conf/video.sdp to simplify video streaming (no such file was present in var). * Tag airframe after successful test flight Performed test flight in cyberzoo with course2018_orangeavoid flightplan in NAV mode. * Fix Gazebo tool, add Simulation - Gazebo + Joystick session The new gzclient_launcher.sh script sets up the correct GAZEBO_MODEL_PATH (which was not available when gzclient was started from the Paparazzi Center even if it was added to ~/.bashrc). It also allows the client to be killed from the pprz center. * FIX remove natnet from simulation session * Retune INDI for Bebop1 with bumpers Previous tuning resulted in slight attitude oscillations. * Tag after successful testflight * Add GCS flags * Change gazebo bebop colors * Disable light and shadows Top light caused ogre crash in gazebo 8.3 (not sure if version error or weak gpu). Disabled shadow for performance since there is no light anymore. * Reduce bebop front cam FoV to fix AxisAlignedBox crash in gazebo7 Apparaetly gazebo 7 does not yet support wide-angle cameras with HFoVs above 180deg. * Distance Counter Python * Cleanup after merge * Remove image.c fix (see #2237) * Add 'Simulation - Gazebo' session Avoids errors when joystick is not connected. --- ...ardrone2_orangeavoid_course2017_gazebo.xml | 215 ------------------ ...7.xml => bebop_course2018_orangeavoid.xml} | 17 +- ...ml => course2018_orangeavoid_cyberzoo.xml} | 6 +- conf/simulator/gazebo/airframes/bebop.xml | 1 + conf/simulator/gazebo/models/bebop/bebop.sdf | 18 +- conf/userconf/tudelft/course2017_conf.xml | 26 --- conf/userconf/tudelft/course2018_conf.xml | 14 ++ ...panel.xml => course2018_control_panel.xml} | 88 ++++--- .../python/distance_counter/dist.py | 43 ++++ .../distance_counter/distance_counter.py | 124 ++++++++++ 10 files changed, 265 insertions(+), 287 deletions(-) delete mode 100644 conf/airframes/tudelft/ardrone2_orangeavoid_course2017_gazebo.xml rename conf/airframes/tudelft/{bebop_orangeavoid_course2017.xml => bebop_course2018_orangeavoid.xml} (97%) rename conf/flight_plans/tudelft/{course2017_avoid_orange_cyberzoo.xml => course2018_orangeavoid_cyberzoo.xml} (96%) delete mode 100644 conf/userconf/tudelft/course2017_conf.xml create mode 100644 conf/userconf/tudelft/course2018_conf.xml rename conf/userconf/tudelft/{course2017_control_panel.xml => course2018_control_panel.xml} (54%) create mode 100755 sw/ground_segment/python/distance_counter/dist.py create mode 100644 sw/ground_segment/python/distance_counter/distance_counter.py diff --git a/conf/airframes/tudelft/ardrone2_orangeavoid_course2017_gazebo.xml b/conf/airframes/tudelft/ardrone2_orangeavoid_course2017_gazebo.xml deleted file mode 100644 index a1e6fc6f0b..0000000000 --- a/conf/airframes/tudelft/ardrone2_orangeavoid_course2017_gazebo.xml +++ /dev/null @@ -1,215 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - - - - - -
- - - - - - - - - - -
- - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - -
- - - -
- - - - - - - -
- -
- - -
- - -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - -
- -
- - - - - - - -
- -
- - - -
- - -
- - - - -
- -
- - - - - -
- diff --git a/conf/airframes/tudelft/bebop_orangeavoid_course2017.xml b/conf/airframes/tudelft/bebop_course2018_orangeavoid.xml similarity index 97% rename from conf/airframes/tudelft/bebop_orangeavoid_course2017.xml rename to conf/airframes/tudelft/bebop_course2018_orangeavoid.xml index 20d6ece207..ef04b0a450 100644 --- a/conf/airframes/tudelft/bebop_orangeavoid_course2017.xml +++ b/conf/airframes/tudelft/bebop_course2018_orangeavoid.xml @@ -1,7 +1,7 @@ - Vision Course TUDelft V2017 + Vision Course TUDelft V2018 @@ -41,9 +41,7 @@ - - - + @@ -72,8 +70,7 @@ - - + @@ -183,10 +180,10 @@
- - - - + + + + diff --git a/conf/flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml b/conf/flight_plans/tudelft/course2018_orangeavoid_cyberzoo.xml similarity index 96% rename from conf/flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml rename to conf/flight_plans/tudelft/course2018_orangeavoid_cyberzoo.xml index 5b668e437f..2cb57617c7 100644 --- a/conf/flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml +++ b/conf/flight_plans/tudelft/course2018_orangeavoid_cyberzoo.xml @@ -1,6 +1,6 @@ - +
#include "subsystems/datalink/datalink.h" #include "subsystems/electrical.h" @@ -8,7 +8,7 @@ #include "subsystems/ahrs.h" // Note: use 'git submodule update --init -- sw/ext/tudelft_gazebo_models/' - // to download the required models. + // to download the required models. #define NPS_GAZEBO_WORLD "cyberzoo_orange_poles.world"
@@ -48,7 +48,7 @@ !(nav_block >= IndexOfBlock('Land here')) && (autopilot_in_flight() == true) )" deroute="Standby"/--> - 5) && !(IndexOfBlock('Holding point') > nav_block) && !(nav_block >= IndexOfBlock('Land here')) && (autopilot_in_flight() == true) )" deroute="Land here"/> diff --git a/conf/simulator/gazebo/airframes/bebop.xml b/conf/simulator/gazebo/airframes/bebop.xml index 926e5dc2f1..9ec25a7765 100644 --- a/conf/simulator/gazebo/airframes/bebop.xml +++ b/conf/simulator/gazebo/airframes/bebop.xml @@ -8,6 +8,7 @@ +
diff --git a/conf/simulator/gazebo/models/bebop/bebop.sdf b/conf/simulator/gazebo/models/bebop/bebop.sdf index ce6c6838d7..0d4d0e71f3 100644 --- a/conf/simulator/gazebo/models/bebop/bebop.sdf +++ b/conf/simulator/gazebo/models/bebop/bebop.sdf @@ -9,7 +9,7 @@ - 0.38905 + 0.536 0.000906 0.001242 @@ -34,6 +34,10 @@ 0.15 0.05 0.05 + + 0.82 0.07 0.07 1 + 0.82 0.07 0.07 1 + @@ -65,8 +69,8 @@ - 0.25 0.85 0.95 1 - 0.25 0.85 0.95 1 + 0.82 0.07 0.07 1 + 0.82 0.07 0.07 1 @@ -129,8 +133,8 @@ - 0.25 0.85 0.95 1 - 0.25 0.85 0.95 1 + 0.82 0.07 0.07 1 + 0.82 0.07 0.07 1 @@ -192,11 +196,11 @@ 15.0 - 4608 + 3746 3288 R8G8B8 - 3.69 + 3.00 equisolid_angle true diff --git a/conf/userconf/tudelft/course2017_conf.xml b/conf/userconf/tudelft/course2017_conf.xml deleted file mode 100644 index 6f0b002d98..0000000000 --- a/conf/userconf/tudelft/course2017_conf.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - diff --git a/conf/userconf/tudelft/course2018_conf.xml b/conf/userconf/tudelft/course2018_conf.xml new file mode 100644 index 0000000000..b10a8f34aa --- /dev/null +++ b/conf/userconf/tudelft/course2018_conf.xml @@ -0,0 +1,14 @@ + + + diff --git a/conf/userconf/tudelft/course2017_control_panel.xml b/conf/userconf/tudelft/course2018_control_panel.xml similarity index 54% rename from conf/userconf/tudelft/course2017_control_panel.xml rename to conf/userconf/tudelft/course2018_control_panel.xml index 712ac1dde2..59eb0ee5e1 100644 --- a/conf/userconf/tudelft/course2017_control_panel.xml +++ b/conf/userconf/tudelft/course2018_control_panel.xml @@ -4,8 +4,15 @@ - - + + + + + + + + + @@ -23,7 +30,56 @@
- + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -34,31 +90,11 @@ + - - - + + - - - - - - - - - - - - - - - - - - - -
diff --git a/sw/ground_segment/python/distance_counter/dist.py b/sw/ground_segment/python/distance_counter/dist.py new file mode 100755 index 0000000000..487d2ce8c5 --- /dev/null +++ b/sw/ground_segment/python/distance_counter/dist.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python +# +# Copyright (C) 2016 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 . +# + +import wx +import sys +import argparse +import distance_counter + +parser = argparse.ArgumentParser(description='Capture PAYLOAD messages over the IVY bus and forward to a remote application.', epilog='payload.py is part of the paparazzi-uav project.') +settings = parser.parse_args() + +print(settings) + +class DistanceFrame(wx.App): + def OnInit(self): + self.main = distance_counter.DistanceCounterFrame(settings) + self.main.Show() + self.SetTopWindow(self.main) + return True + +def main(): + application = DistanceFrame(0) + application.MainLoop() + +if __name__ == '__main__': + main() diff --git a/sw/ground_segment/python/distance_counter/distance_counter.py b/sw/ground_segment/python/distance_counter/distance_counter.py new file mode 100644 index 0000000000..206689b60c --- /dev/null +++ b/sw/ground_segment/python/distance_counter/distance_counter.py @@ -0,0 +1,124 @@ +# +# Copyright (C) 2016 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 . +# + +import wx +import sys +import os +import threading +import socket +import array +from cStringIO import StringIO +import wx +import array +import Image +import math + + +PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../..'))) + +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") + +from pprzlink.ivy import IvyMessagesInterface + +WIDTH = 300 + + + +class DistanceCounterFrame(wx.Frame): + + def message_recv(self, ac_id, msg): + if msg.name == "INS": + + self.msg_count = self.msg_count + 1 + + newx = float(msg.get_field(0)) / 256.0 + newy = float(msg.get_field(1)) / 256.0 + + moved = ((newx - self.ins_msg_x) ** 2 + (newy - self.ins_msg_y) ** 2) + if self.init == 0: + self.init = 1 + else: + self.distance = self.distance + math.sqrt(moved) + + self.ins_msg_x = newx + self.ins_msg_y = newy + self.ins_msg_z = msg.get_field(2) + + + # graphical update + wx.CallAfter(self.update) + + def update(self): + self.Refresh() + + def OnSize(self, event): + self.w = event.GetSize()[0] + self.h = event.GetSize()[1] + self.Refresh() + + def OnPaint(self, e): + # Paint Area + 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)) + font = wx.Font(11, wx.DEFAULT, wx.NORMAL, wx.NORMAL) + dc.SetFont(font) + dc.DrawText("INS Packets:" + str(self.msg_count),2,2) + dc.DrawText("Data: " + str(self.ins_msg_x) + ", " + str(self.ins_msg_y) + ", " + str(self.ins_msg_z) + ".",2,22) + dc.DrawText("Distance: " + str(round(float(self.distance)/1.0,2)) + " m",2,22+20) + + + + def __init__(self, _settings): + + # Command line arguments + self.settings = _settings + + # Statistics + self.data = { 'packets': 0, 'bytes': 0} + + self.w = WIDTH + self.h = WIDTH + + # Frame + wx.Frame.__init__(self, id=-1, parent=None, name=u'Distance Counter', + size=wx.Size(self.w, self.h), title=u'Distance Counter') + + self.Bind(wx.EVT_PAINT, self.OnPaint) + self.Bind(wx.EVT_SIZE, self.OnSize) + self.Bind(wx.EVT_CLOSE, self.OnClose) + + # IVY + self.interface = IvyMessagesInterface("DistanceCounter") + self.interface.subscribe(self.message_recv) + + self.msg_count = 0 + self.distance = 0 + self.ins_msg_x = 0 + self.ins_msg_y = 0 + self.ins_msg_z = 0 + self.init = 0 + + def OnClose(self, event): + self.interface.shutdown() + self.Destroy()