simulation/gz_bridge: enable navsat plugin for accurate positioning o…

…f real life maps in Gazebo (#22638) * publish the global groundtruth from the navsat callback and rearrange the local groundtruth as the altitude reference now has a dependency on the global groundtruth being initialized --------- Signed-off-by: frederik <[email protected]>
PX4 · Feb 13, 2024 · bb53781 · bb53781
1 parent c9ad60e
commit bb53781
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 66 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -41,19 +41,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 
 	echo "INFO  [init] Gazebo simulator"
 
-	# set local coordinate frame reference
-	if [ -n "${PX4_HOME_LAT}" ]; then
-		param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
-	fi
-
-	if [ -n "${PX4_HOME_LON}" ]; then
-		param set SIM_GZ_HOME_LON ${PX4_HOME_LON}
-	fi
-
-	if [ -n "${PX4_HOME_ALT}" ]; then
-		param set SIM_GZ_HOME_ALT ${PX4_HOME_ALT}
-	fi
-
 	# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
 	if [ -z "${PX4_GZ_STANDALONE}" ]; then
 

diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -216,6 +216,15 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	// GPS: /world/$WORLD/model/$MODEL/link/base_link/sensor/navsat_sensor/navsat
+	std::string nav_sat_topic = "/world/" + _world_name + "/model/" + _model_name +
+				    "/link/base_link/sensor/navsat_sensor/navsat";
+
+	if (!_node.Subscribe(nav_sat_topic, &GZBridge::navSatCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", nav_sat_topic.c_str());
+		return PX4_ERROR;
+	}
+
 	if (!_mixing_interface_esc.init(_model_name)) {
 		PX4_ERR("failed to init ESC output");
 		return PX4_ERROR;
@@ -565,10 +574,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 			vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time();
 			_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());
-			}
-
 			vehicle_local_position_s local_position_groundtruth{};
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 			local_position_groundtruth.timestamp_sample = time_us;
@@ -595,31 +600,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 
 			local_position_groundtruth.heading = euler.psi();
 
-			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_timestamp = _pos_ref.getProjectionReferenceTimestamp();
-
-			local_position_groundtruth.timestamp = hrt_absolute_time();
-			_lpos_ground_truth_pub.publish(local_position_groundtruth);
-
 			if (_pos_ref.isInitialized()) {
-				// publish position groundtruth
-				vehicle_global_position_s global_position_groundtruth{};
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-				global_position_groundtruth.timestamp_sample = time_us;
-#else
-				global_position_groundtruth.timestamp_sample = hrt_absolute_time();
-#endif
 
-				_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
-						   global_position_groundtruth.lat, global_position_groundtruth.lon);
+				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 = _alt_ref;
+				local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
+				local_position_groundtruth.xy_global = true;
+				local_position_groundtruth.z_global = true;
 
-				global_position_groundtruth.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);
+			} else {
+				local_position_groundtruth.ref_lat = static_cast<double>(NAN);
+				local_position_groundtruth.ref_lon = static_cast<double>(NAN);
+				local_position_groundtruth.ref_alt = NAN;
+				local_position_groundtruth.ref_timestamp = 0;
+				local_position_groundtruth.xy_global = false;
+				local_position_groundtruth.z_global = false;
 			}
 
+			local_position_groundtruth.timestamp = hrt_absolute_time();
+			_lpos_ground_truth_pub.publish(local_position_groundtruth);
+
 			pthread_mutex_unlock(&_node_mutex);
 			return;
 		}
@@ -704,6 +705,44 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry
 	pthread_mutex_unlock(&_node_mutex);
 }
 
+void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_node_mutex);
+
+	const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000);
+
+	if (time_us > _world_time_us.load()) {
+		updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec());
+	}
+
+	_timestamp_prev = time_us;
+
+	// initialize gps position
+	if (!_pos_ref.isInitialized()) {
+		_pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time());
+		_alt_ref = nav_sat.altitude();
+
+	} else {
+		// publish GPS groundtruth
+		vehicle_global_position_s global_position_groundtruth{};
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+		global_position_groundtruth.timestamp_sample = time_us;
+#else
+		global_position_groundtruth.timestamp_sample = hrt_absolute_time();
+#endif
+		global_position_groundtruth.lat = nav_sat.latitude_deg();
+		global_position_groundtruth.lon = nav_sat.longitude_deg();
+		global_position_groundtruth.alt = nav_sat.altitude();
+		_gpos_ground_truth_pub.publish(global_position_groundtruth);
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
 void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU)
 {
 	// FLU (ROS) to FRD (PX4) static rotation

diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp
@@ -105,6 +105,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void imuCallback(const gz::msgs::IMU &imu);
 	void poseInfoCallback(const gz::msgs::Pose_V &pose);
 	void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry);
+	void navSatCallback(const gz::msgs::NavSat &nav_sat);
 
 	/**
 	*
@@ -139,6 +140,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	pthread_mutex_t _node_mutex;
 
 	MapProjection _pos_ref{};
+	double _alt_ref{}; // starting altitude reference
 
 	matrix::Vector3d _position_prev{};
 	matrix::Vector3d _velocity_prev{};
@@ -153,10 +155,4 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	float _temperature{288.15};  // 15 degrees
 
 	gz::transport::Node _node;
-
-	DEFINE_PARAMETERS(
-		(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
-	)
 };
diff --git a/src/modules/simulation/gz_bridge/parameters.c b/src/modules/simulation/gz_bridge/parameters.c
@@ -40,26 +40,3 @@
  */
 PARAM_DEFINE_INT32(SIM_GZ_EN, 0);
 
-/**
- * simulator origin latitude
- *
- * @unit deg
- * @group Simulator
- */
-PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LAT, 47.397742f);
-
-/**
- * simulator origin longitude
- *
- * @unit deg
- * @group Simulator
- */
-PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LON, 8.545594);
-
-/**
- * simulator origin altitude
- *
- * @unit m
- * @group Simulator
- */
-PARAM_DEFINE_FLOAT(SIM_GZ_HOME_ALT, 488.0);