Follow model in gz

This commit makes the gz bridge follow the spawned model

F
Update src/modules/simulation/gz_bridge/GZBridge.cpp

Co-authored-by: Frederik Markus <80588263+frede791@users.noreply.github.com>
Cleanup

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -150,16 +150,20 @@ int GZBridge::init()
 
 		// If PX4_GZ_STANDALONE has been set, you can try to connect but GZ_SIM_RESOURCE_PATH needs to be set correctly to work.
 		else {
-			if (_node.Request(create_service, req, 1000, rep, result)) {
-				if (!rep.data() || !result) {
-					PX4_ERR("EntityFactory service call failed.");
-					return PX4_ERROR;
-				}
-
-			} else {
-				PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly.");
+			if (!callEntityFactoryService(create_service, req)) {
 				return PX4_ERROR;
 			}
+
+			system_usleep(2000000);
+
+			gz::msgs::StringMsg follow_msg{};
+			follow_msg.set_data(_model_name);
+			callStringMsgService("/gui/follow", follow_msg);
+			gz::msgs::Vector3d follow_offset_msg{};
+			follow_offset_msg.set_x(-6.0);
+			follow_offset_msg.set_y(0.0);
+			follow_offset_msg.set_z(6.0);
+			callVector3dService("/gui/follow/offset", follow_offset_msg);
 		}
 	}
 
@@ -829,6 +833,70 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan)
 	_obstacle_distance_pub.publish(obs);
 }
 
+bool GZBridge::callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req)
+{
+	bool result;
+	gz::msgs::Boolean rep;
+
+	if (_node.Request(service, req, 1000, rep, result)) {
+		if (!rep.data() || !result) {
+			PX4_ERR("EntityFactory service call failed.");
+			return PX4_ERROR;
+		}
+
+	} else {
+		PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly.");
+		return PX4_ERROR;
+	}
+
+	return true;
+}
+
+bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req)
+{
+	bool result;
+
+	gz::msgs::Boolean rep;
+
+	if (_node.Request(service, req, 1000, rep, result)) {
+		if (!rep.data() || !result) {
+			PX4_ERR("String service call failed");
+			return false;
+
+		}
+	}
+
+	else {
+		PX4_ERR("Service call timed out: %s", service.c_str());
+		return false;
+	}
+
+	return true;
+}
+
+bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::Vector3d &req)
+{
+	bool result;
+
+	gz::msgs::Boolean rep;
+
+	if (_node.Request(service, req, 1000, rep, result)) {
+		if (!rep.data() || !result) {
+			PX4_ERR("String service call failed");
+			return false;
+
+		}
+	}
+
+	else {
+		PX4_ERR("Service call timed out: %s", service.c_str());
+		return false;
+	}
+
+	return true;
+}
+
+
 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

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -69,6 +69,7 @@
 #include <gz/msgs/model.pb.h>
 #include <gz/msgs/odometry_with_covariance.pb.h>
 #include <gz/msgs/laserscan.pb.h>
+#include <gz/msgs9/gz/msgs/stringmsg.pb.h>
 
 using namespace time_literals;
 
@@ -110,6 +111,34 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void navSatCallback(const gz::msgs::NavSat &nav_sat);
 	void laserScanCallback(const gz::msgs::LaserScan &scan);
 
+	/**
+	 * @brief Call Entityfactory service
+	 *
+	 * @param req
+	 * @return true
+	 * @return false
+	 */
+	bool callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req);
+
+	/**
+	 * @brief Call String service
+	 *
+	 * @param service
+	 * @param req
+	 * @return true
+	 * @return false
+	 */
+	bool callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req);
+
+	/**
+	 * @brief Call Vector3d Service
+	 *
+	 * @param service
+	 * @param req
+	 * @return true
+	 * @return false
+	 */
+	bool callVector3dService(const std::string &service, const gz::msgs::Vector3d &req);
 	/**
 	*
 	* Convert a quaterion from FLU_to_ENU frames (ROS convention)