diff --git a/CMakeLists.txt b/CMakeLists.txt index e653fc1..4d916a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,11 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror) + + # ignore the private-field-unused warning with clang + if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-unused-private-field) + endif() endif() find_package(rclcpp REQUIRED) @@ -51,7 +56,7 @@ endif() add_library(driver SHARED src/libcaer_wrapper.cpp src/driver.cpp src/message_converter.cpp src/device/device.cpp src/device/davis.cpp src/device/dvxplorer.cpp) -target_include_directories(driver PUBLIC include) +target_include_directories(driver PUBLIC include include/device) target_link_libraries(driver ${rclcpp_components_TARGETS} ${event_camera_msgs_TARGETS} ${sensor_msgs_TARGETS} ${std_srvs_TARGETS} libcaer::caer rclcpp::rclcpp diff --git a/include/libcaer_driver/callback_handler.hpp b/include/libcaer_driver/callback_handler.hpp index 7b9f32f..b5535e3 100644 --- a/include/libcaer_driver/callback_handler.hpp +++ b/include/libcaer_driver/callback_handler.hpp @@ -32,9 +32,8 @@ class CallbackHandler virtual void deviceDisconnectedCallback() = 0; virtual void polarityPacketCallback( uint64_t t, const libcaer::events::PolarityEventPacket & packet) = 0; - virtual void framePacketCallback( - uint64_t t, const libcaer::events::FrameEventPacket & packet) = 0; - virtual void imu6PacketCallback(uint64_t t, const libcaer::events::IMU6EventPacket & packet) = 0; + virtual void framePacketCallback(const libcaer::events::FrameEventPacket & packet) = 0; + virtual void imu6PacketCallback(const libcaer::events::IMU6EventPacket & packet) = 0; }; } // namespace libcaer_driver #endif // LIBCAER_DRIVER__CALLBACK_HANDLER_HPP_ diff --git a/include/libcaer_driver/driver.hpp b/include/libcaer_driver/driver.hpp index 9435f2f..9b08006 100644 --- a/include/libcaer_driver/driver.hpp +++ b/include/libcaer_driver/driver.hpp @@ -62,8 +62,8 @@ class Driver : public rclcpp::Node, public CallbackHandler void deviceDisconnectedCallback() override; void polarityPacketCallback( uint64_t t, const libcaer::events::PolarityEventPacket & packet) override; - void framePacketCallback(uint64_t t, const libcaer::events::FrameEventPacket & packet) override; - void imu6PacketCallback(uint64_t t, const libcaer::events::IMU6EventPacket & packet) override; + void framePacketCallback(const libcaer::events::FrameEventPacket & packet) override; + void imu6PacketCallback(const libcaer::events::IMU6EventPacket & packet) override; // ---------------- end of inherited ----------- diff --git a/launch/driver_composition.launch.py b/launch/driver_composition.launch.py index ddb40a4..41a38a5 100644 --- a/launch/driver_composition.launch.py +++ b/launch/driver_composition.launch.py @@ -50,6 +50,7 @@ def launch_setup(context, *args, **kwargs): "aps_frame_mode": 1, "auto_exposure_enabled": False, "auto_exposure_illumination": 127.0, + "time_reset_delay": 0, # increase for syncing mult. cameras }, ], extra_arguments=[{"use_intra_process_comms": True}], diff --git a/launch/start_recording.launch.py b/launch/start_recording.launch.py index adbb21a..e5351e1 100644 --- a/launch/start_recording.launch.py +++ b/launch/start_recording.launch.py @@ -40,7 +40,10 @@ def launch_setup(context, *args, **kwargs): name='recorder', parameters=[ { - 'record.topics': ['/event_camera/events'], + 'record.topics': [ + '/event_camera/events', + '/event_camera/imu', + ], # 'record.topics': ['/event_camera/events', # '/event_camera/image_raw'], 'record.start_paused': False, diff --git a/src/driver.cpp b/src/driver.cpp index 0a21fad..e0ed0f8 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -535,12 +535,11 @@ static int32_t compute_new_exposure_time( return (newTime); } -void Driver::framePacketCallback(uint64_t t, const libcaer::events::FrameEventPacket & packet) +void Driver::framePacketCallback(const libcaer::events::FrameEventPacket & packet) { if (cameraPub_.getNumSubscribers() > 0) { std::vector> msgs; - (void)message_converter::convert_frame_packet( - &msgs, packet, cameraFrameId_, rclcpp::Time(t, RCL_SYSTEM_TIME)); + (void)message_converter::convert_frame_packet(&msgs, packet, cameraFrameId_, rosBaseTime_); for (auto & img : msgs) { sensor_msgs::msg::CameraInfo::UniquePtr cinfo( new sensor_msgs::msg::CameraInfo(cameraInfoMsg_)); @@ -567,12 +566,11 @@ void Driver::framePacketCallback(uint64_t t, const libcaer::events::FrameEventPa } } -void Driver::imu6PacketCallback(uint64_t t, const libcaer::events::IMU6EventPacket & packet) +void Driver::imu6PacketCallback(const libcaer::events::IMU6EventPacket & packet) { if (imuPub_->get_subscription_count() > 0) { std::vector> msgs; - (void)message_converter::convert_imu6_packet( - &msgs, packet, imuFrameId_, rclcpp::Time(t, RCL_SYSTEM_TIME)); + (void)message_converter::convert_imu6_packet(&msgs, packet, imuFrameId_, rosBaseTime_); for (auto & msg : msgs) { imuPub_->publish(std::move(msg)); } diff --git a/src/libcaer_wrapper.cpp b/src/libcaer_wrapper.cpp index 1ba8b12..cfbd464 100644 --- a/src/libcaer_wrapper.cpp +++ b/src/libcaer_wrapper.cpp @@ -205,12 +205,12 @@ void LibcaerWrapper::processPacket( } case FRAME_EVENT: { const auto & fpacket = static_cast(packet); - callbackHandler_->framePacketCallback(nsSinceEpoch, fpacket); + callbackHandler_->framePacketCallback(fpacket); break; } case IMU6_EVENT: { const auto & fpacket = static_cast(packet); - callbackHandler_->imu6PacketCallback(nsSinceEpoch, fpacket); + callbackHandler_->imu6PacketCallback(fpacket); break; } default: