├── .gitignore ├── config ├── ids_xcpe.bias ├── silky_ev_cam.bias ├── trigger_pins.yaml └── trigger_pins_stereo.yaml ├── udev └── rules.d │ ├── 99-evkv2.rules │ ├── ca_device.rules │ └── 88-cyusb.rules ├── images ├── ros_event_camera.png └── time_offset_example.png ├── .flake8.ini ├── metavision_driver.repos ├── nodelet_plugins.xml ├── launch ├── sync_test.launch ├── driver_nodelet.launch ├── cyclone_dds_config.xml ├── recording_driver.launch ├── recorder_node.launch.py ├── recording_stereo_driver.launch ├── sync_test.launch.py ├── stereo_driver_node.launch ├── driver_node.launch ├── start_recording.launch.py ├── recording_driver.launch.py ├── driver_node.launch.py ├── recording_stereo_driver.launch.py ├── driver_composition.launch.py └── stereo_driver.launch.py ├── .clang-format ├── .github ├── workflows │ └── ci.yaml └── docker │ ├── Dockerfile.focal_noetic_galactic │ ├── Dockerfile.armv7_focal_noetic_metavision │ ├── Dockerfile.jetson_r34_noetic_metavision │ ├── Dockerfile.amd64_jammy_humble_openeb │ ├── Dockerfile.arm64v8_jammy_humble_openeb │ ├── Dockerfile.focal_noetic_galactic_metavision │ └── README.md ├── CONTRIBUTING.md ├── include └── metavision_driver │ ├── driver_ros1_base.h │ ├── check_endian.h │ ├── bias_parameter.h │ ├── callback_handler.h │ ├── resize_hack.h │ ├── driver_ros1.h │ ├── driver_ros2.h │ ├── ros_time_keeper.h │ ├── logging.h │ └── metavision_wrapper.h ├── src ├── driver_node_ros1.cpp ├── driver_node_ros2.cpp ├── stop_recording.py ├── start_recording.py ├── driver_nodelet_ros1.cpp ├── stop_recording_ros2.py ├── bias_parameter.cpp ├── test_time_stamps_ros1.py ├── test_time_stamps_ros2.py ├── driver_ros1.cpp └── driver_ros2.cpp ├── CMakeLists.txt ├── cfg └── MetaVisionDyn.cfg ├── package.xml ├── cmake ├── ROS2.cmake └── ROS1.cmake ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | launch/__pycache__/* 2 | -------------------------------------------------------------------------------- /config/ids_xcpe.bias: -------------------------------------------------------------------------------- 1 | 0 % bias_diff_off 2 | 0 % bias_diff_on 3 | 0 % bias_fo 4 | 0 % bias_hpf 5 | 0 % bias_refr 6 | -------------------------------------------------------------------------------- /udev/rules.d/99-evkv2.rules: -------------------------------------------------------------------------------- 1 | ACTION=="add", SUBSYSTEM=="usb", ATTR{idVendor}=="03fd", ATTR{idProduct}=="5832", MODE="0666" -------------------------------------------------------------------------------- /images/ros_event_camera.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-event-camera/metavision_driver/HEAD/images/ros_event_camera.png -------------------------------------------------------------------------------- /images/time_offset_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-event-camera/metavision_driver/HEAD/images/time_offset_example.png -------------------------------------------------------------------------------- /udev/rules.d/ca_device.rules: -------------------------------------------------------------------------------- 1 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="31f7", MODE="666", TAG="causb_dev" 2 | -------------------------------------------------------------------------------- /.flake8.ini: -------------------------------------------------------------------------------- 1 | [flake8] 2 | extend-ignore = Q000,D100,D103 3 | import-order-style = google 4 | max-line-length = 99 5 | show-source = true 6 | statistics = true 7 | -------------------------------------------------------------------------------- /config/silky_ev_cam.bias: -------------------------------------------------------------------------------- 1 | 299 % bias_diff 2 | 221 % bias_diff_off 3 | 384 % bias_diff_on 4 | 1477 % bias_fo 5 | 1499 % bias_hpf 6 | 1250 % bias_pr 7 | 1500 % bias_refr 8 | -------------------------------------------------------------------------------- /metavision_driver.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | event_camera_msgs: 3 | type: git 4 | url: https://github.com/ros-event-camera/event_camera_msgs.git 5 | version: master 6 | -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ROS metavision driver nodelet 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/sync_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | AccessModifierOffset: -2 6 | AlignAfterOpenBracket: AlwaysBreak 7 | BraceWrapping: 8 | AfterClass: true 9 | AfterFunction: true 10 | AfterNamespace: true 11 | AfterStruct: true 12 | BreakBeforeBraces: Custom 13 | ColumnLimit: 100 14 | ConstructorInitializerIndentWidth: 0 15 | ContinuationIndentWidth: 2 16 | DerivePointerAlignment: false 17 | PointerAlignment: Middle 18 | ReflowComments: false 19 | ... 20 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # continuous integration workflow 3 | # 4 | name: build repo 5 | 6 | on: 7 | push: 8 | branches: [ master] 9 | pull_request: 10 | branches: [ master] 11 | workflow_dispatch: 12 | branches: [ master] 13 | 14 | jobs: 15 | build_ros2: 16 | uses: ros-misc-utilities/ros_build_scripts/.github/workflows/ci_humble_and_later.yml@master 17 | with: 18 | repo: ${{ github.event.repository.name }} 19 | vcs_url: ${{ github.event.repository.name }}.repos 20 | -------------------------------------------------------------------------------- /udev/rules.d/88-cyusb.rules: -------------------------------------------------------------------------------- 1 | # Cypress USB driver for FX2 and FX3 (C) Cypress Semiconductor Corporation / ATR-LABS 2 | # Rules written by V. Radhakrishnan ( rk@atr-labs.com ) 3 | # Cypress USB vendor ID = 0x04b4 4 | # Note: The cy_renumerate.sh script is required only when using Cypress SDK cyusb_linux tool (to flash via USB) 5 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="04b4", MODE="666", TAG="cyusb_dev", RUN+="/usr/local/bin/cy_renumerate.sh A" 6 | KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="remove", TAG=="cyusb_dev", RUN+="/usr/local/bin/cy_renumerate.sh R" 7 | -------------------------------------------------------------------------------- /launch/driver_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /config/trigger_pins.yaml: -------------------------------------------------------------------------------- 1 | event_camera: # must match node name per ROS2 convention!!! 2 | ros__parameters: # must be present for ROS2 3 | prophesee_pin_config: 4 | evc3a_plugin_gen31: 5 | external: 0 6 | loopback: 6 7 | evc4a_plugin_imx636: 8 | external: 0 9 | loopback: 6 10 | hal_plugin_gen31_evk2: 11 | external: 0 12 | loopback: 3 13 | hal_plugin_gen31_evk3: 14 | external: 0 15 | loopback: 6 16 | hal_plugin_gen31_fx3: 17 | external: 0 18 | loopback: 6 19 | hal_plugin_gen3_fx3: 20 | external: 0 21 | loopback: 6 22 | hal_plugin_gen41_evk2: 23 | external: 1 24 | loopback: 3 25 | hal_plugin_gen4_evk2: 26 | external: 1 27 | loopback: 3 28 | hal_plugin_gen4_fx3: 29 | external: 1 30 | loopback: 6 31 | -------------------------------------------------------------------------------- /launch/cyclone_dds_config.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | auto 6 | default 7 | 65500B 8 | 4000B 9 | 10 | 11 | 12 | 600kB 13 | 14 | 15 | 16 | config 17 | stdout 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /.github/docker/Dockerfile.focal_noetic_galactic: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | LABEL maintainer="bernd.pfrommer@gmail.com" 3 | LABEL version="1.0" 4 | LABEL description="ros noetic + galactic image" 5 | ARG DEBIAN_FRONTEND=noninteractive 6 | RUN apt-get update 7 | 8 | # for add-apt-repository and to make debconf happy 9 | RUN apt-get -y install software-properties-common apt-utils 10 | 11 | # 12 | # install ROS2 galactic 13 | # 14 | RUN add-apt-repository universe 15 | RUN apt-get update && sudo apt-get -y install curl gnupg lsb-release 16 | RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 17 | RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null 18 | RUN apt-get update 19 | RUN apt-get -y install ros-galactic-desktop 20 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | 15 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 16 | line to commit messages to certify that they have the right to submit 17 | the code they are contributing to the project according to the 18 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 19 | -------------------------------------------------------------------------------- /include/metavision_driver/driver_ros1_base.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__DRIVER_ROS1_BASE_H_ 17 | #define METAVISION_DRIVER__DRIVER_ROS1_BASE_H_ 18 | 19 | namespace metavision_driver 20 | { 21 | class DriverROS1Base 22 | { 23 | public: 24 | DriverROS1Base() {} 25 | virtual ~DriverROS1Base() {} 26 | }; 27 | } // namespace metavision_driver 28 | #endif // METAVISION_DRIVER__DRIVER_ROS1_BASE_H_ 29 | -------------------------------------------------------------------------------- /src/driver_node_ros1.cpp: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "metavision_driver/driver_ros1.h" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | ros::init(argc, argv, "driver_node"); 23 | ros::NodeHandle pnh("~"); 24 | 25 | try { 26 | metavision_driver::DriverROS1 node(pnh); 27 | ros::spin(); 28 | } catch (const std::exception & e) { 29 | ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what()); 30 | return (-1); 31 | } 32 | return (0); 33 | } 34 | -------------------------------------------------------------------------------- /src/driver_node_ros2.cpp: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2021 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | #include 18 | 19 | #include "metavision_driver/driver_ros2.h" 20 | 21 | int main(int argc, char * argv[]) 22 | { 23 | rclcpp::init(argc, argv); 24 | auto node = std::make_shared(rclcpp::NodeOptions()); 25 | 26 | RCLCPP_INFO(node->get_logger(), "driver_node started up!"); 27 | // actually run the node 28 | rclcpp::spin(node); // should not return 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /launch/recording_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 10 | 11 | 12 | ["/event_camera/events"] 13 | 14 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/stop_recording.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # ----------------------------------------------------------------------------- 3 | # Copyright 2021 Bernd Pfrommer 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | 18 | import actionlib 19 | from nodelet_rosbag.msg import StopAction, StopGoal 20 | import rospy 21 | 22 | 23 | if __name__ == "__main__": 24 | rospy.init_node("stop_recording_client") 25 | print("sending command to stop recording!") 26 | client = actionlib.SimpleActionClient("stop", StopAction) 27 | client.wait_for_server() 28 | goal = StopGoal() 29 | client.send_goal(goal) 30 | client.wait_for_result(rospy.Duration.from_sec(5.0)) 31 | print("recording should be stopped now...") 32 | -------------------------------------------------------------------------------- /src/start_recording.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # ----------------------------------------------------------------------------- 3 | # Copyright 2021 Bernd Pfrommer 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | 18 | import actionlib 19 | from nodelet_rosbag.msg import StartAction, StartGoal 20 | import rospy 21 | 22 | 23 | if __name__ == "__main__": 24 | rospy.init_node("start_recording_client") 25 | print("sending command to start recording!") 26 | client = actionlib.SimpleActionClient("start", StartAction) 27 | client.wait_for_server() 28 | goal = StartGoal() 29 | client.send_goal(goal) 30 | client.wait_for_result(rospy.Duration.from_sec(5.0)) 31 | print("recording should be started now...") 32 | -------------------------------------------------------------------------------- /include/metavision_driver/check_endian.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__CHECK_ENDIAN_H_ 17 | #define METAVISION_DRIVER__CHECK_ENDIAN_H_ 18 | 19 | namespace metavision_driver 20 | { 21 | namespace check_endian 22 | { 23 | // check endianness 24 | inline bool isBigEndian() 25 | { 26 | const union { 27 | uint32_t i; 28 | char c[4]; 29 | } combined_int = {0x01020304}; // from stackoverflow 30 | return (combined_int.c[0] == 1); 31 | } 32 | } // namespace check_endian 33 | } // namespace metavision_driver 34 | #endif // METAVISION_DRIVER__CHECK_ENDIAN_H_ 35 | -------------------------------------------------------------------------------- /include/metavision_driver/bias_parameter.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2023 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__BIAS_PARAMETER_H_ 17 | #define METAVISION_DRIVER__BIAS_PARAMETER_H_ 18 | 19 | #include 20 | #include 21 | 22 | namespace metavision_driver 23 | { 24 | struct BiasParameter 25 | { 26 | BiasParameter(int nv, int xv, const std::string & inf) : minVal(nv), maxVal(xv), info(inf) {} 27 | int minVal; 28 | int maxVal; 29 | std::string info; 30 | 31 | static const std::map & getAll(const std::string & sensorVersion); 32 | }; 33 | } // namespace metavision_driver 34 | #endif // METAVISION_DRIVER__BIAS_PARAMETER_H_ 35 | -------------------------------------------------------------------------------- /src/driver_nodelet_ros1.cpp: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2021 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #include "metavision_driver/driver_ros1.h" 22 | 23 | namespace metavision_driver 24 | { 25 | class DriverNodeletROS1 : public nodelet::Nodelet 26 | { 27 | public: 28 | void onInit() override 29 | { 30 | nh_ = getPrivateNodeHandle(); 31 | driver_ = std::make_shared(nh_); 32 | } 33 | 34 | private: 35 | // ------ variables -------- 36 | std::shared_ptr driver_; 37 | ros::NodeHandle nh_; 38 | }; 39 | } // namespace metavision_driver 40 | 41 | #include 42 | PLUGINLIB_EXPORT_CLASS(metavision_driver::DriverNodeletROS1, nodelet::Nodelet) 43 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2021 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | cmake_minimum_required(VERSION 3.16) 17 | project(metavision_driver) 18 | 19 | # 20 | # figure out ROS1 vs ROS2 21 | # 22 | if(DEFINED ENV{ROS_VERSION}) 23 | set(ROS_VERSION "$ENV{ROS_VERSION}") 24 | message(STATUS "using env variable ROS_VERSION: ${ROS_VERSION}") 25 | else() 26 | find_package(catkin QUIET) 27 | if(catkin_FOUND) 28 | message(STATUS "found catkin package, must be ROS1!") 29 | set(ROS_VERSION "1") 30 | else() 31 | message(STATUS "catkin package not found, must be ROS2!") 32 | set(ROS_VERSION "2") 33 | endif() 34 | endif() 35 | 36 | if(ROS_VERSION EQUAL "2") 37 | include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) 38 | elseif(ROS_VERSION EQUAL "1") 39 | include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) 40 | else() 41 | message(SEND_ERROR "invalid ROS_VERSION: ${ROS_VERSION}") 42 | endif() 43 | -------------------------------------------------------------------------------- /include/metavision_driver/callback_handler.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2021 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__CALLBACK_HANDLER_H_ 17 | #define METAVISION_DRIVER__CALLBACK_HANDLER_H_ 18 | 19 | #if METAVISION_VERSION < 5 20 | #include 21 | #else 22 | #include 23 | #endif 24 | 25 | namespace metavision_driver 26 | { 27 | class CallbackHandler 28 | { 29 | public: 30 | CallbackHandler() {} 31 | virtual ~CallbackHandler() {} 32 | virtual void rawDataCallback(uint64_t t, const uint8_t * start, const uint8_t * end) = 0; 33 | virtual void eventCDCallback( 34 | uint64_t t, const Metavision::EventCD * start, const Metavision::EventCD * end) = 0; 35 | }; 36 | } // namespace metavision_driver 37 | #endif // METAVISION_DRIVER__CALLBACK_HANDLER_H_ 38 | -------------------------------------------------------------------------------- /launch/recorder_node.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2021 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | import launch 19 | from launch.actions import OpaqueFunction 20 | from launch_ros.actions import Node 21 | 22 | 23 | def launch_setup(context, *args, **kwargs): 24 | """Create simple node.""" 25 | node = Node( 26 | package="metavision_driver", 27 | # prefix=['xterm -e gdb -ex run --args'], 28 | executable="recorder_node", 29 | output="screen", 30 | name="recorder_node", 31 | parameters=[{"topics": ["/event_camera/events", "/image"], "base_name": "events_"}], 32 | remappings=[("~/events", "/events")], 33 | ) 34 | return [node] 35 | 36 | 37 | def generate_launch_description(): 38 | """Create simple node by calling opaque function.""" 39 | return launch.LaunchDescription([OpaqueFunction(function=launch_setup)]) 40 | -------------------------------------------------------------------------------- /.github/docker/Dockerfile.armv7_focal_noetic_metavision: -------------------------------------------------------------------------------- 1 | FROM arm32v7/ros:noetic-ros-base-focal 2 | LABEL maintainer="bernd.pfrommer@gmail.com" 3 | LABEL version="1.0" 4 | LABEL description="arm32v7 ubuntu 20.04 + ros noetic + metavision SDK (latest)" 5 | 6 | RUN apt-get update 7 | RUN apt-get -y install software-properties-common apt-utils python3-osrf-pycommon python3-catkin-tools libceres-dev python3-wstool iputils-ping wget libcanberra-gtk-module mesa-utils cmake libboost-program-options-dev libeigen3-dev libavcodec58 libavformat58 libswscale5 libswresample3 libavutil56 libusb-1.0-0 libpcre2-16-0 libdouble-conversion3 libxcb-xinput0 libxcb-xinerama0 build-essential unzip curl git libgtest-dev libboost-all-dev libusb-1.0-0-dev libglew-dev libglfw3-dev libcanberra-gtk-module ffmpeg libyaml-cpp-dev 8 | 9 | 10 | # 11 | # build image common which contains the required camera_info_manager 12 | # 13 | RUN bash -c 'source /opt/ros/noetic/setup.bash && mkdir -p /tmp/noetic-extras/src && cd /tmp/noetic-extras/src && git clone https://github.com/ros-perception/image_common.git && cd .. && catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/noetic && rm -rf /tmp/noetic-extras' 14 | 15 | # 16 | # compile openeb from source and install it 17 | # 18 | RUN mkdir /tmp/openeb && cd /tmp/openeb && git clone https://github.com/prophesee-ai/openeb.git && cd openeb && mkdir build && cd build && cmake .. -DCOMPILE_PYTHON3_BINDINGS=OFF -DBUILD_TESTING=OFF && cmake --build . --config Release -- -j 4 && cmake --build . --target install && rm -rf /tmp/openeb 19 | 20 | -------------------------------------------------------------------------------- /src/stop_recording_ros2.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # ----------------------------------------------------------------------------- 3 | # Copyright 2024 Bernd Pfrommer 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | 18 | import subprocess 19 | 20 | import rclpy 21 | 22 | 23 | def main(args=None): 24 | rclpy.init(args=args) 25 | cname = '/metavision_driver_container' 26 | output = subprocess.run(['ros2', 'component', 'list', cname], stdout=subprocess.PIPE) 27 | lines = output.stdout.decode('utf-8').splitlines() 28 | stopped = False 29 | for line in lines: 30 | a = line.split() 31 | if a[1] == '/recorder': 32 | output = subprocess.run( 33 | ['ros2', 'component', 'unload', cname, f'{a[0]}'], stdout=subprocess.PIPE 34 | ) 35 | stopped = True 36 | if stopped: 37 | print('recording stopped!') 38 | else: 39 | print('WARNING: recording not stopped because none was running!') 40 | 41 | 42 | if __name__ == '__main__': 43 | main() 44 | -------------------------------------------------------------------------------- /include/metavision_driver/resize_hack.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__RESIZE_HACK_H_ 17 | #define METAVISION_DRIVER__RESIZE_HACK_H_ 18 | namespace metavision_driver 19 | { 20 | // 21 | // resize without initializing, taken from 22 | // https://stackoverflow.com/questions/15219984/ 23 | // using-vectorchar-as-a-buffer-without-initializing-it-on-resize 24 | 25 | template 26 | void resize_hack(V & v, size_t newSize) 27 | { 28 | struct vt 29 | { 30 | typename V::value_type v; 31 | vt() {} 32 | }; 33 | static_assert(sizeof(vt[10]) == sizeof(typename V::value_type[10]), "alignment error"); 34 | typedef std::vector< 35 | vt, typename std::allocator_traits::template rebind_alloc> 36 | V2; 37 | reinterpret_cast(v).resize(newSize); 38 | } 39 | } // namespace metavision_driver 40 | #endif // METAVISION_DRIVER__RESIZE_HACK_H_ 41 | -------------------------------------------------------------------------------- /.github/docker/Dockerfile.jetson_r34_noetic_metavision: -------------------------------------------------------------------------------- 1 | FROM dustynv/ros:noetic-ros-base-l4t-r34.1.1 2 | LABEL maintainer="bernd.pfrommer@gmail.com" 3 | LABEL version="1.0" 4 | LABEL description="jetson ubuntu 18.04 + ros noetic + openeb (latest)" 5 | ARG ROS1_VERSION=noetic 6 | 7 | RUN apt-get update 8 | RUN apt-get -y install software-properties-common apt-utils python3-osrf-pycommon python3-catkin-tools libceres-dev python3-wstool iputils-ping wget libcanberra-gtk-module mesa-utils cmake libboost-program-options-dev libeigen3-dev libavcodec58 libavformat58 libswscale5 libswresample3 libavutil56 libusb-1.0-0 libpcre2-16-0 libdouble-conversion3 libxcb-xinput0 libxcb-xinerama0 build-essential unzip curl git libgtest-dev libboost-all-dev libusb-1.0-0-dev libglew-dev libglfw3-dev libcanberra-gtk-module ffmpeg libyaml-cpp-dev 9 | 10 | RUN apt-get -y install ros-${ROS1_VERSION}-pybind11-catkin 11 | 12 | # 13 | # build image common which contains the required camera_info_manager 14 | # 15 | RUN bash -c 'source /opt/ros/noetic/setup.bash && mkdir -p /tmp/noetic-extras/src && cd /tmp/noetic-extras/src && git clone https://github.com/ros-perception/image_common.git && cd .. && catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/noetic && rm -rf /tmp/noetic-extras' 16 | 17 | # 18 | # compile openeb from source and install it 19 | # 20 | RUN mkdir /tmp/openeb && cd /tmp/openeb && git clone https://github.com/prophesee-ai/openeb.git && cd openeb && mkdir build && cd build && cmake .. -DCOMPILE_PYTHON3_BINDINGS=OFF -DBUILD_TESTING=OFF && cmake --build . --config Release -- -j 4 && cmake --build . --target install && rm -rf /tmp/openeb 21 | 22 | -------------------------------------------------------------------------------- /config/trigger_pins_stereo.yaml: -------------------------------------------------------------------------------- 1 | event_cam_0: # must match node name per ROS2 convention!!! 2 | ros__parameters: # must be present for ROS2 3 | prophesee_pin_config: 4 | evc3a_plugin_gen31: 5 | external: 0 6 | loopback: 6 7 | evc4a_plugin_imx636: 8 | external: 0 9 | loopback: 6 10 | hal_plugin_gen31_evk2: 11 | external: 0 12 | loopback: 3 13 | hal_plugin_gen31_evk3: 14 | external: 0 15 | loopback: 6 16 | hal_plugin_gen31_fx3: 17 | external: 0 18 | loopback: 6 19 | hal_plugin_gen3_fx3: 20 | external: 0 21 | loopback: 6 22 | hal_plugin_gen41_evk2: 23 | external: 1 24 | loopback: 3 25 | hal_plugin_gen4_evk2: 26 | external: 1 27 | loopback: 3 28 | hal_plugin_gen4_fx3: 29 | external: 1 30 | loopback: 6 31 | event_cam_1: 32 | ros__parameters: # must be present for ROS2 33 | prophesee_pin_config: 34 | evc3a_plugin_gen31: 35 | external: 0 36 | loopback: 6 37 | evc4a_plugin_imx636: 38 | external: 0 39 | loopback: 6 40 | hal_plugin_gen31_evk2: 41 | external: 0 42 | loopback: 3 43 | hal_plugin_gen31_evk3: 44 | external: 0 45 | loopback: 6 46 | hal_plugin_gen31_fx3: 47 | external: 0 48 | loopback: 6 49 | hal_plugin_gen3_fx3: 50 | external: 0 51 | loopback: 6 52 | hal_plugin_gen41_evk2: 53 | external: 1 54 | loopback: 3 55 | hal_plugin_gen4_evk2: 56 | external: 1 57 | loopback: 3 58 | hal_plugin_gen4_fx3: 59 | external: 1 60 | loopback: 6 61 | -------------------------------------------------------------------------------- /.github/docker/Dockerfile.amd64_jammy_humble_openeb: -------------------------------------------------------------------------------- 1 | FROM ros:humble-perception-jammy 2 | LABEL maintainer="bernd.pfrommer@gmail.com" 3 | LABEL version="1.0" 4 | LABEL description="amd64 jammy/humble image with openeb" 5 | ARG DEBIAN_FRONTEND=noninteractive 6 | ARG ROS2_VERSION=humble 7 | 8 | RUN apt-get update 9 | 10 | # for add-apt-repository and to make debconf happy 11 | RUN apt-get -y install software-properties-common apt 12 | 13 | # 14 | # some additional ROS2 packages 15 | # 16 | # 17 | # some additional ROS2 packages 18 | # 19 | RUN apt-get -y install python3-osrf-pycommon ros-${ROS2_VERSION}-image-transport ros-${ROS2_VERSION}-cv-bridge ros-${ROS2_VERSION}-sensor-msgs ros-${ROS2_VERSION}-std-msgs ros-${ROS2_VERSION}-rosbag2* ros-${ROS2_VERSION}-tf2-ros ros-${ROS2_VERSION}-image-geometry ros-${ROS2_VERSION}-tf2-geometry-msgs ros-${ROS2_VERSION}-camera-info-manager ros-${ROS2_VERSION}-ament-clang-format ros-${ROS2_VERSION}-ament-cmake-clang-format ros-${ROS2_VERSION}-ament-cmake-* 20 | 21 | # 22 | # some additional ubuntu packages: 23 | # 24 | RUN apt-get -y install git libopencv-dev 25 | 26 | # 27 | # install openeb prerequisites 28 | # 29 | 30 | RUN apt-get -y install software-properties-common apt-utils python3-osrf-pycommon libceres-dev python3-wstool iputils-ping wget libcanberra-gtk-module mesa-utils cmake libboost-program-options-dev libeigen3-dev libavcodec58 libavformat58 libswscale5 libswresample3 libavutil56 libusb-1.0-0 libpcre2-16-0 libdouble-conversion3 libxcb-xinput0 libxcb-xinerama0 build-essential unzip curl git libgtest-dev libboost-all-dev libusb-1.0-0-dev libglew-dev libglfw3-dev libcanberra-gtk-module ffmpeg libyaml-cpp-dev 31 | 32 | # 33 | # compile openeb from source and install it 34 | # 35 | RUN mkdir /tmp/openeb && cd /tmp/openeb && git clone https://github.com/prophesee-ai/openeb.git && cd openeb && mkdir build && cd build && cmake .. -DCOMPILE_PYTHON3_BINDINGS=OFF -DBUILD_TESTING=OFF && cmake --build . --config Release -- -j 4 && cmake --build . --target install && rm -rf /tmp/openeb 36 | -------------------------------------------------------------------------------- /.github/docker/Dockerfile.arm64v8_jammy_humble_openeb: -------------------------------------------------------------------------------- 1 | FROM arm64v8/ros:humble-perception-jammy 2 | LABEL maintainer="bernd.pfrommer@gmail.com" 3 | LABEL version="1.0" 4 | LABEL description="arm64v8 jammy/humble image with openeb" 5 | ARG DEBIAN_FRONTEND=noninteractive 6 | ARG ROS2_VERSION=humble 7 | 8 | RUN apt-get update 9 | 10 | # for add-apt-repository and to make debconf happy 11 | RUN apt-get -y install software-properties-common apt 12 | 13 | # 14 | # some additional ROS2 packages 15 | # 16 | # 17 | # some additional ROS2 packages 18 | # 19 | RUN apt-get -y install python3-osrf-pycommon ros-${ROS2_VERSION}-image-transport ros-${ROS2_VERSION}-cv-bridge ros-${ROS2_VERSION}-sensor-msgs ros-${ROS2_VERSION}-std-msgs ros-${ROS2_VERSION}-rosbag2* ros-${ROS2_VERSION}-tf2-ros ros-${ROS2_VERSION}-image-geometry ros-${ROS2_VERSION}-tf2-geometry-msgs ros-${ROS2_VERSION}-camera-info-manager ros-${ROS2_VERSION}-ament-clang-format ros-${ROS2_VERSION}-ament-cmake-clang-format ros-${ROS2_VERSION}-ament-cmake-* 20 | 21 | # 22 | # some additional ubuntu packages: 23 | # 24 | RUN apt-get -y install git libopencv-dev 25 | 26 | # 27 | # install openeb prerequisites 28 | # 29 | 30 | RUN apt-get -y install software-properties-common apt-utils python3-osrf-pycommon libceres-dev python3-wstool iputils-ping wget libcanberra-gtk-module mesa-utils cmake libboost-program-options-dev libeigen3-dev libavcodec58 libavformat58 libswscale5 libswresample3 libavutil56 libusb-1.0-0 libpcre2-16-0 libdouble-conversion3 libxcb-xinput0 libxcb-xinerama0 build-essential unzip curl git libgtest-dev libboost-all-dev libusb-1.0-0-dev libglew-dev libglfw3-dev libcanberra-gtk-module ffmpeg libyaml-cpp-dev 31 | 32 | # 33 | # compile openeb from source and install it 34 | # 35 | RUN mkdir /tmp/openeb && cd /tmp/openeb && git clone https://github.com/prophesee-ai/openeb.git && cd openeb && mkdir build && cd build && cmake .. -DCOMPILE_PYTHON3_BINDINGS=OFF -DBUILD_TESTING=OFF && cmake --build . --config Release -- -j 4 && cmake --build . --target install && rm -rf /tmp/openeb 36 | -------------------------------------------------------------------------------- /launch/recording_stereo_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 9 | 10 | 11 | ["/event_cam_0/events", "/event_cam_1/events"] 12 | 13 | 14 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /launch/sync_test.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2022 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument as LaunchArg 20 | from launch.actions import OpaqueFunction 21 | from launch.substitutions import LaunchConfiguration as LaunchConfig 22 | from launch_ros.actions import Node 23 | 24 | 25 | def launch_setup(context, *args, **kwargs): 26 | """Create simple node.""" 27 | topic_0 = LaunchConfig("cam_0_topic") 28 | topic_1 = LaunchConfig("cam_1_topic") 29 | node = Node( 30 | package="metavision_driver", 31 | executable="sync_test", 32 | output="screen", 33 | # prefix=['xterm -e gdb -ex run --args'], 34 | name="sync_test", 35 | parameters=[{"use_ros_time": True}], 36 | remappings=[("~/events_cam_0", topic_0), ("~/events_cam_1", topic_1)], 37 | ) 38 | return [node] 39 | 40 | 41 | def generate_launch_description(): 42 | """Create simple node by calling opaque function.""" 43 | return launch.LaunchDescription( 44 | [ 45 | LaunchArg( 46 | "cam_0_topic", 47 | default_value=["/event_cam_0/events"], 48 | description="event topic cam 0", 49 | ), 50 | LaunchArg( 51 | "cam_1_topic", 52 | default_value=["/event_cam_1/events"], 53 | description="event topic cam 1", 54 | ), 55 | OpaqueFunction(function=launch_setup), 56 | ] 57 | ) 58 | -------------------------------------------------------------------------------- /.github/docker/Dockerfile.focal_noetic_galactic_metavision: -------------------------------------------------------------------------------- 1 | FROM berndpfrommer/focal_noetic_galactic 2 | LABEL maintainer="bernd.pfrommer@gmail.com" 3 | LABEL version="1.0" 4 | LABEL description="focal custom Docker Image for building ROS and ROS2 packages" 5 | ARG DEBIAN_FRONTEND=noninteractive 6 | ARG ROS1_VERSION=noetic 7 | ARG ROS2_VERSION=galactic 8 | 9 | # for add-apt-repository and to make debconf happy 10 | RUN apt-get -y install software-properties-common apt-utils 11 | 12 | # 13 | # some additional ROS1 packages 14 | # 15 | RUN apt-get -y install python3-osrf-pycommon python3-catkin-tools ros-${ROS1_VERSION}-image-transport ros-${ROS1_VERSION}-cv-bridge ros-${ROS1_VERSION}-sensor-msgs ros-${ROS1_VERSION}-std-msgs ros-${ROS1_VERSION}-rosbag ros-${ROS1_VERSION}-eigen-conversions ros-${ROS1_VERSION}-tf2-ros ros-${ROS1_VERSION}-image-geometry ros-${ROS1_VERSION}-tf2-geometry-msgs ros-${ROS1_VERSION}-message-generation ros-${ROS1_VERSION}-message-runtime ros-${ROS1_VERSION}-dynamic-reconfigure ros-${ROS1_VERSION}-camera-info-manager ros-${ROS1_VERSION}-pybind11-catkin 16 | 17 | 18 | # 19 | # some additional ROS2 packages 20 | # 21 | RUN apt-get -y install python3-osrf-pycommon python3-catkin-tools ros-${ROS2_VERSION}-image-transport ros-${ROS2_VERSION}-cv-bridge ros-${ROS2_VERSION}-sensor-msgs ros-${ROS2_VERSION}-std-msgs ros-${ROS2_VERSION}-rosbag2* ros-${ROS2_VERSION}-tf2-ros ros-${ROS2_VERSION}-image-geometry ros-${ROS2_VERSION}-tf2-geometry-msgs ros-${ROS2_VERSION}-camera-info-manager ros-${ROS2_VERSION}-ament-cmake-clang-format ros-${ROS2_VERSION}-ament-cmake-* 22 | 23 | # colcon 24 | RUN apt-get -y install python3-colcon-common-extensions 25 | 26 | # 27 | # some additional ubuntu packages: 28 | # 29 | # - git 30 | # - opencv 31 | # - libceres (for e.g. multicam-calibration) 32 | # 33 | RUN apt-get -y install git libopencv-dev libceres-dev 34 | 35 | # 36 | # install metavision SDK 37 | # 38 | # add metavision SDK repo 39 | RUN echo 'deb [arch=amd64 trusted=yes] https://apt.prophesee.ai/dists/public/7l58osgr/ubuntu focal essentials' > /etc/apt/sources.list.d/essentials.list 40 | RUN apt-get update 41 | 42 | # install some dependencies (may already be there) 43 | RUN apt-get -y install iputils-ping wget libcanberra-gtk-module mesa-utils cmake libboost-program-options-dev libeigen3-dev 44 | RUN apt-get -y install metavision-essentials 45 | -------------------------------------------------------------------------------- /launch/stereo_driver_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /launch/driver_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 20 | 21 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /src/bias_parameter.cpp: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2023 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "metavision_driver/bias_parameter.h" 17 | 18 | namespace metavision_driver 19 | { 20 | static const std::map> biasParameters{ 21 | {{"UNKNOWN", {}}, 22 | {"3.1", 23 | {{"bias_diff_on", {374, 499, "on threshold level"}}, 24 | {"bias_diff_off", {100, 234, "off threshold level"}}, 25 | {"bias_fo", {1250, 1800, "source follower low pass filter"}}, 26 | {"bias_hpf", {900, 1800, "differentiator high pass filter"}}, 27 | {"bias_pr", {0, 1800, "photoreceptor (frontend) bias"}}, 28 | {"bias_refr", {1300, 1800, "refractory time bias"}}}}, 29 | {"4.1", 30 | {{"bias_diff_on", {95, 140, "on threshold level"}}, 31 | {"bias_diff_off", {25, 65, "off threshold level"}}, 32 | {"bias_fo", {45, 110, "source follower low pass filter"}}, 33 | {"bias_hpf", {0, 120, "differentiator high pass filter"}}, 34 | {"bias_refr", {30, 100, "refractory time bias"}}}}, 35 | {"4.2", 36 | {{"bias_diff_on", {-85, 140, "on threshold level"}}, 37 | {"bias_diff_off", {-35, 190, "off threshold level"}}, 38 | {"bias_fo", {-35, 55, "source follower low pass filter"}}, 39 | {"bias_hpf", {0, 120, "differentiator high pass filter"}}, 40 | {"bias_refr", {-20, 235, "refractory time bias"}}}}}}; 41 | 42 | const std::map & BiasParameter::getAll( 43 | const std::string & sensorVersion) 44 | { 45 | const auto it = biasParameters.find(sensorVersion); 46 | if (it == biasParameters.end()) { 47 | return (biasParameters.find("UNKNOWN")->second); 48 | } 49 | return (it->second); 50 | } 51 | } // namespace metavision_driver 52 | -------------------------------------------------------------------------------- /.github/docker/README.md: -------------------------------------------------------------------------------- 1 | # How to create the docker images 2 | 3 | ## Build the base image (ROS1 and ROS2 combined) 4 | Change into the ``.github/docker`` directory and build the base image: 5 | ``` 6 | os_flavor=focal 7 | ros1_flavor=noetic 8 | ros2_flavor=galactic 9 | your_dockerhub_name= 10 | combined=${os_flavor}_${ros1_flavor}_${ros2_flavor} 11 | docker build -t ${your_dockerhub_name}/${combined} - < Dockerfile.${combined} 12 | ``` 13 | 14 | Upload the base image to dockerhub: 15 | ``` 16 | docker login 17 | docker push ${your_dockerhub_name}/${combined} 18 | ``` 19 | 20 | ## Build docker image with Metavision SDK and other missing packages 21 | 22 | If you do this from scratch: get the magic apt repo line by 23 | downloading the SDK from Prophesee's web site, then hack it into the 24 | ``_metavision`` image. 25 | 26 | Build and push image with remaining packages: 27 | ``` 28 | docker build -t ${your_dockerhub_name}/${combined}_metavision - < Dockerfile.${combined}_metavision 29 | docker push ${your_dockerhub_name}/${combined}_metavision 30 | ``` 31 | 32 | 33 | ## Build docker image for the Jetson 34 | 35 | For cross-compilation, i.e. running ARM v7, [first do this](https://medium.com/@Smartcow_ai/building-arm64-based-docker-containers-for-nvidia-jetson-devices-on-an-x86-based-host-d72cfa535786): 36 | ``` 37 | sudo apt-get install qemu binfmt-support qemu-user-static 38 | docker run --rm --privileged multiarch/qemu-user-static --reset -p yes 39 | ``` 40 | 41 | Build and push the container with OpenEB which now has the plugins as well: 42 | ``` 43 | docker build . -f Dockerfile.jetson_r34_noetic_metavision -t ${your_dockerhub_name}/jetson_r34_noetic_metavision 44 | docker push ${your_dockerhub_name}/jetson_r34_noetic_metavision 45 | ``` 46 | 47 | ## Build docker image for armv7 48 | 49 | For cross-compilation, i.e. running ARM v7, [first do this](https://medium.com/@Smartcow_ai/building-arm64-based-docker-containers-for-nvidia-jetson-devices-on-an-x86-based-host-d72cfa535786): 50 | ``` 51 | sudo apt-get install qemu binfmt-support qemu-user-static 52 | docker run --rm --privileged multiarch/qemu-user-static --reset -p yes 53 | ``` 54 | 55 | Build and push the container with OpenEB which now has the plugins as well: 56 | ``` 57 | docker build . -f Dockerfile.jetson_r34_noetic_metavision -t ${your_dockerhub_name}/jetson_r34_noetic_metavision 58 | docker push ${your_dockerhub_name}/jetson_r34_noetic_metavision 59 | ``` 60 | -------------------------------------------------------------------------------- /cfg/MetaVisionDyn.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | PACKAGE = "metavision_driver" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | # default values are for SilkyEVCam #0198 8 | 9 | # bias for on events (compared to bias_diff) 10 | # range from bias_diff + 1 to 1800 11 | gen.add("bias_diff_on", int_t, 0, "on threshold level", 350, -85, 499); 12 | 13 | # bias for off events (compared to bias_diff) 14 | # range from zero to bias_diff - 1 15 | gen.add("bias_diff_off", int_t, 0, "off threshold level", 384, -35, 298); 16 | 17 | # From metavision documentation: 18 | # 19 | # In Gen3, "bias_fo" is a voltage, in mV. Decreasing "bias_fo" value will increase the 20 | # bandwidth (make the sensor faster) and also increase the background noise. 21 | # Increasing "bias_fo" value will decrease both speed and noise 22 | 23 | # In Gen4, "bias_fo" is Idac value. Increasing "bias_fo" value will increase the bandwidth 24 | # (make the sensor faster) and also increase the background noise. Decreasing "bias_fo" 25 | # value will decrease both speed and noise 26 | 27 | gen.add("bias_fo", int_t, 0, "source follower low pass filter", 1477, -35, 1800); 28 | 29 | # 30 | # From metavision documentation: 31 | # 32 | # High-Pass Filter (bias_hpf) controls the strength of a High-Pass Filter that removes low 33 | # frequency noise (spontaneous background activity): 34 | # 35 | # In Gen3, With low "bias_hpf" values, high frequencies are filtered out, but if the 36 | # value is too low, the quality of the signal can be impacted. 37 | # 38 | # In Gen4, with high "bias_hpf" values, low frequencies are filtered out, but if the 39 | # value is too high, the quality of the signal can be impacted. 40 | 41 | gen.add("bias_hpf", int_t, 0, "differentiator high pass filter", 1499, 0, 1800); 42 | 43 | # 44 | # From metavision documentation: 45 | # 46 | # controls the front-end part of the pixel, the photoreceptor bandwidth. 47 | # High "bias_pr" value leads to small bias currents in the amplifier, hence limiting the 48 | # bandwidth of the photoreceptor. We recommend not changing this bias, unless very 49 | # specific conditions. 50 | gen.add("bias_pr", int_t, 0, "photoreceptor (frontend) bias", 1500, 0, 1800); 51 | 52 | # 53 | # From metavision documentation: 54 | # 55 | # sets the "refractory period" during which the Change Detector is switched 56 | # off after generating an event. This parameter controls the sampling rate of 57 | # the pixels and hence the output data rate of the sensor. 58 | 59 | gen.add("bias_refr", int_t, 0, "refractory time bias", -20, 0, 1800); 60 | exit(gen.generate(PACKAGE, "metavision_driver", "MetaVisionDyn")) 61 | -------------------------------------------------------------------------------- /launch/start_recording.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2024 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | from datetime import datetime 19 | 20 | from launch import LaunchDescription 21 | from launch.actions import DeclareLaunchArgument as LaunchArg 22 | from launch.actions import OpaqueFunction 23 | from launch.substitutions import LaunchConfiguration as LaunchConfig 24 | from launch_ros.actions import LoadComposableNodes 25 | from launch_ros.descriptions import ComposableNode 26 | 27 | 28 | def make_name(prefix, context): 29 | now = datetime.now() 30 | return prefix.perform(context) + now.strftime('%Y_%m_%d-%H_%M_%S') 31 | 32 | 33 | def launch_setup(context, *args, **kwargs): 34 | launch_action = LoadComposableNodes( 35 | target_container=LaunchConfig('container_name'), 36 | composable_node_descriptions=[ 37 | ComposableNode( 38 | package='rosbag2_transport', 39 | plugin='rosbag2_transport::Recorder', 40 | name='recorder', 41 | parameters=[ 42 | { 43 | 'record.topics': [['/event_camera/events']], 44 | 'record.start_paused': False, 45 | 'storage.uri': make_name(LaunchConfig('bag_prefix'), context), 46 | } 47 | ], 48 | extra_arguments=[{'use_intra_process_comms': True}], 49 | ), 50 | ], 51 | ) 52 | return [launch_action] 53 | 54 | 55 | def generate_launch_description(): 56 | """Create composable node by calling opaque function.""" 57 | return LaunchDescription( 58 | [ 59 | LaunchArg( 60 | 'container_name', 61 | default_value=['metavision_driver_container'], 62 | description='name of camera driver container node', 63 | ), 64 | LaunchArg( 65 | 'bag_prefix', 66 | default_value=['events_'], 67 | description='prefix of rosbag', 68 | ), 69 | OpaqueFunction(function=launch_setup), 70 | ] 71 | ) 72 | -------------------------------------------------------------------------------- /launch/recording_driver.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2021 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument as LaunchArg 20 | from launch.actions import OpaqueFunction 21 | from launch.substitutions import LaunchConfiguration as LaunchConfig 22 | from launch_ros.actions import ComposableNodeContainer 23 | from launch_ros.descriptions import ComposableNode 24 | 25 | 26 | def launch_setup(context, *args, **kwargs): 27 | """Create composable node.""" 28 | cam_name = LaunchConfig("camera_name") 29 | cam_str = cam_name.perform(context) 30 | container = ComposableNodeContainer( 31 | name="metavision_driver_container", 32 | namespace="", 33 | package="rclcpp_components", 34 | executable="component_container", 35 | # prefix=['xterm -e gdb -ex run --args'], 36 | composable_node_descriptions=[ 37 | ComposableNode( 38 | package="metavision_driver", 39 | plugin="metavision_driver::DriverROS2", 40 | name=cam_name, 41 | parameters=[ 42 | { 43 | "use_multithreading": False, 44 | "statistics_print_interval": 2.0, 45 | "camerainfo_url": "", 46 | "frame_id": "", 47 | "event_message_time_threshold": 1.0e-3, 48 | } 49 | ], 50 | remappings=[("~/events", cam_str + "/events")], 51 | extra_arguments=[{"use_intra_process_comms": True}], 52 | ), 53 | ComposableNode( 54 | package="rosbag2_composable_recorder", 55 | plugin="rosbag2_composable_recorder::ComposableRecorder", 56 | name="recorder", 57 | parameters=[ 58 | { 59 | "topics": ["/event_camera/events"], 60 | "bag_name": LaunchConfig("bag"), 61 | "bag_prefix": LaunchConfig("bag_prefix"), 62 | } 63 | ], 64 | remappings=[("~/events", cam_str + "/events")], 65 | extra_arguments=[{"use_intra_process_comms": True}], 66 | ), 67 | ], 68 | output="screen", 69 | ) 70 | return [container] 71 | 72 | 73 | def generate_launch_description(): 74 | """Create composable node by calling opaque function.""" 75 | return launch.LaunchDescription( 76 | [ 77 | LaunchArg("camera_name", default_value=["event_camera"], description="camera name"), 78 | LaunchArg("bag", default_value=[""], description="name of output bag"), 79 | LaunchArg("bag_prefix", default_value=["events_"], description="prefix of output bag"), 80 | OpaqueFunction(function=launch_setup), 81 | ] 82 | ) 83 | -------------------------------------------------------------------------------- /launch/driver_node.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2022 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument as LaunchArg 20 | from launch.actions import OpaqueFunction 21 | from launch.actions import SetEnvironmentVariable 22 | from launch.substitutions import LaunchConfiguration as LaunchConfig 23 | from launch_ros.actions import Node 24 | 25 | 26 | def launch_setup(context, *args, **kwargs): 27 | """Create simple node.""" 28 | cam_name = LaunchConfig("camera_name") 29 | node = Node( 30 | package="metavision_driver", 31 | executable="driver_node", 32 | output="screen", 33 | # prefix=['xterm -e gdb -ex run --args'], 34 | name=cam_name, 35 | parameters=[ 36 | # trigger_config, # loads the whole file 37 | { 38 | "use_multithreading": False, 39 | "send_queue_size": 1, 40 | "statistics_print_interval": 2.0, 41 | "bias_file": "", # bias_config, 42 | "camerainfo_url": "", 43 | "frame_id": "", 44 | "serial": LaunchConfig("serial"), 45 | "erc_mode": "disabled", 46 | # "erc_rate": 100000000, 47 | "trail_filter": False, 48 | "trail_filter_type": "stc_cut_trail", 49 | "trail_filter_threshold": 5000, 50 | # "roi": [0, 0, 320, 320], 51 | # "roni": False, 52 | # valid: 'external', 'loopback', 'disabled' 53 | "trigger_in_mode": "external", 54 | # valid: 'enabled', 'disabled' 55 | # "trigger_out_mode": "enabled", 56 | # "trigger_out_period": 100000, # in usec 57 | # "trigger_duty_cycle": 0.5, # fraction high/low 58 | "event_message_time_threshold": 1.0e-3, 59 | # "bias_diff_off": 0, 60 | # "bias_diff_on": 0, 61 | # "bias_hpf": 0, 62 | # "bias_fo": 0, 63 | # "bias_refr": 0, 64 | }, 65 | ], 66 | remappings=[], 67 | ) 68 | preload = SetEnvironmentVariable( 69 | name="LD_PRELOAD", value="/usr/lib/gcc/x86_64-linux-gnu/13/libasan.so" 70 | ) 71 | asan_options = SetEnvironmentVariable( 72 | name="ASAN_OPTIONS", value="new_delete_type_mismatch=0" 73 | ) 74 | return [preload, asan_options, node] 75 | 76 | 77 | def generate_launch_description(): 78 | """Create simple node by calling opaque function.""" 79 | return launch.LaunchDescription( 80 | [ 81 | LaunchArg( 82 | "camera_name", default_value=["event_camera"], description="camera name" 83 | ), 84 | LaunchArg( 85 | "serial", default_value=[""], description="serial number of camera" 86 | ), 87 | OpaqueFunction(function=launch_setup), 88 | ] 89 | ) 90 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | metavision_driver 5 | 1.0.0 6 | ROS1 and ROS2 drivers for metavision based event cameras 7 | Bernd Pfrommer 8 | Apache License 2.0 9 | 10 | 11 | catkin 12 | dynamic_reconfigure 13 | nodelet 14 | 15 | 16 | ament_cmake 17 | ament_cmake_auto 18 | ament_cmake_ros 19 | rclcpp 20 | rclcpp_components 21 | ament_cmake_copyright 22 | ament_cmake_cppcheck 23 | ament_cmake_cpplint 24 | ament_cmake_flake8 25 | ament_cmake_lint_cmake 26 | 28 | ament_cmake_xmllint 29 | ament_cmake_clang_format 30 | 31 | 35 | 36 | 37 | event_camera_msgs 38 | ros_environment 39 | std_srvs 40 | 41 | 42 | wget 43 | unzip 44 | curl 45 | git 46 | cmake 47 | hdf5-tools 48 | openeb_vendor 49 | boost 50 | libusb-1.0-dev 51 | libhdf5-dev 52 | libglew-dev 53 | libglfw3-dev 54 | ffmpeg 55 | libopencv-dev 56 | libopenscenegraph 57 | gtest 58 | 59 | 60 | 61 | 62 | catkin 63 | ament_cmake 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/test_time_stamps_ros1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # ----------------------------------------------------------------------------- 3 | # Copyright 2022 Bernd Pfrommer 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # 18 | """test code for event packet time stamp debugging.""" 19 | 20 | import argparse 21 | 22 | import matplotlib.pyplot as plt 23 | import numpy as np 24 | import rosbag 25 | 26 | 27 | def read_bag(args): 28 | print(f"opening bag {args.bag} ...") 29 | bag = rosbag.Bag(args.bag) 30 | print("iterating through messages...") 31 | t0_ros, t0_sensor = None, None 32 | t_last_evt = None 33 | ros_times, sensor_times, rec_times = [], [], [] 34 | 35 | for topic, msg, t_rec in bag.read_messages(topics=[args.topic]): 36 | # unpack time stamps for all events in the message 37 | packed = np.frombuffer(msg.events, dtype=np.uint64) 38 | dt = np.bitwise_and(packed, 0xFFFFFFFF) 39 | t_sensor = dt + msg.time_base 40 | t_ros = dt + msg.header.stamp.to_nsec() 41 | t_rec_nsec = t_rec.to_nsec() 42 | if not t_last_evt: 43 | t_last_evt = t_ros[0] - 1 44 | if not t0_ros: 45 | t0_ros = t_ros[0] 46 | t0_sensor = t_sensor[0] 47 | t0_rec = t_rec_nsec 48 | 49 | dt_msg = t_ros[0] - t_last_evt 50 | if dt_msg < 0: 51 | print("ERROR: timestamp going backward at time: ", t_ros[0]) 52 | if t_ros[-1] > t_rec_nsec: 53 | print( 54 | "WARN: timestamp from the future (can happen): ", 55 | f"{t_ros[0]} diff: {(t_ros[-1] - t_rec_nsec) * 1e-9}", 56 | ) 57 | t_last_evt = t_ros[-1] 58 | ros_times.append(t_ros[0] - t0_ros) 59 | sensor_times.append(t_sensor[0] - t0_sensor) 60 | rec_times.append(t_rec_nsec - t0_rec) 61 | 62 | return ( 63 | np.array(ros_times).astype(np.float), 64 | np.array(sensor_times).astype(np.float), 65 | np.array(rec_times).astype(np.float), 66 | ) 67 | 68 | 69 | if __name__ == "__main__": 70 | parser = argparse.ArgumentParser(description="examine ROS time stamps for event packet bag.") 71 | parser.add_argument( 72 | "--bag", 73 | "-b", 74 | action="store", 75 | default=None, 76 | required=True, 77 | help="bag file to read events from", 78 | ) 79 | parser.add_argument( 80 | "--topic", help="Event topic to read", default="/event_camera/events", type=str 81 | ) 82 | ros_times, sensor_times, rec_times = read_bag(parser.parse_args()) 83 | fig = plt.figure() 84 | ax = fig.add_subplot(111) 85 | ax.plot( 86 | ros_times * 1e-9, (ros_times - sensor_times) * 1e-9, "g", label="ros stamp - sensor time" 87 | ) 88 | ax.plot( 89 | ros_times * 1e-9, 90 | (rec_times - ros_times) * 1e-9, 91 | "r.", 92 | label="rec time - ros stamp", 93 | markersize=0.2, 94 | ) 95 | ax.set_xlabel("time [sec]") 96 | ax.set_ylabel("time differences [sec]") 97 | ax.legend() 98 | ax.set_ylim([-0.004, 0.005]) 99 | ax.set_title("time offsets to ROS message header stamps") 100 | plt.show() 101 | -------------------------------------------------------------------------------- /include/metavision_driver/driver_ros1.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__DRIVER_ROS1_H_ 17 | #define METAVISION_DRIVER__DRIVER_ROS1_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "metavision_driver/MetaVisionDynConfig.h" 28 | #include "metavision_driver/bias_parameter.h" 29 | #include "metavision_driver/callback_handler.h" 30 | #include "metavision_driver/resize_hack.h" 31 | 32 | namespace metavision_driver 33 | { 34 | class MetavisionWrapper; // forward decl 35 | 36 | class DriverROS1 : public CallbackHandler 37 | { 38 | using Config = MetaVisionDynConfig; 39 | using EventPacketMsg = event_camera_msgs::EventPacket; 40 | using Trigger = std_srvs::Trigger; 41 | 42 | public: 43 | explicit DriverROS1(ros::NodeHandle & nh); 44 | ~DriverROS1(); 45 | 46 | // ---------------- inherited from CallbackHandler ----------- 47 | void rawDataCallback(uint64_t t, const uint8_t * start, const uint8_t * end) override; 48 | void eventCDCallback( 49 | uint64_t t, const Metavision::EventCD * begin, const Metavision::EventCD * end) override; 50 | // ---------------- end of inherited ----------- 51 | 52 | private: 53 | // service call to dump biases 54 | bool saveBiases(Trigger::Request & req, Trigger::Response & res); 55 | 56 | // related to dynanmic config (runtime parameter update) 57 | void setBias(int * field, const std::string & name); 58 | int getBias(const std::string & name) const; 59 | 60 | void configure(Config & config, int level); 61 | 62 | // for primary sync 63 | bool secondaryReadyCallback(Trigger::Request & req, Trigger::Response & res); 64 | 65 | // misc helper functions 66 | void start(); 67 | bool stop(); 68 | void configureWrapper(const std::string & name); 69 | void initializeBiasParameters(const std::string & sensorVersion); 70 | // ------------------------ variables ------------------------------ 71 | ros::NodeHandle nh_; 72 | std::shared_ptr wrapper_; 73 | int width_; // image width 74 | int height_; // image height 75 | bool isBigEndian_; 76 | std::string frameId_; // ROS frame id 77 | std::string encoding_; 78 | uint64_t seq_{0}; // sequence number 79 | size_t reserveSize_{0}; // recommended reserve size 80 | uint64_t lastMessageTime_{0}; 81 | uint64_t messageThresholdTime_{0}; // threshold time for sending message 82 | size_t messageThresholdSize_{0}; // threshold size for sending message 83 | EventPacketMsg::Ptr msg_; 84 | ros::Publisher eventPub_; 85 | 86 | // ------ related to sync 87 | ros::ServiceServer secondaryReadyServer_; 88 | // ------ related to dynamic config and services 89 | Config config_; 90 | std::shared_ptr> configServer_; 91 | ros::ServiceServer saveBiasService_; 92 | using ParameterMap = std::map; 93 | ParameterMap biasParameters_; 94 | }; 95 | } // namespace metavision_driver 96 | #endif // METAVISION_DRIVER__DRIVER_ROS1_H_ 97 | -------------------------------------------------------------------------------- /include/metavision_driver/driver_ros2.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2021 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__DRIVER_ROS2_H_ 17 | #define METAVISION_DRIVER__DRIVER_ROS2_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "metavision_driver/bias_parameter.h" 28 | #include "metavision_driver/callback_handler.h" 29 | #include "metavision_driver/resize_hack.h" 30 | 31 | namespace metavision_driver 32 | { 33 | class MetavisionWrapper; // forward decl 34 | 35 | class DriverROS2 : public rclcpp::Node, public CallbackHandler 36 | { 37 | using EventPacketMsg = event_camera_msgs::msg::EventPacket; 38 | using Trigger = std_srvs::srv::Trigger; 39 | 40 | public: 41 | explicit DriverROS2(const rclcpp::NodeOptions & options); 42 | ~DriverROS2(); 43 | 44 | // ---------------- inherited from CallbackHandler ----------- 45 | void rawDataCallback(uint64_t t, const uint8_t * start, const uint8_t * end) override; 46 | void eventCDCallback( 47 | uint64_t t, const Metavision::EventCD * begin, const Metavision::EventCD * end) override; 48 | // ---------------- end of inherited ----------- 49 | 50 | private: 51 | // service calls 52 | void saveBiases( 53 | const std::shared_ptr request, 54 | const std::shared_ptr response); 55 | void saveSettings( 56 | const std::shared_ptr request, 57 | const std::shared_ptr response); 58 | 59 | // related to dynanmic config (runtime parameter update) 60 | rcl_interfaces::msg::SetParametersResult parameterChanged( 61 | const std::vector & params); 62 | void onParameterEvent(std::shared_ptr event); 63 | void addBiasParameter(const std::string & n, const BiasParameter & bp); 64 | void initializeBiasParameters(const std::string & sensorVersion); 65 | void declareBiasParameters(const std::string & sensorVersion); 66 | 67 | // misc helper functions 68 | void start(); 69 | bool stop(); 70 | void configureWrapper(const std::string & name); 71 | bool saveStuff(const std::string & fname, const std::shared_ptr response); 72 | 73 | // ------------------------ variables ------------------------------ 74 | std::shared_ptr wrapper_; 75 | int width_; // image width 76 | int height_; // image height 77 | bool isBigEndian_; 78 | std::string frameId_; 79 | std::string encoding_; 80 | uint64_t seq_{0}; // sequence number 81 | size_t reserveSize_{0}; // recommended reserve size 82 | uint64_t lastMessageTime_{0}; 83 | uint64_t messageThresholdTime_{0}; // threshold time for sending message 84 | size_t messageThresholdSize_{0}; // threshold size for sending message 85 | EventPacketMsg::UniquePtr msg_; 86 | rclcpp::Publisher::SharedPtr eventPub_; 87 | // ------ related to sync 88 | rclcpp::Service::SharedPtr secondaryReadyServer_; 89 | rclcpp::TimerBase::SharedPtr oneOffTimer_; 90 | // ------ related to dynamic config and services 91 | typedef std::map ParameterMap; 92 | rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callbackHandle_; 93 | std::shared_ptr>> 94 | parameterSubscription_; 95 | ParameterMap biasParameters_; 96 | rclcpp::Service::SharedPtr saveBiasesService_; 97 | rclcpp::Service::SharedPtr saveSettingsService_; 98 | }; 99 | } // namespace metavision_driver 100 | #endif // METAVISION_DRIVER__DRIVER_ROS2_H_ 101 | -------------------------------------------------------------------------------- /cmake/ROS2.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2021 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | set(CMAKE_CXX_STANDARD 14) 17 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 18 | set(CMAKE_CXX_EXTENSIONS OFF) 19 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 20 | 21 | # add_compile_options(-fsanitize=address) 22 | # add_link_options(-fsanitize=address) 23 | 24 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 25 | # metavision SDK header files produce warnings, switch off for now 26 | #add_compile_options(-Wall -Wextra -Wpedantic -Werror) 27 | add_compile_options(-Wall -Wextra -Wpedantic) 28 | endif() 29 | 30 | if(NOT CMAKE_BUILD_TYPE) 31 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 32 | endif() 33 | 34 | # there is no "driver" component for MV 5.x, but the MV 35 | # cmake file requires a COMPONENTS argument 36 | find_package(MetavisionSDK COMPONENTS driver QUIET) 37 | 38 | if(${MetavisionSDK_VERSION_MAJOR} LESS 5) 39 | set(MV_COMPONENTS driver) 40 | else() 41 | set(MV_COMPONENTS base core stream) 42 | endif() 43 | 44 | # now that we know the MV version, require the components 45 | find_package(MetavisionSDK COMPONENTS ${MV_COMPONENTS} REQUIRED) 46 | add_definitions(-DMETAVISION_VERSION=${MetavisionSDK_VERSION_MAJOR}) 47 | 48 | find_package(ament_cmake REQUIRED) 49 | find_package(ament_cmake_auto REQUIRED) 50 | find_package(ament_cmake_ros REQUIRED) 51 | 52 | 53 | set(ROS2_DEPENDENCIES 54 | "rclcpp" 55 | "rclcpp_components" 56 | "event_camera_msgs" 57 | "std_srvs" 58 | ) 59 | 60 | foreach(pkg ${ROS2_DEPENDENCIES}) 61 | find_package(${pkg} REQUIRED) 62 | endforeach() 63 | 64 | ament_auto_find_build_dependencies(REQUIRED ${ROS2_DEPENDENCIES}) 65 | 66 | # 67 | # --------- driver (composable component) ------------- 68 | 69 | ament_auto_add_library(driver_ros2 SHARED 70 | src/metavision_wrapper.cpp 71 | src/bias_parameter.cpp 72 | src/driver_ros2.cpp) 73 | 74 | set(MV_COMPONENTS_QUAL ${MV_COMPONENTS}) 75 | list(TRANSFORM MV_COMPONENTS_QUAL PREPEND "MetavisionSDK::") 76 | 77 | target_include_directories(driver_ros2 PRIVATE include) 78 | target_link_libraries(driver_ros2 ${MV_COMPONENTS_QUAL}) 79 | 80 | rclcpp_components_register_nodes(driver_ros2 "metavision_driver::DriverROS2") 81 | 82 | # --------- driver (plain old node) ------------- 83 | 84 | ament_auto_add_executable(driver_node 85 | src/driver_node_ros2.cpp) 86 | 87 | 88 | # the node must go into the project specific lib directory or else 89 | # the launch file will not find it 90 | install(TARGETS 91 | driver_node 92 | DESTINATION lib/${PROJECT_NAME}/) 93 | 94 | # the shared library goes into the global lib dir so it can 95 | # be used as a composable node by other projects 96 | 97 | install(TARGETS 98 | driver_ros2 99 | DESTINATION lib) 100 | 101 | install(PROGRAMS 102 | src/stop_recording_ros2.py 103 | DESTINATION lib/${PROJECT_NAME}/) 104 | 105 | install(DIRECTORY 106 | launch 107 | DESTINATION share/${PROJECT_NAME}/ 108 | FILES_MATCHING PATTERN "*.py") 109 | 110 | # install some example bias files 111 | install(DIRECTORY 112 | config 113 | DESTINATION share/${PROJECT_NAME}/) 114 | 115 | if(BUILD_TESTING) 116 | find_package(ament_cmake REQUIRED) 117 | find_package(ament_cmake_copyright REQUIRED) 118 | find_package(ament_cmake_cppcheck REQUIRED) 119 | find_package(ament_cmake_cpplint REQUIRED) 120 | find_package(ament_cmake_flake8 REQUIRED) 121 | find_package(ament_cmake_lint_cmake REQUIRED) 122 | # find_package(ament_cmake_pep257 REQUIRED) # (does not work on galactic/foxy) 123 | find_package(ament_cmake_xmllint REQUIRED) 124 | find_package(ament_cmake_clang_format REQUIRED) 125 | 126 | ament_copyright() 127 | ament_cppcheck(LANGUAGE c++) 128 | ament_cpplint(FILTERS "-build/include,-runtime/indentation_namespace") 129 | ament_flake8(--config ${CMAKE_CURRENT_SOURCE_DIR}/.flake8.ini) 130 | # ament_lint_cmake(--filter=-readability/wonkycase) 131 | ament_lint_cmake() 132 | # ament_pep257() # (does not work on galactic/foxy) 133 | ament_xmllint() 134 | ament_clang_format(CONFIG_FILE .clang-format) 135 | endif() 136 | 137 | ament_package() 138 | -------------------------------------------------------------------------------- /cmake/ROS1.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2021 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | include(FetchContent) 17 | 18 | find_package(MetavisionSDK COMPONENTS driver QUIET) 19 | 20 | # if metavision sdk is not present, download it into the build directory 21 | set(MUST_INSTALL_METAVISION FALSE) 22 | 23 | if(NOT MetavisionSDK_FOUND) 24 | message(STATUS "metavision SDK is not installed, must build it") 25 | # must set various variables for OpenEB *before* fetching openEB. 26 | # CMAKE_ARGS seems to not work for FetchContent_Declare() 27 | # set(COMPILE_3DVIEW OFF CACHE INTERNAL "Build 3d viewer") 28 | set(COMPILE_PLAYER OFF CACHE INTERNAL "Build player") 29 | set(COMPILE_PYTHON3_BINDINGS OFF CACHE INTERNAL "build python3 bindings") 30 | set(UDEV_RULES_SYSTEM_INSTALL OFF CACHE INTERNAL "install udev rules") 31 | 32 | # The following line must have zero indent. It disables the cmake linter 33 | # lint_cmake: -readability/wonkycase 34 | FetchContent_Declare( 35 | metavision 36 | GIT_REPOSITORY https://github.com/ros-event-camera/openeb.git 37 | GIT_TAG 4.2.0-ros) 38 | 39 | FetchContent_MakeAvailable(metavision) 40 | message(STATUS "metavision SDK fetched and made available") 41 | # do this to avoid the "install" target being run on the metavision sdk 42 | if(IS_DIRECTORY "${metavision_SOURCE_DIR}") 43 | set_property(DIRECTORY ${metavision_SOURCE_DIR} PROPERTY EXCLUDE_FROM_ALL YES) 44 | endif() 45 | # Why is this variable not set automatically ??? 46 | set(MetavisionSDK_VERSION_MAJOR 4) 47 | set(MUST_INSTALL_METAVISION TRUE) 48 | else() 49 | message(STATUS "metavision SDK is installed, not building it") 50 | endif() 51 | 52 | #add_compile_options(-Wall -Wextra -pedantic -Werror) 53 | add_compile_options(-Wall -Wextra -Wpedantic) 54 | #add_compile_definitions(USING_ROS_1) 55 | add_definitions(-DUSING_ROS_1) 56 | 57 | find_package(catkin REQUIRED COMPONENTS 58 | roscpp 59 | nodelet 60 | dynamic_reconfigure 61 | event_camera_msgs 62 | std_srvs) 63 | 64 | # MetavisionSDK is now found otherwise 65 | # find_package(MetavisionSDK COMPONENTS driver REQUIRED) 66 | 67 | add_definitions(-DMETAVISION_VERSION=${MetavisionSDK_VERSION_MAJOR}) 68 | 69 | generate_dynamic_reconfigure_options( 70 | cfg/MetaVisionDyn.cfg) 71 | 72 | include_directories( 73 | include 74 | ${catkin_INCLUDE_DIRS}) 75 | 76 | catkin_package(CATKIN_DEPENDS dynamic_reconfigure) 77 | 78 | # 79 | # --------- driver ------------- 80 | 81 | # code common to nodelet and node 82 | add_library(driver_common 83 | src/driver_ros1.cpp src/bias_parameter.cpp src/metavision_wrapper.cpp) 84 | target_link_libraries(driver_common MetavisionSDK::driver ${catkin_LIBRARIES}) 85 | # to ensure messages get built before executable 86 | add_dependencies(driver_common ${metavision_driver_EXPORTED_TARGETS}) 87 | 88 | # nodelet 89 | add_library(driver_nodelet src/driver_nodelet_ros1.cpp) 90 | target_link_libraries(driver_nodelet driver_common MetavisionSDK::driver ${catkin_LIBRARIES}) 91 | # to ensure messages get built before executable 92 | add_dependencies(driver_nodelet ${metavision_driver_EXPORTED_TARGETS}) 93 | 94 | # node 95 | add_executable(driver_node src/driver_node_ros1.cpp) 96 | target_link_libraries(driver_node driver_common MetavisionSDK::driver ${catkin_LIBRARIES}) 97 | # to ensure messages get built before executable 98 | add_dependencies(driver_node ${metavision_driver_EXPORTED_TARGETS}) 99 | 100 | 101 | ############# 102 | ## Install ## 103 | ############# 104 | 105 | install(TARGETS driver_node 106 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 107 | 108 | install(TARGETS driver_nodelet 109 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 110 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 111 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 112 | 113 | install(FILES nodelet_plugins.xml 114 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 115 | 116 | install(DIRECTORY launch 117 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 118 | FILES_MATCHING PATTERN "*.launch") 119 | 120 | # install some example bias files 121 | install(DIRECTORY 122 | config 123 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 124 | 125 | if(MUST_INSTALL_METAVISION) 126 | install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/_deps/metavision-build/lib" 127 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 128 | endif() 129 | 130 | 131 | ############# 132 | ## Testing ## 133 | ############# 134 | 135 | # To be done... 136 | -------------------------------------------------------------------------------- /launch/recording_stereo_driver.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2021 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | import os 19 | 20 | from ament_index_python.packages import get_package_share_directory 21 | import launch 22 | from launch.actions import DeclareLaunchArgument as LaunchArg 23 | from launch.actions import OpaqueFunction 24 | from launch.substitutions import LaunchConfiguration as LaunchConfig 25 | from launch_ros.actions import ComposableNodeContainer 26 | from launch_ros.descriptions import ComposableNode 27 | 28 | 29 | def launch_setup(context, *args, **kwargs): 30 | """Create composable node.""" 31 | cam_0_name = LaunchConfig("camera_0_name") 32 | cam_0_str = cam_0_name.perform(context) 33 | cam_1_name = LaunchConfig("camera_1_name") 34 | cam_1_str = cam_1_name.perform(context) 35 | pkg_name = "metavision_driver" 36 | share_dir = get_package_share_directory(pkg_name) 37 | bias_config = os.path.join(share_dir, "config", "silky_ev_cam.bias") # noqa F841 38 | # 39 | # camera 0 40 | # 41 | cam_0 = ComposableNode( 42 | package="metavision_driver", 43 | plugin="metavision_driver::DriverROS2", 44 | name=cam_0_name, 45 | parameters=[ 46 | { 47 | "use_multithreading": False, 48 | # 'bias_file': bias_config, 49 | "camerainfo_url": "", 50 | "frame_id": "cam_0", 51 | "serial": "CenturyArks:evc3a_plugin_gen31:00000198", 52 | "sync_mode": "primary", 53 | "event_message_time_threshold": 1.0e-3, 54 | } 55 | ], 56 | remappings=[ 57 | ("~/events", cam_0_str + "/events"), 58 | # must remap so primary listens to secondary's ready message 59 | ("~/ready", cam_1_str + "/ready"), 60 | ], 61 | extra_arguments=[{"use_intra_process_comms": True}], 62 | ) 63 | # 64 | # camera 1 65 | # 66 | cam_1 = ComposableNode( 67 | package="metavision_driver", 68 | plugin="metavision_driver::DriverROS2", 69 | name=cam_1_name, 70 | parameters=[ 71 | { 72 | "use_multithreading": False, 73 | # 'bias_file': bias_config, 74 | "camerainfo_url": "", 75 | "frame_id": "cam_1", 76 | "serial": "CenturyArks:evc3a_plugin_gen31:00000293", 77 | "sync_mode": "secondary", 78 | "event_message_time_threshold": 1.0e-3, 79 | } 80 | ], 81 | remappings=[("~/events", cam_1_str + "/events")], 82 | extra_arguments=[{"use_intra_process_comms": True}], 83 | ) 84 | # 85 | # recorder 86 | # 87 | recorder = ComposableNode( 88 | package="rosbag2_composable_recorder", 89 | plugin="rosbag2_composable_recorder::ComposableRecorder", 90 | name="recorder", 91 | parameters=[ 92 | { 93 | "topics": ["/" + cam_0_str + "/events", "/" + cam_1_str + "/events"], 94 | "bag_name": LaunchConfig("bag"), 95 | "bag_prefix": LaunchConfig("bag_prefix"), 96 | }, 97 | ], 98 | extra_arguments=[{"use_intra_process_comms": True}], 99 | ) 100 | # 101 | # the container for all 3 executables, avoid network messages 102 | # 103 | container = ComposableNodeContainer( 104 | name="metavision_driver_container", 105 | namespace="", 106 | package="rclcpp_components", 107 | executable="component_container", 108 | # prefix=['xterm -e gdb -ex run --args'], 109 | composable_node_descriptions=[cam_0, cam_1, recorder], 110 | output="screen", 111 | ) 112 | return [container] 113 | 114 | 115 | def generate_launch_description(): 116 | """Create composable node by calling opaque function.""" 117 | return launch.LaunchDescription( 118 | [ 119 | LaunchArg( 120 | "camera_0_name", 121 | default_value=["event_cam_0"], 122 | description="camera name of camera 0", 123 | ), 124 | LaunchArg( 125 | "camera_1_name", 126 | default_value=["event_cam_1"], 127 | description="camera name of camera 1", 128 | ), 129 | LaunchArg("bag", default_value=[""], description="name of output bag"), 130 | LaunchArg("bag_prefix", default_value=["events_"], description="prefix of output bag"), 131 | OpaqueFunction(function=launch_setup), 132 | ] 133 | ) 134 | -------------------------------------------------------------------------------- /src/test_time_stamps_ros2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # ----------------------------------------------------------------------------- 3 | # Copyright 2022 Bernd Pfrommer 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # 18 | """ 19 | Test code for event packet time stamp debugging. 20 | 21 | Some code snippets for rosbag reading were taken from 22 | https://github.com/ros2/rosbag2/blob/master/rosbag2_py/test/test_sequential_reader.py 23 | """ 24 | 25 | import argparse 26 | 27 | import matplotlib.pyplot as plt 28 | import numpy as np 29 | from rclpy.serialization import deserialize_message 30 | from rclpy.time import Time 31 | import rosbag2_py 32 | from rosidl_runtime_py.utilities import get_message 33 | 34 | 35 | def get_rosbag_options(path, serialization_format="cdr"): 36 | storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") 37 | 38 | converter_options = rosbag2_py.ConverterOptions( 39 | input_serialization_format=serialization_format, 40 | output_serialization_format=serialization_format, 41 | ) 42 | 43 | return storage_options, converter_options 44 | 45 | 46 | def read_bag(args): 47 | bag_path = str(args.bag) 48 | storage_options, converter_options = get_rosbag_options(bag_path) 49 | 50 | reader = rosbag2_py.SequentialReader() 51 | print(f"opening bag {args.bag}") 52 | reader.open(storage_options, converter_options) 53 | 54 | topic_types = reader.get_all_topics_and_types() 55 | 56 | # Create a map for quicker lookup 57 | type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} 58 | 59 | # Set filter for topic of string type 60 | storage_filter = rosbag2_py.StorageFilter(topics=[args.topic]) 61 | reader.set_filter(storage_filter) 62 | 63 | print("iterating through messages...") 64 | t0_ros, t0_sensor = None, None 65 | t_last_evt = None 66 | ros_times, sensor_times, rec_times = [], [], [] 67 | num_ts_future = 0 68 | 69 | while reader.has_next(): 70 | (topic, data, t_rec) = reader.read_next() 71 | msg_type = get_message(type_map[topic]) 72 | msg = deserialize_message(data, msg_type) 73 | # unpack time stamps for all events in the message 74 | packed = np.frombuffer(msg.events, dtype=np.uint64) 75 | dt = np.bitwise_and(packed, 0xFFFFFFFF) 76 | t_sensor = dt + msg.time_base 77 | 78 | t_ros = dt + Time.from_msg(msg.header.stamp).nanoseconds 79 | t_rec_nsec = t_rec 80 | if not t_last_evt: 81 | t_last_evt = t_ros[0] - 1 82 | if not t0_ros: 83 | t0_ros = t_ros[0] 84 | t0_sensor = t_sensor[0] 85 | t0_rec = t_rec_nsec 86 | 87 | dt_msg = t_ros[0] - t_last_evt 88 | if dt_msg < 0: 89 | print("ERROR: timestamp going backward at time: ", t_ros[0]) 90 | if t_ros[-1] > t_rec_nsec: 91 | num_ts_future += 1 92 | # print('WARN: timestamp from the future (can happen): ', 93 | # f'{t_ros[0]} diff: {(t_ros[-1] - t_rec_nsec) * 1e-9}') 94 | t_last_evt = t_ros[-1] 95 | ros_times.append(t_ros[0] - t0_ros) 96 | sensor_times.append(t_sensor[0] - t0_sensor) 97 | rec_times.append(t_rec_nsec - t0_rec) 98 | 99 | print( 100 | "fraction of messages with header stamp > recording stamp:", 101 | f"{num_ts_future * 100 / len(ros_times)}%", 102 | ) 103 | return ( 104 | np.array(ros_times).astype(np.float), 105 | np.array(sensor_times).astype(np.float), 106 | np.array(rec_times).astype(np.float), 107 | ) 108 | 109 | 110 | if __name__ == "__main__": 111 | parser = argparse.ArgumentParser(description="examine ROS time stamps for event packet bag.") 112 | parser.add_argument( 113 | "--bag", 114 | "-b", 115 | action="store", 116 | default=None, 117 | required=True, 118 | help="bag file to read events from", 119 | ) 120 | parser.add_argument( 121 | "--topic", help="Event topic to read", default="/event_camera/events", type=str 122 | ) 123 | ros_times, sensor_times, rec_times = read_bag(parser.parse_args()) 124 | fig = plt.figure() 125 | ax = fig.add_subplot(111) 126 | ax.plot( 127 | ros_times * 1e-9, (ros_times - sensor_times) * 1e-9, "g", label="ros stamp - sensor time" 128 | ) 129 | ax.plot( 130 | ros_times * 1e-9, 131 | (rec_times - ros_times) * 1e-9, 132 | "r.", 133 | label="rec time - ros stamp", 134 | markersize=0.2, 135 | ) 136 | ax.set_xlabel("time [sec]") 137 | ax.set_ylabel("time differences [sec]") 138 | ax.legend() 139 | ax.set_title("time offsets to ROS message header stamps") 140 | plt.show() 141 | -------------------------------------------------------------------------------- /launch/driver_composition.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2021 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | 19 | import launch 20 | from launch.actions import DeclareLaunchArgument as LaunchArg 21 | from launch.actions import OpaqueFunction 22 | from launch.actions import SetEnvironmentVariable 23 | from launch.conditions import IfCondition 24 | from launch.substitutions import LaunchConfiguration as LaunchConfig 25 | from launch_ros.actions import ComposableNodeContainer 26 | from launch_ros.descriptions import ComposableNode 27 | 28 | 29 | fixed_params = { 30 | "use_multithreading": False, 31 | "bias_file": "", 32 | "camerainfo_url": "", 33 | "frame_id": "", 34 | "event_message_time_threshold": 1.0e-3, 35 | } 36 | 37 | 38 | def make_renderer(camera): 39 | return ComposableNode( 40 | package="event_camera_renderer", 41 | plugin="event_camera_renderer::Renderer", 42 | name=camera + "_renderer", 43 | parameters=[{"fps": 25.0}], 44 | remappings=[("~/events", camera + "/events")], 45 | extra_arguments=[{"use_intra_process_comms": True}], 46 | ) 47 | 48 | 49 | def make_fibar(camera): 50 | return ComposableNode( 51 | package="event_image_reconstruction_fibar", 52 | plugin="event_image_reconstruction_fibar::Fibar", 53 | name=camera + "_fibar", 54 | parameters=[{"fps": 25.0}], 55 | remappings=[("~/events", camera + "/events")], 56 | extra_arguments=[{"use_intra_process_comms": True}], 57 | ) 58 | 59 | 60 | def make_camera(camera, params, remappings): 61 | return ComposableNode( 62 | package="metavision_driver", 63 | plugin="metavision_driver::DriverROS2", 64 | name=camera, 65 | parameters=params, 66 | remappings=remappings, 67 | extra_arguments=[{"use_intra_process_comms": True}], 68 | ) 69 | 70 | 71 | def launch_setup(context, *args, **kwargs): 72 | """Create composable node.""" 73 | camera = LaunchConfig("camera_name").perform(context) 74 | params = [ 75 | { 76 | "serial": LaunchConfig("serial").perform(context), 77 | "settings": LaunchConfig("settings").perform(context), 78 | } 79 | ] 80 | remappings = [] 81 | nodes = [make_camera(camera, params, remappings)] 82 | 83 | if IfCondition(LaunchConfig("with_renderer")).evaluate(context): 84 | nodes += [make_renderer(camera)] 85 | if IfCondition(LaunchConfig("with_fibar")).evaluate(context): 86 | nodes += [make_fibar(camera)] 87 | container = ComposableNodeContainer( 88 | name="metavision_driver_container", 89 | namespace="", 90 | package="rclcpp_components", 91 | executable="component_container_isolated", 92 | composable_node_descriptions=nodes, 93 | output="screen", 94 | ) 95 | 96 | debug_with_libasan = False 97 | if debug_with_libasan: 98 | preload = SetEnvironmentVariable( 99 | name="LD_PRELOAD", value="/usr/lib/gcc/x86_64-linux-gnu/13/libasan.so" 100 | ) 101 | asan_options = SetEnvironmentVariable( 102 | name="ASAN_OPTIONS", value="new_delete_type_mismatch=0" 103 | ) 104 | return [preload, asan_options, container] 105 | 106 | return [container] 107 | 108 | 109 | def generate_launch_description(): 110 | """Create composable node by calling opaque function.""" 111 | return launch.LaunchDescription( 112 | [ 113 | LaunchArg( 114 | "camera_name", 115 | default_value=["event_camera"], 116 | description="name of camera", 117 | ), 118 | LaunchArg( 119 | "serial", 120 | default_value=["4110030785"], 121 | description="serial number of camera", 122 | ), 123 | LaunchArg( 124 | "settings", 125 | default_value=[""], 126 | description="settings file for camera", 127 | ), 128 | LaunchArg( 129 | "fps", 130 | default_value=["fps"], 131 | description="renderer and fibar frame rate in Hz", 132 | ), 133 | LaunchArg( 134 | "with_renderer", 135 | default_value="false", 136 | description="if renderers should be started as well", 137 | ), 138 | LaunchArg( 139 | "with_fibar", 140 | default_value="false", 141 | description="if fibar reconstruction should be started as well", 142 | ), 143 | OpaqueFunction(function=launch_setup), 144 | ] 145 | ) 146 | -------------------------------------------------------------------------------- /launch/stereo_driver.launch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # Copyright 2025 Bernd Pfrommer 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument as LaunchArg 20 | from launch.actions import OpaqueFunction 21 | from launch.actions import SetEnvironmentVariable 22 | from launch.conditions import IfCondition 23 | from launch.substitutions import LaunchConfiguration as LaunchConfig 24 | from launch_ros.actions import ComposableNodeContainer 25 | from launch_ros.descriptions import ComposableNode 26 | 27 | 28 | common_params = { 29 | "use_multithreading": False, 30 | "bias_file": "", 31 | "camerainfo_url": "", 32 | "event_message_time_threshold": 1.0e-3, 33 | } 34 | 35 | 36 | def make_renderers(cameras): 37 | nodes = [ 38 | ComposableNode( 39 | package="event_camera_renderer", 40 | plugin="event_camera_renderer::Renderer", 41 | name=cam + "_renderer", 42 | parameters=[{"fps": 25.0}], 43 | remappings=[("~/events", cam + "/events")], 44 | extra_arguments=[{"use_intra_process_comms": True}], 45 | ) 46 | for cam in cameras 47 | ] 48 | return nodes 49 | 50 | 51 | def make_fibars(cameras): 52 | nodes = [ 53 | ComposableNode( 54 | package="event_image_reconstruction_fibar", 55 | plugin="event_image_reconstruction_fibar::Fibar", 56 | name=cam + "_fibar", 57 | parameters=[{"fps": 25.0}], 58 | remappings=[("~/events", cam + "/events")], 59 | extra_arguments=[{"use_intra_process_comms": True}], 60 | ) 61 | for cam in cameras 62 | ] 63 | return nodes 64 | 65 | 66 | def make_cameras(cameras, params): 67 | nodes = [ 68 | ComposableNode( 69 | package="metavision_driver", 70 | plugin="metavision_driver::DriverROS2", 71 | name=cam, 72 | parameters=[ 73 | common_params, 74 | {"serial": params[cam]["serial"]}, 75 | {"settings": params[cam]["settings"]}, 76 | {"sync_mode": params[cam]["sync_mode"]}, 77 | ], 78 | remappings=params[cam]["remappings"], 79 | extra_arguments=[{"use_intra_process_comms": True}], 80 | ) 81 | for cam in cameras 82 | ] 83 | return nodes 84 | 85 | 86 | def launch_setup(context, *args, **kwargs): 87 | """Create composable node.""" 88 | cam_0_name = LaunchConfig("camera_0_name") 89 | cam_0 = cam_0_name.perform(context) 90 | cam_1_name = LaunchConfig("camera_1_name") 91 | cam_1 = cam_1_name.perform(context) 92 | cameras = (cam_0, cam_1) 93 | specific_params = { 94 | cam_0: { 95 | "serial": LaunchConfig("camera_0_serial").perform(context), 96 | "settings": LaunchConfig("camera_0_settings").perform(context), 97 | "sync_mode": "primary", 98 | "remappings": [ 99 | ("~/events", cam_0 + "/events"), 100 | ("~/ready", cam_1 + "/ready"), 101 | ], 102 | }, 103 | cam_1: { 104 | "serial": LaunchConfig("camera_1_serial").perform(context), 105 | "settings": LaunchConfig("camera_1_settings").perform(context), 106 | "sync_mode": "secondary", 107 | "remappings": [("~/events", cam_1 + "/events")], 108 | }, 109 | } 110 | nodes = make_cameras(cameras, specific_params) 111 | if IfCondition(LaunchConfig("with_renderer")).evaluate(context): 112 | nodes += make_renderers(cameras) 113 | if IfCondition(LaunchConfig("with_fibar")).evaluate(context): 114 | nodes += make_fibars(cameras) 115 | container = ComposableNodeContainer( 116 | name="metavision_driver_container", 117 | namespace="", 118 | package="rclcpp_components", 119 | executable="component_container_isolated", 120 | composable_node_descriptions=nodes, 121 | output="screen", 122 | ) 123 | debug_with_libasan = False 124 | if debug_with_libasan: 125 | preload = SetEnvironmentVariable( 126 | name="LD_PRELOAD", value="/usr/lib/gcc/x86_64-linux-gnu/13/libasan.so" 127 | ) 128 | asan_options = SetEnvironmentVariable( 129 | name="ASAN_OPTIONS", value="new_delete_type_mismatch=0" 130 | ) 131 | return [preload, asan_options, container] 132 | return [container] 133 | 134 | 135 | def generate_launch_description(): 136 | """Create composable node by calling opaque function.""" 137 | return launch.LaunchDescription( 138 | [ 139 | LaunchArg( 140 | "camera_0_name", 141 | default_value=["event_cam_0"], 142 | description="camera name of camera 0", 143 | ), 144 | LaunchArg( 145 | "camera_1_name", 146 | default_value=["event_cam_1"], 147 | description="camera name of camera 1", 148 | ), 149 | LaunchArg( 150 | "camera_0_serial", 151 | default_value=["4110030785"], 152 | description="serial number of camera 0", 153 | ), 154 | LaunchArg( 155 | "camera_1_serial", 156 | default_value=["4110030791"], 157 | description="serial number of camera 1", 158 | ), 159 | LaunchArg( 160 | "camera_0_settings", 161 | default_value=[""], 162 | description="settings file for camera 0", 163 | ), 164 | LaunchArg( 165 | "camera_1_settings", 166 | default_value=[""], 167 | description="settings file for camera 1", 168 | ), 169 | LaunchArg( 170 | "fps", 171 | default_value=["fps"], 172 | description="renderer frame rate in Hz", 173 | ), 174 | LaunchArg( 175 | "with_renderer", 176 | default_value="false", 177 | description="if renderers should be started as well", 178 | ), 179 | LaunchArg( 180 | "with_fibar", 181 | default_value="false", 182 | description="if fibar reconstruction should be started as well", 183 | ), 184 | OpaqueFunction(function=launch_setup), 185 | ] 186 | ) 187 | -------------------------------------------------------------------------------- /include/metavision_driver/ros_time_keeper.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__ROS_TIME_KEEPER_H_ 17 | #define METAVISION_DRIVER__ROS_TIME_KEEPER_H_ 18 | 19 | #include // min/max 20 | #include 21 | #include 22 | #include 23 | 24 | #include "metavision_driver/logging.h" 25 | 26 | namespace metavision_driver 27 | { 28 | class ROSTimeKeeper 29 | { 30 | template 31 | inline static T clamp(const T & a, const T & low, const T & high) 32 | { 33 | return std::max(low, std::min(a, high)); 34 | } 35 | static inline int64_t clamp_buffering_delay(uint64_t rosT, uint64_t trialTime) 36 | { 37 | const int64_t MAX_BUFFER_DELAY = 2000000L; // 2 ms 38 | return ( 39 | clamp(static_cast(rosT) - static_cast(trialTime), 0L, MAX_BUFFER_DELAY)); 40 | } 41 | 42 | public: 43 | explicit ROSTimeKeeper(const std::string & name) : loggerName_(name) {} 44 | inline void setLastROSTime(uint64_t t) { lastROSTime_ = t; } 45 | inline uint64_t updateROSTimeOffset(double dt_sensor, uint64_t rosT) 46 | { 47 | if (rosT0_ == 0) { // first time here 48 | resetROSTime(rosT, dt_sensor); 49 | } 50 | if (dt_sensor < prevSensorTime_) { 51 | LOG_ERROR_NAMED("BAD! Sensor time going backwards, replug camera!"); 52 | } 53 | double sensor_inc = std::max(dt_sensor - prevSensorTime_, 0.0); 54 | const int64_t sensor_inc_int = static_cast(sensor_inc); 55 | prevSensorTime_ = dt_sensor; 56 | const int64_t ros_inc = static_cast(rosT) - static_cast(prevROSTime_); 57 | prevROSTime_ = rosT; 58 | // emergency reset if sensor time changed much more than ROS time (wall clock) 59 | // - sensor time changed more than 20% with respect to ros time 60 | const int64_t abs_change_diff = std::abs(sensor_inc_int - ros_inc); 61 | if (abs_change_diff * 20 > std::abs(ros_inc) && abs_change_diff > 20000000L) { 62 | LOG_WARN_NAMED( 63 | "sensor time jumped by " << sensor_inc * 1e-9 << "s vs wall clock: " << ros_inc * 1e-9 64 | << ", resetting ROS stamp time!"); 65 | sensor_inc = 0; 66 | resetROSTime(rosT, dt_sensor); 67 | } 68 | // compute time in seconds elapsed since ROS startup 69 | const double dt_ros = static_cast(rosT - rosT0_); 70 | // difference between elapsed ROS time and elapsed sensor Time 71 | const double dt = dt_ros - dt_sensor; 72 | // compute moving average of elapsed time difference. 73 | // the averaging constant alpha is picked such that the average is 74 | // taken over about 10 sec: 75 | // alpha = elapsed_time / 10sec 76 | constexpr double f = 1.0 / (10.0e9); 77 | const double alpha = clamp(sensor_inc * f, 0.0001, 0.1); 78 | averageTimeDifference_ = averageTimeDifference_ * (1.0 - alpha) + alpha * dt; 79 | // 80 | // We want to use sensor time, but adjust it for the average clock 81 | // skew between sensor time and ros time, plus some unknown buffering delay dt_buf 82 | // (to be estimated) 83 | // 84 | // t_ros_adj 85 | // = t_sensor + avg(t_ros - t_sensor) + dt_buf 86 | // = t_sensor_0 + dt_sensor + avg(t_ros_0 + dt_ros - (t_sensor_0 + dt_sensor)) + dt_buf 87 | // [now use t_sensor_0 and t_ros_0 == constant] 88 | // = t_ros_0 + avg(dt_ros - dt_sensor) + dt_sensor + dt_buf 89 | // =: ros_time_offset + dt_sensor; 90 | // 91 | // Meaning once ros_time_offset has been computed, the adjusted ros timestamp 92 | // is obtained by just adding the sensor elapsed time (dt_sensor) that is reported 93 | // by the SDK. 94 | 95 | const uint64_t dt_sensor_int = static_cast(dt_sensor); 96 | const int64_t avg_timediff_int = static_cast(averageTimeDifference_); 97 | const uint64_t MIN_EVENT_DELTA_T = 0LL; // minimum time gap between packets 98 | 99 | // First test if the new ros time stamp (trialTime) would be in future. If yes, then 100 | // the buffering delay has been underestimated and must be adjusted. 101 | 102 | const uint64_t trialTime = rosT0_ + avg_timediff_int + dt_sensor_int; 103 | 104 | if (rosT < trialTime + bufferingDelay_) { // time stamp would be in the future 105 | // compute buffering delay as clamped rosT - trialTime 106 | bufferingDelay_ = clamp_buffering_delay(rosT, trialTime); 107 | } 108 | 109 | // The buffering delay could make the time stamps go backwards. 110 | // Ensure that this does not happen. This safeguard may cause 111 | // time stamps to be (temporarily) in the future, there is no way around 112 | // that. 113 | if (trialTime + bufferingDelay_ < lastROSTime_ + MIN_EVENT_DELTA_T) { 114 | // compute buffering delay as clamped lastROSTime_ + MIN_EVENT_DELTA_T - trialTime 115 | bufferingDelay_ = clamp_buffering_delay(lastROSTime_ + MIN_EVENT_DELTA_T, trialTime); 116 | } 117 | 118 | // warn if the ros header stamp is off by more than 10ms vs current stamp 119 | const int64_t ros_tdiff = 120 | static_cast(rosT) - static_cast(trialTime) - bufferingDelay_; 121 | if (std::abs(ros_tdiff) > 10000000LL) { 122 | #ifdef USING_ROS_1 123 | LOG_WARN_NAMED_FMT_THROTTLE(5000, "ROS timestamp off by: %.2fms", ros_tdiff * 1e-6); 124 | #else 125 | rclcpp::Clock clock; 126 | RCLCPP_WARN_THROTTLE( 127 | rclcpp::get_logger(loggerName_), clock, 5000, "ROS timestamp off by: %.2fms", 128 | ros_tdiff * 1e-6); 129 | #endif 130 | } 131 | 132 | const uint64_t rosTimeOffset = rosT0_ + avg_timediff_int + bufferingDelay_; 133 | 134 | return (rosTimeOffset); 135 | } 136 | 137 | private: 138 | inline void resetROSTime(const uint64_t rosT, const double dt_sensor) 139 | { 140 | rosT0_ = rosT; 141 | // initialize to dt_ros - dt_sensor because dt_ros == 0 142 | averageTimeDifference_ = -dt_sensor; 143 | lastROSTime_ = rosT; 144 | prevROSTime_ = rosT; 145 | bufferingDelay_ = 0; 146 | prevSensorTime_ = dt_sensor; 147 | } 148 | 149 | // --------- variables 150 | std::string loggerName_; 151 | uint64_t rosT0_{0}; // time when first callback happened 152 | double averageTimeDifference_{0}; // average of elapsed_ros_time - elapsed_sensor_time 153 | double prevSensorTime_{0}; // sensor time during previous update 154 | int64_t bufferingDelay_{0}; // estimate of buffering delay 155 | uint64_t lastROSTime_{0}; // the last event's ROS time stamp 156 | uint64_t prevROSTime_{0}; // last time the ROS offset was updated 157 | }; 158 | 159 | } // namespace metavision_driver 160 | #endif // METAVISION_DRIVER__ROS_TIME_KEEPER_H_ 161 | -------------------------------------------------------------------------------- /include/metavision_driver/logging.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2020 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__LOGGING_H_ 17 | #define METAVISION_DRIVER__LOGGING_H_ 18 | #ifdef USING_ROS_1 19 | #include 20 | #define LOG_INFO_NAMED(...) \ 21 | { \ 22 | ROS_INFO_STREAM(__VA_ARGS__); \ 23 | } 24 | #define LOG_WARN_NAMED(...) \ 25 | { \ 26 | ROS_WARN_STREAM(__VA_ARGS__); \ 27 | } 28 | #define LOG_ERROR_NAMED(...) \ 29 | { \ 30 | ROS_ERROR_STREAM(__VA_ARGS__); \ 31 | } 32 | 33 | #define LOG_INFO_NAMED_FMT(...) \ 34 | { \ 35 | ROS_INFO(__VA_ARGS__); \ 36 | } 37 | #define LOG_WARN_NAMED_FMT(...) \ 38 | { \ 39 | ROS_WARN(__VA_ARGS__); \ 40 | } 41 | #define LOG_ERROR_NAMED_FMT(...) \ 42 | { \ 43 | ROS_ERROR(__VA_ARGS__); \ 44 | } 45 | 46 | #define LOG_INFO_NAMED_FMT_THROTTLE(...) \ 47 | { \ 48 | ROS_INFO_THROTTLE(__VA_ARGS__); \ 49 | } 50 | #define LOG_WARN_NAMED_FMT_THROTTLE(...) \ 51 | { \ 52 | ROS_WARN_THROTTLE(__VA_ARGS__); \ 53 | } 54 | #define LOG_ERROR_NAMED_FMT_THROTTLE(...) \ 55 | { \ 56 | ROS_ERROR_THROTTLE(__VA_ARGS__); \ 57 | } 58 | 59 | #else 60 | 61 | #include 62 | #include 63 | #include 64 | 65 | #define BOMB_OUT(...) \ 66 | { \ 67 | RCLCPP_ERROR_STREAM(get_logger(), __VA_ARGS__); \ 68 | std::stringstream SS; \ 69 | SS << __VA_ARGS__; \ 70 | throw(std::runtime_error(SS.str())); \ 71 | } 72 | #define BOMB_OUT_NODE(...) \ 73 | { \ 74 | RCLCPP_ERROR_STREAM(node_->get_logger(), __VA_ARGS__); \ 75 | std::stringstream SS; \ 76 | SS << __VA_ARGS__; \ 77 | throw(std::runtime_error(SS.str())); \ 78 | } 79 | #define LOG_INFO_NAMED(...) \ 80 | { \ 81 | RCLCPP_INFO_STREAM(rclcpp::get_logger(loggerName_), __VA_ARGS__); \ 82 | } 83 | #define LOG_WARN_NAMED(...) \ 84 | { \ 85 | RCLCPP_WARN_STREAM(rclcpp::get_logger(loggerName_), __VA_ARGS__); \ 86 | } 87 | #define LOG_ERROR_NAMED(...) \ 88 | { \ 89 | RCLCPP_ERROR_STREAM(rclcpp::get_logger(loggerName_), __VA_ARGS__); \ 90 | } 91 | 92 | #define LOG_INFO(...) \ 93 | { \ 94 | RCLCPP_INFO_STREAM(get_logger(), __VA_ARGS__); \ 95 | } 96 | #define LOG_WARN(...) \ 97 | { \ 98 | RCLCPP_WARN_STREAM(get_logger(), __VA_ARGS__); \ 99 | } 100 | #define LOG_ERROR(...) \ 101 | { \ 102 | RCLCPP_ERROR_STREAM(get_logger(), __VA_ARGS__); \ 103 | } 104 | 105 | #define LOG_INFO_NODE(...) \ 106 | { \ 107 | RCLCPP_INFO_STREAM(node_->get_logger(), __VA_ARGS__); \ 108 | } 109 | #define LOG_WARN_NODE(...) \ 110 | { \ 111 | RCLCPP_WARN_STREAM(node_->get_logger(), __VA_ARGS__); \ 112 | } 113 | #define LOG_ERROR_NODE(...) \ 114 | { \ 115 | RCLCPP_ERROR_STREAM(node_->get_logger(), __VA_ARGS__); \ 116 | } 117 | 118 | #define LOG_INFO_FMT(...) \ 119 | { \ 120 | RCLCPP_INFO(get_logger(), __VA_ARGS__); \ 121 | } 122 | #define LOG_WARN_FMT(...) \ 123 | { \ 124 | RCLCPP_WARN(get_logger(), __VA_ARGS__); \ 125 | } 126 | #define LOG_ERROR_FMT(...) \ 127 | { \ 128 | RCLCPP_ERROR(get_logger(), __VA_ARGS__); \ 129 | } 130 | 131 | #define LOG_INFO_NODE_FMT(...) \ 132 | { \ 133 | RCLCPP_INFO(node_->get_logger(), __VA_ARGS__); \ 134 | } 135 | #define LOG_WARN_NODE_FMT(...) \ 136 | { \ 137 | RCLCPP_WARN(node_->get_logger(), __VA_ARGS__); \ 138 | } 139 | #define LOG_ERROR_NODE_FMT(...) \ 140 | { \ 141 | RCLCPP_ERROR(node_->get_logger(), __VA_ARGS__); \ 142 | } 143 | #define LOG_INFO_NAMED_FMT(...) \ 144 | { \ 145 | RCLCPP_INFO(rclcpp::get_logger(loggerName_), __VA_ARGS__); \ 146 | } 147 | #define LOG_WARN_NAMED_FMT(...) \ 148 | { \ 149 | RCLCPP_WARN(rclcpp::get_logger(loggerName_), __VA_ARGS__); \ 150 | } 151 | #define LOG_ERROR_NAMED_FMT(...) \ 152 | { \ 153 | RCLCPP_ERROR(rclcpp::get_logger(loggerName_), __VA_ARGS__); \ 154 | } 155 | 156 | #define LOG_INFO_NODE_FMT_THROTTLE(...) \ 157 | { \ 158 | RCLCPP_INFO_THROTTLE(node_->get_logger(), *node_->get_clock(), __VA_ARGS__); \ 159 | } 160 | #define LOG_WARN_NODE_FMT_THROTTLE(...) \ 161 | { \ 162 | RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), __VA_ARGS__); \ 163 | } 164 | #define LOG_ERROR_NODE_FMT_THROTTLE(...) \ 165 | { \ 166 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), __VA_ARGS__); \ 167 | } 168 | 169 | #endif // USING_ROS_1 170 | 171 | #define BOMB_OUT_CERR(...) \ 172 | { \ 173 | std::cerr << __VA_ARGS__ << std::endl; \ 174 | std::stringstream SS; \ 175 | SS << __VA_ARGS__; \ 176 | throw(std::runtime_error(SS.str())); \ 177 | } 178 | 179 | #endif // METAVISION_DRIVER__LOGGING_H_ 180 | -------------------------------------------------------------------------------- /include/metavision_driver/metavision_wrapper.h: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2021 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef METAVISION_DRIVER__METAVISION_WRAPPER_H_ 17 | #define METAVISION_DRIVER__METAVISION_WRAPPER_H_ 18 | 19 | #if METAVISION_VERSION < 5 20 | #include 21 | #else 22 | #include 23 | #endif 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "metavision_driver/callback_handler.h" 36 | 37 | namespace ph = std::placeholders; 38 | 39 | // temporary feature to check for events outside ROI. To be removed 40 | // when related SDK/driver/camera issues have been resolved. 41 | // #define CHECK_IF_OUTSIDE_ROI 42 | 43 | namespace metavision_driver 44 | { 45 | class MetavisionWrapper 46 | { 47 | public: 48 | struct QueueElement 49 | { 50 | QueueElement() {} 51 | QueueElement(const void * s, size_t n, uint64_t t) : start(s), numBytes(n), timeStamp(t) {} 52 | // ----- variables 53 | const void * start{0}; 54 | size_t numBytes{0}; 55 | uint64_t timeStamp{0}; 56 | }; 57 | 58 | struct Stats 59 | { 60 | size_t msgsSent{0}; 61 | size_t msgsRecv{0}; 62 | size_t bytesSent{0}; 63 | size_t bytesRecv{0}; 64 | size_t maxQueueSize{0}; 65 | }; 66 | 67 | struct TrailFilter 68 | { 69 | bool enabled{false}; 70 | std::string type{"INVALID"}; 71 | uint32_t threshold{5000}; 72 | }; 73 | 74 | typedef std::map> HardwarePinConfig; 75 | 76 | explicit MetavisionWrapper(const std::string & loggerName); 77 | ~MetavisionWrapper(); 78 | 79 | int getBias(const std::string & name); 80 | bool hasBias(const std::string & name); 81 | int setBias(const std::string & name, int val); 82 | bool initialize(bool useMultithreading); 83 | bool saveBiases(); 84 | bool saveSettings(); 85 | inline void updateMsgsSent(int inc) 86 | { 87 | std::unique_lock lock(statsMutex_); 88 | stats_.msgsSent += inc; 89 | } 90 | inline void updateBytesSent(int inc) 91 | { 92 | std::unique_lock lock(statsMutex_); 93 | stats_.bytesSent += inc; 94 | } 95 | bool stop(); 96 | int getWidth() const { return (width_); } 97 | int getHeight() const { return (height_); } 98 | const std::string & getSerialNumber() const { return (serialNumber_); } 99 | const std::string & getSoftwareInfo() const { return (softwareInfo_); } 100 | const std::string & getExternalTriggerInMode() const { return (triggerInMode_); } 101 | const std::string & getSyncMode() const { return (syncMode_); } 102 | const std::string & getSensorVersion() const { return (sensorVersion_); } 103 | const std::string & getFromFile() const { return (fromFile_); } 104 | const std::string & getEncodingFormat() const { return (encodingFormat_); } 105 | 106 | void setSerialNumber(const std::string & sn) { serialNumber_ = sn; } 107 | void setFromFile(const std::string & f) { fromFile_ = f; } 108 | void setSyncMode(const std::string & sm) { syncMode_ = sm; } 109 | bool startCamera(CallbackHandler * h); 110 | void setLoggerName(const std::string & s) { loggerName_ = s; } 111 | void setStatisticsInterval(double sec) { statsInterval_ = sec; } 112 | void setEncodingFormat(const std::string & f) { encodingFormat_ = f; } 113 | void setBiasFile(const std::string & bf) { biasFile_ = bf; } 114 | void setSettingsFile(const std::string & s) { settingsFile_ = s; } 115 | 116 | // ROI is a double vector with length multiple of 4: 117 | // (x_top_1, y_top_1, width_1, height_1, 118 | // x_top_2, y_top_2, width_2, height_2, .....) 119 | void setROI(const std::vector & roi) { roi_ = roi; } 120 | void setRONI(bool useRONI) { useRONI_ = useRONI; } 121 | void setExternalTriggerInMode(const std::string & mode) { triggerInMode_ = mode; } 122 | void setExternalTriggerOutMode( 123 | const std::string & mode, const int period, const double duty_cycle); 124 | void setHardwarePinConfig(const HardwarePinConfig & config) { hardwarePinConfig_ = config; } 125 | void setEventRateController(const std::string & mode, const int rate) 126 | { 127 | ercMode_ = mode; 128 | ercRate_ = rate; 129 | } 130 | void setMIPIFramePeriod(int usec) { mipiFramePeriod_ = usec; } 131 | void setTrailFilter(const std::string & type, const uint32_t threshold, const bool state); 132 | 133 | bool triggerActive() const 134 | { 135 | return (triggerInMode_ != "disabled" || triggerOutMode_ != "disabled"); 136 | } 137 | bool triggerInActive() const { return (triggerInMode_ != "disabled"); } 138 | void setDecodingEvents(bool decodeEvents); 139 | 140 | private: 141 | bool initializeCamera(); 142 | bool openCamera(); 143 | bool loadSettings(); 144 | bool loadBiases(); 145 | void printBiases(); 146 | 147 | void runtimeErrorCallback(const Metavision::CameraException & e); 148 | void statusChangeCallback(const Metavision::CameraStatus & s); 149 | 150 | void rawDataCallback(const uint8_t * data, size_t size); 151 | void rawDataCallbackMultithreaded(const uint8_t * data, size_t size); 152 | void cdCallback(const Metavision::EventCD * start, const Metavision::EventCD * end); 153 | void extTriggerCallback( 154 | const Metavision::EventExtTrigger * start, const Metavision::EventExtTrigger * end); 155 | 156 | void processingThread(); 157 | void statsThread(); 158 | void applyROI(const std::vector & roi, bool roni); 159 | void applySyncMode(const std::string & mode); 160 | void configureExternalTriggers( 161 | const std::string & mode_in, const std::string & mode_out, const int period, 162 | const double duty_cycle); 163 | void configureEventRateController(const std::string & mode, const int rate); 164 | void activateTrailFilter(); 165 | void configureMIPIFramePeriod(int usec, const std::string & sensorName); 166 | void printStatistics(); 167 | // ------------ variables 168 | CallbackHandler * callbackHandler_{0}; 169 | Metavision::Camera cam_; 170 | Metavision::CallbackId statusChangeCallbackId_; 171 | bool statusChangeCallbackActive_{false}; 172 | Metavision::CallbackId runtimeErrorCallbackId_; 173 | bool runtimeErrorCallbackActive_{false}; 174 | Metavision::CallbackId rawDataCallbackId_; 175 | bool rawDataCallbackActive_{false}; 176 | Metavision::CallbackId contrastCallbackId_; 177 | bool contrastCallbackActive_{false}; 178 | Metavision::CallbackId extTriggerCallbackId_; 179 | bool extTriggerCallbackActive_{false}; 180 | int width_{0}; // image width 181 | int height_{0}; // image height 182 | std::string biasFile_; 183 | std::string settingsFile_; 184 | std::string serialNumber_; 185 | std::string fromFile_; 186 | std::string softwareInfo_; 187 | std::string syncMode_; 188 | std::string triggerInMode_; // disabled, enabled, loopback 189 | std::string triggerOutMode_; // disabled, enabled 190 | int triggerOutPeriod_; // period (in microseconds) of trigger out 191 | double triggerOutDutyCycle_; // duty cycle (fractional) of trigger out 192 | HardwarePinConfig hardwarePinConfig_; 193 | std::string ercMode_; 194 | int ercRate_; 195 | TrailFilter trailFilter_; 196 | int mipiFramePeriod_{-1}; 197 | std::string loggerName_{"driver"}; 198 | std::vector roi_; 199 | bool useRONI_{false}; 200 | std::string encodingFormat_{"EVT3"}; 201 | std::string sensorVersion_{"0.0"}; 202 | // -- related to statistics 203 | double statsInterval_{2.0}; // time between printouts 204 | std::chrono::time_point lastPrintTime_; 205 | Stats stats_; 206 | std::mutex statsMutex_; 207 | std::shared_ptr statsThread_; 208 | 209 | // ----------- 210 | // related to multi threading 211 | bool useMultithreading_{false}; 212 | std::mutex mutex_; 213 | std::condition_variable cv_; 214 | std::deque queue_; 215 | std::shared_ptr processingThread_; 216 | bool keepRunning_{true}; 217 | }; 218 | } // namespace metavision_driver 219 | #endif // METAVISION_DRIVER__METAVISION_WRAPPER_H_ 220 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /src/driver_ros1.cpp: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "metavision_driver/driver_ros1.h" 17 | 18 | #include 19 | 20 | #include "metavision_driver/check_endian.h" 21 | #include "metavision_driver/metavision_wrapper.h" 22 | 23 | namespace metavision_driver 24 | { 25 | namespace ph = std::placeholders; 26 | DriverROS1::DriverROS1(ros::NodeHandle & nh) : nh_(nh) 27 | { 28 | configureWrapper(ros::this_node::getName()); 29 | 30 | encoding_ = nh.param("encoding", "evt3"); 31 | if (encoding_ != "evt3") { 32 | ROS_ERROR_STREAM("invalid encoding: " << encoding_); 33 | throw std::runtime_error("invalid encoding!"); 34 | } 35 | messageThresholdTime_ = 36 | uint64_t(std::abs(nh_.param("event_message_time_threshold", 1e-3) * 1e9)); 37 | messageThresholdSize_ = 38 | static_cast(std::abs(nh_.param("event_message_size_threshold", 1024 * 1024))); 39 | 40 | eventPub_ = nh_.advertise("events", nh_.param("send_queue_size", 1000)); 41 | wrapper_->setEncodingFormat(encoding_); 42 | if (wrapper_->getSyncMode() == "primary") { 43 | // defer starting the primary until the secondary is up 44 | ros::ServiceClient client = nh_.serviceClient("ready"); 45 | Trigger trig; 46 | while (!client.call(trig)) { 47 | ROS_INFO_STREAM("waiting for secondary to come up..."); 48 | ros::Duration(0, 1000000000).sleep(); // sleep for a second 49 | } 50 | ROS_INFO_STREAM("secondary is up: " << trig.response.message); 51 | start(); // only now can this be started 52 | } else if (wrapper_->getSyncMode() == "secondary") { 53 | // on the secondary first start the camera before bringing 54 | // up the server 55 | start(); 56 | secondaryReadyServer_ = 57 | nh_.advertiseService("ready", &DriverROS1::secondaryReadyCallback, this); 58 | } else { // standalone mode 59 | start(); 60 | } 61 | } 62 | 63 | DriverROS1::~DriverROS1() 64 | { 65 | stop(); 66 | wrapper_.reset(); // invoke destructor 67 | } 68 | 69 | bool DriverROS1::saveBiases(Trigger::Request & req, Trigger::Response & res) 70 | { 71 | (void)req; 72 | res.success = false; 73 | if (wrapper_) { 74 | res.success = wrapper_->saveBiases(); 75 | } 76 | res.message += (res.success ? "succeeded" : "failed"); 77 | return (res.success); 78 | } 79 | 80 | int DriverROS1::getBias(const std::string & name) const 81 | { 82 | if (biasParameters_.find(name) != biasParameters_.end()) { 83 | return (wrapper_->getBias(name)); 84 | } 85 | return (0); 86 | } 87 | 88 | void DriverROS1::setBias(int * field, const std::string & name) 89 | { 90 | auto it = biasParameters_.find(name); 91 | if (it != biasParameters_.end()) { 92 | auto & bp = it->second; 93 | int val = std::min(std::max(*field, bp.minVal), bp.maxVal); 94 | if (val != *field) { 95 | ROS_WARN_STREAM(name << " must be between " << bp.minVal << " and " << bp.maxVal); 96 | } 97 | wrapper_->setBias(name, val); 98 | const int new_val = wrapper_->getBias(name); // read back 99 | *field = new_val; // feed back if not changed to desired value! 100 | } 101 | } 102 | 103 | void DriverROS1::configure(Config & config, int level) 104 | { 105 | if (level < 0) { // initial call 106 | // initialize config from current settings 107 | config.bias_diff_off = getBias("bias_diff_off"); 108 | config.bias_diff_on = getBias("bias_diff_on"); 109 | config.bias_fo = getBias("bias_fo"); 110 | config.bias_hpf = getBias("bias_hpf"); 111 | config.bias_pr = getBias("bias_pr"); 112 | config.bias_refr = getBias("bias_refr"); 113 | ROS_INFO("initialized config to camera biases"); 114 | } else { 115 | setBias(&config.bias_diff_off, "bias_diff_off"); 116 | setBias(&config.bias_diff_on, "bias_diff_on"); 117 | setBias(&config.bias_fo, "bias_fo"); 118 | setBias(&config.bias_hpf, "bias_hpf"); 119 | setBias(&config.bias_pr, "bias_pr"); 120 | setBias(&config.bias_refr, "bias_refr"); 121 | } 122 | config_ = config; // remember current values 123 | } 124 | 125 | bool DriverROS1::secondaryReadyCallback(Trigger::Request &, Trigger::Response & res) 126 | { 127 | res.success = true; 128 | res.message += "succeeded"; 129 | return (true); 130 | } 131 | 132 | void DriverROS1::start() 133 | { 134 | wrapper_->setStatisticsInterval(nh_.param("statistics_print_interval", 1.0)); 135 | if (!wrapper_->initialize( 136 | nh_.param("use_multithreading", false), nh_.param("bias_file", ""))) { 137 | ROS_ERROR("driver initialization failed!"); 138 | throw std::runtime_error("driver init failed!"); 139 | } 140 | 141 | if (wrapper_->getSyncMode() == "secondary") { 142 | ROS_INFO("secondary is decoding events..."); 143 | wrapper_->setDecodingEvents(true); 144 | } 145 | 146 | if (frameId_.empty()) { 147 | // default frame id to last 4 digits of serial number 148 | const auto sn = wrapper_->getSerialNumber(); 149 | frameId_ = (sn.size() > 4) ? sn.substr(sn.size() - 4) : std::string("event_cam"); 150 | } 151 | ROS_INFO_STREAM("using frame id: " << frameId_); 152 | 153 | if (wrapper_->getEncodingFormat() != encoding_) { 154 | ROS_ERROR_STREAM( 155 | "encoding mismatch, camera has: " << wrapper_->getEncodingFormat() << ", but expecting " 156 | << encoding_); 157 | throw std::runtime_error("encoding mismatch!"); 158 | } 159 | 160 | // ------ get other parameters from camera 161 | width_ = wrapper_->getWidth(); 162 | height_ = wrapper_->getHeight(); 163 | isBigEndian_ = check_endian::isBigEndian(); 164 | 165 | // ------ start camera, may get callbacks from then on 166 | wrapper_->startCamera(this); 167 | 168 | if (wrapper_->getFromFile().empty()) { 169 | initializeBiasParameters(wrapper_->getSensorVersion()); 170 | // hook up dynamic config server *after* the camera has 171 | // been initialized so we can read the bias values 172 | configServer_.reset(new dynamic_reconfigure::Server(nh_)); 173 | configServer_->setCallback(boost::bind(&DriverROS1::configure, this, _1, _2)); 174 | 175 | saveBiasService_ = nh_.advertiseService("save_biases", &DriverROS1::saveBiases, this); 176 | } 177 | } 178 | 179 | void DriverROS1::initializeBiasParameters(const std::string & sensorVersion) 180 | { 181 | biasParameters_ = BiasParameter::getAll(sensorVersion); 182 | if (biasParameters_.empty()) { 183 | ROS_WARN_STREAM("unknown sensor version " << sensorVersion << ", disabling tunable biases"); 184 | } 185 | } 186 | 187 | bool DriverROS1::stop() 188 | { 189 | if (wrapper_) { 190 | return (wrapper_->stop()); 191 | } 192 | return (false); 193 | } 194 | 195 | static MetavisionWrapper::HardwarePinConfig get_hardware_pin_config( 196 | const ros::NodeHandle & nh, std::string nn) 197 | { 198 | MetavisionWrapper::HardwarePinConfig config; 199 | XmlRpc::XmlRpcValue pin_config; 200 | // strip leading "/" from node name 201 | nn.erase(std::remove(nn.begin(), nn.end(), '/'), nn.end()); 202 | std::string path = nn + "/ros__parameters/prophesee_pin_config"; 203 | nh.getParam(path, pin_config); 204 | if (pin_config.getType() != XmlRpc::XmlRpcValue::TypeStruct) { 205 | ROS_ERROR_STREAM("got invalid config, no trigger parameter with name " << path); 206 | ROS_ERROR_STREAM("node name " << nn << " must match pin config yaml file!"); 207 | throw std::runtime_error("invalid hardware pin config!"); 208 | } 209 | for (const auto & pin_map : pin_config) { 210 | config.emplace(pin_map.first, std::map{}); 211 | for (auto & pin_entry : pin_map.second) { 212 | config[pin_map.first][pin_entry.first] = static_cast(pin_entry.second); 213 | } 214 | } 215 | return (config); 216 | } 217 | 218 | void DriverROS1::configureWrapper(const std::string & name) 219 | { 220 | wrapper_ = std::make_shared(name); 221 | wrapper_->setSerialNumber(nh_.param("serial", "")); 222 | wrapper_->setFromFile(nh_.param("from_file", "")); 223 | wrapper_->setSyncMode(nh_.param("sync_mode", "standalone")); 224 | auto roi = nh_.param>("roi", std::vector()); 225 | if (!roi.empty()) { 226 | ROS_INFO_STREAM("using ROI with " << (roi.size() / 4) << " rectangle(s)"); 227 | } 228 | wrapper_->setROI(roi); 229 | ROS_INFO_STREAM("sync mode: " << wrapper_->getSyncMode()); 230 | // disabled, enabled, loopback 231 | wrapper_->setExternalTriggerInMode(nh_.param("trigger_in_mode", "disabled")); 232 | // disabled, enabled 233 | wrapper_->setExternalTriggerOutMode( 234 | nh_.param("trigger_out_mode", "disabled"), 235 | nh_.param("trigger_out_period", 100000), // trigger out period in usec 236 | nh_.param("trigger_duty_cycle", 0.5)); // fraction of cycle that trigger is HIGH 237 | 238 | // disabled, enabled, na 239 | wrapper_->setEventRateController( 240 | nh_.param("erc_mode", "na"), // Event Rate Controller Mode 241 | nh_.param("erc_rate", 100000000)); // Event Rate Controller Rate 242 | 243 | wrapper_->setMIPIFramePeriod(nh_.param("mipi_frame_period", -1)); 244 | 245 | // Get information on external pin configuration per hardware setup 246 | if (wrapper_->triggerActive()) { 247 | wrapper_->setHardwarePinConfig(get_hardware_pin_config(nh_, ros::this_node::getName())); 248 | } 249 | } 250 | 251 | void DriverROS1::rawDataCallback(uint64_t t, const uint8_t * start, const uint8_t * end) 252 | { 253 | if (eventPub_.getNumSubscribers() != 0) { 254 | if (!msg_) { 255 | msg_.reset(new EventPacketMsg()); 256 | msg_->header.frame_id = frameId_; 257 | msg_->header.seq = seq_++; 258 | msg_->time_base = 0; // not used here 259 | msg_->encoding = encoding_; 260 | msg_->seq = msg_->header.seq; 261 | msg_->width = width_; 262 | msg_->height = height_; 263 | msg_->header.stamp = ros::Time().fromNSec(t); 264 | msg_->events.reserve(reserveSize_); 265 | } 266 | const size_t n = end - start; 267 | auto & events = msg_->events; 268 | const size_t oldSize = events.size(); 269 | resize_hack(events, oldSize + n); 270 | memcpy(reinterpret_cast(events.data() + oldSize), start, n); 271 | 272 | if (t - lastMessageTime_ > messageThresholdTime_ || events.size() > messageThresholdSize_) { 273 | reserveSize_ = std::max(reserveSize_, events.size()); 274 | wrapper_->updateBytesSent(events.size()); 275 | wrapper_->updateMsgsSent(1); 276 | eventPub_.publish(std::move(msg_)); 277 | lastMessageTime_ = t; 278 | msg_.reset(); 279 | } 280 | } else { 281 | if (msg_) { 282 | msg_.reset(); 283 | } 284 | } 285 | } 286 | 287 | void DriverROS1::eventCDCallback( 288 | uint64_t, const Metavision::EventCD * start, const Metavision::EventCD * end) 289 | { 290 | // This callback will only be exercised during startup on the 291 | // secondary until good data is available. The moment good time stamps 292 | // are available we disable decoding and use the raw interface. 293 | bool hasZeroTime(false); 294 | for (auto e = start; e != end; e++) { 295 | if (e->t == 0) { 296 | hasZeroTime = true; 297 | break; 298 | } 299 | } 300 | if (!hasZeroTime) { 301 | // finally the primary is up, no longer need the expensive decoding 302 | ROS_INFO("secondary sees primary up!"); 303 | wrapper_->setDecodingEvents(false); 304 | } 305 | } 306 | 307 | } // namespace metavision_driver 308 | -------------------------------------------------------------------------------- /src/driver_ros2.cpp: -------------------------------------------------------------------------------- 1 | // -*-c++-*--------------------------------------------------------------------------------------- 2 | // Copyright 2022 Bernd Pfrommer 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "metavision_driver/driver_ros2.h" 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "metavision_driver/check_endian.h" 27 | #include "metavision_driver/logging.h" 28 | #include "metavision_driver/metavision_wrapper.h" 29 | 30 | namespace metavision_driver 31 | { 32 | DriverROS2::DriverROS2(const rclcpp::NodeOptions & options) 33 | : Node( 34 | "metavision_driver", 35 | rclcpp::NodeOptions(options).automatically_declare_parameters_from_overrides(true)) 36 | { 37 | configureWrapper(get_name()); 38 | 39 | this->get_parameter_or("encoding", encoding_, std::string("evt3")); 40 | if (encoding_ != "evt3") { // Other encodings have no support yet from event_camera_codecs. 41 | LOG_ERROR("unsupported encoding: " << encoding_); 42 | throw std::runtime_error("invalid encoding!"); 43 | } 44 | double mtt; 45 | this->get_parameter_or("event_message_time_threshold", mtt, 1e-3); 46 | messageThresholdTime_ = uint64_t(std::abs(mtt) * 1e9); 47 | int64_t mts; 48 | this->get_parameter_or("event_message_size_threshold", mts, int64_t(1000000000)); 49 | messageThresholdSize_ = static_cast(std::abs(mts)); 50 | 51 | int qs; 52 | this->get_parameter_or("send_queue_size", qs, 1000); 53 | eventPub_ = this->create_publisher( 54 | "~/events", rclcpp::QoS(rclcpp::KeepLast(qs)).best_effort().durability_volatile()); 55 | 56 | if (wrapper_->getSyncMode() == "primary") { 57 | // delay primary until secondary is up and running 58 | // need to delay this to finish the constructor and release the thread 59 | oneOffTimer_ = this->create_wall_timer(std::chrono::seconds(1), [=]() { 60 | oneOffTimer_->cancel(); 61 | auto client = this->create_client("~/ready"); 62 | while (!client->wait_for_service(std::chrono::seconds(1))) { 63 | if (!rclcpp::ok()) { 64 | RCLCPP_ERROR(this->get_logger(), "Interrupted!"); 65 | throw std::runtime_error("interrupted!"); 66 | } 67 | RCLCPP_INFO(this->get_logger(), "primary waiting for secondary..."); 68 | } 69 | RCLCPP_INFO_STREAM(this->get_logger(), "secondary is up!"); 70 | start(); // only now can this be started 71 | }); 72 | } else if (wrapper_->getSyncMode() == "secondary") { 73 | start(); 74 | // creation of server signals to primary that we are ready. 75 | // We don't handle the callback 76 | secondaryReadyServer_ = this->create_service( 77 | "~/ready", 78 | [=](const std::shared_ptr, std::shared_ptr) {}); 79 | } else { 80 | // standalone mode 81 | start(); 82 | } 83 | } 84 | 85 | DriverROS2::~DriverROS2() 86 | { 87 | stop(); 88 | wrapper_.reset(); // invoke destructor 89 | } 90 | 91 | void DriverROS2::saveBiases( 92 | const std::shared_ptr, const std::shared_ptr response) 93 | { 94 | response->success = false; 95 | response->message = "bias file write "; 96 | if (wrapper_) { 97 | response->success = wrapper_->saveBiases(); 98 | } 99 | response->message += (response->success ? "succeeded" : "failed"); 100 | } 101 | 102 | void DriverROS2::saveSettings( 103 | const std::shared_ptr, const std::shared_ptr response) 104 | { 105 | response->success = false; 106 | response->message = "settings file write "; 107 | if (wrapper_) { 108 | response->success = wrapper_->saveSettings(); 109 | } 110 | response->message += (response->success ? "succeeded" : "failed"); 111 | } 112 | 113 | rcl_interfaces::msg::SetParametersResult DriverROS2::parameterChanged( 114 | const std::vector & params) 115 | { 116 | rcl_interfaces::msg::SetParametersResult res; 117 | res.successful = false; 118 | res.reason = "not set"; 119 | for (const auto & p : params) { 120 | const auto it = biasParameters_.find(p.get_name()); 121 | if (it != biasParameters_.end()) { 122 | if (wrapper_) { 123 | // TODO(Bernd): check value if possible and reject if out of bounds 124 | res.successful = true; 125 | res.reason = "successfully set"; 126 | } 127 | } else { 128 | res.successful = true; 129 | res.reason = "ignored unknown bias"; 130 | } 131 | } 132 | return (res); 133 | } 134 | 135 | void DriverROS2::onParameterEvent(std::shared_ptr event) 136 | { 137 | if (event->node != this->get_fully_qualified_name()) { 138 | return; 139 | } 140 | std::vector validEvents; 141 | for (auto it = biasParameters_.begin(); it != biasParameters_.end(); ++it) { 142 | validEvents.push_back(it->first); 143 | } 144 | // need to make copy to work around Foxy API 145 | auto ev = std::make_shared(*event); 146 | rclcpp::ParameterEventsFilter filter( 147 | ev, validEvents, {rclcpp::ParameterEventsFilter::EventType::CHANGED}); 148 | for (auto & it : filter.get_events()) { 149 | const std::string & name = it.second->name; 150 | const auto bp_it = biasParameters_.find(name); 151 | if (bp_it != biasParameters_.end()) { 152 | if (wrapper_) { 153 | // apply bias to SDK. The driver may adjust the parameter value! 154 | const int oldVal = wrapper_->getBias(name); 155 | const int val = it.second->value.integer_value; 156 | if (oldVal != val) { 157 | const int newVal = wrapper_->setBias(name, val); 158 | if (val != newVal) { 159 | // communicate adjusted value to ROS world 160 | this->set_parameter(rclcpp::Parameter(name, newVal)); 161 | } 162 | } 163 | } 164 | } 165 | } 166 | } 167 | 168 | std::vector split_string(const std::string & s) 169 | { 170 | std::stringstream ss(s); 171 | std::string tmp; 172 | std::vector words; 173 | while (getline(ss, tmp, '.')) { 174 | words.push_back(tmp); 175 | } 176 | return (words); 177 | } 178 | 179 | void DriverROS2::addBiasParameter(const std::string & name, const BiasParameter & bp) 180 | { 181 | if (wrapper_->hasBias(name)) { 182 | rcl_interfaces::msg::IntegerRange rg; 183 | rg.from_value = bp.minVal; 184 | rg.to_value = bp.maxVal; 185 | rg.step = 1; 186 | rcl_interfaces::msg::ParameterDescriptor pd; 187 | pd.name = name; 188 | pd.type = rclcpp::ParameterType::PARAMETER_INTEGER; 189 | pd.integer_range.push_back(rg); 190 | pd.description = bp.info; 191 | biasParameters_.insert(ParameterMap::value_type(name, pd)); 192 | } 193 | } 194 | 195 | void DriverROS2::initializeBiasParameters(const std::string & sensorVersion) 196 | { 197 | const auto map = BiasParameter::getAll(sensorVersion); 198 | if (map.empty()) { 199 | LOG_WARN("unknown sensor version " << sensorVersion << ", disabling tunable biases"); 200 | } else { 201 | for (const auto & p : map) { // loop through params 202 | addBiasParameter(p.first, p.second); 203 | } 204 | } 205 | } 206 | 207 | void DriverROS2::declareBiasParameters(const std::string & sensorVersion) 208 | { 209 | initializeBiasParameters(sensorVersion); 210 | for (const auto & p : biasParameters_) { 211 | const auto & name = p.first; 212 | try { 213 | const int defBias = wrapper_->getBias(name); 214 | try { 215 | this->declare_parameter(name, rclcpp::ParameterValue(defBias), p.second, false); 216 | LOG_INFO_FMT("%-20s value: %4d", name.c_str(), defBias); 217 | } catch (rclcpp::exceptions::InvalidParameterTypeException & e) { 218 | LOG_WARN("cannot declare parameter " << name << ": " << e.what()); 219 | } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { 220 | const rclcpp::Parameter val = this->get_node_parameters_interface()->get_parameter(name); 221 | if (val.as_int() != defBias) { 222 | LOG_INFO_FMT("setting bias %-20s to %4ld", name.c_str(), val.as_int()); 223 | wrapper_->setBias(name, val.as_int()); 224 | } else { 225 | LOG_INFO_FMT("%-20s value: %4d", name.c_str(), defBias); 226 | } 227 | } catch (const std::exception & e) { 228 | LOG_WARN("error thrown " << e.what()); 229 | } 230 | } catch (const std::runtime_error & e) { 231 | LOG_WARN("cannot get default bias for " << name << ", skipping it!"); 232 | } 233 | } 234 | } 235 | 236 | void DriverROS2::start() 237 | { 238 | // must wait with initialize() until all trigger params have been set 239 | wrapper_->setEncodingFormat(encoding_); 240 | bool useMT; 241 | this->get_parameter_or("use_multithreading", useMT, true); 242 | double printInterval; 243 | this->get_parameter_or("statistics_print_interval", printInterval, 1.0); 244 | wrapper_->setStatisticsInterval(printInterval); 245 | std::string biasFile; 246 | this->get_parameter_or("bias_file", biasFile, std::string("")); 247 | wrapper_->setBiasFile(biasFile); 248 | std::string settingsFile; 249 | this->get_parameter_or("settings", settingsFile, ""); 250 | wrapper_->setSettingsFile(settingsFile); 251 | 252 | if (!wrapper_->initialize(useMT)) { 253 | LOG_ERROR("driver initialization failed!"); 254 | throw std::runtime_error("driver initialization failed!"); 255 | } 256 | if (wrapper_->getSyncMode() == "secondary") { 257 | // For Gen3 the secondary will produce data with time stamps == 0 258 | // until it sees a clock signal. To filter out those events we 259 | // decode the events and wait until the time stamps are good. 260 | // Only then do we switch to "raw" mode. 261 | LOG_INFO("secondary is decoding events..."); 262 | wrapper_->setDecodingEvents(true); 263 | } 264 | 265 | if (frameId_.empty()) { 266 | // default frame id to last 4 digits of serial number 267 | const auto sn = wrapper_->getSerialNumber(); 268 | frameId_ = (sn.size() > 4) ? sn.substr(sn.size() - 4) : std::string("event_cam"); 269 | } 270 | LOG_INFO("using frame id: " << frameId_); 271 | 272 | if (wrapper_->getEncodingFormat() != encoding_) { 273 | LOG_ERROR( 274 | "encoding mismatch, camera has: " << wrapper_->getEncodingFormat() << ", but expecting " 275 | << encoding_); 276 | throw std::runtime_error("encoding mismatch!"); 277 | } 278 | 279 | // ------ get other parameters from camera 280 | width_ = wrapper_->getWidth(); 281 | height_ = wrapper_->getHeight(); 282 | isBigEndian_ = check_endian::isBigEndian(); 283 | 284 | // ------ start camera, may get callbacks from then on 285 | wrapper_->startCamera(this); 286 | 287 | if (wrapper_->getFromFile().empty()) { 288 | declareBiasParameters(wrapper_->getSensorVersion()); 289 | callbackHandle_ = this->add_on_set_parameters_callback( 290 | std::bind(&DriverROS2::parameterChanged, this, std::placeholders::_1)); 291 | parameterSubscription_ = rclcpp::AsyncParametersClient::on_parameter_event( 292 | this->get_node_topics_interface(), 293 | std::bind(&DriverROS2::onParameterEvent, this, std::placeholders::_1)); 294 | 295 | saveBiasesService_ = this->create_service( 296 | "~/save_biases", 297 | std::bind(&DriverROS2::saveBiases, this, std::placeholders::_1, std::placeholders::_2)); 298 | saveSettingsService_ = this->create_service( 299 | "~/save_settings", 300 | std::bind(&DriverROS2::saveSettings, this, std::placeholders::_1, std::placeholders::_2)); 301 | } 302 | } 303 | 304 | bool DriverROS2::stop() 305 | { 306 | if (wrapper_) { 307 | return (wrapper_->stop()); 308 | } 309 | return false; 310 | } 311 | 312 | static MetavisionWrapper::HardwarePinConfig get_hardware_pin_config(rclcpp::Node * node) 313 | { 314 | MetavisionWrapper::HardwarePinConfig config; 315 | // builds map, e.g: 316 | // config["evc3a_plugin_gen31"]["external"] = 0 317 | // config["evc3a_plugin_gen31"]["loopback"] = 6 318 | const auto params = node->list_parameters({"prophesee_pin_config"}, 10 /* 10 deep */); 319 | for (const auto & name : params.names) { 320 | auto a = split_string(name); 321 | if (a.size() != 3) { 322 | RCLCPP_ERROR_STREAM(node->get_logger(), "invalid pin config found: " << name); 323 | } else { 324 | int64_t pin; 325 | node->get_parameter(name, pin); 326 | auto it_bool = config.insert({a[1], std::map()}); 327 | it_bool.first->second.insert({a[2], static_cast(pin)}); 328 | } 329 | } 330 | return (config); 331 | } 332 | 333 | void DriverROS2::configureWrapper(const std::string & name) 334 | { 335 | wrapper_ = std::make_shared(name); 336 | std::string sn; 337 | this->get_parameter_or("serial", sn, std::string("")); 338 | wrapper_->setSerialNumber(sn); 339 | std::string fromFile; 340 | this->get_parameter_or("from_file", fromFile, std::string("")); 341 | wrapper_->setFromFile(fromFile); 342 | std::string syncMode; 343 | this->get_parameter_or("sync_mode", syncMode, std::string("standalone")); 344 | wrapper_->setSyncMode(syncMode); 345 | LOG_INFO("sync mode: " << syncMode); 346 | bool trailFilter; 347 | this->get_parameter_or("trail_filter", trailFilter, false); 348 | std::string trailFilterType; 349 | this->get_parameter_or("trail_filter_type", trailFilterType, std::string("trail")); 350 | int trailFilterThreshold; 351 | this->get_parameter_or("trail_filter_threshold", trailFilterThreshold, 0); 352 | if (trailFilter) { 353 | LOG_INFO( 354 | "Using tail filter in " << trailFilterType << " mode with threshold " 355 | << trailFilterThreshold); 356 | wrapper_->setTrailFilter( 357 | trailFilterType, static_cast(trailFilterThreshold), trailFilter); 358 | } 359 | std::vector roi_long; 360 | this->get_parameter_or("roi", roi_long, std::vector()); 361 | std::vector r(roi_long.begin(), roi_long.end()); 362 | if (!r.empty()) { 363 | LOG_INFO("using ROI with " << (r.size() / 4) << " rectangle(s)"); 364 | for (size_t i = 0; i < r.size() / 4; i += 4) { 365 | LOG_INFO(r[4 * i] << " " << r[4 * i + 1] << " " << r[4 * i + 2] << " " << r[4 * i + 3]); 366 | } 367 | } 368 | wrapper_->setROI(r); 369 | bool roni; 370 | this->get_parameter_or("roni", roni, false); 371 | wrapper_->setRONI(roni); 372 | std::string tInMode; 373 | this->get_parameter_or("trigger_in_mode", tInMode, std::string("disabled")); 374 | if (tInMode != "disabled") { 375 | LOG_INFO("trigger in mode: " << tInMode); 376 | } 377 | // disabled, enabled, loopback 378 | wrapper_->setExternalTriggerInMode(tInMode); 379 | // disabled, enabled 380 | std::string tOutMode; 381 | this->get_parameter_or("trigger_out_mode", tOutMode, std::string("disabled")); 382 | int64_t tOutPeriod; 383 | this->get_parameter_or("trigger_out_period", tOutPeriod, 100000L); // trigger out period in usec 384 | double tOutCycle; 385 | this->get_parameter_or("trigger_duty_cycle", tOutCycle, 0.5); // fraction of cycle trigger HIGH 386 | if (tOutMode != "disabled") { 387 | LOG_INFO("trigger out mode: " << tOutMode); 388 | LOG_INFO("trigger out period: " << tOutPeriod); 389 | LOG_INFO("trigger out duty cycle: " << tOutCycle); 390 | } 391 | wrapper_->setExternalTriggerOutMode(tOutMode, tOutPeriod, tOutCycle); 392 | 393 | // disabled, enabled, na 394 | std::string ercMode; // Event Rate Controller Mode 395 | this->get_parameter_or("erc_mode", ercMode, std::string("na")); 396 | int ercRate; // Event Rate Controller Rate 397 | this->get_parameter_or("erc_rate", ercRate, 100000000); 398 | wrapper_->setEventRateController(ercMode, ercRate); 399 | 400 | if (wrapper_->triggerActive()) { 401 | wrapper_->setHardwarePinConfig(get_hardware_pin_config(this)); 402 | } 403 | int mipiFramePeriod{-1}; 404 | this->get_parameter_or("mipi_frame_period", mipiFramePeriod, -1); 405 | wrapper_->setMIPIFramePeriod(mipiFramePeriod); 406 | } 407 | 408 | void DriverROS2::rawDataCallback(uint64_t t, const uint8_t * start, const uint8_t * end) 409 | { 410 | if (eventPub_->get_subscription_count() > 0) { 411 | if (!msg_) { 412 | msg_.reset(new EventPacketMsg()); 413 | msg_->header.frame_id = frameId_; 414 | msg_->time_base = 0; // not used here 415 | msg_->encoding = encoding_; 416 | msg_->seq = seq_++; 417 | msg_->width = width_; 418 | msg_->height = height_; 419 | msg_->header.stamp = rclcpp::Time(t, RCL_SYSTEM_TIME); 420 | msg_->events.reserve(reserveSize_); 421 | } 422 | const size_t n = end - start; 423 | auto & events = msg_->events; 424 | const size_t oldSize = events.size(); 425 | resize_hack(events, oldSize + n); 426 | memcpy(reinterpret_cast(events.data() + oldSize), start, n); 427 | 428 | if (t - lastMessageTime_ > messageThresholdTime_ || events.size() > messageThresholdSize_) { 429 | reserveSize_ = std::max(reserveSize_, events.size()); 430 | wrapper_->updateBytesSent(events.size()); 431 | eventPub_->publish(std::move(msg_)); 432 | lastMessageTime_ = t; 433 | wrapper_->updateMsgsSent(1); 434 | } 435 | } else { 436 | if (msg_) { 437 | msg_.reset(); 438 | } 439 | } 440 | } 441 | 442 | void DriverROS2::eventCDCallback( 443 | uint64_t, const Metavision::EventCD * start, const Metavision::EventCD * end) 444 | { 445 | // This callback will only be exercised during startup on the 446 | // secondary until good data is available. The moment good time stamps 447 | // are available we disable decoding and use the raw interface. 448 | bool hasZeroTime(false); 449 | for (auto e = start; e != end; e++) { 450 | if (e->t == 0) { 451 | hasZeroTime = true; 452 | break; 453 | } 454 | } 455 | if (!hasZeroTime) { 456 | // finally the primary is up, no longer need the expensive decoding 457 | LOG_INFO("secondary sees primary up!"); 458 | wrapper_->setDecodingEvents(false); 459 | } 460 | } 461 | 462 | } // namespace metavision_driver 463 | 464 | RCLCPP_COMPONENTS_REGISTER_NODE(metavision_driver::DriverROS2) 465 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # metavision_driver 2 | 3 | A ROS2 driver for event based cameras using Prophesee's Metavision SDK. 4 | This driver is not written or supported by Prophesee. 5 | 6 | ![banner image](images/ros_event_camera.png) 7 | 8 | If you are looking for more speed and features than the [official 9 | Prophesee ROS 10 | driver](https://github.com/prophesee-ai/prophesee_ros_wrapper) you 11 | have found the right repository. This driver can cope with the large amount of 12 | data produced by Prophesee's Gen3 and later sensors because it does 13 | little more than getting the RAW (currently EVT3 format) events from 14 | the camera and publishing them in ROS 15 | [event_camera_msgs](https://github.com/ros-event-camera/event_camera_msgs) 16 | format. 17 | 18 | The events can be decoded and displayed using the following ROS packages: 19 | 20 | - [event_camera_codecs](https://github.com/ros-event-camera/event_camera_codecs) 21 | has C++ routines to decode event_camera_msgs. 22 | - [event_camera_py](https://github.com/ros-event-camera/event_camera_py) 23 | module for fast event decoding in python. 24 | - [event_camera_renderer](https://github.com/ros-event-camera/event_camera_renderer) 25 | a node / nodelet that renders and publishes ROS image messages. 26 | - [event_camera_tools](https://github.com/ros-event-camera/event_camera_tools) 27 | a set of tools to echo, monitor performance and convert 28 | ``event_camera_msgs`` to legacy formats and into "RAW" format. 29 | 30 | ## Supported platforms 31 | 32 | Tested on ROS2 Humble and later with Metavision SDK (OpenEB) 5.0.0 33 | Notes: 34 | 35 | - Metavision 4.2.0 and 4.6.2 worked in previous versions but are no longer tested 36 | - ROS1 is EOL and no longer supported. There is however an 37 | unmaintained [ROS1 branch](https://github.com/ros-event-camera/metavision_driver/tree/ros1). 38 | 39 | Tested on the following hardware: 40 | 41 | - [IDS ueye XCP-E](https://www.ids-imaging.us/ueye-evs-cameras.html) 42 | - [SilkyEVCam VGA (Gen 3.1 sensor)](https://centuryarks.com/en/silkyevcam-vga/) 43 | - [SilkyEVCam HD (Gen 4 sensor)](https://centuryarks.com/en/silkyevcam-hd/) 44 | - [Prophesee EVK4 (Gen 4 sensor)](https://www.prophesee.ai/event-camera-evk4/) 45 | 46 | Explicitly not supported: any data in the old EVT2 format. The sensor 47 | must produce data in the EVT3 format. The new EVT4 format is not yet supported. 48 | 49 | ## Installation from binaries 50 | 51 | Install apt packages via 52 | 53 | ```bash 54 | sudo apt install ros-${ROS_DISTRO}-metavision-driver 55 | ``` 56 | 57 | This will also install a version of OpenEB under ``/opt/ros/${ROS_DISTRO}``, and 58 | the driver will preference those libraries over OpenEB/MetavisionSDK libraries installed 59 | elsewhere. But beware that paths are set up correctly for other applications such 60 | that they pick up the libraries they have been built against. 61 | 62 | On top of installing the driver binaries, you will have to install the right udev files and, 63 | (for non-Prophesee cameras) also the camera-specific plugins. 64 | 65 | - For Prophesee EVK cameras: Unless you also install OpenEB, install the required udev 66 | rules from the source tree directory ``udev/rules.d/``, and restart udev, 67 | see [OpenEB](https://github.com/prophesee-ai/openeb). 68 | 69 | - For the SilkyEV cameras, install the [plugin](https://centuryarks.com/en/download/) that 70 | matches the OpenEB version that the [ROS OpenEB vendor package](https://github.com/ros-event-camera/openeb_vendor/) 71 | is currently using (see the ``CMakeLists.txt`` file). The required udev files should be automatically installed by the 72 | plugin installer. Make 73 | - For the IDS cameras, download the plugin (a debian package) from the "Downloads" tab of the respective camera page, 74 | for instance for the [XCP-E camera](https://www.ids-imaging.us/store_us/products/cameras/ue-39b0xcp-e.html). Get the 75 | version that matches the one that the [ROS OpenEB vendor package](https://github.com/ros-event-camera/openeb_vendor/) 76 | is currently using (see the ``CMakeLists.txt`` file). The debian package however depends on the metavision-sdk-bin package. 77 | To avoid installing said package, extract the files from the plugin package, run the install scripts for the 78 | udev files and plugins, and copy the plugins into the right location: 79 | 80 | ```bash 81 | dpkg-deb -x ./ueye-evs_5.0.0.3_amd64.deb ./ids # extract all files in deb package into "ids" directory 82 | sudo ./ids/usr/lib/ids/ueye_evs/scripts/add_udev_rules.sh 83 | sudo ./ids/usr/lib/ids/ueye_evs/scripts/add_ueye_evs_users_group.sh 84 | sudo mkdir -p /usr/lib/ids/ueye_evs/hal/plugins 85 | sudo cp ./ids/usr/lib/ids/ueye_evs/hal/plugins/lib*.so /usr/lib/ids/ueye_evs/hal/plugins/ 86 | ``` 87 | 88 | Finally, make sure that the ``MV_HAL_PLUGIN_PATH`` is pointing to the plugins: 89 | 90 | ```bash 91 | export MV_HAL_PLUGIN_PATH=/usr/lib/CenturyArks/hal/plugins:/usr/lib/ids/ueye_evs/hal/plugins 92 | ``` 93 | 94 | If you can't get the binary installation working, install OpenEB and the plugins, and build the ROS driver from source, without 95 | using the openeb_vendor package. The ROS metavision driver should recognize that OpenEB is installed, and compile against 96 | those header files and libraries. 97 | 98 | ## How to build from source 99 | 100 | Prerequisites: 101 | 102 | - install [OpenEB](https://github.com/prophesee-ai/openeb) 103 | - install ``vcs`` (Ubuntu package ``python3-vcstool``). 104 | 105 | Make sure you have your ROS2 environment sourced such that ROS_VERSION is set. 106 | Create a workspace (``metavision_driver_ws``), clone this repo, and use ``vcs`` 107 | to pull in the remaining dependencies: 108 | 109 | ```bash 110 | pkg=metavision_driver 111 | mkdir -p ~/${pkg}_ws/src 112 | cd ~/${pkg}_ws 113 | git clone https://github.com/ros-event-camera/metavision_driver.git src/${pkg} 114 | cd src 115 | vcs import < ${pkg}/${pkg}.repos 116 | cd .. 117 | ``` 118 | 119 | Now configure and build: 120 | 121 | ```bash 122 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_EXPORT_COMPILE_COMMANDS=ON 123 | . install/setup.bash 124 | ``` 125 | 126 | ## Driver Features 127 | 128 | This driver differs from the Prophesee ROS driver in the following ways: 129 | 130 | - publishes 131 | [event_camera_msgs](https://github.com/ros-event-camera/event_camera_msgs) 132 | that store more densely and are faster to access than the older 133 | message formats. 134 | - less CPU consumption by avoiding unnecessary memory copies. 135 | - implemented as nodelet such that it can be run in the same address space as 136 | e.g. a rosbag record nodelet without worrying about message loss in transmission. 137 | - prints out message rate statistics so you know when the sensor 138 | saturates bandwidth. 139 | - supports these additional features: 140 | - dynamic reconfiguration for bias parameters 141 | - ROI specification 142 | - camera synchronization (stereo) 143 | - external trigger events 144 | - event rate control 145 | 146 | Parameters: 147 | 148 | - ``bias_file``: path to file with camera biases. See example in the 149 | ``biases`` directory. 150 | - ``settings``: path to json file with camera settings such as pixel masks. 151 | - ``from_file``: path to Metavision raw file. Instead of opening 152 | camera, driver plays back data from this file. This will not be in 153 | real time, usually faster. 154 | - ``serial``: specifies serial number of camera to open (useful for 155 | stereo). To learn serial number format first start driver without 156 | specifying serial number and look at the log files. 157 | - ``encoding``: event data format. This will configure the event data 158 | format of the camera. Default: ``evt3`` (Note: lower case!). 159 | Valid is only ``evt3`` at the moment because there is no decoder for ``evt2`` and ``evt21`` 160 | implemented in the event\_camera\_codecs package. 161 | - ``event_message_time_threshold``: (in seconds) minimum time span of 162 | events to be aggregated in one ROS event message before message is sent. Defaults to 1ms. 163 | In its default setting however the SDK provides packets only every 4ms. To increase SDK 164 | callback frequency, tune ``mipi_frame_period`` if available for your sensor. 165 | - ``event_message_size_threshold``: (in bytes) minimum size of events 166 | (in bytes) to be aggregated in one ROS event message before message is sent. Defaults to 1MB. 167 | - ``statistics_print_interval``: time in seconds between statistics printouts. 168 | - ``send_queue_size``: outgoing ROS message send queue size (defaults 169 | to 1000 messages). 170 | - ``use_multithreading``: decouples the SDK callback from the 171 | processing to ensure the SDK does not drop messages (defaults to 172 | false). The SDK already queues up messages but there is no documentation on 173 | the queue size and no way to determine if messages are dropped. Use multithreading to 174 | minimize the risk of dropping messages. However, be aware that this incurs an 175 | extra memory copy and threading overhead, raising the maximum CPU 176 | load by about 50% of a CPU. 177 | - ``frame_id``: the frame id to use in the ROS message header 178 | - ``roi``: sets hardware region of interest (ROI). You can set 179 | multiple ROI rectangles with this parameter by concatenation: 180 | ``[top_left_x_1, top_left_y_1, width_1, height_1, top_left_x_2, top_left_y_2, width_2...]``. 181 | The length of the ``roi`` parameter vector must therefore be a multiple 182 | of 4. Beware that when using multiple ROIs, per Metavision SDK documentation: 183 | ["Any line or column enabled by a single ROI is also enabled for all the other"](https://docs.prophesee.ai/stable/api/cpp/driver/features.html#_CPPv4N10Metavision3RoiE). 184 | - ``roni``: if ``true``, inverts the meaning of the ROI to become region of non-interest. Default: ``false``. 185 | - ``erc_mode``: event rate control mode (Gen4 sensor): ``na``, 186 | ``disabled``, ``enabled``. Default: ``na``. 187 | - ``erc_rate``: event rate control rate (Gen4 sensor) events/sec. Default: 100000000. 188 | - ``mipi_frame_period``:: mipi frame period in usec. Only available on some sensors. 189 | 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). 190 | - ``trail_filter``: enable/disable event trail filter. Default: False. 191 | - ``trail_filter_type``: type of trail filter. Allowed values: ``trail``, ``stc_cut_trail``, ``stc_keep_trail``. 192 | Default: ``trail``. See Metavision SDK documentation. 193 | - ``trail_filter_threshold``: Filter threshold, see MetavisionSDK documentation. Default: 5000. 194 | - ``sync_mode``: Used to synchronize the time stamps across multiple 195 | cameras (tested for only 2). The cameras must be connected via a 196 | sync cable, and two separate ROS driver nodes are started, see 197 | example launch files. The ``primary`` node's ``ready`` topic must be 198 | remapped so it receives the ``secondary`` node's ``ready`` messages. 199 | Allowed values: 200 | - ``standalone`` (default): freerunning camera, no sync. 201 | - ``primary``: camera that drives the sync clock. Will not start 202 | publishing data until it receives a ``ready`` message from the secondary. 203 | - ``secondary``: camera receiving the sync clock. Will send 204 | ``ready`` messages until it receives a sync signal from the primary. 205 | - ``trigger_in_mode``: Controls the mode of the trigger input hardware. 206 | Allowed values: 207 | - ``disabled`` (default): Does not enable this functionality within the hardware 208 | - ``external``: Enables the external hardware pin to route to the trigger input hardware. 209 | This will be the pin on the camera's connector. 210 | - ``loopback``: Connects the trigger out pin to the trigger input hardware. Only available 211 | on Gen3 sensors (SilkyEVCam VGA). 212 | - ``trigger_out_mode``: Controls the mode of the trigger output 213 | hardware. NOTE: 4-th gen sensors no longer support trigger out! 214 | Allowed values: 215 | - ``disabled`` (default): Does not enable this functionality within the hardware 216 | - ``enabled``: Enables the external hardware pin to route to the trigger in hardware. 217 | - ``trigger_out_period``: Controls the period in microseconds of the trigger out pulse. 218 | - ``trigger_out_duty_cycle``: Controls the duty cycle of the trigger 219 | out pulse. This is the period ratio. 220 | 221 | Services: 222 | 223 | - ``save_biases``: write out current bias settings to bias file. For 224 | this to work the ``bias_file`` parameter must be set to a non-empty value. 225 | - ``save_settings``: write out current camera settings to settings file. For 226 | this to work the ``settings`` parameter must be set to a non-empty value. 227 | 228 | Dynamic reconfiguration parameters 229 | (see [MetaVision documentation here](https://docs.prophesee.ai/stable/hw/manuals/biases.html)): 230 | 231 | - ``bias_diff`` (read only) 232 | - ``bias_diff_off`` 233 | - ``bias_diff_on`` 234 | - ``bias_fo`` 235 | - ``bias_hpf`` 236 | - ``bias_pr`` 237 | - ``bias_refr`` 238 | 239 | ## How to use 240 | 241 | ### Running the driver 242 | 243 | You can just start up a ROS node: 244 | 245 | ```bash 246 | ros2 launch metavision_driver driver_node.launch.py # (run as node) 247 | ``` 248 | 249 | but if you want to efficiently record the events (see below), you 250 | should launch instead as a composable node: 251 | 252 | ```bash 253 | ros2 launch metavision_driver driver_composition.launch.py # (run as composable node) 254 | ``` 255 | 256 | 257 | ### Visualizing the events 258 | 259 | To visualize the events, run a ``renderer`` node from the 260 | [event_camera_renderer](https://github.com/ros-event-camera/event_camera_renderer) package: 261 | 262 | ```bash 263 | ros2 launch event_camera_renderer renderer.launch.py 264 | ``` 265 | 266 | The renderer node publishes an image that can be visualized with e.g. ``rqt_image_view`` 267 | 268 | ### Recording on ROS2 versions older than Jazzy 269 | 270 | You will need to install the [composable recorder](https://github.com/berndpfrommer/rosbag2_composable_recorder) 271 | into your workspace as well (see below), and launch the combined driver and recorder: 272 | 273 | ```bash 274 | ros2 launch metavision_driver recording_driver.launch.py 275 | ros2 run rosbag2_composable_recorder start_recording.py 276 | ``` 277 | 278 | To stop the recording, Ctrl-C the ``recording_driver.launch.py`` script. 279 | 280 | ### Recording on ROS2 Jazzy or later 281 | 282 | Launch the composable driver: 283 | 284 | ```bash 285 | ros2 launch metavision_driver driver_composition.launch.py 286 | ``` 287 | 288 | and start the recording like this: 289 | 290 | ```bash 291 | ros2 launch metavision_driver start_recording.launch.py 292 | ``` 293 | 294 | Then stop it like so: 295 | 296 | ```bash 297 | ros2 run metavision_driver stop_recording_ros2.py 298 | ``` 299 | 300 | Note that the start/stop scripts and launch files need to be adjusted to fit your choice of 301 | node names and topics, but if you leave everything default it should work out of the box. 302 | 303 | ## CPU load 304 | 305 | Here are some approximate performance numbers on a 16 thread (8-core) AMD 306 | Ryzen 7480h laptop with max clock speed of 2.9GHz. All numbers were obtained 307 | by producing maximum event rates about (48Mevs) with a SilkyEVCam: 308 | 309 | All CPU loads below are with sensor saturating at close to 50Mevs. 310 | Middleware used was cyclonedds. 311 | 312 | | settings | single threaded | multi threaded | note | 313 | |--------------------------------|-----------------|----------------|--------------------------------------| 314 | | driver no subscriber | 22% | 59% | no pub, extra copy for multithreaded | 315 | | driver with subscriber | 35% | 44% | does interprocess communication | 316 | | driver + rosbag record node | 80% | 90% | combined driver + record cpu load | 317 | | driver + rosbag record composable | 58% | 80% | single process no ipc but disk/io | 318 | 319 | ## About ROS time stamps 320 | 321 | The SDK provides hardware event time stamps directly from the 322 | camera. For efficiency reasons the packets are not decoded and so the 323 | sensor time stamps are not available to the driver. Therefore the ROS driver 324 | simply puts the host wall clock arrival time of the *first* SDK packet 325 | into the ROS packet's header stamp field. 326 | 327 | ## External triggering 328 | 329 | External triggers on Prophesee cameras allows for a signal to be injected 330 | into the event stream. This is useful for synchronizing external 331 | devices. The event stream contained in the packages will now contain 332 | trigger events that can be recovered with the decoder. 333 | 334 | Prophesee provides documentation on the trigger functions at a high 335 | level 336 | [here](https://docs.prophesee.ai/stable/hw/manuals/timing_interfaces.html). 337 | 338 | [Trigger out](https://docs.prophesee.ai/stable/hw/manuals/timing_interfaces.html#trigger-out) 339 | functionality is exposed through ``trigger_out_mode``, 340 | ``trigger_out_period``, and ``trigger_out_duty_cycle``. These 341 | variables follow the same meaning as laid out in the internal 342 | documentation. 343 | 344 | - ``trigger_out_mode`` can be ``enabled`` or ``disabled`` 345 | - ``trigger_out_period`` can be from 2us to 1h (units are us) 346 | - ``trigger_out_duty_cycle`` is the pulse width ratio 347 | (``trigger_out_period * trigger_out_duty_cycle`` must be at least 1us) 348 | 349 | [Trigger in](https://docs.prophesee.ai/stable/hw/manuals/timing_interfaces.html#trigger-in) 350 | functionality is abstracted away from pins to just ``loopback`` or 351 | ``external`` as the pin mappings are constant for a given camera 352 | configuration. 353 | 354 | - ``trigger_in_mode`` allows the user to specify for each camera 355 | ``loopback`` or ``external`` and lookup which pins are associated 356 | with that camera. 357 | 358 | **WARNING** Running synchronization and triggers at the same time is 359 | possible, but requires understanding of your camera's underlying 360 | hardware (as most share trigger out and sync out pins). 361 | 362 | ### Hardware configuration 363 | 364 | The hardware configuration file is ``config/trigger_pins.yaml``. The 365 | mappings for ``hal_plugin_gen*`` come from 366 | [Prophesee 367 | documentation](https://docs.prophesee.ai/3.1.2/metavision_sdk/modules/hal/samples/hal_viewer.html#configuring-trigger-in-and-trigger-out). 368 | The mapping for ``evc3a_plugin_gen31`` has been validated on the 369 | SilkyEvCam (March 2022). The mapping goes from the HAL Software Info 370 | to pin numbers. 371 | 372 | If your camera is not yet supported, the software info is printed out 373 | on driver startup. Look for a line that contains: 374 | 375 | ```text 376 | Plugin Software Name: 377 | ``` 378 | 379 | This will be the key to place under ``prophesee_pin_config`` which can 380 | then be populated based on your camera's documentation. 381 | 382 | **WARNING** If this file is not loaded (or your camera is not yet 383 | supported), the default pin loaded will be 0. This may work in some 384 | cases, but not all. 385 | 386 | ### SilkyEvCam 387 | 388 | Documentation on the SilkyEvCam pinout can be found 389 | [here on page 6](https://www.centuryarks.com/images/product/sensor/silkyevcam/SilkyEvCam-USB_Spec_Rev102.pdf). 390 | This system uses 3.3V logic for both trigger in as well as trigger out. 391 | 392 | While the loopback configuration is internal to the chip, the trigger 393 | out line will still pulse externally. This is useful if using an event 394 | camera to trigger an external system as you will maintain the timing 395 | relative to the internal clock (after association between the trigger 396 | event and the external system). 397 | 398 | ### Other cameras 399 | 400 | External triggering works on SilkyEVCam HD, i.e. you can trigger the 401 | SilkyEVCam HD externally the same way as the SilkyEVCam VGA. 402 | 403 | ## License 404 | 405 | This software is issued under the Apache License Version 2.0. 406 | --------------------------------------------------------------------------------