diff --git a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp index 2ffdc4cc..16bd04f3 100644 --- a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp +++ b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp @@ -764,8 +764,20 @@ void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name, static_cam_tf_body_msg.transform.translation.x = camera_setting.position.x(); static_cam_tf_body_msg.transform.translation.y = camera_setting.position.y(); static_cam_tf_body_msg.transform.translation.z = camera_setting.position.z(); + + // Existing quaternion from camera settings tf2::Quaternion quat; - quat.setRPY(math_common::deg2rad(camera_setting.rotation.roll), math_common::deg2rad(camera_setting.rotation.pitch), math_common::deg2rad(camera_setting.rotation.yaw)); + quat.setRPY(math_common::deg2rad(camera_setting.rotation.roll), + math_common::deg2rad(camera_setting.rotation.pitch), + math_common::deg2rad(camera_setting.rotation.yaw)); + + // Rotation fix for ROS convention + tf2::Quaternion additional_rotation(-0.5, 0.5, -0.5, 0.5); + + // Multiply the quaternions to apply the additional rotation + quat *= additional_rotation; + + // Assign the result to the message static_cam_tf_body_msg.transform.rotation.x = quat.x(); static_cam_tf_body_msg.transform.rotation.y = quat.y(); static_cam_tf_body_msg.transform.rotation.z = quat.z(); diff --git a/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge_camera.cpp b/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge_camera.cpp index a0b9be1a..38319658 100644 --- a/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge_camera.cpp +++ b/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge_camera.cpp @@ -107,6 +107,12 @@ void doImageUpdate() 0, 1, 0, 0, 0, 1}; + info_msg->p = {fx, 0, cx, 0, + 0, fy, cy, 0, + 0, 0, 1.0, 0}; + + info_pub->publish(*info_msg); + fps_statistic.addCount(); } @@ -174,7 +180,7 @@ int main(int argc, char ** argv) // load settings camera_name = nh->declare_parameter("camera_name", ""); - camera_frame_prefix = nh->declare_parameter("camera_frame_prefix", "/fsds/camera"); + camera_frame_prefix = nh->declare_parameter("camera_frame_prefix", "/fsds/"); camera_frame_id = camera_frame_prefix + camera_name; framerate = nh->declare_parameter("framerate", 0.0);