diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch
index 1efe1b89..8c77a0f4 100644
--- a/clover/launch/clover.launch
+++ b/clover/launch/clover.launch
@@ -45,6 +45,7 @@
+
diff --git a/clover/src/optical_flow.cpp b/clover/src/optical_flow.cpp
index 7207f516..36a98b6d 100644
--- a/clover/src/optical_flow.cpp
+++ b/clover/src/optical_flow.cpp
@@ -25,6 +25,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -57,6 +58,9 @@ private:
std::unique_ptr tf_listener_;
bool calc_flow_gyro_;
float flow_gyro_default_;
+ bool disable_on_vpe_;
+ ros::Subscriber vpe_sub_;
+ ros::Time last_vpe_time_;
std::shared_ptr> dyn_srv_;
void onInit()
@@ -87,6 +91,11 @@ private:
img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
+ disable_on_vpe_ = nh_priv.param("disable_on_vpe", false);
+ if (disable_on_vpe_) {
+ vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this);
+ }
+
dyn_srv_ = std::make_shared>(nh_priv);
dynamic_reconfigure::Server::CallbackType cb;
@@ -121,6 +130,12 @@ private:
{
if (!enabled_) return;
+ if (disable_on_vpe_ &&
+ !last_vpe_time_.isZero() &&
+ (msg->header.stamp - last_vpe_time_).toSec() < 0.1) {
+ return;
+ }
+
parseCameraInfo(cinfo);
auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -236,6 +251,14 @@ private:
prev_ = curr_.clone();
prev_stamp_ = msg->header.stamp;
+ // Publish estimated angular velocity
+ geometry_msgs::TwistStamped velo;
+ velo.header.stamp = msg->header.stamp;
+ velo.header.frame_id = fcu_frame_id_;
+ 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);
+
publish_debug:
// Publish debug image
if (img_pub_.getNumSubscribers() > 0) {
@@ -248,14 +271,6 @@ publish_debug:
out_msg.image = img;
img_pub_.publish(out_msg.toImageMsg());
}
-
- // Publish estimated angular velocity
- geometry_msgs::TwistStamped velo;
- velo.header.stamp = msg->header.stamp;
- velo.header.frame_id = fcu_frame_id_;
- 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);
}
}
@@ -284,6 +299,10 @@ publish_debug:
prev_ = Mat(); // clear previous frame
}
}
+
+ void vpeCallback(const geometry_msgs::PoseStamped& vpe) {
+ last_vpe_time_ = vpe.header.stamp;
+ }
};
PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)
diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py
index 2159b0f6..64100ea9 100755
--- a/clover/src/selfcheck.py
+++ b/clover/src/selfcheck.py
@@ -603,7 +603,14 @@ def check_optical_flow():
get_param('SENS_FLOW_MAXHGT'))
except rospy.ROSException:
- failure('no optical flow data (from Raspberry)')
+ if rospy.get_param('optical_flow/disable_on_vpe', False):
+ try:
+ rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+ info('no optical flow as disable_on_vpe is true')
+ except:
+ failure('no optical flow on RPi, disable_on_vpe is true, but no vision pose also')
+ else:
+ failure('no optical flow on RPi')
@check('Rangefinder')