optical_flow: implement optical_flow/enabled dynamic parameter

clover/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   image_transport
   cv_bridge
+  dynamic_reconfigure
 )
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -126,10 +127,9 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+  cfg/Flow.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -211,6 +211,8 @@ add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use

clover/cfg/Flow.cfg

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+PACKAGE = "clover"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("enabled", bool_t, 0, "if optical flow enabled", True)
+
+exit(gen.generate(PACKAGE, "clover", "Flow"))

clover/package.xml

@@ -39,6 +39,7 @@
   <depend>tf2_web_republisher</depend>
   <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
   <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
+  <depend>dynamic_reconfigure</depend>
   <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->

clover/src/optical_flow.cpp

@@ -22,11 +22,13 @@
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
+#include <clover/FlowConfig.h>
 
 using cv::Mat;
 
@@ -38,6 +40,7 @@ public:
 	{}
 
 private:
+	bool enabled_;
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
 	std::string fcu_frame_id_, local_frame_id_;
@@ -54,6 +57,7 @@ private:
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
 	float flow_gyro_default_;
+	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
 	{
@@ -83,6 +87,12 @@ private:
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
+		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
+
+		cb = std::bind(&OpticalFlow::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
 		NODELET_INFO("Optical Flow initialized");
 	}
 
@@ -109,6 +119,8 @@ private:
 
 	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
 	{
+		if (!enabled_) return;
+
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -264,6 +276,14 @@ publish_debug:
 
 		return flow;
 	}
+
+	void paramCallback(clover::FlowConfig &config, uint32_t level)
+	{
+		enabled_ = config.enabled;
+		if (!enabled_) {
+			prev_ = Mat(); // clear previous frame
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)