add rtf service to gzbridge

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -211,6 +211,23 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	// Set Physics rtf
+	const char *speed_factor_str = std::getenv("PX4_SIM_SPEED_FACTOR");
+
+	if (speed_factor_str) {
+
+		double speed_factor = std::atof(speed_factor_str);
+		gz::msgs::Physics p_req;
+		p_req.set_max_step_size(speed_factor * 0.004);
+		p_req.set_real_time_factor(-1.0);
+		std::string world_physics = "/world/" + _world_name + "/set_physics";
+		std::string physics_service{world_physics};
+
+		if (!callPhysicsMsgService(physics_service, p_req)) {
+			return PX4_ERROR;
+		}
+	}
+
 	// Laser Scan: optional
 	std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan";
 
@@ -886,6 +903,28 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service)
 	return true;
 }
 
+bool GZBridge::callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req)
+{
+	bool result;
+	gz::msgs::Boolean rep;
+
+	if (_node.Request(service, req, 5000, rep, result)) {
+		if (!result) {
+			PX4_ERR("Physics service call failed.");
+			return false;
+
+		} else {
+			return true;
+		}
+
+	} else {
+		PX4_ERR("Physics Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly.");
+		return false;
+	}
+
+	return true;
+}
+
 bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req)
 {
 	bool result;

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -161,6 +161,8 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	*/
 	static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
 
+	bool callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req);
+
 	// Subscriptions
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};