Support for running simulation with gz sim in hitl mode

.gitmodules

@@ -91,3 +91,6 @@
 [submodule "boards/ssrc/saluki-nxp93"]
 	path = boards/ssrc/saluki-nxp93
 	url = [email protected]:tiiuae/saluki-nxp93.git
+[submodule "Tools/simulation/gz/plugins/px4-gzsim-plugins"]
+	path = Tools/simulation/gz/plugins/px4-gzsim-plugins
+	url = https://github.com/tiiuae/px4-gzsim-plugins.git

Tools/simulation/gz/hitl_run.sh

@@ -0,0 +1,43 @@
+#!/bin/bash
+
+#example for run
+#./Tools/simulation/gz/hitl_run.sh x500/model_hitl.sdf
+
+MODEL_PATH=$1
+
+if [ -z $MODEL_PATH ]; then
+    echo "You should specify a path to the using model"
+    echo "./Tools/simulation/gz/hitl_run.sh x500/model_hitl.sdf"
+    exit
+fi
+
+if [ ! -f $(pwd)/build/px4_sitl_default/rootfs/gz_env.sh ]; then
+    echo "You should build px4_sitl_default"
+    echo "make px4_sitl_default"
+    exit
+fi
+
+source $(pwd)/build/px4_sitl_default/rootfs/gz_env.sh
+
+if [ ! -f $GZ_SIM_SYSTEM_PLUGIN_PATH/libmavlink_hitl_gazebosim.so ]; then
+    echo "You should build gz-sim"
+    echo "make px4_sitl gz-sim"
+    exit
+fi
+
+spawn_model() {
+    sleep 5
+    MODEL=${PX4_GZ_MODELS}/${MODEL_PATH}
+    NAME="HITL_Drone"
+    REQUEST="sdf_filename: \"${MODEL}\", name: \"${NAME}\" pose: {position: {x: 1.01, y: 0.98, z: 0.83}}"
+    gz service -s /world/default/create \
+            --reqtype gz.msgs.EntityFactory \
+            --reptype gz.msgs.Boolean \
+            --timeout 1000 \
+            --req "`echo $REQUEST`"
+}
+
+spawn_model &
+
+WORD_PATH="${PX4_GZ_WORLDS}/default.sdf"
+gz sim ${WORD_PATH} -r

Tools/simulation/gz/models/ssrc_holybro_x500/model_hitl.sdf

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='ssrc_holybro_x500_hitl'>
+    <include merge='true'>
+      <uri>model://ssrc_holybro_x500</uri>
+    </include>
+    <plugin filename="libmavlink_hitl_gazebosim.so" name="mavlink_interface::GazeboMavlinkInterface">
+      <robotNamespace/>
+      <imuSubTopic>/link/base_link/sensor/imu_sensor/imu</imuSubTopic>
+      <poseSubTopic>/pose/info</poseSubTopic>
+      <mavlink_addr>192.168.200.101</mavlink_addr>
+      <mavlink_udp_local_port>14542</mavlink_udp_local_port>
+      <mavlink_udp_remote_port>14543</mavlink_udp_remote_port>
+      <mavlink_tcp_port>4560</mavlink_tcp_port>
+      <use_tcp>0</use_tcp>
+    </plugin>
+  </model>
+</sdf>

Tools/simulation/gz/models/x500/model_hitl.sdf

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='x500_hitl'>
+    <include merge='true'>
+      <uri>model://x500</uri>
+    </include>
+    <plugin filename="libmavlink_hitl_gazebosim.so" name="mavlink_interface::GazeboMavlinkInterface">
+      <robotNamespace/>
+      <imuSubTopic>/link/base_link/sensor/imu_sensor/imu</imuSubTopic>
+      <poseSubTopic>/pose/info</poseSubTopic>
+      <mavlink_addr>192.168.0.3</mavlink_addr>
+      <mavlink_udp_local_port>14542</mavlink_udp_local_port>
+      <mavlink_udp_remote_port>14543</mavlink_udp_remote_port>
+      <mavlink_tcp_port>4560</mavlink_tcp_port>
+      <use_tcp>0</use_tcp>
+    </plugin>
+  </model>
+</sdf>

src/modules/simulation/gz_bridge/gz_env.sh.in

@@ -4,3 +4,5 @@ export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models
 export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds
 
 export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS
+
+export GZ_SIM_SYSTEM_PLUGIN_PATH=@PX4_SOURCE_DIR@/build/px4_sitl_default/build_gz-sim_plugins

src/modules/simulation/simulator_mavlink/CMakeLists.txt

@@ -57,6 +57,7 @@ px4_add_module(
 
 include(sitl_targets_flightgear.cmake)
 include(sitl_targets_gazebo-classic.cmake)
+include(sitl_targets_gz-sim.cmake)
 include(sitl_targets_jmavsim.cmake)
 include(sitl_targets_jsbsim.cmake)
 

src/modules/simulation/simulator_mavlink/sitl_targets_gz-sim.cmake

@@ -0,0 +1,52 @@
+set(GAZEBO_VERSION 8)
+
+find_package(gz-sim8 QUIET)
+
+if(gz-sim8_FOUND)
+
+	# Estimate an appropriate number of parallel jobs
+	cmake_host_system_information(RESULT AVAILABLE_PHYSICAL_MEMORY QUERY AVAILABLE_PHYSICAL_MEMORY)
+	cmake_host_system_information(RESULT NUMBER_OF_LOGICAL_CORES QUERY NUMBER_OF_LOGICAL_CORES)
+
+	set(parallel_jobs 1)
+
+	if(NOT NUMBER_OF_LOGICAL_CORES)
+		include(ProcessorCount)
+		ProcessorCount(NUMBER_OF_LOGICAL_CORES)
+	endif()
+
+	if(NOT AVAILABLE_PHYSICAL_MEMORY AND NUMBER_OF_LOGICAL_CORES GREATER_EQUAL 4)
+		# Memory estimate unavailable, use N-2 jobs
+		math(EXPR parallel_jobs "${NUMBER_OF_LOGICAL_CORES} - 2")
+	endif()
+
+	if(AVAILABLE_PHYSICAL_MEMORY)
+		# Allow an additional job for every 1.5GB of available physical memory
+		math(EXPR parallel_jobs "${AVAILABLE_PHYSICAL_MEMORY}/(3*1024/2)")
+	else()
+		set(AVAILABLE_PHYSICAL_MEMORY "?")
+	endif()
+
+	if(parallel_jobs GREATER NUMBER_OF_LOGICAL_CORES)
+		set(parallel_jobs ${NUMBER_OF_LOGICAL_CORES})
+	endif()
+
+	if(parallel_jobs LESS 1)
+		set(parallel_jobs 1)
+	endif()
+
+	include(ExternalProject)
+
+	ExternalProject_Add(gz-sim
+		SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/gz/plugins/px4-gzsim-plugins/
+		BINARY_DIR ${PX4_BINARY_DIR}/build_gz-sim_plugins
+		INSTALL_COMMAND ""
+		DEPENDS mavlink_c_generate
+		USES_TERMINAL_CONFIGURE true
+		USES_TERMINAL_BUILD true
+		EXCLUDE_FROM_ALL true
+		BUILD_ALWAYS 1
+		BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${parallel_jobs}
+	)
+endif()
+