Gazebo Simulation Enablement (#20319)

This commit is contained in:
Benjamin Perseghetti 2022-09-29 13:49:31 -04:00 committed by GitHub
parent f89df9d986
commit f9522e831c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
48 changed files with 269 additions and 2269 deletions

8
.vscode/tasks.json vendored
View File

@ -218,10 +218,10 @@
"options": {
"cwd": "${workspaceFolder}",
"env": {
"IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/ignition/models",
"IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gz/models",
}
},
"command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/ignition/worlds/${input:ignWorld}.sdf",
"command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
"isBackground": true,
"presentation": {
"echo": true,
@ -342,8 +342,8 @@
"inputs": [
{
"type": "pickString",
"id": "ignWorld",
"description": "Ignition world",
"id": "gzWorld",
"description": "gz world",
"options": [
"default"
],

View File

@ -1,15 +1,15 @@
#!/bin/sh
#
# @name Ignition Gazebo X3
# @name Gazebo x500
#
# @type Quadrotor
#
. ${R}etc/init.d/rc.mc_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=ignition}
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500}
PX4_SIM_WORLD=${PX4_SIM_WORLD:=default}
param set-default CA_AIRFRAME 0
param set-default CA_ROTOR_COUNT 4
@ -30,19 +30,19 @@ 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_IGN_FUNC1 101
param set-default SIM_IGN_FUNC2 102
param set-default SIM_IGN_FUNC3 103
param set-default SIM_IGN_FUNC4 104
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_IGN_MIN1 150
param set-default SIM_IGN_MIN2 150
param set-default SIM_IGN_MIN3 150
param set-default SIM_IGN_MIN4 150
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_IGN_MAX1 1000
param set-default SIM_IGN_MAX2 1000
param set-default SIM_IGN_MAX3 1000
param set-default SIM_IGN_MAX4 1000
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

View File

@ -17,7 +17,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
exit 1
fi
elif [ "$PX4_SIMULATOR" = "ignition" ]; then
elif [ "$PX4_SIMULATOR" = "gz" ]; then
# source generated gazebo_env.sh for IGN_GAZEBO_RESOURCE_PATH
if [ -f gazebo_env.sh ]; then
@ -26,28 +26,52 @@ elif [ "$PX4_SIMULATOR" = "ignition" ]; then
. ../gazebo_env.sh
fi
ign_world=$( ign topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
# shellcheck disable=SC2236
if [ ! -z $PX4_GZ_VERBOSE ]; then
if [ "$PX4_GZ_VERBOSE" -le "4" ] && [ "$PX4_GZ_VERBOSE" -ge "1" ]; then
gz_verbose=$PX4_GZ_VERBOSE
else
gz_verbose=4
echo "WARN [init] PX4_GZ_VERBOSE was passed: $PX4_GZ_VERBOSE, not in range [1,4], defaulting to: $gz_verbose."
fi
echo "INFO [init] PX4_GZ_VERBOSE set to $gz_verbose."
else
gz_verbose=1
echo "INFO [init] PX4_GZ_VERBOSE not explicitly set, defaulting to: $gz_verbose."
fi
if [ -z $ign_world ]; then
gz_world=$( ign topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
# starting ign gazebo with ${PX4_SIM_WORLD} world
if [ -z $gz_world ]; then
# starting ign gazebo with ${PX4_GZ_WORLD} world
echo "INFO [init] starting ign gazebo"
ign gazebo --verbose=1 -r -s "${PX4_IGN_GAZEBO_WORLDS}/${PX4_SIM_WORLD}.sdf" &
# shellcheck disable=SC2153
ign gazebo --verbose=$gz_verbose -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
if [ -z $HEADLESS ]; then
# HEADLESS not set, starting ign gazebo gui
ign gazebo -g &
fi
else
echo "INFO [init] ign gazebo already running world: $ign_world"
PX4_SIM_WORLD=$ign_world
echo "INFO [init] ign gazebo already running world: $gz_world"
PX4_GZ_WORLD=$gz_world
fi
if [ -z $PX4_IGN_MODEL_POSE ]; then
# start ignition bridge without pose arg.
echo "WARN [init] PX4_IGN_MODEL_POSE not set, spawning at origin."
if simulator_ignition_bridge start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"; then
# shellcheck disable=SC2236
if [ ! -z $PX4_GZ_MODEL ] && [ -z $PX4_GZ_MODEL_NAME ]; then
# shellcheck disable=SC2236
if [ ! -z $PX4_GZ_MODEL_POSE ]; then
# Clean potential input line formatting.
model_pose="$( echo ${PX4_GZ_MODEL_POSE} | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/ / /g; s/ /,/g' )"
echo "INFO [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}"
else
echo "WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin."
model_pose="0,0,0,0,0,0"
fi
# start gz bridge with pose arg.
if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
sensor_baro_sim start
sensor_gps_sim start
sensor_mag_sim start
@ -55,22 +79,29 @@ elif [ "$PX4_SIMULATOR" = "ignition" ]; then
echo "ERROR [init] ign gazebo failed to start"
exit 1
fi
elif [ ! -z $PX4_GZ_MODEL_NAME ] && [ -z $PX4_GZ_MODEL ]; then
if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
sensor_baro_sim start
sensor_gps_sim start
sensor_mag_sim start
else
echo "ERROR [init] ign gazebo failed to start"
exit 1
fi
elif [ ! -z $PX4_SIM_MODEL ] && [ -z $PX4_GZ_MODEL_NAME ] && [ -z $PX4_GZ_MODEL ]; then
echo "WARN [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL."
if gz_bridge start -m "${PX4_SIM_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
sensor_baro_sim start
sensor_gps_sim start
sensor_mag_sim start
else
echo "ERROR [init] ign gazebo failed to start"
exit 1
fi
else
# Clean potential input line formatting.
model_pose="$( echo ${PX4_IGN_MODEL_POSE} | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/ / /g; s/ /,/g' )"
echo "INFO [init] PX4_IGN_MODEL_POSE set, spawning at: ${model_pose}"
# start ignition bridge with pose arg.
if simulator_ignition_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"; then
sensor_baro_sim start
sensor_gps_sim start
sensor_mag_sim start
else
echo "ERROR [init] ign gazebo failed to start"
exit 1
fi
echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_GZ_MODEL"
exit 1
fi
else

View File

@ -16,6 +16,7 @@ set -e
INSTALL_NUTTX="true"
INSTALL_SIM="true"
INSTALL_ARCH=`uname -m`
INSTALL_SIM_JAMMY="false"
# Parse arguments
for arg in "$@"
@ -28,6 +29,10 @@ do
INSTALL_SIM="false"
fi
if [[ $arg == "--sim_jammy" ]]; then
INSTALL_SIM_JAMMY="true"
fi
done
# detect if running in docker
@ -67,6 +72,10 @@ elif [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
echo "Ubuntu 18.04"
elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
echo "Ubuntu 20.04"
elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
echo "Ubuntu 22.04, simulation build off by default."
echo "Use --sim_jammy to enable simulation build."
INSTALL_SIM=$INSTALL_SIM_JAMMY
fi
@ -146,7 +155,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
util-linux \
vim-common \
;
if [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" ]]; then
sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
kconfig-frontends \
;

View File

@ -0,0 +1,17 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version='1.9'>
<model name='x500-Depth'>
<include merge='true'>
<uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</uri>
</include>
<include merge='true'>
<uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/OakD-Lite</uri>
<pose>.12 .03 .242 0 0 0</pose>
</include>
<joint name="CameraJoint" type="fixed">
<parent>base_link</parent>
<child>OakD-Lite/base_link</child>
<pose relative_to="base_link">.12 .03 .242 0 0 0</pose>
</joint>
</model>
</sdf>

View File

Before

Width:  |  Height:  |  Size: 32 KiB

After

Width:  |  Height:  |  Size: 32 KiB

View File

Before

Width:  |  Height:  |  Size: 50 KiB

After

Width:  |  Height:  |  Size: 50 KiB

View File

Before

Width:  |  Height:  |  Size: 27 KiB

After

Width:  |  Height:  |  Size: 27 KiB

View File

Before

Width:  |  Height:  |  Size: 27 KiB

After

Width:  |  Height:  |  Size: 27 KiB

View File

Before

Width:  |  Height:  |  Size: 23 KiB

After

Width:  |  Height:  |  Size: 23 KiB

View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version='1.9'>
<model name='x500'>
<include merge='true'>
<uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/x500-Base</uri>
</include>
</model>
</sdf>

View File

Before

Width:  |  Height:  |  Size: 32 KiB

After

Width:  |  Height:  |  Size: 32 KiB

View File

Before

Width:  |  Height:  |  Size: 51 KiB

After

Width:  |  Height:  |  Size: 51 KiB

View File

Before

Width:  |  Height:  |  Size: 28 KiB

After

Width:  |  Height:  |  Size: 28 KiB

View File

Before

Width:  |  Height:  |  Size: 27 KiB

After

Width:  |  Height:  |  Size: 27 KiB

View File

Before

Width:  |  Height:  |  Size: 23 KiB

After

Width:  |  Height:  |  Size: 23 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 1.5 MiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 25 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 26 KiB

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.

Before

Width:  |  Height:  |  Size: 1.5 MiB

File diff suppressed because one or more lines are too long

View File

@ -1,11 +0,0 @@
<?xml version="1.0"?>
<model>
<name>x500-Base</name>
<version>1.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Benjamin Perseghetti</name>
<email>bperseghetti@rudislabs.com</email>
</author>
<description>Model of the NXP HoverGames Drone development kit (KIT-HGDRONEK66). The PX4 software compatible kit provides mechanical, RC remote and other components needed to evaluate the RDDRONE-FMUK66 reference design. The FMU includes 100Base-T1 Automotive Ethernet, dual CAN transceivers, as well as SE050 secure element, and works with add on boards NavQPlus, MR-T1ETH8, MR-T1ADAPT, and CAN-nodes such as UCANS32K1SIC. Kit may be used with, and contains the components needed for the HoverGames.com coding challenges.</description>
</model>

View File

@ -1,516 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version='1.9'>
<model name='x500-Base'>
<pose>0 0 .24 0 0 0</pose>
<self_collide>false</self_collide>
<static>false</static>
<link name="base_link">
<inertial>
<mass>2.0</mass>
<inertia>
<ixx>0.02166666666666667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.02166666666666667</iyy>
<iyz>0</iyz>
<izz>0.04000000000000001</izz>
</inertia>
</inertial>
<gravity>true</gravity>
<velocity_decay/>
<visual name="base_link_visual">
<pose>0 0 .025 0 0 3.141592654</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/NXP-HGD-CF.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="5010_motor_base_0">
<pose>0.174 0.174 .032 0 0 -.45</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="5010_motor_base_1">
<pose>-0.174 0.174 .032 0 0 -.45</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="5010_motor_base_2">
<pose>0.174 -0.174 .032 0 0 -.45</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="5010_motor_base_3">
<pose>-0.174 -0.174 .032 0 0 -.45</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
<visual name="NXP_FMUK66_FRONT">
<pose>0.047 .001 .043 1 0 1.57</pose>
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>.013 .007</size>
</plane>
</geometry>
<material>
<diffuse>1.0 1.0 1.0</diffuse>
<specular>1.0 1.0 1.0</specular>
<pbr>
<metal>
<albedo_map>model://x500-Base/materials/textures/nxp.png</albedo_map>
</metal>
</pbr>
</material>
</visual>
<visual name="NXP_FMUK66_TOP">
<pose>-0.023 0 .0515 0 0 -1.57</pose>
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>.013 .007</size>
</plane>
</geometry>
<material>
<diffuse>1.0 1.0 1.0</diffuse>
<specular>1.0 1.0 1.0</specular>
<pbr>
<metal>
<albedo_map>model://x500-Base/materials/textures/nxp.png</albedo_map>
</metal>
</pbr>
</material>
</visual>
<visual name="RDDRONE_FMUK66_TOP">
<pose>-.03 0 .0515 0 0 -1.57</pose>
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>.032 .0034</size>
</plane>
</geometry>
<material>
<diffuse>1.0 1.0 1.0</diffuse>
<specular>1.0 1.0 1.0</specular>
<pbr>
<metal>
<albedo_map>model://x500-Base/materials/textures/rd.png</albedo_map>
</metal>
</pbr>
</material>
</visual>
<collision name="base_link_collision_0">
<pose>0 0 .007 0 0 0</pose>
<geometry>
<box>
<size>0.35355339059327373 0.35355339059327373 0.05</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<collision name="base_link_collision_1">
<pose>0 -0.098 -.123 -0.35 0 0</pose>
<geometry>
<box>
<size>0.015 0.015 0.21</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<collision name="base_link_collision_2">
<pose>0 0.098 -.123 0.35 0 0</pose>
<geometry>
<box>
<size>0.015 0.015 0.21</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<collision name="base_link_collision_3">
<pose>0 -0.132 -.2195 0 0 0</pose>
<geometry>
<box>
<size>0.25 0.015 0.015</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<collision name="base_link_collision_4">
<pose>0 0.132 -.2195 0 0 0</pose>
<geometry>
<box>
<size>0.25 0.015 0.015</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>250</update_rate>
</sensor>
</link>
<link name="rotor_0">
<gravity>true</gravity>
<self_collide>false</self_collide>
<velocity_decay/>
<pose>0.174 -0.174 0.06 0 0 0</pose>
<inertial>
<mass>0.016076923076923075</mass>
<inertia>
<ixx>3.8464910483993325e-07</ixx>
<iyy>2.6115851691700804e-05</iyy>
<izz>2.649858234714004e-05</izz>
</inertia>
</inertial>
<visual name="rotor_0_visual">
<pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500-Base/meshes/1345_prop_ccw.stl</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<visual name="rotor_0_visual_motor_bell">
<pose>0 0 -.032 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="rotor_0_collision">
<pose>0 0 0 0 0 0 </pose>
<geometry>
<box>
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
</link>
<joint name="rotor_0_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_0</child>
<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">
<gravity>true</gravity>
<self_collide>false</self_collide>
<velocity_decay/>
<pose>-0.174 0.174 0.06 0 0 0</pose>
<inertial>
<mass>0.016076923076923075</mass>
<inertia>
<ixx>3.8464910483993325e-07</ixx>
<iyy>2.6115851691700804e-05</iyy>
<izz>2.649858234714004e-05</izz>
</inertia>
</inertial>
<visual name="rotor_1_visual">
<pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500-Base/meshes/1345_prop_ccw.stl</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<visual name="rotor_1_visual_motor_top">
<pose>0 0 -.032 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="rotor_1_collision">
<pose>0 0 0 0 0 0 </pose>
<geometry>
<box>
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
</link>
<joint name="rotor_1_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_1</child>
<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">
<gravity>true</gravity>
<self_collide>false</self_collide>
<velocity_decay/>
<pose>0.174 0.174 0.06 0 0 0</pose>
<inertial>
<mass>0.016076923076923075</mass>
<inertia>
<ixx>3.8464910483993325e-07</ixx>
<iyy>2.6115851691700804e-05</iyy>
<izz>2.649858234714004e-05</izz>
</inertia>
</inertial>
<visual name="rotor_2_visual">
<pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500-Base/meshes/1345_prop_cw.stl</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<visual name="rotor_2_visual_motor_top">
<pose>0 0 -.032 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="rotor_2_collision">
<pose>0 0 0 0 0 0 </pose>
<geometry>
<box>
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
</link>
<joint name="rotor_2_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_2</child>
<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">
<gravity>true</gravity>
<self_collide>false</self_collide>
<velocity_decay/>
<pose>-0.174 -0.174 0.06 0 0 0</pose>
<inertial>
<mass>0.016076923076923075</mass>
<inertia>
<ixx>3.8464910483993325e-07</ixx>
<iyy>2.6115851691700804e-05</iyy>
<izz>2.649858234714004e-05</izz>
</inertia>
</inertial>
<visual name="rotor_3_visual">
<pose>-0.022 -0.14638461538461536 -0.016 0 0 0</pose>
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500-Base/meshes/1345_prop_cw.stl</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/DarkGrey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<visual name="rotor_3_visual_motor_top">
<pose>0 0 -.032 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500-Base/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="rotor_3_collision">
<pose>0 0 0 0 0 0 </pose>
<geometry>
<box>
<size>0.2792307692307692 0.016923076923076923 0.0008461538461538462</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
<friction>
<ode/>
</friction>
</surface>
</collision>
</link>
<joint name="rotor_3_joint" type="revolute">
<parent>base_link</parent>
<child>rotor_3</child>
<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>
</model>
</sdf>

View File

@ -1,85 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version='1.9'>
<model name='x500-Depth'>
<include merge='true'>
<uri>model://x500-Base</uri>
</include>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>model/x500-Depth</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>model/x500-Depth</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>model/x500-Depth</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>model/x500-Depth</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>
<include merge='true'>
<uri>https://fuel.gazebosim.org/1.0/RudisLaboratories/models/OakD-Lite</uri>
<pose>.12 .03 .242 0 0 0</pose>
</include>
<joint name="CameraJoint" type="fixed">
<parent>base_link</parent>
<child>OakD-Lite/base_link</child>
<pose relative_to="base_link">.12 .03 .242 0 0 0</pose>
</joint>
</model>
</sdf>

View File

@ -1,76 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<sdf version='1.9'>
<model name='x500'>
<include merge='true'>
<uri>model://x500-Base</uri>
</include>
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
<robotNamespace>model/x500</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>model/x500</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>model/x500</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>model/x500</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.

Before

Width:  |  Height:  |  Size: 32 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 51 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 28 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 27 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 23 KiB

View File

@ -41,7 +41,7 @@ CONFIG_MODULES_REPLAY=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_COMMON_SIMULATION=y
CONFIG_MODULES_SIMULATION_SIMULATOR_IGNITION_BRIDGE=y
CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_UUV_ATT_CONTROL=y
CONFIG_MODULES_UUV_POS_CONTROL=y

View File

@ -2,7 +2,7 @@
"version": "0.2.0",
"configurations": [
{
"name": "SITL (Ignition Gazebo)",
"name": "SITL (GZ)",
"type": "cppdbg",
"request": "launch",
"program": "${command:cmake.launchTargetPath}",
@ -15,7 +15,7 @@
"environment": [
{
"name": "PX4_SIM_MODEL",
"value": "${input:PX4_IGN_GZ_SIM_MODEL}"
"value": "${input:PX4_GZ_MODEL}"
}
],
"externalConsole": false,
@ -226,8 +226,8 @@
"inputs": [
{
"type": "pickString",
"id": "PX4_IGN_GZ_SIM_MODEL",
"description": "Ignition Gazebo vehicle model",
"id": "PX4_GZ_MODEL",
"description": "GZ vehicle model",
"options": [
"x500",
],

View File

@ -48,13 +48,13 @@ if(ignition-transport_FOUND)
set(IGN_TRANSPORT_VER ${ignition-transport_VERSION_MAJOR})
px4_add_module(
MODULE modules__simulation__ignition_bridge
MAIN simulator_ignition_bridge
MODULE modules__simulation__gz_bridge
MAIN gz_bridge
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
SimulatorIgnitionBridge.cpp
SimulatorIgnitionBridge.hpp
GZBridge.cpp
GZBridge.hpp
DEPENDS
mixer_module
px4_work_queue
@ -63,33 +63,33 @@ if(ignition-transport_FOUND)
module.yaml
)
file(GLOB ign_models
file(GLOB gz_models
LIST_DIRECTORIES true
RELATIVE ${PX4_SOURCE_DIR}/Tools/simulation/ignition/models
RELATIVE ${PX4_SOURCE_DIR}/Tools/simulation/gz/models
CONFIGURE_DEPENDS
${PX4_SOURCE_DIR}/Tools/simulation/ignition/models/*
${PX4_SOURCE_DIR}/Tools/simulation/gz/models/*
)
file(GLOB ign_worlds
file(GLOB gz_worlds
CONFIGURE_DEPENDS
${PX4_SOURCE_DIR}/Tools/simulation/ignition/worlds/*.sdf
${PX4_SOURCE_DIR}/Tools/simulation/gz/worlds/*.sdf
)
foreach(model ${ign_models})
foreach(world ${ign_worlds})
foreach(model ${gz_models})
foreach(world ${gz_worlds})
get_filename_component("world_name" ${world} NAME_WE)
if(world_name MATCHES "default")
add_custom_target(ign_${model}
add_custom_target(gz_${model}
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} $<TARGET_FILE:px4>
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS px4
)
else()
add_custom_target(ign_${model}_${world_name}
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} PX4_SIM_WORLD=${world_name} $<TARGET_FILE:px4>
add_custom_target(gz_${model}_${world_name}
COMMAND ${CMAKE_COMMAND} -E env PX4_GZ_MODEL=${model} PX4_GZ_WORLD=${world_name} $<TARGET_FILE:px4>
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS px4
@ -99,8 +99,8 @@ if(ignition-transport_FOUND)
endforeach()
# TODO: PX4_IGN_MODELS_PATH
# PX4_IGN_WORLDS_PATH
# TODO: PX4_GZ_MODELS_PATH
# PX4_GZ_WORLDS_PATH
configure_file(gazebo_env.sh.in ${PX4_BINARY_DIR}/rootfs/gazebo_env.sh)
endif()

View File

@ -31,7 +31,7 @@
*
****************************************************************************/
#include "SimulatorIgnitionBridge.hpp"
#include "GZBridge.hpp"
#include <uORB/Subscription.hpp>
@ -42,10 +42,12 @@
#include <iostream>
#include <string>
SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str) :
GZBridge::GZBridge(const char *world, const char *name, const char *model,
const char *pose_str) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_world_name(world),
_model_name(model),
_model_name(name),
_model_sim(model),
_model_pose(pose_str)
{
pthread_mutex_init(&_mutex, nullptr);
@ -53,7 +55,7 @@ SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *
updateParams();
}
SimulatorIgnitionBridge::~SimulatorIgnitionBridge()
GZBridge::~GZBridge()
{
// TODO: unsubscribe
@ -62,71 +64,73 @@ SimulatorIgnitionBridge::~SimulatorIgnitionBridge()
}
}
int SimulatorIgnitionBridge::init()
int GZBridge::init()
{
// service call to create model
// 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\""
ignition::msgs::EntityFactory req{};
req.set_sdf_filename(_model_name + "/model.sdf");
if (!_model_sim.empty()) {
// TODO: support model instances?
// req.set_name("model_instance_name"); // New name for the entity, overrides the name on the SDF.
req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities
// service call to create model
// ign service -s /world/${PX4_GZ_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_GZ_MODEL}/model.sdf\""
ignition::msgs::EntityFactory req{};
req.set_sdf_filename(_model_sim + "/model.sdf");
if (!_model_pose.empty()) {
PX4_INFO("Requested Model Position: %s", _model_pose.c_str());
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
std::vector<double> model_pose_v;
req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities
std::stringstream ss(_model_pose);
if (!_model_pose.empty()) {
PX4_INFO("Requested Model Position: %s", _model_pose.c_str());
while (ss.good()) {
std::string substr;
std::getline(ss, substr, ',');
model_pose_v.push_back(std::stod(substr));
std::vector<float> model_pose_v;
std::stringstream ss(_model_pose);
while (ss.good()) {
std::string substr;
std::getline(ss, substr, ',');
model_pose_v.push_back(std::stof(substr));
}
while (model_pose_v.size() < 6) {
model_pose_v.push_back(0.0);
}
ignition::msgs::Pose *p = req.mutable_pose();
ignition::msgs::Vector3d *position = p->mutable_position();
position->set_x(model_pose_v[0]);
position->set_y(model_pose_v[1]);
position->set_z(model_pose_v[2]);
ignition::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
q.Normalize();
ignition::msgs::Quaternion *orientation = p->mutable_orientation();
orientation->set_x(q.X());
orientation->set_y(q.Y());
orientation->set_z(q.Z());
orientation->set_w(q.W());
}
while (model_pose_v.size() < 6) {
model_pose_v.push_back(0.0);
}
//world/$WORLD/create service.
ignition::msgs::Boolean rep;
bool result;
std::string create_service = "/world/" + _world_name + "/create";
ignition::msgs::Pose *p = req.mutable_pose();
ignition::msgs::Vector3d *position = p->mutable_position();
position->set_x(model_pose_v[0]);
position->set_y(model_pose_v[1]);
position->set_z(model_pose_v[2]);
if (_node.Request(create_service, req, 1000, rep, result)) {
if (!rep.data() || !result) {
PX4_ERR("EntityFactory service call failed");
return PX4_ERROR;
}
ignition::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
q.Normalize();
ignition::msgs::Quaternion *orientation = p->mutable_orientation();
orientation->set_x(q.X());
orientation->set_y(q.Y());
orientation->set_z(q.Z());
orientation->set_w(q.W());
}
//world/$WORLD/create service.
ignition::msgs::Boolean rep;
bool result;
std::string create_service = "/world/" + _world_name + "/create";
if (_node.Request(create_service, req, 1000, rep, result)) {
if (!rep.data() || !result) {
PX4_ERR("EntityFactory service call failed");
} else {
PX4_ERR("Service call timed out");
return PX4_ERROR;
}
} else {
PX4_ERR("Service call timed out");
return PX4_ERROR;
}
// clock
std::string clock_topic = "/world/" + _world_name + "/clock";
if (!_node.Subscribe(clock_topic, &SimulatorIgnitionBridge::clockCallback, this)) {
if (!_node.Subscribe(clock_topic, &GZBridge::clockCallback, this)) {
PX4_ERR("failed to subscribe to %s", clock_topic.c_str());
return PX4_ERROR;
}
@ -134,7 +138,7 @@ int SimulatorIgnitionBridge::init()
// pose: /world/$WORLD/pose/info
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
if (!_node.Subscribe(world_pose_topic, &SimulatorIgnitionBridge::poseInfoCallback, this)) {
if (!_node.Subscribe(world_pose_topic, &GZBridge::poseInfoCallback, this)) {
PX4_ERR("failed to subscribe to %s", world_pose_topic.c_str());
return PX4_ERROR;
}
@ -142,15 +146,15 @@ int SimulatorIgnitionBridge::init()
// IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
if (!_node.Subscribe(imu_topic, &SimulatorIgnitionBridge::imuCallback, this)) {
if (!_node.Subscribe(imu_topic, &GZBridge::imuCallback, this)) {
PX4_ERR("failed to subscribe to %s", imu_topic.c_str());
return PX4_ERROR;
}
// ESC feedback: /model/X3/command/motor_speed
std::string motor_speed_topic = "/model/" + _model_name + "/command/motor_speed";
// ESC feedback: /x500/command/motor_speed
std::string motor_speed_topic = "/" + _model_name + "/command/motor_speed";
if (!_node.Subscribe(motor_speed_topic, &SimulatorIgnitionBridge::motorSpeedCallback, this)) {
if (!_node.Subscribe(motor_speed_topic, &GZBridge::motorSpeedCallback, this)) {
PX4_ERR("failed to subscribe to %s", motor_speed_topic.c_str());
return PX4_ERROR;
}
@ -160,9 +164,8 @@ int SimulatorIgnitionBridge::init()
PX4_INFO("subscribed: %s", sub_topic.c_str());
}
// output eg /model/X3/command/motor_speed
std::string actuator_topic = "/model/" + _model_name + "/command/motor_speed";
// output eg /X500/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()) {
@ -174,11 +177,13 @@ int SimulatorIgnitionBridge::init()
return OK;
}
int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
int GZBridge::task_spawn(int argc, char *argv[])
{
const char *world_name = "default";
const char *model_name = nullptr;
const char *model_pose = nullptr;
const char *model_sim = nullptr;
const char *px4_instance = nullptr;
bool error_flag = false;
@ -186,14 +191,14 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "w:m:p:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'w':
// world
world_name = myoptarg;
break;
case 'm':
case 'n':
// model
model_name = myoptarg;
break;
@ -203,6 +208,16 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
model_pose = myoptarg;
break;
case 'm':
// pose
model_sim = myoptarg;
break;
case 'i':
// pose
px4_instance = myoptarg;
break;
case '?':
error_flag = true;
break;
@ -218,13 +233,27 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
PX4_INFO("world: %s, model: %s", world_name, model_name);
if (!model_pose) {
model_pose = "";
}
SimulatorIgnitionBridge *instance = new SimulatorIgnitionBridge(world_name, model_name, model_pose);
if (!model_sim) {
model_sim = "";
}
if (!px4_instance) {
if (!model_name) {
model_name = model_sim;
}
} else if (!model_name) {
std::string model_name_std = std::string(model_sim) + "_" + std::string(px4_instance);
model_name = model_name_std.c_str();
}
PX4_INFO("world: %s, model name: %s, simulation model: %s", world_name, model_name, model_sim);
GZBridge *instance = new GZBridge(world_name, model_name, model_sim, model_pose);
if (instance) {
_object.store(instance);
@ -270,7 +299,7 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
bool SimulatorIgnitionBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec)
bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec)
{
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
struct timespec ts;
@ -287,7 +316,7 @@ bool SimulatorIgnitionBridge::updateClock(const uint64_t tv_sec, const uint64_t
return false;
}
void SimulatorIgnitionBridge::clockCallback(const ignition::msgs::Clock &clock)
void GZBridge::clockCallback(const ignition::msgs::Clock &clock)
{
pthread_mutex_lock(&_mutex);
@ -300,7 +329,7 @@ void SimulatorIgnitionBridge::clockCallback(const ignition::msgs::Clock &clock)
pthread_mutex_unlock(&_mutex);
}
void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
{
if (hrt_absolute_time() == 0) {
return;
@ -355,7 +384,7 @@ void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
pthread_mutex_unlock(&_mutex);
}
void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
{
if (hrt_absolute_time() == 0) {
return;
@ -475,7 +504,7 @@ void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pos
pthread_mutex_unlock(&_mutex);
}
void SimulatorIgnitionBridge::motorSpeedCallback(const ignition::msgs::Actuators &actuators)
void GZBridge::motorSpeedCallback(const ignition::msgs::Actuators &actuators)
{
if (hrt_absolute_time() == 0) {
return;
@ -504,8 +533,8 @@ void SimulatorIgnitionBridge::motorSpeedCallback(const ignition::msgs::Actuators
pthread_mutex_unlock(&_mutex);
}
bool SimulatorIgnitionBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
bool GZBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
unsigned active_output_count = 0;
@ -534,7 +563,7 @@ bool SimulatorIgnitionBridge::updateOutputs(bool stop_motors, uint16_t outputs[M
return false;
}
void SimulatorIgnitionBridge::Run()
void GZBridge::Run()
{
if (should_exit()) {
ScheduleClear();
@ -563,7 +592,7 @@ void SimulatorIgnitionBridge::Run()
pthread_mutex_unlock(&_mutex);
}
int SimulatorIgnitionBridge::print_status()
int GZBridge::print_status()
{
//perf_print_counter(_cycle_perf);
_mixing_output.printStatus();
@ -571,12 +600,12 @@ int SimulatorIgnitionBridge::print_status()
return 0;
}
int SimulatorIgnitionBridge::custom_command(int argc, char *argv[])
int GZBridge::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int SimulatorIgnitionBridge::print_usage(const char *reason)
int GZBridge::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
@ -588,17 +617,19 @@ int SimulatorIgnitionBridge::print_usage(const char *reason)
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("simulator_ignition_bridge", "driver");
PRINT_MODULE_USAGE_NAME("gz_bridge", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Fuel model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, nullptr, "Model Pose", false);
PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('i', nullptr, nullptr, "PX4 instance", false);
PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int simulator_ignition_bridge_main(int argc, char *argv[])
extern "C" __EXPORT int gz_bridge_main(int argc, char *argv[])
{
return SimulatorIgnitionBridge::main(argc, argv);
return GZBridge::main(argc, argv);
}

View File

@ -60,11 +60,11 @@
using namespace time_literals;
class SimulatorIgnitionBridge : public ModuleBase<SimulatorIgnitionBridge>, public OutputModuleInterface
class GZBridge : public ModuleBase<GZBridge>, public OutputModuleInterface
{
public:
SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str);
~SimulatorIgnitionBridge() override;
GZBridge(const char *world, const char *name, const char *model, const char *pose_str);
~GZBridge() override;
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
@ -121,16 +121,17 @@ private:
const std::string _world_name;
const std::string _model_name;
const std::string _model_sim;
const std::string _model_pose;
MixingOutput _mixing_output{"SIM_IGN", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
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_IGN_HOME_LAT>) _param_sim_home_lat,
(ParamFloat<px4::params::SIM_IGN_HOME_LON>) _param_sim_home_lon,
(ParamFloat<px4::params::SIM_IGN_HOME_ALT>) _param_sim_home_alt
(ParamFloat<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,
(ParamFloat<px4::params::SIM_GZ_HOME_LON>) _param_sim_home_lon,
(ParamFloat<px4::params::SIM_GZ_HOME_ALT>) _param_sim_home_alt
)
};

View File

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

View File

@ -0,0 +1,6 @@
#!/usr/bin/env bash
export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models
export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS

View File

@ -1,7 +1,7 @@
module_name: SIM_IGN
module_name: SIM_GZ
actuator_output:
output_groups:
- param_prefix: SIM_IGN
- param_prefix: SIM_GZ
channel_label: Channel
num_channels: 8
standard_params:

View File

@ -37,7 +37,7 @@
* @unit deg
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LAT, 47.397742f);
PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LAT, 47.397742f);
/**
* simulator origin longitude
@ -45,7 +45,7 @@ PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LAT, 47.397742f);
* @unit deg
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LON, 8.545594);
PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LON, 8.545594);
/**
* simulator origin altitude
@ -53,4 +53,4 @@ PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LON, 8.545594);
* @unit m
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_ALT, 488.0);
PARAM_DEFINE_FLOAT(SIM_GZ_HOME_ALT, 488.0);

View File

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

View File

@ -1,6 +0,0 @@
#!/usr/bin/env bash
export PX4_IGN_GAZEBO_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/models
export PX4_IGN_GAZEBO_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/worlds
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_IGN_GAZEBO_MODELS