Spiri
/
spiri-sdk
12
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases 1 Wiki Activity

working gazebo sim and sim_drone.py

This commit is contained in:
Burak Ozter 2024-10-25 14:49:07 -03:00
parent e56e3c06c0
commit 56456f5950
107 changed files with 16354 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
.env Normal file
View File

@ -0,0 +1,19 @@
DRONE_SYS_ID=1
#this is used for multi-vehicle sim. first vehicle would have these
TRACKER_INSTANCE=0
SERIAL0_PORT=5760
SITL_PORT=5501
MAVLINK_PORT=14560
#This is the vehicle ardupilot will use.
ARDUPILOT_VEHICLE="-v ArduCopte -f gazebo-iris --model=JSON"
#gazebo-sdk related
#Number of drones to be spawned in the gazebo world.
SIM_DRONE_COUNT=2
#This is not needed to be modified.
FDM_PORT_IN=9002
#The world file you want to use.
WORLD_FILE_NAME="citadel_hill_world.sdf"
#World name in your world file. Not the same as world file name necessarily.
WORLD_NAME="citadel_hill"
#Vehicle model folder name where model.config and model.xacro.sdf are present.
DRONE_MODEL="spiri_mu"

46
docker-compose.yml
View File

@ -4,6 +4,8 @@ services:
gui-tools: gui-tools:
runtime: nvidia runtime: nvidia
env_file:
- .env
build: build:
context: ./guiTools/ context: ./guiTools/
@ -28,7 +30,7 @@ services:
devices: devices:
# Provide access to GPU devices # Provide access to GPU devices
- /dev/dri:/dev/dri - /dev/dri:/dev/dri
# network_mode: host network_mode: host
ipc: host ipc: host
#user: "${UID}:${GID}" #user: "${UID}:${GID}"
privileged: true # Allow privileged access if necessary (e.g., for GPU access) privileged: true # Allow privileged access if necessary (e.g., for GPU access)
@ -45,28 +47,37 @@ services:
ardupilot: ardupilot:
container_name: ardupilot-$DRONE_SYS_ID
image: git.spirirobotics.com/spiri/ardupilot:spiri-master image: git.spirirobotics.com/spiri/ardupilot:spiri-master
profiles:
- uav-sim
env_file:
- .env
command: command:
- /bin/bash - /bin/bash
- -c - -c
- | - |
./Tools/autotest/sim_vehicle.py -v ArduCopter -f gazebo-iris --model=JSON --no-rebuild \ ./Tools/autotest/sim_vehicle.py $ARDUPILOT_VEHICLE --no-rebuild \
--enable-dds --sim-address=$(python3 -c 'import socket; print(socket.gethostbyname("gz-test"))') --no-mavproxy --no-mavproxy --enable-dds --sysid $DRONE_SYS_ID -I$TRACKER_INSTANCE
stdin_open: true stdin_open: true
tty: true tty: true
network_mode: host
mavproxy: mavproxy:
container_name: mavproxy-$DRONE_SYS_ID
image: git.spirirobotics.com/spiri/services-mavproxy:main image: git.spirirobotics.com/spiri/services-mavproxy:main
profiles:
- uav-sim
env_file:
- .env
command: > command: >
mavproxy.py --non-interactive mavproxy.py --non-interactive
--out tcpin:0.0.0.0:5760 --master tcp:127.0.0.1:$SERIAL0_PORT
--master tcp:ardupilot:5760 --out udpout:0.0.0.0:$MAVLINK_PORT
--out udpin:0.0.0.0:14550 --sitl 127.0.0.1:$SITL_PORT
--out udpin:0.0.0.0:14551 --out udp:0.0.0.0:14550
--out udpin:0.0.0.0:5000
restart: always restart: always
ports: network_mode: host
- 5760:5760
mavros: mavros:
#This service bridges our mavlink-based robot-coprosessor into ROS #This service bridges our mavlink-based robot-coprosessor into ROS
@ -91,16 +102,21 @@ services:
soft: 1024 soft: 1024
hard: 524288 hard: 524288
mavros2: mavros2:
#This service bridges our mavlink-based robot-coprosessor into ROS container_name: mavros2-$DRONE_SYS_ID
#In this example it connects to a simulated coprocessor.
image: git.spirirobotics.com/spiri/services-ros2-mavros:main image: git.spirirobotics.com/spiri/services-ros2-mavros:main
command: ros2 launch mavros px4.launch fcu_url:="udp://:14555@mavproxy:14551" tgt_system:="1" profiles:
#environment: - uav-sim
# - "ROS_MASTER_URI=http://ros-master:11311" env_file:
- .env
command: ros2 launch mavros apm.launch fcu_url:="udp://0.0.0.0:$MAVLINK_PORT@:14555" namespace:="spiri$DRONE_SYS_ID" tgt_system:="$DRONE_SYS_ID"
depends_on: depends_on:
ardupilot: ardupilot:
condition: service_started condition: service_started
mavproxy:
condition: service_started
restart: always restart: always
ipc: host
network_mode: host
deploy: deploy:
resources: resources:
limits: limits:

50
guiTools/Dockerfile
View File

@ -1,8 +1,54 @@
FROM osrf/ros:jazzy-desktop-full FROM osrf/ros:jazzy-desktop-full
RUN apt-get update ENV ROS_DISTRO=jazzy
RUN apt-get install qterminal -y RUN apt-get update -y
#Debugging tools
RUN apt-get install -y wget lsb-release curl gnupg net-tools inetutils-ping build-essential qterminal
#Gazebo Install
RUN curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
RUN apt-get update -y
RUN apt-get install -y nano \
libgz-sim8-dev \
rapidjson-dev \
libopencv-dev \
libgstreamer1.0-dev \
libgstreamer-plugins-base1.0-dev \
gstreamer1.0-plugins-bad \
gstreamer1.0-libav \
gstreamer1.0-gl \
gstreamer1.0-plugins-good \
gstreamer1.0-plugins-bad \
gstreamer1.0-plugins-ugly
RUN apt-get install -y \
ros-${ROS_DISTRO}-xacro \
ros-${ROS_DISTRO}-gazebo-msgs \
ros-${ROS_DISTRO}-ros-gz
ENV workspace=/home/gz-workspace/ardupilot_gazebo
WORKDIR ${workspace}/
#Burak: Copy ardupilot_gazebo files from local to the image.
# I didn't do git pull because this is working right now.
# I don't want to pull where github/ardupilot_gazebo might push breaking changes and this sdk would stop working in the next build.
COPY ./ardupilot_gazebo ${workspace}/
COPY --link --from=git.spirirobotics.com/spiri/gazebo-resources:main /models /${workspace}/models/
COPY --link --from=git.spirirobotics.com/spiri/gazebo-resources:main /worlds /${workspace}/worlds/
RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
RUN echo "export GZ_SIM_SYSTEM_PLUGIN_PATH=${workspace}/build:$GZ_SIM_SYSTEM_PLUGIN_PATH" >> ~/.bashrc
RUN echo "export GZ_SIM_RESOURCE_PATH=${workspace}/models:${workspace}/worlds:$GZ_SIM_RESOURCE_PATH" >> ~/.bashrc
ENV GZ_SIM_SYSTEM_PLUGIN_PATH=${workspace}/build
ENV GZ_SIM_RESOURCE_PATH=${workspace}/models:/home/gz-workspace/ardupilot_gazebo/worlds
WORKDIR ${workspace}/build
RUN cmake ..
RUN make -j16
RUN make install
WORKDIR ${workspace}
COPY ./spawn_drones.sh /spawn_drones.sh
RUN chmod +x /spawn_drones.sh
COPY ./launcher.py /launcher.py COPY ./launcher.py /launcher.py
CMD python3 /launcher.py CMD python3 /launcher.py

38
guiTools/ardupilot_gazebo/.github/CONTRIBUTING.md vendored Normal file
View File

@ -0,0 +1,38 @@
# How to contribute to the ArduPilot project?
If you are reading this page, you are possibly interested in contributing to our project. We have a very active (and friendly) developer group and would love to have the help! Possible ways you can help:
* Testing the code
* Filing issues on github, when you see a problem (or adding detail to existing issues that effect you)
* Fixing issues
* Adding new features
* Reviewing existing pull requests, and notifying the maintainer if it passes your code review.
# How to make a good bug report...
* Make sure your bug is not a support issue. Support issues should go to [the support forums](http://discuss.ardupilot.org) and include a .bin log file if possible. If you're not sure you have a bug, you should seek support first.
* Search for your bug, make sure it is not already reported. If it is already reported, make a comment on that issue.
* Only report one bug per issue report.
* Write a clear and concise summary. Be specific about what component of the software you are writing about, and follow the convention: "Copter: blah blah blah"
* Write a clear and concise description, with **particularly clear steps** to reproduce the problem. Include logs that display the bug. **Try to report only facts in your issue report, keeping your assumptions out of it.**
* The majority of issues open now are good or acceptable by these guidelines. Please refer to them for examples.
# Submitting patches
Please see our [wiki article](https://ardupilot.org/dev/docs/submitting-patches-back-to-master.html).
# Development Team
The ArduPilot project is open source and [maintained](https://github.com/ArduPilot/ardupilot#maintainers) by a team of volunteers.
To contribute, you can send a pull request on GitHub.
New developers are recommended to join the `#general` channel on
[Discord](https://ardupilot.org/discord).
You can also join the
[development discussion on Discourse](https://discuss.ardupilot.org/c/development-team),
or [Discord](https://ardupilot.org/discord).
Note that these are NOT for user tech support, and are moderated
for new users to prevent off-topic discussion.

36
guiTools/ardupilot_gazebo/.github/ISSUE_TEMPLATE/bug_report.md vendored Normal file
View File

@ -0,0 +1,36 @@
---
name: Bug report
about: If you are sure you have found a bug, then choose this to open a bug report
---
**IF YOU DON'T REMOVE THESE FOUR LINES, THEN YOUR REPORT WILL BE CLOSED AUTOMATICALLY**
Questions and user problems should be directed at the forum (http://discuss.ardupilot.org)
_**Please be very sure you have found a bug when opening this issue**_
If there was a previous discussion in the forum, link to it
## Bug report
**Issue details**
_Please describe the problem_
**Version**
_What version was the issue encountered with_
**Platform**
[ ] All
[ ] AntennaTracker
[ ] Copter
[ ] Plane
[ ] Rover
[ ] Submarine
**Airframe type**
_What type of airframe (flying wing, glider, hex, Y6, octa etc)_
**Hardware type**
_What autopilot hardware was used? (Pixhawk, Cube, Pixracer, Navio2, etc)_
**Logs**
_Please provide a link to any relevant logs that show the issue_

5
guiTools/ardupilot_gazebo/.github/ISSUE_TEMPLATE/config.yml vendored Normal file
View File

@ -0,0 +1,5 @@
blank_issues_enabled: false
contact_links:
- name: User Support and General Questions
url: https://discuss.ardupilot.org/
about: Please ask on https://discuss.ardupilot.org/

32
guiTools/ardupilot_gazebo/.github/ISSUE_TEMPLATE/feature_request.md vendored Normal file
View File

@ -0,0 +1,32 @@
---
name: Feature request
about: If you have an idea for a new feature, choose this option
---
**IF YOU DON'T REMOVE THESE FOUR LINES, THEN YOUR REQUEST WILL BE CLOSED AUTOMATICALLY**
Questions and user problems should be directed at the forum (http://discuss.ardupilot.org)
_**Please do a careful search before opening this, there are already a lot of feature requests**_
If there was a previous discussion in the forum, link to it
## Feature request
**Is your feature request related to a problem? Please describe.**
_A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]_
**Describe the solution you'd like**
_A clear and concise description of what you want to happen._
**Describe alternatives you've considered**
_A clear and concise description of any alternative solutions or features you've considered._
**Platform**
[ ] All
[ ] AntennaTracker
[ ] Copter
[ ] Plane
[ ] Rover
[ ] Submarine
**Additional context**
_Add any other context or screenshots about the feature request here._

13
guiTools/ardupilot_gazebo/.github/SUPPORT.md vendored Normal file
View File

@ -0,0 +1,13 @@
# Support
Our GitHub isn't the appropriate place for getting support on ArduPilot usage. Please look below for the two options you have available for getting help solving any issue you have using ArduPilot.
If you are having trouble with **code development** please ask in our Discord: https://ardupilot.org/discord
## Free support
If you want free, community-based support, please post in our forum: http://discuss.ardupilot.org
## Commercial support
If you need fast, paid support, please consult our wiki page and choose a company: http://ardupilot.org/ardupilot/docs/common-commercial-support.html

10
guiTools/ardupilot_gazebo/.github/workflows/ccache.env vendored Normal file
View File

@ -0,0 +1,10 @@
# common ccache env vars for CI
export CCACHE_SLOPPINESS=file_stat_matches
mkdir -p ~/.ccache
echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
echo "compression = true" >> ~/.ccache/ccache.conf
echo "compression_level = 6" >> ~/.ccache/ccache.conf
echo "max_size = 400M" >> ~/.ccache/ccache.conf
ccache -s
ccache -z

21
guiTools/ardupilot_gazebo/.github/workflows/ccpcheck.yml vendored Normal file
View File

@ -0,0 +1,21 @@
# GitHub Action to run cppcheck
#
name: cppcheck
on: [push, pull_request]
jobs:
cpplint:
runs-on: ubuntu-22.04
name: cppcheck
steps:
- name: Checkout
uses: actions/checkout@v3
- name: Install cppcheck
run: |
sudo apt-get update
sudo apt-get install cppcheck
- name: Run cppcheck
run: |
cppcheck --std=c++17 ./include/*.hh
cppcheck --std=c++17 ./src/*.cc

28
guiTools/ardupilot_gazebo/.github/workflows/ccplint.yml vendored Normal file
View File

@ -0,0 +1,28 @@
# GitHub Action to run cpplint
#
# The cpplint configuration file is:
#
# ./CPPLINT.cfg
#
name: ccplint
on: [push, pull_request]
jobs:
cpplint:
runs-on: ubuntu-22.04
name: cpplint
steps:
- name: Checkout
uses: actions/checkout@v3
- name: Setup
uses: actions/setup-python@v4
with:
python-version: '3.10'
- name: Install cpplint
run: |
pip install cpplint
- name: Run cpplint
run: |
cpplint ./include/*.hh
cpplint ./src/*.cc

70
guiTools/ardupilot_gazebo/.github/workflows/ubuntu-build.yml vendored Normal file
View File

@ -0,0 +1,70 @@
name: ubuntu-build
on: [push, pull_request, workflow_dispatch]
# paths:
# - "*"
# - "!README.md" <-- don't rebuild on doc change
concurrency:
group: ci-${{github.workflow}}-${{ github.ref }}
cancel-in-progress: true
jobs:
build:
runs-on: ubuntu-22.04
name: ubuntu-build
steps:
- name: Checkout
uses: actions/checkout@v3
with:
submodules: 'recursive'
- name: Install Build Dependencies
shell: bash
run: |
sudo apt update && sudo apt install --no-install-recommends -y \
lsb-release \
sudo \
software-properties-common \
wget \
make \
cmake \
ccache \
g++ \
git
sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
sudo apt update && sudo apt install --no-install-recommends -y \
rapidjson-dev \
libopencv-dev \
libunwind-dev \
libgstreamer1.0-dev \
libgstreamer-plugins-base1.0-dev \
gstreamer1.0-plugins-bad \
gstreamer1.0-libav \
gstreamer1.0-gl \
gz-harmonic
# Put ccache into github cache for faster build
- name: Prepare ccache timestamp
id: ccache_cache_timestamp
run: |
NOW=$(date -u +"%F-%T")
echo "::set-output name=timestamp::${NOW}"
- name: Cache ccache files
uses: actions/cache@v2
with:
path: ~/.ccache
key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
restore-keys: ${{github.workflow}}-ccache- # restore ccache from either previous build on this branch or on master
- name: Setup ccache
run: |
. .github/workflows/ccache.env
- name: Build
shell: bash
run: |
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
make -j4

105
guiTools/ardupilot_gazebo/.gitignore vendored Normal file
View File

@ -0,0 +1,105 @@
build/
Build/
bin/
lib/
msg_gen/
srv_gen/
msg/*Action.msg
msg/*ActionFeedback.msg
msg/*ActionGoal.msg
msg/*ActionResult.msg
msg/*Feedback.msg
msg/*Goal.msg
msg/*Result.msg
msg/_*.py
# Generated by dynamic reconfigure
*.cfgc
/cfg/cpp/
/cfg/*.py
# Ignore generated docs
*.dox
*.wikidoc
# eclipse stuff
.project
.cproject
# vscode
.vscode
.devcontainer
# qcreator stuff
CMakeLists.txt.user
srv/_*.py
*.pcd
*.pyc
qtcreator-*
*.user
/planning/cfg
/planning/docs
/planning/src
*~
# Emacs
.#*
# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm
# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
# Catch everything
.idea/
.cmake-build-*
# User-specific stuff:
.idea/**/workspace.xml
.idea/**/tasks.xml
.idea/dictionaries
# Sensitive or high-churn files:
.idea/**/dataSources/
.idea/**/dataSources.ids
.idea/**/dataSources.xml
.idea/**/dataSources.local.xml
.idea/**/sqlDataSources.xml
.idea/**/dynamic.xml
.idea/**/uiDesigner.xml
# Gradle:
.idea/**/gradle.xml
.idea/**/libraries
# Mongo Explorer plugin:
.idea/**/mongoSettings.xml
## File-based project format:
*.iws
## Plugin-specific files:
# IntelliJ
/out/
# mpeltonen/sbt-idea plugin
.idea_modules/
# JIRA plugin
atlassian-ide-plugin.xml
# Cursive Clojure plugin
.idea/replstate.xml
# Crashlytics plugin (for Android Studio and IntelliJ)
com_crashlytics_export_strings.xml
crashlytics.properties
crashlytics-build.properties
fabric.properties
cmake-build-debug
# Catkin custom files
CATKIN_IGNORE

155
guiTools/ardupilot_gazebo/CMakeLists.txt Normal file
View File

@ -0,0 +1,155 @@
cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
project(ardupilot_gazebo)
# --------------------------------------------------------------------------- #
# If ament_cmake is found build as an ament package, otherwise ignore.
# This is so the system may be built for Gazebo only, if ROS is not available.
find_package(ament_cmake QUIET)
if(${ament_cmake_FOUND})
message("Building ${PROJECT_NAME} as an `ament_cmake` project.")
endif()
# --------------------------------------------------------------------------- #
# Compile as C++14.
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# --------------------------------------------------------------------------- #
# Find gz-sim and dependencies.
# Harmonic (default)
if("$ENV{GZ_VERSION}" STREQUAL "harmonic" OR NOT DEFINED "ENV{GZ_VERSION}")
find_package(gz-cmake3 REQUIRED)
set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR})
gz_find_package(gz-common5 REQUIRED)
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
gz_find_package(gz-rendering8 REQUIRED)
set(GZ_RENDERING_VER ${gz-rendering8_VERSION_MAJOR})
gz_find_package(gz-sim8 REQUIRED)
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Harmonic")
# Garden
elseif("$ENV{GZ_VERSION}" STREQUAL "garden")
find_package(gz-cmake3 REQUIRED)
set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR})
gz_find_package(gz-common5 REQUIRED)
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
gz_find_package(gz-rendering7 REQUIRED)
set(GZ_RENDERING_VER ${gz-rendering7_VERSION_MAJOR})
gz_find_package(gz-sim7 REQUIRED)
set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Garden")
else()
message(FATAL_ERROR "Unsupported GZ_VERSION: $ENV{GZ_VERSION}")
endif()
# --------------------------------------------------------------------------- #
find_package(RapidJSON REQUIRED)
find_package(OpenCV REQUIRED)
pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0)
# --------------------------------------------------------------------------- #
# Build plugin.
add_library(ArduPilotPlugin
SHARED
src/ArduPilotPlugin.cc
src/SocketUDP.cc
src/Util.cc
)
target_include_directories(ArduPilotPlugin PRIVATE
include
)
target_link_libraries(ArduPilotPlugin PRIVATE
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
)
add_library(ParachutePlugin
SHARED
src/ParachutePlugin.cc
)
target_include_directories(ParachutePlugin PRIVATE
include
)
target_link_libraries(ParachutePlugin PRIVATE
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
)
add_library(CameraZoomPlugin
SHARED
src/CameraZoomPlugin.cc
)
target_include_directories(CameraZoomPlugin PRIVATE
include
)
target_link_libraries(CameraZoomPlugin PRIVATE
gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER}
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
)
add_library(GstCameraPlugin
SHARED
src/GstCameraPlugin.cc
)
target_include_directories(GstCameraPlugin PRIVATE
include
${OpenCV_INCLUDE_DIRS}
${GST_INCLUDE_DIRS}
)
target_link_libraries(GstCameraPlugin PRIVATE
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
${OpenCV_LIBS}
${GST_LINK_LIBRARIES}
)
# --------------------------------------------------------------------------- #
# Install.
install(
TARGETS
ArduPilotPlugin
ParachutePlugin
CameraZoomPlugin
GstCameraPlugin
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY
config/
DESTINATION share/${PROJECT_NAME}/config
)
install(
DIRECTORY
models/
DESTINATION share/${PROJECT_NAME}/models
)
install(
DIRECTORY
worlds/
DESTINATION share/${PROJECT_NAME}/worlds
)
# --------------------------------------------------------------------------- #
# Register as an ament package if ament_cmake is available.
if(${ament_cmake_FOUND})
ament_environment_hooks(
"${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
ament_environment_hooks(
"${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.sh.in")
ament_package()
endif()

60
guiTools/ardupilot_gazebo/CPPLINT.cfg Normal file
View File

@ -0,0 +1,60 @@
# filters
#
# 1. [-whitespace/braces]
# { should almost always be at the end of the previous line
#
# Allow:
# bool SocketUDP::pollin(uint32_t timeout_ms)
# {
#
# Instead of:
# bool SocketUDP::pollin(uint32_t timeout_ms) {
#
# 2. [-runtime/references]
# Is this a non-const reference? If so, make const or use a pointer
#
# Allow:
# void get_client_address(const char *&ip_addr, uint16_t &port);
#
# Instead of:
# void get_client_address(const char *&ip_addr, uint16_t *port);
#
# 3. [-whitespace/indent]
# private: should be indented +1 space
#
# 4. [-whitespace/blank_line]
# Do not leave a blank line after "private:"
#
# 3 and 4 are to allow Gazebo Sim class formatting where each
# function / method must have an access specifier.
#
#
# 5. [-whitespace/newline]
# An else should appear on the same line as the preceding }
#
# Allow:
# }
# else if (time > this->dataPtr->lastUpdateTime)
# {
#
# Instead of:
# } else if (time > this->dataPtr->lastUpdateTime) {
#
# 6. [-build/include_subdir]
# Include the directory when naming header files
#
# This prevent an error when including the plugin headers as:
# #include "GimbalSmall2dPlugin.hh"
#
# 7. [-build/c++11], [+build/c++14]
#
# This is to allow headers not approved for C++11 such as <chrono> etc.
#
# 8. [-legal/copyright]
# Do not require a copyright header in each file
#
set noparent
filter=-whitespace/braces,-runtime/references,-whitespace/indent,-whitespace/blank_line,-whitespace/newline,-build/include_subdir,-build/c++11,+build/c++14,-legal/copyright
linelength=80
root=include

73
guiTools/ardupilot_gazebo/LICENSE Normal file
View File

@ -0,0 +1,73 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files.
"Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions:
(a) You must give any other recipients of the Work or Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License.
You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives.
Copyright 2024 unsalted_salt
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

166
guiTools/ardupilot_gazebo/LICENSE.md Normal file
View File

@ -0,0 +1,166 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
This version of the GNU Lesser General Public License incorporates
the terms and conditions of version 3 of the GNU General Public
License, supplemented by the additional permissions listed below.
0. Additional Definitions.
As used herein, "this License" refers to version 3 of the GNU Lesser
General Public License, and the "GNU GPL" refers to version 3 of the GNU
General Public License.
"The Library" refers to a covered work governed by this License,
other than an Application or a Combined Work as defined below.
An "Application" is any work that makes use of an interface provided
by the Library, but which is not otherwise based on the Library.
Defining a subclass of a class defined by the Library is deemed a mode
of using an interface provided by the Library.
A "Combined Work" is a work produced by combining or linking an
Application with the Library. The particular version of the Library
with which the Combined Work was made is also called the "Linked
Version".
The "Minimal Corresponding Source" for a Combined Work means the
Corresponding Source for the Combined Work, excluding any source code
for portions of the Combined Work that, considered in isolation, are
based on the Application, and not on the Linked Version.
The "Corresponding Application Code" for a Combined Work means the
object code and/or source code for the Application, including any data
and utility programs needed for reproducing the Combined Work from the
Application, but excluding the System Libraries of the Combined Work.
1. Exception to Section 3 of the GNU GPL.
You may convey a covered work under sections 3 and 4 of this License
without being bound by section 3 of the GNU GPL.
2. Conveying Modified Versions.
If you modify a copy of the Library, and, in your modifications, a
facility refers to a function or data to be supplied by an Application
that uses the facility (other than as an argument passed when the
facility is invoked), then you may convey a copy of the modified
version:
a) under this License, provided that you make a good faith effort to
ensure that, in the event an Application does not supply the
function or data, the facility still operates, and performs
whatever part of its purpose remains meaningful, or
b) under the GNU GPL, with none of the additional permissions of
this License applicable to that copy.
3. Object Code Incorporating Material from Library Header Files.
The object code form of an Application may incorporate material from
a header file that is part of the Library. You may convey such object
code under terms of your choice, provided that, if the incorporated
material is not limited to numerical parameters, data structure
layouts and accessors, or small macros, inline functions and templates
(ten or fewer lines in length), you do both of the following:
a) Give prominent notice with each copy of the object code that the
Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the object code with a copy of the GNU GPL and this license
document.
4. Combined Works.
You may convey a Combined Work under terms of your choice that,
taken together, effectively do not restrict modification of the
portions of the Library contained in the Combined Work and reverse
engineering for debugging such modifications, if you also do each of
the following:
a) Give prominent notice with each copy of the Combined Work that
the Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the Combined Work with a copy of the GNU GPL and this license
document.
c) For a Combined Work that displays copyright notices during
execution, include the copyright notice for the Library among
these notices, as well as a reference directing the user to the
copies of the GNU GPL and this license document.
d) Do one of the following:
0) Convey the Minimal Corresponding Source under the terms of this
License, and the Corresponding Application Code in a form
suitable for, and under terms that permit, the user to
recombine or relink the Application with a modified version of
the Linked Version to produce a modified Combined Work, in the
manner specified by section 6 of the GNU GPL for conveying
Corresponding Source.
1) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (a) uses at run time
a copy of the Library already present on the user's computer
system, and (b) will operate properly with a modified version
of the Library that is interface-compatible with the Linked
Version.
e) Provide Installation Information, but only if you would otherwise
be required to provide such information under section 6 of the
GNU GPL, and only to the extent that such information is
necessary to install and execute a modified version of the
Combined Work produced by recombining or relinking the
Application with a modified version of the Linked Version. (If
you use option 4d0, the Installation Information must accompany
the Minimal Corresponding Source and Corresponding Application
Code. If you use option 4d1, you must provide the Installation
Information in the manner specified by section 6 of the GNU GPL
for conveying Corresponding Source.)
5. Combined Libraries.
You may place library facilities that are a work based on the
Library side by side in a single library together with other library
facilities that are not Applications and are not covered by this
License, and convey such a combined library under terms of your
choice, if you do both of the following:
a) Accompany the combined library with a copy of the same work based
on the Library, uncombined with any other library facilities,
conveyed under the terms of this License.
b) Give prominent notice with the combined library that part of it
is a work based on the Library, and explaining where to find the
accompanying uncombined form of the same work.
6. Revised Versions of the GNU Lesser General Public License.
The Free Software Foundation may publish revised and/or new versions
of the GNU Lesser General Public License from time to time. Such new
versions will be similar in spirit to the present version, but may
differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the
Library as you received it specifies that a certain numbered version
of the GNU Lesser General Public License "or any later version"
applies to it, you have the option of following the terms and
conditions either of that published version or of any later version
published by the Free Software Foundation. If the Library as you
received it does not specify a version number of the GNU Lesser
General Public License, you may choose any version of the GNU Lesser
General Public License ever published by the Free Software Foundation.
If the Library as you received it specifies that a proxy can decide
whether future versions of the GNU Lesser General Public License shall
apply, that proxy's public statement of acceptance of any version is
permanent authorization for you to choose that version for the
Library.

344
guiTools/ardupilot_gazebo/README.md Normal file
View File

@ -0,0 +1,344 @@
# ArduPilot Gazebo Plugin
[![ubuntu-build](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ubuntu-build.yml/badge.svg)](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ubuntu-build.yml)
[![ccplint](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccplint.yml/badge.svg)](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccplint.yml)
[![cppcheck](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccpcheck.yml/badge.svg)](https://github.com/ArduPilot/ardupilot_gazebo/actions/workflows/ccpcheck.yml)
This is the official ArduPilot plugin for [Gazebo](https://gazebosim.org/home).
It replaces the previous
[`ardupilot_gazebo`](https://github.com/khancyr/ardupilot_gazebo)
plugin and provides support for the recent releases of the Gazebo simulator
[(Gazebo Garden)](https://gazebosim.org/docs/garden/install) and [(Gazebo Harmonic)](https://gazebosim.org/docs/harmonic/install).
It also adds the following features:
- More flexible data exchange between SITL and Gazebo using JSON.
- Additional sensors supported.
- True simulation lockstepping. It is now possible to use GDB to stop
the Gazebo time for debugging.
- Improved 3D rendering using the `ogre2` rendering engine.
The project comprises a Gazebo plugin to connect to ArduPilot SITL
(Software In The Loop) and some example models and worlds.
## Prerequisites
Gazebo Garden or Harmonic is supported on Ubuntu 22.04 (Jammy).
Harmonic is recommended.
If you are running Ubuntu as a virtual machine you will need at least
Ubuntu 20.04 in order to have the OpenGL support required for the
`ogre2` render engine. Gazebo and ArduPilot SITL will also run on macOS
(Big Sur, Monterey and Venturua; Intel and M1 devices).
Follow the instructions for a binary install of
[Gazebo Garden](https://gazebosim.org/docs/garden/install) or [Gazebo Harmonic](https://gazebosim.org/docs/harmonic/install)
and verify that Gazebo is running correctly.
Set up an [ArduPilot development environment](https://ardupilot.org/dev/index.html).
In the following it is assumed that you are able to run ArduPilot SITL using
the [MAVProxy GCS](https://ardupilot.org/mavproxy/index.html).
## Installation
Install additional dependencies:
### Ubuntu
#### Garden (apt)
Manual - Gazebo Garden Dependencies:
```bash
sudo apt update
sudo apt install libgz-sim7-dev rapidjson-dev
sudo apt install libopencv-dev libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl
```
#### Harmonic (apt)
Manual - Gazebo Harmonic Dependencies:
```bash
sudo apt update
sudo apt install libgz-sim8-dev rapidjson-dev
sudo apt install libopencv-dev libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl
```
#### Rosdep
Use rosdep with
[osrf's rosdep rules](https://github.com/osrf/osrf-rosdep?tab=readme-ov-file#1-use-rosdep-to-resolve-gazebo-libraries)
to manage all dependencies. This is driven off of the environment variable `GZ_VERSION`.
```bash
export GZ_VERSION=harmonic # or garden
sudo bash -c 'wget https://raw.githubusercontent.com/osrf/osrf-rosdep/master/gz/00-gazebo.list -O /etc/ros/rosdep/sources.list.d/00-gazebo.list'
rosdep update
rosdep resolve gz-harmonic # or gz-garden
# Navigate to your ROS workspace before the next command.
rosdep install --from-paths src --ignore-src -y
```
### macOS
```bash
brew update
brew install rapidjson
brew install opencv gstreamer
```
Ensure the `GZ_VERSION` environment variable is set to either
`garden` or `harmonic`.
Clone the repo and build:
```bash
git clone https://github.com/ArduPilot/ardupilot_gazebo
cd ardupilot_gazebo
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
make -j4
```
## Configure
Set the Gazebo environment variables in your `.bashrc` or `.zshrc` or in
the terminal used to run Gazebo.
#### Terminal
Assuming that you have cloned the repository to `$HOME/ardupilot_gazebo`:
```bash
export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH
export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH
```
#### .bashrc or .zshrc
Assuming that you have cloned the repository to `$HOME/ardupilot_gazebo`:
```bash
echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:${GZ_SIM_SYSTEM_PLUGIN_PATH}' >> ~/.bashrc
echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:${GZ_SIM_RESOURCE_PATH}' >> ~/.bashrc
```
Reload your terminal with `source ~/.bashrc` (or `source ~/.zshrc` on macOS).
## Usage
### 1. Iris quad-copter
#### Run Gazebo
```bash
gz sim -v4 -r iris_runway.sdf
```
The `-v4` parameter is not mandatory, it shows additional information and is
useful for troubleshooting.
#### Run ArduPilot SITL
To run an ArduPilot simulation with Gazebo, the frame should have `gazebo-`
in it and have `JSON` as model. Other commandline parameters are the same
as usual on SITL.
```bash
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --map --console
```
#### Arm and takeoff
```bash
STABILIZE> mode guided
GUIDED> arm throttle
GUIDED> takeoff 5
```
### 2. Zephyr delta wing
The Zephyr delta wing is positioned on the runway for vertical take-off.
#### Run Gazebo
```bash
gz sim -v4 -r zephyr_runway.sdf
```
#### Run ArduPilot SITL
```bash
sim_vehicle.py -v ArduPlane -f gazebo-zephyr --model JSON --map --console
```
#### Arm, takeoff and circle
```bash
MANUAL> mode fbwa
FBWA> arm throttle
FBWA> rc 3 1800
FBWA> mode circle
```
#### Increase the simulation speed
The `zephyr_runway.sdf` world has a `<physics>` element configured to run
faster than real time:
```xml
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>-1.0</real_time_factor>
</physics>
```
To see the effect of the speed-up set the param `SIM_SPEEDUP` to a value
greater than one:
```bash
MANUAL> param set SIM_SPEEDUP 10
```
### 3. Streaming camera video
Images from camera sensors may be streamed with GStreamer using
the `GstCameraPlugin` sensor plugin. The example gimbal models include the
plugin element:
```xml
<plugin name="GstCameraPlugin"
filename="GstCameraPlugin">
<udp_host>127.0.0.1</udp_host>
<udp_port>5600</udp_port>
<use_basic_pipeline>true</use_basic_pipeline>
<use_cuda>false</use_cuda>
</plugin>
```
The `<image_topic>` and `<enable_topic>` parameters are deduced from the
topic name for the camera sensor, but may be overriden if required.
The `gimbal.sdf` world includes a 3 degrees of freedom gimbal with a
zoomable camera. To start streaming:
```bash
gz topic -t /world/gimbal/model/mount/model/gimbal/link/pitch_link/sensor/camera/image/enable_streaming -m gz.msgs.Boolean -p "data: 1"
```
Display the streamed video:
```bash
gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false
```
View the streamed camera frames in [QGC](http://qgroundcontrol.com/):
`Open QGC > Application Settings > Video Settings > Select UDP h.264 Video Stream & use port 5600`
![qgc_video_settings](https://github.com/user-attachments/assets/61fa4c2a-37e2-47cf-abcf-9f110d9c2015)
### 4. Using 3d Gimbal
The Iris model is equipped with a 3d gimbal and camera that can be controlled directly in MAVProxy using the RC overrides.
#### Run Gazebo
```bash
gz sim -v4 -r iris_runway.sdf
```
#### Run ArduPilot SITL with a specified parameter file
```bash
cd ardupilot
sim_vehicle.py -D -v ArduCopter -f JSON --add-param-file=$HOME/ardupilot_gazebo/config/gazebo-iris-gimbal.parm --console --map
```
Control action for gimbal over RC channel:
| Action | Channel | RC Low | RC High |
| ------------- | ------------- | ------------- | ------------- |
| Roll | RC6 | Roll Left | Roll Right |
| Pitch | RC7 | Pitch Down | Pitch Up |
| Yaw | RC8 | Yaw Left | Yaw Right |
Example usage:
`rc 6 1100` - Gimbal rolls left
`rc 7 1900` - Gimbal pitch upwards
`rc 8 1500` - Gimbal yaw neutral
## Models
In addition to the Iris and Zephyr models included here, a selection
of models configured use the ArduPilot Gazebo plugin is available in
[ArduPilot/SITL_Models](https://github.com/ArduPilot/SITL_Models).
Click on the images to see further details.
<table>
<tr>
<td title="Alti Transition">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/AltiTransition.md">
<img src="https://user-images.githubusercontent.com/24916364/150612555-958a64d4-c434-4f90-94bd-678e6b6011ec.png" width="100%" style="display: block;">
</a>
</td>
<td title="SkyCat TVBS">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/SkyCatTVBS.md">
<img src="https://user-images.githubusercontent.com/24916364/145025150-4e7e48e1-3e83-4c83-be7b-b944db1d9152.png" width="100%" style="display: block;">
</a>
</td>
<td title="Skywalker X8">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/SkywalkerX8.md">
<img src="https://user-images.githubusercontent.com/24916364/142733947-1a39e963-0aea-4b1b-a57b-85455b2278fe.png" width="100%" style="display: block;">
</a>
</td>
</tr>
<tr>
<td title="Quadruped">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/Quadruped.md">
<img src="https://user-images.githubusercontent.com/24916364/144449710-5bab34b4-dabf-410f-b276-d290ddbb54b2.gif" width="100%" style="display: block;">
</a>
</td>
<td title="WildThumper">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/WildThumper.md">
<img src="https://user-images.githubusercontent.com/24916364/144286154-231ac9b3-e54b-489f-b35e-bc2adb4b1aa0.png" width="100%" style="display: block;">
</a>
</td>
<td title="Rover Playpen">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/RoverPlayPen.md">
<img src="https://user-images.githubusercontent.com/24916364/144513412-1b0661f1-fdf8-4aed-a745-e8bb73ffca91.jpg" width="100%" style="display: block;">
</a>
</td>
</tr>
</td>
</tr>
<tr>
<td title="Swan-K1">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/Swan-K1.md">
<img src="https://user-images.githubusercontent.com/24916364/210408630-01e5f56d-57ba-430e-b04d-62cb8d232527.png" width="100%" style="display: block;">
</a>
</td>
<td title="Sawppy Rover">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/Sawppy.md">
<img src="https://user-images.githubusercontent.com/24916364/210653579-e635ffc2-2962-4221-83a8-9622915a4121.png" width="100%" style="display: block;">
</a>
</td>
<td title="Hexapod Copter">
<a href="https://github.com/ArduPilot/SITL_Models/blob/master/Gazebo/docs/HexapodCopter.md">
<img src="https://user-images.githubusercontent.com/24916364/225340320-9aa31fe2-4602-4036-ba6b-491f72097c01.jpg" width="100%" style="display: block;">
</a>
</td>
</tr>
</table>
## Troubleshooting
For issues concerning installing and running Gazebo on your platform please
consult the Gazebo documentation for [troubleshooting frequent issues](https://gazebosim.org/docs/harmonic/troubleshooting#ubuntu).

23
guiTools/ardupilot_gazebo/cmake/cmake_uninstall.cmake.in Normal file
View File

@ -0,0 +1,23 @@
# Source: https://gitlab.kitware.com/cmake/community/-/wikis/FAQ#can-i-do-make-uninstall-with-cmake
if(NOT EXISTS "@CMAKE_BINARY_DIR@/install_manifest.txt")
message(FATAL_ERROR "Cannot find install manifest: @CMAKE_BINARY_DIR@/install_manifest.txt")
endif()
file(READ "@CMAKE_BINARY_DIR@/install_manifest.txt" files)
string(REGEX REPLACE "\n" ";" files "${files}")
foreach(file ${files})
message(STATUS "Uninstalling $ENV{DESTDIR}${file}")
if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
exec_program(
"@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\""
OUTPUT_VARIABLE rm_out
RETURN_VALUE rm_retval
)
if(NOT "${rm_retval}" STREQUAL 0)
message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}")
endif()
else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
message(STATUS "File $ENV{DESTDIR}${file} does not exist.")
endif()
endforeach()

32
guiTools/ardupilot_gazebo/config/gazebo-iris-gimbal.parm Normal file
View File

@ -0,0 +1,32 @@
# Iris is X frame
FRAME_CLASS 1
FRAME_TYPE 1
# Match servo out for motors
MOT_PWM_MIN 1100
MOT_PWM_MAX 1900
# Gimbal on mount 1 has 3 DOF
MNT1_TYPE 1 # Servo
MNT1_PITCH_MAX 45
MNT1_PITCH_MIN -135
MNT1_ROLL_MAX 30
MNT1_ROLL_MIN -30
MNT1_YAW_MAX 160
MNT1_YAW_MIN -160
# Gimbal RC in
RC6_MAX 1900
RC6_MIN 1100
RC6_OPTION 212 # Mount1 Roll
RC7_MAX 1900
RC7_MIN 1100
RC7_OPTION 213 # Mount1 Pitch
RC8_MAX 1900
RC8_MIN 1100
RC8_OPTION 214 # Mount1 Yaw
# Gimbal servo out
SERVO9_FUNCTION 8 # Mount1Roll
SERVO10_FUNCTION 7 # Mount1Pitch
SERVO11_FUNCTION 6 # Mount1Yaw

3
guiTools/ardupilot_gazebo/entrypoint.sh Executable file
View File

@ -0,0 +1,3 @@
#!/bin/bash
source ~/.bashrc
exec "$@"

4
guiTools/ardupilot_gazebo/hooks/ardupilot_gazebo.dsv.in Normal file
View File

@ -0,0 +1,4 @@
prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share;@CMAKE_INSTALL_PREFIX@/share
prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/models
prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/worlds
prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/

3
guiTools/ardupilot_gazebo/hooks/ardupilot_gazebo.sh.in Normal file
View File

@ -0,0 +1,3 @@
ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/models"
ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/@PROJECT_NAME@/worlds"
ament_prepend_unique_value GZ_SIM_PLUGIN_PATH "$AMENT_CURRENT_PREFIX/lib/@PROJECT_NAME@"

186
guiTools/ardupilot_gazebo/include/ArduPilotPlugin.hh Executable file
View File

@ -0,0 +1,186 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef ARDUPILOTPLUGIN_HH_
#define ARDUPILOTPLUGIN_HH_
#include <array>
#include <memory>
#include <gz/sim/System.hh>
#include <sdf/sdf.hh>
namespace gz
{
namespace sim
{
namespace systems
{
/// \todo(srmainwaring) handle 16 or 32 based on magic
// The servo packet received from ArduPilot SITL. Defined in SIM_JSON.h.
struct servo_packet_16 {
uint16_t magic; // 18458 expected magic value
uint16_t frame_rate;
uint32_t frame_count;
uint16_t pwm[16];
};
struct servo_packet_32 {
uint16_t magic; // 29569 expected magic value
uint16_t frame_rate;
uint32_t frame_count;
uint16_t pwm[32];
};
// Forward declare private data class
class ArduPilotSocketPrivate;
class ArduPilotPluginPrivate;
/// \brief Interface ArduPilot from ardupilot stack
/// modeled after SITL/SIM_*
///
/// The plugin requires the following parameters:
/// <control> control description block
/// <!-- inputs from Ardupilot -->
/// "channel" attribute, ardupilot control channel
/// <multiplier> command multiplier
/// <offset> command offset
/// <servo_max> upper limit for PWM input
/// <servo_min> lower limit for PWM input
/// <!-- output to Gazebo -->
/// <type> type of control, VELOCITY, POSITION, EFFORT or COMMAND
/// <useForce> 1 if joint forces are applied, 0 to set joint directly
/// <p_gain> velocity pid p gain
/// <i_gain> velocity pid i gain
/// <d_gain> velocity pid d gain
/// <i_max> velocity pid max integral correction
/// <i_min> velocity pid min integral correction
/// <cmd_max> velocity pid max command torque
/// <cmd_min> velocity pid min command torque
/// <jointName> motor joint, torque applied here
/// <cmd_topic> topic to publish commands that are processed
/// by other plugins
///
/// <turningDirection> rotor turning direction, 'cw' or 'ccw'
/// <frequencyCutoff> filter incoming joint state
/// <samplingRate> sampling rate for filtering incoming joint state
/// <rotorVelocitySlowdownSim> for rotor aliasing problem, experimental
///
/// <imuName> scoped name for the imu sensor
/// <anemometer> scoped name for the wind sensor
/// <connectionTimeoutMaxCount> timeout before giving up on
/// controller synchronization
/// <have_32_channels> set true if 32 channels are enabled
///
class GZ_SIM_VISIBLE ArduPilotPlugin:
public gz::sim::System,
public gz::sim::ISystemConfigure,
public gz::sim::ISystemPostUpdate,
public gz::sim::ISystemPreUpdate,
public gz::sim::ISystemReset
{
/// \brief Constructor.
public: ArduPilotPlugin();
/// \brief Destructor.
public: ~ArduPilotPlugin();
public: void Reset(const UpdateInfo &_info,
EntityComponentManager &_ecm) final;
/// \brief Load configuration from SDF on startup.
public: void Configure(const gz::sim::Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
gz::sim::EntityComponentManager &_ecm,
gz::sim::EventManager &_eventMgr) final;
/// \brief Do the part of one update loop that involves making
/// changes to simulation.
public: void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) final;
/// \brief Do the part of one update loop that involves
/// reading results from simulation.
public: void PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm) final;
/// \brief Load control channels
private: void LoadControlChannels(
sdf::ElementPtr _sdf,
gz::sim::EntityComponentManager &_ecm);
/// \brief Load IMU sensors
private: void LoadImuSensors(
sdf::ElementPtr _sdf,
gz::sim::EntityComponentManager &_ecm);
/// \brief Load GPS sensors
private: void LoadGpsSensors(
sdf::ElementPtr _sdf,
gz::sim::EntityComponentManager &_ecm);
/// \brief Load range sensors
private: void LoadRangeSensors(
sdf::ElementPtr _sdf,
gz::sim::EntityComponentManager &_ecm);
/// \brief Load wind sensors
private: void LoadWindSensors(
sdf::ElementPtr _sdf,
gz::sim::EntityComponentManager &_ecm);
/// \brief Update the control surfaces controllers.
/// \param[in] _info Update information provided by the server.
private: void OnUpdate();
/// \brief Update PID Joint controllers.
/// \param[in] _dt time step size since last update.
private: void ApplyMotorForces(
const double _dt,
gz::sim::EntityComponentManager &_ecm);
/// \brief Reset PID Joint controllers.
private: void ResetPIDs();
/// \brief Receive a servo packet from ArduPilot
///
/// Returns true if a servo packet was received, otherwise false.
private: bool ReceiveServoPacket();
/// \brief Update the motor commands given servo PWM values
private: void UpdateMotorCommands(const std::array<uint16_t, 32> &_pwm);
/// \brief Create the state JSON
private: void CreateStateJSON(
double _simTime,
const gz::sim::EntityComponentManager &_ecm) const;
/// \brief Send state to ArduPilot
private: void SendState() const;
/// \brief Initialise flight dynamics model socket
private: bool InitSockets(sdf::ElementPtr _sdf) const;
/// \brief Private data pointer.
private: std::unique_ptr<ArduPilotPluginPrivate> dataPtr;
};
} // namespace systems
} // namespace sim
} // namespace gz
#endif // ARDUPILOTPLUGIN_HH_

67
guiTools/ardupilot_gazebo/include/CameraZoomPlugin.hh Normal file
View File

@ -0,0 +1,67 @@
/*
Copyright (C) 2023 ArduPilot.org
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef CAMERAZOOMPLUGIN_HH_
#define CAMERAZOOMPLUGIN_HH_
#include <memory>
#include <gz/sim/System.hh>
namespace gz {
namespace sim {
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems {
/// \brief Camera zoom plugin.
class CameraZoomPlugin :
public System,
public ISystemConfigure,
public ISystemPreUpdate,
public ISystemPostUpdate
{
/// \brief Destructor
public: virtual ~CameraZoomPlugin();
/// \brief Constructor
public: CameraZoomPlugin();
// Documentation inherited
public: void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) final;
// Documentation inherited
public: void PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm) final;
// Documentation inherited
public: void Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &) final;
/// \internal
/// \brief Private implementation
private: class Impl;
private: std::unique_ptr<Impl> impl;
};
} // namespace systems
}
} // namespace sim
} // namespace gz
#endif // CAMERAZOOMPLUGIN_HH_

93
guiTools/ardupilot_gazebo/include/GstCameraPlugin.hh Normal file
View File

@ -0,0 +1,93 @@
/*
* Copyright (C) 2024 ArduPilot
*/
/*
* Copyright (C) 2012-2016 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef GSTCAMERAPLUGIN_HH_
#define GSTCAMERAPLUGIN_HH_
#include <memory>
#include <gz/sim/System.hh>
namespace gz {
namespace sim {
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems {
/// \brief Plugin to stream camera sensor data using GStreamer.
/// \class GstCameraPlugin
///
/// A Gazebo plugin that can be attached to a camera and then streams the
/// video data using gstreamer.
///
/// Parameters
/// <udp_host> the UDP host IP, defaults to 127.0.0.1
/// <udp_port> the UDP port, defaults to 5600
/// <rtmp_location> the RTMP location
/// <use_basic_pipeline> set to true if not using <rtmp_location>
/// <use_cuda> set to true to use CUDA (if available)
/// <image_topic> the camera image topic
/// <enable_topic> the topic to enable / disable video streaming
///
/// Start streaming
/// assumes: <enable_topic>/camera/enable_streaming<enable_topic>
///
/// gz topic -t "/camera/enable_streaming" -m gz.msgs.Boolean -p "data: 1"
///
/// Connect to the stream via command line and open an OpenGL window:
///
/// gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp,
/// media=(string)video, clock-rate=(int)90000,
/// encoding-name=(string)H264'
/// ! rtph264depay ! avdec_h264 ! videoconvert
/// ! autovideosink sync=false
///
class GstCameraPlugin :
public System,
public ISystemConfigure,
public ISystemPreUpdate
{
/// \brief Destructor
public: virtual ~GstCameraPlugin();
/// \brief Constructor
public: GstCameraPlugin();
// Documentation inherited
public: void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) final;
// Documentation inherited
public: void Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &) final;
/// \internal
/// \brief Private implementation
private: class Impl;
private: std::unique_ptr<Impl> impl;
};
} // namespace systems
}
} // namespace sim
} // namespace gz
#endif // GSTCAMERAPLUGIN_HH_

80
guiTools/ardupilot_gazebo/include/ParachutePlugin.hh Normal file
View File

@ -0,0 +1,80 @@
/*
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef PARACHUTEPLUGIN_HH_
#define PARACHUTEPLUGIN_HH_
#include <memory>
#include <gz/sim/System.hh>
namespace gz {
namespace sim {
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems {
/// \brief Parachute releaase plugin which may be attached to a model.
///
/// ## System Parameters:
///
/// `<parent_link>` The link in the target model to attach the parachute.
/// Required.
///
/// `<child_model>` The name of the parachute model.
/// Required.
///
/// `<child_link>` The base link of the parachute model (bridle point).
/// Required.
///
/// `<child_pose>` The relative pose of parent link to the child link.
/// The default value is: `0, 0, 0, 0, 0, 0`.
///
/// `<cmd_topic>` The topic to receive the parachute release command.
/// The default value is: `/model/<model_name>/parachute/cmd_release`.
///
class ParachutePlugin :
public System,
public ISystemPreUpdate,
public ISystemConfigure
{
/// \brief Destructor
public: virtual ~ParachutePlugin();
/// \brief Constructor
public: ParachutePlugin();
// Documentation inherited
public: void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) final;
// Documentation inherited
public: void Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &) final;
/// \internal
/// \brief Private implementation
private: class Impl;
private: std::unique_ptr<Impl> impl;
};
} // namespace systems
}
} // namespace sim
} // namespace gz
#endif // PARACHUTEPLUGIN_HH_

81
guiTools/ardupilot_gazebo/include/SelectionBuffer.hh Normal file
View File

@ -0,0 +1,81 @@
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef SELECTIONBUFFER_HH_
#define SELECTIONBUFFER_HH_
#include <memory>
#include <string>
#include <gazebo/util/system.hh>
namespace Ogre
{
class Entity;
class RenderTarget;
class SceneManager;
} // Ogre
namespace gazebo
{
namespace rendering
{
struct SelectionBufferPrivate;
class GZ_RENDERING_VISIBLE SelectionBuffer
{
/// \brief Constructor
/// \param[in] _camera Name of the camera to generate a selection
/// buffer for.
/// \param[in] _mgr Pointer to the scene manager.
/// \param[in] _renderTarget Pointer to the render target.
public: SelectionBuffer(const std::string &_cameraName,
Ogre::SceneManager *_mgr, Ogre::RenderTarget *_renderTarget);
/// \brief Destructor
public: ~SelectionBuffer();
/// \brief Handle on mouse click
/// \param[in] _x X coordinate in pixels.
/// \param[in] _y Y coordinate in pixels.
/// \return Returns the Ogre entity at the coordinate.
public: Ogre::Entity *OnSelectionClick(int _x, int _y);
/// \brief Debug show overlay
/// \param[in] _show True to show the selection buffer in an overlay.
public: void ShowOverlay(bool _show);
/// \brief Call this to update the selection buffer contents
public: void Update();
/// \brief Delete the render texture
private: void DeleteRTTBuffer();
/// \brief Create the render texture
private: void CreateRTTBuffer();
/// \brief Create the selection buffer offscreen render texture.
private: void CreateRTTOverlays();
/// \internal
/// \brief Pointer to private data.
private: std::unique_ptr<SelectionBufferPrivate> dataPtr;
};
} // namespace rendering
} // namespace gazebo
#endif // SELECTIONBUFFER_HH_

81
guiTools/ardupilot_gazebo/include/SocketUDP.hh Normal file
View File

@ -0,0 +1,81 @@
/*
Copyright (C) 2024 ardupilot.org
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef SOCKETUDP_HH_
#define SOCKETUDP_HH_
#include <fcntl.h>
#include <unistd.h>
#ifdef _WIN32
#include <winsock2.h>
#include <Ws2tcpip.h>
#else
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <sys/select.h>
#endif
/// \brief Simple UDP socket handling class.
class SocketUDP {
public:
/// \brief Constructor.
SocketUDP(bool reuseaddress, bool blocking);
/// \brief Destructor.
~SocketUDP();
/// \brief Bind socket to address and port.
bool bind(const char *address, uint16_t port);
/// \brief Set reuse address option.
bool set_reuseaddress();
/// \brief Set blocking state.
bool set_blocking(bool blocking);
/// \brief Send data to address and port.
ssize_t
sendto(const void *buf, size_t size, const char *address, uint16_t port);
/// \brief Receive data.
ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms);
/// \brief Get last client address and port
void get_client_address(const char *&ip_addr, uint16_t &port);
private:
/// \brief File descriptor.
struct sockaddr_in in_addr{};
/// \brief File descriptor.
int fd = -1;
/// \brief Poll for incoming data with timeout.
bool pollin(uint32_t timeout_ms);
/// \brief Make a sockaddr_in struct from address and port.
void make_sockaddr(const char *address, uint16_t port,
struct sockaddr_in &sockaddr);
};
#endif // SOCKETUDP_HH_

60
guiTools/ardupilot_gazebo/include/Util.hh Normal file
View File

@ -0,0 +1,60 @@
/*
Copyright (C) 2022 ardupilot.org
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <string>
#include <unordered_set>
#include <vector>
#include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/Export.hh>
namespace gz
{
namespace sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE {
/// \brief Helper function to get an entity given its unscoped name.
///
/// \param[in] _name Entity's unscoped name.
/// \param[in] _ecm Immutable reference to ECM.
/// \param[in] _relativeTo Entity that the unscoped name is relative to.
/// If not provided, the unscoped name could be relative to any entity.
/// \return All entities that match the unscoped name and relative to
/// requirements, or an empty set otherwise.
std::unordered_set<Entity> EntitiesFromUnscopedName(
const std::string &_name, const EntityComponentManager &_ecm,
Entity _relativeTo = kNullEntity);
/// \brief Get the ID of a joint entity which is a descendent of this model.
///
/// A replacement for gz::sim::Model::JointByName which does not resolve
/// joints for nested models.
/// \param[in] _ecm Entity-component manager.
/// \param[in] _entity Model entity.
/// \param[in] _name Scoped joint name.
/// \return Joint entity.
Entity JointByName(EntityComponentManager &_ecm,
Entity _modelEntity,
const std::string &_name);
}
} // namespace sim
} // namespace gz

63
guiTools/ardupilot_gazebo/models/gimbal_small_1d/meshes/base_arm.dae Normal file
View File

File diff suppressed because one or more lines are too long

63
guiTools/ardupilot_gazebo/models/gimbal_small_1d/meshes/base_main.dae Normal file
View File

File diff suppressed because one or more lines are too long

63
guiTools/ardupilot_gazebo/models/gimbal_small_1d/meshes/tilt.dae Normal file
View File

File diff suppressed because one or more lines are too long

20
guiTools/ardupilot_gazebo/models/gimbal_small_1d/model.config Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0" ?>
<model>
<name>Gimbal Small 1D</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>anonymous</name>
<email>anonymous</email>
</author>
<description>
1D gimbal for small camera (go pro size) on drone.
3D mesh: http://www.thingiverse.com/thing:397579
by: http://www.thingiverse.com/Motorpixiegimbals/about.
20 December, 2022: model retrieved from
http://models.gazebosim.org/gimbal_small_2d/
and modified for Gazebo Sim (material scripts replaced).
Renamed as this version has 1 DOF.
</description>
</model>

153
guiTools/ardupilot_gazebo/models/gimbal_small_1d/model.sdf Normal file
View File

@ -0,0 +1,153 @@
<?xml version='1.0'?>
<sdf version='1.9'>
<model name='gimbal_small_1d'>
<pose>0 0 0.18 0 0 0</pose>
<link name='base_link'>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
</inertia>
</inertial>
<visual name='base_main_viz'>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_1d/meshes/base_main.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.1 0.1 0.1</ambient>
<diffuse>0.1 0.1 0.1</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
</material>
</visual>
<visual name='base_arm_viz'>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_1d/meshes/base_arm.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.1 0.1 0.1</ambient>
<diffuse>0.1 0.1 0.1</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
</material>
</visual>
<collision name='base_col'>
<pose>0.01 0.075 -0.025 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.05 0.15</size>
</box>
</geometry>
</collision>
</link>
<link name='tilt_link'>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name='tilt_viz'>
<pose>0 0 -0.005 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_1d/meshes/tilt.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.4 0.4 0.4</ambient>
<diffuse>0.4 0.4 0.4</diffuse>
<specular>0.1 0.1 0.1 1.0</specular>
</material>
</visual>
<collision name='tilt_col'>
<pose>0 0 -0.005 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_1d/meshes/tilt.dae</uri>
</mesh>
</geometry>
</collision>
<visual name='camera_viz'>
<pose>0 0 0.02 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.050</length>
</cylinder>
</geometry>
<material>
<ambient>0.4 0.4 0.4</ambient>
<diffuse>0.4 0.4 0.4</diffuse>
<specular>0.1 0.1 0.1 1.0</specular>
</material>
</visual>
<collision name='camera_col'>
<pose>0 0 0.02 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.050</length>
</cylinder>
</geometry>
</collision>
<sensor name="camera" type="camera">
<pose degrees="true">0 0 0 -90 -90 0</pose>
<camera>
<horizontal_fov>2.0</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>15000</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>1</visualize>
<plugin name="GstCameraPlugin"
filename="GstCameraPlugin">
<udp_host>127.0.0.1</udp_host>
<udp_port>5600</udp_port>
<use_basic_pipeline>true</use_basic_pipeline>
<use_cuda>false</use_cuda>
</plugin>
</sensor>
</link>
<joint name='tilt_joint' type='revolute'>
<parent>base_link</parent>
<child>tilt_link</child>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<damping>0.01</damping>
</dynamics>
<limit>
<lower>-3.1415926</lower>
<upper>3.1415926</upper>
</limit>
</axis>
<pose>0 0 0.02 0 0 0</pose>
</joint>
</model>
</sdf>

63
guiTools/ardupilot_gazebo/models/gimbal_small_2d/meshes/base_arm.dae Normal file
View File

File diff suppressed because one or more lines are too long

63
guiTools/ardupilot_gazebo/models/gimbal_small_2d/meshes/base_main.dae Normal file
View File

File diff suppressed because one or more lines are too long

63
guiTools/ardupilot_gazebo/models/gimbal_small_2d/meshes/tilt.dae Normal file
View File

File diff suppressed because one or more lines are too long

20
guiTools/ardupilot_gazebo/models/gimbal_small_2d/model.config Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0" ?>
<model>
<name>Gimbal Small 2D</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>anonymous</name>
<email>anonymous</email>
</author>
<description>
3D gimbal for small camera (go pro size) on drone.
3D mesh: http://www.thingiverse.com/thing:397579
by: http://www.thingiverse.com/Motorpixiegimbals/about.
20 December, 2022: model retrieved from
http://models.gazebosim.org/gimbal_small_2d/
and modified for Gazebo Sim (material scripts replaced)
Enable arm joint to provide 2 DOF.
</description>
</model>

189
guiTools/ardupilot_gazebo/models/gimbal_small_2d/model.sdf Normal file
View File

@ -0,0 +1,189 @@
<?xml version='1.0'?>
<sdf version='1.9'>
<model name='gimbal_small_2d'>
<pose>0 0 0.18 0 0 0</pose>
<link name='base_link'>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
</inertia>
</inertial>
<visual name='base_main_viz'>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_2d/meshes/base_main.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.1 0.1 0.1</ambient>
<diffuse>0.1 0.1 0.1</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
</material>
</visual>
<collision name='base_col'>
<pose>0.01 0.075 -0.025 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.05 0.15</size>
</box>
</geometry>
</collision>
</link>
<link name='roll_link'>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name='roll_viz'>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_2d/meshes/base_arm.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.1 0.1 0.1</ambient>
<diffuse>0.1 0.1 0.1</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
</material>
</visual>
<collision name='roll_col'>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_2d/meshes/base_arm.dae</uri>
</mesh>
</geometry>
</collision>
</link>
<joint name='roll_joint' type='revolute'>
<parent>base_link</parent>
<child>roll_link</child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.01</damping>
</dynamics>
<limit>
<lower>-3.1415926</lower>
<upper>3.1415926</upper>
</limit>
</axis>
<pose>0.01 0 -0.04 0 0 0</pose>
</joint>
<link name='tilt_link'>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name='tilt_viz'>
<pose>0 0 -0.005 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_2d/meshes/tilt.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.4 0.4 0.4</ambient>
<diffuse>0.4 0.4 0.4</diffuse>
<specular>0.1 0.1 0.1 1.0</specular>
</material>
</visual>
<collision name='tilt_col'>
<pose>0 0 -0.005 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://gimbal_small_2d/meshes/tilt.dae</uri>
</mesh>
</geometry>
</collision>
<visual name='camera_viz'>
<pose>0 0 0.02 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.050</length>
</cylinder>
</geometry>
<material>
<ambient>0.4 0.4 0.4</ambient>
<diffuse>0.4 0.4 0.4</diffuse>
<specular>0.1 0.1 0.1 1.0</specular>
</material>
</visual>
<collision name='camera_col'>
<pose>0 0 0.02 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.050</length>
</cylinder>
</geometry>
</collision>
<sensor name="camera" type="camera">
<pose degrees="true">0 0 0 -90 -90 0</pose>
<camera>
<horizontal_fov>2.0</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>15000</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>1</visualize>
<plugin name="GstCameraPlugin"
filename="GstCameraPlugin">
<udp_host>127.0.0.1</udp_host>
<udp_port>5600</udp_port>
<use_basic_pipeline>true</use_basic_pipeline>
<use_cuda>false</use_cuda>
</plugin>
</sensor>
</link>
<joint name='tilt_joint' type='revolute'>
<parent>roll_link</parent>
<child>tilt_link</child>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<damping>0.01</damping>
</dynamics>
<limit>
<lower>-3.1415926</lower>
<upper>3.1415926</upper>
</limit>
</axis>
<pose>0 0 0.02 0 0 0</pose>
</joint>
</model>
</sdf>

676
guiTools/ardupilot_gazebo/models/gimbal_small_3d/meshes/base_plate.dae Normal file
View File

File diff suppressed because one or more lines are too long

448
guiTools/ardupilot_gazebo/models/gimbal_small_3d/meshes/camera_enclosure.dae Normal file
View File

File diff suppressed because one or more lines are too long

526
guiTools/ardupilot_gazebo/models/gimbal_small_3d/meshes/roll_arm.dae Normal file
View File

File diff suppressed because one or more lines are too long

552
guiTools/ardupilot_gazebo/models/gimbal_small_3d/meshes/yaw_arm.dae Normal file
View File

File diff suppressed because one or more lines are too long

21
guiTools/ardupilot_gazebo/models/gimbal_small_3d/model.config Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0" ?>
<model>
<name>Gimbal Small 3D</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Rhys Mainwaring</name>
<email>rhys.mainwaring@me.com</email>
</author>
<description>
MotorPixie 3D Gimbal for DJI Phantom 2 Vision - Discontinued.
3D mesh: https://www.thingiverse.com/thing:391130
by: http://www.thingiverse.com/Motorpixiegimbals/about.
11 May, 2023: model retrieved from
https://www.thingiverse.com/thing:391130
and modified for Gazebo in FreeCAD and Blender.
</description>
</model>

240
guiTools/ardupilot_gazebo/models/gimbal_small_3d/model.sdf Normal file
View File

@ -0,0 +1,240 @@
<?xml version="1.0"?>
<sdf version="1.9">
<model name="gimbal_small_3d">
<pose>0 0 0.18 0 0 0</pose>
<link name="base_link">
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
</inertia>
</inertial>
<visual name="base_visual">
<geometry>
<mesh>
<uri>model://gimbal_small_3d/meshes/base_plate.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.1 0.1 0.1</ambient>
<diffuse>0.1 0.1 0.1</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
</material>
</visual>
<collision name="base_collision">
<pose>0.01 0.075 -0.025 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.05 0.15</size>
</box>
</geometry>
</collision>
</link>
<link name="yaw_link">
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name="yaw_visual">
<geometry>
<mesh>
<uri>model://gimbal_small_3d/meshes/yaw_arm.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.1 0.1 0.1</ambient>
<diffuse>0.1 0.1 0.1</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
</material>
</visual>
<collision name="yaw_collision">
<geometry>
<mesh>
<uri>model://gimbal_small_3d/meshes/yaw_arm.dae</uri>
</mesh>
</geometry>
</collision>
</link>
<joint name="yaw_joint" type="revolute">
<parent>base_link</parent>
<child>yaw_link</child>
<axis>
<xyz>0 1 0</xyz>
<dynamics>
<damping>0.01</damping>
</dynamics>
<limit>
<lower>-3.1415926</lower>
<upper>3.1415926</upper>
</limit>
</axis>
<pose>0.0105 0.065 -0.002 0 0 0</pose>
</joint>
<link name="roll_link">
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name="roll_visual">
<geometry>
<mesh>
<uri>model://gimbal_small_3d/meshes/roll_arm.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.1 0.1 0.1</ambient>
<diffuse>0.1 0.1 0.1</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
</material>
</visual>
<collision name="roll_collision">
<geometry>
<mesh>
<uri>model://gimbal_small_3d/meshes/roll_arm.dae</uri>
</mesh>
</geometry>
</collision>
</link>
<joint name="roll_joint" type="revolute">
<parent>yaw_link</parent>
<child>roll_link</child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.01</damping>
</dynamics>
<limit>
<lower>-3.1415926</lower>
<upper>3.1415926</upper>
</limit>
</axis>
<pose>0.0099 0.002 -0.05 0 0 0</pose>
</joint>
<link name="pitch_link">
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<visual name="pitch_visual">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://gimbal_small_3d/meshes/camera_enclosure.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.4 0.4 0.4</ambient>
<diffuse>0.4 0.4 0.4</diffuse>
<specular>0.1 0.1 0.1 1.0</specular>
</material>
</visual>
<collision name="pitch_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://gimbal_small_3d/meshes/camera_enclosure.dae</uri>
</mesh>
</geometry>
</collision>
<visual name="camera_visual">
<pose>0 0 0.02 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.050</length>
</cylinder>
</geometry>
<material>
<ambient>0.4 0.4 0.4</ambient>
<diffuse>0.4 0.4 0.4</diffuse>
<specular>0.1 0.1 0.1 1.0</specular>
</material>
</visual>
<collision name="camera_collision">
<pose>0 0 0.02 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.025</radius>
<length>0.050</length>
</cylinder>
</geometry>
</collision>
<sensor name="camera" type="camera">
<pose>0 0 0 -1.57 -1.57 0</pose>
<camera>
<horizontal_fov>2.0</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>15000</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>1</visualize>
<plugin filename="CameraZoomPlugin"
name="CameraZoomPlugin">
<max_zoom>125.0</max_zoom>
<slew_rate>0.42514285714</slew_rate>
</plugin>
<plugin name="GstCameraPlugin"
filename="GstCameraPlugin">
<udp_host>127.0.0.1</udp_host>
<udp_port>5600</udp_port>
<use_basic_pipeline>true</use_basic_pipeline>
<use_cuda>false</use_cuda>
</plugin>
</sensor>
</link>
<joint name="pitch_joint" type="revolute">
<parent>roll_link</parent>
<child>pitch_link</child>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<damping>0.01</damping>
</dynamics>
<limit>
<lower>-3.1415926</lower>
<upper>3.1415926</upper>
</limit>
</axis>
<pose>0.045 0.0021 0.0199 0 0 0</pose>
</joint>
</model>
</sdf>

16
guiTools/ardupilot_gazebo/models/ground_plane/model.config Normal file
View File

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>Ground Plane</name>
<version>1.0</version>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>Nate Koenig</name>
<email>nate@osrfoundation.org</email>
</author>
<description>
A simple ground plane.
</description>
</model>

42
guiTools/ardupilot_gazebo/models/ground_plane/model.sdf Normal file
View File

@ -0,0 +1,42 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<collide_bitmask>0xffff</collide_bitmask>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
</link>
</model>
</sdf>

38
guiTools/ardupilot_gazebo/models/iris_with_ardupilot/model.config Executable file
View File

@ -0,0 +1,38 @@
<?xml version="1.0"?>
<model>
<name>Iris with ArduPilot</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Fadri Furrer</name>
<email>fadri.furrer@mavt.ethz.ch</email>
</author>
<author>
<name>Michael Burri</name>
</author>
<author>
<name>Mina Kamel</name>
</author>
<author>
<name>Janosch Nikolic</name>
</author>
<author>
<name>Markus Achtelik</name>
</author>
<maintainer email="hsu@osrfoundation.org">john hsu</maintainer>
<description>
Starting with iris_with_standoffs
add LiftDragPlugin
add ArduCopterPlugin
</description>
<depend>
<model>
<uri>model://iris_with_standoffs</uri>
<version>1.0</version>
</model>
</depend>
</model>

882
guiTools/ardupilot_gazebo/models/iris_with_ardupilot/model.sdf Executable file
View File

@ -0,0 +1,882 @@
<?xml version='1.0'?>
<sdf version="1.9">
<model name="iris_with_ardupilot">
<include>
<uri>model://iris_with_standoffs</uri>
</include>
<!--
visual markers for debugging
- blade root - teal
- blade tip - orange
- blade cp - yellow
- blade forward - red
- blade upward - blue
-->
<!--
<link name="rotor_0_blade_1_cp">
<pose>0.13 -0.22 0.216 0 0 0</pose>
<visual name='rotor_0_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_tip'>
<pose>0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_cp'>
<pose>0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_cp_forward'>
<pose>0.084 0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_cp_upward'>
<pose>0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<link name="rotor_0_blade_2_cp">
<gravity>0</gravity>
<pose>0.13 -0.22 0.216 0 0 0</pose>
<visual name='rotor_0_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_tip'>
<pose>-0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_cp'>
<pose>-0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_cp_forward'>
<pose>-0.084 -0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_0_visual_cp_upward'>
<pose>-0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_0_blade_1_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_0</parent>
<child>rotor_0_blade_1_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="rotor_0_blade_2_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_0</parent>
<child>rotor_0_blade_2_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="rotor_1_blade_1_cp">
<gravity>0</gravity>
<pose>-0.13 0.2 0.216 0 0 0</pose>
<visual name='rotor_1_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_tip'>
<pose>0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_cp'>
<pose>0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_cp_forward'>
<pose>0.084 0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_cp_upward'>
<pose>0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<link name="rotor_1_blade_2_cp">
<gravity>0</gravity>
<pose>-0.13 0.2 0.216 0 0 0</pose>
<visual name='rotor_1_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_tip'>
<pose>-0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_cp'>
<pose>-0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_cp_forward'>
<pose>-0.084 -0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_1_visual_cp_upward'>
<pose>-0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_1_blade_1_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_1</parent>
<child>rotor_1_blade_1_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="rotor_1_blade_2_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_1</parent>
<child>rotor_1_blade_2_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="rotor_2_blade_1_cp">
<gravity>0</gravity>
<pose>0.13 0.22 0.216 0 0 0</pose>
<visual name='rotor_2_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_tip'>
<pose>0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_cp'>
<pose>0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_cp_forward'>
<pose>0.084 -0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_cp_upward'>
<pose>0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<link name="rotor_2_blade_2_cp">
<gravity>0</gravity>
<pose>0.13 0.22 0.216 0 0 0</pose>
<visual name='rotor_2_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_tip'>
<pose>-0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_cp'>
<pose>-0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_cp_forward'>
<pose>-0.084 0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_2_visual_cp_upward'>
<pose>-0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_2_blade_1_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_2</parent>
<child>rotor_2_blade_1_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="rotor_2_blade_2_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_2</parent>
<child>rotor_2_blade_2_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="rotor_3_blade_1_cp">
<gravity>0</gravity>
<pose>-0.13 -0.2 0.216 0 0 0</pose>
<visual name='rotor_3_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_tip'>
<pose>0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_cp'>
<pose>0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_cp_forward'>
<pose>0.084 -0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_cp_upward'>
<pose>0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<link name="rotor_3_blade_2_cp">
<gravity>0</gravity>
<pose>-0.13 -0.2 0.216 0 0 0</pose>
<visual name='rotor_3_visual_root'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 1 0.5</ambient>
<diffuse>0 1 1 0.5</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_tip'>
<pose>-0.12 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
<material>
<ambient>1.0 0.65 0</ambient>
<diffuse>1.0 0.65 0</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_cp'>
<pose>-0.084 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_cp_forward'>
<pose>-0.084 0.02 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
</material>
</visual>
<visual name='rotor_3_visual_cp_upward'>
<pose>-0.084 0 0.02 0 0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
</link>
<joint name="rotor_3_blade_1_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_3</parent>
<child>rotor_3_blade_1_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<joint name="rotor_3_blade_2_cp_joint" type="revolute">
<parent>iris_with_standoffs::rotor_3</parent>
<child>rotor_3_blade_2_cp</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
-->
<!-- plugins -->
<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_0_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_1_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_2_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_3_joint</joint_name>
</plugin>
<plugin name="ArduPilotPlugin"
filename="ArduPilotPlugin">
<!-- Port settings -->
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<have_32_channels>0</have_32_channels>
<!-- Frame conventions
Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
-->
<modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
<!-- Sensors -->
<imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<control channel="0">
<jointName>iris_with_standoffs::rotor_0_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="1">
<jointName>iris_with_standoffs::rotor_1_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="2">
<jointName>iris_with_standoffs::rotor_2_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="3">
<jointName>iris_with_standoffs::rotor_3_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
</plugin>
</model>
</sdf>

45
guiTools/ardupilot_gazebo/models/iris_with_gimbal/model.config Executable file
View File

@ -0,0 +1,45 @@
<?xml version="1.0"?>
<model>
<name>Iris with Gimbal</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Fadri Furrer</name>
<email>fadri.furrer@mavt.ethz.ch</email>
</author>
<author>
<name>Michael Burri</name>
</author>
<author>
<name>Mina Kamel</name>
</author>
<author>
<name>Janosch Nikolic</name>
</author>
<author>
<name>Markus Achtelik</name>
</author>
<author>
<name>Rhys Mainwaring</name>
</author>
<description>
Starting with iris_with_standoffs
add LiftDragPlugin
add ArduCopterPlugin
add gimbal_small_2d
</description>
<depend>
<model>
<uri>model://gimbal_small_2d</uri>
<version>2.0</version>
</model>
<model>
<uri>model://iris_with_standoffs</uri>
<version>2.0</version>
</model>
</depend>
</model>

344
guiTools/ardupilot_gazebo/models/iris_with_gimbal/model.sdf Executable file
View File

@ -0,0 +1,344 @@
<?xml version='1.0'?>
<sdf version="1.9">
<model name="iris_with_gimbal">
<include>
<uri>model://iris_with_standoffs</uri>
</include>
<include>
<uri>model://gimbal_small_3d</uri>
<name>gimbal</name>
<pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>
</include>
<joint name="gimbal_joint" type="revolute">
<parent>iris_with_standoffs::base_link</parent>
<child>gimbal::base_link</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<!-- plugins -->
<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_0_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_1_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_2_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_3_joint</joint_name>
</plugin>
<plugin name="ArduPilotPlugin"
filename="ArduPilotPlugin">
<!-- Port settings -->
<fdm_addr>0.0.0.0</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<!-- Frame conventions
Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
-->
<modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
<!-- Sensors -->
<imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<control channel="0">
<jointName>iris_with_standoffs::rotor_0_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="1">
<jointName>iris_with_standoffs::rotor_1_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="2">
<jointName>iris_with_standoffs::rotor_2_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="3">
<jointName>iris_with_standoffs::rotor_3_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<!-- roll range is -30 to +30 deg -->
<control channel="8">
<jointName>gimbal::roll_joint</jointName>
<multiplier>1.047197551196</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_roll</cmd_topic>
<p_gain>2</p_gain>
</control>
<!-- pitch range is -135 to +45 deg -->
<control channel="9">
<jointName>gimbal::pitch_joint</jointName>
<multiplier>-3.14159265</multiplier>
<offset>-0.75</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_pitch</cmd_topic>
<p_gain>2</p_gain>
</control>
<!-- yaw range is -160 to +160 deg -->
<control channel="10">
<jointName>gimbal::yaw_joint</jointName>
<multiplier>-5.5850536</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_yaw</cmd_topic>
<p_gain>2</p_gain>
</control>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::roll_joint</joint_name>
<topic>/gimbal/cmd_roll</topic>
<p_gain>2</p_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::pitch_joint</joint_name>
<topic>/gimbal/cmd_pitch</topic>
<p_gain>2</p_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::yaw_joint</joint_name>
<topic>/gimbal/cmd_yaw</topic>
<p_gain>2</p_gain>
</plugin>
</model>
</sdf>

345
guiTools/ardupilot_gazebo/models/iris_with_gimbal/model.xacro.sdf Normal file
View File

@ -0,0 +1,345 @@
<?xml version='1.0'?>
<sdf version="1.9" xmlns:xacro="http://www.ros.org/wiki/xacro">
<model name="iris_with_gimbal">
<xacro:arg name="fdm_port_in" default="9002"/>
<include>
<uri>model://iris_with_standoffs</uri>
</include>
<include>
<uri>model://gimbal_small_3d</uri>
<name>gimbal</name>
<pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>
</include>
<joint name="gimbal_joint" type="revolute">
<parent>iris_with_standoffs::base_link</parent>
<child>gimbal::base_link</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<!-- plugins -->
<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_0_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_1_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_2_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_3_joint</joint_name>
</plugin>
<plugin name="ArduPilotPlugin"
filename="ArduPilotPlugin">
<!-- Port settings -->
<fdm_addr>0.0.0.0</fdm_addr>
<fdm_port_in>$(arg fdm_port_in)</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<!-- Frame conventions
Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
-->
<modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
<!-- Sensors -->
<imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<control channel="0">
<jointName>iris_with_standoffs::rotor_0_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="1">
<jointName>iris_with_standoffs::rotor_1_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="2">
<jointName>iris_with_standoffs::rotor_2_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="3">
<jointName>iris_with_standoffs::rotor_3_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<!-- roll range is -30 to +30 deg -->
<control channel="8">
<jointName>gimbal::roll_joint</jointName>
<multiplier>1.047197551196</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_roll</cmd_topic>
<p_gain>2</p_gain>
</control>
<!-- pitch range is -135 to +45 deg -->
<control channel="9">
<jointName>gimbal::pitch_joint</jointName>
<multiplier>-3.14159265</multiplier>
<offset>-0.75</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_pitch</cmd_topic>
<p_gain>2</p_gain>
</control>
<!-- yaw range is -160 to +160 deg -->
<control channel="10">
<jointName>gimbal::yaw_joint</jointName>
<multiplier>-5.5850536</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_yaw</cmd_topic>
<p_gain>2</p_gain>
</control>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::roll_joint</joint_name>
<topic>/gimbal/cmd_roll</topic>
<p_gain>2</p_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::pitch_joint</joint_name>
<topic>/gimbal/cmd_pitch</topic>
<p_gain>2</p_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::yaw_joint</joint_name>
<topic>/gimbal/cmd_yaw</topic>
<p_gain>2</p_gain>
</plugin>
</model>
</sdf>

160
guiTools/ardupilot_gazebo/models/iris_with_standoffs/meshes/iris.dae Executable file
View File

File diff suppressed because one or more lines are too long

160
guiTools/ardupilot_gazebo/models/iris_with_standoffs/meshes/iris_prop_ccw.dae Executable file
View File

File diff suppressed because one or more lines are too long

160
guiTools/ardupilot_gazebo/models/iris_with_standoffs/meshes/iris_prop_cw.dae Executable file
View File

File diff suppressed because one or more lines are too long

32
guiTools/ardupilot_gazebo/models/iris_with_standoffs/model.config Executable file
View File

@ -0,0 +1,32 @@
<?xml version="1.0"?>
<model>
<name>Iris with Standoffs</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Fadri Furrer</name>
<email>fadri.furrer@mavt.ethz.ch</email>
</author>
<author>
<name>Michael Burri</name>
</author>
<author>
<name>Mina Kamel</name>
</author>
<author>
<name>Janosch Nikolic</name>
</author>
<author>
<name>Markus Achtelik</name>
</author>
<maintainer email="hsu@osrfoundation.org">john hsu</maintainer>
<description>
A copy of the 3DR Iris model taken from
https://github.com/PX4/sitl_gazebo/tree/master/models
Local modifications include adding standoffs,
inertia and collision updates..
</description>
</model>

476
guiTools/ardupilot_gazebo/models/iris_with_standoffs/model.sdf Executable file
View File

@ -0,0 +1,476 @@
<?xml version='1.0'?>
<sdf version='1.9'>
<model name='iris_with_standoffs'>
<pose>0 0 0 0 0 0</pose>
<link name='base_link'>
<velocity_decay>
<linear>0.0</linear>
<angular>0.0</angular>
</velocity_decay>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>1.5</mass>
<inertia>
<ixx>0.008</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.015</iyy>
<iyz>0</iyz>
<izz>0.017</izz>
</inertia>
</inertial>
<collision name='base_collision'>
<pose>0 0 -0.08 0 0 0</pose>
<geometry>
<box>
<size>0.47 0.47 0.23</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<max_vel>100.0</max_vel>
<min_depth>0.001</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>100000.0</mu>
<mu2>100000.0</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name='base_visual'>
<geometry>
<mesh>
<uri>model://iris_with_standoffs/meshes/iris.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.05 0.05 0.05</ambient>
<diffuse>0.05 0.05 0.05</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<visual name='front_left_leg_visual'>
<pose>0.123 0.22 -0.11 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.17</length>
</cylinder>
</geometry>
<material>
<ambient>0.05 0.05 0.05</ambient>
<diffuse>0.05 0.05 0.05</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<visual name='front_right_leg_visual'>
<pose>0.123 -0.22 -0.11 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.17</length>
</cylinder>
</geometry>
<material>
<ambient>0.05 0.05 0.05</ambient>
<diffuse>0.05 0.05 0.05</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<visual name='rear_left_leg_visual'>
<pose>-0.140 0.21 -0.11 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.17</length>
</cylinder>
</geometry>
<material>
<ambient>0.05 0.05 0.05</ambient>
<diffuse>0.05 0.05 0.05</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<visual name='rear_right_leg_visual'>
<pose>-0.140 -0.21 -0.11 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.17</length>
</cylinder>
</geometry>
<material>
<ambient>0.05 0.05 0.05</ambient>
<diffuse>0.05 0.05 0.05</diffuse>
<specular>0.01 0.01 0.01 1.0</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
</link>
<link name='imu_link'>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.15</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00002</iyy>
<iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<sensor name="imu_sensor" type="imu">
<pose degrees="true">0 0 0 180 0 0</pose>
<always_on>1</always_on>
<update_rate>1000.0</update_rate>
</sensor>
</link>
<joint name='imu_joint' type='revolute'>
<child>imu_link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<damping>1.0</damping>
</dynamics>
</axis>
</joint>
<link name='rotor_0'>
<pose>0.13 -0.22 0.023 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000166704</iyy>
<iyz>0</iyz>
<izz>0.000167604</izz>
</inertia>
</inertial>
<collision name='rotor_0_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_0_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<self_collide>0</self_collide>
</link>
<joint name='rotor_0_joint' type='revolute'>
<child>rotor_0</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>0.004</damping>
</dynamics>
</axis>
<physics>
<ode>
<implicit_spring_damper>1</implicit_spring_damper>
</ode>
</physics>
</joint>
<link name='rotor_1'>
<pose>-0.13 0.2 0.023 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000166704</iyy>
<iyz>0</iyz>
<izz>0.000167604</izz>
</inertia>
</inertial>
<collision name='rotor_1_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_1_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0 0 1 1</ambient>
<diffuse>0 0 1 1</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<self_collide>0</self_collide>
</link>
<joint name='rotor_1_joint' type='revolute'>
<child>rotor_1</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>0.004</damping>
</dynamics>
</axis>
<physics>
<ode>
<implicit_spring_damper>1</implicit_spring_damper>
</ode>
</physics>
</joint>
<link name='rotor_2'>
<pose>0.13 0.22 0.023 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000166704</iyy>
<iyz>0</iyz>
<izz>0.000167604</izz>
</inertia>
</inertial>
<collision name='rotor_2_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_2_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0 1 0</ambient>
<diffuse>0 1 0</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<self_collide>0</self_collide>
</link>
<joint name='rotor_2_joint' type='revolute'>
<child>rotor_2</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>0.004</damping>
</dynamics>
</axis>
<physics>
<ode>
<implicit_spring_damper>1</implicit_spring_damper>
</ode>
</physics>
</joint>
<link name='rotor_3'>
<pose>-0.13 -0.2 0.023 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.025</mass>
<inertia>
<ixx>9.75e-06</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000166704</iyy>
<iyz>0</iyz>
<izz>0.000167604</izz>
</inertia>
</inertial>
<collision name='rotor_3_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='rotor_3_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0 1 0</ambient>
<diffuse>0 1 0</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
<metalness>0.5</metalness>
<roughness>0.5</roughness>
</metal>
</pbr>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<self_collide>0</self_collide>
</link>
<joint name='rotor_3_joint' type='revolute'>
<child>rotor_3</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<damping>0.004</damping>
</dynamics>
</axis>
<physics>
<ode>
<implicit_spring_damper>1</implicit_spring_damper>
</ode>
</physics>
</joint>
</model>
</sdf>

174
guiTools/ardupilot_gazebo/models/parachute_small/meshes/parachute_small.dae Normal file
View File

File diff suppressed because one or more lines are too long

18
guiTools/ardupilot_gazebo/models/parachute_small/model.config Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<model>
<name>Parachute small</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<license>Apache License, Version 2.0</license>
<author>
<name>Aurelien Roy</name>
<email>aurroy@hotmail.com</email>
</author>
<description>
A parachute. It avoids hitting the ground too hard ;-)
This model has been imported from github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin.git
</description>
</model>

66
guiTools/ardupilot_gazebo/models/parachute_small/model.sdf Normal file
View File

@ -0,0 +1,66 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="parachute_small">
<pose>0 0 0 0 0 0</pose>
<link name="chute">
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 2.3 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.06250</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.06250</iyy>
<iyz>0.00</iyz>
<izz>0.06250</izz>
</inertia>
</inertial>
<collision name="collision">
<pose>0 0 2.3 0 0 0</pose>
<geometry>
<sphere>
<radius>1.25</radius>
</sphere>
</geometry>
</collision>
<visual name="visual">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>2 2 2</scale>
<uri>meshes/parachute_small.dae</uri>
</mesh>
</geometry>
</visual>
<!-- viual at bridle (located at the link origin) -->
<visual name="bridle_visual">
<geometry>
<sphere>
<radius>0.025</radius>
</sphere>
</geometry>
<material>
<ambient>0.0 0.3 0.7</ambient>
<diffuse>0.0 0.3 0.7</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
</link>
<plugin name="gz::sim::systems::LiftDrag"
filename="gz-sim-lift-drag-system">
<a0>0</a0>
<cla>0</cla>
<cda>1.75</cda> <!-- Such that Cd = 1.75 at alpha = 90deg (= upright parachute) -->
<alpha_stall>1.5708</alpha_stall> <!-- = upright parachute -->
<cla_stall>0</cla_stall>
<cda_stall>0.01</cda_stall>
<cp>0 0 1.25</cp> <!-- About the vertical center of the chute -->
<area>3</area> <!-- Area of a circle of diameter 1m = 0.79, 3m2 is for a better visualization -->
<air_density>1.204</air_density>
<forward>0 0 0</forward>
<upward>0 0 1</upward>
<link_name>chute</link_name>
</plugin>
</model>
</sdf>

BIN
guiTools/ardupilot_gazebo/models/runway/materials/textures/Grass004_2K_AmbientOcclusion.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.5 MiB

BIN
guiTools/ardupilot_gazebo/models/runway/materials/textures/Grass004_2K_Color copy.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 7.3 MiB

BIN
guiTools/ardupilot_gazebo/models/runway/materials/textures/Grass004_2K_Color.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 989 KiB

BIN
guiTools/ardupilot_gazebo/models/runway/materials/textures/Grass004_2K_Displacement.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.3 MiB

BIN
guiTools/ardupilot_gazebo/models/runway/materials/textures/Grass004_2K_NormalGL.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.6 MiB

BIN
guiTools/ardupilot_gazebo/models/runway/materials/textures/Grass004_2K_Roughness.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.3 MiB

BIN
guiTools/ardupilot_gazebo/models/runway/materials/textures/runway.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 18 KiB

188
guiTools/ardupilot_gazebo/models/runway/meshes/airfield.dae Normal file
View File

@ -0,0 +1,188 @@
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
<asset>
<contributor>
<author>Blender User</author>
<authoring_tool>Blender 2.82.7 commit date:2020-03-12, commit time:05:06, hash:375c7dc4caf4</authoring_tool>
</contributor>
<created>2021-09-15T16:31:09</created>
<modified>2021-09-15T16:31:09</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>
<library_cameras>
<camera id="Camera-camera" name="Camera">
<optics>
<technique_common>
<perspective>
<xfov sid="xfov">39.59775</xfov>
<aspect_ratio>1.777778</aspect_ratio>
<znear sid="znear">0.1</znear>
<zfar sid="zfar">100</zfar>
</perspective>
</technique_common>
</optics>
<extra>
<technique profile="blender">
<shiftx sid="shiftx" type="float">0</shiftx>
<shifty sid="shifty" type="float">0</shifty>
<dof_distance sid="dof_distance" type="float">10</dof_distance>
</technique>
</extra>
</camera>
</library_cameras>
<library_lights>
<light id="Light-light" name="Light">
<technique_common>
<point>
<color sid="color">1000 1000 1000</color>
<constant_attenuation>1</constant_attenuation>
<linear_attenuation>0</linear_attenuation>
<quadratic_attenuation>0.00111109</quadratic_attenuation>
</point>
</technique_common>
<extra>
<technique profile="blender">
<type sid="type" type="int">0</type>
<flag sid="flag" type="int">0</flag>
<mode sid="mode" type="int">1</mode>
<gamma sid="blender_gamma" type="float">1</gamma>
<red sid="red" type="float">1</red>
<green sid="green" type="float">1</green>
<blue sid="blue" type="float">1</blue>
<shadow_r sid="blender_shadow_r" type="float">0</shadow_r>
<shadow_g sid="blender_shadow_g" type="float">0</shadow_g>
<shadow_b sid="blender_shadow_b" type="float">0</shadow_b>
<energy sid="blender_energy" type="float">1000</energy>
<dist sid="blender_dist" type="float">29.99998</dist>
<spotsize sid="spotsize" type="float">75</spotsize>
<spotblend sid="spotblend" type="float">0.15</spotblend>
<att1 sid="att1" type="float">0</att1>
<att2 sid="att2" type="float">1</att2>
<falloff_type sid="falloff_type" type="int">2</falloff_type>
<clipsta sid="clipsta" type="float">0.04999995</clipsta>
<clipend sid="clipend" type="float">30.002</clipend>
<bias sid="bias" type="float">1</bias>
<soft sid="soft" type="float">3</soft>
<bufsize sid="bufsize" type="int">2880</bufsize>
<samp sid="samp" type="int">3</samp>
<buffers sid="buffers" type="int">1</buffers>
<area_shape sid="area_shape" type="int">1</area_shape>
<area_size sid="area_size" type="float">0.1</area_size>
<area_sizey sid="area_sizey" type="float">0.1</area_sizey>
<area_sizez sid="area_sizez" type="float">1</area_sizez>
</technique>
</extra>
</light>
</library_lights>
<library_effects>
<effect id="Material_001-effect">
<profile_COMMON>
<newparam sid="Grass004_2K_Color_jpg-surface">
<surface type="2D">
<init_from>Grass004_2K_Color_jpg</init_from>
</surface>
</newparam>
<newparam sid="Grass004_2K_Color_jpg-sampler">
<sampler2D>
<source>Grass004_2K_Color_jpg-surface</source>
</sampler2D>
</newparam>
<technique sid="common">
<lambert>
<emission>
<color sid="emission">0 0 0 1</color>
</emission>
<diffuse>
<texture texture="Grass004_2K_Color_jpg-sampler" texcoord="UVMap"/>
</diffuse>
<index_of_refraction>
<float sid="ior">1.45</float>
</index_of_refraction>
</lambert>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_images>
<image id="Grass004_2K_Color_jpg" name="Grass004_2K_Color_jpg">
<init_from>../materials/textures/Grass004_2K_Color.jpg</init_from>
</image>
</library_images>
<library_materials>
<material id="Material_001-material" name="Material.001">
<instance_effect url="#Material_001-effect"/>
</material>
</library_materials>
<library_geometries>
<geometry id="Plane-mesh" name="Plane">
<mesh>
<source id="Plane-mesh-positions">
<float_array id="Plane-mesh-positions-array" count="12">-1 -1 0 1 -1 0 -1 1 0 1 1 0</float_array>
<technique_common>
<accessor source="#Plane-mesh-positions-array" count="4" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Plane-mesh-normals">
<float_array id="Plane-mesh-normals-array" count="3">0 0 1</float_array>
<technique_common>
<accessor source="#Plane-mesh-normals-array" count="1" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Plane-mesh-map-0">
<float_array id="Plane-mesh-map-0-array" count="12">-250 250 250 -250 250 250 -250 250 -250 -250 250 -250</float_array>
<technique_common>
<accessor source="#Plane-mesh-map-0-array" count="6" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Plane-mesh-vertices">
<input semantic="POSITION" source="#Plane-mesh-positions"/>
</vertices>
<triangles material="Material_001-material" count="2">
<input semantic="VERTEX" source="#Plane-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Plane-mesh-normals" offset="1"/>
<input semantic="TEXCOORD" source="#Plane-mesh-map-0" offset="2" set="0"/>
<p>1 0 0 2 0 1 0 0 2 1 0 3 3 0 4 2 0 5</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="Scene" name="Scene">
<node id="Plane" name="Plane" type="NODE">
<matrix sid="transform">5000 0 0 0 0 5000 0 0 0 0 1 0 0 0 0 1</matrix>
<instance_geometry url="#Plane-mesh" name="Plane">
<bind_material>
<technique_common>
<instance_material symbol="Material_001-material" target="#Material_001-material">
<bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
</instance_material>
</technique_common>
</bind_material>
</instance_geometry>
</node>
<node id="Camera" name="Camera" type="NODE">
<matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
<instance_camera url="#Camera-camera"/>
</node>
<node id="Light" name="Light" type="NODE">
<matrix sid="transform">-0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
<instance_light url="#Light-light"/>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
</COLLADA>

15
guiTools/ardupilot_gazebo/models/runway/model.config Executable file
View File

@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Runway</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Rhys Mainwaring</name>
<email>rhys.mainwaring@me.com</email>
</author>
<description>
Runway
</description>
</model>

61
guiTools/ardupilot_gazebo/models/runway/model.sdf Executable file
View File

@ -0,0 +1,61 @@
<?xml version='1.0'?>
<sdf version="1.9">
<model name="runway">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="runway">
<pose degrees="true">0 0 0 0 0 90</pose>
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>1500 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<pbr>
<metal>
<albedo_map>materials/textures/runway.png</albedo_map>
<roughness>0.6</roughness>
<metalness>0</metalness>
</metal>
</pbr>
</material>
</visual>
<visual name="airfield">
<pose>0 0 -0.01 0 0 0</pose>
<cast_shadows>false</cast_shadows>
<geometry>
<mesh>
<uri>model://runway/meshes/airfield.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<pbr>
<metal>
<albedo_map>materials/textures/Grass004_2K_Color.jpg</albedo_map>
<ambient_occlusion_map>materials/textures/Grass004_2K_AmbientOcclusion.jpg</ambient_occlusion_map>
<normal_map>materials/textures/Grass004_2K_NormalGL.jpg</normal_map>
<roughness_map>materials/textures/Grass004_2K_Roughness.jpg</roughness_map>
<roughness>0.6</roughness>
<metalness>0</metalness>
</metal>
</pbr>
</material>
</visual>
</link>
</model>
</sdf>

16
guiTools/ardupilot_gazebo/models/sun/model.config Normal file
View File

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>Sun</name>
<version>1.0</version>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>Nate Koenig</name>
<email>nate@osrfoundation.org</email>
</author>
<description>
A directional light for the sun.
</description>
</model>

16
guiTools/ardupilot_gazebo/models/sun/model.sdf Normal file
View File

@ -0,0 +1,16 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
</sdf>

BIN
guiTools/ardupilot_gazebo/models/zephyr/materials/textures/wing.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 232 KiB

174
guiTools/ardupilot_gazebo/models/zephyr/meshes/parachute_small.dae Normal file
View File

File diff suppressed because one or more lines are too long

651
guiTools/ardupilot_gazebo/models/zephyr/meshes/wing.dae Normal file
View File

File diff suppressed because one or more lines are too long

24
guiTools/ardupilot_gazebo/models/zephyr/model.config Normal file
View File

@ -0,0 +1,24 @@
<?xml version="1.0"?>
<model>
<name>Zephyr Delta Wing</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Cole Biesemeyer</name>
<email>cole.bsmr@gmail.com</email>
</author>
<author>
<name>Nate Koenig</name>
<email>natekoenig@gmail.com</email>
</author>
<description>
A model of a Zephyr delta wing.
Updated materials for Gazebo Sim.
</description>
</model>

285
guiTools/ardupilot_gazebo/models/zephyr/model.sdf Normal file
View File

@ -0,0 +1,285 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="zephyr">
<link name="wing">
<inertial>
<pose>0 -0.12 0 0 0 0</pose>
<mass>1.5</mass>
<inertia>
<ixx>0.083137104</ixx>
<ixy>0</ixy>
<iyy>0.387382402</iyy>
<ixz>0</ixz>
<iyz>0</iyz>
<izz>0.469845106</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<mesh>
<uri>model://zephyr/meshes/wing.dae</uri>
<submesh>
<name>Wing</name>
<center>true</center>
</submesh>
</mesh>
</geometry>
</visual>
<collision name="body_collision">
<geometry>
<mesh>
<uri>model://zephyr/meshes/wing.dae</uri>
<submesh>
<name>Wing</name>
<center>true</center>
</submesh>
</mesh>
</geometry>
</collision>
<collision name="right_rudder_collision">
<pose>-0.76435 0.33918 0.002 -0.03 0 0</pose>
<geometry>
<box>
<size>.005 0.12993 .12688</size>
</box>
</geometry>
</collision>
<collision name="left_rudder_collision">
<pose>0.76435 0.33918 0.002 -0.03 0 0</pose>
<geometry>
<box>
<size>.005 0.12993 .12688</size>
</box>
</geometry>
</collision>
<!-- save center of pressure locations for debugging -->
<visual name="cp_wing">
<pose>0 -0.1 0.0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
<visual name="cp_flap_left">
<pose>0.7 0.20 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
<material>
<ambient>1 0 0</ambient>
<diffuse>1 0 0</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
<visual name="cp_flap_right">
<pose>-0.7 0.20 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
<material>
<ambient>0 1 0</ambient>
<diffuse>0 1 0</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
</link>
<link name="propeller">
<pose degrees="true">0 0.07 0.008 0 90 0</pose>
<inertial>
<mass>.05</mass>
<inertia>
<ixx>0.000367571</ixx>
<ixy>0</ixy>
<iyy>0.00036985</iyy>
<ixz>0</ixz>
<iyz>0</iyz>
<izz>0.000003187</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<mesh>
<uri>model://zephyr/meshes/wing.dae</uri>
<submesh>
<name>Propeller</name>
<center>true</center>
</submesh>
</mesh>
</geometry>
</visual>
<collision name="blade1">
<pose>0 0 0.074205 0 0 0.3</pose>
<geometry>
<box>
<size>0.02561 0.00541 0.14841</size>
</box>
</geometry>
</collision>
<collision name="blade2">
<pose>0 0 -0.074205 0 0 -0.3</pose>
<geometry>
<box>
<size>0.02561 0.00541 0.14841</size>
</box>
</geometry>
</collision>
</link>
<link name="flap_left">
<pose>0.4530 .239 0.007 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0.32445671</pose>
<mass>.1</mass>
<inertia>
<ixx>0.000102319</ixx>
<ixy>0</ixy>
<iyy>0.00334417</iyy>
<ixz>0</ixz>
<iyz>0</iyz>
<izz>0.003446072</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<mesh>
<uri>model://zephyr/meshes/wing.dae</uri>
<submesh>
<name>Flap_Left</name>
<center>true</center>
</submesh>
</mesh>
</geometry>
</visual>
<collision name="collision">
<pose>-0.01 0.01 0 0 0 0.32445671</pose>
<geometry>
<box>
<size>0.633463031 0.110694312 0.005</size>
</box>
</geometry>
</collision>
</link>
<link name="flap_right">
<pose>-0.4530 .239 0.007 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 -0.32445671</pose>
<mass>.1</mass>
<inertia>
<ixx>0.000102319</ixx>
<ixy>0</ixy>
<iyy>0.00334417</iyy>
<ixz>0</ixz>
<iyz>0</iyz>
<izz>0.003446072</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<mesh>
<uri>model://zephyr/meshes/wing.dae</uri>
<submesh>
<name>Flap_Right</name>
<center>true</center>
</submesh>
</mesh>
</geometry>
</visual>
<collision name="collision">
<pose>0.01 0.01 0 0 0 -0.32445671</pose>
<geometry>
<box>
<size>0.633463031 0.110694312 0.005</size>
</box>
</geometry>
</collision>
</link>
<joint name="propeller_joint" type="revolute">
<parent>wing</parent>
<child>propeller</child>
<axis>
<xyz>0 -1 0</xyz>
<dynamics>
<damping>0.002</damping>
</dynamics>
</axis>
</joint>
<joint name="flap_left_joint" type="revolute">
<parent>wing</parent>
<child>flap_left</child>
<pose>0 -0.04 0 0 0 0</pose>
<axis>
<xyz>1 0.330321014 0</xyz>
<limit>
<lower>-0.524</lower>
<upper>0.524</upper>
</limit>
<dynamics>
<damping>0.1</damping>
</dynamics>
</axis>
</joint>
<joint name="flap_right_joint" type="revolute">
<parent>wing</parent>
<child>flap_right</child>
<pose>0 -0.04 0 0 0 0</pose>
<axis>
<xyz>1 -0.330321014 0</xyz>
<limit>
<lower>-0.524</lower>
<upper>0.524</upper>
</limit>
<dynamics>
<damping>0.1</damping>
</dynamics>
</axis>
</joint>
<!-- sensors -->
<link name='imu_link'>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.15</mass>
<inertia>
<ixx>0.00002</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00002</iyy>
<iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<sensor name="imu_sensor" type="imu">
<pose degrees="true">0 0 0 180 0 -90</pose>
<always_on>1</always_on>
<update_rate>1000.0</update_rate>
</sensor>
</link>
<joint name='imu_joint' type='revolute'>
<child>imu_link</child>
<parent>wing</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<damping>1.0</damping>
</dynamics>
</axis>
</joint>
</model>
</sdf>

BIN
guiTools/ardupilot_gazebo/models/zephyr/thumbnails/1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 12 KiB

BIN
guiTools/ardupilot_gazebo/models/zephyr/thumbnails/2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 26 KiB

BIN
guiTools/ardupilot_gazebo/models/zephyr/thumbnails/3.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.8 KiB

BIN
guiTools/ardupilot_gazebo/models/zephyr/thumbnails/4.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 5.7 KiB

BIN
guiTools/ardupilot_gazebo/models/zephyr/thumbnails/5.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.8 KiB

22
guiTools/ardupilot_gazebo/models/zephyr_with_ardupilot/model.config Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<model>
<name>Zephyr Delta Wing With Ardupilot</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Cole Biesemeyer</name>
<email>cole.bsmr@gmail.com</email>
</author>
<author>
<name>Nate Koenig</name>
<email>natekoenig@gmail.com</email>
</author>
<description>
A model of a Zephyr delta wing with Ardupilot integration.
</description>
</model>

268
guiTools/ardupilot_gazebo/models/zephyr_with_ardupilot/model.sdf Normal file
View File

@ -0,0 +1,268 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="zephyr_with_ardupilot">
<include>
<uri>model://zephyr</uri>
</include>
<!-- plugins -->
<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<!-- wing -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.13</a0>
<cla>3.7</cla>
<cda>0.06417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>0 -0.1 0</cp>
<area>0.50</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>zephyr::wing</link_name>
</plugin>
<!-- left_wing -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.15</a0>
<cla>6.8</cla>
<cda>0.06417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.6391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>0.7 0.20 0</cp>
<area>0.10</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>zephyr::wing</link_name>
<control_joint_name>zephyr::flap_left_joint</control_joint_name>
<control_joint_rad_to_cl>-5.0</control_joint_rad_to_cl>
</plugin>
<!-- right_wing -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.15</a0>
<cla>6.8</cla>
<cda>0.06417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.6391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>-0.7 0.20 0</cp>
<area>0.10</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>zephyr::wing</link_name>
<control_joint_name>zephyr::flap_right_joint</control_joint_name>
<control_joint_rad_to_cl>-5.0</control_joint_rad_to_cl>
</plugin>
<!-- left_rudder -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.0</a0>
<cla>4.752798721</cla>
<cda>0.6417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>-0.76 0.30 0.025</cp>
<area>0.12</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>1 0 0</upward>
<link_name>zephyr::wing</link_name>
</plugin>
<!-- right_rudder -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.0</a0>
<cla>4.752798721</cla>
<cda>0.6417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>0.76 0.30 0.025</cp>
<area>0.12</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>1 0 0</upward>
<link_name>zephyr::wing</link_name>
</plugin>
<!-- propeller_blade_1 -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.30</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.02</area>
<air_density>1.2041</air_density>
<cp>0 0 0.074205</cp>
<forward>-1 0 0</forward>
<upward>0 -1 0</upward>
<link_name>zephyr::propeller</link_name>
</plugin>
<!-- propeller_blade_2 -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.30</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.02</area>
<air_density>1.2041</air_density>
<cp>0 0 -0.074205</cp>
<forward>1 0 0</forward>
<upward>0 -1 0</upward>
<link_name>zephyr::propeller</link_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>zephyr::propeller_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>zephyr::flap_left_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>zephyr::flap_right_joint</joint_name>
</plugin>
<plugin name="ArduPilotPlugin" filename="ArduPilotPlugin">
<!-- Port settings -->
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<!-- Frame conventions
modelXYZToAirplaneXForwardZDown:
- transforms body frame from orientation in Gazebo to NED
gazeboXYZToNED
- transforms world from Gazebo convention xyz = N -E -D
to ArduPilot convention xyz = NED
Zephyr is oriented x-left, y-back, z-up
-->
<modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 -90</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
<!-- Sensors -->
<imuName>zephyr::imu_link::imu_sensor</imuName>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<!--
SERVO3_FUNCTION 70 (Throttle)
SERVO3_MAX 1900
SERVO3_MIN 1100
SERVO3_REVERSED 0
SERVO3_TRIM 1100
-->
<control channel="2">
<jointName>zephyr::propeller_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>0.0</cmd_min>
</control>
<!--
SERVO1_FUNCTION 77 (Elevon Left)
SERVO1_MAX 1900
SERVO1_MIN 1100
SERVO1_REVERSED 1
SERVO1_TRIM 1500
pwm: => [1100, 1900]
input: => [0, 1]
offset: -0.5 => [-0.5, 0.5]
scale: 2.0 => [-1.0, 1.0]
scale: 0.524 => [-0.524, 0.524]
-->
<control channel="0">
<jointName>zephyr::flap_left_joint</jointName>
<useForce>1</useForce>
<multiplier>-1.048</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>POSITION</type>
<p_gain>10.0</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
<!--
SERVO2_FUNCTION 78 (Elevon Right)
SERVO2_MAX 1900
SERVO2_MIN 1100
SERVO2_REVERSED 1
SERVO2_TRIM 1500
-->
<control channel="1">
<jointName>zephyr::flap_right_joint</jointName>
<useForce>1</useForce>
<multiplier>-1.048</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>POSITION</type>
<p_gain>10.0</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
</plugin>
</model>
</sdf>

27
guiTools/ardupilot_gazebo/models/zephyr_with_parachute/model.config Normal file
View File

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<model>
<name>Zephyr Delta Wing With Parachute</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Cole Biesemeyer</name>
<email>cole.bsmr@gmail.com</email>
</author>
<author>
<name>Nate Koenig</name>
<email>natekoenig@gmail.com</email>
</author>
<author>
<name>Rhys Mainwaring</name>
<email>rhys.mainwaring@me.com</email>
</author>
<description>
A model of a Zephyr delta wing with Ardupilot and Parachute plugins.
</description>
</model>

315
guiTools/ardupilot_gazebo/models/zephyr_with_parachute/model.sdf Normal file
View File

@ -0,0 +1,315 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<model name="zephyr_with_parachute">
<include>
<uri>model://zephyr</uri>
</include>
<!--
The attachment link connected by a ball joint is to allow the
aircraft to rotate freely below parachute, as detachable joint
must be fixed.
-->
<link name="parachute_attachment_link">
<pose degrees="true">0 0 0.1 0 0 -90</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</visual>
</link>
<joint name="parachute_attachment_joint" type="ball">
<child>parachute_attachment_link</child>
<parent>zephyr::wing</parent>
</joint>
<!-- plugins -->
<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<!-- wing -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.13</a0>
<cla>3.7</cla>
<cda>0.06417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>0 -0.1 0</cp>
<area>0.50</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>zephyr::wing</link_name>
</plugin>
<!-- left_wing -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.15</a0>
<cla>6.8</cla>
<cda>0.06417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.6391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>0.7 0.20 0</cp>
<area>0.10</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>zephyr::wing</link_name>
<control_joint_name>zephyr::flap_left_joint</control_joint_name>
<control_joint_rad_to_cl>-5.0</control_joint_rad_to_cl>
</plugin>
<!-- right_wing -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.15</a0>
<cla>6.8</cla>
<cda>0.06417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.6391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>-0.7 0.20 0</cp>
<area>0.10</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>zephyr::wing</link_name>
<control_joint_name>zephyr::flap_right_joint</control_joint_name>
<control_joint_rad_to_cl>-5.0</control_joint_rad_to_cl>
</plugin>
<!-- left_rudder -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.0</a0>
<cla>4.752798721</cla>
<cda>0.6417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>-0.76 0.30 0.025</cp>
<area>0.12</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>1 0 0</upward>
<link_name>zephyr::wing</link_name>
</plugin>
<!-- right_rudder -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.0</a0>
<cla>4.752798721</cla>
<cda>0.6417112299</cda>
<cma>0.0</cma>
<alpha_stall>0.3391428111</alpha_stall>
<cla_stall>-3.85</cla_stall>
<cda_stall>-0.9233984055</cda_stall>
<cma_stall>0.0</cma_stall>
<cp>0.76 0.30 0.025</cp>
<area>0.12</area>
<air_density>1.2041</air_density>
<forward>0 -1 0</forward>
<upward>1 0 0</upward>
<link_name>zephyr::wing</link_name>
</plugin>
<!-- propeller_blade_1 -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.30</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.02</area>
<air_density>1.2041</air_density>
<cp>0 0 0.074205</cp>
<forward>-1 0 0</forward>
<upward>0 -1 0</upward>
<link_name>zephyr::propeller</link_name>
</plugin>
<!-- propeller_blade_2 -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.30</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.02</area>
<air_density>1.2041</air_density>
<cp>0 0 -0.074205</cp>
<forward>1 0 0</forward>
<upward>0 -1 0</upward>
<link_name>zephyr::propeller</link_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>zephyr::propeller_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>zephyr::flap_left_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>zephyr::flap_right_joint</joint_name>
</plugin>
<plugin name="ArduPilotPlugin" filename="ArduPilotPlugin">
<!-- Port settings -->
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<!-- Frame conventions
modelXYZToAirplaneXForwardZDown:
- transforms body frame from orientation in Gazebo to NED
gazeboXYZToNED
- transforms world from Gazebo convention xyz = N -E -D
to ArduPilot convention xyz = NED
Zephyr is oriented x-left, y-back, z-up
-->
<modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 -90</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
<!-- Sensors -->
<imuName>zephyr::imu_link::imu_sensor</imuName>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<!--
SERVO3_FUNCTION 70 (Throttle)
SERVO3_MAX 1900
SERVO3_MIN 1100
SERVO3_REVERSED 0
SERVO3_TRIM 1100
-->
<control channel="2">
<jointName>zephyr::propeller_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>0.0</cmd_min>
</control>
<!--
SERVO1_FUNCTION 77 (Elevon Left)
SERVO1_MAX 1900
SERVO1_MIN 1100
SERVO1_REVERSED 1
SERVO1_TRIM 1500
pwm: => [1100, 1900]
input: => [0, 1]
offset: -0.5 => [-0.5, 0.5]
scale: 2.0 => [-1.0, 1.0]
scale: 0.524 => [-0.524, 0.524]
-->
<control channel="0">
<jointName>zephyr::flap_left_joint</jointName>
<useForce>1</useForce>
<multiplier>-1.048</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>POSITION</type>
<p_gain>10.0</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
<!--
SERVO2_FUNCTION 78 (Elevon Right)
SERVO2_MAX 1900
SERVO2_MIN 1100
SERVO2_REVERSED 1
SERVO2_TRIM 1500
-->
<control channel="1">
<jointName>zephyr::flap_right_joint</jointName>
<useForce>1</useForce>
<multiplier>-1.048</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>POSITION</type>
<p_gain>10.0</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
</control>
<!--
SERVO5_FUNCTION 27.0 # Parachute
SERVO5_MAX 1900
SERVO5_MIN 1100
SERVO5_REVERSED 0 # Normal
SERVO5_TRIM 1500
-->
<control channel="4">
<jointName>parachute_attachment_joint</jointName>
<type>COMMAND</type>
<cmd_topic>/parachute/cmd_release</cmd_topic>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
</control>
</plugin>
</model>
</sdf>

30
guiTools/ardupilot_gazebo/package.xml Normal file
View File

@ -0,0 +1,30 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ardupilot_gazebo</name>
<version>0.0.0</version>
<description>Plugins and models for vehicle simulation in Gazebo Sim with ArduPilot SITL controllers</description>
<maintainer email="rhys.mainwaring@me.com">Rhys Mainwaring</maintainer>
<license>LGPL-3.0</license>
<author>Rhys Mainwaring</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rapidjson-dev</build_depend>
<!-- Harmonic (default) -->
<depend condition="$GZ_VERSION == harmonic">gz-cmake3</depend>
<depend condition="$GZ_VERSION == harmonic">gz-sim8</depend>
<depend condition="$GZ_VERSION == ''">gz-cmake3</depend>
<depend condition="$GZ_VERSION == ''">gz-sim8</depend>
<!-- Garden -->
<depend condition="$GZ_VERSION == garden">gz-cmake3</depend>
<depend condition="$GZ_VERSION == garden">gz-sim7</depend>
<test_depend>ament_lint_auto</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

1998
guiTools/ardupilot_gazebo/src/ArduPilotPlugin.cc Executable file
View File

File diff suppressed because it is too large Load Diff

519
guiTools/ardupilot_gazebo/src/CameraZoomPlugin.cc Normal file
View File

@ -0,0 +1,519 @@
/*
Copyright (C) 2023 ArduPilot.org
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "CameraZoomPlugin.hh"
#include <algorithm>
#include <atomic>
#include <cmath>
#include <limits>
#include <mutex>
#include <string>
#include <gz/common/Profiler.hh>
#include <gz/math/Angle.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Quaternion.hh>
#include <gz/plugin/Register.hh>
#include <gz/rendering/Camera.hh>
#include <gz/rendering/RenderEngine.hh>
#include <gz/rendering/RenderingIface.hh>
#include <gz/rendering/Scene.hh>
#include <gz/sim/components/Camera.hh>
#include <gz/sim/components/Model.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/ParentEntity.hh>
#include <gz/sim/components/Link.hh>
#include <gz/sim/components/Sensor.hh>
#include <gz/sim/components/World.hh>
#include <gz/sim/rendering/Events.hh>
#include "gz/sim/Events.hh"
#include <gz/sim/Link.hh>
#include <gz/sim/Model.hh>
/// \todo(srmainwaring) use when gz-sim7 v7.5 is released.
// #include <gz/sim/Sensor.hh>
#include <gz/sim/World.hh>
#include <gz/sim/Util.hh>
#include <gz/transport/Node.hh>
#include <sdf/Camera.hh>
#include <sdf/Sensor.hh>
namespace gz {
namespace sim {
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems {
//////////////////////////////////////////////////
class CameraZoomPlugin::Impl
{
/// \brief Handle a zoom command.
public: void OnZoom(const msgs::Double &_msg);
/// \brief Initialise the rendering camera.
public: void InitialiseCamera();
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
/// \brief Check sensor entity is valid.
public: bool SensorValid(const EntityComponentManager &_ecm) const
{
return nullptr !=
_ecm.Component<components::Sensor>(this->cameraSensorEntity);
}
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
/// \brief Get sensor name.
public: std::optional<std::string> SensorName(
const EntityComponentManager &_ecm) const
{
return _ecm.ComponentData<components::Name>(this->cameraSensorEntity);
}
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
/// \brief Get sensor parent.
public: std::optional<Entity> SensorParent(
const EntityComponentManager &_ecm) const
{
auto parent =
_ecm.Component<components::ParentEntity>(this->cameraSensorEntity);
if (!parent)
return std::nullopt;
return std::optional<sim::Entity>(parent->Data());
}
/// \brief World occupied by the parent model.
public: World world{kNullEntity};
/// \brief The parent model.
public: Model parentModel{kNullEntity};
/// \brief Camera sensor.
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
// public: Sensor cameraSensor{kNullEntity};
public: Entity cameraSensorEntity{kNullEntity};
/// \brief Name of the camera.
public: std::string cameraName;
/// \brief Name of the topic to subscribe to zoom commands.
public: std::string zoomTopic;
/// \brief Flag to mark if zoom command has changed.
public: std::atomic<bool> zoomChanged{false};
/// \brief Value of the most recently received zoom command.
public: std::atomic<double> zoomCommand{1.0};
/// \brief Reference horizontal field of view (radians).
public: double refHfov{2.0};
/// \brief Goal horizontal field of view (radians).
public: std::atomic<double> goalHfov{2.0};
/// \brief Current zoom factor.
public: double curZoom{1.0};
/// \brief Maximum zoom factor.
public: double maxZoom{10.0};
/// \brief Slew rate (meters change in focal length per second).
/// Default: infinity, which causes instant changes in focal length.
public: double slewRate{std::numeric_limits<double>::infinity()};
/// \brief Minimum zoom factor == 1.0.
public: static constexpr double minZoom{1.0};
/// \brief Flag set to true if the plugin is correctly initialised.
public: bool isValidConfig{false};
/// \brief Connections to event callbacks.
public: std::vector<common::ConnectionPtr> connections;
/// \brief Transport node for subscriptions.
public: transport::Node node;
/// \brief Reset the camera and scene when the tear down event is received.
public: void OnRenderTeardown();
//// \brief Pointer to the rendering scene
public: rendering::ScenePtr scene;
/// \brief Pointer to the rendering camera
public: rendering::CameraPtr camera;
/// \brief Convert from focal length to FOV for a rectilinear lens
/// \ref https://en.wikipedia.org/wiki/Focal_length
/// @param sensorWidth Diagonal sensor width [meter]
/// @param focalLength The focal length [meter]
/// @return The field of view [rad]
public: static double FocalLengthToFov(
double sensorWidth, double focalLength);
/// \brief Convert from FOV to focal length for a rectilinear lens
/// \ref https://en.wikipedia.org/wiki/Focal_length
/// @param sensorWidth Diagonal sensor width [meter]
/// @param fov The field of view [rad]
/// @return The focal length [meter]
public: static double FovToFocalLength(
double sensorWidth, double fov);
/// @brief Compute diagonal sensor width given focal length and FOV
/// @param focalLength Focal length [meter]
/// @param fov Field of view [rad]
/// @return Sensor width [m]
public: static double SensorWidth(
double focalLength, double fov);
};
//////////////////////////////////////////////////
void CameraZoomPlugin::Impl::OnZoom(const msgs::Double &_msg)
{
this->zoomCommand = _msg.data();
this->zoomChanged = true;
}
//////////////////////////////////////////////////
void CameraZoomPlugin::Impl::InitialiseCamera()
{
// Wait for render engine to be available.
if (rendering::loadedEngines().empty())
{
return;
}
// Get scene.
if (!this->scene)
{
this->scene = rendering::sceneFromFirstRenderEngine();
}
// Return if scene not ready or no sensors available.
if (this->scene == nullptr ||
!this->scene->IsInitialized() ||
this->scene->SensorCount() == 0)
{
gzwarn << "No scene or camera sensors available.\n";
return;
}
// Get camera.
if (!this->camera)
{
auto sensor = this->scene->SensorByName(this->cameraName);
if (!sensor)
{
gzerr << "Unable to find sensor: [" << this->cameraName << "]."
<< std::endl;
return;
}
this->camera = std::dynamic_pointer_cast<rendering::Camera>(sensor);
if (!this->camera)
{
gzerr << "[" << this->cameraName << "] is not a camera."
<< std::endl;
return;
}
}
}
//////////////////////////////////////////////////
void CameraZoomPlugin::Impl::OnRenderTeardown()
{
gzdbg << "CameraZoomPlugin disabled.\n";
this->camera.reset();
this->scene.reset();
this->isValidConfig = false;
}
//////////////////////////////////////////////////
double CameraZoomPlugin::Impl::FocalLengthToFov(
double sensorWidth, double focalLength)
{
return 2 * std::atan2(sensorWidth, 2 * focalLength);
}
//////////////////////////////////////////////////
double CameraZoomPlugin::Impl::FovToFocalLength(
double sensorWidth, double fov)
{
// This is derived from FocalLengthToFov.
return sensorWidth / (2 * std::tan(fov / 2));
}
//////////////////////////////////////////////////
double CameraZoomPlugin::Impl::SensorWidth(
double focalLength, double fov)
{
// This is derived from FocalLengthToFov.
return 2 * std::tan(fov / 2) * focalLength;
}
//////////////////////////////////////////////////
//////////////////////////////////////////////////
CameraZoomPlugin::~CameraZoomPlugin() = default;
//////////////////////////////////////////////////
CameraZoomPlugin::CameraZoomPlugin() :
impl(std::make_unique<CameraZoomPlugin::Impl>())
{
}
//////////////////////////////////////////////////
void CameraZoomPlugin::Configure(
const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &_eventMgr)
{
// Capture camera sensor.
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
// this->impl->cameraSensor = Sensor(_entity);
// if (!this->impl->cameraSensor.Valid(_ecm))
this->impl->cameraSensorEntity = _entity;
if (!this->impl->SensorValid(_ecm))
{
gzerr << "CameraZoomPlugin must be attached to a camera sensor. "
"Failed to initialize.\n";
return;
}
// Display plugin load status.
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
// if (auto maybeName = this->impl->cameraSensor.Name(_ecm))
if (auto maybeName = this->impl->SensorName(_ecm))
{
gzdbg << "CameraZoomPlugin attached to sensor ["
<< maybeName.value() << "].\n";
}
else
{
gzerr << "Camera sensor has invalid name.\n";
return;
}
// Retrieve parent model.
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
// if (auto maybeParentLink = this->impl->cameraSensor.Parent(_ecm))
if (auto maybeParentLink = this->impl->SensorParent(_ecm))
{
Link link(maybeParentLink.value());
if (link.Valid(_ecm))
{
if (auto maybeParentModel = link.ParentModel(_ecm))
{
this->impl->parentModel = maybeParentModel.value();
}
}
}
if (!this->impl->parentModel.Valid(_ecm))
{
gzerr << "CameraZoomPlugin - parent model not found. "
"Failed to initialize.\n";
return;
}
// Retrieve world entity.
this->impl->world = World(
_ecm.EntityByComponents(components::World()));
if (!this->impl->world.Valid(_ecm))
{
gzerr << "CameraZoomPlugin - world not found. "
"Failed to initialize.\n";
return;
}
// Parameters
if (_sdf->HasElement("max_zoom"))
{
this->impl->maxZoom = _sdf->Get<double>("max_zoom");
}
if (_sdf->HasElement("slew_rate"))
{
this->impl->slewRate = _sdf->Get<double>("slew_rate");
}
// Configure zoom command topic.
{
std::vector<std::string> topics;
if (_sdf->HasElement("topic"))
{
topics.push_back(_sdf->Get<std::string>("topic"));
}
auto parentModelName = this->impl->parentModel.Name(_ecm);
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
// auto sensorName = this->impl->cameraSensor.Name(_ecm).value();
auto sensorName = this->impl->SensorName(_ecm).value();
topics.push_back("/model/" + parentModelName +
"/sensor/" + sensorName + "/zoom/cmd_zoom");
this->impl->zoomTopic = validTopic(topics);
}
// Subscriptions.
this->impl->node.Subscribe(
this->impl->zoomTopic,
&CameraZoomPlugin::Impl::OnZoom, this->impl.get());
gzdbg << "CameraZoomPlugin subscribing to messages on "
<< "[" << this->impl->zoomTopic << "]\n";
// Connections
this->impl->connections.push_back(
_eventMgr.Connect<gz::sim::events::RenderTeardown>(
std::bind(&CameraZoomPlugin::Impl::OnRenderTeardown,
this->impl.get())));
this->impl->isValidConfig = true;
}
//////////////////////////////////////////////////
void CameraZoomPlugin::PreUpdate(
const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
GZ_PROFILE("CameraZoomPlugin::PreUpdate");
if (!this->impl->isValidConfig)
return;
// Set up the render connection.
if (!this->impl->camera)
{
this->impl->InitialiseCamera();
return;
}
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
// Entity cameraEntity = this->impl->cameraSensor.Entity();
Entity cameraEntity = this->impl->cameraSensorEntity;
auto comp = _ecm.Component<components::Camera>(cameraEntity);
if (!comp)
return;
if (this->impl->zoomChanged)
{
// Only calculate goal once each time zoom is changed.
const auto requestedZoomCmd = this->impl->zoomCommand.load();
const auto clampedZoomCmd = std::clamp(requestedZoomCmd,
this->impl->minZoom, this->impl->maxZoom);
if (std::abs(requestedZoomCmd - clampedZoomCmd) >
std::numeric_limits<double>::epsilon())
{
gzwarn << "Requested zoom command of " << requestedZoomCmd
<< " has been clamped to " << clampedZoomCmd << ".\n";
}
this->impl->goalHfov = this->impl->refHfov / clampedZoomCmd;
this->impl->zoomChanged = false;
}
// Update component.
sdf::Sensor &sensor = comp->Data();
sdf::Camera *cameraSdf = sensor.CameraSensor();
if (!cameraSdf)
return;
const auto oldHfov = cameraSdf->HorizontalFov().Radian();
// Goal is achieved, nothing to update.
if (std::abs(this->impl->goalHfov - oldHfov) <
std::numeric_limits<double>::epsilon())
return;
const auto curFocalLength = cameraSdf->LensFocalLength();
// This value should be static every iteration.
const auto sensorWidth = CameraZoomPlugin::Impl::SensorWidth(
curFocalLength, oldHfov);
const auto goalFocalLength = CameraZoomPlugin::Impl::FovToFocalLength(
sensorWidth, this->impl->goalHfov);
double newFocalLength;
if (std::isfinite(this->impl->slewRate)) {
// This is the amount of time passed since the last update.
const auto dt = _info.dt;
// How many meters the focal length could change per update loop
const auto maxFocalLengthChange = this->impl->slewRate *
std::chrono::duration<double>(dt).count();
// How many meters the focal length should change this iteration
const auto deltaFL = std::min(maxFocalLengthChange,
std::abs(curFocalLength - goalFocalLength));
if (goalFocalLength > curFocalLength)
{
newFocalLength = curFocalLength + deltaFL;
}
else
{
newFocalLength = curFocalLength - deltaFL;
}
} else {
newFocalLength = goalFocalLength;
}
const auto newHfov = CameraZoomPlugin::Impl::FocalLengthToFov(
sensorWidth, newFocalLength);
// Update rendering camera with the latest focal length.
cameraSdf->SetHorizontalFov(newHfov);
_ecm.SetChanged(cameraEntity, components::Camera::typeId,
ComponentState::OneTimeChange);
// Update rendering camera.
this->impl->camera->SetHFOV(newHfov);
}
//////////////////////////////////////////////////
void CameraZoomPlugin::PostUpdate(
const UpdateInfo &/*_info*/,
const EntityComponentManager &_ecm)
{
if (!this->impl->cameraName.empty())
return;
/// \todo(srmainwaring) replace with `gz::sim::Sensor` when available.
// Entity cameraEntity = this->impl->cameraSensor.Entity();
Entity cameraEntity = this->impl->cameraSensorEntity;
this->impl->cameraName =
removeParentScope(scopedName(cameraEntity, _ecm, "::", false), "::");
gzdbg << "Camera name: [" << this->impl->cameraName << "].\n";
}
//////////////////////////////////////////////////
} // namespace systems
}
} // namespace sim
} // namespace gz
GZ_ADD_PLUGIN(
gz::sim::systems::CameraZoomPlugin,
gz::sim::System,
gz::sim::systems::CameraZoomPlugin::ISystemConfigure,
gz::sim::systems::CameraZoomPlugin::ISystemPreUpdate,
gz::sim::systems::CameraZoomPlugin::ISystemPostUpdate)
GZ_ADD_PLUGIN_ALIAS(
gz::sim::systems::CameraZoomPlugin,
"CameraZoomPlugin")

599
guiTools/ardupilot_gazebo/src/GstCameraPlugin.cc Normal file
View File

@ -0,0 +1,599 @@
/*
* Copyright (C) 2012-2016 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include "GstCameraPlugin.hh"
#include <gst/app/gstappsrc.h>
#include <gst/gst.h>
#include <iostream>
#include <string>
#include <thread>
#include <gz/plugin/Register.hh>
#include <gz/rendering/Camera.hh>
#include <gz/rendering/RenderingIface.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/Sensor.hh>
#include <gz/sim/Util.hh>
#include <gz/sim/components/Camera.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/Sensor.hh>
#include <gz/sim/rendering/Events.hh>
#include <gz/transport/Node.hh>
#include <opencv2/opencv.hpp>
namespace gz {
namespace sim {
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems {
//////////////////////////////////////////////////
class GstCameraPlugin::Impl {
public:
void InitializeCamera();
void StartStreaming();
static void *StartThread(void *);
void StartGstThread();
void OnImage(const msgs::Image &msg);
void OnVideoStreamEnable(const msgs::Boolean &_msg);
void OnRenderTeardown();
void StopStreaming();
void StopGstThread();
std::string udpHost{"127.0.0.1"};
int udpPort{5600};
bool useRtmpPipeline{false};
std::string rtmpLocation;
bool useBasicPipeline{false};
bool useCuda{false};
std::string imageTopic;
std::string enableTopic;
unsigned int width{0};
unsigned int height{0};
// Unused by actual pipeline since it's based on the gazebo topic rate?
unsigned int rate{5};
pthread_t threadId;
bool isGstMainLoopActive{false};
bool requestedStartStreaming{false};
GMainLoop *gst_loop{nullptr};
GstElement *source{nullptr};
void CreateMpeg2tsPipeline(GstElement *pipeline);
void CreateRtmpPipeline(GstElement *pipeline);
void CreateGenericPipeline(GstElement *pipeline);
GstElement *CreateEncoder();
bool is_initialised{false};
Sensor parentSensor;
rendering::ScenePtr scene;
rendering::CameraPtr camera;
std::string cameraName;
std::vector<common::ConnectionPtr> connections;
transport::Node node;
};
//////////////////////////////////////////////////
GstCameraPlugin::GstCameraPlugin()
: impl(std::make_unique<GstCameraPlugin::Impl>())
{
}
GstCameraPlugin::~GstCameraPlugin()
{
impl->OnRenderTeardown();
}
void GstCameraPlugin::Configure(
const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &_eventMgr)
{
impl->parentSensor = Sensor(_entity);
if (!impl->parentSensor.Valid(_ecm))
{
gzerr << "GstCameraPlugin: must be attached to a camera sensor. "
"Failed to initialize" << std::endl;
return;
}
if (auto maybeName = impl->parentSensor.Name(_ecm))
{
gzmsg << "GstCameraPlugin: attached to sensor ["
<< maybeName.value() << "]" << std::endl;
}
else
{
gzerr << "GstCameraPlugin: camera sensor has invalid name. "
"Failed to initialize" << std::endl;
return;
}
if (_sdf->HasElement("udp_host"))
{
impl->udpHost = _sdf->Get<std::string>("udp_host");
}
if (_sdf->HasElement("udp_port"))
{
impl->udpPort = _sdf->Get<int>("udp_port");
}
gzmsg << "GstCameraPlugin: streaming video to "
<< impl->udpHost << ":"
<< impl->udpPort << std::endl;
// uses MPEG2TS pipeline by default. RTMP and Generic are
// mutually exclusive with priority to RTMP
if (_sdf->HasElement("rtmp_location"))
{
impl->rtmpLocation = _sdf->Get<std::string>("rtmp_location");
impl->useRtmpPipeline = true;
}
else if (_sdf->HasElement("use_basic_pipeline"))
{
impl->useBasicPipeline = _sdf->Get<bool>("use_basic_pipeline");
}
// Use CUDA for video encoding
if (_sdf->HasElement("use_cuda"))
{
impl->useCuda = _sdf->Get<bool>("use_cuda");
}
if (_sdf->HasElement("image_topic"))
{
impl->imageTopic = _sdf->Get<std::string>("image_topic");
}
if (_sdf->HasElement("enable_topic"))
{
impl->enableTopic = _sdf->Get<std::string>("enable_topic");
}
//! @note subscriptions are deferred to Pre-Update as the enclosing
// sensor must be fully initialised before entity - component queries
// for topics names etc. to succeed.
// subscribe to events
impl->connections.push_back(
_eventMgr.Connect<gz::sim::events::RenderTeardown>(
std::bind(&GstCameraPlugin::Impl::OnRenderTeardown, impl.get())));
}
void GstCameraPlugin::PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
if (impl->cameraName.empty())
{
Entity cameraEntity = impl->parentSensor.Entity();
impl->cameraName = removeParentScope(
scopedName(cameraEntity, _ecm, "::", false), "::");
gzmsg << "GstCameraPlugin: camera name ["
<< impl->cameraName << "]" << std::endl;
}
// complete initialisation deferred from Configure()
if (!impl->is_initialised)
{
if (impl->imageTopic.empty())
{
auto maybeTopic = impl->parentSensor.Topic(_ecm);
if (!maybeTopic.has_value())
{
return;
}
impl->imageTopic = maybeTopic.value();
}
if (impl->enableTopic.empty())
{
auto maybeTopic = impl->parentSensor.Topic(_ecm);
if (!maybeTopic.has_value())
{
return;
}
impl->enableTopic = maybeTopic.value() + "/enable_streaming";
}
gzmsg << "GstCameraPlugin: image topic ["
<< impl->imageTopic << "]" << std::endl;
gzmsg << "GstCameraPlugin: enable topic ["
<< impl->enableTopic << "]" << std::endl;
// subscribe to gazebo topics
impl->node.Subscribe(impl->imageTopic,
&GstCameraPlugin::Impl::OnImage, impl.get());
impl->node.Subscribe(impl->enableTopic,
&GstCameraPlugin::Impl::OnVideoStreamEnable, impl.get());
impl->is_initialised = true;
}
if (!impl->camera && !impl->cameraName.empty())
{
impl->InitializeCamera();
return;
}
}
void GstCameraPlugin::Impl::InitializeCamera()
{
// Wait for render engine to be available.
if (rendering::loadedEngines().empty())
{
return;
}
// Get scene.
if (!scene)
{
scene = rendering::sceneFromFirstRenderEngine();
}
// Return if scene not ready or no sensors available.
if (scene == nullptr || !scene->IsInitialized()
|| scene->SensorCount() == 0)
{
gzwarn << "GstCameraPlugin: no scene or camera sensors available"
<< std::endl;
return;
}
// Get camera.
if (!camera)
{
auto sensor = scene->SensorByName(cameraName);
if (!sensor)
{
gzerr << "GstCameraPlugin: unable to find sensor ["
<< cameraName << "]" << std::endl;
return;
}
camera = std::dynamic_pointer_cast<rendering::Camera>(sensor);
if (!camera)
{
gzerr << "GstCameraPlugin: sensor ["
<< cameraName << "] is not a camera" << std::endl;
return;
}
}
}
void GstCameraPlugin::Impl::StartStreaming()
{
if (!isGstMainLoopActive)
{
pthread_create(&threadId, NULL, StartThread, this);
}
}
void *GstCameraPlugin::Impl::StartThread(void *param)
{
GstCameraPlugin::Impl *impl = (GstCameraPlugin::Impl *)param;
impl->StartGstThread();
return nullptr;
}
void GstCameraPlugin::Impl::StartGstThread()
{
gst_init(nullptr, nullptr);
gst_loop = g_main_loop_new(nullptr, FALSE);
if (!gst_loop)
{
gzerr << "GstCameraPlugin: failed to create GStreamer main loop"
<< std::endl;
return;
}
GstElement *pipeline = gst_pipeline_new(nullptr);
if (!pipeline)
{
gzerr << "GstCameraPlugin: GStreamer pipeline failed" << std::endl;
return;
}
source = gst_element_factory_make("appsrc", nullptr);
if (useRtmpPipeline)
{
CreateRtmpPipeline(pipeline);
}
else if (useBasicPipeline)
{
CreateGenericPipeline(pipeline);
}
else
{
CreateMpeg2tsPipeline(pipeline);
}
// Configure source element
g_object_set(G_OBJECT(source), "caps",
gst_caps_new_simple("video/x-raw",
"format", G_TYPE_STRING, "I420",
"width", G_TYPE_INT, width,
"height", G_TYPE_INT, height,
"framerate", GST_TYPE_FRACTION,
this->rate, 1, nullptr),
"is-live", TRUE,
"do-timestamp", TRUE,
"stream-type", GST_APP_STREAM_TYPE_STREAM,
"format", GST_FORMAT_TIME, nullptr);
gst_object_ref(source);
// Start
auto ret = gst_element_set_state(pipeline, GST_STATE_PLAYING);
if (ret != GST_STATE_CHANGE_SUCCESS)
{
gzmsg << "GstCameraPlugin: GStreamer element set state returned: "
<< ret << std::endl;
}
// this call blocks until the main_loop is killed
gzmsg << "GstCameraPlugin: starting GStreamer main loop" << std::endl;
isGstMainLoopActive = true;
g_main_loop_run(gst_loop);
isGstMainLoopActive = false;
gzmsg << "GstCameraPlugin: stopping GStreamer main loop" << std::endl;
// Clean up
gst_element_set_state(pipeline, GST_STATE_NULL);
gst_object_unref(GST_OBJECT(pipeline));
gst_object_unref(source);
g_main_loop_unref(gst_loop);
gst_loop = nullptr;
source = nullptr;
}
void GstCameraPlugin::Impl::CreateRtmpPipeline(GstElement *pipeline)
{
gzdbg << "GstCameraPlugin: creating RTMP pipeline" << std::endl;
GstElement *queue = gst_element_factory_make("queue", nullptr);
GstElement *converter = gst_element_factory_make("videoconvert", nullptr);
GstElement *encoder = CreateEncoder();
GstElement *payloader = gst_element_factory_make("flvmux", nullptr);
GstElement *sink = gst_element_factory_make("rtmpsink", nullptr);
g_object_set(G_OBJECT(sink), "location", rtmpLocation.c_str(), nullptr);
if (!source || !queue || !converter || !encoder || !payloader || !sink)
{
gzerr << "GstCameraPlugin: failed to create GStreamer elements"
<< std::endl;
return;
}
// Connect all elements to pipeline
gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder,
payloader, sink, nullptr);
// Link all elements
if (gst_element_link_many(source, queue, converter, encoder,
payloader, sink, nullptr) != TRUE)
{
gzerr << "GstCameraPlugin: failed to link GStreamer elements"
<< std::endl;
return;
}
}
void GstCameraPlugin::Impl::CreateGenericPipeline(GstElement *pipeline)
{
gzdbg << "GstCameraPlugin: creating generic pipeline" << std::endl;
GstElement *queue = gst_element_factory_make("queue", nullptr);
GstElement *converter = gst_element_factory_make("videoconvert", nullptr);
GstElement *encoder = CreateEncoder();
GstElement *payloader = gst_element_factory_make("rtph264pay", nullptr);
GstElement *sink = gst_element_factory_make("udpsink", nullptr);
g_object_set(G_OBJECT(sink), "host", udpHost.c_str(),
"port", udpPort, nullptr);
if (!source || !queue || !converter || !encoder || !payloader || !sink)
{
gzerr << "GstCameraPlugin: failed to create GStreamer elements"
<< std::endl;
return;
}
// Connect all elements to pipeline
gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder,
payloader, sink, nullptr);
// Link all elements
if (gst_element_link_many(source, queue, converter, encoder,
payloader, sink, nullptr) != TRUE)
{
gzerr << "GstCameraPlugin: failed to link GStreamer elements"
<< std::endl;
return;
}
}
void GstCameraPlugin::Impl::CreateMpeg2tsPipeline(GstElement *pipeline)
{
gzdbg << "GstCameraPlugin: creating MPEG2TS pipeline" << std::endl;
GstElement *queue = gst_element_factory_make("queue", nullptr);
GstElement *converter = gst_element_factory_make("videoconvert", nullptr);
GstElement *encoder = CreateEncoder();
GstElement *h264_parser = gst_element_factory_make("h264parse", nullptr);
GstElement *payloader = gst_element_factory_make("mpegtsmux", nullptr);
GstElement *queue_mpeg = gst_element_factory_make("queue", nullptr);
GstElement *sink = gst_element_factory_make("udpsink", nullptr);
g_object_set(G_OBJECT(payloader), "alignment", 7, nullptr);
g_object_set(G_OBJECT(sink), "host", udpHost.c_str(), "port", udpPort,
"sync", false, nullptr);
if (!source || !queue || !converter || !encoder || !h264_parser
|| !payloader || !queue_mpeg || !sink)
{
gzerr << "GstCameraPlugin: failed to create GStreamer elements"
<< std::endl;
return;
}
gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder,
h264_parser, payloader, queue_mpeg, sink, nullptr);
if (gst_element_link_many(source, queue, converter, encoder,
h264_parser, payloader, queue_mpeg, sink, nullptr) != TRUE)
{
gzerr << "GstCameraPlugin: failed to link GStreamer elements"
<< std::endl;
return;
}
}
GstElement* GstCameraPlugin::Impl::CreateEncoder()
{
GstElement* encoder{nullptr};
if (useCuda)
{
gzdbg << "Using Cuda" << std::endl;
encoder = gst_element_factory_make("nvh264enc", nullptr);
g_object_set(G_OBJECT(encoder), "bitrate", 800, "preset", 1, nullptr);
}
else
{
encoder = gst_element_factory_make("x264enc", nullptr);
g_object_set(G_OBJECT(encoder), "bitrate", 800, "speed-preset", 6,
"tune", 4, "key-int-max", 10, nullptr);
}
return encoder;
}
void GstCameraPlugin::Impl::OnImage(const msgs::Image &msg)
{
if (requestedStartStreaming)
{
width = msg.width();
height = msg.height();
StartStreaming();
requestedStartStreaming = false;
return;
}
if (!isGstMainLoopActive) return;
// Alloc buffer
const guint size = width * height * 1.5;
GstBuffer *buffer = gst_buffer_new_allocate(NULL, size, NULL);
if (!buffer)
{
gzerr << "GstCameraPlugin: gst_buffer_new_allocate failed"
<< std::endl;
return;
}
GstMapInfo map;
if (!gst_buffer_map(buffer, &map, GST_MAP_WRITE))
{
gzerr << "GstCameraPlugin: gst_buffer_map failed" << std::endl;
return;
}
// Color Conversion from RGB to YUV
cv::Mat frame = cv::Mat(height, width, CV_8UC3);
cv::Mat frameYUV = cv::Mat(height, width, CV_8UC3);
frame.data = reinterpret_cast<unsigned char *>(
const_cast<char *>(msg.data().c_str()));
cvtColor(frame, frameYUV, cv::COLOR_RGB2YUV_I420);
memcpy(map.data, frameYUV.data, size);
gst_buffer_unmap(buffer, &map);
GstFlowReturn ret =
gst_app_src_push_buffer(GST_APP_SRC(this->source), buffer);
if (ret != GST_FLOW_OK)
{
// Something wrong, stop pushing
gzerr << "GstCameraPlugin: gst_app_src_push_buffer failed"
<< std::endl;
g_main_loop_quit(gst_loop);
}
}
void GstCameraPlugin::Impl::OnVideoStreamEnable(const msgs::Boolean &msg)
{
gzmsg << "GstCameraPlugin:: streaming: "
<< (msg.data() ? "started" : "stopped") << std::endl;
if (msg.data())
{
requestedStartStreaming = true;
}
else
{
requestedStartStreaming = false;
StopStreaming();
}
}
void GstCameraPlugin::Impl::OnRenderTeardown()
{
StopStreaming();
camera.reset();
scene.reset();
}
void GstCameraPlugin::Impl::StopStreaming()
{
if (isGstMainLoopActive)
{
StopGstThread();
pthread_join(threadId, NULL);
isGstMainLoopActive = false;
}
}
void GstCameraPlugin::Impl::StopGstThread()
{
if (gst_loop)
{
g_main_loop_quit(gst_loop);
}
}
//////////////////////////////////////////////////
} // namespace systems
} // namespace GZ_SIM_VERSION_NAMESPACE
} // namespace sim
} // namespace gz
GZ_ADD_PLUGIN(
gz::sim::systems::GstCameraPlugin,
gz::sim::System,
gz::sim::systems::GstCameraPlugin::ISystemConfigure,
gz::sim::systems::GstCameraPlugin::ISystemPreUpdate)
GZ_ADD_PLUGIN_ALIAS(
gz::sim::systems::GstCameraPlugin,
"GstCameraPlugin")

392
guiTools/ardupilot_gazebo/src/ParachutePlugin.cc Normal file
View File

@ -0,0 +1,392 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include "ParachutePlugin.hh"
#include <gz/msgs/entity_factory.pb.h>
#include <string>
#include <gz/plugin/Register.hh>
#include <gz/common/Profiler.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Quaternion.hh>
#include <gz/sim/components/DetachableJoint.hh>
#include <gz/sim/components/Model.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/ParentEntity.hh>
#include <gz/sim/components/Link.hh>
#include <gz/sim/components/World.hh>
#include <gz/sim/Link.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/World.hh>
#include <gz/sim/Util.hh>
#include <gz/transport/Node.hh>
namespace gz {
namespace sim {
inline namespace GZ_SIM_VERSION_NAMESPACE {
namespace systems {
//////////////////////////////////////////////////
class ParachutePlugin::Impl
{
/// \brief Callback for subscription to the release command.
///
/// \param _msg
/// The command message. Should be a normalised PWM level in [0, 1].
public: void OnCommand(const msgs::Double &_msg);
/// \brief World occupied by the parent model.
public: World world{kNullEntity};
/// \brief Name of the world entity.
public: std::string worldName;
/// \brief Model entity that will release the parachute.
public: Model parentModel{kNullEntity};
/// \brief Name of the model entity.
public: std::string parentModelName;
/// \brief Link entity to attach the parachute.
public: Link parentLink{kNullEntity};
/// \brief Name of the link entity to attach the parachute.
public: std::string parentLinkName;
/// \brief The parachute model.
public: Model childModel{kNullEntity};
/// \brief Link entity of the parachute model to attach to parent.
public: Link childLink{kNullEntity};
/// \brief Name of the parachute model.
public: std::string childModelName;
/// \brief Name of the link entity in parachute to attach to parent.
public: std::string childLinkName;
/// \brief The detachable joint entity created when the parachute is attached.
public: Entity detachableJointEntity{kNullEntity};
/// \brief A flag set when the parachute has it's initial position saved.
public: bool initPosSaved{false};
/// \brief The pose of the parachute when first created.
public: math::Pose3d initialPos;
/// \brief The pose of the parachute relative to the parent link.
public: math::Pose3d childPose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
/// \brief Name of the topic to subscribe to release commands.
public: std::string commandTopic;
/// \brief Value of the most recently received command.
public: std::atomic<double> command{0.0};
/// \brief Flag set to true if parachute released and attachment required.
public: std::atomic<bool> attachRequested{false};
/// \brief Flag set to true if the model is correctly initialised.
public: bool validConfig{false};
/// \brief Flag set to true if the parachute is created and attached.
public: bool attached{false};
/// \brief Flag set to true if the parachute should be created and attached.
public: bool shouldAttach{false};
/// \brief Flag set to true when the parachute pose is set
/// and it is ready to attach.
public: bool modelOk{false};
/// \brief Flag set to true when the parachute model has been created.
public: bool parachuteCreated{false};
/// \brief Transport node for subscriptions.
public: transport::Node node;
};
//////////////////////////////////////////////////
void ParachutePlugin::Impl::OnCommand(const msgs::Double &_msg)
{
this->command = _msg.data();
if (this->command > 0.9)
{
this->attachRequested = true;
}
}
//////////////////////////////////////////////////
//////////////////////////////////////////////////
ParachutePlugin::~ParachutePlugin() = default;
//////////////////////////////////////////////////
ParachutePlugin::ParachutePlugin() :
impl(std::make_unique<ParachutePlugin::Impl>())
{
}
//////////////////////////////////////////////////
void ParachutePlugin::Configure(
const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &)
{
// capture model entity
this->impl->parentModel = Model(_entity);
if (!this->impl->parentModel.Valid(_ecm))
{
gzerr << "ParachutePlugin should be attached to a model. "
"Failed to initialize.\n";
return;
}
this->impl->parentModelName = this->impl->parentModel.Name(_ecm);
// retrieve world entity
this->impl->world = World(
_ecm.EntityByComponents(components::World()));
if (!this->impl->world.Valid(_ecm))
{
gzerr << "ParachutePlugin - world not found. "
"Failed to initialize.\n";
return;
}
if (this->impl->world.Name(_ecm).has_value())
{
this->impl->worldName = this->impl->world.Name(_ecm).value();
}
// parameters
if (_sdf->HasElement("parent_link"))
{
this->impl->parentLinkName = _sdf->Get<std::string>("parent_link");
}
else
{
gzerr << "ParachutePlugin requires parameter 'parent_link'. "
"Failed to initialize.\n";
return;
}
if (_sdf->HasElement("child_model"))
{
this->impl->childModelName = _sdf->Get<std::string>("child_model");
}
else
{
gzerr << "ParachutePlugin requires parameter 'child_model'. "
"Failed to initialize.\n";
return;
}
if (_sdf->HasElement("child_link"))
{
this->impl->childLinkName = _sdf->Get<std::string>("child_link");
}
else
{
gzerr << "ParachutePlugin requires parameter 'child_link'. "
"Failed to initialize.\n";
return;
}
if (_sdf->HasElement("child_pose"))
{
this->impl->childPose = _sdf->Get<math::Pose3d>("child_pose");
gzdbg << "child_pos: pos: " << this->impl->childPose.Pos() << std::endl;
gzdbg << "child_pos: rot: " << this->impl->childPose.Rot() << std::endl;
}
// configure parachute release command topic
{
std::vector<std::string> topics;
if (_sdf->HasElement("cmd_topic"))
{
topics.push_back(_sdf->Get<std::string>("cmd_topic"));
}
topics.push_back("/model/" + this->impl->parentModelName +
"/parachute/cmd_release");
this->impl->commandTopic = validTopic(topics);
}
// resolve links
this->impl->parentLink = Link(_ecm.EntityByComponents(
components::Link(),
components::ParentEntity(this->impl->parentModel.Entity()),
components::Name(this->impl->parentLinkName)));
if (!this->impl->parentLink.Valid(_ecm))
{
gzerr << "ParachutePlugin - parent link ["
<< this->impl->parentLinkName
<< "] not found. "
"Failed to initialize.\n";
return;
}
this->impl->parentLink.EnableVelocityChecks(_ecm);
// subscriptions
this->impl->node.Subscribe(
this->impl->commandTopic,
&ParachutePlugin::Impl::OnCommand, this->impl.get());
gzdbg << "ParachutePlugin subscribing to messages on "
<< "[" << this->impl->commandTopic << "]\n";
this->impl->validConfig = true;
}
//////////////////////////////////////////////////
void ParachutePlugin::PreUpdate(
const UpdateInfo &/*_info*/,
EntityComponentManager &_ecm)
{
GZ_PROFILE("ParachutePlugin::PreUpdate");
if (this->impl->validConfig &&
this->impl->shouldAttach &&
!this->impl->attached)
{
// create parachute
if (!this->impl->parachuteCreated)
{
bool result;
msgs::EntityFactory req;
msgs::Boolean res;
// request creation of parachute (child) model
req.set_sdf_filename(this->impl->childModelName);
bool executed = this->impl->node.Request(
"world/" + this->impl->worldName + "/create",
req, 5000, res, result);
if (executed)
{
if (result)
{
gzdbg << "Parachute model created.\n";
this->impl->parachuteCreated = true;
}
else
{
gzerr << "Failed to create parachute model.\n";
this->impl->shouldAttach = false;
}
}
else
{
gzdbg << "Parachute model creation timed out.\n";
this->impl->shouldAttach = false;
}
}
// retrive parachute model entity
this->impl->childModel = Model(_ecm.EntityByComponents(
components::Model(),
components::Name(this->impl->childModelName)));
if (this->impl->childModel.Valid(_ecm))
{
this->impl->childLink = Link(_ecm.EntityByComponents(
components::Link(),
components::ParentEntity(this->impl->childModel.Entity()),
components::Name(this->impl->childLinkName)));
// parent link world pose
auto X_WPl = worldPose(this->impl->parentLink.Entity(), _ecm);
auto X_PC = this->impl->childPose;
auto X_WC = X_WPl * X_PC;
// debug - check pose
gzdbg << "X_WPl: " << X_WPl.Pos() << ", "
<< X_WPl.Rot().Euler() << "\n";
gzdbg << "X_PC: " << X_PC.Pos() << ", "
<< X_PC.Rot().Euler() << "\n";
gzdbg << "X_WC: " << X_WC.Pos() << ", "
<< X_WC.Rot().Euler() << "\n";
this->impl->childModel.SetWorldPoseCmd(_ecm, X_WC);
X_WC = worldPose(this->impl->childModel.Entity(), _ecm);
// debug - check if child pose has updated
gzdbg << "X_WC: " << X_WC.Pos() << ", "
<< X_WC.Rot().Euler() << "\n";
if (!this->impl->initPosSaved)
{
this->impl->initialPos = X_WC;
this->impl->initPosSaved = true;
}
if (this->impl->initialPos != X_WC)
{
gzdbg << "Model OK\n";
this->impl->modelOk = true;
}
if (this->impl->childLink.Valid(_ecm) &&
this->impl->modelOk)
{
// connect the models using a detachable joint
this->impl->detachableJointEntity = _ecm.CreateEntity();
_ecm.CreateComponent(
this->impl->detachableJointEntity,
components::DetachableJoint({
this->impl->parentLink.Entity(),
this->impl->childLink.Entity(),
"fixed"}));
this->impl->attached = true;
this->impl->shouldAttach = false;
}
else if (!this->impl->childLink.Valid(_ecm))
{
gzerr << "ParachutePlugin - child link ["
<< this->impl->childLinkName
<< "] not found. "
"Failed to create joint.\n";
}
}
}
// attach requested
if (this->impl->attachRequested &&
this->impl->detachableJointEntity == kNullEntity &&
!this->impl->shouldAttach)
{
gzdbg << "Parachute release requested!\n";
this->impl->shouldAttach = true;
this->impl->attachRequested = false;
}
}
//////////////////////////////////////////////////
} // namespace systems
}
} // namespace sim
} // namespace gz
GZ_ADD_PLUGIN(
gz::sim::systems::ParachutePlugin,
gz::sim::System,
gz::sim::systems::ParachutePlugin::ISystemConfigure,
gz::sim::systems::ParachutePlugin::ISystemPreUpdate)
GZ_ADD_PLUGIN_ALIAS(
gz::sim::systems::ParachutePlugin,
"ParachutePlugin")

149
guiTools/ardupilot_gazebo/src/SocketUDP.cc Normal file
View File

@ -0,0 +1,149 @@
/*
Copyright (C) 2024 ardupilot.org
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "SocketUDP.hh"
#include <cstdio>
#include <cstdlib>
#include <cstring>
SocketUDP::SocketUDP(bool reuseaddress, bool blocking) {
fd = socket(AF_INET, SOCK_DGRAM, 0);
if (fd < 0) {
perror("SocketUDP creation failed");
exit(EXIT_FAILURE);
}
#ifndef _WIN32
// Windows does not support FD_CLOEXEC
fcntl(fd, F_SETFD, FD_CLOEXEC);
#endif
if (reuseaddress) {
set_reuseaddress();
}
if (blocking) {
set_blocking(true);
}
}
SocketUDP::~SocketUDP() {
if (fd != -1) {
::close(fd);
fd = -1;
}
}
bool SocketUDP::bind(const char *address, uint16_t port) {
struct sockaddr_in server_addr{};
make_sockaddr(address, port, server_addr);
if (::bind(fd, reinterpret_cast<sockaddr *>(&server_addr),
sizeof(server_addr)) != 0) {
perror("SocketUDP Bind failed");
#ifdef _WIN32
closesocket(fd);
#else
close(fd);
#endif
return false;
}
return true;
}
bool SocketUDP::set_reuseaddress() {
int one = 1;
return (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != -1);
}
bool SocketUDP::set_blocking(bool blocking) {
int fcntl_ret;
#ifdef _WIN32
u_long mode = blocking ? 0 : 1;
fcntl_ret = ioctlsocket(fd, FIONBIO, reinterpret_cast<u_long FAR *>(&mode));
#else
if (blocking) {
fcntl_ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) & ~O_NONBLOCK);
} else {
fcntl_ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK);
}
#endif
return fcntl_ret != -1;
}
ssize_t SocketUDP::sendto(const void *buf, size_t size, const char *address,
uint16_t port) {
struct sockaddr_in sockaddr_out{};
make_sockaddr(address, port, sockaddr_out);
return ::sendto(fd, buf, size, 0,
reinterpret_cast<sockaddr *>(&sockaddr_out),
sizeof(sockaddr_out));
}
/*
receive some data
*/
ssize_t SocketUDP::recv(void *buf, size_t size, uint32_t timeout_ms) {
if (!pollin(timeout_ms)) {
return -1;
}
socklen_t len = sizeof(in_addr);
return ::recvfrom(fd, buf, size, MSG_DONTWAIT,
reinterpret_cast<sockaddr *>(&in_addr), &len);
}
void SocketUDP::get_client_address(const char *&ip_addr, uint16_t &port) {
ip_addr = inet_ntoa(in_addr.sin_addr);
port = ntohs(in_addr.sin_port);
}
bool SocketUDP::pollin(uint32_t timeout_ms) {
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(fd, &fds);
tv.tv_sec = timeout_ms / 1000;
tv.tv_usec = (timeout_ms % 1000) * 1000UL;
if (select(fd + 1, &fds, nullptr, nullptr, &tv) != 1) {
return false;
}
return true;
}
void SocketUDP::make_sockaddr(const char *address, uint16_t port,
struct sockaddr_in &sockaddr) {
sockaddr = {};
sockaddr.sin_family = AF_INET;
sockaddr.sin_addr.s_addr = inet_addr(address);
sockaddr.sin_port = htons(port);
#ifdef HAVE_SOCK_SIN_LEN
sockaddr.sin_len = sizeof(sockaddr);
#endif
}

115
guiTools/ardupilot_gazebo/src/Util.cc Normal file
View File

@ -0,0 +1,115 @@
/*
Copyright (C) 2022 ardupilot.org
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Util.hh"
#include <gz/sim/components/Joint.hh>
#include <gz/sim/components/JointVelocity.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/ParentEntity.hh>
#include <gz/sim/Util.hh>
namespace gz
{
namespace sim
{
inline namespace GZ_SIM_VERSION_NAMESPACE {
//////////////////////////////////////////////////
std::unordered_set<Entity> EntitiesFromUnscopedName(
const std::string &_name, const EntityComponentManager &_ecm,
Entity _relativeTo)
{
// holds entities that match
std::vector<Entity> entities;
if (_relativeTo == kNullEntity)
{
// search everything
entities = _ecm.EntitiesByComponents(components::Name(_name));
}
else
{
// search all descendents
auto descendents = _ecm.Descendants(_relativeTo);
for (const auto& descendent : descendents)
{
if (_ecm.EntityHasComponentType(descendent,
gz::sim::components::Name::typeId))
{
auto nameComp = _ecm.Component<gz::sim::components::Name>(descendent);
if (nameComp->Data() == _name)
{
entities.push_back(descendent);
}
}
}
}
if (entities.empty())
return {};
return std::unordered_set<Entity>(entities.begin(), entities.end());
}
//////////////////////////////////////////////////
Entity JointByName(EntityComponentManager &_ecm,
Entity _modelEntity,
const std::string &_name)
{
// Retrieve entities from a scoped name.
// See for example:
// https://github.com/gazebosim/ign-gazebo/pull/955
// which applies to the LiftDrag plugin
auto entities = entitiesFromScopedName(_name, _ecm, _modelEntity);
if (entities.empty())
{
gzerr << "Joint with name [" << _name << "] not found. "
<< "The joint will not respond to ArduPilot commands\n";
return kNullEntity;
}
else if (entities.size() > 1)
{
gzwarn << "Multiple joint entities with name[" << _name << "] found. "
<< "Using the first one.\n";
}
Entity joint = *entities.begin();
// Validate
if (!_ecm.EntityHasComponentType(joint,
components::Joint::typeId))
{
gzerr << "Entity with name[" << _name << "] is not a joint\n";
return kNullEntity;
}
// Ensure the joint has a velocity component
if (!_ecm.EntityHasComponentType(joint,
components::JointVelocity::typeId))
{
_ecm.CreateComponent(joint,
components::JointVelocity());
}
return joint;
};
}
} // namespace sim
} // namespace gz

301
guiTools/ardupilot_gazebo/tests/worlds/test_anemometer.sdf Normal file
View File

@ -0,0 +1,301 @@
<?xml version="1.0" ?>
<!--
Usage
Ensure tests/worlds is added to GZ_SIM_RESOURCE_PATH
Gazebo
gz sim -v4 -s -r test_anemometer.sdf
SITL
sim_vehicle.py -D -v Rover -/-model JSON -/-console -/-map
MANUAL> param set WNDVN_TYPE 11
MANUAL> param set WNDVN_SPEED_TYPE 11
MANUAL> param set WNDVN_SPEED_OFS 0
MANUAL> module load sail
-->
<sdf version="1.9">
<world name="test_anemometer">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
<background_color>0.8 0.8 0.8</background_color>
</plugin>
<plugin filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin filename="asv_sim2-anemometer-system"
name="gz::sim::systems::Anemometer">
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-navsat-system"
name="gz::sim::systems::NavSat">
</plugin>
<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
<sky></sky>
</scene>
<spherical_coordinates>
<latitude_deg>-35.363262</latitude_deg>
<longitude_deg>149.165237</longitude_deg>
<elevation>584</elevation>
<heading_deg>0</heading_deg>
<surface_model>EARTH_WGS84</surface_model>
</spherical_coordinates>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.6 0.6 0.6 1</specular>
<direction>-0.5 0.1 -0.9</direction>
</light>
<!-- Wind directed from the north at 5 m/s -->
<wind>
<linear_velocity>0 -5 0</linear_velocity>
</wind>
<model name="axes">
<static>1</static>
<link name="link">
<visual name="r">
<cast_shadows>0</cast_shadows>
<pose>5 0 0.1 0 0 0</pose>
<geometry>
<box>
<size>10 0.01 0.01</size>
</box>
</geometry>
<material>
<ambient>1 0 0 0.8</ambient>
<diffuse>1 0 0 0.8</diffuse>
<emissive>1 0 0 0.8</emissive>
<specular>0.5 0.5 0.5 0.8</specular>
</material>
</visual>
<visual name="g">
<cast_shadows>0</cast_shadows>
<pose>0 5 0.1 0 0 0</pose>
<geometry>
<box>
<size>0.01 10 0.01</size>
</box>
</geometry>
<material>
<ambient>0 1 0 0.8</ambient>
<diffuse>0 1 0 0.8</diffuse>
<emissive>0 1 0 0.8</emissive>
<specular>0.5 0.5 0.5 0.8</specular>
</material>
</visual>
<visual name="b">
<cast_shadows>0</cast_shadows>
<pose>0 0 5.1 0 0 0</pose>
<geometry>
<box>
<size>0.01 0.01 10</size>
</box>
</geometry>
<material>
<ambient>0 0 1 0.8</ambient>
<diffuse>0 0 1 0.8</diffuse>
<emissive>0 0 1 0.8</emissive>
<specular>0.5 0.5 0.5 0.8</specular>
</material>
</visual>
<sensor name="navsat_sensor" type="navsat">
<always_on>1</always_on>
<update_rate>1</update_rate>
</sensor>
</link>
</model>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="anemometer">
<pose degrees="true">0 0 0.5 0 0 90</pose>
<link name="base_link">
<inertial>
<mass>10</mass>
<inertia>
<ixx>1.6</ixx>
<ixy>0</ixy>
<iyy>1.6</iyy>
<iyz>0</iyz>
<izz>1.6</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>0.0 0.5 0.0 0.7</ambient>
<diffuse>0.0 0.5 0.0 0.7</diffuse>
<specular>0.1 0.1 0.1 0.7</specular>
</material>
</visual>
<visual name="direction_visual">
<pose>0.425 0 0.5005 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.001</length>
</cylinder>
</geometry>
<material>
<ambient>1 0 0 0.7</ambient>
<diffuse>1 0 0 0.7</diffuse>
<specular>0.1 0.1 0.1 0.7</specular>
</material>
</visual>
</link>
<link name='imu_link'>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.15</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00002</iyy>
<iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<sensor name="imu_sensor" type="imu">
<pose degrees="true">0 0 0 180 0 0</pose>
<always_on>1</always_on>
<update_rate>1000.0</update_rate>
</sensor>
</link>
<joint name='imu_joint' type='revolute'>
<child>imu_link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<damping>1.0</damping>
</dynamics>
</axis>
</joint>
<link name='anemometer_link'>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.15</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00002</iyy>
<iyz>0</iyz>
<izz>0.00002</izz>
</inertia>
</inertial>
<sensor name="anemometer" type="custom" gz:type="anemometer">
<always_on>1</always_on>
<update_rate>30</update_rate>
<gz:anemometer>
<noise type="gaussian">
<mean>0.2</mean>
<stddev>0.1</stddev>
</noise>
</gz:anemometer>
</sensor>
</link>
<joint name='anemometer_joint' type='revolute'>
<child>anemometer_link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<damping>1.0</damping>
</dynamics>
</axis>
</joint>
<plugin name="ArduPilotPlugin"
filename="ArduPilotPlugin">
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
<modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
<imuName>imu_link::imu_sensor</imuName>
<anemometer>anemometer_link::anemometer</anemometer>
</plugin>
</model>
</world>
</sdf>

163
guiTools/ardupilot_gazebo/tests/worlds/test_gimbal.sdf Executable file
View File

@ -0,0 +1,163 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<world name="test_gimbal">
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin name="gz::sim::systems::Physics"
filename="gz-sim-physics-system">
</plugin>
<plugin name="gz::sim::systems::Sensors"
filename="gz-sim-sensors-system">
<render_engine>ogre2</render_engine>
</plugin>
<plugin name="gz::sim::systems::UserCommands"
filename="gz-sim-user-commands-system">
</plugin>
<plugin name="gz::sim::systems::SceneBroadcaster"
filename="gz-sim-scene-broadcaster-system">
</plugin>
<plugin name="gz::sim::systems::Imu"
filename="gz-sim-imu-system">
</plugin>
<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
<sky></sky>
</scene>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<include>
<uri>model://runway</uri>
<pose degrees="true">-29 545 0 0 0 363</pose>
</include>
<!-- gimbal -->
<!-- <include>
<uri>model://gimbal_small_2d</uri>
<name>gimbal</name>
<pose degrees="true">0 0 0.11 -90 0 90</pose>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>roll_link</joint_name>
<topic>arm_cmd</topic>
<p_gain>2</p_gain>
<i_gain>0.1</i_gain>
<d_gain>0.01</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>1000</cmd_max>
<cmd_min>-1000</cmd_min>
</plugin>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>tilt_joint</joint_name>
<topic>tilt_cmd</topic>
<p_gain>2</p_gain>
<i_gain>0.1</i_gain>
<d_gain>0.01</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>1000</cmd_max>
<cmd_min>-1000</cmd_min>
</plugin>
</include> -->
<!-- nested -->
<model name="gimbal_base">
<pose>2 0 0.5 0 0 0</pose>
<link name="base_link">
<inertial>
<mass>0.25</mass>
<inertia>
<ixx>0.000417</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.000417</iyy>
<iyz>0.00</iyz>
<izz>0.000417</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
<include>
<uri>model://gimbal_small_2d</uri>
<name>gimbal</name>
<pose degrees="true">0 0 0.61 -90 0 -90</pose>
</include>
<joint name="gimbal_joint" type="revolute">
<parent>base_link</parent>
<child>gimbal::base_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
</dynamics>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin name="gz::sim::systems::JointStatePublisher"
filename="gz-sim-joint-state-publisher-system">
</plugin>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>gimbal::roll_joint</joint_name>
<topic>/gimbal/cmd_roll</topic>
<p_gain>2</p_gain>
</plugin>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>gimbal::tilt_joint</joint_name>
<topic>/gimbal/cmd_pitch</topic>
<p_gain>2</p_gain>
</plugin>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>gimbal::yaw_joint</joint_name>
<topic>/gimbal/cmd_yaw</topic>
<p_gain>2</p_gain>
</plugin>
</model>
</world>
</sdf>

365
guiTools/ardupilot_gazebo/tests/worlds/test_nested_model.sdf Executable file
View File

@ -0,0 +1,365 @@
<?xml version="1.0" ?>
<!--
Run example
gz sim -v4 -r test_nested_model.sdf
Move rotor in model1
gz topic -t "/rotor1_cmd" -m gz.msgs.Double -p "data: 1"
Move rotor in model2 (nested)
gz topic -t "/rotor2_cmd" -m gz.msgs.Double -p "data: 1"
Move rotor in model3
gz topic -t "/rotor3_cmd" -m gz.msgs.Double -p "data: 1"
-->
<sdf version="1.9">
<world name="test_nested_model">
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
</scene>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<!-- model with a position controlled joint -->
<model name="model1">
<pose>0 0 0.5 0 0 0</pose>
<link name="base1_link">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.166666667</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.166666667</iyy>
<iyz>0.00</iyz>
<izz>0.166666667</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
<link name="rotor1_link">
<pose>0 0 0.6 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>1.66667E-05</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.000841667</iyy>
<iyz>0.00</iyz>
<izz>0.000841667</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 0.1 0.1</size>
</box>
</geometry>
<material>
<ambient>1 0.5 0</ambient>
<diffuse>1 0.5 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 0.1 0.1</size>
</box>
</geometry>
</collision>
</link>
<joint name="rotor1_joint" type="revolute">
<parent>base1_link</parent>
<child>rotor1_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
</dynamics>
<limit>
<lower>-3.14159265</lower>
<upper>3.14159265</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>rotor1_joint</joint_name>
<topic>rotor1_cmd</topic>
<p_gain>2</p_gain>
</plugin>
</model>
<!-- nested model with two position controlled joints -->
<model name="model3">
<pose>2 0 0.5 0 0 0</pose>
<link name="base3_link">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.166666667</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.166666667</iyy>
<iyz>0.00</iyz>
<izz>0.166666667</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>0 1 1</ambient>
<diffuse>0 1 1</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
<link name="rotor3_link">
<pose>0 0 0.6 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>1.66667E-05</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.000841667</iyy>
<iyz>0.00</iyz>
<izz>0.000841667</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 0.1 0.1</size>
</box>
</geometry>
<material>
<ambient>1 0.5 0</ambient>
<diffuse>1 0.5 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 0.1 0.1</size>
</box>
</geometry>
</collision>
</link>
<joint name="rotor3_joint" type="revolute">
<parent>base3_link</parent>
<child>rotor3_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
</dynamics>
<limit>
<lower>-3.14159265</lower>
<upper>3.14159265</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>rotor3_joint</joint_name>
<topic>rotor3_cmd</topic>
<p_gain>2</p_gain>
</plugin>
<model name="model2">
<pose>1.0 0 0 0 0 0</pose>
<link name="base2_link">
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.166666667</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.166666667</iyy>
<iyz>0.00</iyz>
<izz>0.166666667</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
<link name="rotor2_link">
<pose>0 0 0.6 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>1.66667E-05</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.000841667</iyy>
<iyz>0.00</iyz>
<izz>0.000841667</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 0.1 0.1</size>
</box>
</geometry>
<material>
<ambient>1 0.5 0</ambient>
<diffuse>1 0.5 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 0.1 0.1</size>
</box>
</geometry>
</collision>
</link>
<joint name="rotor2_joint" type="revolute">
<parent>base2_link</parent>
<child>rotor2_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
</dynamics>
<limit>
<lower>-3.14159265</lower>
<upper>3.14159265</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>rotor2_joint</joint_name>
<topic>rotor2_cmd</topic>
<p_gain>2</p_gain>
</plugin>
</model>
<joint name="model2_joint" type="revolute">
<parent>base3_link</parent>
<child>model2::base2_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
</dynamics>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
</model>
</world>
</sdf>

143
guiTools/ardupilot_gazebo/tests/worlds/test_parachute.sdf Executable file
View File

@ -0,0 +1,143 @@
<?xml version="1.0" ?>
<!--
Parachute Example
A standalone example using the parachute plugin.
A box is dropped from 50m. The parachute is deployed by issuing a
command with value 1.0 to the topic /parachute/cmd_release.
Server
======
gz sim -v4 -s test_parachute.sdf
Publish
=======
gz topic -t /parachute/cmd_release -m gz.msgs.Double -p 'data:1.0'
-->
<sdf version="1.9">
<world name="test_parachute">
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>0.1</real_time_factor>
</physics>
<plugin filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
<sky></sky>
</scene>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<include>
<uri>model://runway</uri>
<pose degrees="true">-29 545 0 0 0 363</pose>
</include>
<model name="payload">
<pose degrees="true">0 0 50 0 0 90</pose>
<link name="base_link">
<inertial>
<mass>0.25</mass>
<inertia>
<ixx>0.000417</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.000417</iyy>
<iyz>0.00</iyz>
<izz>0.000417</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
</link>
<link name="attachment_link">
<pose>0 0 0.6 0 0 0</pose>
<inertial>
<mass>0.025</mass>
<inertia>
<ixx>0.0000417</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.0000417</iyy>
<iyz>0.00</iyz>
<izz>0.0000417</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
<material>
<ambient>0.7 0.7 0.7</ambient>
<diffuse>0.7 0.7 0.7</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
<collision name="collision">
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
</link>
<joint name="attachment_joint" type="ball">
<child>attachment_link</child>
<parent>base_link</parent>
</joint>
<plugin filename="ParachutePlugin" name="ParachutePlugin">
<parent_link>attachment_link</parent_link>
<child_model>parachute_small</child_model>
<child_link>chute</child_link>
<child_pose>0 0 0 0 0 0</child_pose>
<cmd_topic>/parachute/cmd_release</cmd_topic>
</plugin>
</model>
</world>
</sdf>

288
guiTools/ardupilot_gazebo/worlds/gimbal.sdf Executable file
View File

@ -0,0 +1,288 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<world name="gimbal">
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin name="gz::sim::systems::Physics"
filename="gz-sim-physics-system">
</plugin>
<plugin name="gz::sim::systems::Sensors"
filename="gz-sim-sensors-system">
<render_engine>ogre2</render_engine>
</plugin>
<plugin name="gz::sim::systems::UserCommands"
filename="gz-sim-user-commands-system">
</plugin>
<plugin name="gz::sim::systems::SceneBroadcaster"
filename="gz-sim-scene-broadcaster-system">
</plugin>
<plugin name="gz::sim::systems::Imu"
filename="gz-sim-imu-system">
</plugin>
<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
<sky></sky>
</scene>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<!-- Gimbal mount fixed to world. -->
<model name="mount">
<link name="mount_link">
<inertial>
<mass>10</mass>
<inertia>
<ixx>0.13733333</ixx>
<ixy>0.00</ixy>
<ixz>0.00</ixz>
<iyy>0.13733333</iyy>
<iyz>0.00</iyz>
<izz>0.008</izz>
</inertia>
</inertial>
<visual name="post_visual">
<pose>-0.2 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.04</radius>
<length>1.0</length>
</cylinder>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<visual name="plate_visual">
<pose>-0.1 0 1.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<ambient>1 1 0</ambient>
<diffuse>1 1 0</diffuse>
<specular>0.1 0.1 0 1</specular>
</material>
</visual>
<collision name="post_collision">
<pose>-0.2 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.04</radius>
<length>1.0</length>
</cylinder>
</geometry>
</collision>
<collision name="plate_collision">
<pose>-0.1 0 1.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.2</radius>
<length>0.02</length>
</cylinder>
</geometry>
</collision>
</link>
<joint name="mount_joint" type="fixed">
<parent>world</parent>
<child>mount_link</child>
</joint>
<include>
<uri>model://gimbal_small_3d</uri>
<name>gimbal</name>
<pose degrees="true">0 0 0.9055 90 0 90</pose>
</include>
<joint name="gimbal_joint" type="revolute">
<parent>mount_link</parent>
<child>gimbal::base_link</child>
<axis>
<dynamics>
<damping>0.5</damping>
</dynamics>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin name="gz::sim::systems::JointStatePublisher"
filename="gz-sim-joint-state-publisher-system">
</plugin>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>gimbal::yaw_joint</joint_name>
<topic>/gimbal/cmd_yaw</topic>
<p_gain>2</p_gain>
</plugin>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>gimbal::roll_joint</joint_name>
<topic>/gimbal/cmd_roll</topic>
<p_gain>2</p_gain>
</plugin>
<plugin name="gz::sim::systems::JointPositionController"
filename="gz-sim-joint-position-controller-system">
<joint_name>gimbal::pitch_joint</joint_name>
<topic>/gimbal/cmd_pitch</topic>
<p_gain>2</p_gain>
</plugin>
</model>
<!-- @online{GazeboFuel-OpenRobotics-Oak-tree,
title={Oak tree},
organization={Open Robotics},
date={2021},
month={January},
day={2},
author={OpenRobotics},
url={https://fuel.gazebosim.org/1.0/OpenRobotics/models/Oak tree},
} -->
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Oak tree</uri>
<name>oak_tree0</name>
<pose>10 10 0 0 0 0</pose>
</include>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Oak tree</uri>
<name>oak_tree1</name>
<pose>8 15 0 0 0 0</pose>
</include>
<!-- @online{GazeboFuel-OpenRobotics-Pine-Tree,
title={Pine Tree},
organization={Open Robotics},
date={2021},
month={January},
day={2},
author={OpenRobotics},
url={https://fuel.gazebosim.org/1.0/OpenRobotics/models/Pine Tree},
} -->
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Pine Tree</uri>
<name>pine_tree0</name>
<pose>15 -15 0 0 0 0</pose>
</include>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Pine Tree</uri>
<name>pine_tree1</name>
<pose>10 -20 0 0 0 0</pose>
</include>
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Pine Tree</uri>
<name>pine_tree2</name>
<pose>12 -10 0 0 0 0</pose>
</include>
<!-- @online{GazeboFuel-OpenRobotics-ground_station,
title={ground_station},
organization={Open Robotics},
date={2022},
month={December},
day={2},
author={OpenRobotics},
url={https://fuel.gazebosim.org/1.0/OpenRobotics/models/ground_station},
} -->
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/ground_station</uri>
<name>ground_station</name>
<pose>10 0 0 0 0 0</pose>
</include>
<!-- @online{GazeboFuel-OpenRobotics-Walking-person,
title={Walking person},
organization={Open Robotics},
date={2018},
month={January},
day={6},
author={OpenRobotics},
url={https://fuel.gazebosim.org/1.0/OpenRobotics/models/Walking person},
} -->
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Walking person</uri>
<name>walking_person0</name>
<pose>8 2 0 0 0 0</pose>
</include>
<!-- @online{GazeboFuel-plateau-Casual-female,
title={Casual female},
organization={Open Robotics},
date={2022},
month={April},
day={6},
author={plateau},
url={https://fuel.gazebosim.org/1.0/plateau/models/Casual female},
} -->
<include>
<uri>https://fuel.gazebosim.org/1.0/plateau/models/Casual female</uri>
<name>casual_female0</name>
<pose degrees="true">6 -4 0 0 0 145</pose>
</include>
<!-- @online{GazeboFuel-OpenRobotics-Prius-Hybrid,
title={Prius Hybrid},
organization={Open Robotics},
date={2020},
month={March},
day={6},
author={OpenRobotics},
url={https://fuel.gazebosim.org/1.0/OpenRobotics/models/Prius Hybrid},
} -->
<include>
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Prius Hybrid</uri>
<name>prius_hybrid0</name>
<pose degrees="true">0 7 0 0 0 40</pose>
</include>
</world>
</sdf>

124
guiTools/ardupilot_gazebo/worlds/iris_runway.sdf Executable file
View File

@ -0,0 +1,124 @@
<?xml version="1.0" ?>
<sdf version="1.9">
<world name="iris_runway">
<physics name="1ms" type="ignore">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-navsat-system"
name="gz::sim::systems::NavSat">
</plugin>
<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
<sky></sky>
</scene>
<spherical_coordinates>
<latitude_deg>-35.363262</latitude_deg>
<longitude_deg>149.165237</longitude_deg>
<elevation>584</elevation>
<heading_deg>0</heading_deg>
<surface_model>EARTH_WGS84</surface_model>
</spherical_coordinates>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="axes">
<static>1</static>
<link name="link">
<visual name="r">
<cast_shadows>0</cast_shadows>
<pose>5 0 0.1 0 0 0</pose>
<geometry>
<box>
<size>10 0.01 0.01</size>
</box>
</geometry>
<material>
<ambient>1 0 0 0.8</ambient>
<diffuse>1 0 0 0.8</diffuse>
<emissive>1 0 0 0.8</emissive>
<specular>0.5 0.5 0.5 0.8</specular>
</material>
</visual>
<visual name="g">
<cast_shadows>0</cast_shadows>
<pose>0 5 0.1 0 0 0</pose>
<geometry>
<box>
<size>0.01 10 0.01</size>
</box>
</geometry>
<material>
<ambient>0 1 0 0.8</ambient>
<diffuse>0 1 0 0.8</diffuse>
<emissive>0 1 0 0.8</emissive>
<specular>0.5 0.5 0.5 0.8</specular>
</material>
</visual>
<visual name="b">
<cast_shadows>0</cast_shadows>
<pose>0 0 5.1 0 0 0</pose>
<geometry>
<box>
<size>0.01 0.01 10</size>
</box>
</geometry>
<material>
<ambient>0 0 1 0.8</ambient>
<diffuse>0 0 1 0.8</diffuse>
<emissive>0 0 1 0.8</emissive>
<specular>0.5 0.5 0.5 0.8</specular>
</material>
</visual>
<sensor name="navsat_sensor" type="navsat">
<always_on>1</always_on>
<update_rate>1</update_rate>
</sensor>
</link>
</model>
<include>
<uri>model://runway</uri>
<pose degrees="true">-29 545 0 0 0 363</pose>
</include>
<!-- <include>
<uri>model://iris_with_gimbal</uri>
<pose degrees="true">0 0 0.195 0 0 90</pose>
</include> -->
</world>
</sdf>

Some files were not shown because too many files have changed in this diff Show More