diff --git a/README.md b/README.md index 6c32e79..c221576 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@ If you are looking for more speed and features than the [official Prophesee ROS driver](https://github.com/prophesee-ai/prophesee_ros_wrapper) you have found the right repository. This driver can cope with the large amount of -data produced by Prophesee's Gen3 and Gen4 sensors because it does +data produced by Prophesee's Gen3 and later sensors because it does little more than getting the RAW (currently EVT3 format) events from the camera and publishing them in ROS [event_camera_msgs](https://github.com/ros-event-camera/event_camera_msgs) @@ -33,8 +33,8 @@ Tested on the following platforms: - ROS Noetic (legacy, please transition to ROS2) - ROS2 Humble and Rolling (also compiles on other versions, see CI) -- Ubuntu 20.04, 22.04, 24.04 LTS -- Metavision SDK (OpenEB) 4.2.0, 4.6.2 +- Ubuntu 22.04, 24.04 LTS +- Metavision SDK (OpenEB) 4.2.0, 4.6.2, 5.0.0 Tested on the following hardware: @@ -44,7 +44,7 @@ Tested on the following hardware: - [Prophesee EVK4 (Gen 4 sensor)](https://www.prophesee.ai/event-camera-evk4/) Explicitly not supported: any data in the old EVT2 format. The sensor -must produce data in the EVT3 format or later. +must produce data in the EVT3 format. The new EVT4 format is not yet supported. ## Installation from binaries diff --git a/cmake/ROS1.cmake b/cmake/ROS1.cmake index 81d2516..fb3f887 100644 --- a/cmake/ROS1.cmake +++ b/cmake/ROS1.cmake @@ -42,13 +42,13 @@ if(NOT MetavisionSDK_FOUND) if(IS_DIRECTORY "${metavision_SOURCE_DIR}") set_property(DIRECTORY ${metavision_SOURCE_DIR} PROPERTY EXCLUDE_FROM_ALL YES) endif() - + # Why is this variable not set automatically ??? + set(MetavisionSDK_VERSION_MAJOR 4) set(MUST_INSTALL_METAVISION TRUE) else() message(STATUS "metavision SDK is installed, not building it") endif() - #add_compile_options(-Wall -Wextra -pedantic -Werror) add_compile_options(-Wall -Wextra -Wpedantic) #add_compile_definitions(USING_ROS_1) @@ -64,9 +64,7 @@ find_package(catkin REQUIRED COMPONENTS # MetavisionSDK is now found otherwise # find_package(MetavisionSDK COMPONENTS driver REQUIRED) -if(MetavisionSDK_VERSION_MAJOR LESS 4) - add_definitions(-DUSING_METAVISION_3) -endif() +add_definitions(-DMETAVISION_VERSION=${MetavisionSDK_VERSION_MAJOR}) generate_dynamic_reconfigure_options( cfg/MetaVisionDyn.cfg) diff --git a/cmake/ROS2.cmake b/cmake/ROS2.cmake index 2d1ea4e..f69666b 100644 --- a/cmake/ROS2.cmake +++ b/cmake/ROS2.cmake @@ -28,12 +28,20 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE RelWithDebInfo) endif() -find_package(MetavisionSDK COMPONENTS driver REQUIRED) - -if(MetavisionSDK_VERSION_MAJOR LESS 4) - add_definitions(-DUSING_METAVISION_3) +# there is no "driver" component for MV 5.x, but the MV +# cmake file requires a COMPONENTS argument +find_package(MetavisionSDK COMPONENTS driver QUIET) + +if(${MetavisionSDK_VERSION_MAJOR} LESS 5) + set(MV_COMPONENTS driver) +else() + set(MV_COMPONENTS base core stream) endif() +# now that we know the MV version, require the components +find_package(MetavisionSDK COMPONENTS ${MV_COMPONENTS} REQUIRED) +add_definitions(-DMETAVISION_VERSION=${MetavisionSDK_VERSION_MAJOR}) + find_package(ament_cmake REQUIRED) find_package(ament_cmake_auto REQUIRED) find_package(ament_cmake_ros REQUIRED) @@ -60,8 +68,11 @@ ament_auto_add_library(driver_ros2 SHARED src/bias_parameter.cpp src/driver_ros2.cpp) +set(MV_COMPONENTS_QUAL ${MV_COMPONENTS}) +list(TRANSFORM MV_COMPONENTS_QUAL PREPEND "MetavisionSDK::") + target_include_directories(driver_ros2 PRIVATE include) -target_link_libraries(driver_ros2 MetavisionSDK::driver) +target_link_libraries(driver_ros2 ${MV_COMPONENTS_QUAL}) rclcpp_components_register_nodes(driver_ros2 "metavision_driver::DriverROS2") diff --git a/include/metavision_driver/callback_handler.h b/include/metavision_driver/callback_handler.h index 3f5fe12..a7b9045 100644 --- a/include/metavision_driver/callback_handler.h +++ b/include/metavision_driver/callback_handler.h @@ -16,7 +16,11 @@ #ifndef METAVISION_DRIVER__CALLBACK_HANDLER_H_ #define METAVISION_DRIVER__CALLBACK_HANDLER_H_ +#if METAVISION_VERSION < 5 #include +#else +#include +#endif namespace metavision_driver { diff --git a/include/metavision_driver/metavision_wrapper.h b/include/metavision_driver/metavision_wrapper.h index a67e201..8e2d909 100644 --- a/include/metavision_driver/metavision_wrapper.h +++ b/include/metavision_driver/metavision_wrapper.h @@ -16,7 +16,11 @@ #ifndef METAVISION_DRIVER__METAVISION_WRAPPER_H_ #define METAVISION_DRIVER__METAVISION_WRAPPER_H_ +#if METAVISION_VERSION < 5 #include +#else +#include +#endif #include #include diff --git a/src/metavision_wrapper.cpp b/src/metavision_wrapper.cpp index 0fe57ab..e692bec 100644 --- a/src/metavision_wrapper.cpp +++ b/src/metavision_wrapper.cpp @@ -17,7 +17,7 @@ #include "metavision_driver/logging.h" -#ifdef USING_METAVISION_3 +#if METAVISION_VERSION < 4 #include #include #else @@ -28,8 +28,11 @@ #include #include #include +#include #include +#include #include +#include #include #include @@ -46,14 +49,15 @@ namespace metavision_driver { -#ifdef USING_METAVISION_3 +#if METAVISION_VERSION < 4 using CameraSynchronization = Metavision::I_DeviceControl; using Window = Metavision::Roi::Rectangle; using ErcModule = Metavision::I_Erc; #else using CameraSynchronization = Metavision::I_CameraSynchronization; -using Window = Metavision::Roi::Window; using ErcModule = Metavision::I_ErcModule; +using Window = Metavision::I_ROI::Window; + static const std::map channelMap = { {"external", Metavision::I_TriggerIn::Channel::Main}, {"aux", Metavision::I_TriggerIn::Channel::Aux}, @@ -87,9 +91,8 @@ MetavisionWrapper::~MetavisionWrapper() { stop(); } int MetavisionWrapper::getBias(const std::string & name) { - const Metavision::Biases biases = cam_.biases(); - Metavision::I_LL_Biases * hw_biases = biases.get_facility(); - const auto pmap = hw_biases->get_all_biases(); + const auto biases = cam_.get_device().get_facility(); + const auto pmap = biases->get_all_biases(); auto it = pmap.find(name); if (it == pmap.end()) { LOG_ERROR_NAMED("unknown bias parameter: " << name); @@ -100,9 +103,8 @@ int MetavisionWrapper::getBias(const std::string & name) bool MetavisionWrapper::hasBias(const std::string & name) { - Metavision::Biases & biases = cam_.biases(); - Metavision::I_LL_Biases * hw_biases = biases.get_facility(); - const auto pmap = hw_biases->get_all_biases(); + const auto biases = cam_.get_device().get_facility(); + const auto pmap = biases->get_all_biases(); auto it = pmap.find(name); return (it != pmap.end()); } @@ -114,15 +116,14 @@ int MetavisionWrapper::setBias(const std::string & name, int val) LOG_WARN_NAMED("ignoring change to parameter: " << name); return (val); } - Metavision::Biases & biases = cam_.biases(); - Metavision::I_LL_Biases * hw_biases = biases.get_facility(); - const int prev = hw_biases->get(name); + const auto biases = cam_.get_device().get_facility(); + const int prev = biases->get(name); if (val != prev) { - if (!hw_biases->set(name, val)) { + if (!biases->set(name, val)) { LOG_WARN_NAMED("cannot set parameter " << name << " to " << val); } } - const int now = hw_biases->get(name); // read back what actually took hold + const int now = biases->get(name); // read back what actually took hold LOG_INFO_NAMED("changed " << name << " from " << prev << " to " << val << " adj to: " << now); return (now); } @@ -214,7 +215,7 @@ void MetavisionWrapper::applyROI(const std::vector & roi) y_max_ = std::max(static_cast(rect.y + rect.height), y_max_); #endif } - cam_.roi().set(rects); + cam_.get_device().get_facility()->set_windows(rects); } } else { #ifdef CHECK_IF_OUTSIDE_ROI @@ -356,7 +357,11 @@ bool MetavisionWrapper::initializeCamera() LOG_INFO_NAMED("sensor name: " << sinfo.name_); if (!biasFile_.empty()) { try { +#if METAVISION_VERSION < 5 cam_.biases().set_from_file(biasFile_); +#else + cam_.get_device().get_facility()->load_from_file(biasFile_); +#endif LOG_INFO_NAMED("using bias file: " << biasFile_); } catch (const Metavision::CameraException & e) { LOG_WARN_NAMED("reading bias file failed with error: " << e.what()); @@ -364,9 +369,8 @@ bool MetavisionWrapper::initializeCamera() } } else if (fromFile_.empty()) { // only load biases when not playing from file! LOG_INFO_NAMED("no bias file provided, using camera defaults:"); - const Metavision::Biases biases = cam_.biases(); - Metavision::I_LL_Biases * hw_biases = biases.get_facility(); - const auto pmap = hw_biases->get_all_biases(); + const auto biases = cam_.get_device().get_facility(); + const auto pmap = biases->get_all_biases(); for (const auto & bp : pmap) { LOG_INFO_NAMED("found bias param: " << bp.first << " " << bp.second); } @@ -375,8 +379,13 @@ bool MetavisionWrapper::initializeCamera() serialNumber_ = cam_.get_camera_configuration().serial_number; LOG_INFO_NAMED("camera serial number: " << serialNumber_); const auto & g = cam_.geometry(); +#if METAVISION_VERSION < 5 width_ = g.width(); height_ = g.height(); +#else + width_ = g.get_width(); + height_ = g.get_height(); +#endif LOG_INFO_NAMED("sensor geometry: " << width_ << " x " << height_); if (fromFile_.empty()) { applySyncMode(syncMode_); @@ -503,7 +512,11 @@ bool MetavisionWrapper::saveBiases() return (false); } else { try { +#if METAVISION_VERSION < 5 cam_.biases().save_to_file(biasFile_); +#else + cam_.get_device().get_facility()->save_to_file(biasFile_); +#endif LOG_INFO_NAMED("biases written to file: " << biasFile_); } catch (const Metavision::CameraException & e) { LOG_WARN_NAMED("failed to write bias file: " << e.what());