merging fix/stereo

davis_ros_driver/include/davis_ros_driver/driver.h

@@ -27,6 +27,7 @@
 #include <dvs_msgs/Event.h>
 #include <dvs_msgs/EventArray.h>
 #include <std_msgs/Empty.h>
+#include <std_msgs/Time.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/Image.h>
 
@@ -77,7 +78,8 @@ class DavisRosDriver {
   dynamic_reconfigure::Server<davis_ros_driver::DAVIS_ROS_DriverConfig>::CallbackType dynamic_reconfigure_callback_;
 
   ros::Subscriber reset_sub_;
-  void resetTimestampsCallback(const std_msgs::Empty::ConstPtr& msg);
+  ros::Publisher reset_pub_;
+  void resetTimestampsCallback(const std_msgs::Time::ConstPtr& msg);
 
   ros::Subscriber imu_calibration_sub_;
   void imuCalibrationCallback(const std_msgs::Empty::ConstPtr& msg);
@@ -102,6 +104,7 @@ class DavisRosDriver {
   std::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
 
   struct caer_davis_info davis_info_;
+  bool master_;
   std::string device_id_;
 
   ros::Time reset_time_;

davis_ros_driver/launch/stereo.launch

@@ -0,0 +1,21 @@
+<launch>
+  <!-- master DAVIS -->
+  <node ns="davis_left" name="davis_ros_driver" pkg="davis_ros_driver" type="davis_ros_driver" output="screen">
+    <param name="serial_number" type="str" value="02460074" />
+    <rosparam command="load" file="$(find davis_ros_driver)/config/indoors.yaml" />
+    <param name="master" value="True"/>
+
+    <!-- timeout before resetting time -->
+    <param name="reset_timestamps_delay" value="2.0"/>
+  </node>
+
+  <!-- slave DAVIS -->
+  <node ns="davis_right" name="davis_ros_driver" pkg="davis_ros_driver" type="davis_ros_driver" output="screen">
+    <param name="serial_number" type="str" value="84010026" />
+    <rosparam command="load" file="$(find davis_ros_driver)/config/indoors.yaml" />
+    <param name="master" value="False"/>
+
+    <!-- get reset signal from master -->
+    <remap from="reset_timestamps" to="/davis_left/reset_timestamps"/>
+  </node>
+</launch>

davis_ros_driver/src/driver.cpp

@@ -44,54 +44,60 @@ DavisRosDriver::DavisRosDriver(ros::NodeHandle & nh, ros::NodeHandle nh_private)
     parameter_bias_update_required_(false),
     imu_calibration_running_(false)
 {
-
-    // load parameters
-    std::string dvs_serial_number;
-    nh_private.param<std::string>("serial_number", dvs_serial_number, "");
-    bool master;
-    nh_private.param<bool>("master", master, true);
-    double reset_timestamps_delay;
-    nh_private.param<double>("reset_timestamps_delay", reset_timestamps_delay, -1.0);
-    nh_private.param<int>("imu_calibration_sample_size", imu_calibration_sample_size_, 1000);
-
-    // initialize bias
-    bias.linear_acceleration.x = 0.0;
-    bias.linear_acceleration.y = 0.0;
-    bias.linear_acceleration.z = 0.0;
-    bias.angular_velocity.x = 0.0;
-    bias.angular_velocity.y = 0.0;
-    bias.angular_velocity.z = 0.0;
-
-    // set namespace
-    ns = ros::this_node::getNamespace();
-    if (ns == "/")
-        ns = "/dvs";
-
-    event_array_pub_ = nh_.advertise<dvs_msgs::EventArray>(ns + "/events", 10);
-    camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(ns + "/camera_info", 1);
-    imu_pub_ = nh_.advertise<sensor_msgs::Imu>(ns + "/imu", 10);
-    image_pub_ = nh_.advertise<sensor_msgs::Image>(ns + "/image_raw", 1);
-    exposure_pub_ = nh_.advertise<std_msgs::Float32>(ns + "/exposure", 10);
-
-    caerConnect();
-    current_config_.streaming_rate = 30;
-    delta_ = boost::posix_time::microseconds(1e6/current_config_.streaming_rate);
-
+  // load parameters
+  nh_private.param<std::string>("serial_number", device_id_, "");
+  nh_private.param<bool>("master", master_, true);
+  double reset_timestamps_delay;
+  nh_private.param<double>("reset_timestamps_delay", reset_timestamps_delay, -1.0);
+  nh_private.param<int>("imu_calibration_sample_size", imu_calibration_sample_size_, 1000);
+
+  // initialize bias
+  bias.linear_acceleration.x = 0.0;
+  bias.linear_acceleration.y = 0.0;
+  bias.linear_acceleration.z = 0.0;
+  bias.angular_velocity.x = 0.0;
+  bias.angular_velocity.y = 0.0;
+  bias.angular_velocity.z = 0.0;
+
+  // set namespace
+  ns = ros::this_node::getNamespace();
+  if (ns == "/")
+    ns = "/dvs";
+
+  event_array_pub_ = nh_.advertise<dvs_msgs::EventArray>(ns + "/events", 10);
+  camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(ns + "/camera_info", 1);
+  imu_pub_ = nh_.advertise<sensor_msgs::Imu>(ns + "/imu", 10);
+  image_pub_ = nh_.advertise<sensor_msgs::Image>(ns + "/image_raw", 1);
+  exposure_pub_ = nh_.advertise<std_msgs::Float32>(ns + "/exposure", 10);
+
+  // reset timestamps is publisher as master, subscriber as slave
+  if (master_)
+  {
+    reset_pub_ = nh_.advertise<std_msgs::Time>((ns + "/reset_timestamps").c_str(), 1);
+  }
+  else
+  {
     reset_sub_ = nh_.subscribe((ns + "/reset_timestamps").c_str(), 1, &DavisRosDriver::resetTimestampsCallback, this);
-    imu_calibration_sub_ = nh_.subscribe((ns + "/calibrate_imu").c_str(), 1, &DavisRosDriver::imuCalibrationCallback, this);
-    snapshot_sub_ = nh_.subscribe((ns + "/trigger_snapshot").c_str(), 1, &DavisRosDriver::snapshotCallback, this);
-
-    // Dynamic reconfigure
-    dynamic_reconfigure_callback_ = boost::bind(&DavisRosDriver::callback, this, _1, _2);
-    server_.reset(new dynamic_reconfigure::Server<davis_ros_driver::DAVIS_ROS_DriverConfig>(nh_private));
-    server_->setCallback(dynamic_reconfigure_callback_);
-
-    // start timer to reset timestamps for synchronization
-    if (reset_timestamps_delay > 0.0)
-    {
-        timestamp_reset_timer_ = nh_.createTimer(ros::Duration(reset_timestamps_delay), &DavisRosDriver::resetTimerCallback, this);
-        ROS_INFO("Started timer to reset timestamps on master DVS for synchronization (delay=%3.2fs).", reset_timestamps_delay);
-    }
+  }
+
+  caerConnect();
+  current_config_.streaming_rate = 30;
+  delta_ = boost::posix_time::microseconds(1e6/current_config_.streaming_rate);
+
+  imu_calibration_sub_ = nh_.subscribe((ns + "/calibrate_imu").c_str(), 1, &DavisRosDriver::imuCalibrationCallback, this);
+  snapshot_sub_ = nh_.subscribe((ns + "/trigger_snapshot").c_str(), 1, &DavisRosDriver::snapshotCallback, this);
+
+  // Dynamic reconfigure
+  dynamic_reconfigure_callback_ = boost::bind(&DavisRosDriver::callback, this, _1, _2);
+  server_.reset(new dynamic_reconfigure::Server<davis_ros_driver::DAVIS_ROS_DriverConfig>(nh_private));
+  server_->setCallback(dynamic_reconfigure_callback_);
+
+  // start timer to reset timestamps for synchronization
+  if (reset_timestamps_delay > 0.0)
+  {
+    timestamp_reset_timer_ = nh_.createTimer(ros::Duration(reset_timestamps_delay), &DavisRosDriver::resetTimerCallback, this);
+    ROS_INFO("Started timer to reset timestamps on master DVS for synchronization (delay=%3.2fs).", reset_timestamps_delay);
+  }
 }
 
 DavisRosDriver::~DavisRosDriver()
@@ -120,66 +126,69 @@ void DavisRosDriver::dataStop()
 void DavisRosDriver::caerConnect()
 {
 
-    // start driver
-    bool dvs_running = false;
-    while (!dvs_running)
-    {
-        //driver_ = new dvs::DvsDriver(dvs_serial_number, master);
-        davis_handle_ = caerDeviceOpen(1, CAER_DEVICE_DAVIS, 0, 0, NULL);
+  // start driver
+  bool device_is_running = false;
+  while (!device_is_running)
+  {
+    const char* serial_number_restrict = (device_id_ == "") ? NULL : device_id_.c_str();
 
-        //dvs_running = driver_->isDeviceRunning();
-        dvs_running = !(davis_handle_ == NULL);
-
-        if (!dvs_running)
-        {
-            ROS_WARN("Could not find DVS. Will retry every second.");
-            ros::Duration(1.0).sleep();
-            ros::spinOnce();
-        }
-
-        if (!ros::ok())
-        {
-            return;
-        }
+    if(serial_number_restrict)
+    {
+      ROS_WARN("Requested serial number: %s", device_id_.c_str());
     }
 
-    davis_info_ = caerDavisInfoGet(davis_handle_);
+    davis_handle_ = caerDeviceOpen(1, CAER_DEVICE_DAVIS, 0, 0, serial_number_restrict);
+
+    // was opening successful?
+    device_is_running = !(davis_handle_ == NULL);
 
-    if (davis_info_.chipID == DAVIS_CHIP_DAVIS346B)
+    if (!device_is_running)
     {
-        device_id_ = "DAVIS-346-" + std::string(davis_info_.deviceString).substr(21, 5);
+      ROS_WARN("Could not find DAVIS. Will retry every second.");
+      ros::Duration(1.0).sleep();
+      ros::spinOnce();
     }
-    else
+
+    if (!ros::ok())
     {
-        device_id_ = "DAVIS-" + std::string(davis_info_.deviceString).substr(18, 8);
+      return;
     }
+  }
 
-    ROS_INFO("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", davis_info_.deviceString,
-             davis_info_.deviceID, davis_info_.deviceIsMaster, davis_info_.dvsSizeX, davis_info_.dvsSizeY,
-             davis_info_.logicVersion);
+  davis_info_ = caerDavisInfoGet(davis_handle_);
+  device_id_ = "DAVIS-" + std::string(davis_info_.deviceString).substr(14, 8);
 
-    // Send the default configuration before using the device.
-    // No configuration is sent automatically!
-    caerDeviceSendDefaultConfig(davis_handle_);
+  ROS_INFO("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", davis_info_.deviceString,
+           davis_info_.deviceID, davis_info_.deviceIsMaster, davis_info_.dvsSizeX, davis_info_.dvsSizeY,
+           davis_info_.logicVersion);
 
-    // Re-send params from param server if not first connection
-    parameter_bias_update_required_ = true;
-    parameter_update_required_ = true;
+  if (master_ && !davis_info_.deviceIsMaster)
+  {
+    ROS_WARN("Device %s should be master, but is not!", device_id_.c_str());
+  }
 
-    // camera info handling
-    ros::NodeHandle nh_ns(ns);
-    camera_info_manager_.reset(new camera_info_manager::CameraInfoManager(nh_ns, device_id_));
+  // Send the default configuration before using the device.
+  // No configuration is sent automatically!
+  caerDeviceSendDefaultConfig(davis_handle_);
 
-    // spawn threads
-    running_ = true;
-    parameter_thread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&DavisRosDriver::changeDvsParameters, this)));
-    readout_thread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&DavisRosDriver::readout, this)));
+  // Re-send params from param server if not first connection
+  parameter_bias_update_required_ = true;
+  parameter_update_required_ = true;
 
-    // wait for driver to be ready
-    ros::Duration(0.5).sleep();
+  // camera info handling
+  ros::NodeHandle nh_ns(ns);
+  camera_info_manager_.reset(new camera_info_manager::CameraInfoManager(nh_ns, device_id_));
 
-    // initialize timestamps
-    resetTimestamps();
+  // initialize timestamps
+  resetTimestamps();
+
+  // spawn threads
+  running_ = true;
+  parameter_thread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&DavisRosDriver::changeDvsParameters, this)));
+  readout_thread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&DavisRosDriver::readout, this)));
+
+  // wait for driver to be ready
+  ros::Duration(0.5).sleep();
 }
 
 void DavisRosDriver::onDisconnectUSB(void* driver)
@@ -190,14 +199,33 @@ void DavisRosDriver::onDisconnectUSB(void* driver)
 
 void DavisRosDriver::resetTimestamps()
 {
-    ROS_INFO("Reset timestamps on %s", device_id_.c_str());
-    caerDeviceConfigSet(davis_handle_, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_TIMESTAMP_RESET, 1);
-    reset_time_ = ros::Time::now();
+  caerDeviceConfigSet(davis_handle_, DAVIS_CONFIG_MUX, DAVIS_CONFIG_MUX_TIMESTAMP_RESET, 1);
+  reset_time_ = ros::Time::now();
+
+  ROS_INFO("Reset timestamps on %s to %.9f.", device_id_.c_str(), reset_time_.toSec());
+
+  // if master, publish reset time to slaves
+  if (master_)
+  {
+    std_msgs::Time reset_msg;
+    reset_msg.data = reset_time_;
+    reset_pub_.publish(reset_msg);
+  }
 }
 
-void DavisRosDriver::resetTimestampsCallback(const std_msgs::Empty::ConstPtr& msg)
+void DavisRosDriver::resetTimestampsCallback(const std_msgs::Time::ConstPtr& msg)
 {
+  // if slave, only adjust offset time
+  if (!davis_info_.deviceIsMaster)
+  {
+    ROS_INFO("Adapting reset time of master on slave %s.", device_id_.c_str());
+    reset_time_ = msg->data;
+  }
+  // if master, or not single camera configuration, just reset timestamps
+  else
+  {
     resetTimestamps();
+  }
 }
 
 void DavisRosDriver::imuCalibrationCallback(const std_msgs::Empty::ConstPtr &msg)
@@ -257,8 +285,8 @@ void DavisRosDriver::snapshotCallback(const std_msgs::Empty::ConstPtr& msg)
 
 void DavisRosDriver::resetTimerCallback(const ros::TimerEvent& te)
 {
-    resetTimestamps();
-    timestamp_reset_timer_.stop();
+  timestamp_reset_timer_.stop();
+  resetTimestamps();
 }
 
 void DavisRosDriver::changeDvsParameters()

dvs_renderer/launch/stereo_davis.launch

@@ -0,0 +1,29 @@
+<launch>
+  <!-- DAVIS drivers -->
+  <include file="$(find davis_ros_driver)/launch/stereo.launch" />
+
+  <!-- visualization -->
+  <node name="dvs_renderer_left" pkg="dvs_renderer" type="dvs_renderer">
+    <param name="display_method" value="red-blue"/>
+    <remap from="events" to="/davis_left/events" />
+    <remap from="image" to="/davis_left/image_raw" />
+    <remap from="dvs_rendering" to="dvs_rendering_left" />
+  </node>
+
+  <node name="dvs_renderer_right" pkg="dvs_renderer" type="dvs_renderer">
+    <param name="display_method" value="red-blue"/>
+    <remap from="events" to="/davis_right/events" />
+    <remap from="image" to="/davis_right/image_raw" />
+    <remap from="dvs_rendering" to="dvs_rendering_right" />
+  </node>
+
+  <!-- display -->
+  <node name="image_view_left" pkg="image_view" type="image_view">
+    <remap from="image" to="dvs_rendering_left"/>
+  </node>
+
+  <node name="image_view_right" pkg="image_view" type="image_view">
+    <remap from="image" to="dvs_rendering_right"/>
+  </node>
+
+</launch>

dvs_ros_driver/include/dvs_ros_driver/driver.h

@@ -27,6 +27,7 @@
 #include <dvs_msgs/Event.h>
 #include <dvs_msgs/EventArray.h>
 #include <std_msgs/Empty.h>
+#include <std_msgs/Time.h>
 
 // DVS driver
 #include <libcaer/libcaer.h>
@@ -64,7 +65,8 @@ class DvsRosDriver {
   dynamic_reconfigure::Server<dvs_ros_driver::DVS_ROS_DriverConfig>::CallbackType dynamic_reconfigure_callback_;
 
   ros::Subscriber reset_sub_;
-  void resetTimestampsCallback(std_msgs::Empty msg);
+  ros::Publisher reset_pub_;
+  void resetTimestampsCallback(const std_msgs::Time::ConstPtr &msg);
 
   boost::shared_ptr<boost::thread> parameter_thread_;
   boost::shared_ptr<boost::thread> readout_thread_;
@@ -75,6 +77,7 @@ class DvsRosDriver {
   camera_info_manager::CameraInfoManager* camera_info_manager_;
 
   struct caer_dvs128_info dvs128_info_;
+  bool master_;
   std::string device_id_;
 
   ros::Time reset_time_;