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')