mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-30 04:06:33 +08:00
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
GitHub
@@ -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
|
||||
|
||||
Vendored
+5
@@ -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
|
||||
|
||||
Vendored
+28
-141
@@ -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"
|
||||
}
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -212,6 +212,7 @@ elif ! replay tryapplyparams
|
||||
then
|
||||
. px4-rc.simulator
|
||||
fi
|
||||
|
||||
load_mon start
|
||||
battery_simulator start
|
||||
tone_alarm start
|
||||
|
||||
@@ -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"
|
||||
Submodule Tools/simulation-ignition deleted from 483193d9b8
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 206 KiB |
@@ -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>
|
||||
@@ -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>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 182 KiB |
File diff suppressed because it is too large
Load Diff
@@ -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>
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 88 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 112 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 166 KiB |
@@ -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>
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
After Width: | Height: | Size: 46 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 57 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 33 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 34 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 38 KiB |
@@ -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>
|
||||
+10
-2
@@ -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/"
|
||||
|
||||
@@ -0,0 +1,4 @@
|
||||
CONFIG_DRIVERS_PWM_OUT_SIM=n
|
||||
CONFIG_MODULES_SIH=n
|
||||
CONFIG_BOARD_LOCKSTEP=y
|
||||
CONFIG_MODULES_IGNITION_SIMULATOR=y
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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"
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -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)
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig MODULES_IGNITION_SIMULATOR
|
||||
bool "ignition_simulator"
|
||||
default n
|
||||
---help---
|
||||
Enable support for ignition_simulator
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user