Add device re-connection

include/openni2_camera/openni2_driver.h

@@ -85,6 +85,7 @@ class OpenNI2Driver
 
   void advertiseROSTopics();
 
+  void monitorConnection(const ros::TimerEvent& event);
   void colorConnectCb();
   void depthConnectCb();
   void irConnectCb();
@@ -104,13 +105,22 @@ class OpenNI2Driver
   void setColorVideoMode(const OpenNI2VideoMode& color_video_mode);
   void setDepthVideoMode(const OpenNI2VideoMode& depth_video_mode);
 
+  int extractBusID(const std::string& uri) const;
+  bool isConnected() const;
+
+  void forceSetExposure();
+
   ros::NodeHandle& nh_;
   ros::NodeHandle& pnh_;
 
   boost::shared_ptr<OpenNI2DeviceManager> device_manager_;
   boost::shared_ptr<OpenNI2Device> device_;
 
   std::string device_id_;
+  int bus_id_;
+
+  /** \brief indicates if reconnect logic is enabled. */
+  bool enable_reconnect_;
 
   /** \brief get_serial server*/
   ros::ServiceServer get_serial_server;
@@ -127,6 +137,9 @@ class OpenNI2Driver
   image_transport::CameraPublisher pub_ir_;
   ros::Publisher pub_projector_info_;
 
+  /** \brief timer for connection monitoring thread */
+  ros::Timer timer_;
+
   /** \brief Camera info manager objects. */
   boost::shared_ptr<camera_info_manager::CameraInfoManager> color_info_manager_, ir_info_manager_;
 

src/openni2_driver.cpp

@@ -52,7 +52,8 @@ OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
     ir_subscribers_(false),
     color_subscribers_(false),
     depth_subscribers_(false),
-    depth_raw_subscribers_(false)
+    depth_raw_subscribers_(false),
+    enable_reconnect_(false)
 {
 
   genVideoModeTableMap();
@@ -74,6 +75,16 @@ OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
 
   advertiseROSTopics();
 
+  if( enable_reconnect_ )
+  {
+    ROS_WARN_STREAM("Reconnect has been enabled, only one camera "
+                    << "should be plugged into each bus");
+    timer_ = nh_.createTimer(ros::Duration(1.0), &OpenNI2Driver::monitorConnection, this);
+  }
+  else
+  {
+    ROS_WARN_STREAM("Reconnect has been disabled");
+  }
 }
 
 void OpenNI2Driver::advertiseROSTopics()
@@ -301,22 +312,65 @@ void OpenNI2Driver::applyConfigToOpenNIDevice()
     ROS_ERROR("Could not set auto white balance. Reason: %s", exception.what());
   }
 
-  try
+
+  // Workaound for https://github.com/ros-drivers/openni2_camera/issues/51
+  // This is only needed when any of the 3 setting change.  For simplicity
+  // this check is always performed and exposure set.
+  if( (!auto_exposure_ && !auto_white_balance_) && exposure_ != 0 )
   {
-    if (!config_init_ || (old_config_.exposure != exposure_))
-      device_->setExposure(exposure_);
+    ROS_INFO_STREAM("Forcing exposure set, when auto exposure/white balance disabled");
+    forceSetExposure();
   }
-  catch (const OpenNI2Exception& exception)
+  else
   {
-    ROS_ERROR("Could not set exposure. Reason: %s", exception.what());
+    // Setting the exposure the old way, although this should not have an effect
+    try
+    {
+      if (!config_init_ || (old_config_.exposure != exposure_))
+        device_->setExposure(exposure_);
+    }
+    catch (const OpenNI2Exception& exception)
+    {
+      ROS_ERROR("Could not set exposure. Reason: %s", exception.what());
+    }
   }
 
   device_->setUseDeviceTimer(use_device_time_);
+}
+
+
 
+void OpenNI2Driver::forceSetExposure()
+{
+  int current_exposure_ = device_->getExposure();
+  try
+  {
+    if( current_exposure_ == exposure_ )
+    {
+      if( exposure_ < 254 )
+      {
+        device_->setExposure(exposure_ + 1);
+      }
+      else
+      {
+        device_->setExposure(exposure_ - 1);
+      }
+    }
+    device_->setExposure(exposure_);
+  }
+  catch (const OpenNI2Exception& exception)
+  {
+    ROS_ERROR("Could not set exposure. Reason: %s", exception.what());
+  }
 }
 
 void OpenNI2Driver::colorConnectCb()
 {
+  if( !device_ )
+  {
+    ROS_WARN_STREAM("Callback in " << __FUNCTION__ <<  "failed due to null device");
+    return;
+  }
   boost::lock_guard<boost::mutex> lock(connect_mutex_);
 
   color_subscribers_ = pub_color_.getNumSubscribers() > 0;
@@ -336,6 +390,15 @@ void OpenNI2Driver::colorConnectCb()
     ROS_INFO("Starting color stream.");
     device_->startColorStream();
 
+    // Workaound for https://github.com/ros-drivers/openni2_camera/issues/51
+    if( exposure_ != 0 )
+    {
+      ROS_INFO_STREAM("Exposure is set to " << exposure_ << ", forcing on color stream start");
+        //delay for stream to start, before setting exposure
+      boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+      forceSetExposure();
+    }
+
   }
   else if (!color_subscribers_ && device_->isColorStreamStarted())
   {
@@ -356,6 +419,11 @@ void OpenNI2Driver::colorConnectCb()
 
 void OpenNI2Driver::depthConnectCb()
 {
+  if( !device_ )
+  {
+    ROS_WARN_STREAM("Callback in " << __FUNCTION__ <<  "failed due to null device");
+    return;
+  }
   boost::lock_guard<boost::mutex> lock(connect_mutex_);
 
   depth_subscribers_ = pub_depth_.getNumSubscribers() > 0;
@@ -379,6 +447,11 @@ void OpenNI2Driver::depthConnectCb()
 
 void OpenNI2Driver::irConnectCb()
 {
+  if( !device_ )
+  {
+    ROS_WARN_STREAM("Callback in " << __FUNCTION__ <<  "failed due to null device");
+    return;
+  }
   boost::lock_guard<boost::mutex> lock(connect_mutex_);
 
   ir_subscribers_ = pub_ir_.getNumSubscribers() > 0;
@@ -619,6 +692,8 @@ void OpenNI2Driver::readConfigFromParameterServer()
   pnh_.param("rgb_camera_info_url", color_info_url_, std::string());
   pnh_.param("depth_camera_info_url", ir_info_url_, std::string());
 
+  pnh_.param("enable_reconnect", enable_reconnect_, true);
+
 }
 
 std::string OpenNI2Driver::resolveDeviceURI(const std::string& device_id) throw(OpenNI2Exception)
@@ -743,6 +818,7 @@ void OpenNI2Driver::initDevice()
     {
       std::string device_URI = resolveDeviceURI(device_id_);
       device_ = device_manager_->getDevice(device_URI);
+      bus_id_ = extractBusID(device_->getUri() );
     }
     catch (const OpenNI2Exception& exception)
     {
@@ -768,6 +844,113 @@ void OpenNI2Driver::initDevice()
 
 }
 
+
+int OpenNI2Driver::extractBusID(const std::string& uri) const
+{
+  // URI format is <vendor ID>/<product ID>@<bus number>/<device number>
+  unsigned first = uri.find('@');
+  unsigned last = uri.find('/', first);
+  std::string bus_id = uri.substr (first+1,last-first-1);
+  int rtn = atoi(bus_id.c_str());
+  return rtn;
+}
+
+
+bool OpenNI2Driver::isConnected() const
+{
+  // TODO: The current isConnected logic assumes that there is only one sensor
+  // on the bus of interest.  In the future, we could compare serial numbers
+  // to make certain the same camera as been re-connected.
+  boost::shared_ptr<std::vector<std::string> > list =
+      device_manager_->getConnectedDeviceURIs();
+  for (std::size_t i = 0; i != list->size(); ++i)
+  {
+    int uri_bus_id = extractBusID( list->at(i) );
+    if( uri_bus_id == bus_id_ )
+    {
+      return true;
+    }
+  }
+  return false;
+}
+
+void OpenNI2Driver::monitorConnection(const ros::TimerEvent &event)
+{
+  // If the connection is lost, clean up the device.  If connected
+  // and the devices is not initialized, then initialize.
+  if( isConnected() )
+  {
+    if( !device_ )
+    {
+      ROS_INFO_STREAM("Detected re-connect...attempting reinit");
+      try
+      {
+        {
+        boost::lock_guard<boost::mutex> lock(connect_mutex_);
+        std::string device_URI = resolveDeviceURI(device_id_);
+        device_ = device_manager_->getDevice(device_URI);
+        bus_id_ = extractBusID(device_->getUri() );
+        while (ros::ok() && !device_->isValid())
+        {
+          ROS_INFO("Waiting for device initialization, before configuring and restarting publishers");
+          boost::this_thread::sleep(boost::posix_time::milliseconds(100));
+        }
+        }
+        ROS_INFO_STREAM("Re-applying configuration to camera on re-init");
+        config_init_ = false;
+        applyConfigToOpenNIDevice();
+
+        // The color stream must be started in order to adjust the exposure white
+        // balance.
+        ROS_INFO_STREAM("Starting color stream to adjust camera");
+        colorConnectCb();
+
+        // If auto exposure/white balance is disabled, then the rbg image won't
+        // be adjusted properly.  This is a work around for now, but the final
+        // implimentation should only allow reconnection when auto exposure and
+        // white balance are disabled, and FIXED exposure is used instead.
+        if((!auto_exposure_ && !auto_white_balance_ ) && exposure_ == 0)
+        {
+          ROS_WARN_STREAM("Reconnection should not be enabled if auto expousre"
+                          << "/white balance are disabled.  Temporarily working"
+                          << " around this issue");
+          ROS_WARN_STREAM("Toggling exposure and white balance to auto on re-connect"
+                          << ", otherwise image will be very dark");
+          device_->setAutoExposure(true);
+          device_->setAutoWhiteBalance(true);
+          ROS_INFO_STREAM("Waiting for color camera to come up and adjust");
+          // It takes about 2.5 seconds for the camera to adjust
+          boost::this_thread::sleep(boost::posix_time::milliseconds(2500));
+          ROS_WARN_STREAM("Resetting auto exposure and white balance to previous values");
+          device_->setAutoExposure(auto_exposure_);
+          device_->setAutoWhiteBalance(auto_white_balance_);
+        }
+
+        ROS_INFO_STREAM("Restarting publishers, if needed");
+        irConnectCb();
+        depthConnectCb();
+        ROS_INFO_STREAM("Done re-initializing cameras");
+      }
+
+      catch (const OpenNI2Exception& exception)
+      {
+        if (!device_)
+        {
+          ROS_INFO_STREAM("Failed to re-initialize device on bus: " << bus_id_
+                          << ", reason: " << exception.what());
+        }
+      }
+    }
+  }
+  else if( device_ )
+  {
+    ROS_WARN_STREAM("Detected loss of connection.  Stopping all streams and resetting device");
+    device_->stopAllStreams();
+    device_.reset();
+  }
+}
+
+
 void OpenNI2Driver::genVideoModeTableMap()
 {
   /*