dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

new Ignition Gazebo simulation interface architecture (#20057) 

- much simpler direct interface using Ignition Transport 
 - in tree models and worlds
 - control allocation output configuration, no more magic actuator mapping to mavlink and back
 - currently requires no custom Gazebo plugins (keeping things as lightweight and simple as possible)

Co-authored-by: Jaeyoung-Lim <jalim@ethz.ch>
This commit is contained in:
Daniel Agar 2022-08-22 10:58:19 -04:00 committed by GitHub
parent 889a2fddea
commit cfc579542e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
58 changed files with 29260 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -53,9 +53,6 @@
[submodule "src/lib/events/libevents"]
path = src/lib/events/libevents
url = https://github.com/mavlink/libevents.git
[submodule "Tools/simulation-ignition"]
path = Tools/simulation-ignition
url = https://github.com/PX4/px4-simulation-ignition.git
[submodule "src/lib/crypto/libtomcrypt"]
path = src/lib/crypto/libtomcrypt
url = https://github.com/PX4/libtomcrypt.git

5
.vscode/cmake-variants.yaml vendored
View File

@ -6,6 +6,11 @@ CONFIG:
buildType: RelWithDebInfo
settings:
CONFIG: px4_sitl_default
px4_sitl_ign:
short: px4_sitl_ign
buildType: RelWithDebInfo
settings:
CONFIG: px4_sitl_ign
px4_sitl_rtps:
short: px4_sitl_rtps
buildType: RelWithDebInfo

169
.vscode/tasks.json vendored
View File

@ -118,124 +118,28 @@
},
"problemMatcher": []
},
{
"label": "gazebo build",
"type": "shell",
"command": "make px4_sitl_default sitl_gazebo",
"options": {
"cwd": "${workspaceFolder}"
},
"problemMatcher": [],
"presentation":{
"echo": true,
"reveal": "always",
"focus": false,
"panel": "shared",
"showReuseMessage": false,
"clear": false,
}
},
{
"label": "gazebo start",
"type": "shell",
"dependsOn": "gazebo build",
"options": {
"cwd": "${workspaceFolder}",
"env": {
"GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
"GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/sitl_gazebo/models",
"PX4_SIM_SPEED_FACTOR": "1"
}
},
"command": "gzserver --verbose ${workspaceFolder}/Tools/sitl_gazebo/worlds/${input:gazeboWorld}.world",
"isBackground": true,
"presentation": {
"echo": true,
"reveal": "never",
"focus": false,
"panel": "shared",
"showReuseMessage": false,
"clear": false
},
"problemMatcher": [
{
"pattern": [
{
"regexp": ".",
"file": 1,
"location": 2,
"message": 3
}
],
"background": {
"activeOnStart": true,
"beginsPattern": ".",
"endsPattern": ".",
}
}
]
},
{
"label": "gazebo",
"type": "shell",
"dependsOn": "gazebo start",
"options": {
"cwd": "${workspaceFolder}",
"env": {
"GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
"GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/sitl_gazebo/models",
"IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gazebo/models",
"PX4_SIM_SPEED_FACTOR": "1"
}
},
"command": "gz model --verbose --spawn-file=${workspaceFolder}/Tools/sitl_gazebo/models/${input:vehicleModel}/${input:vehicleModel}.sdf --model-name=${input:vehicleModel} -x 1.01 -y 0.98 -z 0.83",
"isBackground": false,
"presentation": {
"echo": true,
"reveal": "never",
"focus": false,
"panel": "shared",
"showReuseMessage": false,
"clear": false
},
"problemMatcher": [
{
"pattern": [
{
"regexp": ".",
"file": 1,
"location": 2,
"message": 3
}
],
"background": {
"activeOnStart": true,
"beginsPattern": ".",
"endsPattern": ".",
}
}
]
},
{
"label": "gazebo client",
"type": "shell",
"dependsOn": "gazebo build",
"options": {
"cwd": "${workspaceFolder}",
"env": {
"GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo",
"GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/sitl_gazebo/models",
"PX4_SIM_SPEED_FACTOR": "1"
}
},
"command": "gzclient --verbose",
"command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gazebo/worlds/${input:gazeboWorld}.sdf",
"isBackground": true,
"presentation": {
"echo": true,
"reveal": "never",
"revealProblems": "onProblem",
"focus": false,
"panel": "shared",
"panel": "dedicated",
"showReuseMessage": false,
"clear": false
"clear": false,
"close": true
},
"problemMatcher": [
{
@ -255,32 +159,38 @@
}
]
},
{
"label": "gazebo kill",
"type": "shell",
"command": "killall gzserver",
"command": "pkill -9 -f ign || true",
"presentation": {
"echo": true,
"reveal": "silent",
"reveal": "never",
"revealProblems": "onProblem",
"focus": false,
"panel": "shared",
"panel": "dedicated",
"showReuseMessage": false,
"clear": false
"clear": false,
"close": true
},
"problemMatcher": [],
"dependsOn":["px4_sitl_cleanup"]
},
{
"label": "px4_sitl_cleanup",
"type": "shell",
"command": "rm -rfv /tmp/px4*",
"command": "rm -rfv /tmp/px4* || true",
"presentation": {
"echo": true,
"reveal": "silent",
"reveal": "never",
"revealProblems": "onProblem",
"focus": false,
"panel": "shared",
"panel": "dedicated",
"showReuseMessage": false,
"clear": false
"clear": false,
"close": true
},
"problemMatcher": [],
"dependsOn":["px4_kill"]
@ -288,14 +198,16 @@
{
"label": "px4_kill",
"type": "shell",
"command": "killall px4 || true",
"command": "pkill -9 px4 || true",
"presentation": {
"echo": true,
"reveal": "silent",
"reveal": "never",
"revealProblems": "onProblem",
"focus": false,
"panel": "shared",
"panel": "dedicated",
"showReuseMessage": false,
"clear": false
"clear": false,
"close": true
},
"problemMatcher": []
},
@ -335,37 +247,12 @@
}
],
"inputs": [
{
"type": "pickString",
"id": "vehicleModel",
"description": "gazebo model",
"options": [
"iris",
"typhoon_h480",
"plane",
"plane_catapult",
"plane_lidar",
"standard_vtol",
"tailsitter",
"tiltrotor",
"r1_rover",
"boat"
],
"default": "iris"
},
{
"type": "pickString",
"id": "gazeboWorld",
"description": "gazebo world",
"options": [
"baylands",
"empty",
"ksql_airport",
"mcmillan_airfield",
"sonoma_raceway",
"warehouse",
"windy",
"yosemite"
"empty"
],
"default": "empty"
}

46
ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3 Normal file
View File

@ -0,0 +1,46 @@
#!/bin/sh
#
# @name Ignition Gazebo X3
#
# @type Quadrotor
#
. ${R}etc/init.d/rc.mc_defaults
param set-default SYS_CTRL_ALLOC 1
param set-default CA_AIRFRAME 0
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 0.13
param set-default CA_ROTOR0_PY 0.22
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR1_PX -0.13
param set-default CA_ROTOR1_PY -0.20
param set-default CA_ROTOR1_KM 0.05
param set-default CA_ROTOR2_PX 0.13
param set-default CA_ROTOR2_PY -0.22
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.13
param set-default CA_ROTOR3_PY 0.20
param set-default CA_ROTOR3_KM -0.05
param set-default SIM_GZ_FUNC1 101
param set-default SIM_GZ_FUNC2 102
param set-default SIM_GZ_FUNC3 103
param set-default SIM_GZ_FUNC4 104
param set-default SIM_GZ_MIN1 150
param set-default SIM_GZ_MIN2 150
param set-default SIM_GZ_MIN3 150
param set-default SIM_GZ_MIN4 150
param set-default SIM_GZ_MAX1 1000
param set-default SIM_GZ_MAX2 1000
param set-default SIM_GZ_MAX3 1000
param set-default SIM_GZ_MAX4 1000
param set-default MPC_THR_HOVER 0.60

76
ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4 Normal file
View File

@ -0,0 +1,76 @@
#!/bin/sh
#
# @name Ignition Gazebo X4
#
# @type Quadrotor
#
. ${R}etc/init.d/rc.mc_defaults
param set-default SYS_CTRL_ALLOC 1
param set-default CA_AIRFRAME 0
param set-default CA_ROTOR_COUNT 6
param set-default CA_ROTOR0_PX 0.25
param set-default CA_ROTOR0_PY -0.15
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR1_PX 0.00
param set-default CA_ROTOR1_PY -0.29
param set-default CA_ROTOR1_KM -0.05
param set-default CA_ROTOR2_PX -0.25
param set-default CA_ROTOR2_PY -0.15
param set-default CA_ROTOR2_KM 0.05
param set-default CA_ROTOR3_PX -0.25
param set-default CA_ROTOR3_PY 0.15
param set-default CA_ROTOR3_KM -0.05
param set-default CA_ROTOR4_PX 0.00
param set-default CA_ROTOR4_PY 0.29
param set-default CA_ROTOR4_KM 0.05
param set-default CA_ROTOR5_PX 0.25
param set-default CA_ROTOR5_PY 0.15
param set-default CA_ROTOR5_KM -0.05
param set-default SIM_GZ_FUNC1 101
param set-default SIM_GZ_FUNC2 102
param set-default SIM_GZ_FUNC3 103
param set-default SIM_GZ_FUNC4 104
param set-default SIM_GZ_FUNC5 105
param set-default SIM_GZ_FUNC6 106
param set-default SIM_GZ_MIN1 150
param set-default SIM_GZ_MIN2 150
param set-default SIM_GZ_MIN3 150
param set-default SIM_GZ_MIN4 150
param set-default SIM_GZ_MIN5 150
param set-default SIM_GZ_MIN6 150
param set-default SIM_GZ_MAX1 1000
param set-default SIM_GZ_MAX2 1000
param set-default SIM_GZ_MAX3 1000
param set-default SIM_GZ_MAX4 1000
param set-default SIM_GZ_MAX5 1000
param set-default SIM_GZ_MAX6 1000
param set-default MC_PITCHRATE_D 0.0016
param set-default MC_PITCHRATE_I 0.2500
param set-default MC_PITCHRATE_P 0.1831
param set-default MC_PITCH_P 5.216
param set-default MC_ROLLRATE_D 0.0022
param set-default MC_ROLLRATE_I 0.2095
param set-default MC_ROLLRATE_P 0.1570
param set-default MC_ROLL_P 6.081
param set-default MC_YAWRATE_D 0.0009
param set-default MC_YAWRATE_I 0.1800
param set-default MC_YAWRATE_P 0.1773
param set-default MC_YAW_P 5.386490
param set-default MPC_THR_HOVER 0.61

5
ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
View File

@ -72,9 +72,14 @@ px4_add_romfs_files(
1070_boat
3010_quadrotor_x
3011_hexarotor_x
4001_x3
17001_tf-g1
17002_tf-g2
2507_cloudship
6001_x4
6011_typhoon_h480
6011_typhoon_h480.post
)

51
ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
View File

@ -1,20 +1,43 @@
#!/bin/sh
# shellcheck disable=SC2154
simulator_tcp_port=$((4560+px4_instance))
PX4_SIM_WORLD="${PX4_SIM_WORLD:="empty"}" # default to empty world?
echo "PX4_SIM_MODEL: ${PX4_SIM_MODEL}"
echo "PX4_SIM_WORLD: ${PX4_SIM_WORLD}"
# TODO: verify if world exists
ign service --info --service /world/${PX4_SIM_WORLD}/create
echo $?
ign service -s /world/${PX4_SIM_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_SIM_MODEL}/model.sdf\""
if ignition_simulator start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"
then
sensor_baro_sim start
sensor_gps_sim start
sensor_mag_sim start
# Check if PX4_SIM_HOSTNAME environment variable is empty
# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
# If both are empty use localhost for simulator
if [ -z "${PX4_SIM_HOSTNAME}" ]; then
if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
echo "PX4 SIM HOST: localhost"
simulator start -c $simulator_tcp_port
else
echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port
fi
else
echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME"
simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port
simulator_tcp_port=$((4560+px4_instance))
# Check if PX4_SIM_HOSTNAME environment variable is empty
# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
# If both are empty use localhost for simulator
if [ -z "${PX4_SIM_HOSTNAME}" ]; then
if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
echo "PX4 SIM HOST: localhost"
simulator start -c $simulator_tcp_port
else
echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port
fi
else
echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME"
simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port
fi
fi

1
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -212,6 +212,7 @@ elif ! replay tryapplyparams
then
. px4-rc.simulator
fi
load_mon start
battery_simulator start
tone_alarm start

23
Tools/setup_ignition.bash
View File

@ -1,23 +0,0 @@
#!/bin/bash
#
# Setup environment to make PX4 visible to Gazebo.
#
# Note, this is not necessary if using a ROS catkin workspace with the px4
# package as the paths are exported.
#
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
if [ "$#" != 2 ]; then
echo -e "usage: source setup_gazebo.bash src_dir build_dir\n"
return 1
fi
SRC_DIR=$1
BUILD_DIR=$2
# setup Gazebo env and update package path
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo
export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$IGN_GAZEBO_SYSTEM_PLUGIN_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:${SRC_DIR}/Tools/simulation-ignition/models
echo -e "LD_LIBRARY_PATH $LD_LIBRARY_PATH"

1
Tools/simulation-ignition

@ -1 +0,0 @@
Subproject commit 483193d9b8b89211c3b970c735b4fbb5f724b63a

63
Tools/simulation/gazebo/models/x3/meshes/propeller_ccw.dae Normal file
View File

File diff suppressed because one or more lines are too long

63
Tools/simulation/gazebo/models/x3/meshes/propeller_cw.dae Normal file
View File

File diff suppressed because one or more lines are too long

10172
Tools/simulation/gazebo/models/x3/meshes/x3.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
Tools/simulation/gazebo/models/x3/meshes/x3.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 206 KiB

20
Tools/simulation/gazebo/models/x3/model.config Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0"?>
<model>
<name>x3</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name>Carlos Aguero</name>
<email>caguero@openrobotics.org</email>
</author>
<author>
<name>Cole Biesemeyer</name>
<email>cole@openrobotics.org</email>
</author>
<description>
</description>
</model>

371
Tools/simulation/gazebo/models/x3/model.sdf Normal file
View File

@ -0,0 +1,371 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.6">
<model name="x3">
<pose>0 0 0.053302 0 0 0</pose>
<link name="base_link">
<pose frame="">0 0 0 0 -0 0</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>1.5</mass>
<inertia>
<ixx>0.0347563</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.07</iyy>
<iyz>0</iyz>
<izz>0.0977</izz>
</inertia>
</inertial>
<collision name="base_link_inertia_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.47 0.47 0.11</size>
</box>
</geometry>
</collision>
<visual name="base_link_inertia_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/x3.dae</uri>
</mesh>
</geometry>
</visual>
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>250</update_rate>
</sensor>
</link>
<link name="rotor_0">
<pose frame="">0.13 -0.22 0.023 0 -0 0</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>4.17041e-05</iyy>
<iyz>0</iyz>
<izz>4.26041e-05</izz>
</inertia>
</inertial>
<collision name="rotor_0_collision">
<pose frame="">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 frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>meshes/propeller_ccw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay />
</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>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_1">
<pose frame="">-0.13 0.2 0.023 0 -0 0</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>4.17041e-05</iyy>
<iyz>0</iyz>
<izz>4.26041e-05</izz>
</inertia>
</inertial>
<collision name="rotor_1_collision">
<pose frame="">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 frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>meshes/propeller_ccw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Red</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay />
</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>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_2">
<pose frame="">0.13 0.22 0.023 0 -0 0</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>4.17041e-05</iyy>
<iyz>0</iyz>
<izz>4.26041e-05</izz>
</inertia>
</inertial>
<collision name="rotor_2_collision">
<pose frame="">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 frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>meshes/propeller_cw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay />
</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>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_3">
<pose frame="">-0.13 -0.2 0.023 0 -0 0</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>4.17041e-05</iyy>
<iyz>0</iyz>
<izz>4.26041e-05</izz>
</inertia>
</inertial>
<collision name="rotor_3_collision">
<pose frame="">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 frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>meshes/propeller_cw.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Red</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay />
</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>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<plugin filename="libignition-gazebo-imu-system.so" name="ignition::gazebo::systems::Imu" />
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x3</robotNamespace>
<jointName>rotor_0_joint</jointName>
<linkName>rotor_0</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>0</motorNumber>
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x3</robotNamespace>
<jointName>rotor_1_joint</jointName>
<linkName>rotor_1</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>1</motorNumber>
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x3</robotNamespace>
<jointName>rotor_2_joint</jointName>
<linkName>rotor_2</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>2</motorNumber>
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x3</robotNamespace>
<jointName>rotor_3_joint</jointName>
<linkName>rotor_3</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>8.54858e-06</motorConstant>
<momentConstant>0.016</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>3</motorNumber>
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
</model>
</sdf>

BIN
Tools/simulation/gazebo/models/x4/materials/textures/led.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

BIN
Tools/simulation/gazebo/models/x4/materials/textures/spotlight.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 182 KiB

529
Tools/simulation/gazebo/models/x4/meshes/led.dae Normal file
View File

@ -0,0 +1,529 @@
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset><contributor><author></author><authoring_tool>FBX COLLADA exporter</authoring_tool><comments></comments></contributor><created>2018-08-21T21:36:07Z</created><keywords></keywords><modified>2018-08-21T21:36:07Z</modified><revision></revision><subject></subject><title></title><unit meter="1.000000" name="centimeter"></unit><up_axis>Z_UP</up_axis></asset>
<library_images>
<image id="LEDIndicator_ncl1_2-image" name="LEDIndicator_ncl1_2"><init_from>../materials/textures/led.jpg</init_from></image>
</library_images>
<library_materials>
<material id="LEDIndicator_ncl1_1" name="LEDIndicator_ncl1_1">
<instance_effect url="#LEDIndicator_ncl1_1-fx"/>
</material>
</library_materials>
<library_effects>
<effect id="LEDIndicator_ncl1_1-fx" name="LEDIndicator_ncl1_1">
<profile_COMMON>
<technique sid="standard">
<phong>
<emission>
<color sid="emission">0.000000 0.000000 0.000000 1.000000</color>
</emission>
<ambient>
<color sid="ambient">0.588235 0.588235 0.588235 1.000000</color>
</ambient>
<diffuse>
<texture texture="LEDIndicator_ncl1_2-image" texcoord="CHANNEL0">
<extra>
<technique profile="MAYA">
<wrapU sid="wrapU0">TRUE</wrapU>
<wrapV sid="wrapV0">TRUE</wrapV>
<blend_mode>ADD</blend_mode>
</technique>
</extra>
</texture>
</diffuse>
<specular>
<color sid="specular">0.000000 0.000000 0.000000 1.000000</color>
</specular>
<shininess>
<float sid="shininess">2.000000</float>
</shininess>
<reflective>
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
</reflective>
<reflectivity>
<float sid="reflectivity">1.000000</float>
</reflectivity>
<transparent opaque="RGB_ZERO">
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
</transparent>
<transparency>
<float sid="transparency">0.000000</float>
</transparency>
</phong>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_geometries>
<geometry id="LEDIndicator-lib" name="LEDIndicatorMesh">
<mesh>
<source id="LEDIndicator-POSITION">
<float_array id="LEDIndicator-POSITION-array" count="147">
-0.068518 0.000000 0.202589
-0.059339 -0.034466 0.202589
-0.118677 0.000000 0.152431
-0.102777 -0.059696 0.152431
-0.137036 0.000000 0.083912
-0.118677 -0.068932 0.083912
-0.179977 -0.104537 0.036913
-0.179977 0.104537 0.036913
-0.179977 -0.104537 0.000445
-0.179977 0.104537 0.000445
0.000000 0.068932 0.202589
-0.034259 0.059697 0.202589
-0.059338 0.034466 0.202589
0.000000 0.119393 0.152431
-0.059338 0.103397 0.152431
-0.102777 0.059697 0.152431
0.000000 0.137863 0.083912
-0.068518 0.119393 0.083912
-0.118677 0.068932 0.083912
-0.034259 -0.059697 0.202589
-0.059339 -0.103397 0.152431
-0.000000 -0.119393 0.152431
-0.068518 -0.119393 0.083912
-0.000000 -0.068932 0.202589
0.034259 -0.059697 0.202589
0.059338 -0.034466 0.202589
0.059338 -0.103397 0.152431
-0.000000 -0.137863 0.083912
0.068518 -0.119393 0.083912
-0.000000 -0.209074 0.036913
-0.000000 -0.209074 0.000445
0.068518 -0.000000 0.202589
0.102777 -0.059697 0.152431
0.118677 -0.000000 0.152431
0.118677 -0.068932 0.083912
0.137036 -0.000000 0.083912
0.179977 -0.104537 0.036913
0.179977 -0.104537 0.000445
0.000000 0.000000 0.220949
0.059339 0.034466 0.202589
0.034259 0.059697 0.202589
0.102777 0.059696 0.152431
0.059339 0.103397 0.152431
0.118677 0.068932 0.083912
0.068518 0.119393 0.083912
0.000000 0.209074 0.036913
0.179977 0.104537 0.036913
0.000000 0.209074 0.000445
0.179977 0.104537 0.000445
</float_array>
<technique_common>
<accessor source="#LEDIndicator-POSITION-array" count="49" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="LEDIndicator-Normal0">
<float_array id="LEDIndicator-Normal0-array" count="846">
0.000000 -0.000000 1.000000
-0.459980 0.263947 0.847791
-0.530731 0.000000 0.847540
0.000000 -0.000000 1.000000
-0.530731 0.000000 0.847540
-0.459980 -0.263946 0.847791
-0.875931 0.000000 0.482436
-0.530731 0.000000 0.847540
-0.459980 0.263947 0.847791
-0.459980 0.263947 0.847791
-0.759342 0.436180 0.482853
-0.875931 0.000000 0.482436
-0.759343 -0.436179 0.482853
-0.459980 -0.263946 0.847791
-0.530731 0.000000 0.847540
-0.530731 0.000000 0.847540
-0.875931 0.000000 0.482436
-0.759343 -0.436179 0.482853
-0.836841 0.000000 0.547447
-0.875931 0.000000 0.482436
-0.759342 0.436180 0.482853
-0.759342 0.436180 0.482853
-0.684937 0.393187 0.613404
-0.836841 0.000000 0.547447
-0.684938 -0.393186 0.613404
-0.759343 -0.436179 0.482853
-0.875931 0.000000 0.482436
-0.875931 0.000000 0.482436
-0.836841 0.000000 0.547447
-0.684938 -0.393186 0.613404
-0.836841 0.000000 0.547447
-0.684937 0.393187 0.613404
-0.797292 0.459082 0.391880
-0.797292 -0.459081 0.391880
-0.797292 0.459082 0.391880
-0.866677 0.498870 0.000000
-0.866677 0.498870 0.000000
-0.866677 -0.498869 0.000000
-0.797292 -0.459081 0.391880
-0.797292 -0.459081 0.391880
-0.684938 -0.393186 0.613404
-0.836841 0.000000 0.547447
-0.836841 0.000000 0.547447
-0.797292 0.459082 0.391880
-0.797292 -0.459081 0.391880
0.000000 -0.000000 1.000000
0.000000 0.529118 0.848548
-0.265980 0.457875 0.848296
0.000000 -0.000000 1.000000
-0.265980 0.457875 0.848296
-0.459980 0.263947 0.847791
-0.439292 0.757014 0.483687
-0.265980 0.457875 0.848296
0.000000 0.529118 0.848548
0.000000 0.529118 0.848548
0.000001 0.875010 0.484104
-0.439292 0.757014 0.483687
-0.759342 0.436180 0.482853
-0.459980 0.263947 0.847791
-0.265980 0.457875 0.848296
-0.265980 0.457875 0.848296
-0.439292 0.757014 0.483687
-0.759342 0.436180 0.482853
-0.419462 0.722641 0.549401
-0.439292 0.757014 0.483687
0.000001 0.875010 0.484104
0.000001 0.875010 0.484104
0.000001 0.788497 0.615039
-0.419462 0.722641 0.549401
-0.684937 0.393187 0.613404
-0.759342 0.436180 0.482853
-0.439292 0.757014 0.483687
-0.439292 0.757014 0.483687
-0.419462 0.722641 0.549401
-0.684937 0.393187 0.613404
-0.419462 0.722641 0.549401
0.000001 0.788497 0.615039
0.000001 0.919830 0.392316
-0.797292 0.459082 0.391880
0.000001 0.919830 0.392316
0.000001 1.000000 0.000000
0.000001 1.000000 0.000000
-0.866677 0.498870 0.000000
-0.797292 0.459082 0.391880
-0.797292 0.459082 0.391880
-0.684937 0.393187 0.613404
-0.419462 0.722641 0.549401
-0.419462 0.722641 0.549401
0.000001 0.919830 0.392316
-0.797292 0.459082 0.391880
0.000000 -0.000000 1.000000
-0.459980 -0.263946 0.847791
-0.265980 -0.457875 0.848296
0.000000 -0.000000 1.000000
-0.265980 -0.457875 0.848296
-0.000000 -0.529118 0.848548
-0.439293 -0.757013 0.483687
-0.265980 -0.457875 0.848296
-0.459980 -0.263946 0.847791
-0.459980 -0.263946 0.847791
-0.759343 -0.436179 0.482853
-0.439293 -0.757013 0.483687
-0.000000 -0.875010 0.484104
-0.000000 -0.529118 0.848548
-0.265980 -0.457875 0.848296
-0.265980 -0.457875 0.848296
-0.439293 -0.757013 0.483687
-0.000000 -0.875010 0.484104
-0.419463 -0.722640 0.549401
-0.439293 -0.757013 0.483687
-0.759343 -0.436179 0.482853
-0.759343 -0.436179 0.482853
-0.684938 -0.393186 0.613404
-0.419463 -0.722640 0.549401
-0.000000 -0.788497 0.615039
-0.000000 -0.875010 0.484104
-0.439293 -0.757013 0.483687
-0.439293 -0.757013 0.483687
-0.419463 -0.722640 0.549401
-0.000000 -0.788497 0.615039
-0.419463 -0.722640 0.549401
-0.684938 -0.393186 0.613404
-0.797292 -0.459081 0.391880
-0.000001 -0.919830 0.392316
-0.797292 -0.459081 0.391880
-0.866677 -0.498869 0.000000
-0.866677 -0.498869 0.000000
-0.000001 -1.000000 0.000000
-0.000001 -0.919830 0.392316
-0.000001 -0.919830 0.392316
-0.000000 -0.788497 0.615039
-0.419463 -0.722640 0.549401
-0.419463 -0.722640 0.549401
-0.797292 -0.459081 0.391880
-0.000001 -0.919830 0.392316
0.000000 -0.000000 1.000000
-0.000000 -0.529118 0.848548
0.265980 -0.457875 0.848296
0.000000 -0.000000 1.000000
0.265980 -0.457875 0.848296
0.459980 -0.263947 0.847791
0.439292 -0.757014 0.483687
0.265980 -0.457875 0.848296
-0.000000 -0.529118 0.848548
-0.000000 -0.529118 0.848548
-0.000000 -0.875010 0.484104
0.439292 -0.757014 0.483687
0.759342 -0.436180 0.482853
0.459980 -0.263947 0.847791
0.265980 -0.457875 0.848296
0.265980 -0.457875 0.848296
0.439292 -0.757014 0.483687
0.759342 -0.436180 0.482853
0.419462 -0.722641 0.549401
0.439292 -0.757014 0.483687
-0.000000 -0.875010 0.484104
-0.000000 -0.875010 0.484104
-0.000000 -0.788497 0.615039
0.419462 -0.722641 0.549401
0.684937 -0.393187 0.613404
0.759342 -0.436180 0.482853
0.439292 -0.757014 0.483687
0.439292 -0.757014 0.483687
0.419462 -0.722641 0.549401
0.684937 -0.393187 0.613404
0.419462 -0.722641 0.549401
-0.000000 -0.788497 0.615039
-0.000001 -0.919830 0.392316
0.797292 -0.459082 0.391880
-0.000001 -0.919830 0.392316
-0.000001 -1.000000 0.000000
-0.000001 -1.000000 0.000000
0.866677 -0.498871 0.000000
0.797292 -0.459082 0.391880
0.797292 -0.459082 0.391880
0.684937 -0.393187 0.613404
0.419462 -0.722641 0.549401
0.419462 -0.722641 0.549401
-0.000001 -0.919830 0.392316
0.797292 -0.459082 0.391880
0.000000 -0.000000 1.000000
0.459980 -0.263947 0.847791
0.530731 -0.000000 0.847540
0.000000 -0.000000 1.000000
0.530731 -0.000000 0.847540
0.459981 0.263946 0.847791
0.875931 -0.000001 0.482436
0.530731 -0.000000 0.847540
0.459980 -0.263947 0.847791
0.459980 -0.263947 0.847791
0.759342 -0.436180 0.482853
0.875931 -0.000001 0.482436
0.759343 0.436179 0.482853
0.459981 0.263946 0.847791
0.530731 -0.000000 0.847540
0.530731 -0.000000 0.847540
0.875931 -0.000001 0.482436
0.759343 0.436179 0.482853
0.836841 -0.000001 0.547447
0.875931 -0.000001 0.482436
0.759342 -0.436180 0.482853
0.759342 -0.436180 0.482853
0.684937 -0.393187 0.613404
0.836841 -0.000001 0.547447
0.684938 0.393186 0.613404
0.759343 0.436179 0.482853
0.875931 -0.000001 0.482436
0.875931 -0.000001 0.482436
0.836841 -0.000001 0.547447
0.684938 0.393186 0.613404
0.836841 -0.000001 0.547447
0.684937 -0.393187 0.613404
0.797292 -0.459082 0.391880
0.797292 0.459081 0.391880
0.797292 -0.459082 0.391880
0.866677 -0.498871 0.000000
0.866677 -0.498871 0.000000
0.866677 0.498869 -0.000000
0.797292 0.459081 0.391880
0.797292 0.459081 0.391880
0.684938 0.393186 0.613404
0.836841 -0.000001 0.547447
0.836841 -0.000001 0.547447
0.797292 -0.459082 0.391880
0.797292 0.459081 0.391880
0.000000 -0.000000 1.000000
0.459981 0.263946 0.847791
0.265980 0.457875 0.848296
0.000000 -0.000000 1.000000
0.265980 0.457875 0.848296
0.000000 0.529118 0.848548
0.439293 0.757013 0.483687
0.265980 0.457875 0.848296
0.459981 0.263946 0.847791
0.459981 0.263946 0.847791
0.759343 0.436179 0.482853
0.439293 0.757013 0.483687
0.000001 0.875010 0.484104
0.000000 0.529118 0.848548
0.265980 0.457875 0.848296
0.265980 0.457875 0.848296
0.439293 0.757013 0.483687
0.000001 0.875010 0.484104
0.419463 0.722640 0.549401
0.439293 0.757013 0.483687
0.759343 0.436179 0.482853
0.759343 0.436179 0.482853
0.684938 0.393186 0.613404
0.419463 0.722640 0.549401
0.000001 0.788497 0.615039
0.000001 0.875010 0.484104
0.439293 0.757013 0.483687
0.439293 0.757013 0.483687
0.419463 0.722640 0.549401
0.000001 0.788497 0.615039
0.419463 0.722640 0.549401
0.684938 0.393186 0.613404
0.797292 0.459081 0.391880
0.000001 0.919830 0.392316
0.797292 0.459081 0.391880
0.866677 0.498869 -0.000000
0.866677 0.498869 -0.000000
0.000001 1.000000 0.000000
0.000001 0.919830 0.392316
0.000001 0.919830 0.392316
0.000001 0.788497 0.615039
0.419463 0.722640 0.549401
0.419463 0.722640 0.549401
0.797292 0.459081 0.391880
0.000001 0.919830 0.392316
0.000000 0.000000 -1.000000
0.000000 0.000000 -1.000000
0.000000 -0.000000 -1.000000
0.000000 -0.000000 -1.000000
0.000000 -0.000000 -1.000000
0.000000 -0.000000 -1.000000
0.000000 0.000000 -1.000000
0.000000 -0.000000 -1.000000
0.000000 -0.000000 -1.000000
0.000000 -0.000000 -1.000000
0.000000 0.000000 -1.000000
0.000000 -0.000000 -1.000000
</float_array>
<technique_common>
<accessor source="#LEDIndicator-Normal0-array" count="282" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="LEDIndicator-UV0">
<float_array id="LEDIndicator-UV0-array" count="180">
0.331040 0.999938
0.220938 0.799324
0.331041 0.775815
0.441144 0.799325
0.141050 0.582960
0.331041 0.554785
0.521033 0.582960
0.102792 0.361034
0.331041 0.328646
0.559290 0.361035
0.001390 0.116209
0.660694 0.116211
0.000033 0.000039
0.662052 0.000041
0.331040 0.999938
0.220938 0.799324
0.331041 0.775815
0.441144 0.799325
0.141050 0.582960
0.331041 0.554785
0.521033 0.582960
0.102792 0.361034
0.331041 0.328646
0.559290 0.361035
0.001390 0.116209
0.660694 0.116211
0.000033 0.000039
0.662052 0.000041
0.331040 0.999938
0.220938 0.799324
0.331041 0.775815
0.441144 0.799325
0.141050 0.582960
0.331041 0.554785
0.521033 0.582960
0.102792 0.361034
0.331041 0.328646
0.559290 0.361035
0.001390 0.116209
0.660694 0.116211
0.000033 0.000039
0.662052 0.000041
0.331040 0.999938
0.220938 0.799324
0.331041 0.775815
0.441144 0.799325
0.141050 0.582960
0.331041 0.554785
0.521033 0.582960
0.102792 0.361034
0.331041 0.328646
0.559290 0.361035
0.001390 0.116209
0.660694 0.116211
0.000033 0.000039
0.662052 0.000041
0.331040 0.999938
0.220938 0.799324
0.331041 0.775815
0.441144 0.799325
0.141050 0.582960
0.331041 0.554785
0.521033 0.582960
0.102792 0.361034
0.331041 0.328646
0.559290 0.361035
0.001390 0.116209
0.660694 0.116211
0.000033 0.000039
0.662052 0.000041
0.331040 0.999938
0.220938 0.799324
0.331041 0.775815
0.441144 0.799325
0.141050 0.582960
0.331041 0.554785
0.521033 0.582960
0.102792 0.361034
0.331041 0.328646
0.559290 0.361035
0.001390 0.116209
0.660694 0.116211
0.000033 0.000039
0.662052 0.000041
0.283335 0.017853
0.278499 0.011567
0.283335 0.005282
0.293006 0.005282
0.297841 0.011567
0.293006 0.017853
</float_array>
<technique_common>
<accessor source="#LEDIndicator-UV0-array" count="90" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="LEDIndicator-VERTEX">
<input semantic="POSITION" source="#LEDIndicator-POSITION"/>
</vertices>
<triangles count="94" material="LEDIndicator_ncl1_1"><input semantic="VERTEX" offset="0" source="#LEDIndicator-VERTEX"/><input semantic="NORMAL" offset="1" source="#LEDIndicator-Normal0"/><input semantic="TEXCOORD" offset="2" set="0" source="#LEDIndicator-UV0"/><p> 38 0 0 12 1 1 0 2 2 38 3 0 0 4 2 1 5 3 2 6 5 0 7 2 12 8 1 12 9 1 15 10 4 2 11 5 3 12 6 1 13 3 0 14 2 0 15 2 2 16 5 3 17 6 4 18 8 2 19 5 15 20 4 15 21 4 18 22 7 4 23 8 5 24 9 3 25 6 2 26 5 2 27 5 4 28 8 5 29 9 4 30 8 18 31 7 7 32 10 6 33 11 7 34 10 9 35 12 9 36 12 8 37 13 6 38 11 6 39 11 5 40 9 4 41 8 4 42 8 7 43 10 6 44 11 38 45 14 10 46 15 11 47 16 38 48 14 11 49 16 12 50 17 14 51 19 11 52 16 10 53 15 10 54 15 13 55 18 14 56 19 15 57 20 12 58 17 11 59 16 11 60 16 14 61 19 15 62 20 17 63 22 14 64 19 13 65 18 13 66 18 16 67 21 17 68 22 18 69 23 15 70 20 14 71 19 14 72 19 17 73 22 18 74 23 17 75 22 16 76 21 45 77 24 7 78 25 45 79 24 47 80 26 47 81 26 9 82 27 7 83 25 7 84 25 18 85 23 17 86 22 17 87 22 45 88 24 7 89 25 38 90 28 1 91 29 19 92 30 38 93 28 19 94 30 23 95 31 20 96 33 19 97 30 1 98 29 1 99 29 3 100 32 20 101 33 21 102 34 23 103 31 19 104 30 19 105 30 20 106 33 21 107 34 22 108 36 20 109 33 3 110 32 3 111 32 5 112 35 22 113 36 27 114 37 21 115 34 20 116 33 20 117 33 22 118 36 27 119 37 22 120 36 5 121 35 6 122 38 29 123 39 6 124 38 8 125 40 8 126 40 30 127 41 29 128 39 29 129 39 27 130 37 22 131 36 22 132 36 6 133 38 29 134 39 38 135 42 23 136 43 24 137 44 38 138 42 24 139 44 25 140 45 26 141 47 24 142 44 23 143 43 23 144 43 21 145 46 26 146 47 32 147 48 25 148 45 24 149 44 24 150 44 26 151 47 32 152 48 28 153 50 26 154 47 21 155 46 21 156 46 27 157 49 28 158 50 34 159 51 32 160 48 26 161 47 26 162 47 28 163 50 34 164 51 28 165 50 27 166 49 29 167 52 36 168 53 29 169 52 30 170 54 30 171 54 37 172 55 36 173 53 36 174 53 34 175 51 28 176 50 28 177 50 29 178 52 36 179 53 38 180 56 25 181 57 31 182 58 38 183 56 31 184 58 39 185 59 33 186 61 31 187 58 25 188 57 25 189 57 32 190 60 33 191 61 41 192 62 39 193 59 31 194 58 31 195 58 33 196 61 41 197 62 35 198 64 33 199 61 32 200 60 32 201 60 34 202 63 35 203 64 43 204 65 41 205 62 33 206 61 33 207 61 35 208 64 43 209 65 35 210 64 34 211 63 36 212 66 46 213 67 36 214 66 37 215 68 37 216 68 48 217 69 46 218 67 46 219 67 43 220 65 35 221 64 35 222 64 36 223 66 46 224 67 38 225 70 39 226 71 40 227 72 38 228 70 40 229 72 10 230 73 42 231 75 40 232 72 39 233 71 39 234 71 41 235 74 42 236 75 13 237 76 10 238 73 40 239 72 40 240 72 42 241 75 13 242 76 44 243 78 42 244 75 41 245 74 41 246 74 43 247 77 44 248 78 16 249 79 13 250 76 42 251 75 42 252 75 44 253 78 16 254 79 44 255 78 43 256 77 46 257 80 45 258 81 46 259 80 48 260 82 48 261 82 47 262 83 45 263 81 45 264 81 16 265 79 44 266 78 44 267 78 46 268 80 45 269 81 47 270 85 48 271 86 37 272 87 37 273 87 30 274 88 8 275 89 47 276 85 37 277 87 8 278 89 9 279 84 47 280 85 8 281 89</p></triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="LEDIndicator" name="LEDIndicator">
<node name="HuskyReplacement" id="HuskyReplacement" sid="HuskyReplacement"><matrix sid="matrix">0.100000 0.000000 0.000000 0.000000 0.000000 0.099400 0.000000 0.000000 0.000000 0.000000 0.100000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix><extra><technique profile="FCOLLADA"><visibility>1.000000</visibility></technique></extra><node name="LEDIndicator" id="LEDIndicator" sid="LEDIndicator"><matrix sid="matrix">1.000000 -0.000000 -0.000000 0.000000 0.000000 1.000000 -0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix><instance_geometry url="#LEDIndicator-lib"><bind_material><technique_common><instance_material symbol="LEDIndicator_ncl1_1" target="#LEDIndicator_ncl1_1"/></technique_common></bind_material></instance_geometry><extra><technique profile="FCOLLADA"><visibility>1.000000</visibility></technique></extra></node></node>
<extra><technique profile="MAX3D"><frame_rate>30.000000</frame_rate></technique><technique profile="FCOLLADA"><start_time>0.000000</start_time><end_time>3.333333</end_time></technique></extra>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#LEDIndicator"></instance_visual_scene>
</scene>
</COLLADA>

66
Tools/simulation/gazebo/models/x4/meshes/led_lamp.dae Normal file
View File

@ -0,0 +1,66 @@
<?xml version="1.0" encoding="UTF-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<author>VCGLab</author>
<authoring_tool>VCGLib | MeshLab</authoring_tool>
</contributor>
<created>Wed Aug 22 18:54:08 2018 GMT</created>
<modified>Wed Aug 22 18:54:08 2018 GMT</modified>
<up_axis>Y_UP</up_axis>
</asset>
<library_geometries>
<geometry id="shape0-lib" name="shape0">
<mesh>
<source id="shape0-lib-positions" name="position">
<float_array id="shape0-lib-positions-array" count="147">-0.0068518 0 0.0202589 -0.0059339 -0.00342592 0.0202589 -0.0118677 0 0.0152431 -0.0102777 -0.00593378 0.0152431 -0.0137036 0 0.0083912 -0.0118677 -0.00685184 0.0083912 -0.0179977 -0.010391 0.0036913 -0.0179977 0.010391 0.0036913 -0.0179977 -0.010391 4.45e-05 -0.0179977 0.010391 4.45e-05 0 0.00685184 0.0202589 -0.0034259 0.00593388 0.0202589 -0.0059338 0.00342592 0.0202589 0 0.0118677 0.0152431 -0.0059338 0.0102777 0.0152431 -0.0102777 0.00593388 0.0152431 0 0.0137036 0.0083912 -0.0068518 0.0118677 0.0083912 -0.0118677 0.00685184 0.0083912 -0.0034259 -0.00593388 0.0202589 -0.0059339 -0.0102777 0.0152431 0 -0.0118677 0.0152431 -0.0068518 -0.0118677 0.0083912 0 -0.00685184 0.0202589 0.0034259 -0.00593388 0.0202589 0.0059338 -0.00342592 0.0202589 0.0059338 -0.0102777 0.0152431 0 -0.0137036 0.0083912 0.0068518 -0.0118677 0.0083912 0 -0.020782 0.0036913 0 -0.020782 4.45e-05 0.0068518 0 0.0202589 0.0102777 -0.00593388 0.0152431 0.0118677 0 0.0152431 0.0118677 -0.00685184 0.0083912 0.0137036 0 0.0083912 0.0179977 -0.010391 0.0036913 0.0179977 -0.010391 4.45e-05 0 0 0.0220949 0.0059339 0.00342592 0.0202589 0.0034259 0.00593388 0.0202589 0.0102777 0.00593378 0.0152431 0.0059339 0.0102777 0.0152431 0.0118677 0.00685184 0.0083912 0.0068518 0.0118677 0.0083912 0 0.020782 0.0036913 0.0179977 0.010391 0.0036913 0 0.020782 4.45e-05 0.0179977 0.010391 4.45e-05</float_array>
<technique_common>
<accessor count="49" source="#shape0-lib-positions-array" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="shape0-lib-normals" name="normal">
<float_array id="shape0-lib-normals-array" count="180">-0.258207 0.0691885 0.963609 -0.258207 -0.069181 0.963609 -0.694739 0.18616 0.694753 -0.694739 0.186157 0.694754 -0.694754 -0.186144 0.694743 -0.694739 -0.18616 0.694753 -0.935114 0.250567 0.250555 -0.935115 0.250557 0.250563 -0.93511 -0.25057 0.250567 -0.935117 -0.250558 0.250555 -0.0691854 0.258206 0.963609 -0.189023 0.189018 0.963609 -0.186156 0.694749 0.694744 -0.186162 0.694744 0.694747 -0.50859 0.508578 0.694756 -0.508583 0.508597 0.694746 -0.250569 0.935111 0.250566 -0.250561 0.935115 0.250557 -0.684541 0.68456 0.250561 -0.684544 0.684555 0.250566 -0.189018 -0.189021 0.96361 -0.0691854 -0.258206 0.963609 -0.508583 -0.508591 0.694751 -0.508596 -0.508586 0.694745 -0.186155 -0.694745 0.694748 -0.186159 -0.694746 0.694746 -0.684558 -0.684545 0.250556 -0.684544 -0.684554 0.250571 -0.250566 -0.935114 0.250557 -0.250561 -0.935114 0.250562 0.0691854 -0.258206 0.963609 0.189023 -0.189018 0.963609 0.186156 -0.694749 0.694744 0.186162 -0.694744 0.694747 0.50859 -0.508578 0.694756 0.508583 -0.508597 0.694746 0.250569 -0.935111 0.250566 0.250561 -0.935115 0.250557 0.684541 -0.68456 0.250561 0.684544 -0.684555 0.250566 0.258207 -0.0691885 0.963609 0.258207 0.069181 0.963609 0.694739 -0.18616 0.694753 0.694739 -0.186157 0.694754 0.694754 0.186144 0.694743 0.694739 0.18616 0.694753 0.935114 -0.250567 0.250555 0.935115 -0.250557 0.250563 0.93511 0.25057 0.250567 0.935117 0.250558 0.250555 0.189018 0.189021 0.96361 0.0691854 0.258206 0.963609 0.508583 0.508591 0.694751 0.508596 0.508586 0.694745 0.186155 0.694745 0.694748 0.186159 0.694746 0.694746 0.684558 0.684545 0.250556 0.684544 0.684554 0.250571 0.250566 0.935114 0.250557 0.250561 0.935114 0.250562</float_array>
<technique_common>
<accessor count="60" source="#shape0-lib-normals-array" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="shape0-lib-map" name="map">
<float_array id="shape0-lib-map-array" count="360">0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0</float_array>
<technique_common>
<accessor count="180" source="#shape0-lib-map-array" stride="2">
<param name="U" type="float"/>
<param name="V" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="shape0-lib-vertices">
<input semantic="POSITION" source="#shape0-lib-positions"/>
</vertices>
<triangles count="60">
<input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/>
<input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/>
<input offset="2" semantic="TEXCOORD" source="#shape0-lib-map"/>
<p>38 0 0 12 0 1 0 0 2 38 1 3 0 1 4 1 1 5 2 2 6 0 2 7 12 2 8 12 3 9 15 3 10 2 3 11 3 4 12 1 4 13 0 4 14 0 5 15 2 5 16 3 5 17 4 6 18 2 6 19 15 6 20 15 7 21 18 7 22 4 7 23 5 8 24 3 8 25 2 8 26 2 9 27 4 9 28 5 9 29 38 10 30 10 10 31 11 10 32 38 11 33 11 11 34 12 11 35 14 12 36 11 12 37 10 12 38 10 13 39 13 13 40 14 13 41 15 14 42 12 14 43 11 14 44 11 15 45 14 15 46 15 15 47 17 16 48 14 16 49 13 16 50 13 17 51 16 17 52 17 17 53 18 18 54 15 18 55 14 18 56 14 19 57 17 19 58 18 19 59 38 20 60 1 20 61 19 20 62 38 21 63 19 21 64 23 21 65 20 22 66 19 22 67 1 22 68 1 23 69 3 23 70 20 23 71 21 24 72 23 24 73 19 24 74 19 25 75 20 25 76 21 25 77 22 26 78 20 26 79 3 26 80 3 27 81 5 27 82 22 27 83 27 28 84 21 28 85 20 28 86 20 29 87 22 29 88 27 29 89 38 30 90 23 30 91 24 30 92 38 31 93 24 31 94 25 31 95 26 32 96 24 32 97 23 32 98 23 33 99 21 33 100 26 33 101 32 34 102 25 34 103 24 34 104 24 35 105 26 35 106 32 35 107 28 36 108 26 36 109 21 36 110 21 37 111 27 37 112 28 37 113 34 38 114 32 38 115 26 38 116 26 39 117 28 39 118 34 39 119 38 40 120 25 40 121 31 40 122 38 41 123 31 41 124 39 41 125 33 42 126 31 42 127 25 42 128 25 43 129 32 43 130 33 43 131 41 44 132 39 44 133 31 44 134 31 45 135 33 45 136 41 45 137 35 46 138 33 46 139 32 46 140 32 47 141 34 47 142 35 47 143 43 48 144 41 48 145 33 48 146 33 49 147 35 49 148 43 49 149 38 50 150 39 50 151 40 50 152 38 51 153 40 51 154 10 51 155 42 52 156 40 52 157 39 52 158 39 53 159 41 53 160 42 53 161 13 54 162 10 54 163 40 54 164 40 55 165 42 55 166 13 55 167 44 56 168 42 56 169 41 56 170 41 57 171 43 57 172 44 57 173 16 58 174 13 58 175 42 58 176 42 59 177 44 59 178 16 59 179</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="VisualSceneNode" name="VisualScene">
<node id="node" name="node">
<instance_geometry url="#shape0-lib"/>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#VisualSceneNode"/>
</scene>
</COLLADA>

160
Tools/simulation/gazebo/models/x4/meshes/neo11_propeller_ccw.dae Normal file
View File

File diff suppressed because one or more lines are too long

160
Tools/simulation/gazebo/models/x4/meshes/neo11_propeller_cw.dae Normal file
View File

File diff suppressed because one or more lines are too long

74
Tools/simulation/gazebo/models/x4/meshes/pelican.dae Normal file
View File

File diff suppressed because one or more lines are too long

63
Tools/simulation/gazebo/models/x4/meshes/propeller_ccw.dae Normal file
View File

File diff suppressed because one or more lines are too long

63
Tools/simulation/gazebo/models/x4/meshes/propeller_cw.dae Normal file
View File

File diff suppressed because one or more lines are too long

2770
Tools/simulation/gazebo/models/x4/meshes/spotlight.dae Normal file
View File

File diff suppressed because one or more lines are too long

140
Tools/simulation/gazebo/models/x4/meshes/vi_camera.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
Tools/simulation/gazebo/models/x4/meshes/vi_camera.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 88 KiB

139
Tools/simulation/gazebo/models/x4/meshes/vi_sensor.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
Tools/simulation/gazebo/models/x4/meshes/vi_sensor.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 112 KiB

12596
Tools/simulation/gazebo/models/x4/meshes/x4.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
Tools/simulation/gazebo/models/x4/meshes/x4.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 166 KiB

17
Tools/simulation/gazebo/models/x4/model.config Normal file
View File

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<model>
<name>x4</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name>Luis Pinto</name>
<email>luis.pinto@mov.ai</email>
</author>
<description>
X4 UAV with sensor configuration #1: IMU, pressure sensor, magnetometer + GPS , RGBD
</description>
</model>

822
Tools/simulation/gazebo/models/x4/model.sdf Normal file
View File

@ -0,0 +1,822 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.6">
<model name="x4">
<pose>0 0 0.121078 0 0 0</pose>
<link name="base_link">
<pose frame="">0 0 0 0 -0 0</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>3.42</mass>
<inertia>
<ixx>0.075</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.075</iyy>
<iyz>0</iyz>
<izz>0.148916</izz>
</inertia>
</inertial>
<collision name="base_link_inertia_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.3 0.3 0.25</size>
</box>
</geometry>
</collision>
<visual name="base_link_inertia_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/x4.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="marker_visual_1">
<pose frame="">-0.1 0 0.077 0 -0.1 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/led.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="marker_visual_2">
<pose frame="">-0.09 0.059 0.059 -0.785397 -0 0.2</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/led.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="marker_visual_3">
<pose frame="">-0.09 -0.059 0.059 0.785397 -0 -0.2</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/led.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="downward_flashlight_visual">
<pose frame="">-0.043704 0 0.102914 -0.2 0.000158 -1.57002</pose>
<geometry>
<mesh>
<scale>0.01 0.01 0.01</scale>
<uri>meshes/spotlight.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="left_flashlight_visual">
<pose frame="">-0.071985 0.090826 0.066102 0.06 -4.8e-05 -1.27</pose>
<geometry>
<mesh>
<scale>0.01 0.01 0.01</scale>
<uri>meshes/spotlight.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="right_flashlight_visual">
<pose frame="">-0.071985 -0.090826 0.066102 0.06 -4.8e-05 -1.87</pose>
<geometry>
<mesh>
<scale>0.01 0.01 0.01</scale>
<uri>meshes/spotlight.dae</uri>
</mesh>
</geometry>
</visual>
<light name="right_light_source" type="spot">
<pose frame="">-0.0 -0.11 0.07 3.131592653589795 -1.5107899999999999 2.841592653589791</pose>
<attenuation>
<range>15</range>
<linear>0</linear>
<constant>0.1</constant>
<quadratic>0.01</quadratic>
</attenuation>
<diffuse>0.8 0.8 0.5 1</diffuse>
<specular>0.8 0.8 0.5 1</specular>
<spot>
<inner_angle>1</inner_angle>
<outer_angle>1.1</outer_angle>
<falloff>1</falloff>
</spot>
<direction>0 0 -1</direction>
<cast_shadows>1</cast_shadows>
</light>
<light name="downward_flashlight_source" type="spot">
<pose frame="">0.03 0 0.09 -0.01 -1.3708026535897933 0</pose>
<attenuation>
<range>15</range>
<linear>0</linear>
<constant>0.1</constant>
<quadratic>0.01</quadratic>
</attenuation>
<diffuse>0.8 0.8 0.5 1</diffuse>
<specular>0.8 0.8 0.5 1</specular>
<spot>
<inner_angle>1</inner_angle>
<outer_angle>1.1</outer_angle>
<falloff>1</falloff>
</spot>
<direction>0 0 -1</direction>
<cast_shadows>1</cast_shadows>
</light>
<light name="left_flashlight_source" type="spot">
<pose frame="">-0.0 0.11 0.07 3.131592653589795 -1.5107899999999999 -2.841592653589791</pose>
<attenuation>
<range>15</range>
<linear>0</linear>
<constant>0.1</constant>
<quadratic>0.01</quadratic>
</attenuation>
<diffuse>0.8 0.8 0.5 1</diffuse>
<specular>0.8 0.8 0.5 1</specular>
<spot>
<inner_angle>1</inner_angle>
<outer_angle>1.1</outer_angle>
<falloff>1</falloff>
</spot>
<direction>0 0 -1</direction>
<cast_shadows>1</cast_shadows>
</light>
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>250</update_rate>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.009</stddev>
<bias_mean>0.00075</bias_mean>
<bias_stddev>0.005</bias_stddev>
<dynamic_bias_stddev>0.00002</dynamic_bias_stddev>
<dynamic_bias_correlation_time>400.0</dynamic_bias_correlation_time>
<precision>0.00025</precision>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.009</stddev>
<bias_mean>0.00075</bias_mean>
<bias_stddev>0.005</bias_stddev>
<dynamic_bias_stddev>0.00002</dynamic_bias_stddev>
<dynamic_bias_correlation_time>400.0</dynamic_bias_correlation_time>
<precision>0.00025</precision>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.009</stddev>
<bias_mean>0.00075</bias_mean>
<bias_stddev>0.005</bias_stddev>
<dynamic_bias_stddev>0.00002</dynamic_bias_stddev>
<dynamic_bias_correlation_time>400.0</dynamic_bias_correlation_time>
<precision>0.00025</precision>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.021</stddev>
<bias_mean>0.05</bias_mean>
<bias_stddev>0.0075</bias_stddev>
<dynamic_bias_stddev>0.000375</dynamic_bias_stddev>
<dynamic_bias_correlation_time>175.0</dynamic_bias_correlation_time>
<precision>0.005</precision>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.021</stddev>
<bias_mean>0.05</bias_mean>
<bias_stddev>0.0075</bias_stddev>
<dynamic_bias_stddev>0.000375</dynamic_bias_stddev>
<dynamic_bias_correlation_time>175.0</dynamic_bias_correlation_time>
<precision>0.005</precision>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.021</stddev>
<bias_mean>0.05</bias_mean>
<bias_stddev>0.0075</bias_stddev>
<dynamic_bias_stddev>0.000375</dynamic_bias_stddev>
<dynamic_bias_correlation_time>175.0</dynamic_bias_correlation_time>
<precision>0.005</precision>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
<visual name="camera_mount_base_visual">
<pose>0.05 0 -0.030 0 0.0 0</pose>
<geometry>
<box>
<size>0.06 0.06 0.006</size>
</box>
</geometry>
</visual>
<visual name="camera_mount_arm_visual">
<pose>0.08 0 -0.040 0 0.0 0</pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.08</length>
</cylinder>
</geometry>
</visual>
<visual name="camera_mount_arm2_visual">
<pose>0.08 0 -0.08 0 1.57 0</pose>
<geometry>
<cylinder>
<radius>0.01</radius>
<length>0.025</length>
</cylinder>
</geometry>
</visual>
<visual name="camera_visual">
<pose>0.1 0 -0.08 0 0 0</pose>
<geometry>
<box>
<size>0.02 0.025 0.025</size>
</box>
</geometry>
</visual>
<sensor name="camera_front" type="rgbd_camera">
<pose>0.2 0 0 0 +.785 0</pose>
<always_on>1</always_on>
<update_rate>20</update_rate>
<camera name="camera_front">
<horizontal_fov>1.0472</horizontal_fov>
<lens>
<intrinsics>
<!-- fx = fy = width / ( 2 * tan (hfov / 2 ) ) -->
<fx>277.1</fx>
<fy>277.1</fy>
<!-- cx = ( width + 1 ) / 2 -->
<cx>160.5</cx>
<!-- cy = ( height + 1 ) / 2 -->
<cy>120.5</cy>
<s>0</s>
</intrinsics>
</lens>
<distortion>
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<p1>0.0</p1>
<p2>0.0</p2>
<center>0.5 0.5</center>
</distortion>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.01</near>
<far>300</far>
</clip>
<depth_camera>
<clip>
<near>0.1</near>
<far>10</far>
</clip>
</depth_camera>
<noise>
<type>gaussian</type>
<mean>0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<link name="rotor_0">
<pose frame="">0.247 0.1506 0.028 0.087267 0 0.523599</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>8.13545e-05</iyy>
<iyz>0</iyz>
<izz>8.22545e-05</izz>
</inertia>
</inertial>
<collision name="rotor_0_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1397</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode />
</contact>
<friction>
<ode />
</friction>
</surface>
</collision>
<visual name="rotor_0_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/neo11_propeller_ccw.dae</uri>
</mesh>
</geometry>
<material>
<diffuse>1 0 0 1</diffuse>
<script>
<name>Gazebo/Red</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
<cast_shadows>0</cast_shadows>
</visual>
<gravity>1</gravity>
<velocity_decay />
</link>
<joint name="rotor_0_joint" type="revolute">
<child>rotor_0</child>
<parent>base_link</parent>
<axis>
<xyz>0.043578 -0.075479 0.996195</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_1">
<pose frame="">-0.00067 0.28929 0.028 0 -0.087267 0</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>8.13545e-05</iyy>
<iyz>0</iyz>
<izz>8.22545e-05</izz>
</inertia>
</inertial>
<collision name="rotor_1_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1397</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode />
</contact>
<friction>
<ode />
</friction>
</surface>
</collision>
<visual name="rotor_1_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/neo11_propeller_cw.dae</uri>
</mesh>
</geometry>
<material>
<diffuse>0 0 1 1</diffuse>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
<cast_shadows>0</cast_shadows>
</visual>
<gravity>1</gravity>
<velocity_decay />
</link>
<joint name="rotor_1_joint" type="revolute">
<child>rotor_1</child>
<parent>base_link</parent>
<axis>
<xyz>-0.087156 0 0.996195</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_2">
<pose frame="">-0.2501 0.1454 0.028 0.087267 -0 2.61799</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>8.13545e-05</iyy>
<iyz>0</iyz>
<izz>8.22545e-05</izz>
</inertia>
</inertial>
<collision name="rotor_2_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1397</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode />
</contact>
<friction>
<ode />
</friction>
</surface>
</collision>
<visual name="rotor_2_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/neo11_propeller_ccw.dae</uri>
</mesh>
</geometry>
<material>
<diffuse>0 0 1 1</diffuse>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
<cast_shadows>0</cast_shadows>
</visual>
<gravity>1</gravity>
<velocity_decay />
</link>
<joint name="rotor_2_joint" type="revolute">
<child>rotor_2</child>
<parent>base_link</parent>
<axis>
<xyz>0.043578 0.075479 0.996195</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_3">
<pose frame="">-0.2501 -0.1454 0.028 -0.087267 -0 -2.61799</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>8.13545e-05</iyy>
<iyz>0</iyz>
<izz>8.22545e-05</izz>
</inertia>
</inertial>
<collision name="rotor_3_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1397</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode />
</contact>
<friction>
<ode />
</friction>
</surface>
</collision>
<visual name="rotor_3_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/neo11_propeller_cw.dae</uri>
</mesh>
</geometry>
<material>
<diffuse>0 0 1 1</diffuse>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
<cast_shadows>0</cast_shadows>
</visual>
<gravity>1</gravity>
<velocity_decay />
</link>
<joint name="rotor_3_joint" type="revolute">
<child>rotor_3</child>
<parent>base_link</parent>
<axis>
<xyz>0.043578 -0.075479 0.996195</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_4">
<pose frame="">-0.00067 -0.28929 0.028 -0 0.087267 -3.14159</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>8.13545e-05</iyy>
<iyz>0</iyz>
<izz>8.22545e-05</izz>
</inertia>
</inertial>
<collision name="rotor_4_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1397</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode />
</contact>
<friction>
<ode />
</friction>
</surface>
</collision>
<visual name="rotor_4_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/neo11_propeller_ccw.dae</uri>
</mesh>
</geometry>
<material>
<diffuse>0 0 1 1</diffuse>
<script>
<name>Gazebo/Blue</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
<cast_shadows>0</cast_shadows>
</visual>
<gravity>1</gravity>
<velocity_decay />
</link>
<joint name="rotor_4_joint" type="revolute">
<child>rotor_4</child>
<parent>base_link</parent>
<axis>
<xyz>-0.087156 -0 0.996195</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name="rotor_5">
<pose frame="">0.247 -0.1506 0.028 -0.087267 0 -0.523599</pose>
<inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<mass>0.005</mass>
<inertia>
<ixx>9.75e-07</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>8.13545e-05</iyy>
<iyz>0</iyz>
<izz>8.22545e-05</izz>
</inertia>
</inertial>
<collision name="rotor_5_collision">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<length>0.005</length>
<radius>0.1397</radius>
</cylinder>
</geometry>
<surface>
<contact>
<ode />
</contact>
<friction>
<ode />
</friction>
</surface>
</collision>
<visual name="rotor_5_visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/neo11_propeller_cw.dae</uri>
</mesh>
</geometry>
<material>
<diffuse>1 0 0 1</diffuse>
<script>
<name>Gazebo/Red</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
<cast_shadows>0</cast_shadows>
</visual>
<gravity>1</gravity>
<velocity_decay />
</link>
<joint name="rotor_5_joint" type="revolute">
<child>rotor_5</child>
<parent>base_link</parent>
<axis>
<xyz>0.043578 0.075479 0.996195</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<plugin filename="libignition-gazebo-imu-system.so" name="ignition::gazebo::systems::Imu" />
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x4</robotNamespace>
<jointName>rotor_0_joint</jointName>
<linkName>rotor_0</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>1.269e-05</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>0</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rotorDragCoefficient>0</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x4</robotNamespace>
<jointName>rotor_1_joint</jointName>
<linkName>rotor_1</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>1.269e-05</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>1</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rotorDragCoefficient>0</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x4</robotNamespace>
<jointName>rotor_2_joint</jointName>
<linkName>rotor_2</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>1.269e-05</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>2</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rotorDragCoefficient>0</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x4</robotNamespace>
<jointName>rotor_3_joint</jointName>
<linkName>rotor_3</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>1.269e-05</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>3</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rotorDragCoefficient>0</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x4</robotNamespace>
<jointName>rotor_4_joint</jointName>
<linkName>rotor_4</linkName>
<turningDirection>ccw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>1.269e-05</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>4</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rotorDragCoefficient>0</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>x4</robotNamespace>
<jointName>rotor_5_joint</jointName>
<linkName>rotor_5</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0182</timeConstantUp>
<timeConstantDown>0.0182</timeConstantDown>
<maxRotVelocity>1000.0</maxRotVelocity>
<motorConstant>1.269e-05</motorConstant>
<momentConstant>0.016754</momentConstant>
<commandSubTopic>command/motor_speed</commandSubTopic>
<motorNumber>5</motorNumber>
<rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
<rotorDragCoefficient>0</rotorDragCoefficient>
<rollingMomentCoefficient>0</rollingMomentCoefficient>
<rotorVelocitySlowdownSim>2</rotorVelocitySlowdownSim>
<motorType>velocity</motorType>
</plugin>
<plugin filename="ignition-gazebo-odometry-publisher-system" name="ignition::gazebo::systems::OdometryPublisher">
<dimensions>3</dimensions>
<odom_frame>x4/odom</odom_frame>
<robot_base_frame>x4/base_footprint</robot_base_frame>
</plugin>
</model>
</sdf>

BIN
Tools/simulation/gazebo/models/x4/thumbnails/1.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 46 KiB

BIN
Tools/simulation/gazebo/models/x4/thumbnails/2.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 57 KiB

BIN
Tools/simulation/gazebo/models/x4/thumbnails/3.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 33 KiB

BIN
Tools/simulation/gazebo/models/x4/thumbnails/4.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 34 KiB

BIN
Tools/simulation/gazebo/models/x4/thumbnails/5.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 38 KiB

68
Tools/simulation/gazebo/worlds/empty.sdf Normal file
View File

@ -0,0 +1,68 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.9">
<world name="empty">
<physics type="ode">
<max_step_size>0.004</max_step_size>
<real_time_factor>1.0</real_time_factor>
<real_time_update_rate>250</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>true</shadows>
</scene>
<gui fullscreen="0">
<!-- 3D scene -->
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
</plugin>
</gui>
<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>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</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>
</world>
</sdf>

12
Tools/sitl_run.sh
View File

@ -102,6 +102,7 @@ for file in "$@"; do
done
export PX4_SIM_MODEL=${model}
export PX4_SIM_WORLD=${world}
SIM_PID=0
@ -194,8 +195,15 @@ elif [ "$program" == "ignition" ] && [ -z "$no_sim" ]; then
else
ignition_headless=""
fi
source "$src_path/Tools/setup_ignition.bash" "${src_path}" "${build_path}"
ign gazebo --force-version 5 ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"&
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:${src_path}/Tools/simulation/gazebo/models
# TODO: verify if world already exists?
ign service --info --service /world/empty/create
echo $?
ign gazebo ${ignition_headless} -r "${src_path}/Tools/simulation/gazebo/worlds/${world}.sdf" &
elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then
echo "FG setup"
cd "${src_path}/Tools/flightgear_bridge/"

4
boards/px4/sitl/ign.px4board Normal file
View File

@ -0,0 +1,4 @@
CONFIG_DRIVERS_PWM_OUT_SIM=n
CONFIG_MODULES_SIH=n
CONFIG_BOARD_LOCKSTEP=y
CONFIG_MODULES_IGNITION_SIMULATOR=y

2
cmake/px4_add_common_flags.cmake
View File

@ -165,7 +165,7 @@ function(px4_add_common_flags)
-Wno-overloaded-virtual # TODO: fix and remove
)
if(NOT CMAKE_BUILD_TYPE STREQUAL FuzzTesting)
if((NOT CMAKE_BUILD_TYPE STREQUAL FuzzTesting) AND (NOT PX4_CONFIG MATCHES "px4_sitl"))
list(APPEND cxx_flags
-fno-rtti
)

14
platforms/posix/Debug/launch_sim.json.in
View File

@ -145,18 +145,10 @@
"id": "PX4_SIM_MODEL",
"description": "PX4_SIM_MODEL",
"options": [
"iris",
"typhoon_h480",
"plane",
"plane_catapult",
"plane_lidar",
"standard_vtol",
"tailsitter",
"tiltrotor",
"r1_rover",
"boat"
"x3",
"x4"
],
"default": "iris"
"default": "x3"
}
]
}

36
platforms/posix/cmake/sitl_target.cmake
View File

@ -53,7 +53,7 @@ if(parallel_jobs LESS 1)
endif()
message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available.
Limiting sitl_gazebo and simulation-ignition concurrent jobs to ${parallel_jobs}")
Limiting sitl_gazebo concurrent jobs to ${parallel_jobs}")
# project to build sitl_gazebo if necessary
px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/sitl_gazebo")
@ -73,20 +73,6 @@ ExternalProject_Add(sitl_gazebo
BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${parallel_jobs}
)
px4_add_git_submodule(TARGET git_ign_gazebo PATH "${PX4_SOURCE_DIR}/Tools/simulation-ignition")
ExternalProject_Add(simulation-ignition
SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation-ignition
CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
BINARY_DIR ${PX4_BINARY_DIR}/build_ign_gazebo
INSTALL_COMMAND ""
DEPENDS git_ign_gazebo
USES_TERMINAL_CONFIGURE true
USES_TERMINAL_BUILD true
EXCLUDE_FROM_ALL true
BUILD_ALWAYS 1
BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${parallel_jobs}
)
ExternalProject_Add(mavsdk_tests
SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests
CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
@ -129,7 +115,6 @@ set(viewers
none
jmavsim
gazebo
ignition
)
set(debuggers
@ -218,8 +203,6 @@ foreach(viewer ${viewers})
add_dependencies(${_targ_name} px4 sitl_gazebo)
elseif(viewer STREQUAL "jmavsim")
add_dependencies(${_targ_name} px4 git_jmavsim)
elseif(viewer STREQUAL "ignition")
add_dependencies(${_targ_name} px4 simulation-ignition)
endif()
else()
if(viewer STREQUAL "gazebo")
@ -409,6 +392,23 @@ if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
endforeach()
endif()
# Ignition Gazebo
add_custom_target(ignition_x3
COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> none ignition x3 empty ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS logs_symlink
)
add_custom_target(ignition_x4
COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> none ignition x4 empty ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS logs_symlink
)
string(REPLACE ";" "," posix_vmd_make_target_list "${all_posix_vmd_make_targets}")
add_custom_target(list_vmd_make_targets

2
src/include/visibility.h
View File

@ -81,7 +81,7 @@
/* We should include cstdlib or stdlib.h but this doesn't
* compile because many C++ files include stdlib.h and would
* need to get changed. */
#pragma GCC poison exit
//#pragma GCC poison exit
#endif // !defined(__PX4_NUTTX)

73
src/modules/ignition_simulator/CMakeLists.txt Normal file
View File

@ -0,0 +1,73 @@
############################################################################
#
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
add_compile_options(-frtti -fexceptions)
#add_compile_options(-fexceptions)
# Find the Ignition_Transport library
find_package(ignition-transport
REQUIRED COMPONENTS core
NAMES
ignition-transport8
ignition-transport10
ignition-transport11
#QUIET
)
message(STATUS "PACKAGE_FIND_NAME: ${ignition-transport_NAME}")
message(STATUS "PACKAGE_FIND_VERSION_MAJOR: ${ignition-transport_VERSION_MAJOR}")
message(STATUS "PACKAGE_FIND_VERSION_MINOR: ${ignition-transport_VERSION_MINOR}")
message(STATUS "PACKAGE_FIND_VERSION_COUNT: ${ignition-transport_VERSION_COUNT}")
set(IGN_TRANSPORT_VER ${ignition-transport_VERSION_MAJOR})
px4_add_module(
MODULE modules__ignition_simulator
MAIN ignition_simulator
COMPILE_FLAGS
#${MAX_CUSTOM_OPT_LEVEL}
-O0
SRCS
IgnitionSimulator.cpp
IgnitionSimulator.hpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
mixer_module
px4_work_queue
MODULE_CONFIG
module.yaml
)
target_link_libraries(modules__ignition_simulator PRIVATE ignition-transport${IGN_TRANSPORT_VER}::core)

382
src/modules/ignition_simulator/IgnitionSimulator.cpp Normal file
View File

@ -0,0 +1,382 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "IgnitionSimulator.hpp"
#include <uORB/Subscription.hpp>
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/getopt.h>
#include <iostream>
#include <string>
IgnitionSimulator::IgnitionSimulator(const char *world, const char *model) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_px4_accel(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_world_name(world),
_model_name(model)
{
_px4_accel.set_range(2000.f); // don't care
updateParams();
}
IgnitionSimulator::~IgnitionSimulator()
{
// TODO: unsubscribe
for (auto &sub_topic : _node.SubscribedTopics()) {
_node.Unsubscribe(sub_topic);
}
}
int IgnitionSimulator::init()
{
// clock
std::string clock_topic = "/world/" + _world_name + "/clock";
_node.Subscribe(clock_topic, &IgnitionSimulator::clockCallback, this);
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
_node.Subscribe(world_pose_topic, &IgnitionSimulator::poseInfoCallback, this);
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
_node.EnableStats(imu_topic, true);
_node.Subscribe(imu_topic, &IgnitionSimulator::imuCallback, this);
for (auto &sub_topic : _node.SubscribedTopics()) {
PX4_INFO("subscribed: %s", sub_topic.c_str());
}
// output eg /X3/command/motor_speed
std::string actuator_topic = _model_name + "/command/motor_speed";
_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);
if (!_actuators_pub.Valid()) {
PX4_ERR("failed to advertise %s", actuator_topic.c_str());
return PX4_ERROR;
}
ScheduleNow();
return OK;
}
int IgnitionSimulator::task_spawn(int argc, char *argv[])
{
const char *world_name = "empty";
const char *model_name = nullptr;
bool error_flag = false;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "w:m:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'w':
// world
world_name = myoptarg;
break;
case 'm':
// model
model_name = myoptarg;
break;
case '?':
error_flag = true;
break;
default:
PX4_WARN("unrecognized flag");
error_flag = true;
break;
}
}
if (error_flag) {
return PX4_ERROR;
}
PX4_INFO("world: %s, model: %s", world_name, model_name);
IgnitionSimulator *instance = new IgnitionSimulator(world_name, model_name);
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init() == PX4_OK) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
void IgnitionSimulator::clockCallback(const ignition::msgs::Clock &clock)
{
// uint64_t time_us = clock.sim().sec() * 1e6 + clock.sim().nsec() / 1000;
// PX4_INFO("clock %lu ", time_us);
struct timespec ts;
ts.tv_sec = clock.sim().sec();
ts.tv_nsec = clock.sim().nsec();
px4_clock_settime(CLOCK_MONOTONIC, &ts);
}
void IgnitionSimulator::imuCallback(const ignition::msgs::IMU &imu)
{
// FLU -> FRD
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
ignition::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
imu.linear_acceleration().x(),
imu.linear_acceleration().y(),
imu.linear_acceleration().z()));
ignition::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
imu.angular_velocity().x(),
imu.angular_velocity().y(),
imu.angular_velocity().z()));
//const uint64_t time_us = imu.header().stamp().sec() * 1e6 + imu.header().stamp().nsec() / 1000;
const uint64_t time_us = hrt_absolute_time();
if (time_us != 0) {
_px4_accel.update(time_us, accel_b.X(), accel_b.Y(), accel_b.Z());
_px4_gyro.update(time_us, gyro_b.X(), gyro_b.Y(), gyro_b.Z());
}
}
void IgnitionSimulator::poseInfoCallback(const ignition::msgs::Pose_V &pose)
{
//const uint64_t time_us = pose.header().stamp().sec() * 1e6 + pose.header().stamp().nsec() / 1000;
const uint64_t time_us = hrt_absolute_time();
if (time_us == 0) {
return;
}
for (int p = 0; p < pose.pose_size(); p++) {
if (pose.pose(p).name() == _model_name) {
const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1);
_timestamp_prev = time_us;
ignition::msgs::Vector3d pose_position = pose.pose(p).position();
ignition::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
/**
* @brief Quaternion for rotation between ENU and NED frames
*
* NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
* This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU.
*/
static const auto q_ENU_to_NED = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0);
// ground truth
ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
pose_orientation.w(),
pose_orientation.x(),
pose_orientation.y(),
pose_orientation.z());
ignition::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
ignition::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
// publish attitude groundtruth
vehicle_attitude_s vehicle_attitude_groundtruth{};
vehicle_attitude_groundtruth.timestamp_sample = time_us;
vehicle_attitude_groundtruth.q[0] = q_nb.W();
vehicle_attitude_groundtruth.q[1] = q_nb.X();
vehicle_attitude_groundtruth.q[2] = q_nb.Y();
vehicle_attitude_groundtruth.q[3] = q_nb.Z();
vehicle_attitude_groundtruth.timestamp = hrt_absolute_time();
_attitude_ground_truth_pub.publish(vehicle_attitude_groundtruth);
// publish angular velocity groundtruth
const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)};
vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{};
vehicle_angular_velocity_groundtruth.timestamp_sample = time_us;
const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt;
_euler_prev = euler;
angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz);
vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time();
_angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth);
if (!_pos_ref.isInitialized()) {
_pos_ref.initReference((double)_param_sim_home_lat.get(), (double)_param_sim_home_lon.get(), hrt_absolute_time());
}
vehicle_local_position_s local_position_groundtruth{};
local_position_groundtruth.timestamp_sample = time_us;
// position ENU -> NED
const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()};
const matrix::Vector3d velocity{(position - _position_prev) / dt};
const matrix::Vector3d acceleration{(velocity - _velocity_prev) / dt};
_position_prev = position;
_velocity_prev = velocity;
local_position_groundtruth.ax = acceleration(0);
local_position_groundtruth.ay = acceleration(1);
local_position_groundtruth.az = acceleration(2);
local_position_groundtruth.vx = velocity(0);
local_position_groundtruth.vy = velocity(1);
local_position_groundtruth.vz = velocity(2);
local_position_groundtruth.x = position(0);
local_position_groundtruth.y = position(1);
local_position_groundtruth.z = position(2);
local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
local_position_groundtruth.ref_alt = _param_sim_home_alt.get();
local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
local_position_groundtruth.timestamp = hrt_absolute_time();
_lpos_ground_truth_pub.publish(local_position_groundtruth);
if (_pos_ref.isInitialized()) {
// publish position groundtruth
vehicle_global_position_s global_position_groundtruth{};
global_position_groundtruth.timestamp_sample = time_us;
_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
global_position_groundtruth.lat, global_position_groundtruth.lon);
global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast<float>(position(2));
global_position_groundtruth.timestamp = hrt_absolute_time();
_gpos_ground_truth_pub.publish(global_position_groundtruth);
}
return;
}
}
}
bool IgnitionSimulator::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
ignition::msgs::Actuators rotor_velocity_message;
rotor_velocity_message.mutable_velocity()->Resize(num_outputs, 0);
for (unsigned i = 0; i < num_outputs; i++) {
rotor_velocity_message.set_velocity(i, outputs[i]);
}
if (_actuators_pub.Valid()) {
return _actuators_pub.Publish(rotor_velocity_message);
}
return false;
}
void IgnitionSimulator::Run()
{
if (should_exit()) {
ScheduleClear();
_mixing_output.unregister();
exit_and_cleanup();
return;
}
if (_parameter_update_sub.updated()) {
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
updateParams();
}
_mixing_output.update();
//ScheduleDelayed(1000_us);
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
_mixing_output.updateSubscriptions(true);
}
int IgnitionSimulator::print_status()
{
//perf_print_counter(_cycle_perf);
_mixing_output.printStatus();
return 0;
}
int IgnitionSimulator::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int IgnitionSimulator::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ignition_simulator", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int ignition_simulator_main(int argc, char *argv[])
{
return IgnitionSimulator::main(argc, argv);
}

125
src/modules/ignition_simulator/IgnitionSimulator.hpp Normal file
View File

@ -0,0 +1,125 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/geo/geo.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <ignition/msgs.hh>
#include <ignition/transport.hh>
#include <ignition/msgs/imu.pb.h>
using namespace time_literals;
class IgnitionSimulator : public ModuleBase<IgnitionSimulator>, public OutputModuleInterface
{
public:
IgnitionSimulator(const char *world, const char *model);
~IgnitionSimulator() override;
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
int init();
/** @see ModuleBase::print_status() */
int print_status() override;
private:
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
void mixerChanged() override {}
void Run() override;
void clockCallback(const ignition::msgs::Clock &clock);
void imuCallback(const ignition::msgs::IMU &imu);
void poseInfoCallback(const ignition::msgs::Pose_V &pose);
// Subscriptions
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
uORB::Publication<vehicle_global_position_s> _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
uORB::Publication<vehicle_local_position_s> _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
MapProjection _pos_ref{};
matrix::Vector3d _position_prev{};
matrix::Vector3d _velocity_prev{};
matrix::Vector3f _euler_prev{};
hrt_abstime _timestamp_prev{};
const std::string _world_name;
const std::string _model_name;
MixingOutput _mixing_output{"SIM_GZ", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
ignition::transport::Node _node;
ignition::transport::Node::Publisher _actuators_pub;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_HOME_LAT>) _param_sim_home_lat,
(ParamFloat<px4::params::SIM_HOME_LON>) _param_sim_home_lon,
(ParamFloat<px4::params::SIM_HOME_ALT>) _param_sim_home_alt
)
};

5
src/modules/ignition_simulator/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig MODULES_IGNITION_SIMULATOR
bool "ignition_simulator"
default n
---help---
Enable support for ignition_simulator

10
src/modules/ignition_simulator/module.yaml Normal file
View File

@ -0,0 +1,10 @@
module_name: SIM_GZ
actuator_output:
output_groups:
- param_prefix: SIM_GZ
channel_label: Channel
num_channels: 8
standard_params:
disarmed: { min: 0, max: 1000, default: 0 }
min: { min: 0, max: 1000, default: 10 }
max: { min: 100, max: 1000, default: 1000 }

56
src/modules/ignition_simulator/parameters.c Normal file
View File

@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* simulator origin latitude
*
* @unit deg
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_HOME_LAT, 47.397742f);
/**
* simulator origin longitude
*
* @unit deg
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_HOME_LON, 8.545594);
/**
* simulator origin altitude
*
* @unit m
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_HOME_ALT, 488.0);

1
src/modules/logger/logged_topics.cpp
View File

@ -234,6 +234,7 @@ void LoggedTopics::add_default_topics()
add_topic("time_offset");
add_topic("vehicle_angular_acceleration", 10);
add_topic("vehicle_angular_velocity", 10);
add_topic("vehicle_angular_velocity_groundtruth", 10);
add_topic("vehicle_attitude_groundtruth", 10);
add_topic("vehicle_global_position_groundtruth", 100);
add_topic("vehicle_local_position_groundtruth", 20);

6
src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt
View File

@ -36,7 +36,11 @@ px4_add_library(vehicle_angular_velocity
VehicleAngularVelocity.hpp
)
target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_compile_options(vehicle_angular_velocity
PRIVATE
${MAX_CUSTOM_OPT_LEVEL}
#-DDEBUG_BUILD
)
target_link_libraries(vehicle_angular_velocity
PRIVATE

4
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -112,7 +112,9 @@ bool VehicleAngularVelocity::UpdateSampleRate()
}
// calculate sensor update rate
if ((sample_rate_hz > 0) && PX4_ISFINITE(sample_rate_hz) && (publish_rate_hz > 0) && PX4_ISFINITE(publish_rate_hz)) {
if (PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 10) && (sample_rate_hz < 10'000)
&& PX4_ISFINITE(publish_rate_hz) && (publish_rate_hz > 0)
) {
// check if sample rate error is greater than 1%
const bool sample_rate_changed = (fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f;

8
src/modules/sensors/vehicle_imu/CMakeLists.txt
View File

@ -35,5 +35,11 @@ px4_add_library(vehicle_imu
VehicleIMU.cpp
VehicleIMU.hpp
)
target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_compile_options(vehicle_imu
PRIVATE
${MAX_CUSTOM_OPT_LEVEL}
#-DDEBUG_BUILD
)
target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration)

2
src/modules/simulator/sensor_baro_sim/SensorBaroSim.cpp
View File

@ -156,7 +156,7 @@ void SensorBaroSim::Run()
const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa;
// convert to hPa
float pressure = absolute_pressure_noisy * 0.01f + _sim_baro_off_p.get();
float pressure = absolute_pressure_noisy + _sim_baro_off_p.get();
// calculate temperature in Celsius
float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();

2
src/modules/simulator/sensor_gps_sim/SensorGpsSim.cpp
View File

@ -150,7 +150,7 @@ void SensorGpsSim::Run()
sensor_gps.vdop = 100.f;
}
// sensor_gps.timestamp_sample = gpos.timestamp;
sensor_gps.timestamp_sample = gpos.timestamp_sample;
sensor_gps.time_utc_usec = 0;
sensor_gps.device_id = device_id.devid;
sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees