fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

gz: Use pose specified by PX4_GZ_MODEL_POSE (#24956)

Browse Source 
* gz: use pose specified by PX4_GZ_MODEL_POSE

* gz: fix empty PX4_GZ_MODEL_POSE case

* gz: no pose rather than zero on empty argument
This commit is contained in:
Balduin 2025-06-06 20:30:56 +02:00 committed by GitHub
parent 457ce90541
commit e346e24158
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 23 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -110,25 +110,36 @@ if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"
POSE_ARG=""
if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
pos_x=${pos_x:-0}
pos_y=${pos_y:-0}
pos_z=${pos_z:-0}
sdf_pose_str=""
POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }"
echo "INFO [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
pose_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
pose_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
pose_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
pose_roll=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $4}')
pose_pitch=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $5}')
pose_yaw=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $6}')
pose_x=${pose_x:-0}
pose_y=${pose_y:-0}
pose_z=${pose_z:-0}
pose_roll=${pose_roll:-0}
pose_pitch=${pose_pitch:-0}
pose_yaw=${pose_yaw:-0}
sdf_pose_str="<pose> ${pose_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw} </pose>"
echo "INFO [init] Gazebo model pose: ${pose_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw}"
fi
echo "INFO [init] Spawning model"
echo "INFO [init] Spawning Gazebo model"
# include the actual SDF in this one, containing the pose if given
sdf_str="<sdf version=\"1.6\"> <include> <uri>file://${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf</uri> ${sdf_pose_str} </include> </sdf>"
# Spawn model
${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
--reptype gz.msgs.Boolean --timeout 5000 \
--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
--req "name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false, sdf: '${sdf_str}'" > /dev/null 2>&1
# Wait for model to spawn
sleep 1