gz sitl: add option to use GPS home position from gazebo

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -44,6 +44,7 @@
 
 #include <iostream>
 #include <string>
+#include <cmath>
 
 GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) :
 	ModuleParams(nullptr),
@@ -545,7 +546,16 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 			_angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth);
 
 			if (!_pos_ref.isInitialized()) {
-				_pos_ref.initReference((double)_param_sim_home_lat.get(), (double)_param_sim_home_lon.get(), hrt_absolute_time());
+				if (!_param_use_sim_home.get() || !std::isnan(_gz_home_lat) && !std::isnan(_gz_home_lon)) {
+					_pos_ref.initReference(_param_use_sim_home.get() ? _gz_home_lat : (double)_param_sim_home_lat.get(),
+							       _param_use_sim_home.get() ? _gz_home_lon : (double)_param_sim_home_lon.get(),
+							       hrt_absolute_time());
+
+				} else {
+					// no reference set yet
+					pthread_mutex_unlock(&_node_mutex);
+					return;
+				}
 			}
 
 			vehicle_local_position_s local_position_groundtruth{};
@@ -576,7 +586,8 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 
 			local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
 			local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
-			local_position_groundtruth.ref_alt = _param_sim_home_alt.get();
+			local_position_groundtruth.ref_alt = _param_use_sim_home.get() ? static_cast<float>(_gz_home_alt) :
+							     _param_sim_home_alt.get();
 			local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
 
 			local_position_groundtruth.timestamp = hrt_absolute_time();
@@ -594,7 +605,9 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 				_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
 						   global_position_groundtruth.lat, global_position_groundtruth.lon);
 
-				global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast<float>(position(2));
+				global_position_groundtruth.alt = _param_use_sim_home.get() ? static_cast<float>(_gz_home_alt) :
+								  _param_sim_home_alt.get()
+								  - static_cast<float>(position(2));
 				global_position_groundtruth.timestamp = hrt_absolute_time();
 				_gpos_ground_truth_pub.publish(global_position_groundtruth);
 			}
@@ -607,6 +620,22 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 	pthread_mutex_unlock(&_node_mutex);
 }
 
+void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
+{
+	if (hrt_absolute_time() == 0 || _pos_ref.isInitialized()) {
+		return;
+	}
+
+	pthread_mutex_lock(&_node_mutex);
+
+	// initialize gz gps position
+	_gz_home_lat = nav_sat.latitude_deg();
+	_gz_home_lon = nav_sat.longitude_deg();
+	_gz_home_alt = nav_sat.altitude();
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
 void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry)
 {
 	if (hrt_absolute_time() == 0) {

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -109,6 +109,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void barometerCallback(const gz::msgs::FluidPressure &air_pressure);
 	void imuCallback(const gz::msgs::IMU &imu);
 	void poseInfoCallback(const gz::msgs::Pose_V &pose);
+	void navSatCallback(const gz::msgs::NavSat &nav_sat);
 	void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry);
 
 	/**
@@ -163,11 +164,18 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 
 	float _temperature{288.15};  // 15 degrees
 
+	double _gz_home_lat{std::numeric_limits<double>::quiet_NaN()};
+
+	double _gz_home_lon{std::numeric_limits<double>::quiet_NaN()};
+
+	double _gz_home_alt{std::numeric_limits<double>::quiet_NaN()};
+
 	gz::transport::Node::Publisher _cmd_vel_pub;
 
 	gz::transport::Node _node;
 
 	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::SIM_GZ_USE_SIM_HOME>) _param_use_sim_home,
 		(ParamFloat<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,
 		(ParamFloat<px4::params::SIM_GZ_HOME_LON>) _param_sim_home_lon,
 		(ParamFloat<px4::params::SIM_GZ_HOME_ALT>) _param_sim_home_alt

src/modules/simulation/gz_bridge/parameters.c

@@ -49,6 +49,15 @@ PARAM_DEFINE_INT32(SIM_GZ_EN, 0);
  */
 PARAM_DEFINE_INT32(SIM_GZ_RUN_GZSIM, 1);
 
+/**
+ * use simulator origin (home) position
+ *
+ * @boolean
+ * @reboot_required true
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_GZ_USE_SIM_HOME, 0);
+
 /**
  * simulator origin latitude
  *