From dbd9a4a238a55a7dc71f89c707eee8c8b4b19c8a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 13 May 2021 18:52:22 +0300 Subject: [PATCH] optical_flow: publish debug image even when calc_flow_gyro failed --- clover/src/optical_flow.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/clover/src/optical_flow.cpp b/clover/src/optical_flow.cpp index 616e2464..b7182cd8 100644 --- a/clover/src/optical_flow.cpp +++ b/clover/src/optical_flow.cpp @@ -206,7 +206,7 @@ private: } catch (const tf2::TransformException& e) { // Invalidate previous frame prev_.release(); - return; + goto publish_debug; } } @@ -218,6 +218,10 @@ private: flow_.quality = (uint8_t)(response * 255); flow_pub_.publish(flow_); + prev_ = curr_.clone(); + prev_stamp_ = msg->header.stamp; + +publish_debug: // Publish debug image if (img_pub_.getNumSubscribers() > 0) { // publish debug image @@ -234,12 +238,9 @@ private: static geometry_msgs::TwistStamped velo; velo.header.stamp = msg->header.stamp; velo.header.frame_id = fcu_frame_id_; - velo.twist.angular.x = flow_.integrated_x / integration_time.toSec(); - velo.twist.angular.y = flow_.integrated_y / integration_time.toSec(); + velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec(); + velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec(); velo_pub_.publish(velo); - - prev_ = curr_.clone(); - prev_stamp_ = msg->header.stamp; } }