From 2ea7cc015bcb3a2e1675872bc904f564596f78c9 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sun, 30 Nov 2025 16:55:37 -0800 Subject: [PATCH] Make gz camera follow vehicle closely F --- src/modules/simulation/gz_bridge/GZBridge.cpp | 17 +++++++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 1 + 2 files changed, 18 insertions(+) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3ef65e186f..32b76f084c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -89,6 +89,8 @@ int GZBridge::init() return PX4_ERROR; } + _camera_pub = _node.Advertise("/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(); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 9f90f4dfea..2cc0982baf 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/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;