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

removal of PX4_GZ_MODEL env variable and fix of ground glitching (#22400) 

Removal of PX4_GZ_MODEL env variable and fix of ground glitching

Signed-off-by: frederik <frederik@auterion.com>
Co-authored-by: frederik <frederik@auterion.com>
Co-authored-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
This commit is contained in:
Frederik Markus 2023-11-29 10:08:04 +01:00 committed by GitHub
parent 59abab8379
commit f00d97d974
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 10 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -92,7 +92,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
fi fi
# start gz_bridge # start gz_bridge
if [ -n "${PX4_GZ_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
# model specified, gz_bridge will spawn model # model specified, gz_bridge will spawn model
if [ -n "${PX4_GZ_MODEL_POSE}" ]; then if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
@ -106,7 +106,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
fi fi
# start gz bridge with pose arg. # 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 if gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
if param compare -s SENS_EN_BAROSIM 1 if param compare -s SENS_EN_BAROSIM 1
then then
sensor_baro_sim start sensor_baro_sim start
@ -129,7 +129,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
exit 1 exit 1
fi fi
elif [ -n "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
# model name specificed, gz_bridge will attach to existing model # model name specificed, gz_bridge will attach to existing model
if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
@ -155,35 +155,8 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
exit 1 exit 1
fi fi
elif [ -n "${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#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
if param compare -s SENS_EN_BAROSIM 1
then
sensor_baro_sim start
fi
if param compare -s SENS_EN_GPSSIM 1
then
sensor_gps_sim start
fi
if param compare -s SENS_EN_MAGSIM 1
then
sensor_mag_sim start
fi
if param compare -s SENS_EN_ARSPDSIM 1
then
sensor_airspeed_sim start
fi
else
echo "ERROR [init] gz_bridge failed to start"
exit 1
fi
else else
echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_GZ_MODEL" echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
exit 1 exit 1
fi fi

6
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -95,6 +95,12 @@ int GZBridge::init()
model_pose_v.push_back(0.0); model_pose_v.push_back(0.0);
} }
// If model position z is less equal than 0, move above floor to prevent floor glitching
if (model_pose_v[2] <= 0.0) {
PX4_INFO("Model position z is less or equal 0.0, moving upwards");
model_pose_v[2] = 1.0;
}
gz::msgs::Pose *p = req.mutable_pose(); gz::msgs::Pose *p = req.mutable_pose();
gz::msgs::Vector3d *position = p->mutable_position(); gz::msgs::Vector3d *position = p->mutable_position();
position->set_x(model_pose_v[0]); position->set_x(model_pose_v[0]);