├── .gitignore ├── .travis.yml ├── LICENSE ├── README.md ├── openuavapp ├── docker-compose.yml ├── dockerfiles │ ├── django │ │ ├── Dockerfile │ │ └── requirements.txt │ ├── nginx │ │ ├── Dockerfile │ │ └── config │ │ │ └── django.conf │ ├── openuav_sample │ │ ├── Dockerfile │ │ ├── django_files │ │ │ ├── project_urls.py │ │ │ ├── query_urls.py │ │ │ └── query_view.py │ │ ├── install_geographiclib_datasets.sh │ │ ├── ros-setups │ │ │ ├── README.md │ │ │ ├── intel-edison │ │ │ │ ├── README.md │ │ │ │ ├── install_indigo_mavros_opencv.sh │ │ │ │ ├── install_kinetic_mavros.sh │ │ │ │ ├── install_ros.sh │ │ │ │ ├── install_ros_with_opencv.sh │ │ │ │ ├── setpoint_demo.py │ │ │ │ └── update_packages.sh │ │ │ ├── ubuntu-14 │ │ │ │ ├── setup-install.sh │ │ │ │ ├── setup-mavlink-mavros.sh │ │ │ │ └── setup-ros.sh │ │ │ └── ubuntu-16 │ │ │ │ ├── GeographicLib-1.48.tar.gz │ │ │ │ ├── README.txt │ │ │ │ ├── setup-gym-gzweb-uavnav.sh │ │ │ │ ├── setup-install.sh │ │ │ │ ├── setup-mavlink-mavros.sh │ │ │ │ └── setup-ros.sh │ │ └── setup.sh │ └── postgres │ │ ├── Dockerfile │ │ └── init.sql ├── manage.py ├── nvidia-docker-compose.yml ├── openuav │ ├── __init__.py │ ├── settings.py │ ├── urls.py │ └── wsgi.py └── sim │ ├── __init__.py │ ├── admin.py │ ├── apps.py │ ├── exceptions.py │ ├── migrations │ └── __init__.py │ ├── models.py │ ├── static │ └── sim │ │ ├── loading.gif │ │ ├── loading2.gif │ │ └── randomColor.js │ ├── templates │ └── sim │ │ ├── dev_console.html │ │ ├── dev_console_first.html │ │ ├── dev_console_second.html │ │ ├── dev_console_unsecure.html │ │ ├── dev_console_unsecure_first.html │ │ ├── dev_console_unsecure_second.html │ │ └── error.html │ ├── tests.py │ ├── urls.py │ └── views.py ├── samples ├── .gitignore ├── dronekit-mavros │ ├── inputs │ │ ├── controllers │ │ │ └── dostuff.py │ │ ├── measures │ │ │ ├── measureInterRobotDistance.py │ │ │ └── score.sh │ │ ├── models │ │ │ └── f450-1 │ │ │ │ ├── f450-1.sdf │ │ │ │ └── model.config │ │ ├── parameters │ │ │ └── swarm.sh │ │ ├── setup │ │ │ ├── gen_gazebo_ros_spawn.py │ │ │ ├── gen_mavros.py │ │ │ ├── gen_px4_sitl.py │ │ │ ├── posix_sitl_openuav_swarm_base.launch │ │ │ ├── testArmAll.py │ │ │ └── testCreateUAVSwarm.py │ │ └── world │ │ │ └── empty.world │ └── run_this.sh ├── formation │ ├── config0_0.txt │ ├── inputs │ │ ├── controllers │ │ │ ├── pid.py │ │ │ ├── sequencer.py │ │ │ └── simple_Formation.py │ │ ├── measures │ │ │ ├── measureInterRobotDistance.py │ │ │ └── score.sh │ │ ├── models │ │ │ └── f450-1 │ │ │ │ ├── f450-1.sdf │ │ │ │ └── model.config │ │ ├── parameters │ │ │ └── swarm.sh │ │ ├── setup │ │ │ ├── .profile │ │ │ ├── gen_gazebo_ros_spawn.py │ │ │ ├── gen_mavros.py │ │ │ ├── gen_px4_sitl.py │ │ │ ├── posix_sitl_openuav_swarm_base.launch │ │ │ ├── testArmAll.py │ │ │ └── testCreateUAVSwarm.py │ │ └── world │ │ │ ├── empty.world │ │ │ └── terrain.world │ ├── launch.sh │ └── run_this.sh ├── leader-follower │ ├── inputs │ │ ├── controllers │ │ │ ├── test_1_Loop.py │ │ │ ├── test_3_Follow.py │ │ │ └── test_4_Velocity.py │ │ ├── measures │ │ │ ├── measureInterRobotDistance.py │ │ │ └── score.sh │ │ ├── models │ │ │ └── f450-1 │ │ │ │ ├── f450-1.sdf │ │ │ │ └── model.config │ │ ├── parameters │ │ │ └── swarm.sh │ │ ├── setup │ │ │ ├── gen_gazebo_ros_spawn.py │ │ │ ├── gen_mavros.py │ │ │ ├── gen_px4_sitl.py │ │ │ ├── posix_sitl_openuav_swarm_base.launch │ │ │ ├── testArmAll.py │ │ │ └── testCreateUAVSwarm.py │ │ └── world │ │ │ └── empty.world │ ├── launch.sh │ └── run_this.sh ├── testSimulation │ ├── inputs │ │ ├── controllers │ │ │ ├── test_1_Loop.py │ │ │ └── test_3_Follow.py │ │ ├── measures │ │ │ ├── measureInterRobotDistance.py │ │ │ └── score.sh │ │ ├── models │ │ │ └── f450-1 │ │ │ │ ├── f450-1.sdf │ │ │ │ └── model.config │ │ ├── parameters │ │ │ └── swarm.sh │ │ ├── setup │ │ │ ├── testArmAll.py │ │ │ └── testCreateUAVSwarm.py │ │ └── world │ │ │ └── empty.world │ ├── outputs │ │ └── average_measure.txt │ └── run_this.sh └── turtlebot │ ├── config0_0.txt │ ├── inputs │ ├── controllers │ │ ├── pid.py │ │ ├── sequencer.py │ │ ├── simple_Formation.py │ │ ├── turtlebotLoop.py │ │ └── uavFollow.py │ ├── measures │ │ ├── measureInterRobotDistance.py │ │ └── score.sh │ ├── models │ │ └── f450-1 │ │ │ ├── f450-1.sdf │ │ │ └── model.config │ ├── parameters │ │ └── swarm.sh │ ├── setup │ │ ├── .profile │ │ ├── gen_gazebo_ros_spawn.py │ │ ├── gen_mavros.py │ │ ├── gen_px4_sitl.py │ │ ├── posix_sitl_openuav_swarm_base.launch │ │ ├── testArmAll.py │ │ ├── testCreateUAVSwarm.py │ │ ├── test_kobuki.launch │ │ └── turtlebot_model.launch.xml │ └── world │ │ ├── empty.world │ │ └── terrain.world │ ├── launch.sh │ └── run_this.sh └── tests ├── build.sh ├── docker-compose.yml ├── measure.js └── test_setup.sh /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | **/*.py[cod] 4 | *.csv 5 | *Log*.txt 6 | */outputs/* 7 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | before_install: 4 | - ./tests/test_setup.sh 5 | - ./tests/build.sh 6 | script: 7 | - node tests/measure.js 8 | language: generic 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Project website: https://openuav.us 2 | 3 | Here are the instructions for installing the simulation testbed on an Ubuntu 16.04 machine with an NVIDIA graphics card. Our test box is a Lenovo C730 with an NVIDIA GeForce RTX 2070 graphics card. Ensure that you have correct drivers installed by searching for and running the appropriate binary. For us, it was NVIDIA-Linux-x86_64-410.93.run found at https://www.nvidia.com/Download/index.aspx?lang=en-us . 4 | 5 | 6 | - curl -sSL https://get.docker.com/ | sh 7 | - sudo usermod -aG $USER docker 8 | - sudo systemctl start docker 9 | - sudo apt-get install docker-compose 10 | - git clone https://github.com/Open-UAV/openuav-playground 11 | - xhost +local: 12 | 13 | - Do understand the implications of sharing xserver. 14 | 15 | - curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list 16 | - sudo apt-get update 17 | - sudo apt-get install -y nvidia-docker 18 | - pip install nvidia-docker-compose 19 | - cd ~/openuav-playground/openuavapp 20 | - nvidia-docker-compose build 21 | 22 | This will take a while. 23 | 24 | Once the build is complete, you will be able to run simulations and see it by running gzclient and rqt in the simulaiton container. 25 | Example: 26 | - nvidia-docker run -dit --net=openuavapp_default --name=openuavapp_x${3:-`date +%s`} -v /tmp/.X11-unix:/tmp/.X11-unix -v /home/jdas/openuav-playground/samples/formation/:/simulation -e DISPLAY=:0 --entrypoint "/home/setup.sh" openuavapp_openuav 27 | 28 | Assume the assigned simulation container name is openuav_x12345678 , you can verify yours by using docker ps 29 | 30 | - docker exec -it openuav_x12345678 bash 31 | - # source /usr/share/gazebo-7/setup.sh 32 | - # gzclient 33 | 34 | Gazebo client will pop up on your host. 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /openuavapp/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '2' 2 | 3 | services: 4 | db: 5 | build: dockerfiles/postgres/ 6 | web: 7 | restart: always 8 | build: dockerfiles/django/ 9 | command: bash -c "while ! pg_isready -h db ; do sleep 1 ; done && python manage.py makemigrations && python manage.py migrate && uwsgi --http-socket :80 -T -p 5 --module openuav.wsgi" 10 | # command: bash -c "while ! pg_isready -h db ; do sleep 1 ; done && python manage.py makemigrations && python manage.py migrate && python3 manage.py runserver 0.0.0.0:80" 11 | volumes: 12 | - .:/code 13 | ports: 14 | - "8001:80" 15 | depends_on: 16 | - db 17 | openuav: 18 | build: dockerfiles/openuav_sample/ 19 | volumes: 20 | - ../samples/testSimulation:/simulation 21 | - /tmp/.X11-unix:/tmp/.X11-unix 22 | environment: 23 | - DISPLAY=$DISPLAY 24 | expose: 25 | - "8000" 26 | - "31819" 27 | - "80" 28 | - "9090" 29 | entrypoint: /home/setup.sh 30 | 31 | networks: 32 | default: 33 | ipam: 34 | driver: default 35 | config: 36 | - subnet: 172.28.0.0/16 37 | gateway: 172.28.0.1 38 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/django/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM python:3 2 | ENV PYTHONUNBUFFERED 1 3 | RUN mkdir /code 4 | WORKDIR /code 5 | ADD requirements.txt /code/ 6 | RUN apt-get update 7 | RUN pip install -r requirements.txt 8 | RUN apt-get install -y vim 9 | RUN apt-get install -y dnsutils 10 | RUN apt-get install -y postgresql-client-common 11 | RUN apt-get install -y postgresql-client 12 | 13 | # Not verified 14 | RUN django-admin.py startproject openuav . 15 | RUN python3 manage.py startapp sim 16 | # Not verified 17 | 18 | ADD . /code/ 19 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/django/requirements.txt: -------------------------------------------------------------------------------- 1 | Django 2 | gunicorn==19.7.1 3 | psycopg2 4 | uwsgi 5 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/nginx/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM nginx:latest 2 | 3 | RUN rm -f /etc/nginx/conf.d/default.conf 4 | # Config file to be added by mounting volume while launching -------------------------------------------------------------------------------- /openuavapp/dockerfiles/nginx/config/django.conf: -------------------------------------------------------------------------------- 1 | server { 2 | 3 | listen 80; 4 | # server_name not.configured.example.com; 5 | # charset utf-8; 6 | 7 | # location /static { 8 | # alias /data/web/mydjango/static; 9 | # } 10 | 11 | location / { 12 | proxy_pass http://web:80; 13 | proxy_set_header Host $host; 14 | proxy_set_header X-Real-IP $remote_addr; 15 | proxy_set_header X-Forwarded-For $proxy_add_x_forwarded_for; 16 | } 17 | 18 | } 19 | 20 | server { 21 | listen 80; 22 | server_name view-1.openuav.us; 23 | #ssl_certificate /etc/letsencrypt/live/label.ag-0001/fullchain.pem; # managed by Certbot 24 | #ssl_certificate_key /etc/letsencrypt/live/label.ag-0001/privkey.pem; # managed by Certbot 25 | location / { 26 | proxy_set_header X-Real-IP $remote_addr; 27 | proxy_set_header Host $host; 28 | proxy_set_header X-Forwarded-For $proxy_add_x_forwarded_for; 29 | proxy_pass http://172.28.0.6:80; 30 | #proxy_ssl_certificate /etc/letsencrypt/live/label.ag/fullchain.pem; # managed by Certbot 31 | #proxy_ssl_certificate_key /etc/letsencrypt/live/label.ag/privkey.pem; # managed by Certbot 32 | } 33 | } 34 | 35 | server { 36 | listen 80; 37 | server_name view-2.openuav.us; 38 | #ssl_certificate /etc/letsencrypt/live/label.ag-0001/fullchain.pem; # managed by Certbot 39 | #ssl_certificate_key /etc/letsencrypt/live/label.ag-0001/privkey.pem; # managed by Certbot 40 | location / { 41 | proxy_set_header X-Real-IP $remote_addr; 42 | proxy_set_header Host $host; 43 | proxy_set_header X-Forwarded-For $proxy_add_x_forwarded_for; 44 | proxy_pass http://172.28.0.7:80; 45 | #proxy_ssl_certificate /etc/letsencrypt/live/label.ag/fullchain.pem; # managed by Certbot 46 | #proxy_ssl_certificate_key /etc/letsencrypt/live/label.ag/privkey.pem; # managed by Certbot 47 | } 48 | } 49 | 50 | map $http_upgrade $connection_upgrade { 51 | default upgrade; 52 | '' close; 53 | } 54 | 55 | 56 | server { 57 | listen 80; 58 | server_name ros-1.openuav.us; 59 | #ssl_certificate /etc/letsencrypt/live/label.ag-0001/fullchain.pem; # managed by Certbot 60 | #ssl_certificate_key /etc/letsencrypt/live/label.ag-0001/privkey.pem; # managed by Certbot 61 | location / { 62 | proxy_http_version 1.1; 63 | proxy_set_header Upgrade $http_upgrade; 64 | proxy_set_header Connection $connection_upgrade; 65 | proxy_pass http://172.28.0.6:9090; 66 | } 67 | } 68 | 69 | server { 70 | listen 80; 71 | server_name ros-2.openuav.us; 72 | #ssl_certificate /etc/letsencrypt/live/label.ag-0001/fullchain.pem; # managed by Certbot 73 | #ssl_certificate_key /etc/letsencrypt/live/label.ag-0001/privkey.pem; # managed by Certbot 74 | location / { 75 | proxy_http_version 1.1; 76 | proxy_set_header Upgrade $http_upgrade; 77 | proxy_set_header Connection $connection_upgrade; 78 | proxy_pass http://172.28.0.7:9090; 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/django_files/project_urls.py: -------------------------------------------------------------------------------- 1 | from django.urls import include, path 2 | from django.contrib import admin 3 | 4 | urlpatterns = [ 5 | path('query/', include('query.urls')), 6 | path('admin/', admin.site.urls), 7 | ] -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/django_files/query_urls.py: -------------------------------------------------------------------------------- 1 | from django.urls import path 2 | 3 | from . import views 4 | 5 | urlpatterns = [ 6 | path('measures', views.measures, name='measures'), 7 | path('numUavs', views.numUavs, name='numUavs'), 8 | path('debugStmts', views.debugStmts, name='debugStmts'), 9 | path('', views.index, name='index'), 10 | ] -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/django_files/query_view.py: -------------------------------------------------------------------------------- 1 | from django.http import HttpResponse 2 | import subprocess 3 | 4 | def index(request): 5 | return HttpResponse("Hello, world. You're at the query index.") 6 | 7 | def numUavs(request): 8 | cmd = '''cat /simulation/inputs/parameters/swarm.sh | grep num_uavs | grep -o -E '([0-9]|[1-9][0-9])' | tail -n1 ''' 9 | 10 | try: 11 | p1 = subprocess.Popen(['cat', '/simulation/inputs/parameters/swarm.sh'], stdout=subprocess.PIPE) 12 | p2 = subprocess.Popen(['grep', 'num_uavs'], 13 | stdin=p1.stdout, stdout=subprocess.PIPE) 14 | p1.stdout.close() 15 | p3 = subprocess.Popen(['grep', '-o' , '-E', '''([0-9]|[1-9][0-9])'''], 16 | stdin=p2.stdout,stdout=subprocess.PIPE) 17 | p2.stdout.close() 18 | p4 = subprocess.Popen(['tail', '-n1'], 19 | stdin=p3.stdout, stdout=subprocess.PIPE) 20 | p3.stdout.close() 21 | output = p4.communicate()[0] 22 | except: 23 | output = 'Nothing' 24 | 25 | output = output.decode('UTF-8').strip() + '#' 26 | return HttpResponse(output) 27 | 28 | def measures(request): 29 | cmd = '''ps ax | grep measure | wc -l''' 30 | 31 | try: 32 | p1 = subprocess.Popen(['ps', 'ax'], stdout=subprocess.PIPE) 33 | p2 = subprocess.Popen(['grep', 'measure'], 34 | stdin=p1.stdout, stdout=subprocess.PIPE) 35 | p1.stdout.close() 36 | p3 = subprocess.Popen(['wc', '-l'], 37 | stdin=p2.stdout,stdout=subprocess.PIPE) 38 | p2.stdout.close() 39 | output = p3.communicate()[0] 40 | except: 41 | output = 'Nothing' 42 | 43 | output = output.decode('UTF-8').strip() + '#' 44 | return HttpResponse(output) 45 | 46 | def debugStmts(request): 47 | output = 'Debug Statements:' 48 | try: 49 | fo = open("/tmp/debug", "r") 50 | lines = fo.readlines() 51 | output = '
'.join(lines) 52 | fo.close() 53 | except: 54 | output = '' 55 | return HttpResponse(output) 56 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/install_geographiclib_datasets.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to install the model datasets required 3 | # to GeographicLib apply certain conversions 4 | 5 | if [[ $UID != 0 ]]; then 6 | echo "This script require root privileges!" 1>&2 7 | exit 1 8 | fi 9 | 10 | # Install datasets 11 | run_get() { 12 | local dir="$1" 13 | local tool="$2" 14 | local model="$3" 15 | 16 | files=$(shopt -s nullglob dotglob; echo /usr/share/GeographicLib/$dir/$model* /usr/local/share/GeographicLib/$dir/$model*) 17 | if (( ${#files} )); then 18 | echo "GeographicLib $tool dataset $model already exists, skipping" 19 | return 20 | fi 21 | 22 | echo "Installing GeographicLib $tool $model" 23 | geographiclib-get-$tool $model >/dev/null 2>&1 24 | } 25 | 26 | # check which command script is available 27 | if hash geographiclib-get-geoids; then 28 | run_get geoids geoids egm96-5 29 | run_get gravity gravity egm96 30 | run_get magnetic magnetic emm2015 31 | elif hash geographiclib-datasets-download; then # only allows install the goid model dataset 32 | geographiclib-datasets-download egm96_5; 33 | else 34 | echo "OS not supported! Check GeographicLib page for supported OS and lib versions." 1>&2 35 | fi 36 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/README.md: -------------------------------------------------------------------------------- 1 | # ROS Setups 2 | 3 | Instructions and scripts for the installation of ROS on various platforms, such as Odroid and Intel Edison. 4 | 5 | # NOTE: This is the same setup as https://github.com/darknight-007/ros-setups but with changes so that we can install with Docker. It also includes an additional install script for gzweb, and openai-gym. The dockerfile in addition, installs tensorflow-gpu 6 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/intel-edison/install_indigo_mavros_opencv.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # The following installation is based on: http://wiki.ros.org/wiki/edison 4 | # and http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Indigo%20on%20Raspberry%20Pi 5 | 6 | #### NOTE: this is not tested, during opencv compilation the Edison ran out of disk space. 7 | 8 | if [ `whoami` == "root" ]; then 9 | echo "Do not run this as root!" 10 | exit 1 11 | fi 12 | 13 | echo "*** Update sources.list ***" 14 | 15 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu jessie main" > /etc/apt/sources.list.d/ros-latest.list' 16 | 17 | echo "*** Get ROS keys ***" 18 | wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - 19 | 20 | echo "*** Update the OS ***" 21 | sudo apt-get -y update 22 | sudo apt-get -y upgrade 23 | 24 | echo "*** Install required OS packages ***" 25 | sudo apt-get install python-rosdep python-rosinstall-generator python-wstool python-rosinstall build-essential 26 | 27 | echo "*** ROSDEP ***" 28 | sudo rosdep init 29 | rosdep update 30 | 31 | mkdir ~/ros_catkin_ws 32 | cd ~/ros_catkin_ws 33 | 34 | echo "*** rosinstall ***" 35 | rosinstall_generator ros_comm mavros mavros_extras --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo-ros_comm-wet.rosinstall 36 | 37 | echo "*** wstool ***" 38 | sudo wstool init src -j1 indigo-ros_comm-wet.rosinstall 39 | while [ $? != 0 ]; do 40 | echo "*** wstool - download failures, retrying ***" 41 | sudo wstool update -t src -j1 42 | done 43 | 44 | ############### 45 | echo "*** rosdep install - Errors at the end may or may not be normal ***" 46 | cd ~/ros_catkin_ws 47 | # Python errors after the following command are normal. 48 | rosdep install --from-paths src --ignore-src --rosdistro indigo -y --os=debian:jessie 49 | 50 | read -p "Check above if there were any rosdep errors. Exit script and proceed manually if there were." -n1 -s 51 | 52 | echo "*** Install catkin_tools ***" 53 | sudo apt-get install python-catkin-tools 54 | # sudo apt-get install python-psutil 55 | 56 | # echo "*** Upgrading python imports. For some reason we need to do these when using mavros_extras? ***" 57 | # sudo pip install future 58 | # sudo apt-get install libxml2-dev libxslt1-dev 59 | # echo "*** This will take a while ***" 60 | # sudo pip install --upgrade lxml 61 | 62 | echo “******************************************************************” 63 | echo “About to start some heavy building. Go have a looong coffee break.” 64 | echo “******************************************************************” 65 | 66 | echo "*** Building ROS ***" 67 | catkin config --install-space /home/ros/indigo 68 | catkin config --install 69 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 70 | echo "This will take several hours..." 71 | sudo catkin build -j1 72 | # sudo catkin build --mem-limit 50% 73 | # sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /home/ros/indigo 74 | 75 | read -p "ROS built. Press any key to continue..." -n1 -s 76 | 77 | sudo ln -sf /home/ros /opt/ 78 | 79 | # echo "*** Updating .profile and .bashrc ***" 80 | echo "*** Updating .bashrc ***" 81 | # echo "source /home/ros/indigo/setup.bash" >> ~/.profile 82 | # source ~/.profile 83 | 84 | # echo "source ~/ros_catkin_ws/devel/setup.bash" >> ~/.bashrc 85 | echo "source /home/ros/indigo/setup.bash" >> ~/.bashrc 86 | source ~/.bashrc 87 | 88 | cd ~/ros_catkin_ws 89 | 90 | echo "" 91 | echo "*** FINISHED! ***" 92 | 93 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/intel-edison/install_kinetic_mavros.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # The following installation is based on: http://wiki.ros.org/wiki/edison 4 | # and http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Indigo%20on%20Raspberry%20Pi 5 | 6 | #### NOTE: this is not tested, during opencv compilation the Edison ran out of disk space. 7 | 8 | if [ `whoami` == "root" ]; then 9 | echo "Do not run this as root!" 10 | exit 1 11 | fi 12 | 13 | echo "*** Update sources.list ***" 14 | 15 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu jessie main" > /etc/apt/sources.list.d/ros-latest.list' 16 | 17 | echo "*** Get ROS keys ***" 18 | wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - 19 | 20 | echo "*** Update the OS ***" 21 | sudo apt-get -y update 22 | sudo apt-get -y upgrade 23 | 24 | echo "*** Install required OS packages ***" 25 | sudo apt-get -y install python-rosdep python-rosinstall-generator python-wstool python-rosinstall build-essential 26 | 27 | echo "*** ROSDEP ***" 28 | sudo rosdep init 29 | rosdep update 30 | 31 | mkdir ~/ros_catkin_ws 32 | cd ~/ros_catkin_ws 33 | 34 | echo "*** rosinstall ***" 35 | rosinstall_generator ros_comm mavros mavros_extras --rosdistro kinetic --deps --wet-only --exclude roslisp --tar > kinetic-ros_comm-wet.rosinstall 36 | 37 | echo "*** wstool ***" 38 | sudo wstool init src -j1 kinetic-ros_comm-wet.rosinstall 39 | while [ $? != 0 ]; do 40 | echo "*** wstool - download failures, retrying ***" 41 | sudo wstool update -t src -j1 42 | done 43 | 44 | ############### 45 | echo "*** rosdep install - Errors at the end may or may not be normal ***" 46 | cd ~/ros_catkin_ws 47 | # Python errors after the following command are normal. 48 | rosdep install --from-paths src --ignore-src --rosdistro kinetic -y -q -r --os=debian:jessie 49 | 50 | # read -p "Check above if there were any rosdep errors. Exit script and proceed manually if there were." -n1 -s 51 | 52 | echo "*** Install catkin_tools ***" 53 | sudo apt-get -y install python-catkin-tools 54 | # sudo apt-get install python-psutil 55 | 56 | # echo "*** Upgrading python imports. For some reason we need to do these when using mavros_extras? ***" 57 | # sudo pip install future 58 | # sudo apt-get install libxml2-dev libxslt1-dev 59 | # echo "*** This will take a while ***" 60 | # sudo pip install --upgrade lxml 61 | 62 | echo “******************************************************************” 63 | echo “About to start some heavy building. Go have a looong coffee break.” 64 | echo “******************************************************************” 65 | 66 | echo "*** Building ROS ***" 67 | catkin config --install-space /home/ros/kinetic 68 | catkin config --install 69 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 70 | echo "The Edison does not have enough memory to support two jobs at once. Hence we must restrict catkin build to only use 1 job. This will take several hours..." 71 | sudo catkin build -j1 72 | # sudo catkin build --mem-limit 50% 73 | # sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /home/ros/kinetic 74 | 75 | read -p "ROS built. Press any key to continue..." -n1 -s 76 | 77 | sudo ln -sf /home/ros /opt/ 78 | 79 | # echo "*** Updating .profile and .bashrc ***" 80 | echo "*** Updating .bashrc ***" 81 | # echo "source /home/ros/indigo/setup.bash" >> ~/.profile 82 | # source ~/.profile 83 | 84 | # echo "source ~/ros_catkin_ws/devel/setup.bash" >> ~/.bashrc 85 | echo "source /home/ros/kinetic/setup.bash" >> ~/.bashrc 86 | source ~/.bashrc 87 | 88 | cd ~/ros_catkin_ws 89 | 90 | echo "" 91 | echo "*** FINISHED! ***" 92 | 93 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/intel-edison/install_ros.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # The following installation is based on: http://wiki.ros.org/wiki/edison 4 | # and http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Indigo%20on%20Raspberry%20Pi 5 | 6 | if [ `whoami` == "root" ]; then 7 | echo "Do not run this as root!" 8 | exit 1 9 | fi 10 | 11 | echo "*** Update sources.list ***" 12 | 13 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu wheezy main" > /etc/apt/sources.list.d/ros-latest.list' 14 | 15 | echo "*** Get ROS and Raspian keys ***" 16 | wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - 17 | wget http://archive.raspbian.org/raspbian.public.key -O - | sudo apt-key add - 18 | 19 | echo "*** Update the OS ***" 20 | sudo apt-get -y update 21 | sudo apt-get -y upgrade 22 | 23 | echo "*** Install required OS packages ***" 24 | sudo apt-get -y install pkg-config 25 | sudo apt-get -y install python-setuptools python-pip python-yaml python-argparse python-distribute python-docutils python-dateutil python-six 26 | 27 | echo "*** Install required ROS packages ***" 28 | sudo pip install rosdep rosinstall_generator wstool rosinstall 29 | 30 | echo "*** ROSDEP ***" 31 | sudo rosdep init 32 | rosdep update 33 | 34 | mkdir ~/catkin_ws 35 | cd ~/catkin_ws 36 | 37 | echo "*** rosinstall ***" 38 | # This will install only mavros and not mavros-extras (no image 39 | # support which the Edison can’t really handle well anyway). 40 | rosinstall_generator ros_comm mavros --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo-ros_comm-wet.rosinstall 41 | 42 | echo "*** wstool ***" 43 | sudo wstool init src -j1 indigo-ros_comm-wet.rosinstall 44 | while [ $? != 0 ]; do 45 | echo "*** wstool - download failures, retrying ***" 46 | sudo wstool update -t src -j1 47 | done 48 | 49 | echo "*** Install cmake and update sources.list ***" 50 | mkdir ~/ros_catkin_ws/external_src 51 | sudo apt-get -y install checkinstall cmake 52 | sudo sh -c 'echo "deb-src http://mirrordirector.raspbian.org/raspbian/ testing main contrib non-free rpi" >> /etc/apt/sources.list' 53 | sudo sh -c 'echo "deb http://http.debian.net/debian wheezy-backports main" >> /etc/apt/sources.list' 54 | sudo apt-get -y update 55 | 56 | echo "*** Install console bridge ***" 57 | cd ~/ros_catkin_ws/external_src 58 | sudo apt-get -y build-dep console-bridge 59 | apt-get -y source -b console-bridge 60 | sudo dpkg -i libconsole-bridge0.2*.deb libconsole-bridge-dev*.deb 61 | 62 | echo "*** Install liblz4-dev ***" 63 | sudo apt-get -y install liblz4-dev 64 | 65 | echo "*** rosdep install - Errors at the end are normal ***" 66 | cd ~/ros_catkin_ws 67 | # Python errors after the following command are normal. 68 | rosdep install --from-paths src --ignore-src --rosdistro indigo -y -r --os=debian:wheezy 69 | 70 | echo “******************************************************************” 71 | echo “About to start some heavy building. Go have a looong coffee break.” 72 | echo “******************************************************************” 73 | 74 | echo "*** Building ROS ***" 75 | catkin config --install-space /home/ros/indigo 76 | catkin config --install 77 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 78 | sudo catkin build 79 | # sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -DMAVLINK_DIALECT=common --install-space /home/ros/indigo 80 | 81 | sudo ln -sf /home/ros /opt/ 82 | 83 | echo "*** Updating .profile and .bashrc ***" 84 | echo "source /home/ros/indigo/setup.bash" >> ~/.profile 85 | source ~/.profile 86 | 87 | echo "source ~/ros_catkin_ws/devel/setup.bash" >> ~/.bashrc 88 | source ~/.bashrc 89 | 90 | cd ~/ros_catkin_ws 91 | 92 | echo "" 93 | echo "*** FINISHED! ***" 94 | 95 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/intel-edison/install_ros_with_opencv.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # The following installation is based on: http://wiki.ros.org/wiki/edison 4 | # and http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Indigo%20on%20Raspberry%20Pi 5 | 6 | #### NOTE: this is not tested, during opencv compilation the Edison ran out of disk space. 7 | 8 | if [ `whoami` == "root" ]; then 9 | echo "Do not run this as root!" 10 | exit 1 11 | fi 12 | 13 | echo "*** Update sources.list ***" 14 | 15 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu jessie main" > /etc/apt/sources.list.d/ros-latest.list' 16 | 17 | echo "*** Get ROS and Raspian keys ***" 18 | wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - 19 | wget http://archive.raspbian.org/raspbian.public.key -O - | sudo apt-key add - 20 | 21 | echo "*** Update the OS ***" 22 | sudo apt-get -y update 23 | sudo apt-get -y upgrade 24 | 25 | echo "*** Install required OS packages ***" 26 | sudo apt-get -y install pkg-config 27 | sudo apt-get -y install python-setuptools python-pip python-yaml python-argparse python-distribute python-docutils python-dateutil python-setuptools python-six 28 | 29 | echo "*** Install required ROS packages ***" 30 | sudo pip install rosdep rosinstall_generator wstool rosinstall 31 | 32 | echo "*** ROSDEP ***" 33 | sudo rosdep init 34 | rosdep update 35 | 36 | mkdir ~/ros_catkin_ws 37 | cd ~/ros_catkin_ws 38 | 39 | echo "*** rosinstall ***" 40 | rosinstall_generator ros_comm mavros mavros_extras --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo-ros_comm-wet.rosinstall 41 | 42 | echo "*** wstool ***" 43 | sudo wstool init src -j1 indigo-ros_comm-wet.rosinstall 44 | while [ $? != 0 ]; do 45 | echo "*** wstool - download failures, retrying ***" 46 | sudo wstool update -t src -j1 47 | done 48 | 49 | echo "*** Install cmake and update sources.list ***" 50 | mkdir ~/ros_catkin_ws/external_src 51 | sudo apt-get -y install checkinstall cmake 52 | sudo sh -c 'echo "deb-src http://mirrordirector.raspbian.org/raspbian/ testing main contrib non-free rpi" >> /etc/apt/sources.list' 53 | sudo sh -c 'echo "deb http://http.debian.net/debian wheezy-backports main" >> /etc/apt/sources.list' 54 | sudo apt-get -y update 55 | 56 | echo "*** Install console bridge ***" 57 | cd ~/ros_catkin_ws/external_src 58 | sudo apt-get -y build-dep console-bridge 59 | apt-get -y source -b console-bridge 60 | sudo dpkg -i libconsole-bridge0.2*.deb libconsole-bridge-dev*.deb 61 | 62 | echo "*** Install liblz4-dev ***" 63 | sudo apt-get -y install liblz4-dev 64 | 65 | ############### 66 | # MAVROS extras requires libopencv-dev which adds these deps: liburdfdom-headers-dev, liburdfdom-dev 67 | echo "*** Install liburdfdom-headers-dev ***" 68 | cd ~/ros_catkin_ws/external_src 69 | sudo apt-get -y source -b liburdfdom-headers-dev 70 | sudo dpkg -i liburdfdom-headers-dev_*.deb 71 | 72 | echo "*** Install liburdfdom-dev ***" 73 | cd ~/ros_catkin_ws/external_src 74 | sudo apt-get -y install libboost-test-dev libtinyxml-dev 75 | sudo apt-get -y source -b liburdfdom-dev 76 | sudo dpkg -i liburdfdom*.deb 77 | 78 | echo "*** Install libopencv-dev ***" 79 | cd ~/ros_catkin_ws/external_src 80 | git clone https://github.com/Itseez/opencv.git 81 | mkdir opencv/release 82 | cd opencv/release 83 | cmake -D CMAKE_BUILD_TYPE=RELEASE -D ENABLE_PRECOMPILED_HEADERS=OFF -D WITH_LIBV4L=ON -D WITH_V4L=ON -D CMAKE_INSTALL_PREFIX=/usr/local .. 84 | # cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local .. 85 | make 86 | sudo checkinstall -y --pkgname libopencv-dev make install 87 | # < MAVROS extras deps 88 | ############### 89 | read -p "Opencv done. Press any key to continue... " -n1 -s 90 | 91 | echo "*** rosdep install - Errors at the end are normal ***" 92 | cd ~/ros_catkin_ws 93 | # Python errors after the following command are normal. 94 | rosdep install --from-paths src --ignore-src --rosdistro indigo -y -r --os=debian:wheezy 95 | 96 | echo "*** Install catkin_tools ***" 97 | sudo apt-get install --reinstall python-setuptools 98 | sudo pip install --upgrade setuptools 99 | sudo pip install -U catkin_tools 100 | 101 | read -p "catkin_tools done. Press any key to continue... " -n1 -s 102 | 103 | # echo "*** Upgrading python imports. For some reason we need to do these when using mavros_extras? ***" 104 | # sudo pip install future 105 | # sudo apt-get install libxml2-dev libxslt1-dev 106 | # echo "*** This will take a while ***" 107 | # sudo pip install --upgrade lxml 108 | 109 | echo “******************************************************************” 110 | echo “About to start some heavy building. Go have a looong coffee break.” 111 | echo “******************************************************************” 112 | 113 | echo "*** Building ROS ***" 114 | catkin config --install-space /home/ros/indigo 115 | catkin config --install 116 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 117 | sudo catkin build 118 | # sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /home/ros/indigo 119 | 120 | read -p "ROS built. Press any key to continue... " -n1 -s 121 | 122 | sudo ln -sf /home/ros /opt/ 123 | 124 | echo "*** Updating .profile and .bashrc ***" 125 | echo "source /home/ros/indigo/setup.bash" >> ~/.profile 126 | source ~/.profile 127 | 128 | echo "source ~/ros_catkin_ws/devel/setup.bash" >> ~/.bashrc 129 | source ~/.bashrc 130 | 131 | cd ~/ros_catkin_ws 132 | 133 | echo "" 134 | echo "*** FINISHED! ***" 135 | 136 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/intel-edison/update_packages.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # 3 | # Updating packages from a source installation 4 | # 5 | # Following instructions from: 6 | # http://answers.ros.org/question/109656/maintaining-a-ros-source-install-add-packages-update-release/ 7 | # 8 | 9 | # update tools 10 | sudo apt-get -y upgrade 11 | # Note: pip could complain about mission files. if these are broken symlinks just delete them and retry. 12 | # see: https://github.com/pypa/pip/issues/2438 13 | sudo pip install --upgrade catkin-pkg rosinstall_generator wstool rosinstall 14 | 15 | cd ~/ros_catkin_ws 16 | # backup old package pointers 17 | mv indigo-ros_comm-wet.rosinstall indigo-ros_comm-wet.rosinstall_old 18 | # fetch latest package pointers 19 | rosinstall_generator ros_comm mavros --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo-ros_comm-wet.rosinstall 20 | # check which packages have changed 21 | diff indigo-ros_comm-wet.rosinstall indigo-ros_comm-wet.rosinstall_old 22 | 23 | # update .rosinstall 24 | sudo wstool merge -t src indigo-ros_comm-wet.rosinstall 25 | # fetch sources 26 | while [ $? != 0 ]; do 27 | echo "*** wstool - download failures, retrying ***" 28 | sudo wstool up -t src -j1 --delete-changed-uris 29 | done 30 | 31 | # update dependencies 32 | rosdep update 33 | rosdep install --from-paths src --ignore-src --rosdistro indigo -y -r --os=debian:wheezy 34 | 35 | # remove previously built artifacts 36 | # Note: this seemed to be required in my case to rebuild everything correctly, 37 | # else I encountered various issues with dependent libraries (at compile time and at runtime) 38 | mv build_isolated build_isolated_old 39 | mv devel_isolated devel_isolated_old 40 | 41 | # start compilation and have a beer 42 | sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -DMAVLINK_DIALECT=pixhawk --install-space /home/ros/indigo 43 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-14/setup-install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # For automated install, set permissions to avoid sudo/passwd. On standalone VM, run sudo visudo and add the following line to your sudoers file (or use sudo visudo to enter the editor): 3 | # Defaults !tty_tickets 4 | 5 | export DEBIAN_FRONTEND=noninteractive 6 | sudo usermod -a -G dialout $USER 7 | 8 | sudo add-apt-repository ppa:george-edison55/cmake-3.x -y 9 | sudo apt-get update 10 | sudo apt-get -q -y install cmake vim -y 11 | sudo apt-get -q -y install ant protobuf-compiler libeigen3-dev libopencv-dev 12 | sudo apt-get -q -y install python-argparse git-core wget zip python-empy qtcreator cmake build-essential genromfs -y 13 | sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' 14 | wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - 15 | sudo apt-get update 16 | sudo apt-get install gazebo6 17 | sudo apt-get install libgazebo6-dev 18 | 19 | echo "export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo/Build" >> ~/.bashrc 20 | echo "export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo/models" >> ~/.bashrc 21 | echo "export GAZEBO_RESOURCE_PATH=${GAZEBO_MODEL_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo/media" >> ~/.bashrc 22 | 23 | sudo apt-get remove -y gcc-arm-none-eabi gdb-arm-none-eabi binutils-arm-none-eabi 24 | sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa 25 | sudo apt-get update 26 | sudo apt-get -q -y install python-serial openocd flex bison libncurses5-dev autoconf texinfo build-essential \ 27 | libftdi-dev libtool zlib1g-dev \ 28 | python-empy gcc-arm-embedded -y 29 | 30 | cd ~ 31 | mkdir src 32 | cd src 33 | git clone https://github.com/darknight-007/Firmware 34 | cd Firmware 35 | git submodule update --init --recursive 36 | cd Tools/sitl_gazebo 37 | git pull origin master 38 | cd ~ 39 | echo "now type the following at the command prompt:" 40 | echo "cd ~/src/Firmware/ && make posix_sitl_default gazebo" 41 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-14/setup-mavlink-mavros.sh: -------------------------------------------------------------------------------- 1 | mkdir -p ~/catkin_ws/src 2 | cd ~/catkin_ws 3 | source /opt/ros/jade/setup.bash 4 | catkin init 5 | sudo apt-get -y install python-wstool python-rosinstall-generator python-catkin-tools 6 | wstool init ~/catkin_ws/src 7 | rosinstall_generator --rosdistro jade --upstream-development mavros | tee /tmp/mavros.rosinstall 8 | rosinstall_generator --rosdistro jade mavlink | tee -a /tmp/mavros.rosinstall 9 | wstool merge -t src /tmp/mavros.rosinstall 10 | wstool update -t src 11 | rosdep install --from-paths src --ignore-src --rosdistro jade -y 12 | cd ~/catkin_ws/src/ 13 | ln -s ~/src/Firmware/ 14 | ln -s ~/src/Firmware/Tools/sitl_gazebo/ 15 | cd .. 16 | catkin build 17 | echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc 18 | 19 | 20 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-14/setup-ros.sh: -------------------------------------------------------------------------------- 1 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 2 | sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 0xB01FA116 3 | sudo apt-get update 4 | sudo apt-get -y install ros-jade-desktop 5 | sudo apt-get -y install ros-jade-gazebo6-* 6 | sudo rosdep init 7 | rosdep update 8 | echo "source /opt/ros/jade/setup.bash" >> ~/.bashrc 9 | source ~/.bashrc 10 | 11 | 12 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-16/GeographicLib-1.48.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Open-UAV/openuav-playground/b6faaad967479a8e14b7646296fdc7f4830d3fbf/openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-16/GeographicLib-1.48.tar.gz -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-16/README.txt: -------------------------------------------------------------------------------- 1 | Starting with a clean Ubuntu install: 2 | 1. sudo apt-get update && apt-get install -y git 3 | 2. git clone https://github.com/schmittlema/ros-setups.git 4 | 3. cd ros-setups/ubuntu-16 5 | 4. ./setup-ros.sh 6 | 5. ./setup-mavlink-mavros.sh 7 | 6. ./setup-install.sh 8 | 7. ./setup-gym-gzweb-uavnav.sh 9 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-16/setup-gym-gzweb-uavnav.sh: -------------------------------------------------------------------------------- 1 | apt-get install -y python-pip 2 | cd ~ && git clone https://github.com/openai/gym 3 | cd gym && pip install -e . # minimal install 4 | pip install opencv-python 5 | 6 | apt-get install -y python-rospy python-genpy 7 | 8 | apt-get purge -y nodejs 9 | apt-get purge -y node 10 | apt-get install -y wget build-essential \ 11 | && wget http://nodejs.org/dist/v0.10.25/node-v0.10.25.tar.gz \ 12 | && tar -xvf node-v0.10.25.tar.gz \ 13 | && cd node-v0.10.25 \ 14 | && ./configure && make && make install 15 | 16 | curl -sL https://deb.nodesource.com/setup | bash - \ 17 | && apt-get update && apt-get upgrade -q -y && apt-get install -q -y \ 18 | build-essential \ 19 | cmake \ 20 | imagemagick \ 21 | libboost-all-dev \ 22 | libgts-dev \ 23 | libjansson-dev \ 24 | libtinyxml-dev \ 25 | mercurial \ 26 | npm \ 27 | pkg-config \ 28 | psmisc\ 29 | sed \ 30 | tar \ 31 | && rm -rf /var/lib/apt/lists/* 32 | 33 | # install gazebo packages 34 | apt-get install -q -y \ 35 | libgazebo7-dev \ 36 | && rm -rf /var/lib/apt/lists/* 37 | 38 | cd ~; hg clone https://bitbucket.org/osrf/gzweb ~/gzweb 39 | cd ~/gzweb \ 40 | && hg up default \ 41 | 42 | mkdir /root/gzweb/http/client/assets 43 | cd /root/gzweb && bash -c ". /usr/share/gazebo/setup.sh; ./deploy.sh -m local" 44 | cd /ros-setups/ubuntu-16 && tar xfpz GeographicLib-1.48.tar.gz && cd GeographicLib-1.48 && mkdir BUILD && cd BUILD && ../configure && cd .. && make && make install PREFIX=/usr/shar/GeographicLib 45 | 46 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-16/setup-install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # For automated install, set permissions to avoid sudo/passwd. On standalone VM, run sudo visudo and add the following line to your sudoers file (or use sudo visudo to enter the editor): 3 | # Defaults !tty_tickets 4 | 5 | export DEBIAN_FRONTEND=noninteractive 6 | usermod -a -G dialout $USER 7 | apt-get install software-properties-common python-software-properties 8 | add-apt-repository ppa:george-edison55/cmake-3.x -y 9 | apt-get update && apt-get upgrade -y 10 | apt-get -q -y install cmake vim -y 11 | apt-get -q -y install ant protobuf-compiler libeigen3-dev libopencv-dev 12 | apt-get -q -y install python-argparse git-core wget zip python-empy qtcreator cmake build-essential genromfs -y 13 | sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' 14 | wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - 15 | apt-get update && apt-get upgrade -y 16 | # sudo apt-get install libignition-common-dev 17 | apt-get -y install gazebo7 18 | apt-get -y install libgazebo7-dev 19 | 20 | echo "export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo/Build" >> ~/.bashrc 21 | echo "export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo/models" >> ~/.bashrc 22 | echo "export GAZEBO_RESOURCE_PATH=${GAZEBO_MODEL_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo" >> ~/.bashrc 23 | apt-get remove -y gcc-arm-none-eabi gdb-arm-none-eabi binutils-arm-none-eabi 24 | add-apt-repository ppa:team-gcc-arm-embedded/ppa 25 | apt-get update -y 26 | apt-get -q -y install python-serial openocd flex bison libncurses5-dev autoconf texinfo build-essential \ 27 | libftdi-dev libtool zlib1g-dev \ 28 | python-empy gcc-arm-embedded 29 | 30 | cd ~ 31 | mkdir src 32 | cd src 33 | git clone https://github.com/darknight-007/Firmware 34 | cd Firmware 35 | git submodule update --init --recursive -f 36 | cd Tools/sitl_gazebo 37 | git pull origin master 38 | cd ~/src/Firmware/ 39 | make posix_sitl_default 40 | 41 | apt-get install libignition-math2-dev 42 | /bin/bash -c ". /opt/ros/kinetic/setup.bash; . ~/.bashrc ; catkin build" 43 | 44 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-16/setup-mavlink-mavros.sh: -------------------------------------------------------------------------------- 1 | mkdir -p ~/catkin_ws/src 2 | cd ~/catkin_ws 3 | apt-get -y install python-wstool python-rosinstall-generator python-catkin-tools 4 | wstool init ~/catkin_ws/src 5 | rosinstall_generator --rosdistro kinetic --upstream-development mavros | tee /tmp/mavros.rosinstall 6 | rosinstall_generator --rosdistro kinetic mavlink | tee -a /tmp/mavros.rosinstall 7 | wstool merge -t src /tmp/mavros.rosinstall 8 | wstool update -t src 9 | rosdep install --from-paths src --ignore-src --rosdistro kinetic -y --as-root apt:false 10 | cd ~/catkin_ws/src/ 11 | ln -s ~/src/Firmware/ 12 | ln -s ~/src/Firmware/Tools/sitl_gazebo/ 13 | cd .. 14 | /bin/bash -c ". /opt/ros/kinetic/setup.bash; catkin build; . ~/catkin_ws/devel/setup.bash" 15 | echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc 16 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/ros-setups/ubuntu-16/setup-ros.sh: -------------------------------------------------------------------------------- 1 | export DEBIAN_FRONTEND=noninteractive 2 | apt-get update && apt-get install -y lsb-core 3 | sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 4 | apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 0xB01FA116 5 | apt-get update -y 6 | apt-get -y install ros-kinetic-desktop 7 | apt-get -y install ros-kinetic-gazebo-ros-pkgs 8 | rosdep init 9 | rosdep update 10 | echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc 11 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/openuav_sample/setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #Django start server 4 | python3 /django/manage.py makemigrations &> /dev/null 5 | python3 /django/manage.py migrate &> /dev/null 6 | cd /django/ && /usr/bin/uwsgi_python35 --http-socket :31819 -T -p 4 --module DjangoProject.wsgi &> /tmp/a & 7 | #Django start server 8 | 9 | #cd /wetty/wetty 10 | #node app.js -p 3000 &> /dev/null & 11 | #cd 12 | 13 | ## Previous clean-up 14 | rm -rf /root/src/Firmware/Tools/sitl_gazebo/models/f450-tmp-* 15 | rm -f /root/src/Firmware/posix-configs/SITL/init/lpe/f450-tmp-* 16 | rm -f /root/src/Firmware/launch/posix_sitl_multi_tmp.launch 17 | 18 | #chmod -R 777 /root/src && chmod -R 777 /root/catkin_ws 19 | echo "source /root/catkin_ws/devel/setup.bash" >> /home/.profile 20 | 21 | ##################### 22 | ##################### 23 | ## Run user script ## 24 | ##################### 25 | ##################### 26 | 27 | #su term 28 | /simulation/run_this.sh 29 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/postgres/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM postgres 2 | ADD init.sql /docker-entrypoint-initdb.d/ 3 | -------------------------------------------------------------------------------- /openuavapp/dockerfiles/postgres/init.sql: -------------------------------------------------------------------------------- 1 | CREATE DATABASE "agdss"; 2 | CREATE USER aguser WITH PASSWORD 'aguser'; 3 | GRANT ALL PRIVILEGES ON DATABASE "agdss" to aguser; -------------------------------------------------------------------------------- /openuavapp/manage.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | import sys 4 | 5 | if __name__ == "__main__": 6 | os.environ.setdefault("DJANGO_SETTINGS_MODULE", "openuav.settings") 7 | try: 8 | from django.core.management import execute_from_command_line 9 | except ImportError as exc: 10 | raise ImportError( 11 | "Couldn't import Django. Are you sure it's installed and " 12 | "available on your PYTHONPATH environment variable? Did you " 13 | "forget to activate a virtual environment?" 14 | ) from exc 15 | execute_from_command_line(sys.argv) 16 | -------------------------------------------------------------------------------- /openuavapp/nvidia-docker-compose.yml: -------------------------------------------------------------------------------- 1 | networks: 2 | default: 3 | ipam: 4 | config: 5 | - gateway: 172.28.0.1 6 | subnet: 172.28.0.0/16 7 | driver: default 8 | services: 9 | db: 10 | build: dockerfiles/postgres/ 11 | devices: 12 | - /dev/nvidia0 13 | - /dev/nvidia1 14 | - /dev/nvidiactl 15 | - /dev/nvidia-uvm 16 | - /dev/nvidia-uvm-tools 17 | volumes: 18 | - nvidia_driver_390.30:/usr/local/nvidia:ro 19 | openuav: 20 | build: dockerfiles/openuav_sample/ 21 | devices: 22 | - /dev/nvidia0 23 | - /dev/nvidia1 24 | - /dev/nvidiactl 25 | - /dev/nvidia-uvm 26 | - /dev/nvidia-uvm-tools 27 | entrypoint: /home/setup.sh 28 | environment: 29 | - DISPLAY=$DISPLAY 30 | expose: 31 | - '8000' 32 | - '31819' 33 | - '80' 34 | - '9090' 35 | volumes: 36 | - ../samples/testSimulation:/simulation 37 | - /tmp/.X11-unix:/tmp/.X11-unix 38 | - nvidia_driver_390.30:/usr/local/nvidia:ro 39 | web: 40 | build: dockerfiles/django/ 41 | command: bash -c "while ! pg_isready -h db ; do sleep 1 ; done && python manage.py 42 | makemigrations && python manage.py migrate && uwsgi --http-socket :80 -T -p 43 | 5 --module openuav.wsgi" 44 | depends_on: 45 | - db 46 | devices: 47 | - /dev/nvidia0 48 | - /dev/nvidia1 49 | - /dev/nvidiactl 50 | - /dev/nvidia-uvm 51 | - /dev/nvidia-uvm-tools 52 | ports: 53 | - 8001:80 54 | restart: always 55 | volumes: 56 | - .:/code 57 | - nvidia_driver_390.30:/usr/local/nvidia:ro 58 | version: '2' 59 | volumes: 60 | nvidia_driver_390.30: 61 | external: true 62 | -------------------------------------------------------------------------------- /openuavapp/openuav/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Open-UAV/openuav-playground/b6faaad967479a8e14b7646296fdc7f4830d3fbf/openuavapp/openuav/__init__.py -------------------------------------------------------------------------------- /openuavapp/openuav/settings.py: -------------------------------------------------------------------------------- 1 | """ 2 | Django settings for openuav project. 3 | 4 | Generated by 'django-admin startproject' using Django 2.0. 5 | 6 | For more information on this file, see 7 | https://docs.djangoproject.com/en/2.0/topics/settings/ 8 | 9 | For the full list of settings and their values, see 10 | https://docs.djangoproject.com/en/2.0/ref/settings/ 11 | """ 12 | 13 | import os 14 | 15 | # Build paths inside the project like this: os.path.join(BASE_DIR, ...) 16 | BASE_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 17 | 18 | 19 | # Quick-start development settings - unsuitable for production 20 | # See https://docs.djangoproject.com/en/2.0/howto/deployment/checklist/ 21 | 22 | # SECURITY WARNING: keep the secret key used in production secret! 23 | SECRET_KEY = 'wm+&r^9q(*dqdo3k2ynl(1of7-cs011#x3aqn*byzufx^72bd7' 24 | 25 | # SECURITY WARNING: don't run with debug turned on in production! 26 | DEBUG = False 27 | 28 | ALLOWED_HOSTS = ['*'] 29 | 30 | 31 | # Application definition 32 | 33 | INSTALLED_APPS = [ 34 | 'sim.apps.SimConfig', 35 | 'django.contrib.admin', 36 | 'django.contrib.auth', 37 | 'django.contrib.contenttypes', 38 | 'django.contrib.sessions', 39 | 'django.contrib.messages', 40 | 'django.contrib.staticfiles', 41 | ] 42 | 43 | MIDDLEWARE = [ 44 | 'django.middleware.security.SecurityMiddleware', 45 | 'django.contrib.sessions.middleware.SessionMiddleware', 46 | 'django.middleware.common.CommonMiddleware', 47 | 'django.middleware.csrf.CsrfViewMiddleware', 48 | 'django.contrib.auth.middleware.AuthenticationMiddleware', 49 | 'django.contrib.messages.middleware.MessageMiddleware', 50 | 'django.middleware.clickjacking.XFrameOptionsMiddleware', 51 | ] 52 | 53 | ROOT_URLCONF = 'openuav.urls' 54 | 55 | TEMPLATES = [ 56 | { 57 | 'BACKEND': 'django.template.backends.django.DjangoTemplates', 58 | 'DIRS': [], 59 | 'APP_DIRS': True, 60 | 'OPTIONS': { 61 | 'context_processors': [ 62 | 'django.template.context_processors.debug', 63 | 'django.template.context_processors.request', 64 | 'django.contrib.auth.context_processors.auth', 65 | 'django.contrib.messages.context_processors.messages', 66 | ], 67 | }, 68 | }, 69 | ] 70 | 71 | WSGI_APPLICATION = 'openuav.wsgi.application' 72 | 73 | 74 | # Database 75 | # https://docs.djangoproject.com/en/2.0/ref/settings/#databases 76 | 77 | DATABASES = { 78 | 'default': { 79 | 'ENGINE': 'django.db.backends.postgresql', 80 | 'NAME': 'postgres', 81 | 'USER': 'postgres', 82 | 'HOST': 'db', 83 | 'PORT': 5432, 84 | } 85 | } 86 | 87 | 88 | # Password validation 89 | # https://docs.djangoproject.com/en/2.0/ref/settings/#auth-password-validators 90 | 91 | AUTH_PASSWORD_VALIDATORS = [ 92 | { 93 | 'NAME': 'django.contrib.auth.password_validation.UserAttributeSimilarityValidator', 94 | }, 95 | { 96 | 'NAME': 'django.contrib.auth.password_validation.MinimumLengthValidator', 97 | }, 98 | { 99 | 'NAME': 'django.contrib.auth.password_validation.CommonPasswordValidator', 100 | }, 101 | { 102 | 'NAME': 'django.contrib.auth.password_validation.NumericPasswordValidator', 103 | }, 104 | ] 105 | 106 | 107 | # Internationalization 108 | # https://docs.djangoproject.com/en/2.0/topics/i18n/ 109 | 110 | LANGUAGE_CODE = 'en-us' 111 | 112 | TIME_ZONE = 'UTC' 113 | 114 | USE_I18N = True 115 | 116 | USE_L10N = True 117 | 118 | USE_TZ = True 119 | 120 | 121 | # Static files (CSS, JavaScript, Images) 122 | # https://docs.djangoproject.com/en/2.0/howto/static-files/ 123 | 124 | STATIC_URL = '/static/' 125 | -------------------------------------------------------------------------------- /openuavapp/openuav/urls.py: -------------------------------------------------------------------------------- 1 | """openuav URL Configuration 2 | 3 | The `urlpatterns` list routes URLs to views. For more information please see: 4 | https://docs.djangoproject.com/en/2.0/topics/http/urls/ 5 | Examples: 6 | Function views 7 | 1. Add an import: from my_app import views 8 | 2. Add a URL to urlpatterns: path('', views.home, name='home') 9 | Class-based views 10 | 1. Add an import: from other_app.views import Home 11 | 2. Add a URL to urlpatterns: path('', Home.as_view(), name='home') 12 | Including another URLconf 13 | 1. Import the include() function: from django.urls import include, path 14 | 2. Add a URL to urlpatterns: path('blog/', include('blog.urls')) 15 | """ 16 | from django.contrib import admin 17 | from django.urls import path, include 18 | 19 | urlpatterns = [ 20 | path('sim/', include('sim.urls')), 21 | path('admin/', admin.site.urls), 22 | ] 23 | -------------------------------------------------------------------------------- /openuavapp/openuav/wsgi.py: -------------------------------------------------------------------------------- 1 | """ 2 | WSGI config for openuav project. 3 | 4 | It exposes the WSGI callable as a module-level variable named ``application``. 5 | 6 | For more information on this file, see 7 | https://docs.djangoproject.com/en/2.0/howto/deployment/wsgi/ 8 | """ 9 | 10 | import os 11 | 12 | from django.core.wsgi import get_wsgi_application 13 | 14 | os.environ.setdefault("DJANGO_SETTINGS_MODULE", "openuav.settings") 15 | 16 | application = get_wsgi_application() 17 | -------------------------------------------------------------------------------- /openuavapp/sim/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Open-UAV/openuav-playground/b6faaad967479a8e14b7646296fdc7f4830d3fbf/openuavapp/sim/__init__.py -------------------------------------------------------------------------------- /openuavapp/sim/admin.py: -------------------------------------------------------------------------------- 1 | from django.contrib import admin 2 | 3 | # Register your models here. 4 | -------------------------------------------------------------------------------- /openuavapp/sim/apps.py: -------------------------------------------------------------------------------- 1 | from django.apps import AppConfig 2 | 3 | 4 | class SimConfig(AppConfig): 5 | name = 'sim' 6 | -------------------------------------------------------------------------------- /openuavapp/sim/exceptions.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class NoContainerExc(Exception): 4 | """If the container is not running""" 5 | 6 | class ContainerInformationFetchExc(Exception): 7 | """If the server is unable to fetch the data from the simulation container""" 8 | 9 | class NoUserIDExc(Exception): 10 | """If the User id is not present in the request""" 11 | 12 | class InvalidIPExc(Exception): 13 | """The IP format is incorrect""" -------------------------------------------------------------------------------- /openuavapp/sim/migrations/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Open-UAV/openuav-playground/b6faaad967479a8e14b7646296fdc7f4830d3fbf/openuavapp/sim/migrations/__init__.py -------------------------------------------------------------------------------- /openuavapp/sim/models.py: -------------------------------------------------------------------------------- 1 | from django.db import models 2 | 3 | # Create your models here. 4 | -------------------------------------------------------------------------------- /openuavapp/sim/static/sim/loading.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Open-UAV/openuav-playground/b6faaad967479a8e14b7646296fdc7f4830d3fbf/openuavapp/sim/static/sim/loading.gif -------------------------------------------------------------------------------- /openuavapp/sim/static/sim/loading2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Open-UAV/openuav-playground/b6faaad967479a8e14b7646296fdc7f4830d3fbf/openuavapp/sim/static/sim/loading2.gif -------------------------------------------------------------------------------- /openuavapp/sim/templates/sim/dev_console_second.html: -------------------------------------------------------------------------------- 1 |
2 |
3 |
4 | 5 | {% for _ in range %} 6 | 7 | 8 | 9 | 12 | {% endfor %} 13 | 14 | 15 |
nan
16 |
17 | {% for _ in range %} 18 |
nan
19 |
20 | {% endfor %} 21 |
22 |
23 | 24 |
25 |
26 | 27 | {% for _ in range %} 28 |
29 |
30 | 31 |
32 |
33 | 34 |
35 |
36 | {% endfor %} 37 |
38 | 39 | 128 | -------------------------------------------------------------------------------- /openuavapp/sim/templates/sim/dev_console_unsecure_second.html: -------------------------------------------------------------------------------- 1 |
2 |
3 |
4 | 5 | {% for _ in range %} 6 | 7 | 8 | 9 | 12 | {% endfor %} 13 | 14 | 15 |
nan
16 |
17 | {% for _ in range %} 18 |
nan
19 |
20 | {% endfor %} 21 |
22 |
23 | 24 |
25 |
26 | 27 | {% for _ in range %} 28 |
29 |
30 | 31 |
32 |
33 | 34 |
35 |
36 | {% endfor %} 37 |
38 | 39 | -------------------------------------------------------------------------------- /openuavapp/sim/templates/sim/error.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | UAV Simulation 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 |
30 |
31 |

{{error}}

32 |
33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /openuavapp/sim/tests.py: -------------------------------------------------------------------------------- 1 | from django.test import TestCase 2 | 3 | # Create your tests here. 4 | -------------------------------------------------------------------------------- /openuavapp/sim/urls.py: -------------------------------------------------------------------------------- 1 | from django.urls import path 2 | 3 | from . import views 4 | 5 | urlpatterns = [ 6 | path('console/', views.console, name='console'), 7 | path('console1/', views.console1, name='console1'), 8 | path('console2/', views.console2, name='console2'), 9 | path('unsecure_console/', views.unsecure_console, name='unsecure_console'), 10 | path('unsecure_console1/', views.unsecure_console1, name='unsecure_console1'), 11 | path('unsecure_console2/', views.unsecure_console2, name='unsecure_console2'), 12 | path('debugStmts/', views.debugStmts, name='debugStmts'), 13 | path('unsecure_debugStmts/', views.unsecure_debugStmts, name='unsecure_debugStmts'), 14 | path('', views.index, name='index'), 15 | ] 16 | -------------------------------------------------------------------------------- /samples/.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | 3 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/measures/measureInterRobotDistance.py: -------------------------------------------------------------------------------- 1 | """ 2 | testing performance measure for leader-follower 3 | """ 4 | 5 | import rospy 6 | from std_msgs.msg import Float64 7 | from geometry_msgs.msg import PoseStamped 8 | import sys 9 | import math 10 | 11 | 12 | class MeasureInterRobotDistance: 13 | curr_pose = PoseStamped() 14 | measure = 0 15 | leader_pose = PoseStamped() 16 | 17 | def follower_cb(self, msg): 18 | self.curr_pose = msg 19 | 20 | def leader_cb(self, msg): 21 | self.leader_pose = msg 22 | 23 | def measureDistance(self): 24 | follower_x = self.curr_pose.pose.position.x 25 | follower_y = self.curr_pose.pose.position.y 26 | follower_z = self.curr_pose.pose.position.z 27 | 28 | leader_x = self.leader_pose.pose.position.x 29 | leader_y = self.leader_pose.pose.position.y 30 | leader_z = self.leader_pose.pose.position.z 31 | 32 | return math.sqrt((follower_x - leader_x) * (follower_x - leader_x) + (follower_y - leader_y) * (follower_y - leader_y)) 33 | 34 | # + (follower_z - leader_z) * (follower_z - leader_z)) 35 | 36 | 37 | def __init__(self, this_uav, leader_uav): 38 | rospy.init_node('measure', anonymous=True) 39 | self.measure_pub = rospy.Publisher('/measure', Float64, queue_size=10) 40 | rospy.Subscriber('/mavros' + leader_uav + '/local_position/pose', PoseStamped, callback=self.leader_cb) 41 | rospy.Subscriber('/mavros' + this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 42 | 43 | rate = rospy.Rate(10) # Hz 44 | rate.sleep() 45 | 46 | while not rospy.is_shutdown(): 47 | self.measure = self.measureDistance() 48 | self.measure_pub.publish(self.measure) 49 | rate.sleep() 50 | 51 | if __name__ == "__main__": 52 | MeasureInterRobotDistance(sys.argv[1], sys.argv[2]) 53 | 54 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/measures/score.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/score.out 3 | cat /simulation/outputs/score.out -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/models/f450-1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | UPenn F450 4 | 1.0 5 | f450-1.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/parameters/swarm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /root/.bashrc 3 | num_uavs=2 4 | duration_seconds=3000 5 | FOLLOW_D_GAIN=1 6 | LOOP_EDGE=40 7 | DIST_THRESHOLD=4 8 | ALTITUDE=20 9 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/setup/gen_gazebo_ros_spawn.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_gazebo_ros' + uav_str + '.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | 57 | 58 | uav_block = '' + \ 59 | '\n' + \ 60 | '\n' + \ 61 | '\n' + \ 63 | '\n' + \ 65 | '\n' 70 | file_block = uav_block 71 | 72 | print(file_block) 73 | print(os.system("cp " + SOURCE + " " + DEST)) 74 | f=open(DEST,"a") 75 | f.write(file_block + '\n ') 76 | f.close() 77 | 78 | print('DRONES CREATED') 79 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/setup/gen_mavros.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | 6 | def replaceInFile(orig, repl, filename): 7 | os.system('sed -i "s/' + orig + '/' + repl + '/g" ' + filename) 8 | 9 | 10 | NUM_UAVs = int(sys.argv[1]) + 1 11 | PX4_HOME = '/root/src' 12 | print(NUM_UAVs) 13 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 14 | file_block = '' 15 | 16 | for NUM in range(1, NUM_UAVs): 17 | # mavlink 18 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 19 | # simulator start -s -u simulator_udp_port 20 | # mavlink start -u mavlink_start_port -r 4000000 21 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 22 | # 23 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 24 | 25 | mavlink_onboard_remote = 14640 26 | mavlink_start_port = 14656 27 | mavlink_onboard_local = 14657 28 | simulator_udp_port = 14660 29 | 30 | uav_str = str(NUM) 31 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_mavros'+uav_str +'.launch' 32 | 33 | print(uav_str) 34 | print(os.system( 35 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 36 | print(os.system( 37 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-tmp-" + uav_str + ".sdf")) 38 | 39 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 40 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 41 | os.system( 42 | 'cp ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | 44 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 45 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port + 100 * NUM), 47 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local + 100 * NUM), 49 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 50 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote + 100 * NUM), 51 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 52 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), 53 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 54 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), 55 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 56 | 57 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, 58 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 59 | replaceInFile('uav_camera', 60 | 'uav_' + uav_str + '_camera', 61 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 62 | 63 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 64 | 65 | uav_block = '' + \ 66 | '\n' + \ 67 | '\n' + \ 68 | '\n' + \ 71 | '\n' 73 | 74 | mavros_block = ' \ 75 | \ 77 | \ 78 | \ 79 | \ 80 | \ 81 | \ 82 | \ 83 | ' 84 | 85 | file_block = uav_block + '\n' + mavros_block 86 | print(file_block) 87 | 88 | print(os.system("cp " + SOURCE + " " + DEST)) 89 | f = open(DEST, "a") 90 | f.write(file_block + '\n ') 91 | f.close() 92 | 93 | print('DRONES CREATED') 94 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/setup/gen_px4_sitl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_px4_sitl'+ uav_str +'.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | uav_block = '' + \ 57 | '\n' + \ 58 | '\n' + \ 59 | '\n' + \ 62 | '\n' + \ 64 | '\n' 66 | 67 | 68 | file_block = uav_block + '\n' 69 | print(file_block) 70 | 71 | print(os.system("cp " + SOURCE + " " + DEST)) 72 | f=open(DEST,"a") 73 | f.write(file_block + '\n ') 74 | f.close() 75 | 76 | print('DRONES CREATED') 77 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/setup/posix_sitl_openuav_swarm_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/setup/testArmAll.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import subprocess 3 | import os 4 | import sys 5 | from std_msgs.msg import Float64; 6 | 7 | from mavros_msgs.srv import CommandBool, CommandTOL, SetMode 8 | from geometry_msgs.msg import PoseStamped,Pose,Vector3,Twist,TwistStamped 9 | from std_srvs.srv import Empty 10 | import time 11 | 12 | print 'init' 13 | 14 | cur_pose = PoseStamped() 15 | 16 | def pos_cb(msg): 17 | global cur_pose 18 | cur_pose = msg 19 | 20 | NUM_UAV = int(sys.argv[1]) 21 | 22 | mode_proxy = [None for i in range(NUM_UAV)] 23 | arm_proxy = [None for i in range(NUM_UAV)] 24 | 25 | def mavrosTopicStringRoot(uavID=0): 26 | print 'mavros' + str(uavID+1) 27 | return ('mavros' + str(uavID+1)) 28 | 29 | rospy.init_node('multi', anonymous=True) 30 | 31 | #Comm for drones 32 | for uavID in range(0,NUM_UAV): 33 | mode_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/set_mode', SetMode) 34 | arm_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool) 35 | 36 | print 'communication initialization complete' 37 | data = [None for i in range(NUM_UAV)] 38 | 39 | while None in data: 40 | for uavID in range(0, NUM_UAV): 41 | try: 42 | data[uavID] = rospy.wait_for_message(mavrosTopicStringRoot(uavID) + '/global_position/rel_alt', Float64, timeout=5) 43 | except: 44 | pass 45 | 46 | for uavID in range(0, NUM_UAV): 47 | print "wait for service" 48 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/set_mode') 49 | print "got service" 50 | 51 | rate = rospy.Rate(10) 52 | 53 | while not rospy.is_shutdown(): 54 | success = [None for i in range(NUM_UAV)] 55 | for uavID in range(0, NUM_UAV): 56 | try: 57 | success[uavID] = mode_proxy[uavID](1,'OFFBOARD') 58 | except rospy.ServiceException, e: 59 | print ("mavros/set_mode service call failed: %s"%e) 60 | 61 | success = [None for i in range(NUM_UAV)] 62 | for uavID in range(0, NUM_UAV): 63 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/cmd/arming') 64 | 65 | for uavID in range(0, NUM_UAV): 66 | try: 67 | success[uavID] = arm_proxy[uavID](True) 68 | except rospy.ServiceException, e: 69 | print ("mavros1/set_mode service call failed: %s"%e) 70 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/inputs/setup/testCreateUAVSwarm.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import rospy 3 | import sys 4 | import os 5 | from std_msgs.msg import Float64 6 | from geometry_msgs.msg import PoseStamped 7 | import roslaunch 8 | 9 | 10 | cur_pose = PoseStamped() 11 | NUM_UAV = sys.argv[1] 12 | process = subprocess.Popen(["/bin/bash","/root/src/Firmware/Tools/swarm.sh",NUM_UAV],stdout=subprocess.PIPE) 13 | process.wait() 14 | for line in process.stdout: 15 | print line 16 | 17 | launchfile = "posix_sitl_multi_tmp.launch" 18 | fullpath = os.path.join("/root/src/Firmware/launch/", launchfile) 19 | subprocess.Popen(["roslaunch",fullpath]) 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /samples/dronekit-mavros/run_this.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #!/bin/bash 4 | 5 | echo "++++++++INIT++++++++++" 6 | 7 | source /simulation/inputs/parameters/swarm.sh 8 | source /opt/ros/kinetic/setup.bash 9 | source ~/catkin_ws/devel/setup.bash 10 | 11 | 12 | ## Previous clean-up 13 | rm -rf /root/src/Firmware/Tools/sitl_gazebo/models/f450-tmp-* 14 | rm -f /root/src/Firmware/posix-configs/SITL/init/lpe/f450-tmp-* 15 | rm -f /root/src/Firmware/launch/posix_sitl_multi_tmp.launch 16 | 17 | ## world setup # 18 | cp /simulation/inputs/world/empty.world /root/src/Firmware/Tools/sitl_gazebo/worlds/empty.world 19 | cp /simulation/inputs/models/f450-1/f450-1.sdf /root/src/Firmware/Tools/sitl_gazebo/models/f450-1/f450-1.sdf 20 | cp /simulation/inputs/setup/posix_sitl_openuav_swarm_base.launch /root/src/Firmware/launch/posix_sitl_openuav_swarm_base.launch 21 | 22 | mkdir /simulation/outputs 23 | rm -f /simulation/outputs/*.csv 24 | rm -f /simulation/outputs/*.txt 25 | echo "Setup..." #>> /tmp/debug 26 | 27 | 28 | python /simulation/inputs/setup/gen_gazebo_ros_spawn.py $num_uavs 29 | python /simulation/inputs/setup/gen_px4_sitl.py $num_uavs 30 | python /simulation/inputs/setup/gen_mavros.py $num_uavs 31 | 32 | for((i=1;i<=$num_uavs;i+=1)) 33 | do 34 | echo "px4 posix_sitl_multi_gazebo_ros$num_uavs.launch" 35 | echo "launching uav$i ..." >> /tmp/debug 36 | roslaunch px4 posix_sitl_multi_gazebo_ros$i.launch &> /dev/null & 37 | until rostopic echo /gazebo/model_states | grep -m1 f450-tmp-$i ; do : ; done 38 | roslaunch px4 posix_sitl_multi_px4_sitl$i.launch &> /dev/null & 39 | sleep 2 40 | roslaunch px4 posix_sitl_multi_mavros$i.launch &> /dev/null & 41 | until rostopic echo /mavros$i/state | grep -m1 "connected: True" ; do : ; done 42 | echo "launched uav$i ..." >> /tmp/debug 43 | done 44 | 45 | python /simulation/inputs/controllers/dostuff.py $num_uavs 1 "127.0.0.1:14740" &> /dev/null & 46 | 47 | python /simulation/inputs/measures/measureInterRobotDistance.py $num_uavs 1 &> /dev/null & 48 | rosrun web_video_server web_video_server _port:=80 _server_threads:=100 &> /dev/null & 49 | roslaunch rosbridge_server rosbridge_websocket.launch ssl:=false &> /dev/null & 50 | 51 | 52 | echo "Measures..." 53 | roslaunch opencv_apps general_contours.launch image:=/uav_2_camera_front/image_raw debug_view:=false &> /dev/null & 54 | 55 | for((i=1;i<=$num_uavs;i+=1)) 56 | do 57 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /mavros$i/local_position/odom > /simulation/outputs/uav$i.csv & 58 | done 59 | 60 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /measure > /simulation/outputs/measure.csv & 61 | sleep $duration_seconds 62 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/average_measure.txt 63 | 64 | -------------------------------------------------------------------------------- /samples/formation/inputs/controllers/pid.py: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | import numpy as np 4 | 5 | 6 | class PID: 7 | 8 | def __init__(self, p, i, d): 9 | self.Kp = p 10 | self.Kd = d 11 | self.Ki = i 12 | self.error = 0 #e(t) 13 | self.errorM1 = 0 #e(t-1) 14 | self.errorM2 = 0 #e(t-2) 15 | self.cmd = 0 #cmd(t) 16 | self.cmdM = 0 #cmd(t-1) 17 | def update(self, e): 18 | self.errorM2 = self.errorM1 19 | self.errorM1 = self.error 20 | self.error = e 21 | self.cmd = self.cmdM + (self.error)*self.Kp + (self.error - self.errorM1)*self.Kd + (self.error - 2*self.errorM1 + self.errorM2)*self.Ki 22 | self.cmdM = 0 #self.cmd 23 | return self.cmd 24 | -------------------------------------------------------------------------------- /samples/formation/inputs/controllers/sequencer.py: -------------------------------------------------------------------------------- 1 | """ 2 | Authors: Arjun Kumar and Jnaneshwar Das 3 | testing sequencer 4 | """ 5 | 6 | import rospy 7 | import subprocess 8 | import os 9 | import sys 10 | import math 11 | import tf 12 | import time 13 | import numpy as np 14 | 15 | 16 | from std_msgs.msg import Int8, Bool, Float64, Float64MultiArray, MultiArrayLayout, MultiArrayDimension 17 | from std_srvs.srv import Empty 18 | 19 | 20 | class TestSequence: 21 | command = Float64MultiArray() 22 | command.layout = MultiArrayLayout() 23 | command.layout.dim = [MultiArrayDimension()] 24 | 25 | iterator = 1 #step through commands in the sequence - 0 should be reserved for takeoff 26 | 27 | def __init__(self, NUM_UAV): 28 | 29 | global status 30 | 31 | self.command.layout.dim[0].label = 'command' 32 | self.command.layout.dim[0].size = 4 33 | self.command.layout.dim[0].stride = 4*8 34 | self.command.layout.data_offset = 0 35 | 36 | #testing circle data = [x, y, r, n]; n - sequence number 37 | self.command.data = [0,0,10,0] 38 | 39 | status = [Int8() for i in range(NUM_UAV)] 40 | sum_stat = Int8() 41 | sum_stat.data = 0 42 | 43 | #making Anna's Code 44 | 45 | #print 'ls -> ' + str(subprocess.check_output('ls /simulation/AnnaCode', shell=True)) 46 | #print 'make -> ' + str(subprocess.check_output('make /simulation/AnnaCode', shell=True)) 47 | steps = 999 #steps in path generated by /AnnaCode - set once config loaded 48 | 49 | # subscribing to each uav's status 50 | for i in range(NUM_UAV): 51 | 52 | exec ('def status_cb' + str(i) + '(msg): status[' + str(i) + '] = msg') 53 | #exec ('def state_cb' + str(i) + '(msg): state[' + str(i) + '] = msg' ) 54 | rospy.Subscriber('/sequencer/status' + str(i), Int8, callback=eval('status_cb' + str(i))) 55 | 56 | rospy.init_node('sequencer_test', anonymous=True) 57 | 58 | print "INIT\n" 59 | 60 | pub = rospy.Publisher('/sequencer/command', Float64MultiArray, queue_size=10) 61 | rospy.Subscriber('/sequencer/status', Float64MultiArray, callback=self.status_cb) 62 | 63 | rate = rospy.Rate(10) # Hz 64 | rate.sleep() 65 | 66 | # wait to all drones to connect to the sequencer 67 | nc = pub.get_num_connections() 68 | while nc < NUM_UAV: 69 | nc = pub.get_num_connections() 70 | rate.sleep() 71 | 72 | print 'num_connections = ' + str(nc) 73 | print 'publishing - \n' + str(self.command) 74 | pub.publish(self.command) 75 | sys.stdout.flush() 76 | 77 | #print 'SUBPROCESS WRAPPER CALL' 78 | #wrapper = subprocess.Popen('python mpcpsowrapper.py 0 1', shell = True, cwd = '/simulation/AnnaCode') 79 | 80 | 81 | while not rospy.is_shutdown(): 82 | publish = True 83 | 84 | for i in range(NUM_UAV): 85 | if status[i].data < self.command.data[3]: 86 | publish = False 87 | 88 | if publish and self.iterator < 5: 89 | 90 | self.command.data = self.nextCommand() 91 | pub.publish(self.command) 92 | print 'published - ' + str(self.command.data) 93 | continue #continue past next if check so publish goes back to false 94 | 95 | if self.iterator > 4: 96 | pass #following condition 97 | #continue #breaking anna code condition 98 | 99 | if publish and self.iterator > 4 and self.iterator < steps + 6: 100 | for i in range(NUM_UAV): 101 | print status[i].data 102 | print status[i].data < self.command.data[3] 103 | print self.command.data[3] 104 | print publish 105 | 106 | if self.iterator == 5: 107 | print 'running wrapper' 108 | sys.stdout.flush() 109 | #print 'wrapper -> \n' + str(subprocess.check_output('python mpcpsowrapper.py 0 1', shell = True, cwd = '/simulation/AnnaCode')) 110 | config = np.loadtxt('/simulation/config0_0.txt') 111 | print config.ndim 112 | print config.shape 113 | for i in range (0, config.shape[0] - 1): 114 | if config[i,0] == 0: 115 | config = np.delete(config, (range(i,config.shape[0])), axis = 0) 116 | break 117 | steps = config.shape[0] 118 | np.savetxt('/simulation/config0_0.txt',config) 119 | 120 | self.command.data = self.nextCommand() 121 | pub.publish(self.command) 122 | print 'published - ' + str(self.command.data) 123 | 124 | sys.stdout.flush() 125 | 126 | 127 | def status_cb(self, msg): 128 | self.status = msg 129 | 130 | def nextCommand(self): 131 | if self.iterator > 4: 132 | temp = [0,0,0, self.iterator] 133 | elif self.iterator == 4: #flip to velocity control 134 | temp = [0,0,30,self.iterator] 135 | elif self.iterator == 3: 136 | temp = [0,0,30,self.iterator] 137 | elif self.iterator == 2: 138 | temp = [0,0,40,self.iterator] 139 | elif self.iterator == 1: 140 | temp = [0,0,10,self.iterator] 141 | self.iterator += 1 142 | return temp 143 | 144 | if __name__ == "__main__": 145 | TestSequence(int(sys.argv[1])) 146 | -------------------------------------------------------------------------------- /samples/formation/inputs/measures/measureInterRobotDistance.py: -------------------------------------------------------------------------------- 1 | """ 2 | testing performance measure for leader-follower 3 | """ 4 | 5 | import rospy 6 | from std_msgs.msg import Float64 7 | from geometry_msgs.msg import PoseStamped 8 | import sys 9 | import math 10 | 11 | 12 | class MeasureInterRobotDistance: 13 | curr_pose = PoseStamped() 14 | measure = 0 15 | leader_pose = PoseStamped() 16 | 17 | def follower_cb(self, msg): 18 | self.curr_pose = msg 19 | 20 | def leader_cb(self, msg): 21 | self.leader_pose = msg 22 | 23 | def measureDistance(self): 24 | follower_x = self.curr_pose.pose.position.x 25 | follower_y = self.curr_pose.pose.position.y 26 | follower_z = self.curr_pose.pose.position.z 27 | 28 | leader_x = self.leader_pose.pose.position.x 29 | leader_y = self.leader_pose.pose.position.y 30 | leader_z = self.leader_pose.pose.position.z 31 | 32 | return math.sqrt((follower_x - leader_x) * (follower_x - leader_x) + (follower_y - leader_y) * (follower_y - leader_y)) 33 | 34 | # + (follower_z - leader_z) * (follower_z - leader_z)) 35 | 36 | 37 | def __init__(self, this_uav, leader_uav): 38 | rospy.init_node('measure', anonymous=True) 39 | self.measure_pub = rospy.Publisher('/measure', Float64, queue_size=10) 40 | rospy.Subscriber('/mavros' + leader_uav + '/local_position/pose', PoseStamped, callback=self.leader_cb) 41 | rospy.Subscriber('/mavros' + this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 42 | 43 | rate = rospy.Rate(10) # Hz 44 | rate.sleep() 45 | 46 | while not rospy.is_shutdown(): 47 | self.measure = self.measureDistance() 48 | self.measure_pub.publish(self.measure) 49 | rate.sleep() 50 | 51 | if __name__ == "__main__": 52 | MeasureInterRobotDistance(sys.argv[1], sys.argv[2]) 53 | 54 | -------------------------------------------------------------------------------- /samples/formation/inputs/measures/score.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/score.out 3 | cat /simulation/outputs/score.out -------------------------------------------------------------------------------- /samples/formation/inputs/models/f450-1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | UPenn F450 4 | 1.0 5 | f450-1.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /samples/formation/inputs/parameters/swarm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /root/.bashrc 3 | num_uavs=4 4 | duration_seconds=3000 5 | FOLLOW_D_GAIN=1 6 | LOOP_EDGE=40 7 | DIST_THRESHOLD=4 8 | ALTITUDE=15 9 | -------------------------------------------------------------------------------- /samples/formation/inputs/setup/.profile: -------------------------------------------------------------------------------- 1 | source /root/catkin_ws/devel/setup.bash -------------------------------------------------------------------------------- /samples/formation/inputs/setup/gen_gazebo_ros_spawn.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_gazebo_ros' + uav_str + '.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | 57 | 58 | uav_block = '' + \ 59 | '\n' + \ 60 | '\n' + \ 61 | '\n' + \ 63 | '\n' + \ 65 | '\n' 70 | file_block = uav_block 71 | 72 | print(file_block) 73 | print(os.system("cp " + SOURCE + " " + DEST)) 74 | f=open(DEST,"a") 75 | f.write(file_block + '\n ') 76 | f.close() 77 | 78 | print('DRONES CREATED') 79 | -------------------------------------------------------------------------------- /samples/formation/inputs/setup/gen_mavros.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | 6 | def replaceInFile(orig, repl, filename): 7 | os.system('sed -i "s/' + orig + '/' + repl + '/g" ' + filename) 8 | 9 | 10 | NUM_UAVs = int(sys.argv[1]) + 1 11 | PX4_HOME = '/root/src' 12 | print(NUM_UAVs) 13 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 14 | file_block = '' 15 | 16 | for NUM in range(1, NUM_UAVs): 17 | # mavlink 18 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 19 | # simulator start -s -u simulator_udp_port 20 | # mavlink start -u mavlink_start_port -r 4000000 21 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 22 | # 23 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 24 | 25 | mavlink_onboard_remote = 14640 26 | mavlink_start_port = 14656 27 | mavlink_onboard_local = 14657 28 | simulator_udp_port = 14660 29 | 30 | uav_str = str(NUM) 31 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_mavros'+uav_str +'.launch' 32 | 33 | print(uav_str) 34 | print(os.system( 35 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 36 | print(os.system( 37 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-tmp-" + uav_str + ".sdf")) 38 | 39 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 40 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 41 | os.system( 42 | 'cp ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | 44 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 45 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port + 100 * NUM), 47 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local + 100 * NUM), 49 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 50 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote + 100 * NUM), 51 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 52 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), 53 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 54 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), 55 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 56 | 57 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, 58 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 59 | replaceInFile('uav_camera', 60 | 'uav_' + uav_str + '_camera', 61 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 62 | 63 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 64 | 65 | uav_block = '' + \ 66 | '\n' + \ 67 | '\n' + \ 68 | '\n' + \ 71 | '\n' 73 | 74 | mavros_block = ' \ 75 | \ 77 | \ 78 | \ 79 | \ 80 | \ 81 | \ 82 | \ 83 | ' 84 | 85 | file_block = uav_block + '\n' + mavros_block 86 | print(file_block) 87 | 88 | print(os.system("cp " + SOURCE + " " + DEST)) 89 | f = open(DEST, "a") 90 | f.write(file_block + '\n ') 91 | f.close() 92 | 93 | print('DRONES CREATED') 94 | -------------------------------------------------------------------------------- /samples/formation/inputs/setup/gen_px4_sitl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_px4_sitl'+ uav_str +'.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | uav_block = '' + \ 57 | '\n' + \ 58 | '\n' + \ 59 | '\n' + \ 62 | '\n' + \ 64 | '\n' 66 | 67 | 68 | file_block = uav_block + '\n' 69 | print(file_block) 70 | 71 | print(os.system("cp " + SOURCE + " " + DEST)) 72 | f=open(DEST,"a") 73 | f.write(file_block + '\n ') 74 | f.close() 75 | 76 | print('DRONES CREATED') 77 | -------------------------------------------------------------------------------- /samples/formation/inputs/setup/posix_sitl_openuav_swarm_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /samples/formation/inputs/setup/testArmAll.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import subprocess 3 | import os 4 | import sys 5 | from std_msgs.msg import Float64; 6 | 7 | from mavros_msgs.srv import CommandBool, CommandTOL, SetMode 8 | from geometry_msgs.msg import PoseStamped,Pose,Vector3,Twist,TwistStamped 9 | from std_srvs.srv import Empty 10 | import time 11 | 12 | print 'init' 13 | 14 | cur_pose = PoseStamped() 15 | 16 | def pos_cb(msg): 17 | global cur_pose 18 | cur_pose = msg 19 | 20 | NUM_UAV = int(sys.argv[1]) 21 | 22 | mode_proxy = [None for i in range(NUM_UAV)] 23 | arm_proxy = [None for i in range(NUM_UAV)] 24 | 25 | def mavrosTopicStringRoot(uavID=0): 26 | print 'mavros' + str(uavID+1) 27 | return ('mavros' + str(uavID+1)) 28 | 29 | rospy.init_node('multi', anonymous=True) 30 | 31 | #Comm for drones 32 | for uavID in range(0,NUM_UAV): 33 | mode_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/set_mode', SetMode) 34 | arm_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool) 35 | 36 | print 'communication initialization complete' 37 | data = [None for i in range(NUM_UAV)] 38 | 39 | while None in data: 40 | for uavID in range(0, NUM_UAV): 41 | try: 42 | data[uavID] = rospy.wait_for_message(mavrosTopicStringRoot(uavID) + '/global_position/rel_alt', Float64, timeout=5) 43 | except: 44 | pass 45 | 46 | for uavID in range(0, NUM_UAV): 47 | print "wait for service" 48 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/set_mode') 49 | print "got service" 50 | 51 | rate = rospy.Rate(10) 52 | 53 | while not rospy.is_shutdown(): 54 | success = [None for i in range(NUM_UAV)] 55 | for uavID in range(0, NUM_UAV): 56 | try: 57 | success[uavID] = mode_proxy[uavID](1,'OFFBOARD') 58 | except rospy.ServiceException, e: 59 | print ("mavros/set_mode service call failed: %s"%e) 60 | 61 | success = [None for i in range(NUM_UAV)] 62 | for uavID in range(0, NUM_UAV): 63 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/cmd/arming') 64 | 65 | for uavID in range(0, NUM_UAV): 66 | try: 67 | success[uavID] = arm_proxy[uavID](True) 68 | except rospy.ServiceException, e: 69 | print ("mavros1/set_mode service call failed: %s"%e) 70 | -------------------------------------------------------------------------------- /samples/formation/inputs/setup/testCreateUAVSwarm.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import rospy 3 | import sys 4 | import os 5 | from std_msgs.msg import Float64 6 | from geometry_msgs.msg import PoseStamped 7 | import roslaunch 8 | 9 | 10 | cur_pose = PoseStamped() 11 | NUM_UAV = sys.argv[1] 12 | process = subprocess.Popen(["/bin/bash","/root/src/Firmware/Tools/swarm.sh",NUM_UAV],stdout=subprocess.PIPE) 13 | process.wait() 14 | for line in process.stdout: 15 | print line 16 | 17 | launchfile = "posix_sitl_multi_tmp.launch" 18 | fullpath = os.path.join("/root/src/Firmware/launch/", launchfile) 19 | subprocess.Popen(["roslaunch",fullpath]) 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /samples/formation/launch.sh: -------------------------------------------------------------------------------- 1 | #launch the patrol sim 2 | #!/bin/bash 3 | nvidia-docker run -dit --net=openuavapp_default --name=openuavapp_x${3:-`date +%s`} -v /tmp/.X11-unix:/tmp/.X11-unix -v /home/junk/openuav-playground/samples/formation/:/simulation -e DISPLAY=:0 --entrypoint "/home/setup.sh" openuavapp_openuav 4 | 5 | docker ps 6 | 7 | 8 | -------------------------------------------------------------------------------- /samples/formation/run_this.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "++++++++INIT++++++++++" 4 | 5 | source /simulation/inputs/parameters/swarm.sh 6 | source /opt/ros/kinetic/setup.bash 7 | source ~/catkin_ws/devel/setup.bash 8 | 9 | 10 | ## Previous clean-up 11 | rm -rf /root/src/Firmware/Tools/sitl_gazebo/models/f450-tmp-* 12 | rm -f /root/src/Firmware/posix-configs/SITL/init/lpe/f450-tmp-* 13 | rm -f /root/src/Firmware/launch/posix_sitl_multi_tmp.launch 14 | 15 | ## world setup # 16 | cp /simulation/inputs/world/empty.world /root/src/Firmware/Tools/sitl_gazebo/worlds/empty.world 17 | cp /simulation/inputs/models/f450-1/f450-1.sdf /root/src/Firmware/Tools/sitl_gazebo/models/f450-1/f450-1.sdf 18 | cp /simulation/inputs/setup/posix_sitl_openuav_swarm_base.launch /root/src/Firmware/launch/posix_sitl_openuav_swarm_base.launch 19 | 20 | 21 | mkdir /simulation/outputs 22 | rm -f /simulation/outputs/*.csv 23 | echo "Setup..." >> /tmp/debug 24 | 25 | 26 | python /simulation/inputs/setup/gen_gazebo_ros_spawn.py $num_uavs 27 | python /simulation/inputs/setup/gen_px4_sitl.py $num_uavs 28 | python /simulation/inputs/setup/gen_mavros.py $num_uavs 29 | 30 | for((i=1;i<=$num_uavs;i+=1)) 31 | do 32 | echo "px4 posix_sitl_multi_gazebo_ros$num_uavs.launch" 33 | echo "launching uav$i ..." >> /tmp/debug 34 | roslaunch px4 posix_sitl_multi_gazebo_ros$i.launch &> /dev/null & 35 | until rostopic echo /gazebo/model_states | grep -m1 f450-tmp-$i ; do : ; done 36 | roslaunch px4 posix_sitl_multi_px4_sitl$i.launch &> /dev/null & 37 | sleep 2 38 | roslaunch px4 posix_sitl_multi_mavros$i.launch &> /dev/null & 39 | until rostopic echo /mavros$i/state | grep -m1 "connected: True" ; do : ; done 40 | echo "launched uav$i ..." >> /tmp/debug 41 | 42 | done 43 | python /simulation/inputs/measures/measureInterRobotDistance.py $num_uavs 1 &> /dev/null & 44 | roslaunch rosbridge_server rosbridge_websocket.launch ssl:=false &> /dev/null & 45 | rosrun web_video_server web_video_server _port:=80 _server_threads:=100 &> /dev/null & 46 | 47 | for((i = 0;i<$num_uavs;i+=1)) 48 | do 49 | python /simulation/inputs/controllers/simple_Formation.py $i $num_uavs $FOLLOW_D_GAIN &> /simulation/outputs/patroLog$i.txt & 50 | done 51 | sleep 1 52 | echo "Launch Sequencer" #>> /tmp/debug 53 | python /simulation/inputs/controllers/sequencer.py $num_uavs &> /simulation/outputs/sequencerLog.txt & 54 | 55 | 56 | 57 | tensorboard --logdir=/simulation/outputs/ --port=8008 &> /dev/null & 58 | roslaunch opencv_apps general_contours.launch image:=/uav_1_camera_front/image_raw debug_view:=false &> /dev/null & 59 | 60 | echo "Measures..." #>> /tmp/debug 61 | for((i=1;i<=$num_uavs;i+=1)) 62 | do 63 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /mavros$i/local_position/odom > /simulation/outputs/uav$i.csv & 64 | done 65 | 66 | 67 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /measure > /simulation/outputs/measure.csv & 68 | 69 | 70 | sleep $duration_seconds 71 | #cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/average_measure.txt 72 | 73 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/controllers/test_1_Loop.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Jnaneshwar Das 3 | testing looping behavior across a set of waypoints using offboard control 4 | """ 5 | 6 | import rospy 7 | from mavros_msgs.msg import State 8 | from geometry_msgs.msg import PoseStamped, Point, Quaternion 9 | import math 10 | import numpy 11 | import sys 12 | import tf 13 | 14 | 15 | class TestLoop: 16 | curr_pose = PoseStamped() 17 | waypointIndex = 0 18 | distThreshold = 0.5 19 | sim_ctr = 1 20 | des_pose = PoseStamped() 21 | isReadyToFly = False 22 | 23 | def __init__(self, H,V,uav_prefix,yaw): 24 | print yaw 25 | q = tf.transformations.quaternion_from_euler(0, 0, yaw) 26 | self.locations = numpy.matrix([[H, H, V, q[0], q[1], q[2], q[3]], 27 | [-H, H, V, q[0], q[1], q[2], q[3]], 28 | [-H, -H, V, q[0], q[1], q[2], q[3]], 29 | [H, -H, V, q[0], q[1], q[2], q[3]], 30 | ]) 31 | print self.locations 32 | print '/mavros'+ uav_prefix + '/setpoint_position/local' 33 | rospy.init_node('offboard_test', anonymous=True) 34 | pose_pub = rospy.Publisher('/mavros'+ uav_prefix + '/setpoint_position/local', PoseStamped, queue_size=10) 35 | rospy.Subscriber('/mavros'+ uav_prefix + '/local_position/pose', PoseStamped, callback=self.mocap_cb) 36 | rospy.Subscriber('/mavros'+ uav_prefix + '/state', State, callback=self.state_cb) 37 | 38 | rate = rospy.Rate(10) # Hz 39 | rate.sleep() 40 | self.des_pose = self.copy_pose(self.curr_pose) 41 | shape = self.locations.shape 42 | 43 | while not rospy.is_shutdown(): 44 | print self.sim_ctr, shape[0], self.waypointIndex 45 | if self.waypointIndex is shape[0]: 46 | self.waypointIndex = 0 47 | self.sim_ctr += 1 48 | 49 | if self.isReadyToFly: 50 | des_x = self.locations[self.waypointIndex, 0] 51 | des_y = self.locations[self.waypointIndex, 1] 52 | des_z = self.locations[self.waypointIndex, 2] 53 | self.des_pose.pose.position.x = des_x 54 | self.des_pose.pose.position.y = des_y 55 | self.des_pose.pose.position.z = des_z 56 | 57 | self.des_pose.pose.orientation.x = self.locations[self.waypointIndex, 3] 58 | self.des_pose.pose.orientation.y = self.locations[self.waypointIndex, 4] 59 | self.des_pose.pose.orientation.z = self.locations[self.waypointIndex, 5] 60 | self.des_pose.pose.orientation.w = self.locations[self.waypointIndex, 6] 61 | 62 | curr_x = self.curr_pose.pose.position.x 63 | curr_y = self.curr_pose.pose.position.y 64 | curr_z = self.curr_pose.pose.position.z 65 | 66 | azimuth = math.atan2(des_y-curr_y, des_x-curr_x) 67 | quaternion = tf.transformations.quaternion_from_euler(0, 0, azimuth) 68 | print quaternion 69 | self.des_pose.pose.orientation.x = quaternion[0] 70 | self.des_pose.pose.orientation.y = quaternion[1] 71 | self.des_pose.pose.orientation.z = quaternion[2] 72 | self.des_pose.pose.orientation.w = quaternion[3] 73 | 74 | 75 | 76 | dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y) + (curr_z - des_z)*(curr_z - des_z)) 77 | if dist < self.distThreshold: 78 | self.waypointIndex += 1 79 | 80 | pose_pub.publish(self.des_pose) 81 | rate.sleep() 82 | 83 | def copy_pose(self, pose): 84 | pt = pose.pose.position 85 | quat = pose.pose.orientation 86 | copied_pose = PoseStamped() 87 | copied_pose.header.frame_id = pose.header.frame_id 88 | copied_pose.pose.position = Point(pt.x, pt.y, pt.z) 89 | copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) 90 | return copied_pose 91 | 92 | def mocap_cb(self, msg): 93 | # print msg 94 | self.curr_pose = msg 95 | 96 | def state_cb(self,msg): 97 | print msg.mode 98 | if(msg.mode=='OFFBOARD'): 99 | self.isReadyToFly = True 100 | print "readyToFly" 101 | 102 | if __name__ == "__main__": 103 | TestLoop(float(sys.argv[1]), float(sys.argv[2]), sys.argv[3], float(sys.argv[4])) 104 | 105 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/controllers/test_3_Follow.py: -------------------------------------------------------------------------------- 1 | """ 2 | Authors: Arjun Kumar and Jnaneshwar Das 3 | testing follower(leader) controller using offboard position control 4 | """ 5 | 6 | import rospy 7 | from mavros_msgs.msg import State 8 | from geometry_msgs.msg import PoseStamped, Point, Quaternion, TwistStamped 9 | import math 10 | import sys 11 | import tf 12 | 13 | 14 | 15 | class TestFollow: 16 | curr_pose = PoseStamped() 17 | des_pose = PoseStamped() 18 | leader_pose = PoseStamped() 19 | leader_vel = TwistStamped() 20 | 21 | 22 | isReadyToFly = False 23 | #alpha = .7#const scale of leader velocity 24 | 25 | def __init__(self, this_uav,leader_uav, D_GAIN): 26 | rospy.init_node('offboard_test', anonymous=True) 27 | 28 | pose_pub = rospy.Publisher('/mavros'+ this_uav + '/setpoint_position/local', PoseStamped, queue_size=10) 29 | 30 | rospy.Subscriber('/mavros'+ leader_uav + '/local_position/pose', PoseStamped, callback=self.leader_cb) 31 | rospy.Subscriber('/mavros'+ this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 32 | rospy.Subscriber('/mavros'+ this_uav + '/state', State, callback=self.state_cb) 33 | rospy.Subscriber('/mavros'+ leader_uav + '/local_position/velocity_local', TwistStamped, callback=self.leaderVel_cb) 34 | 35 | rate = rospy.Rate(5) # Hz 36 | rate.sleep() 37 | self.des_pose = self.copy_pose(self.curr_pose) 38 | 39 | while not rospy.is_shutdown(): 40 | if self.isReadyToFly: 41 | self.des_pose.pose.position.x = self.leader_pose.pose.position.x + (self.leader_vel.twist.linear.x*D_GAIN) 42 | self.des_pose.pose.position.y = self.leader_pose.pose.position.y + (self.leader_vel.twist.linear.y*D_GAIN) 43 | self.des_pose.pose.position.z = self.leader_pose.pose.position.z + 1 44 | 45 | azimuth = math.atan2(self.leader_pose.pose.position.y-self.curr_pose.pose.position.y, self.leader_pose.pose.position.x-self.curr_pose.pose.position.x) 46 | quaternion = tf.transformations.quaternion_from_euler(0, 0, azimuth) 47 | self.des_pose.pose.orientation.x = quaternion[0] 48 | self.des_pose.pose.orientation.y = quaternion[1] 49 | self.des_pose.pose.orientation.z = quaternion[2] 50 | self.des_pose.pose.orientation.w = quaternion[3] 51 | 52 | 53 | pose_pub.publish(self.des_pose) 54 | rate.sleep() 55 | 56 | def copy_pose(self, pose): 57 | pt = pose.pose.position 58 | quat = pose.pose.orientation 59 | copied_pose = PoseStamped() 60 | copied_pose.header.frame_id = pose.header.frame_id 61 | copied_pose.pose.position = Point(pt.x, pt.y, pt.z) 62 | copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) 63 | return copied_pose 64 | 65 | def follower_cb(self, msg): 66 | self.curr_pose = msg 67 | 68 | def leader_cb(self, msg): 69 | self.leader_pose = msg 70 | 71 | def leaderVel_cb(self, msg): 72 | self.leader_vel = msg 73 | 74 | def state_cb(self,msg): 75 | print msg.mode 76 | if(msg.mode=='OFFBOARD'): 77 | self.isReadyToFly = True 78 | print "readyToFly" 79 | 80 | if __name__ == "__main__": 81 | TestFollow(sys.argv[1], sys.argv[2], float(sys.argv[3])) 82 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/controllers/test_4_Velocity.py: -------------------------------------------------------------------------------- 1 | """ 2 | Authors: Arjun Kumar and Jnaneshwar Das 3 | testing offboard positon control with a simple takeoff script 4 | """ 5 | 6 | import rospy 7 | from mavros_msgs.msg import State 8 | from geometry_msgs.msg import PoseStamped, Point, Quaternion, TwistStamped, Vector3 9 | from nav_msgs.msg import Odometry 10 | import sys 11 | 12 | 13 | 14 | class TestFollow: 15 | curr_pose = PoseStamped() 16 | 17 | des_pose = PoseStamped() 18 | des_vel = TwistStamped() 19 | 20 | 21 | leader_odom = Odometry() 22 | leader_accel = Vector3() 23 | 24 | error = Vector3() 25 | errorD = Vector3() 26 | errorI = Vector3() 27 | errorLast = Vector3() 28 | 29 | prev_odom = Odometry() 30 | 31 | isReadyToFly = False 32 | takeOff = False 33 | 34 | alpha = 1.5 #const scale of leader velocity 35 | kvel = 1.2 #1.2 36 | kaccel = 0 #1.7 37 | 38 | kp = 1.2 39 | ki = 0 40 | kd = 1.2 41 | 42 | def __init__(self, this_uav,leader_uav, H): 43 | rospy.init_node('offboard_test', anonymous=True) 44 | 45 | print "INIT\n" 46 | 47 | pose_pub = rospy.Publisher('/mavros'+ this_uav + '/setpoint_position/local', PoseStamped, queue_size=10) 48 | vel_pub = rospy.Publisher('/mavros' + this_uav + '/setpoint_velocity/cmd_vel', TwistStamped, queue_size = 10) 49 | 50 | rospy.Subscriber('/mavros'+ leader_uav + '/local_position/odom', Odometry, callback=self.leader_cb) 51 | rospy.Subscriber('/mavros'+ this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 52 | rospy.Subscriber('/mavros'+ this_uav + '/state', State, callback=self.state_cb) 53 | 54 | rate = rospy.Rate(10) # Hz 55 | rate.sleep() 56 | 57 | self.des_pose = self.copy_pose(self.curr_pose) 58 | self.des_pose.pose.position.z = 2 59 | self.prev_odom.pose.pose.position.x = -9999 60 | 61 | 62 | #while not self.isReadyToFly: 63 | 64 | print self.des_pose 65 | print "\n" 66 | print self.isReadyToFly 67 | 68 | sys.stdout.flush() 69 | 70 | 71 | 72 | 73 | while not rospy.is_shutdown(): 74 | if self.isReadyToFly: 75 | #---------------------------------- 76 | self.error.x = self.leader_odom.pose.pose.position.x - self.curr_pose.pose.position.x 77 | self.error.y = self.leader_odom.pose.pose.position.y - self.curr_pose.pose.position.y 78 | self.error.z = self.leader_odom.pose.pose.position.z - self.curr_pose.pose.position.z 79 | 80 | if not self.takeOff: 81 | pose_pub.publish(self.des_pose) 82 | rate.sleep() 83 | self.takeOff = True 84 | 85 | self.errorLast.x = self.error.x 86 | self.errorLast.y = self.error.y 87 | self.errorLast.z = self.error.z 88 | 89 | self.errorI.x = self.error.x 90 | self.errorI.y = self.error.y 91 | self.errorI.z = self.error.z 92 | 93 | continue 94 | #---------------------------------- 95 | if self.prev_odom.pose.pose.position.x != -9999: 96 | self.leader_accel.x = self.leader_odom.twist.twist.linear.x - self.prev_odom.twist.twist.linear.x 97 | self.leader_accel.y = self.leader_odom.twist.twist.linear.y - self.prev_odom.twist.twist.linear.y 98 | 99 | self.errorI.x = self.errorI.x + self.error.x 100 | self.errorI.y = self.errorI.y + self.error.y 101 | self.errorI.z = self.errorI.z + self.error.z 102 | 103 | self.errorD.x = self.error.x - self.errorLast.x 104 | self.errorD.y = self.error.y - self.errorLast.y 105 | self.errorD.z = self.error.z - self.errorLast.z 106 | 107 | self.errorLast.x = self.error.x 108 | self.errorLast.y = self.error.y 109 | self.errorLast.z = self.error.z 110 | 111 | self.des_pose.pose.position.x = self.leader_odom.pose.pose.position.x + (self.leader_odom.twist.twist.linear.x * self.alpha) 112 | self.des_pose.pose.position.y = self.leader_odom.pose.pose.position.y + (self.leader_odom.twist.twist.linear.y * self.alpha) 113 | self.des_pose.pose.position.z = H 114 | self.des_pose.pose.orientation = self.leader_odom.pose.pose.orientation 115 | 116 | self.des_vel.twist.linear.x = ((self.leader_odom.pose.pose.position.x - self.curr_pose.pose.position.x) * self.kvel) + (self.leader_accel.x * self.kaccel) 117 | self.des_vel.twist.linear.y = ((self.leader_odom.pose.pose.position.y - self.curr_pose.pose.position.y) * self.kvel) + (self.leader_accel.y * self.kaccel) 118 | self.des_vel.twist.linear.z = (self.leader_odom.pose.pose.position.z + .5) - self.curr_pose.pose.position.z 119 | self.des_vel.twist.angular.x = 0 120 | self.des_vel.twist.angular.y = 0 121 | self.des_vel.twist.angular.z = 0 122 | 123 | self.des_vel.twist.linear.x = (self.kp * self.error.x) + (self.kd * self.errorD.x) + (self.ki * self.errorI.x) 124 | self.des_vel.twist.linear.y = (self.kp * self.error.y) + (self.kd * self.errorD.y) + (self.ki * self.errorI.y) 125 | #self.der_vel.twist.linear.z = (self.leader_odom.pose.pose.position.z + .5) - self.curr_pose.pose.position.z 126 | 127 | self.prev_odom = self.leader_odom 128 | 129 | vel_pub.publish(self.des_vel) 130 | # pose_pub.publish(self.des_pose) 131 | rate.sleep() 132 | 133 | 134 | def copy_pose(self, pose): 135 | pt = pose.pose.position 136 | quat = pose.pose.orientation 137 | copied_pose = PoseStamped() 138 | copied_pose.header.frame_id = pose.header.frame_id 139 | copied_pose.pose.position = Point(pt.x, pt.y, pt.z) 140 | copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) 141 | return copied_pose 142 | 143 | def follower_cb(self, msg): 144 | self.curr_pose = msg 145 | 146 | def leader_cb(self, msg): 147 | self.leader_odom = msg 148 | 149 | def state_cb(self,msg): 150 | print msg.mode 151 | if(msg.mode=='OFFBOARD'): 152 | self.isReadyToFly = True 153 | print "readyToFly" 154 | 155 | if __name__ == "__main__": 156 | TestFollow(sys.argv[1], sys.argv[2], float(sys.argv[3])) 157 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/measures/measureInterRobotDistance.py: -------------------------------------------------------------------------------- 1 | """ 2 | testing performance measure for leader-follower 3 | """ 4 | 5 | import rospy 6 | from std_msgs.msg import Float64 7 | from geometry_msgs.msg import PoseStamped 8 | import sys 9 | import math 10 | 11 | 12 | class MeasureInterRobotDistance: 13 | curr_pose = PoseStamped() 14 | measure = 0 15 | leader_pose = PoseStamped() 16 | 17 | def follower_cb(self, msg): 18 | self.curr_pose = msg 19 | 20 | def leader_cb(self, msg): 21 | self.leader_pose = msg 22 | 23 | def measureDistance(self): 24 | follower_x = self.curr_pose.pose.position.x 25 | follower_y = self.curr_pose.pose.position.y 26 | follower_z = self.curr_pose.pose.position.z 27 | 28 | leader_x = self.leader_pose.pose.position.x 29 | leader_y = self.leader_pose.pose.position.y 30 | leader_z = self.leader_pose.pose.position.z 31 | 32 | return math.sqrt((follower_x - leader_x) * (follower_x - leader_x) + (follower_y - leader_y) * (follower_y - leader_y)) 33 | 34 | # + (follower_z - leader_z) * (follower_z - leader_z)) 35 | 36 | 37 | def __init__(self, this_uav, leader_uav): 38 | rospy.init_node('measure', anonymous=True) 39 | self.measure_pub = rospy.Publisher('/measure', Float64, queue_size=10) 40 | rospy.Subscriber('/mavros' + leader_uav + '/local_position/pose', PoseStamped, callback=self.leader_cb) 41 | rospy.Subscriber('/mavros' + this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 42 | 43 | rate = rospy.Rate(10) # Hz 44 | rate.sleep() 45 | 46 | while not rospy.is_shutdown(): 47 | self.measure = self.measureDistance() 48 | self.measure_pub.publish(self.measure) 49 | rate.sleep() 50 | 51 | if __name__ == "__main__": 52 | MeasureInterRobotDistance(sys.argv[1], sys.argv[2]) 53 | 54 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/measures/score.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/score.out 3 | cat /simulation/outputs/score.out -------------------------------------------------------------------------------- /samples/leader-follower/inputs/models/f450-1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | UPenn F450 4 | 1.0 5 | f450-1.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/parameters/swarm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /root/.bashrc 3 | num_uavs=3 4 | duration_seconds=4000 5 | FOLLOW_D_GAIN=-15 6 | LOOP_EDGE=60 7 | DIST_THRESHOLD=15 8 | ALTITUDE=50 9 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/setup/gen_gazebo_ros_spawn.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_gazebo_ros' + uav_str + '.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | 57 | 58 | uav_block = '' + \ 59 | '\n' + \ 60 | '\n' + \ 61 | '\n' + \ 63 | '\n' + \ 65 | '\n' 70 | file_block = uav_block 71 | 72 | print(file_block) 73 | print(os.system("cp " + SOURCE + " " + DEST)) 74 | f=open(DEST,"a") 75 | f.write(file_block + '\n ') 76 | f.close() 77 | 78 | print('DRONES CREATED') 79 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/setup/gen_mavros.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | 6 | def replaceInFile(orig, repl, filename): 7 | os.system('sed -i "s/' + orig + '/' + repl + '/g" ' + filename) 8 | 9 | 10 | NUM_UAVs = int(sys.argv[1]) + 1 11 | PX4_HOME = '/root/src' 12 | print(NUM_UAVs) 13 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 14 | file_block = '' 15 | 16 | for NUM in range(1, NUM_UAVs): 17 | # mavlink 18 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 19 | # simulator start -s -u simulator_udp_port 20 | # mavlink start -u mavlink_start_port -r 4000000 21 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 22 | # 23 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 24 | 25 | mavlink_onboard_remote = 14640 26 | mavlink_start_port = 14656 27 | mavlink_onboard_local = 14657 28 | simulator_udp_port = 14660 29 | 30 | uav_str = str(NUM) 31 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_mavros'+uav_str +'.launch' 32 | 33 | print(uav_str) 34 | print(os.system( 35 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 36 | print(os.system( 37 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-tmp-" + uav_str + ".sdf")) 38 | 39 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 40 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 41 | os.system( 42 | 'cp ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | 44 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 45 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port + 100 * NUM), 47 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local + 100 * NUM), 49 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 50 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote + 100 * NUM), 51 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 52 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), 53 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 54 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), 55 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 56 | 57 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, 58 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 59 | replaceInFile('uav_camera', 60 | 'uav_' + uav_str + '_camera', 61 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 62 | 63 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 64 | 65 | uav_block = '' + \ 66 | '\n' + \ 67 | '\n' + \ 68 | '\n' + \ 71 | '\n' 73 | 74 | mavros_block = ' \ 75 | \ 77 | \ 78 | \ 79 | \ 80 | \ 81 | \ 82 | \ 83 | ' 84 | 85 | file_block = uav_block + '\n' + mavros_block 86 | print(file_block) 87 | 88 | print(os.system("cp " + SOURCE + " " + DEST)) 89 | f = open(DEST, "a") 90 | f.write(file_block + '\n ') 91 | f.close() 92 | 93 | print('DRONES CREATED') 94 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/setup/gen_px4_sitl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_px4_sitl'+ uav_str +'.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | uav_block = '' + \ 57 | '\n' + \ 58 | '\n' + \ 59 | '\n' + \ 62 | '\n' + \ 64 | '\n' 66 | 67 | 68 | file_block = uav_block + '\n' 69 | print(file_block) 70 | 71 | print(os.system("cp " + SOURCE + " " + DEST)) 72 | f=open(DEST,"a") 73 | f.write(file_block + '\n ') 74 | f.close() 75 | 76 | print('DRONES CREATED') 77 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/setup/posix_sitl_openuav_swarm_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/setup/testArmAll.py: -------------------------------------------------------------------------------- 1 | 2 | import rospy 3 | import subprocess 4 | import os 5 | import sys 6 | from std_msgs.msg import Float64; 7 | from mavros_msgs.srv import CommandBool, CommandTOL, SetMode 8 | from geometry_msgs.msg import PoseStamped,Pose,Vector3,Twist,TwistStamped 9 | from std_srvs.srv import Empty 10 | import time 11 | 12 | print 'init' 13 | 14 | cur_pose = PoseStamped() 15 | 16 | def pos_cb(msg): 17 | global cur_pose 18 | cur_pose = msg 19 | 20 | NUM_UAV = int(sys.argv[1]) 21 | 22 | mode_proxy = [None for i in range(NUM_UAV)] 23 | arm_proxy = [None for i in range(NUM_UAV)] 24 | 25 | def mavrosTopicStringRoot(uavID=0): 26 | print 'mavros' + str(uavID+1) 27 | return ('mavros' + str(uavID+1)) 28 | 29 | rospy.init_node('multi', anonymous=True) 30 | 31 | #Comm for drones 32 | for uavID in range(0,NUM_UAV): 33 | mode_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/set_mode', SetMode) 34 | arm_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool) 35 | 36 | print 'communication initialization complete' 37 | data = [None for i in range(NUM_UAV)] 38 | 39 | while None in data: 40 | for uavID in range(0, NUM_UAV): 41 | try: 42 | data[uavID] = rospy.wait_for_message(mavrosTopicStringRoot(uavID) + '/global_position/rel_alt', Float64, timeout=5) 43 | except: 44 | pass 45 | 46 | for uavID in range(0, NUM_UAV): 47 | print "wait for service" 48 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/set_mode') 49 | print "got service" 50 | 51 | rate = rospy.Rate(10) 52 | 53 | while not rospy.is_shutdown(): 54 | success = [None for i in range(NUM_UAV)] 55 | for uavID in range(0, NUM_UAV): 56 | try: 57 | success[uavID] = mode_proxy[uavID](1,'OFFBOARD') 58 | except rospy.ServiceException, e: 59 | print ("mavros/set_mode service call failed: %s"%e) 60 | 61 | success = [None for i in range(NUM_UAV)] 62 | for uavID in range(0, NUM_UAV): 63 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/cmd/arming') 64 | 65 | for uavID in range(0, NUM_UAV): 66 | try: 67 | success[uavID] = arm_proxy[uavID](True) 68 | except rospy.ServiceException, e: 69 | print ("mavros1/set_mode service call failed: %s"%e) 70 | -------------------------------------------------------------------------------- /samples/leader-follower/inputs/setup/testCreateUAVSwarm.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import rospy 3 | import sys 4 | import os 5 | from std_msgs.msg import Float64 6 | from geometry_msgs.msg import PoseStamped 7 | import roslaunch 8 | 9 | 10 | cur_pose = PoseStamped() 11 | NUM_UAV = sys.argv[1] 12 | process = subprocess.Popen(["/bin/bash","/root/src/Firmware/Tools/swarm.sh",NUM_UAV],stdout=subprocess.PIPE) 13 | process.wait() 14 | for line in process.stdout: 15 | print line 16 | 17 | launchfile = "posix_sitl_multi_tmp.launch" 18 | fullpath = os.path.join("/root/src/Firmware/launch/", launchfile) 19 | subprocess.Popen(["roslaunch",fullpath]) 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /samples/leader-follower/launch.sh: -------------------------------------------------------------------------------- 1 | #launch the patrol sim 2 | #!/bin/bash 3 | nvidia-docker run -dit --net=openuavapp_default --name=openuavapp_x${3:-`date +%s`} -v /tmp/.X11-unix:/tmp/.X11-unix -v /home/junk/openuav-playground/samples/leader-follower/:/simulation -e DISPLAY=:0 --entrypoint "/home/setup.sh" openuavapp_openuav 4 | 5 | docker ps 6 | 7 | 8 | -------------------------------------------------------------------------------- /samples/leader-follower/run_this.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #!/bin/bash 4 | 5 | echo "++++++++INIT++++++++++" 6 | 7 | source /simulation/inputs/parameters/swarm.sh 8 | source /opt/ros/kinetic/setup.bash 9 | source ~/catkin_ws/devel/setup.bash 10 | 11 | 12 | ## Previous clean-up 13 | rm -rf /root/src/Firmware/Tools/sitl_gazebo/models/f450-tmp-* 14 | rm -f /root/src/Firmware/posix-configs/SITL/init/lpe/f450-tmp-* 15 | rm -f /root/src/Firmware/launch/posix_sitl_multi_tmp.launch 16 | 17 | ## world setup # 18 | cp /simulation/inputs/world/empty.world /root/src/Firmware/Tools/sitl_gazebo/worlds/empty.world 19 | cp /simulation/inputs/models/f450-1/f450-1.sdf /root/src/Firmware/Tools/sitl_gazebo/models/f450-1/f450-1.sdf 20 | cp /simulation/inputs/setup/posix_sitl_openuav_swarm_base.launch /root/src/Firmware/launch/posix_sitl_openuav_swarm_base.launch 21 | 22 | mkdir /simulation/outputs 23 | rm -f /simulation/outputs/*.csv 24 | rm -f /simulation/outputs/*.txt 25 | echo "Setup..." #>> /tmp/debug 26 | 27 | 28 | python /simulation/inputs/setup/gen_gazebo_ros_spawn.py $num_uavs 29 | python /simulation/inputs/setup/gen_px4_sitl.py $num_uavs 30 | python /simulation/inputs/setup/gen_mavros.py $num_uavs 31 | 32 | for((i=1;i<=$num_uavs;i+=1)) 33 | do 34 | echo "px4 posix_sitl_multi_gazebo_ros$num_uavs.launch" 35 | echo "launching uav$i ..." >> /tmp/debug 36 | roslaunch px4 posix_sitl_multi_gazebo_ros$i.launch &> /dev/null & 37 | until rostopic echo /gazebo/model_states | grep -m1 f450-tmp-$i ; do : ; done 38 | roslaunch px4 posix_sitl_multi_px4_sitl$i.launch &> /dev/null & 39 | sleep 2 40 | roslaunch px4 posix_sitl_multi_mavros$i.launch &> /dev/null & 41 | until rostopic echo /mavros$i/state | grep -m1 "connected: True" ; do : ; done 42 | echo "launched uav$i ..." >> /tmp/debug 43 | 44 | done 45 | python /simulation/inputs/measures/measureInterRobotDistance.py $num_uavs 1 &> /dev/null & 46 | rosrun web_video_server web_video_server _port:=80 _server_threads:=100 &> /dev/null & 47 | roslaunch rosbridge_server rosbridge_websocket.launch ssl:=false &> /dev/null & 48 | 49 | 50 | python /simulation/inputs/controllers/test_1_Loop.py $LOOP_EDGE $ALTITUDE 1 0 &> /dev/null & 51 | python /simulation/inputs/setup/testArmAll.py $num_uavs &> /dev/null & 52 | 53 | sleep 3 54 | for((i=1;i<$num_uavs;i+=1)) 55 | do 56 | one=1 57 | python /simulation/inputs/controllers/test_3_Follow.py $(( i + one)) $i $FOLLOW_D_GAIN &> /dev/null & 58 | sleep 1 59 | done 60 | roslaunch opencv_apps general_contours.launch image:=/uav_1_camera_front/image_raw debug_view:=false &> /dev/null & 61 | 62 | echo "Measures..." 63 | python /simulation/inputs/measures/measureInterRobotDistance.py $num_uavs 1 &> /dev/null & 64 | 65 | 66 | for((i=1;i<=$num_uavs;i+=1)) 67 | do 68 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /mavros$i/local_position/odom > /simulation/outputs/uav$i.csv & 69 | done 70 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /measure > /simulation/outputs/measure.csv & 71 | sleep $duration_seconds 72 | 73 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/average_measure.txt 74 | 75 | -------------------------------------------------------------------------------- /samples/testSimulation/inputs/controllers/test_1_Loop.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Jnaneshwar Das 3 | testing looping behavior across a set of waypoints using offboard control 4 | """ 5 | 6 | import rospy 7 | from mavros_msgs.msg import State 8 | from geometry_msgs.msg import PoseStamped, Point, Quaternion 9 | import math 10 | import numpy 11 | import sys 12 | import tf 13 | 14 | 15 | class TestLoop: 16 | curr_pose = PoseStamped() 17 | waypointIndex = 0 18 | distThreshold = 0.5 19 | sim_ctr = 1 20 | des_pose = PoseStamped() 21 | isReadyToFly = False 22 | 23 | def __init__(self, H,V,uav_prefix,yaw): 24 | print yaw 25 | q = tf.transformations.quaternion_from_euler(0, 0, yaw) 26 | self.locations = numpy.matrix([[H, H, V, q[0], q[1], q[2], q[3]], 27 | [-H, H, V, q[0], q[1], q[2], q[3]], 28 | [-H, -H, V, q[0], q[1], q[2], q[3]], 29 | [H, -H, V, q[0], q[1], q[2], q[3]], 30 | ]) 31 | print self.locations 32 | print '/mavros'+ uav_prefix + '/setpoint_position/local' 33 | rospy.init_node('offboard_test', anonymous=True) 34 | pose_pub = rospy.Publisher('/mavros'+ uav_prefix + '/setpoint_position/local', PoseStamped, queue_size=10) 35 | rospy.Subscriber('/mavros'+ uav_prefix + '/local_position/pose', PoseStamped, callback=self.mocap_cb) 36 | rospy.Subscriber('/mavros'+ uav_prefix + '/state', State, callback=self.state_cb) 37 | 38 | rate = rospy.Rate(10) # Hz 39 | rate.sleep() 40 | self.des_pose = self.copy_pose(self.curr_pose) 41 | shape = self.locations.shape 42 | 43 | while not rospy.is_shutdown(): 44 | print self.sim_ctr, shape[0], self.waypointIndex 45 | if self.waypointIndex is shape[0]: 46 | self.waypointIndex = 0 47 | self.sim_ctr += 1 48 | 49 | if self.isReadyToFly: 50 | des_x = self.locations[self.waypointIndex, 0] 51 | des_y = self.locations[self.waypointIndex, 1] 52 | des_z = self.locations[self.waypointIndex, 2] 53 | self.des_pose.pose.position.x = des_x 54 | self.des_pose.pose.position.y = des_y 55 | self.des_pose.pose.position.z = des_z 56 | 57 | self.des_pose.pose.orientation.x = self.locations[self.waypointIndex, 3] 58 | self.des_pose.pose.orientation.y = self.locations[self.waypointIndex, 4] 59 | self.des_pose.pose.orientation.z = self.locations[self.waypointIndex, 5] 60 | self.des_pose.pose.orientation.w = self.locations[self.waypointIndex, 6] 61 | 62 | curr_x = self.curr_pose.pose.position.x 63 | curr_y = self.curr_pose.pose.position.y 64 | curr_z = self.curr_pose.pose.position.z 65 | 66 | azimuth = math.atan2(0-curr_y, 20-curr_x) 67 | quaternion = tf.transformations.quaternion_from_euler(0, 0, azimuth) 68 | print quaternion 69 | self.des_pose.pose.orientation.x = quaternion[0] 70 | self.des_pose.pose.orientation.y = quaternion[1] 71 | self.des_pose.pose.orientation.z = quaternion[2] 72 | self.des_pose.pose.orientation.w = quaternion[3] 73 | 74 | 75 | 76 | dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y) + (curr_z - des_z)*(curr_z - des_z)) 77 | if dist < self.distThreshold: 78 | self.waypointIndex += 1 79 | 80 | pose_pub.publish(self.des_pose) 81 | rate.sleep() 82 | 83 | def copy_pose(self, pose): 84 | pt = pose.pose.position 85 | quat = pose.pose.orientation 86 | copied_pose = PoseStamped() 87 | copied_pose.header.frame_id = pose.header.frame_id 88 | copied_pose.pose.position = Point(pt.x, pt.y, pt.z) 89 | copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) 90 | return copied_pose 91 | 92 | def mocap_cb(self, msg): 93 | # print msg 94 | self.curr_pose = msg 95 | 96 | def state_cb(self,msg): 97 | print msg.mode 98 | if(msg.mode=='OFFBOARD'): 99 | self.isReadyToFly = True 100 | print "readyToFly" 101 | 102 | if __name__ == "__main__": 103 | TestLoop(float(sys.argv[1]), float(sys.argv[2]), sys.argv[3], float(sys.argv[4])) 104 | 105 | -------------------------------------------------------------------------------- /samples/testSimulation/inputs/controllers/test_3_Follow.py: -------------------------------------------------------------------------------- 1 | """ 2 | Authors: Arjun Kumar and Jnaneshwar Das 3 | testing follower(leader) controller using offboard position control 4 | """ 5 | 6 | import rospy 7 | from mavros_msgs.msg import State 8 | from geometry_msgs.msg import PoseStamped, Point, Quaternion, TwistStamped 9 | import math 10 | import sys 11 | import tf 12 | 13 | 14 | 15 | class TestFollow: 16 | curr_pose = PoseStamped() 17 | des_pose = PoseStamped() 18 | leader_pose = PoseStamped() 19 | leader_vel = TwistStamped() 20 | 21 | 22 | isReadyToFly = False 23 | #alpha = .7#const scale of leader velocity 24 | 25 | def __init__(self, this_uav,leader_uav, D_GAIN): 26 | rospy.init_node('offboard_test', anonymous=True) 27 | 28 | pose_pub = rospy.Publisher('/mavros'+ this_uav + '/setpoint_position/local', PoseStamped, queue_size=10) 29 | 30 | rospy.Subscriber('/mavros'+ leader_uav + '/local_position/pose', PoseStamped, callback=self.leader_cb) 31 | rospy.Subscriber('/mavros'+ this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 32 | rospy.Subscriber('/mavros'+ this_uav + '/state', State, callback=self.state_cb) 33 | rospy.Subscriber('/mavros'+ leader_uav + '/local_position/velocity', TwistStamped, callback=self.leaderVel_cb) 34 | 35 | rate = rospy.Rate(100) # Hz 36 | rate.sleep() 37 | self.des_pose = self.copy_pose(self.curr_pose) 38 | while not rospy.is_shutdown(): 39 | if self.isReadyToFly: 40 | self.des_pose.pose.position.x = self.leader_pose.pose.position.x + (self.leader_vel.twist.linear.x*D_GAIN) 41 | self.des_pose.pose.position.y = self.leader_pose.pose.position.y + (self.leader_vel.twist.linear.y*D_GAIN) 42 | self.des_pose.pose.position.z = self.leader_pose.pose.position.z + 1 43 | 44 | azimuth = math.atan2(self.leader_pose.pose.position.y-self.curr_pose.pose.position.y, self.leader_pose.pose.position.x-self.curr_pose.pose.position.x) 45 | quaternion = tf.transformations.quaternion_from_euler(0, 0, azimuth) 46 | self.des_pose.pose.orientation.x = quaternion[0] 47 | self.des_pose.pose.orientation.y = quaternion[1] 48 | self.des_pose.pose.orientation.z = quaternion[2] 49 | self.des_pose.pose.orientation.w = quaternion[3] 50 | 51 | 52 | pose_pub.publish(self.des_pose) 53 | rate.sleep() 54 | 55 | def copy_pose(self, pose): 56 | pt = pose.pose.position 57 | quat = pose.pose.orientation 58 | copied_pose = PoseStamped() 59 | copied_pose.header.frame_id = pose.header.frame_id 60 | copied_pose.pose.position = Point(pt.x, pt.y, pt.z) 61 | copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) 62 | return copied_pose 63 | 64 | def follower_cb(self, msg): 65 | self.curr_pose = msg 66 | 67 | def leader_cb(self, msg): 68 | self.leader_pose = msg 69 | 70 | def leaderVel_cb(self, msg): 71 | self.leader_vel = msg 72 | 73 | def state_cb(self,msg): 74 | print msg.mode 75 | if(msg.mode=='OFFBOARD'): 76 | self.isReadyToFly = True 77 | print "readyToFly" 78 | 79 | if __name__ == "__main__": 80 | TestFollow(sys.argv[1], sys.argv[2], float(sys.argv[3])) 81 | -------------------------------------------------------------------------------- /samples/testSimulation/inputs/measures/measureInterRobotDistance.py: -------------------------------------------------------------------------------- 1 | """ 2 | testing performance measure for leader-follower 3 | """ 4 | 5 | import rospy 6 | from std_msgs.msg import Float64 7 | from geometry_msgs.msg import PoseStamped 8 | import sys 9 | import math 10 | 11 | 12 | class MeasureInterRobotDistance: 13 | curr_pose = PoseStamped() 14 | measure = 0 15 | leader_pose = PoseStamped() 16 | 17 | def follower_cb(self, msg): 18 | self.curr_pose = msg 19 | 20 | def leader_cb(self, msg): 21 | self.leader_pose = msg 22 | 23 | def measureDistance(self): 24 | follower_x = self.curr_pose.pose.position.x 25 | follower_y = self.curr_pose.pose.position.y 26 | follower_z = self.curr_pose.pose.position.z 27 | 28 | leader_x = self.leader_pose.pose.position.x 29 | leader_y = self.leader_pose.pose.position.y 30 | leader_z = self.leader_pose.pose.position.z 31 | 32 | return math.sqrt((follower_x - leader_x) * (follower_x - leader_x) + (follower_y - leader_y) * (follower_y - leader_y)) 33 | 34 | # + (follower_z - leader_z) * (follower_z - leader_z)) 35 | 36 | 37 | def __init__(self, this_uav, leader_uav): 38 | rospy.init_node('measure', anonymous=True) 39 | self.measure_pub = rospy.Publisher('/measure', Float64, queue_size=10) 40 | rospy.Subscriber('/mavros' + leader_uav + '/local_position/pose', PoseStamped, callback=self.leader_cb) 41 | rospy.Subscriber('/mavros' + this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 42 | 43 | rate = rospy.Rate(10) # Hz 44 | rate.sleep() 45 | 46 | while not rospy.is_shutdown(): 47 | self.measure = self.measureDistance() 48 | self.measure_pub.publish(self.measure) 49 | rate.sleep() 50 | 51 | if __name__ == "__main__": 52 | MeasureInterRobotDistance(sys.argv[1], sys.argv[2]) 53 | 54 | -------------------------------------------------------------------------------- /samples/testSimulation/inputs/measures/score.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/score.out 3 | cat /simulation/outputs/score.out -------------------------------------------------------------------------------- /samples/testSimulation/inputs/models/f450-1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | UPenn F450 4 | 1.0 5 | f450-1.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /samples/testSimulation/inputs/parameters/swarm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /root/.bashrc 3 | num_uavs=2 4 | duration_seconds=3000 5 | FOLLOW_D_GAIN=1 6 | LOOP_EDGE=40 7 | DIST_THRESHOLD=4 8 | ALTITUDE=20 9 | -------------------------------------------------------------------------------- /samples/testSimulation/inputs/setup/testArmAll.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import subprocess 3 | import os 4 | import sys 5 | from std_msgs.msg import Float64; 6 | 7 | from mavros_msgs.srv import CommandBool, CommandTOL, SetMode 8 | from geometry_msgs.msg import PoseStamped,Pose,Vector3,Twist,TwistStamped 9 | from std_srvs.srv import Empty 10 | import time 11 | cur_pose = PoseStamped() 12 | def pos_cb(msg): 13 | global cur_pose 14 | cur_pose = msg 15 | 16 | NUM_UAV = int(sys.argv[1]) 17 | 18 | mode_proxy = [None for i in range(NUM_UAV)] 19 | arm_proxy = [None for i in range(NUM_UAV)] 20 | 21 | def mavrosTopicStringRoot(uavID=0): 22 | return ('mavros' + str(uavID+1)) 23 | 24 | rospy.init_node('multi', anonymous=True) 25 | 26 | #Comm for drones 27 | for uavID in range(0,NUM_UAV): 28 | mode_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/set_mode', SetMode) 29 | arm_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool) 30 | 31 | print 'communication initialization complete' 32 | data = [None for i in range(NUM_UAV)] 33 | 34 | while None in data: 35 | for uavID in range(0, NUM_UAV): 36 | try: 37 | data[uavID] = rospy.wait_for_message(mavrosTopicStringRoot(uavID) + '/global_position/rel_alt', Float64, timeout=5) 38 | except: 39 | pass 40 | 41 | for uavID in range(0, NUM_UAV): 42 | print "wait for service" 43 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/set_mode') 44 | print "got service" 45 | 46 | rate = rospy.Rate(10) 47 | 48 | while not rospy.is_shutdown(): 49 | success = [None for i in range(NUM_UAV)] 50 | for uavID in range(0, NUM_UAV): 51 | try: 52 | success[uavID] = mode_proxy[uavID](1,'OFFBOARD') 53 | except rospy.ServiceException, e: 54 | print ("mavros/set_mode service call failed: %s"%e) 55 | 56 | success = [None for i in range(NUM_UAV)] 57 | for uavID in range(0, NUM_UAV): 58 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/cmd/arming') 59 | 60 | for uavID in range(0, NUM_UAV): 61 | try: 62 | success[uavID] = arm_proxy[uavID](True) 63 | except rospy.ServiceException, e: 64 | print ("mavros1/set_mode service call failed: %s"%e) 65 | -------------------------------------------------------------------------------- /samples/testSimulation/inputs/setup/testCreateUAVSwarm.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import rospy 3 | import sys 4 | import os 5 | from std_msgs.msg import Float64 6 | from geometry_msgs.msg import PoseStamped 7 | import roslaunch 8 | 9 | 10 | cur_pose = PoseStamped() 11 | NUM_UAV = sys.argv[1] 12 | process = subprocess.Popen(["/bin/bash","/root/src/Firmware/Tools/swarm.sh",NUM_UAV],stdout=subprocess.PIPE) 13 | process.wait() 14 | for line in process.stdout: 15 | print line 16 | 17 | launchfile = "posix_sitl_multi_tmp.launch" 18 | fullpath = os.path.join("/root/src/Firmware/launch/", launchfile) 19 | subprocess.Popen(["roslaunch",fullpath]) 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /samples/testSimulation/outputs/average_measure.txt: -------------------------------------------------------------------------------- 1 | 3.3068 2 | -------------------------------------------------------------------------------- /samples/testSimulation/run_this.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source /simulation/inputs/parameters/swarm.sh 4 | source /opt/ros/kinetic/setup.bash 5 | source ~/catkin_ws/devel/setup.bash 6 | 7 | ## Previous clean-up 8 | rm -rf /root/src/Firmware/Tools/sitl_gazebo/models/f450-tmp-* 9 | rm -f /root/src/Firmware/posix-configs/SITL/init/lpe/f450-tmp-* 10 | rm -f /root/src/Firmware/launch/posix_sitl_multi_tmp.launch 11 | 12 | rm -f /simulation/outputs/*.csv 13 | echo "Setup..." 14 | echo "Setup..." >> /tmp/debug 15 | echo "Setup test ..." >> /tmp/debug 16 | echo "Setup test ..." 17 | python /simulation/inputs/setup/testCreateUAVSwarm.py $num_uavs &> /dev/null & 18 | sleep 15 19 | python /simulation/inputs/setup/testArmAll.py $num_uavs &> /dev/null & 20 | sleep 1 21 | python /simulation/inputs/controllers/test_1_Loop.py $LOOP_EDGE $ALTITUDE 1 0 &> /dev/null & 22 | 23 | 24 | for((i=1;i<$num_uavs;i+=1)) 25 | do 26 | one=1 27 | python /simulation/inputs/controllers/test_3_Follow.py $(( i + one)) $i $FOLLOW_D_GAIN &> /dev/null & 28 | sleep 1 29 | done 30 | roslaunch rosbridge_server rosbridge_websocket.launch ssl:=false &> /dev/null & 31 | 32 | echo "Measures..." 33 | python /simulation/inputs/measures/measureInterRobotDistance.py $num_uavs 1 &> /dev/null & 34 | 35 | 36 | rosrun web_video_server web_video_server _port:=80 _server_threads:=100 &> /dev/null & 37 | tensorboard --logdir=/simulation/outputs/ --port=8008 &> /dev/null & 38 | roslaunch opencv_apps general_contours.launch image:=/uav_2_camera/image_raw_front debug_view:=false &> /dev/null & 39 | 40 | for((i=1;i<=$num_uavs;i+=1)) 41 | do 42 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /mavros$i/local_position/odom > /simulation/outputs/uav$i.csv & 43 | done 44 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /measure > /simulation/outputs/measure.csv & 45 | 46 | 47 | sleep 3600000 48 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/average_measure.txt 49 | echo "exiting test simulation container. should this have happened?" 50 | 51 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/controllers/pid.py: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | import numpy as np 4 | 5 | 6 | class PID: 7 | 8 | def __init__(self, p, i, d): 9 | self.Kp = p 10 | self.Kd = d 11 | self.Ki = i 12 | self.error = 0 #e(t) 13 | self.errorM1 = 0 #e(t-1) 14 | self.errorM2 = 0 #e(t-2) 15 | self.cmd = 0 #cmd(t) 16 | self.cmdM = 0 #cmd(t-1) 17 | def update(self, e): 18 | self.errorM2 = self.errorM1 19 | self.errorM1 = self.error 20 | self.error = e 21 | self.cmd = self.cmdM + (self.error)*self.Kp + (self.error - self.errorM1)*self.Kd + (self.error - 2*self.errorM1 + self.errorM2)*self.Ki 22 | self.cmdM = 0 #self.cmd 23 | return self.cmd 24 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/controllers/sequencer.py: -------------------------------------------------------------------------------- 1 | """ 2 | Authors: Arjun Kumar and Jnaneshwar Das 3 | testing sequencer 4 | """ 5 | 6 | import rospy 7 | import subprocess 8 | import os 9 | import sys 10 | import math 11 | import tf 12 | import time 13 | import numpy as np 14 | 15 | 16 | from std_msgs.msg import Int8, Bool, Float64, Float64MultiArray, MultiArrayLayout, MultiArrayDimension 17 | from std_srvs.srv import Empty 18 | 19 | 20 | class TestSequence: 21 | command = Float64MultiArray() 22 | command.layout = MultiArrayLayout() 23 | command.layout.dim = [MultiArrayDimension()] 24 | 25 | iterator = 1 #step through commands in the sequence - 0 should be reserved for takeoff 26 | 27 | def __init__(self, NUM_UAV): 28 | 29 | global status 30 | 31 | self.command.layout.dim[0].label = 'command' 32 | self.command.layout.dim[0].size = 4 33 | self.command.layout.dim[0].stride = 4*8 34 | self.command.layout.data_offset = 0 35 | 36 | #testing circle data = [x, y, r, n]; n - sequence number 37 | self.command.data = [0,0,10,0] 38 | 39 | status = [Int8() for i in range(NUM_UAV)] 40 | sum_stat = Int8() 41 | sum_stat.data = 0 42 | 43 | #making Anna's Code 44 | 45 | #print 'ls -> ' + str(subprocess.check_output('ls /simulation/AnnaCode', shell=True)) 46 | #print 'make -> ' + str(subprocess.check_output('make /simulation/AnnaCode', shell=True)) 47 | steps = 999 #steps in path generated by /AnnaCode - set once config loaded 48 | 49 | # subscribing to each uav's status 50 | for i in range(NUM_UAV): 51 | 52 | exec ('def status_cb' + str(i) + '(msg): status[' + str(i) + '] = msg') 53 | #exec ('def state_cb' + str(i) + '(msg): state[' + str(i) + '] = msg' ) 54 | rospy.Subscriber('/sequencer/status' + str(i), Int8, callback=eval('status_cb' + str(i))) 55 | 56 | rospy.init_node('sequencer_test', anonymous=True) 57 | 58 | print "INIT\n" 59 | 60 | pub = rospy.Publisher('/sequencer/command', Float64MultiArray, queue_size=10) 61 | rospy.Subscriber('/sequencer/status', Float64MultiArray, callback=self.status_cb) 62 | 63 | rate = rospy.Rate(10) # Hz 64 | rate.sleep() 65 | 66 | # wait to all drones to connect to the sequencer 67 | nc = pub.get_num_connections() 68 | while nc < NUM_UAV: 69 | nc = pub.get_num_connections() 70 | rate.sleep() 71 | 72 | print 'num_connections = ' + str(nc) 73 | print 'publishing - \n' + str(self.command) 74 | pub.publish(self.command) 75 | sys.stdout.flush() 76 | 77 | #print 'SUBPROCESS WRAPPER CALL' 78 | #wrapper = subprocess.Popen('python mpcpsowrapper.py 0 1', shell = True, cwd = '/simulation/AnnaCode') 79 | 80 | 81 | while not rospy.is_shutdown(): 82 | publish = True 83 | 84 | for i in range(NUM_UAV): 85 | if status[i].data < self.command.data[3]: 86 | publish = False 87 | 88 | if publish and self.iterator < 5: 89 | 90 | self.command.data = self.nextCommand() 91 | pub.publish(self.command) 92 | print 'published - ' + str(self.command.data) 93 | continue #continue past next if check so publish goes back to false 94 | 95 | if self.iterator > 4: 96 | pass #following condition 97 | #continue #breaking anna code condition 98 | 99 | if publish and self.iterator > 4 and self.iterator < steps + 6: 100 | for i in range(NUM_UAV): 101 | print status[i].data 102 | print status[i].data < self.command.data[3] 103 | print self.command.data[3] 104 | print publish 105 | 106 | if self.iterator == 5: 107 | print 'running wrapper' 108 | sys.stdout.flush() 109 | #print 'wrapper -> \n' + str(subprocess.check_output('python mpcpsowrapper.py 0 1', shell = True, cwd = '/simulation/AnnaCode')) 110 | config = np.loadtxt('/simulation/config0_0.txt') 111 | print config.ndim 112 | print config.shape 113 | for i in range (0, config.shape[0] - 1): 114 | if config[i,0] == 0: 115 | config = np.delete(config, (range(i,config.shape[0])), axis = 0) 116 | break 117 | steps = config.shape[0] 118 | np.savetxt('/simulation/config0_0.txt',config) 119 | 120 | self.command.data = self.nextCommand() 121 | pub.publish(self.command) 122 | print 'published - ' + str(self.command.data) 123 | 124 | sys.stdout.flush() 125 | 126 | 127 | def status_cb(self, msg): 128 | self.status = msg 129 | 130 | def nextCommand(self): 131 | if self.iterator > 4: 132 | temp = [0,0,0, self.iterator] 133 | elif self.iterator == 4: #flip to velocity control 134 | temp = [0,0,30,self.iterator] 135 | elif self.iterator == 3: 136 | temp = [0,0,30,self.iterator] 137 | elif self.iterator == 2: 138 | temp = [0,0,40,self.iterator] 139 | elif self.iterator == 1: 140 | temp = [0,0,10,self.iterator] 141 | self.iterator += 1 142 | return temp 143 | 144 | if __name__ == "__main__": 145 | TestSequence(int(sys.argv[1])) 146 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/controllers/turtlebotLoop.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Jnaneshwar Das 3 | testing looping behavior across a set of waypoints using offboard control 4 | """ 5 | 6 | import rospy 7 | import math 8 | import numpy 9 | import sys 10 | import tf 11 | import random 12 | 13 | from mavros_msgs.msg import State 14 | from geometry_msgs.msg import PoseStamped, Point, Quaternion, Twist 15 | from nav_msgs.msg import Odometry 16 | from kobuki_msgs.msg import BumperEvent 17 | 18 | class TestLoop: 19 | curr_odom = Odometry() 20 | des_vel = Twist() 21 | bumper = BumperEvent() 22 | 23 | def __init__(self, this_turtlebot): 24 | rospy.init_node('turtlebot_InfiniteWalk', anonymous=True) 25 | vel_pub = rospy.Publisher('/turtlebot'+ str(this_turtlebot + 1) + '/mobile_base/commands/velocity', Twist, queue_size=10) 26 | rospy.Subscriber('/mavros'+ str(this_turtlebot + 1) + '/odom', Odometry, callback=self.odom_cb) 27 | rate = rospy.Rate(10) # Hz 28 | rate.sleep() 29 | 30 | while not rospy.is_shutdown(): 31 | linearX =(random.random() - 1) 32 | angularZ = (random.random() - 1) * .75 33 | self.des_vel.linear.x = linearX 34 | self.des_vel.angular.z = angularZ 35 | vel_pub.publish(self.des_vel) 36 | rate.sleep() 37 | 38 | 39 | def odom_cb(self, msg): 40 | self.curr_odom = msg 41 | 42 | def bumper_cb(self, msg): 43 | self.bumper = msg 44 | 45 | if __name__ == "__main__": 46 | TestLoop(int(sys.argv[1])) 47 | 48 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/controllers/uavFollow.py: -------------------------------------------------------------------------------- 1 | """ 2 | Authors: Arjun Kumar and Jnaneshwar Das 3 | testing follower(leader) controller using offboard position control 4 | """ 5 | 6 | import rospy 7 | import math 8 | import sys 9 | import tf 10 | 11 | from mavros_msgs.msg import State 12 | from geometry_msgs.msg import PoseStamped, Point, Quaternion, TwistStamped 13 | from nav_msgs.msg import Odometry 14 | 15 | class TestFollow: 16 | des_pose = PoseStamped() 17 | curr_pose = PoseStamped() 18 | turtle_odom = Odometry() 19 | 20 | isReadyToFly = False 21 | 22 | def __init__(self, this_uav, lead_turtlebot, D_GAIN): 23 | 24 | rospy.init_node('offboard_test', anonymous=True) 25 | 26 | pose_pub = rospy.Publisher('/mavros'+ str(this_uav + 1) + '/setpoint_position/local', PoseStamped, queue_size=10) 27 | rospy.Subscriber('/mavros'+ str(this_uav + 1) + '/local_position/pose', PoseStamped, callback=self.follower_cb) 28 | rospy.Subscriber('/mavros'+ str(this_uav + 1) + '/state', State, callback=self.state_cb) 29 | rospy.Subscriber('/turtlebot'+ str(lead_turtlebot + 1) + '/odom', Odometry, callback=self.turtle_cb) 30 | 31 | rate = rospy.Rate(100) # Hz 32 | rate.sleep() 33 | self.des_pose = self.copy_pose(self.curr_pose) 34 | 35 | while not rospy.is_shutdown(): 36 | if self.isReadyToFly: 37 | self.des_pose.pose.position.x = self.turtle_odom.pose.pose.position.x + 3 #+ (self.leader_vel.twist.linear.x*D_GAIN) 38 | self.des_pose.pose.position.y = self.turtle_odom.pose.pose.position.y + (this_uav) #+ (self.leader_vel.twist.linear.y*D_GAIN) 39 | self.des_pose.pose.position.z = 10 + (this_uav*3) 40 | 41 | if self.poseStampedXYTwoNorm(self.turtle_odom.pose, self.curr_pose) > .5: 42 | azimuth = math.atan2(self.turtle_odom.pose.pose.position.y-self.curr_pose.pose.position.y, self.turtle_odom.pose.pose.position.x-self.curr_pose.pose.position.x) 43 | quaternion = tf.transformations.quaternion_from_euler(0, 0, azimuth) 44 | self.des_pose.pose.orientation.x = quaternion[0] 45 | self.des_pose.pose.orientation.y = quaternion[1] 46 | self.des_pose.pose.orientation.z = quaternion[2] 47 | self.des_pose.pose.orientation.w = quaternion[3] 48 | #print self.turtle_odom 49 | #print self.des_pose 50 | pose_pub.publish(self.des_pose) 51 | rate.sleep() 52 | def poseStampedXYTwoNorm(self, ps1, ps2): 53 | return math.sqrt(math.pow(ps1.pose.position.x-ps2.pose.position.x,2) + math.pow(ps1.pose.position.y-ps2.pose.position.y,2)) 54 | 55 | def copy_pose(self, pose): 56 | pt = pose.pose.position 57 | quat = pose.pose.orientation 58 | copied_pose = PoseStamped() 59 | copied_pose.header.frame_id = pose.header.frame_id 60 | copied_pose.pose.position = Point(pt.x, pt.y, pt.z) 61 | copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z, quat.w) 62 | return copied_pose 63 | 64 | def follower_cb(self, msg): 65 | self.curr_pose = msg 66 | 67 | def turtle_cb(self, msg): 68 | self.turtle_odom = msg 69 | 70 | def state_cb(self,msg): 71 | print msg.mode 72 | if(msg.mode=='OFFBOARD'): 73 | self.isReadyToFly = True 74 | print "readyToFly" 75 | 76 | if __name__ == "__main__": 77 | TestFollow(int(sys.argv[1]), int(sys.argv[2]), float(sys.argv[3])) 78 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/measures/measureInterRobotDistance.py: -------------------------------------------------------------------------------- 1 | """ 2 | testing performance measure for leader-follower 3 | """ 4 | 5 | import rospy 6 | from std_msgs.msg import Float64 7 | from geometry_msgs.msg import PoseStamped 8 | import sys 9 | import math 10 | 11 | 12 | class MeasureInterRobotDistance: 13 | curr_pose = PoseStamped() 14 | measure = 0 15 | leader_pose = PoseStamped() 16 | 17 | def follower_cb(self, msg): 18 | self.curr_pose = msg 19 | 20 | def leader_cb(self, msg): 21 | self.leader_pose = msg 22 | 23 | def measureDistance(self): 24 | follower_x = self.curr_pose.pose.position.x 25 | follower_y = self.curr_pose.pose.position.y 26 | follower_z = self.curr_pose.pose.position.z 27 | 28 | leader_x = self.leader_pose.pose.position.x 29 | leader_y = self.leader_pose.pose.position.y 30 | leader_z = self.leader_pose.pose.position.z 31 | 32 | return math.sqrt((follower_x - leader_x) * (follower_x - leader_x) + (follower_y - leader_y) * (follower_y - leader_y)) 33 | 34 | # + (follower_z - leader_z) * (follower_z - leader_z)) 35 | 36 | 37 | def __init__(self, this_uav, leader_uav): 38 | rospy.init_node('measure', anonymous=True) 39 | self.measure_pub = rospy.Publisher('/measure', Float64, queue_size=10) 40 | rospy.Subscriber('/mavros' + leader_uav + '/local_position/pose', PoseStamped, callback=self.leader_cb) 41 | rospy.Subscriber('/mavros' + this_uav + '/local_position/pose', PoseStamped, callback=self.follower_cb) 42 | 43 | rate = rospy.Rate(10) # Hz 44 | rate.sleep() 45 | 46 | while not rospy.is_shutdown(): 47 | self.measure = self.measureDistance() 48 | self.measure_pub.publish(self.measure) 49 | rate.sleep() 50 | 51 | if __name__ == "__main__": 52 | MeasureInterRobotDistance(sys.argv[1], sys.argv[2]) 53 | 54 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/measures/score.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/score.out 3 | cat /simulation/outputs/score.out -------------------------------------------------------------------------------- /samples/turtlebot/inputs/models/f450-1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | UPenn F450 4 | 1.0 5 | f450-1.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/parameters/swarm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /root/.bashrc 3 | num_uavs=2 4 | num_ground=2 5 | duration_seconds=3000 6 | FOLLOW_D_GAIN=1 7 | LOOP_EDGE=40 8 | DIST_THRESHOLD=4 9 | ALTITUDE=15 10 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/.profile: -------------------------------------------------------------------------------- 1 | source /root/catkin_ws/devel/setup.bash -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/gen_gazebo_ros_spawn.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_gazebo_ros' + uav_str + '.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | 57 | 58 | uav_block = '' + \ 59 | '\n' + \ 60 | '\n' + \ 61 | '\n' + \ 63 | '\n' + \ 65 | '\n' 70 | file_block = uav_block 71 | 72 | print(file_block) 73 | print(os.system("cp " + SOURCE + " " + DEST)) 74 | f=open(DEST,"a") 75 | f.write(file_block + '\n ') 76 | f.close() 77 | 78 | print('DRONES CREATED') 79 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/gen_mavros.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | 6 | def replaceInFile(orig, repl, filename): 7 | os.system('sed -i "s/' + orig + '/' + repl + '/g" ' + filename) 8 | 9 | 10 | NUM_UAVs = int(sys.argv[1]) + 1 11 | PX4_HOME = '/root/src' 12 | print(NUM_UAVs) 13 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 14 | file_block = '' 15 | 16 | for NUM in range(1, NUM_UAVs): 17 | # mavlink 18 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 19 | # simulator start -s -u simulator_udp_port 20 | # mavlink start -u mavlink_start_port -r 4000000 21 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 22 | # 23 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 24 | 25 | mavlink_onboard_remote = 14640 26 | mavlink_start_port = 14656 27 | mavlink_onboard_local = 14657 28 | simulator_udp_port = 14660 29 | 30 | uav_str = str(NUM) 31 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_mavros'+uav_str +'.launch' 32 | 33 | print(uav_str) 34 | print(os.system( 35 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 36 | print(os.system( 37 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str + "/f450-tmp-" + uav_str + ".sdf")) 38 | 39 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 40 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 41 | os.system( 42 | 'cp ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | 44 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port + 100 * NUM), 45 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port + 100 * NUM), 47 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local + 100 * NUM), 49 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 50 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote + 100 * NUM), 51 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 52 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), 53 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 54 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), 55 | PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 56 | 57 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, 58 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 59 | replaceInFile('uav_camera', 60 | 'uav_' + uav_str + '_camera', 61 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 62 | 63 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 64 | 65 | uav_block = '' + \ 66 | '\n' + \ 67 | '\n' + \ 68 | '\n' + \ 71 | '\n' 73 | 74 | mavros_block = ' \ 75 | \ 77 | \ 78 | \ 79 | \ 80 | \ 81 | \ 82 | \ 83 | ' 84 | 85 | file_block = uav_block + '\n' + mavros_block 86 | print(file_block) 87 | 88 | print(os.system("cp " + SOURCE + " " + DEST)) 89 | f = open(DEST, "a") 90 | f.write(file_block + '\n ') 91 | f.close() 92 | 93 | print('DRONES CREATED') 94 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/gen_px4_sitl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | 5 | def replaceInFile(orig, repl, filename): 6 | os.system('sed -i "s/' + orig + '/' +repl + '/g" ' + filename) 7 | 8 | NUM_UAVs = int(sys.argv[1]) + 1 9 | PX4_HOME = '/root/src' 10 | print(NUM_UAVs) 11 | SOURCE = PX4_HOME + '/Firmware/launch/posix_sitl_openuav_swarm_base.launch' 12 | file_block = '' 13 | 14 | for NUM in range(1, NUM_UAVs): 15 | 16 | # mavlink 17 | # < mavlink_udp_port > simulator_udp_port < / mavlink_udp_port > 18 | # simulator start -s -u simulator_udp_port 19 | # mavlink start -u mavlink_start_port -r 4000000 20 | # mavlink start -u mavlink_onboard_local -r 4000000 -m onboard -o mavlink_onboard_remote 21 | # 22 | # mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u mavlink_start_port 23 | 24 | mavlink_onboard_remote = 14640 25 | mavlink_start_port = 14656 26 | mavlink_onboard_local = 14657 27 | simulator_udp_port = 14660 28 | 29 | uav_str = str(NUM) 30 | DEST = PX4_HOME + '/Firmware/launch/posix_sitl_multi_px4_sitl'+ uav_str +'.launch' 31 | 32 | print(uav_str) 33 | print(os.system( 34 | "cp -r " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-1 " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str)) 35 | print(os.system( 36 | "mv " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-1.sdf " + PX4_HOME + "/Firmware/Tools/sitl_gazebo/models/f450-tmp-" + uav_str +"/f450-tmp-" + uav_str + ".sdf")) 37 | 38 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 39 | os.system( 40 | 'cp '+ PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-1 ' + PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 41 | 42 | replaceInFile(str(simulator_udp_port), str(simulator_udp_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 43 | replaceInFile(str(mavlink_start_port), str(mavlink_start_port+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 44 | replaceInFile(str(mavlink_onboard_local), str(mavlink_onboard_local+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 45 | replaceInFile(str(mavlink_onboard_remote), str(mavlink_onboard_remote+100*NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 46 | replaceInFile('MAV_SYS_ID 2', 'MAV_SYS_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 47 | replaceInFile('MAV_COMP_ID 2', 'MAV_COMP_ID ' + str(NUM), PX4_HOME + '/Firmware/posix-configs/SITL/init/lpe/f450-tmp-' + uav_str) 48 | 49 | replaceInFile('f450-1', 'f450-tmp-' + uav_str, PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str +'/f450-tmp-' + uav_str + '.sdf') 50 | replaceInFile('uav_camera', 51 | 'uav_' + uav_str + '_camera', 52 | PX4_HOME + '/Firmware/Tools/sitl_gazebo/models/f450-tmp-' + uav_str + '/f450-tmp-' + uav_str + '.sdf') 53 | 54 | launch_file = '$PX4_HOME/Firmware/launch/posix_sitl_multi_tmp.launch' 55 | 56 | uav_block = '' + \ 57 | '\n' + \ 58 | '\n' + \ 59 | '\n' + \ 62 | '\n' + \ 64 | '\n' 66 | 67 | 68 | file_block = uav_block + '\n' 69 | print(file_block) 70 | 71 | print(os.system("cp " + SOURCE + " " + DEST)) 72 | f=open(DEST,"a") 73 | f.write(file_block + '\n ') 74 | f.close() 75 | 76 | print('DRONES CREATED') 77 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/posix_sitl_openuav_swarm_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/testArmAll.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import subprocess 3 | import os 4 | import sys 5 | from std_msgs.msg import Float64; 6 | 7 | from mavros_msgs.srv import CommandBool, CommandTOL, SetMode 8 | from geometry_msgs.msg import PoseStamped,Pose,Vector3,Twist,TwistStamped 9 | from std_srvs.srv import Empty 10 | import time 11 | 12 | print 'init' 13 | 14 | cur_pose = PoseStamped() 15 | 16 | def pos_cb(msg): 17 | global cur_pose 18 | cur_pose = msg 19 | 20 | NUM_UAV = int(sys.argv[1]) 21 | 22 | mode_proxy = [None for i in range(NUM_UAV)] 23 | arm_proxy = [None for i in range(NUM_UAV)] 24 | 25 | def mavrosTopicStringRoot(uavID=0): 26 | print 'mavros' + str(uavID+1) 27 | return ('mavros' + str(uavID+1)) 28 | 29 | rospy.init_node('multi', anonymous=True) 30 | 31 | #Comm for drones 32 | for uavID in range(0,NUM_UAV): 33 | mode_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/set_mode', SetMode) 34 | arm_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool) 35 | 36 | print 'communication initialization complete' 37 | data = [None for i in range(NUM_UAV)] 38 | 39 | while None in data: 40 | for uavID in range(0, NUM_UAV): 41 | try: 42 | data[uavID] = rospy.wait_for_message(mavrosTopicStringRoot(uavID) + '/global_position/rel_alt', Float64, timeout=5) 43 | except: 44 | pass 45 | 46 | for uavID in range(0, NUM_UAV): 47 | print "wait for service" 48 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/set_mode') 49 | print "got service" 50 | 51 | rate = rospy.Rate(10) 52 | 53 | while not rospy.is_shutdown(): 54 | success = [None for i in range(NUM_UAV)] 55 | for uavID in range(0, NUM_UAV): 56 | try: 57 | success[uavID] = mode_proxy[uavID](1,'OFFBOARD') 58 | except rospy.ServiceException, e: 59 | print ("mavros/set_mode service call failed: %s"%e) 60 | 61 | success = [None for i in range(NUM_UAV)] 62 | for uavID in range(0, NUM_UAV): 63 | rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/cmd/arming') 64 | 65 | for uavID in range(0, NUM_UAV): 66 | try: 67 | success[uavID] = arm_proxy[uavID](True) 68 | except rospy.ServiceException, e: 69 | print ("mavros1/set_mode service call failed: %s"%e) 70 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/testCreateUAVSwarm.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | import rospy 3 | import sys 4 | import os 5 | from std_msgs.msg import Float64 6 | from geometry_msgs.msg import PoseStamped 7 | import roslaunch 8 | 9 | 10 | cur_pose = PoseStamped() 11 | NUM_UAV = sys.argv[1] 12 | process = subprocess.Popen(["/bin/bash","/root/src/Firmware/Tools/swarm.sh",NUM_UAV],stdout=subprocess.PIPE) 13 | process.wait() 14 | for line in process.stdout: 15 | print line 16 | 17 | launchfile = "posix_sitl_multi_tmp.launch" 18 | fullpath = os.path.join("/root/src/Firmware/launch/", launchfile) 19 | subprocess.Popen(["roslaunch",fullpath]) 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/test_kobuki.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /samples/turtlebot/inputs/setup/turtlebot_model.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /samples/turtlebot/launch.sh: -------------------------------------------------------------------------------- 1 | #launch the patrol sim 2 | #!/bin/bash 3 | nvidia-docker run -dit --net=openuavapp_default --name=openuavapp_TURTLE${3:-`date +%s`} -v /tmp/.X11-unix:/tmp/.X11-unix -v $(pwd):/simulation -e DISPLAY=:0 --entrypoint "/home/setup.sh" openuavapp_openuav 4 | 5 | docker ps 6 | 7 | 8 | -------------------------------------------------------------------------------- /samples/turtlebot/run_this.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "++++++++INIT++++++++++" 4 | 5 | source /simulation/inputs/parameters/swarm.sh 6 | source /opt/ros/kinetic/setup.bash 7 | source ~/catkin_ws/devel/setup.bash 8 | 9 | 10 | ## Previous clean-up 11 | rm -rf /root/src/Firmware/Tools/sitl_gazebo/models/f450-tmp-* 12 | rm -f /root/src/Firmware/posix-configs/SITL/init/lpe/f450-tmp-* 13 | rm -f /root/src/Firmware/launch/posix_sitl_multi_tmp.launch 14 | 15 | ## world setup # 16 | cp /simulation/inputs/world/empty.world /root/src/Firmware/Tools/sitl_gazebo/worlds/empty.world 17 | cp /simulation/inputs/models/f450-1/f450-1.sdf /root/src/Firmware/Tools/sitl_gazebo/models/f450-1/f450-1.sdf 18 | cp /simulation/inputs/setup/posix_sitl_openuav_swarm_base.launch /root/src/Firmware/launch/posix_sitl_openuav_swarm_base.launch 19 | 20 | 21 | mkdir /simulation/outputs 22 | rm -f /simulation/outputs/*.csv 23 | echo "Setup..." >> /tmp/debug 24 | 25 | 26 | python /simulation/inputs/setup/gen_gazebo_ros_spawn.py $num_uavs 27 | python /simulation/inputs/setup/gen_px4_sitl.py $num_uavs 28 | python /simulation/inputs/setup/gen_mavros.py $num_uavs 29 | 30 | for((i=1;i<=$num_uavs;i+=1)) 31 | do 32 | echo "px4 posix_sitl_multi_gazebo_ros$num_uavs.launch" 33 | echo "launching uav$i ..." >> /tmp/debug 34 | roslaunch px4 posix_sitl_multi_gazebo_ros$i.launch &> /dev/null & 35 | until rostopic echo /gazebo/model_states | grep -m1 f450-tmp-$i ; do : ; done 36 | roslaunch px4 posix_sitl_multi_px4_sitl$i.launch &> /dev/null & 37 | sleep 2 38 | roslaunch px4 posix_sitl_multi_mavros$i.launch &> /dev/null & 39 | until rostopic echo /mavros$i/state | grep -m1 "connected: True" ; do : ; done 40 | echo "launched uav$i ..." >> /tmp/debug 41 | done 42 | 43 | 44 | for ((i=1;i<=$num_ground;i+=1)) 45 | do 46 | export ROBOT_INITIAL_POSE="-x 3 -y $(($i * 3))" 47 | echo "launching turtlebot$i at $ROBOT_INITIAL_POSE" 48 | roslaunch /simulation/inputs/setup/test_kobuki.launch namespace:=turtlebot$i & 49 | sleep 5 50 | done 51 | 52 | 53 | python /simulation/inputs/measures/measureInterRobotDistance.py $num_uavs 1 &> /dev/null & 54 | roslaunch rosbridge_server rosbridge_websocket.launch ssl:=false &> /dev/null & 55 | rosrun web_video_server web_video_server _port:=80 _server_threads:=100 &> /dev/null & 56 | 57 | sleep 5 58 | 59 | 60 | for((i = 0;i<$num_uavs;i+=1)) 61 | do 62 | python /simulation/inputs/controllers/uavFollow.py $i $i $FOLLOW_D_GAIN &> /simulation/outputs/patroLog$i.txt & 63 | done 64 | sleep 1 65 | 66 | python /simulation/inputs/controllers/turtlebotLoop.py 0 1 1 &> /simulation/outputs/turtleLog$i.txt & 67 | 68 | ''' 69 | for((i = 0;i<$num_ground;i+=1)) 70 | do 71 | python /simulation/inputs/controllers/turtlebotLoop.py $i $num_uavs $FOLLOW_D_GAIN &> /simulation/outputs/turtleLog$i.txt & 72 | done 73 | sleep 1 74 | ''' 75 | echo "Scripts Launched" #>> /tmp/debug 76 | 77 | python /simulation/inputs/setup/testArmAll.py $num_uavs &> /dev/null & 78 | 79 | 80 | tensorboard --logdir=/simulation/outputs/ --port=8008 &> /dev/null & 81 | roslaunch opencv_apps general_contours.launch image:=/uav_1_camera_front/image_raw debug_view:=false &> /dev/null & 82 | 83 | echo "Measures..." #>> /tmp/debug 84 | for((i=1;i<=$num_uavs;i+=1)) 85 | do 86 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /mavros$i/local_position/odom > /simulation/outputs/uav$i.csv & 87 | done 88 | 89 | 90 | /usr/bin/python -u /opt/ros/kinetic/bin/rostopic echo -p /measure > /simulation/outputs/measure.csv & 91 | 92 | 93 | sleep $duration_seconds 94 | #cat /simulation/outputs/measure.csv | awk -F',' '{sum+=$2; ++n} END { print sum/n }' > /simulation/outputs/average_measure.txt 95 | 96 | -------------------------------------------------------------------------------- /tests/build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cd ./tests 3 | docker-compose up --detach 4 | cd ../ 5 | npm install roslib 6 | docker run -d --net=tests_default --name=openuavapp_x${3:-`date +%s`} -v /tmp/.X11-unix:/tmp/.X11-unix -v /home/travis/build/Open-UAV/openuav-playground/samples/leader-follower:/simulation -e DISPLAY=:0 --entrypoint "/home/setup.sh" dreamslab/openuavapp_openuav:latest -------------------------------------------------------------------------------- /tests/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '2' 2 | 3 | services: 4 | db: 5 | image: dreamslab/openuavapp_db:latest 6 | web: 7 | restart: always 8 | image: dreamslab/openuavapp_web:latest 9 | command: bash -c "while ! pg_isready -h db ; do sleep 1 ; done && python manage.py makemigrations && python manage.py migrate && python3 manage.py runserver 0.0.0.0:80" 10 | volumes: 11 | - .:/code 12 | ports: 13 | - "8001:80" 14 | depends_on: 15 | - db 16 | openuav: 17 | image: dreamslab/openuavapp_openuav:latest 18 | volumes: 19 | - ../samples/testSimulation:/simulation 20 | - /tmp/.X11-unix:/tmp/.X11-unix 21 | environment: 22 | - DISPLAY=$DISPLAY 23 | expose: 24 | - "8000" 25 | - "31819" 26 | - "80" 27 | - "9090" 28 | entrypoint: /home/setup.sh 29 | 30 | networks: 31 | default: 32 | ipam: 33 | driver: default 34 | config: 35 | - subnet: 172.28.0.0/16 36 | gateway: 172.28.0.1 37 | -------------------------------------------------------------------------------- /tests/measure.js: -------------------------------------------------------------------------------- 1 | var ROSLIB = require("roslib"); 2 | function measure() { 3 | ctr = 0 4 | measureTotal = 0 5 | websocket_url_str = 'ws://172.28.0.5:9090'; 6 | ros = new ROSLIB.Ros({ 7 | url: websocket_url_str 8 | }); 9 | listenerMeasure = new ROSLIB.Topic({ 10 | ros: ros, 11 | name: '/measure', 12 | messageType: 'std_msgs/Float64' 13 | }); 14 | listenerMeasure.subscribe(function(message) { 15 | measureTotal = measureTotal + parseFloat(message.data) 16 | ctr = ctr + 1; 17 | measureMean = measureTotal / ctr; 18 | console.log(measureMean); 19 | if (ctr == 250){ 20 | console.log("Exiting after 250 iterations"); 21 | process.exit(0); 22 | } 23 | }); 24 | ros.on('error', function(error) { 25 | console.log('Error connecting to websocket server: ', error.code); 26 | setTimeout(measure, 5000); 27 | }); 28 | } 29 | measure(); -------------------------------------------------------------------------------- /tests/test_setup.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | sudo apt-get -y update 3 | sudo apt-get install -y software-properties-common screen 4 | curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - 5 | sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" -y 6 | sudo apt-get update -y 7 | sudo apt-get install docker-ce -y 8 | sudo usermod -aG docker $USER 9 | sudo curl -L "https://github.com/docker/compose/releases/download/1.23.2/docker-compose-$(uname -s)-$(uname -m)" -o /usr/local/bin/docker-compose 10 | sudo chmod +x /usr/local/bin/docker-compose --------------------------------------------------------------------------------