diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt
index fa64a658..1144902d 100644
--- a/clover/CMakeLists.txt
+++ b/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
diff --git a/clover/cfg/Flow.cfg b/clover/cfg/Flow.cfg
new file mode 100644
index 00000000..21b47682
--- /dev/null
+++ b/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"))
diff --git a/clover/package.xml b/clover/package.xml
index 33ac4f7f..8572828a 100644
--- a/clover/package.xml
+++ b/clover/package.xml
@@ -39,6 +39,7 @@
tf2_web_republisher
python-lxml
python3-lxml
+ dynamic_reconfigure
python-pymavlink
diff --git a/clover/src/optical_flow.cpp b/clover/src/optical_flow.cpp
index 4c66f0de..e244d849 100644
--- a/clover/src/optical_flow.cpp
+++ b/clover/src/optical_flow.cpp
@@ -22,11 +22,13 @@
#include
#include
#include
+#include
#include
#include
#include
#include
#include
+#include
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 tf_listener_;
bool calc_flow_gyro_;
float flow_gyro_default_;
+ std::shared_ptr> dyn_srv_;
void onInit()
{
@@ -83,6 +87,12 @@ private:
img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
+ dyn_srv_ = std::make_shared>(nh_priv);
+ dynamic_reconfigure::Server::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)