Merge pull request #6 from berndpfrommer/frame_based

support for specifying the frames (in sensor time), update to use event_camera_msgs

CMakeLists.txt

@@ -18,10 +18,9 @@ project(frequency_cam)
 
 if(DEFINED ENV{ROS_VERSION})
   if($ENV{ROS_VERSION} EQUAL "2")
-    # to avoid cmake warning: "Manually-specified variables were not used by the project"
-    include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/CMakeListsROS2.txt)
+    include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake)
   elseif($ENV{ROS_VERSION} EQUAL "1")
-    include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/CMakeListsROS1.txt)
+    include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake)
   else()
     message(ERROR "invalid ROS_VERSION environment variable: $ENV{ROS_VERSION}")
   endif()

README.md

@@ -51,11 +51,11 @@ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo  #
 ```
 ros2 launch frequency_cam frequency_cam.launch.py
 ```
-FrequencyCam expects ROS ``event_array_msgs`` messages from the
-[metavision ROS driver](https://github.com/berndpfrommer/metavision_ros_driver)
+FrequencyCam expects ROS ``event_camera_msgs`` messages from the
+[metavision ROS driver](https://github.com/ros-event-camera/metavision_driver)
 here. You should be able to use other cameras (like DVS etc) by
 converting the messages with a ``republish`` nodelet from the
-[event array tools](https://github.com/berndpfrommer/event_array_tools)
+[event camera tools](https://github.com/ros-event-camera/event_camera_tools)
 repository. 
 
 ### Parameters (see launch file):

cmake/CMakeListsROS1.txt → cmake/ROS1.cmake

@@ -20,22 +20,18 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   nodelet
   rosbag
-  event_array_msgs
-  event_array_codecs
+  event_camera_msgs
+  event_camera_codecs
   image_transport
   cv_bridge
-  std_msgs
-  )
+  std_msgs)
 
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS}
-  )
+  ${catkin_INCLUDE_DIRS})
 
 catkin_package()
 
-#
-
 # code common to nodelet and node
 add_library(frequency_cam src/frequency_cam.cpp src/image_maker.cpp src/frequency_cam_ros1.cpp)
 target_link_libraries(frequency_cam ${catkin_LIBRARIES})
@@ -59,17 +55,14 @@ install(TARGETS frequency_cam_node
 install(TARGETS frequency_cam frequency_cam_nodelet
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-  )
-
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
 install(FILES nodelet_plugins.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 install(DIRECTORY launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  FILES_MATCHING PATTERN "*.launch"
-  )
+  FILES_MATCHING PATTERN "*.launch")
 
 
 #############

cmake/CMakeListsROS2.txt → cmake/ROS2.cmake

@@ -28,12 +28,11 @@ set(ROS2_DEPENDENCIES
   "rclcpp"
   "rclcpp_components"
   "rosbag2_cpp"
-  "event_array_msgs"
-  "event_array_codecs"
+  "event_camera_msgs"
+  "event_camera_codecs"
   "image_transport"
   "cv_bridge"
-  "std_msgs"
-  )
+  "std_msgs")
 
 foreach(pkg ${ROS2_DEPENDENCIES})
   find_package(${pkg} REQUIRED)
@@ -45,8 +44,7 @@ ament_auto_find_build_dependencies(REQUIRED ${ROS2_DEPENDENCIES})
 # ---- frequency_cam shared library/component
 #
 ament_auto_add_library(frequency_cam SHARED
-  src/frequency_cam.cpp src/image_maker.cpp src/frequency_cam_ros2.cpp
-  )
+  src/frequency_cam.cpp src/image_maker.cpp src/frequency_cam_ros2.cpp)
 
 rclcpp_components_register_nodes(frequency_cam "frequency_cam::FrequencyCamROS")
 
@@ -66,8 +64,7 @@ ament_auto_add_executable(cpu_benchmark src/cpu_benchmark.cpp)
 
 install(TARGETS
   frequency_cam
-  DESTINATION lib
-)
+  DESTINATION lib)
 
 # the node must go into the project specific lib directory or else
 # the launch file will not find it

frequency_cam.repos

@@ -1,9 +1,9 @@
 repositories:
-  event_array_msgs:
+  event_camera_msgs:
     type: git
-    url: https://github.com/berndpfrommer/event_array_msgs.git
+    url: https://github.com/ros-event-camera/event_camera_msgs.git
     version: master
-  event_array_codecs:
+  event_camera_codecs:
     type: git
-    url: https://github.com/berndpfrommer/event_array_codecs.git
+    url: https://github.com/ros-event-camera/event_camera_codecs.git
     version: master

include/frequency_cam/frequency_cam.h

@@ -16,7 +16,7 @@
 #ifndef FREQUENCY_CAM__FREQUENCY_CAM_H_
 #define FREQUENCY_CAM__FREQUENCY_CAM_H_
 
-#include <event_array_codecs/event_processor.h>
+#include <event_camera_codecs/event_processor.h>
 
 #include <cstdint>
 #include <fstream>
@@ -27,7 +27,7 @@
 
 namespace frequency_cam
 {
-class FrequencyCam : public event_array_codecs::EventProcessor
+class FrequencyCam : public event_camera_codecs::EventProcessor
 {
 public:
   FrequencyCam() {}

include/frequency_cam/frequency_cam_ros1.h

@@ -16,8 +16,8 @@
 #ifndef FREQUENCY_CAM__FREQUENCY_CAM_ROS1_H_
 #define FREQUENCY_CAM__FREQUENCY_CAM_ROS1_H_
 
-#include <event_array_codecs/decoder_factory.h>
-#include <event_array_msgs/EventArray.h>
+#include <event_camera_codecs/decoder_factory.h>
+#include <event_camera_msgs/EventPacket.h>
 #include <image_transport/image_transport.h>
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
@@ -31,7 +31,7 @@ namespace frequency_cam
 class FrequencyCamROS
 {
 public:
-  using EventArray = event_array_msgs::EventArray;
+  using EventPacket = event_camera_codecs::EventPacket;
 
   explicit FrequencyCamROS(const ros::NodeHandle & nh);
   FrequencyCamROS(const FrequencyCamROS &) = delete;
@@ -42,15 +42,15 @@ class FrequencyCamROS
   void statisticsTimerExpired(const ros::TimerEvent &);
   bool initialize();
   void imageConnectCallback(const image_transport::SingleSubscriberPublisher &);
-  void eventMsg(const EventArray::ConstPtr & msg);
+  void eventMsg(const EventPacket::ConstPtr & msg);
 
   // ------ variables ----
   ros::NodeHandle nh_;
   ros::Subscriber eventSub_;  // subscribes to events
   ros::Timer frameTimer_;     // fires once per frame
   ros::Timer statsTimer_;     // for statistics printout
   image_transport::Publisher imagePub_;
-  event_array_codecs::DecoderFactory<FrequencyCam> decoderFactory_;
+  event_camera_codecs::DecoderFactory<EventPacket, FrequencyCam> decoderFactory_;
   std_msgs::Header header_;  // header with frame id etc
   uint32_t seq_{0};          // ROS1 header seqno
   bool isSubscribedToEvents_{false};

include/frequency_cam/frequency_cam_ros2.h

@@ -16,10 +16,11 @@
 #ifndef FREQUENCY_CAM__FREQUENCY_CAM_ROS2_H_
 #define FREQUENCY_CAM__FREQUENCY_CAM_ROS2_H_
 
-#include <event_array_codecs/decoder_factory.h>
+#include <event_camera_codecs/decoder_factory.h>
 
-#include <event_array_msgs/msg/event_array.hpp>
+#include <event_camera_msgs/msg/event_packet.hpp>
 #include <image_transport/image_transport.hpp>
+#include <queue>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <std_msgs/msg/header.hpp>
@@ -32,7 +33,7 @@ namespace frequency_cam
 class FrequencyCamROS : public rclcpp::Node
 {
 public:
-  using EventArray = event_array_msgs::msg::EventArray;
+  using EventPacket = event_camera_msgs::msg::EventPacket;
   explicit FrequencyCamROS(const rclcpp::NodeOptions & options);
 
   FrequencyCamROS(const FrequencyCamROS &) = delete;
@@ -42,16 +43,20 @@ class FrequencyCamROS : public rclcpp::Node
   void frameTimerExpired();
   void statisticsTimerExpired();
   bool initialize();
-  void eventMsg(const EventArray::ConstSharedPtr msg);
+  void eventMsg(const EventPacket::ConstSharedPtr msg);
   void playEventsFromBag(const std::string & bagName, const std::string & bagTopic);
+  void makeAndWriteFrame(uint64_t debugTime, const std::string & path, uint32_t frameCount);
+  void readFrameTimes();
+  bool initializeOnFirstMessage(EventPacket::ConstSharedPtr msg, uint64_t * firstTime);
   // ------ variables ----
   rclcpp::Time lastPubTime_{0};
-  rclcpp::Subscription<EventArray>::SharedPtr eventSub_;
+  rclcpp::Subscription<EventPacket>::SharedPtr eventSub_;
   rclcpp::TimerBase::SharedPtr frameTimer_;
   rclcpp::TimerBase::SharedPtr statsTimer_;
   image_transport::Publisher imagePub_;
-  event_array_codecs::DecoderFactory<FrequencyCam> decoderFactory_;
+  event_camera_codecs::DecoderFactory<EventPacket, FrequencyCam> decoderFactory_;
   std_msgs::msg::Header header_;
+  std::queue<uint64_t> frameTimes_;
 
   uint32_t width_{0};          // image width
   uint32_t height_{0};         // image height

launch/frequency_cam.launch.py

@@ -46,6 +46,7 @@ def launch_setup(context, *args, **kwargs):
              'use_log_frequency': False,
              'overlay_events': True,
              'bag_file': LaunchConfig('bag').perform(context),
+             'frame_time_file': LaunchConfig('frame_time_file').perform(context),
              'publishing_frequency': 25.0}],
         remappings=[
             ('~/events', event_topic),
@@ -61,6 +62,8 @@ def generate_launch_description():
                   description='image topic'),
         LaunchArg('event_topic', default_value=['/event_camera/events'],
                   description='event topic'),
+        LaunchArg('frame_time_file', default_value=[''],
+                  description='file with frame times (sensor time in nanosec)'),
         LaunchArg('bag', default_value=[''],
                   description='name of bag file to read'),
         LaunchArg('use_sim_time', default_value=['false'],

package.xml

@@ -24,8 +24,8 @@
   <depend condition="$ROS_VERSION == 1">rosbag</depend>
 
   <!-- common ROS1/ROS2 dependencies -->
-  <depend>event_array_msgs</depend>
-  <depend>event_array_codecs</depend>  
+  <depend>event_camera_msgs</depend>
+  <depend>event_camera_codecs</depend>  
   <depend>image_transport</depend>
   <depend>cv_bridge</depend>
   <depend>std_msgs</depend>

src/frequency_cam.py

@@ -202,12 +202,12 @@ def make_frequency_map(self, t_now):
 
     def update_state_from_list(self, event_list):
         if len(event_list) > 0:
-            event_array = np.array(event_list, dtype=EventCD)
-            self.update_state(event_array)
+            event_camera = np.array(event_list, dtype=EventCD)
+            self.update_state(event_camera)
 
-    def update_state(self, event_array):
-        self.update_state_filter(event_array)
-        self._events.append(event_array)
+    def update_state(self, event_camera):
+        self.update_state_filter(event_camera)
+        self._events.append(event_camera)
 
     def process_events(self, events):
         if self._timestamps is not None:

src/frequency_cam_ros1.cpp

@@ -83,22 +83,21 @@ void FrequencyCamROS::imageConnectCallback(const image_transport::SingleSubscrib
   }
 }
 
-void FrequencyCamROS::eventMsg(const EventArray::ConstPtr & msg)
+void FrequencyCamROS::eventMsg(const EventPacket::ConstPtr & msg)
 {
   const auto t_start = std::chrono::high_resolution_clock::now();
   if (msg->events.empty()) {
     return;
   }
-  auto decoder = decoderFactory_.getInstance(msg->encoding, msg->width, msg->height);
+  auto decoder = decoderFactory_.getInstance(*msg);
   if (!decoder) {
     ROS_INFO_STREAM("invalid encoding: " << msg->encoding);
     return;
   }
-  decoder->setTimeBase(msg->time_base);
   header_.stamp = msg->header.stamp;
   if (height_ == 0) {
     uint64_t t;
-    if (!decoder->findFirstSensorTime(msg->events.data(), msg->events.size(), &t)) {
+    if (!decoder->findFirstSensorTime(*msg, &t)) {
       return;
     }
     height_ = msg->height;
@@ -109,7 +108,7 @@ void FrequencyCamROS::eventMsg(const EventArray::ConstPtr & msg)
     cam_.initializeState(width_, height_, t, ros::Time(header_.stamp).toNSec());
   }
   // decode will produce callbacks to cam_
-  decoder->decode(&(msg->events[0]), msg->events.size(), &cam_);
+  decoder->decode(*msg, &cam_);
 
   // update statistics
   msgCount_++;