diff --git a/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_camera.hpp b/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_camera.hpp index c6b0339c..3422fb42 100644 --- a/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_camera.hpp +++ b/avt_vimba_camera/include/avt_vimba_camera/avt_vimba_camera.hpp @@ -163,6 +163,10 @@ class AvtVimbaCamera rcl_interfaces::msg::SetParametersResult parameterCallback(const std::vector& parameters); void updateCameraInfo(); void getCurrentState(diagnostic_updater::DiagnosticStatusWrapper& stat); + + double last_frame_sec = 0.0; + void timerCallback(); + rclcpp::TimerBase::SharedPtr timer_; }; } // namespace avt_vimba_camera #endif diff --git a/avt_vimba_camera/src/avt_vimba_camera.cpp b/avt_vimba_camera/src/avt_vimba_camera.cpp index 7a50ead9..cac1556e 100644 --- a/avt_vimba_camera/src/avt_vimba_camera.cpp +++ b/avt_vimba_camera/src/avt_vimba_camera.cpp @@ -64,8 +64,16 @@ AvtVimbaCamera::AvtVimbaCamera(rclcpp::Node::SharedPtr owner_node) updater_.setHardwareID("unknown"); updater_.add(owner_node->get_name(), this, &AvtVimbaCamera::getCurrentState); + timer_ = nh_->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&AvtVimbaCamera::timerCallback, this)); + last_frame_sec = nh_->now().seconds(); +} +void AvtVimbaCamera::timerCallback() +{ + if ( this->nh_->now().seconds() - last_frame_sec > 2) + { + RCLCPP_ERROR_STREAM(nh_->get_logger(), "Did not get frame after " << this->nh_->now().seconds() - last_frame_sec << " seconds"); + } } - void AvtVimbaCamera::start(const std::string& ip_str, const std::string& guid_str, const std::string& frame_id, const std::string& camera_info_url) { @@ -277,6 +285,7 @@ CameraPtr AvtVimbaCamera::openCamera(const std::string& id_str) void AvtVimbaCamera::frameCallback(const FramePtr vimba_frame_ptr) { std::unique_lock lock(config_mutex_); + last_frame_sec = this->nh_->now().seconds(); camera_state_ = OK; diagnostic_msg_ = "Camera operating normally";