fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 11:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Make gz camera follow vehicle closely

Browse Source 
F
This commit is contained in:
JaeyoungLim 2025-11-30 16:55:37 -08:00
parent 0985c22701
commit 2ea7cc015b
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -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();

1
src/modules/simulation/gz_bridge/GZBridge.hpp
View File

@ -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;