diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
index b0025563df..810af8c261 100644
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -38,6 +38,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
+
# set local coordinate frame reference
if [ -n "${PX4_HOME_LAT}" ]; then
param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
@@ -59,36 +60,40 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
. ../gz_env.sh
fi
- # "gz sim" only avaiilable in Garden and later
- GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
- if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
- then
- # "gz sim" from Garden on
- gz_command="gz"
- gz_sub_command="sim"
- else
- echo "ERROR [init] Gazebo gz please install gz-garden"
- exit 1
- fi
+ # Only start up Gazebo if STANDALONE set to false
+ if [ "$STANDALONE" != '1' ]; then
- # look for running ${gz_command} gazebo world
- gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
-
- # shellcheck disable=SC2153
- if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
-
- echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
-
- ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
-
- if [ -z "${HEADLESS}" ]; then
- # HEADLESS not set, starting gui
- ${gz_command} ${gz_sub_command} -g &
+ # "gz sim" only avaiilable in Garden and later
+ GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
+ if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
+ then
+ # "gz sim" from Garden on
+ gz_command="gz"
+ gz_sub_command="sim"
+ else
+ echo "ERROR [init] Gazebo gz please install gz-garden"
+ exit 1
fi
- else
- echo "INFO [init] gazebo already running world: ${gz_world}"
- PX4_GZ_WORLD=${gz_world}
+ # look for running ${gz_command} gazebo world
+ gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+
+ # shellcheck disable=SC2153
+ if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+
+ echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+
+ ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+
+ if [ -z "${HEADLESS}" ]; then
+ # HEADLESS not set, starting gui
+ ${gz_command} ${gz_sub_command} -g &
+ fi
+
+ else
+ echo "INFO [init] gazebo already running world: ${gz_world}"
+ PX4_GZ_WORLD=${gz_world}
+ fi
fi
# start gz_bridge
diff --git a/Tools/simulation/gz/models/rc_cessna/model.sdf b/Tools/simulation/gz/models/rc_cessna/model.sdf
index 099ed777ec..8e5c19a471 100644
--- a/Tools/simulation/gz/models/rc_cessna/model.sdf
+++ b/Tools/simulation/gz/models/rc_cessna/model.sdf
@@ -69,7 +69,7 @@
0.1 0.1 0.1
- model://rc_cessna/meshes/body.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/body.dae
@@ -184,7 +184,7 @@
1 1 1
- model://rc_cessna/meshes/iris_prop_ccw.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/iris_prop_ccw.dae
@@ -229,7 +229,7 @@
0.1 0.1 0.1
- model://rc_cessna/meshes/left_aileron.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_aileron.dae
@@ -385,7 +385,7 @@
0.1 0.1 0.1
- model://rc_cessna/meshes/right_aileron.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_aileron.dae
@@ -412,7 +412,7 @@
0.1 0.1 0.1
- model://rc_cessna/meshes/left_flap.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_flap.dae
@@ -439,7 +439,7 @@
0.1 0.1 0.1
- model://rc_cessna/meshes/right_flap.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_flap.dae
@@ -466,7 +466,7 @@
0.1 0.1 0.1
- model://rc_cessna/meshes/elevators.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/elevators.dae
@@ -493,7 +493,7 @@
0.1 0.1 0.1
- model://rc_cessna/meshes/rudder.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/rudder.dae
diff --git a/Tools/simulation/gz/models/standard_vtol/model.sdf b/Tools/simulation/gz/models/standard_vtol/model.sdf
index 61f9d14d18..2306d2680f 100644
--- a/Tools/simulation/gz/models/standard_vtol/model.sdf
+++ b/Tools/simulation/gz/models/standard_vtol/model.sdf
@@ -43,7 +43,7 @@
0.001 0.001 0.001
- model://standard_vtol/meshes/x8_wing.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_wing.dae
@@ -183,7 +183,7 @@
1 1 1
- model://standard_vtol/meshes/iris_prop_ccw.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae
@@ -246,7 +246,7 @@
1 1 1
- model://standard_vtol/meshes/iris_prop_ccw.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae
@@ -309,7 +309,7 @@
1 1 1
- model://standard_vtol/meshes/iris_prop_ccw.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae
@@ -372,7 +372,7 @@
1 1 1
- model://standard_vtol/meshes/iris_prop_ccw.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae
@@ -436,7 +436,7 @@
0.8 0.8 0.8
- model://standard_vtol/meshes/iris_prop_ccw.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae
@@ -483,7 +483,7 @@
0.001 0.001 0.001
- model://standard_vtol/meshes/x8_elevon_left.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_left.dae
@@ -510,7 +510,7 @@
0.001 0.001 0.001
- model://standard_vtol/meshes/x8_elevon_right.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_right.dae
diff --git a/Tools/simulation/gz/models/x500/model.sdf b/Tools/simulation/gz/models/x500/model.sdf
index b544e01d49..ceb980f8af 100644
--- a/Tools/simulation/gz/models/x500/model.sdf
+++ b/Tools/simulation/gz/models/x500/model.sdf
@@ -23,7 +23,7 @@
1 1 1
- model://x500/meshes/NXP-HGD-CF.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/NXP-HGD-CF.dae
@@ -32,7 +32,7 @@
1 1 1
- model://x500/meshes/5010Base.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae
@@ -41,7 +41,7 @@
1 1 1
- model://x500/meshes/5010Base.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae
@@ -50,7 +50,7 @@
1 1 1
- model://x500/meshes/5010Base.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae
@@ -59,7 +59,7 @@
1 1 1
- model://x500/meshes/5010Base.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae
@@ -77,7 +77,7 @@
1.0 1.0 1.0
- model://x500/materials/textures/nxp.png
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png
@@ -96,7 +96,7 @@
1.0 1.0 1.0
- model://x500/materials/textures/nxp.png
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png
@@ -115,7 +115,7 @@
1.0 1.0 1.0
- model://x500/materials/textures/rd.png
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/rd.png
@@ -250,7 +250,7 @@
0.8461538461538461 0.8461538461538461 0.8461538461538461
- model://x500/meshes/1345_prop_ccw.stl
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl
@@ -265,7 +265,7 @@
1 1 1
- model://x500/meshes/5010Bell.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae
@@ -322,7 +322,7 @@
0.8461538461538461 0.8461538461538461 0.8461538461538461
- model://x500/meshes/1345_prop_ccw.stl
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl
@@ -337,7 +337,7 @@
1 1 1
- model://x500/meshes/5010Bell.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae
@@ -394,7 +394,7 @@
0.8461538461538461 0.8461538461538461 0.8461538461538461
- model://x500/meshes/1345_prop_cw.stl
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl
@@ -409,7 +409,7 @@
1 1 1
- model://x500/meshes/5010Bell.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae
@@ -466,7 +466,7 @@
0.8461538461538461 0.8461538461538461 0.8461538461538461
- model://x500/meshes/1345_prop_cw.stl
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl
@@ -481,7 +481,7 @@
1 1 1
- model://x500/meshes/5010Bell.dae
+ https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae
diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt
index 7e29672edf..4fadc01420 100644
--- a/src/modules/commander/CMakeLists.txt
+++ b/src/modules/commander/CMakeLists.txt
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (c) 2015 PX4 Development Team. All rights reserved.
+# Copyright (c) 2015-2023 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
@@ -31,11 +31,12 @@
#
############################################################################
+add_subdirectory(Arming)
+add_subdirectory(failsafe)
add_subdirectory(failure_detector)
add_subdirectory(HealthAndArmingChecks)
-add_subdirectory(failsafe)
-add_subdirectory(Arming)
add_subdirectory(ModeUtil)
+add_subdirectory(MulticopterThrowLaunch)
px4_add_module(
MODULE modules__commander
@@ -59,20 +60,22 @@ px4_add_module(
mag_calibration.cpp
rc_calibration.cpp
Safety.cpp
+ UserModeIntention.cpp
worker_thread.cpp
MODULE_CONFIG
module.yaml
DEPENDS
+ ArmAuthorization
circuit_breaker
+ failsafe
failure_detector
geo
health_and_arming_checks
hysteresis
- ArmAuthorization
+ mode_util
+ MulticopterThrowLaunch
sensor_calibration
world_magnetic_model
- mode_util
- failsafe
)
px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 491a9313d1..3d8355fa5b 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -34,14 +34,13 @@
#pragma once
/* Helper classes */
-#include "failure_detector/FailureDetector.hpp"
#include "failsafe/failsafe.h"
-#include "Safety.hpp"
-#include "worker_thread.hpp"
+#include "failure_detector/FailureDetector.hpp"
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include "HomePosition.hpp"
#include "ModeManagement.hpp"
#include "UserModeIntention.hpp"
+#include "worker_thread.hpp"
#include
#include
@@ -81,7 +80,6 @@
#include
#include
#include
-#include
#include
using math::constrain;
@@ -176,6 +174,10 @@ private:
void safetyButtonUpdate();
+ bool isThrowLaunchInProgress() const;
+
+ void throwLaunchUpdate();
+
void vtolStatusUpdate();
void updateTunes();
@@ -216,6 +218,7 @@ private:
FailsafeBase &_failsafe{_failsafe_instance};
FailureDetector _failure_detector{this};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
+ MulticopterThrowLaunch _multicopter_throw_launch{this};
Safety _safety{};
WorkerThread _worker_thread{};
ModeManagement _mode_management{
diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
index 7b94bd63bd..7cd8ecab22 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.cpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -42,6 +42,8 @@
#include
#include
+#include
+#include
GZBridge::GZBridge(const char *world, const char *name, const char *model,
const char *pose_str) :
@@ -72,7 +74,18 @@ int GZBridge::init()
// service call to create model
gz::msgs::EntityFactory req{};
- req.set_sdf_filename(_model_sim + "/model.sdf");
+ std::string filename = "../../../Tools/simulation/gz/models/" + _model_sim + "/model.sdf";
+
+ std::ifstream file(filename);
+ std::string fileContent;
+
+ if (file.is_open()) {
+ // Read the file content into a string
+ fileContent = std::string((std::istreambuf_iterator(file)), (std::istreambuf_iterator()));
+ file.close();
+ }
+
+ req.set_sdf(fileContent);
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
@@ -116,15 +129,44 @@ int GZBridge::init()
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");
+ bool gz_called = false;
+ // Check if STANDALONE has been set.
+ char *standalone_val = std::getenv("STANDALONE");
+
+ if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) {
+ // Check if Gazebo has been called and if not attempt to reconnect.
+ while (gz_called == false) {
+ if (_node.Request(create_service, req, 1000, rep, result)) {
+ if (!rep.data() || !result) {
+ PX4_ERR("EntityFactory service call failed");
+ return PX4_ERROR;
+
+ } else {
+ gz_called = true;
+ }
+ }
+
+ // If Gazebo has not been called, wait 2 seconds and try again.
+ else {
+ PX4_WARN("Service call timed out as Gazebo has not been detected.");
+ system_usleep(2000000);
+ }
+ }
+ }
+
+
+ // If STANDALONE has not been set, do not retry to reconnect.
+ else {
+ if (_node.Request(create_service, req, 1000, rep, result)) {
+ if (!rep.data() || !result) {
+ PX4_ERR("EntityFactory service call failed");
+ return PX4_ERROR;
+ }
+
+ } else {
+ PX4_ERR("Service call timed out");
return PX4_ERROR;
}
-
- } else {
- PX4_ERR("Service call timed out");
- return PX4_ERROR;
}
}