Make gz camera follow vehicle closely

F

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -89,6 +89,8 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	_camera_pub = _node.Advertise<gz::msgs::GUICamera>("/gui/move_to/pose");
+
 	// OPTIONAL:
 	if (_sim_gz_en_gps.get()) {
 		if (!subscribeNavsat(false)) {
@@ -506,6 +508,21 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg)
 			const double dt = math::constrain((timestamp - _timestamp_prev) * 1e-6, 0.001, 0.1);
 			_timestamp_prev = timestamp;
 
+			gz::math::Pose3d model_pose = gz::msgs::Convert(msg);
+			gz::math::Vector3d offset = gz::math::Vector3d(5.0, 5.0, 5.0);
+			double offset_yaw = 0.1;
+			gz::math::Vector3d offset_world = model_pose.Rot().RotateVector(offset);
+			gz::math::Vector3d camera_position = model_pose.Pos() + offset_world;
+			double model_yaw = model_pose.Rot().Yaw();
+			// Camera looks at the model with specified pitch and yaw offset
+			gz::math::Quaterniond camera_orientation(0.0, -0.1, model_yaw + offset_yaw);
+
+			gz::msgs::GUICamera camera_msg;
+			gz::msgs::Set(camera_msg.mutable_pose()->mutable_position(), camera_position);
+			gz::msgs::Set(camera_msg.mutable_pose()->mutable_orientation(), camera_orientation);
+			camera_msg.duration = 0.1;
+			_camera_pub.publish(camera_msg);
+
 			gz::msgs::Vector3d pose_position = msg.pose(p).position();
 			gz::msgs::Quaternion pose_orientation = msg.pose(p).orientation();
 

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -178,6 +178,7 @@ private:
 
 	bool _realtime_clock_set{false};
 	gz::transport::Node _node;
+	gz::transport::Node::Publisher _camera_pub;
 
 	// GPS noise model
 	float _gps_pos_noise_n = 0.0f;