diff --git a/README.md b/README.md index b934f3e..6c32e79 100644 --- a/README.md +++ b/README.md @@ -169,6 +169,10 @@ Parameters: - ``erc_rate``: event rate control rate (Gen4 sensor) events/sec. Default: 100000000. - ``mipi_frame_period``:: mipi frame period in usec. Only available on some sensors. Tune this to get faster callback rates from the SDK to the ROS driver. For instance 1008 will give a callback every millisecond. Risk of data corruption when set too low! Default: -1 (not set). +- ``trail_filter``: enable/disable event trail filter. Default: False. +- ``trail_filter_type``: type of trail filter. Allowed values: ``trail``, ``stc_cut_trail``, ``stc_keep_trail``. + Default: ``trail``. See Metavision SDK documentation. +- ``trail_filter_threshold``: Filter threshold, see MetavisionSDK documentation. Default: 5000. - ``sync_mode``: Used to synchronize the time stamps across multiple cameras (tested for only 2). The cameras must be connected via a sync cable, and two separate ROS driver nodes are started, see diff --git a/include/metavision_driver/logging.h b/include/metavision_driver/logging.h index 1eb91d2..d6f256d 100644 --- a/include/metavision_driver/logging.h +++ b/include/metavision_driver/logging.h @@ -76,13 +76,6 @@ SS << __VA_ARGS__; \ throw(std::runtime_error(SS.str())); \ } -#define BOMB_OUT_CERR(...) \ - { \ - std::cerr << __VA_ARGS__ << std::endl; \ - std::stringstream SS; \ - SS << __VA_ARGS__; \ - throw(std::runtime_error(SS.str())); \ - } #define LOG_INFO_NAMED(...) \ { \ RCLCPP_INFO_STREAM(rclcpp::get_logger(loggerName_), __VA_ARGS__); \ @@ -175,4 +168,12 @@ #endif // USING_ROS_1 +#define BOMB_OUT_CERR(...) \ + { \ + std::cerr << __VA_ARGS__ << std::endl; \ + std::stringstream SS; \ + SS << __VA_ARGS__; \ + throw(std::runtime_error(SS.str())); \ + } + #endif // METAVISION_DRIVER__LOGGING_H_ diff --git a/include/metavision_driver/metavision_wrapper.h b/include/metavision_driver/metavision_wrapper.h index 6206f17..a67e201 100644 --- a/include/metavision_driver/metavision_wrapper.h +++ b/include/metavision_driver/metavision_wrapper.h @@ -60,6 +60,13 @@ class MetavisionWrapper size_t maxQueueSize{0}; }; + struct TrailFilter + { + bool enabled; + std::string type; + uint32_t threshold; + }; + typedef std::map> HardwarePinConfig; explicit MetavisionWrapper(const std::string & loggerName); @@ -112,6 +119,7 @@ class MetavisionWrapper ercRate_ = rate; } void setMIPIFramePeriod(int usec) { mipiFramePeriod_ = usec; } + void setTrailFilter(const std::string & type, const uint32_t threshold, const bool state); bool triggerActive() const { @@ -139,6 +147,7 @@ class MetavisionWrapper const std::string & mode_in, const std::string & mode_out, const int period, const double duty_cycle); void configureEventRateController(const std::string & mode, const int rate); + void activateTrailFilter(); void configureMIPIFramePeriod(int usec, const std::string & sensorName); void printStatistics(); // ------------ variables @@ -168,6 +177,7 @@ class MetavisionWrapper HardwarePinConfig hardwarePinConfig_; std::string ercMode_; int ercRate_; + TrailFilter trailFilter_; int mipiFramePeriod_{-1}; std::string loggerName_{"driver"}; std::vector roi_; diff --git a/launch/driver_node.launch.py b/launch/driver_node.launch.py index e6a89c2..b09a305 100644 --- a/launch/driver_node.launch.py +++ b/launch/driver_node.launch.py @@ -50,6 +50,9 @@ def launch_setup(context, *args, **kwargs): "serial": LaunchConfig("serial"), "erc_mode": "enabled", "erc_rate": 100000000, + "trail_filter": False, + "trail_filter_type": "stc_trail_cut", + "trail_filter_threshold": 5000, # 'roi': [0, 0, 100, 100], # valid: 'external', 'loopback', 'disabled' "trigger_in_mode": "external", diff --git a/src/driver_ros2.cpp b/src/driver_ros2.cpp index 7a8e450..7f2152b 100644 --- a/src/driver_ros2.cpp +++ b/src/driver_ros2.cpp @@ -317,6 +317,19 @@ void DriverROS2::configureWrapper(const std::string & name) this->get_parameter_or("sync_mode", syncMode, std::string("standalone")); wrapper_->setSyncMode(syncMode); LOG_INFO("sync mode: " << syncMode); + bool trailFilter; + this->get_parameter_or("trail_filter", trailFilter, false); + std::string trailFilterType; + this->get_parameter_or("trail_filter_type", trailFilterType, std::string("trail")); + int trailFilterThreshold; + this->get_parameter_or("trail_filter_threshold", trailFilterThreshold, 0); + if (trailFilter) { + LOG_INFO( + "Using tail filter in " << trailFilterType << " mode with threshold " + << trailFilterThreshold); + wrapper_->setTrailFilter( + trailFilterType, static_cast(trailFilterThreshold), trailFilter); + } std::vector roi_long; this->get_parameter_or("roi", roi_long, std::vector()); std::vector r(roi_long.begin(), roi_long.end()); diff --git a/src/metavision_wrapper.cpp b/src/metavision_wrapper.cpp index fdb4940..0fe57ab 100644 --- a/src/metavision_wrapper.cpp +++ b/src/metavision_wrapper.cpp @@ -15,6 +15,8 @@ #include "metavision_driver/metavision_wrapper.h" +#include "metavision_driver/logging.h" + #ifdef USING_METAVISION_3 #include #include @@ -23,6 +25,7 @@ #include #endif +#include #include #include #include @@ -62,6 +65,11 @@ static const std::map channelMap static const std::map sensorToMIPIAddress = { {"IMX636", 0xB028}, {"Gen3.1", 0x1508}}; +static const std::map trailFilterMap = { + {"trail", Metavision::I_EventTrailFilterModule::Type::TRAIL}, + {"stc_cut_trail", Metavision::I_EventTrailFilterModule::Type::STC_CUT_TRAIL}, + {"stc_keep_trail", Metavision::I_EventTrailFilterModule::Type::STC_KEEP_TRAIL}}; + static std::string to_lower(const std::string upper) { std::string lower(upper); @@ -119,6 +127,14 @@ int MetavisionWrapper::setBias(const std::string & name, int val) return (now); } +void MetavisionWrapper::setTrailFilter( + const std::string & type, const uint32_t threshold, const bool state) +{ + trailFilter_.enabled = state; + trailFilter_.type = type; + trailFilter_.threshold = threshold; +} + bool MetavisionWrapper::initialize(bool useMultithreading, const std::string & biasFile) { biasFile_ = biasFile; @@ -428,8 +444,33 @@ void MetavisionWrapper::setDecodingEvents(bool decodeEvents) } } +void MetavisionWrapper::activateTrailFilter() +{ + Metavision::I_EventTrailFilterModule * i_trail_filter = + cam_.get_device().get_facility(); + + const auto it = trailFilterMap.find(trailFilter_.type); + if (it == trailFilterMap.end()) { + BOMB_OUT_CERR("unknown trail filter type " << trailFilter_.type); + } + + // Set filter type + if (!i_trail_filter->set_type(it->second)) { + LOG_WARN_NAMED("cannot set type of trail filter!") + } + if (!i_trail_filter->set_threshold(trailFilter_.threshold)) { + LOG_WARN_NAMED("cannot set threshold of trail filter!") + } + + i_trail_filter->enable(trailFilter_.enabled); +} + bool MetavisionWrapper::startCamera(CallbackHandler * h) { + if (trailFilter_.enabled) { + activateTrailFilter(); + } + try { callbackHandler_ = h; if (useMultithreading_) {