diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim index f610c19af0..ee24d195a7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim @@ -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_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw} " + 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=" file://${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf ${sdf_pose_str} " # 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