From 3025895c724a82282d06d2a8e3e5ab8bc0b98a28 Mon Sep 17 00:00:00 2001 From: Tobias Fischer Date: Mon, 20 Apr 2020 07:52:36 +1000 Subject: [PATCH] Fix camera info publishing --- davis_ros_driver/src/driver.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/davis_ros_driver/src/driver.cpp b/davis_ros_driver/src/driver.cpp index c26c973..b5bd6c5 100644 --- a/davis_ros_driver/src/driver.cpp +++ b/davis_ros_driver/src/driver.cpp @@ -711,12 +711,6 @@ void DavisRosDriver::readout() event_array_msg.reset(); } - - if (camera_info_manager_->isCalibrated()) - { - sensor_msgs::CameraInfoPtr camera_info_msg(new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo())); - camera_info_pub_.publish(camera_info_msg); - } } else if (type == IMU6_EVENT) { @@ -837,6 +831,13 @@ void DavisRosDriver::readout() caerDeviceConfigSet(davis_handle_, DAVIS_CONFIG_APS, DAVIS_CONFIG_APS_EXPOSURE, new_exposure); } + + if (camera_info_manager_->isCalibrated()) + { + sensor_msgs::CameraInfoPtr camera_info_msg(new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo())); + camera_info_msg->header.stamp = msg.header.stamp; + camera_info_pub_.publish(camera_info_msg); + } } }