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)