diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 23684e76c1..f5d10989fa 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -70,6 +70,22 @@ int GZBridge::init() { if (!_model_sim.empty()) { + // 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; + } + } + // service call to create model gz::msgs::EntityFactory req{}; req.set_sdf_filename(_model_sim + "/model.sdf"); @@ -205,6 +221,7 @@ int GZBridge::init() return PX4_ERROR; } + // Laser Scan: optional std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan"; @@ -943,6 +960,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; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 27ef424956..32b445decd 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -166,6 +166,8 @@ private: */ 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};