Merge pull request #384 from MFR-Software/master

added camera info publish to the doImageUpdate method

Author: wouter-heerwegh

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

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<std::string>("camera_name", "");
-    camera_frame_prefix = nh->declare_parameter<std::string>("camera_frame_prefix", "/fsds/camera");
+    camera_frame_prefix = nh->declare_parameter<std::string>("camera_frame_prefix", "/fsds/");
     camera_frame_id = camera_frame_prefix + camera_name;
 
     framerate = nh->declare_parameter<double>("framerate", 0.0);