From 85c53e04d338c654ee85cf757cea7783f0889370 Mon Sep 17 00:00:00 2001 From: Diego Jimenez Bravo Date: Sat, 3 Feb 2018 19:14:03 +0100 Subject: [PATCH 1/3] [#954] Ported to python 2.7 and integrated with comm --- .../MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh | 6 +- .../MAVLinkServer/MAVProxy/mavproxy.py | 122 ++++++++++++------ .../MAVLinkServer/MAVProxy/uav_viewer.cfg | 11 -- .../MAVLinkServer/MAVProxy/uav_viewer_py.yml | 30 +++++ 4 files changed, 115 insertions(+), 54 deletions(-) delete mode 100644 src/drivers/MAVLinkServer/MAVProxy/uav_viewer.cfg create mode 100644 src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml diff --git a/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh b/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh index 822e307bc..90f3e925b 100755 --- a/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh +++ b/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh @@ -1,5 +1,5 @@ cd .. -python3.5 setup.py build install --user -python3.5 ./MAVProxy/mavproxy.py --master=10.1.1.191:14550 --console -#python3.5 ./MAVProxy/mavproxy.py --master=0.0.0.0:14550 --console +python2.7 setup.py build install --user +python2.7 ./MAVProxy/mavproxy.py /home/diego/try3/2016-tfg-Diego-Jimenez/MAVProxy/uav_viewer_py.yml --master=10.1.1.191:14550 --console +#python2.7 ./MAVProxy/mavproxy.py /home/diego/try3/2016-tfg-Diego-Jimenez/MAVProxy/uav_viewer_py.yml --master=0.0.0.0:14550 --console diff --git a/src/drivers/MAVLinkServer/MAVProxy/mavproxy.py b/src/drivers/MAVLinkServer/MAVProxy/mavproxy.py index a01e3a77a..8527d4ee7 100755 --- a/src/drivers/MAVLinkServer/MAVProxy/mavproxy.py +++ b/src/drivers/MAVLinkServer/MAVProxy/mavproxy.py @@ -10,7 +10,7 @@ import sys, os, time, socket, signal import fnmatch, errno, threading import serial, select -import queue as Queue +import Queue import imp import traceback import select @@ -20,6 +20,9 @@ import jderobot import multiprocessing import time +import sys +import config +import comm from MAVProxy.modules.lib import textconsole from MAVProxy.modules.lib import rline @@ -28,7 +31,7 @@ from MAVProxy.modules.lib import udp from MAVProxy.modules.lib import tcp -#import easyiceconfig as EasyIce +import easyiceconfig as EasyIce from Pose3D import Pose3DI from CMDVel import CMDVelI @@ -763,7 +766,7 @@ def main_loop(): if operation_takeoff and time_now > time_end_operation_takeoff: print("Taking off") time_init_operation_takeoff = int(round(time.time() * 1000)) - time_end_operation_takeoff = time_init_operation_takeoff + 7000 + time_end_operation_takeoff = time_init_operation_takeoff + 5000 operation_takeoff = False on_air = True mpstate.input_queue.put("takeoff 1") @@ -935,14 +938,19 @@ def run_script(scriptfile): ########################## Jorge Cano CODE ########################## -def openPose3DChannel(Pose3D): +def openPose3DChannel(Pose3D,cfg): status = 0 ic = None Pose2Tx = Pose3D #Pose3D.getPose3DData() - #print(Pose3D) try: - ic = Ice.initialize(sys.argv) - adapter = ic.createObjectAdapterWithEndpoints("Pose3DAdapter", "default -p 9998") + id = Ice.InitializationData() + ic = Ice.initialize(None, id) + + endpoint = cfg.getProperty("Pose3D.Proxy") + name = cfg.getProperty("Pose3D.Name") + + adapter = ic.createObjectAdapterWithEndpoints("Pose3D", endpoint) + object = Pose2Tx adapter.add(object, ic.stringToIdentity("Pose3D")) adapter.activate() @@ -961,17 +969,22 @@ def openPose3DChannel(Pose3D): sys.exit(status) -def openPose3DChannelWP(Pose3D): +def openPose3DChannelWP(Pose3D,cfg): status = 0 ic = None Pose2Rx = Pose3D #Pose3D.getPose3DData() try: - ic = Ice.initialize(sys.argv) - adapter = ic.createObjectAdapterWithEndpoints("Pose3DAdapter", "default -p 9994") + id = Ice.InitializationData() + ic = Ice.initialize(None, id) + + endpoint = cfg.getProperty("Pose3D.Proxy") + name = cfg.getProperty("Pose3D.Name") + + adapter = ic.createObjectAdapterWithEndpoints("Pose3DAdapter", endpoint) + object = Pose2Rx - #print (object.getPose3DData()) - adapter.add(object, ic.stringToIdentity("Pose3D")) + adapter.add(object, ic.stringToIdentity("Pose3DAdapter")) adapter.activate() ic.waitForShutdown() except: @@ -988,15 +1001,20 @@ def openPose3DChannelWP(Pose3D): sys.exit(status) -def openCMDVelChannel(CMDVel): +def openCMDVelChannel(CMDVel,cfg): status = 0 ic = None CMDVel2Rx = CMDVel #CMDVel.getCMDVelData() try: - ic = Ice.initialize(sys.argv) - adapter = ic.createObjectAdapterWithEndpoints("CMDVelAdapter", "default -p 9997") + id = Ice.InitializationData() + ic = Ice.initialize(None, id) + + endpoint = cfg.getProperty("CMDVel.Proxy") + name = cfg.getProperty("CMDVel.Name") + + adapter = ic.createObjectAdapterWithEndpoints("CMDVel", endpoint) + object = CMDVel2Rx - print(object) adapter.add(object, ic.stringToIdentity("CMDVel")) adapter.activate() ic.waitForShutdown() @@ -1014,14 +1032,20 @@ def openCMDVelChannel(CMDVel): sys.exit(status) -def openExtraChannel(Extra): +def openExtraChannel(Extra,cfg): status = 0 ic = None Extra2Tx = Extra try: - ic = Ice.initialize(sys.argv) - adapter = ic.createObjectAdapterWithEndpoints("ExtraAdapter", "default -p 9995") + id = Ice.InitializationData() + ic = Ice.initialize(None, id) + + endpoint = cfg.getProperty("Extra.Proxy") + name = cfg.getProperty("Extra.Name") + + adapter = ic.createObjectAdapterWithEndpoints("Extra", endpoint) + object = Extra2Tx adapter.add(object, ic.stringToIdentity("Extra")) adapter.activate() @@ -1048,15 +1072,22 @@ def getNavdata(self, current=None): data = jderobot.NavdataData() return data -def openNavdataChannel(): +def openNavdataChannel(cfg): status = 0 ic = None Navdata2Tx = NavdataI() try: - ic = Ice.initialize(sys.argv) - adapter = ic.createObjectAdapterWithEndpoints("NavdataAdapter", "default -p 9996") + + id = Ice.InitializationData() + ic = Ice.initialize(None, id) + + endpoint = cfg.getProperty("Navdata.Proxy") + name = cfg.getProperty("Navdata.Name") + + adapter = ic.createObjectAdapterWithEndpoints("Navdata", endpoint) + object = Navdata2Tx adapter.add(object, ic.stringToIdentity("Navdata")) adapter.activate() @@ -1076,6 +1107,9 @@ def openNavdataChannel(): sys.exit(status) def sendCMDVel2Vehicle(CMDVel,Pose3D): + absolute = 0 + relative = 1 + while True: CMDVel2send = CMDVel.getCMDVelData() @@ -1086,15 +1120,22 @@ def sendCMDVel2Vehicle(CMDVel,Pose3D): linearYstring = str(NEDvel[1]) linearZstring = str(NEDvel[2]) - angular = Pose3D2send.q3 + CMDVel.angularZ - if angular > 1: - angular = angular - 2 - elif angular < -1: - angular = angular + 2 - angularZstring = str(angular*180) + #CMDVel.angularZ -1 y 1 + + angular = CMDVel.angularZ + + if angular >= 0: + direction = str(1) + else: + angular = -angular + direction = str(-1) + + angularZstring = str(angular*30) + + movement = str(relative) velocitystring = 'velocity '+ linearXstring + ' ' + linearYstring + ' ' + linearZstring - angularString = 'setyaw ' + angularZstring + ' 1 0' + angularString = 'setyaw ' + angularZstring + ' ' + direction + ' ' + movement process_stdin(velocitystring) # SET_POSITION_TARGET_LOCAL_NED process_stdin(angularString) @@ -1549,27 +1590,28 @@ def quit_handler(signum = None, frame = None): ########################## Jorge Cano CODE ########################## + cfg = config.load(sys.argv[1]) #Open an ICE TX communication and leave it open in a parallel threat - PoseTheading = threading.Thread(target=openPose3DChannel, args=(PH_Pose3D,), name='Pose_Theading') + PoseTheading = threading.Thread(target=openPose3DChannel, args=(PH_Pose3D,cfg,), name='Pose_Theading') PoseTheading.daemon = True PoseTheading.start() # Open an ICE RX communication and leave it open in a parallel threat - CMDVelTheading = threading.Thread(target=openCMDVelChannel, args=(PH_CMDVel,), name='CMDVel_Theading') + CMDVelTheading = threading.Thread(target=openCMDVelChannel, args=(PH_CMDVel,cfg,), name='CMDVel_Theading') CMDVelTheading.daemon = True CMDVelTheading.start() # Open an ICE TX communication and leave it open in a parallel threat - CMDVelTheading = threading.Thread(target=openExtraChannel, args=(PH_Extra,), name='Extra_Theading') + CMDVelTheading = threading.Thread(target=openExtraChannel, args=(PH_Extra,cfg,), name='Extra_Theading') CMDVelTheading.daemon = True CMDVelTheading.start() # Open an ICE channel empty - CMDVelTheading = threading.Thread(target=openNavdataChannel, args=(), name='Navdata_Theading') + CMDVelTheading = threading.Thread(target=openNavdataChannel, args=(cfg,), name='Navdata_Theading') CMDVelTheading.daemon = True CMDVelTheading.start() @@ -1582,15 +1624,15 @@ def quit_handler(signum = None, frame = None): # Open an ICE TX communication and leave it open in a parallel threat - PoseTheading = threading.Thread(target=openPose3DChannelWP, args=(WP_Pose3D,), name='WayPoint_Theading') - PoseTheading.daemon = True - PoseTheading.start() + #PoseTheading = threading.Thread(target=openPose3DChannelWP, args=(WP_Pose3D,cfg,), name='WayPoint_Theading') + #PoseTheading.daemon = True + #PoseTheading.start() - # Open an MAVLink TX communication and leave it open in a parallel threat + ## Open an MAVLink TX communication and leave it open in a parallel threat - PoseTheading = threading.Thread(target=sendWayPoint2Vehicle, args=(WP_Pose3D,), name='WayPoint2Vehicle_Theading') - PoseTheading.daemon = True - PoseTheading.start() + #PoseTheading = threading.Thread(target=sendWayPoint2Vehicle, args=(WP_Pose3D,), name='WayPoint2Vehicle_Theading') + #PoseTheading.daemon = True + #PoseTheading.start() # Open an MAVLink TX communication and leave it open in a parallel threat diff --git a/src/drivers/MAVLinkServer/MAVProxy/uav_viewer.cfg b/src/drivers/MAVLinkServer/MAVProxy/uav_viewer.cfg deleted file mode 100644 index a78c3ba5d..000000000 --- a/src/drivers/MAVLinkServer/MAVProxy/uav_viewer.cfg +++ /dev/null @@ -1,11 +0,0 @@ -UAVViewer.Camera.Proxy=Camera:default -h 10.1.1.191 -p 9999 -UAVViewer.Pose3D.Proxy=Pose3D:default -h 10.1.1.191 -p 9998 -UAVViewer.CMDVel.Proxy=CMDVel:default -h 10.1.1.191 -p 9997 -UAVViewer.Navdata.Proxy=Navdata:default -h 10.1.1.191 -p 9996 -UAVViewer.Extra.Proxy=Extra:default -h 10.1.1.191 -p 9995 - -#UAVViewer.Camera.Proxy=Camera:default -h 0.0.0.0 -p 9999 -#UAVViewer.Pose3D.Proxy=Pose3D:default -h 0.0.0.0 -p 9998 -#UAVViewer.CMDVel.Proxy=CMDVel:default -h 0.0.0.0 -p 9997 -#UAVViewer.Navdata.Proxy=Navdata:default -h 0.0.0.0 -p 9996 -#UAVViewer.Extra.Proxy=Extra:default -h 0.0.0.0 -p 9995 diff --git a/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml b/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml new file mode 100644 index 000000000..d2e72d586 --- /dev/null +++ b/src/drivers/MAVLinkServer/MAVProxy/uav_viewer_py.yml @@ -0,0 +1,30 @@ +Camera: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "default -h 0.0.0.0 -p 9999" + Format: RGB8 + Topic: "/MavLink/image_raw" + Name: MavLinkCamera + +Pose3D: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "default -h 0.0.0.0 -p 9998" + Topic: "/MavLink/Pose3D" + Name: MavLinkPose3d + +CMDVel: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "default -h 0.0.0.0 -p 9997" + Topic: "/MavLink/CMDVel" + Name: MavLinkCMDVel + +Navdata: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "default -h 0.0.0.0 -p 9996" + Topic: "/MavLink/Navdata" + Name: MavLinkNavdata + +Extra: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "default -h 0.0.0.0 -p 9995" + Topic: "/MavLink/Extra" + Name: MavLinkExtra From bc86a305f5ec3eecef4772d743475131e1d51106 Mon Sep 17 00:00:00 2001 From: Diego Jimenez Bravo Date: Sat, 3 Feb 2018 19:15:38 +0100 Subject: [PATCH 2/3] [#954] Ported to python 2.7 and integrated with comm --- src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh b/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh index 90f3e925b..f5091832f 100755 --- a/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh +++ b/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh @@ -1,5 +1,4 @@ cd .. python2.7 setup.py build install --user -python2.7 ./MAVProxy/mavproxy.py /home/diego/try3/2016-tfg-Diego-Jimenez/MAVProxy/uav_viewer_py.yml --master=10.1.1.191:14550 --console -#python2.7 ./MAVProxy/mavproxy.py /home/diego/try3/2016-tfg-Diego-Jimenez/MAVProxy/uav_viewer_py.yml --master=0.0.0.0:14550 --console +python2.7 ./MAVProxy/mavproxy.py ./MAVProxy/uav_viewer_py.yml --master=10.1.1.191:14550 --console From 7a63c8a0708a65cddb8ef8124690b25c6dd8922e Mon Sep 17 00:00:00 2001 From: Diego Jimenez Bravo Date: Sat, 3 Feb 2018 19:17:46 +0100 Subject: [PATCH 3/3] [#951] Ported to python 2.7 and integrated with comm --- src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh b/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh index f5091832f..7df4c9d18 100755 --- a/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh +++ b/src/drivers/MAVLinkServer/MAVProxy/MAVProxyWinLAN.sh @@ -1,4 +1,4 @@ cd .. -python2.7 setup.py build install --user +python2.7 setup.py build install --user python2.7 ./MAVProxy/mavproxy.py ./MAVProxy/uav_viewer_py.yml --master=10.1.1.191:14550 --console