From cfe64da9531d5779cf340489d8dc20777bbfb245 Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Wed, 30 Aug 2017 15:57:32 -0500 Subject: [PATCH 1/4] Initial commit of device reconnection --- include/openni2_camera/openni2_driver.h | 8 ++ src/openni2_driver.cpp | 114 ++++++++++++++++++++++++ 2 files changed, 122 insertions(+) diff --git a/include/openni2_camera/openni2_driver.h b/include/openni2_camera/openni2_driver.h index 7c67d70..771aeef 100644 --- a/include/openni2_camera/openni2_driver.h +++ b/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,6 +105,9 @@ 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; + ros::NodeHandle& nh_; ros::NodeHandle& pnh_; @@ -111,6 +115,7 @@ class OpenNI2Driver boost::shared_ptr device_; std::string device_id_; + int bus_id_; /** \brief get_serial server*/ ros::ServiceServer get_serial_server; @@ -127,6 +132,9 @@ class OpenNI2Driver image_transport::CameraPublisher pub_ir_; ros::Publisher pub_projector_info_; + // timer object + ros::Timer timer_; + /** \brief Camera info manager objects. */ boost::shared_ptr color_info_manager_, ir_info_manager_; diff --git a/src/openni2_driver.cpp b/src/openni2_driver.cpp index 78bb253..d0f9631 100644 --- a/src/openni2_driver.cpp +++ b/src/openni2_driver.cpp @@ -73,6 +73,7 @@ OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) : ROS_DEBUG("Dynamic reconfigure configuration received."); advertiseROSTopics(); + timer_ = nh_.createTimer(ros::Duration(1.0), &OpenNI2Driver::monitorConnection, this); } @@ -317,6 +318,11 @@ void OpenNI2Driver::applyConfigToOpenNIDevice() void OpenNI2Driver::colorConnectCb() { + if( !device_ ) + { + ROS_WARN_STREAM("Callback in " << __FUNCTION__ << "failed due to null device"); + return; + } boost::lock_guard lock(connect_mutex_); color_subscribers_ = pub_color_.getNumSubscribers() > 0; @@ -356,6 +362,11 @@ void OpenNI2Driver::colorConnectCb() void OpenNI2Driver::depthConnectCb() { + if( !device_ ) + { + ROS_WARN_STREAM("Callback in " << __FUNCTION__ << "failed due to null device"); + return; + } boost::lock_guard lock(connect_mutex_); depth_subscribers_ = pub_depth_.getNumSubscribers() > 0; @@ -379,6 +390,11 @@ void OpenNI2Driver::depthConnectCb() void OpenNI2Driver::irConnectCb() { + if( !device_ ) + { + ROS_WARN_STREAM("Callback in " << __FUNCTION__ << "failed due to null device"); + return; + } boost::lock_guard lock(connect_mutex_); ir_subscribers_ = pub_ir_.getNumSubscribers() > 0; @@ -766,8 +782,106 @@ void OpenNI2Driver::initDevice() boost::this_thread::sleep(boost::posix_time::milliseconds(100)); } + bus_id_ = extractBusID(device_->getUri() ); + +} + + +int OpenNI2Driver::extractBusID(const std::string& uri) const +{ + ROS_INFO_STREAM("Extracting bus ID from URI: " << uri); + unsigned first = uri.find('@'); + unsigned last = uri.find('/', first); + std::string bus_id = uri.substr (first+1,last-first-1); + ROS_INFO_STREAM("Extracted bus id substring: " << bus_id); + int rtn = atoi(bus_id.c_str()); + ROS_INFO_STREAM("Converted bus id from string: " << bus_id << " to " << rtn); + return rtn; +} + + +bool OpenNI2Driver::isConnected() const +{ + bool rtn = false; + boost::shared_ptr > list = device_manager_->getConnectedDeviceURIs(); + for (std::size_t i = 0; i != list->size(); ++i) + { + ROS_INFO_STREAM("Comparing URI: " << list->at(i)); + int uri_bus_id = extractBusID( list->at(i) ); + ROS_INFO_STREAM("Current bus id: " << bus_id_ << " vs URI id: " << uri_bus_id); + if( uri_bus_id == bus_id_ ) + { + ROS_INFO_STREAM("Found BUS id match"); + rtn = true; + } + } + return rtn; +} + +void OpenNI2Driver::monitorConnection(const ros::TimerEvent &event) +{ + ROS_INFO_STREAM("Number of connected device: " + << device_manager_->getNumOfConnectedDevices() ); + if( device_ ) + { + ROS_INFO_STREAM("Device URI: " << device_->getUri()); + ROS_INFO_STREAM("Bus ID: " << extractBusID(device_->getUri()) ); + } + + if( isConnected() ) + { + if( !device_ ) + { + ROS_INFO_STREAM("Detected re-connect...attempting reinit"); + try + { + boost::lock_guard lock(connect_mutex_); + std::string device_URI = resolveDeviceURI(device_id_); + device_ = device_manager_->getDevice(device_URI); + } + catch (const OpenNI2Exception& exception) + { + if (!device_) + { + ROS_INFO("No matching device found.... waiting for devices. Reason: %s", + exception.what()); + } + } + + while (ros::ok() && !device_->isValid()) + { + ROS_INFO("Waiting for device initialization, before restarting publishers"); + boost::this_thread::sleep(boost::posix_time::milliseconds(100)); + } + + bus_id_ = extractBusID(device_->getUri() ); +// ROS_INFO_STREAM("Delaying to allow sensor fully come up."); +// ros::Duration(5.0).sleep(); +// ROS_INFO_STREAM("Restarting publishers, if needed"); + irConnectCb(); + colorConnectCb(); + depthConnectCb(); +// ROS_INFO_STREAM("Done with publishers, if needed"); + } + } + else + { + if( device_ ) + { + ROS_WARN_STREAM("Detected loss of connection. Stopping all streams"); + device_->stopAllStreams(); + device_.reset(); + } + else + { + ROS_WARN_STREAM("Detected " << device_manager_->getNumOfConnectedDevices() + << " devices, but internal device is NULL"); + } + } + } + void OpenNI2Driver::genVideoModeTableMap() { /* From 6df48ccd7f2de946145fb9284ddd4219f5b016e4 Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Wed, 30 Aug 2017 22:11:07 -0500 Subject: [PATCH 2/4] Cleaned up logging, added work-around when auto exposure/white balance are disabled --- include/openni2_camera/openni2_driver.h | 5 +- src/openni2_driver.cpp | 126 ++++++++++++++---------- 2 files changed, 78 insertions(+), 53 deletions(-) diff --git a/include/openni2_camera/openni2_driver.h b/include/openni2_camera/openni2_driver.h index 771aeef..0142e4b 100644 --- a/include/openni2_camera/openni2_driver.h +++ b/include/openni2_camera/openni2_driver.h @@ -117,6 +117,9 @@ class OpenNI2Driver 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; @@ -132,7 +135,7 @@ class OpenNI2Driver image_transport::CameraPublisher pub_ir_; ros::Publisher pub_projector_info_; - // timer object + /** \brief timer for connection monitoring thread */ ros::Timer timer_; /** \brief Camera info manager objects. */ diff --git a/src/openni2_driver.cpp b/src/openni2_driver.cpp index d0f9631..03ba904 100644 --- a/src/openni2_driver.cpp +++ b/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(); @@ -73,8 +74,17 @@ OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) : ROS_DEBUG("Dynamic reconfigure configuration received."); advertiseROSTopics(); - timer_ = nh_.createTimer(ros::Duration(1.0), &OpenNI2Driver::monitorConnection, this); + 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() @@ -635,6 +645,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) @@ -759,6 +771,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) { @@ -782,52 +795,42 @@ void OpenNI2Driver::initDevice() boost::this_thread::sleep(boost::posix_time::milliseconds(100)); } - bus_id_ = extractBusID(device_->getUri() ); - } int OpenNI2Driver::extractBusID(const std::string& uri) const { - ROS_INFO_STREAM("Extracting bus ID from URI: " << uri); + // URI format is /@/ unsigned first = uri.find('@'); unsigned last = uri.find('/', first); std::string bus_id = uri.substr (first+1,last-first-1); - ROS_INFO_STREAM("Extracted bus id substring: " << bus_id); int rtn = atoi(bus_id.c_str()); - ROS_INFO_STREAM("Converted bus id from string: " << bus_id << " to " << rtn); return rtn; } bool OpenNI2Driver::isConnected() const { - bool rtn = false; - boost::shared_ptr > list = device_manager_->getConnectedDeviceURIs(); + // 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 > list = + device_manager_->getConnectedDeviceURIs(); for (std::size_t i = 0; i != list->size(); ++i) { - ROS_INFO_STREAM("Comparing URI: " << list->at(i)); int uri_bus_id = extractBusID( list->at(i) ); - ROS_INFO_STREAM("Current bus id: " << bus_id_ << " vs URI id: " << uri_bus_id); if( uri_bus_id == bus_id_ ) { - ROS_INFO_STREAM("Found BUS id match"); - rtn = true; + return true; } } - return rtn; + return false; } void OpenNI2Driver::monitorConnection(const ros::TimerEvent &event) { - ROS_INFO_STREAM("Number of connected device: " - << device_manager_->getNumOfConnectedDevices() ); - if( device_ ) - { - ROS_INFO_STREAM("Device URI: " << device_->getUri()); - ROS_INFO_STREAM("Bus ID: " << extractBusID(device_->getUri()) ); - } - + // If the connection is lost, clean up the device. If connected + // and the devices is not initialized, then initialize. if( isConnected() ) { if( !device_ ) @@ -835,50 +838,69 @@ void OpenNI2Driver::monitorConnection(const ros::TimerEvent &event) ROS_INFO_STREAM("Detected re-connect...attempting reinit"); try { + { boost::lock_guard 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_) + { + 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("No matching device found.... waiting for devices. Reason: %s", - exception.what()); + ROS_INFO_STREAM("Failed to re-initialize device on bus: " << bus_id_ + << ", reason: " << exception.what()); } } - - while (ros::ok() && !device_->isValid()) - { - ROS_INFO("Waiting for device initialization, before restarting publishers"); - boost::this_thread::sleep(boost::posix_time::milliseconds(100)); - } - - bus_id_ = extractBusID(device_->getUri() ); -// ROS_INFO_STREAM("Delaying to allow sensor fully come up."); -// ros::Duration(5.0).sleep(); -// ROS_INFO_STREAM("Restarting publishers, if needed"); - irConnectCb(); - colorConnectCb(); - depthConnectCb(); -// ROS_INFO_STREAM("Done with publishers, if needed"); } } - else + else if( device_ ) { - if( device_ ) - { - ROS_WARN_STREAM("Detected loss of connection. Stopping all streams"); - device_->stopAllStreams(); - device_.reset(); - } - else - { - ROS_WARN_STREAM("Detected " << device_manager_->getNumOfConnectedDevices() - << " devices, but internal device is NULL"); - } + ROS_WARN_STREAM("Detected loss of connection. Stopping all streams and resetting device"); + device_->stopAllStreams(); + device_.reset(); } - } From 681e30c7943e7b273979c0354d1932b3865f3fb2 Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Thu, 31 Aug 2017 07:38:02 -0500 Subject: [PATCH 3/4] Added work around for issue #51 --- include/openni2_camera/openni2_driver.h | 2 + src/openni2_driver.cpp | 55 ++++++++++++++++++++++--- 2 files changed, 52 insertions(+), 5 deletions(-) diff --git a/include/openni2_camera/openni2_driver.h b/include/openni2_camera/openni2_driver.h index 0142e4b..d1704a8 100644 --- a/include/openni2_camera/openni2_driver.h +++ b/include/openni2_camera/openni2_driver.h @@ -108,6 +108,8 @@ class OpenNI2Driver int extractBusID(const std::string& uri) const; bool isConnected() const; + void forceSetExposure(); + ros::NodeHandle& nh_; ros::NodeHandle& pnh_; diff --git a/src/openni2_driver.cpp b/src/openni2_driver.cpp index 03ba904..39026c6 100644 --- a/src/openni2_driver.cpp +++ b/src/openni2_driver.cpp @@ -312,18 +312,56 @@ 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() @@ -352,6 +390,13 @@ 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"); + forceSetExposure(); + } + } else if (!color_subscribers_ && device_->isColorStreamStarted()) { From c4ab205d861560629bd78192b74e1d9da4100e6f Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Thu, 31 Aug 2017 08:27:39 -0500 Subject: [PATCH 4/4] Fixes for reconnection when exposure is fixed --- src/openni2_driver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/openni2_driver.cpp b/src/openni2_driver.cpp index 39026c6..6f749ce 100644 --- a/src/openni2_driver.cpp +++ b/src/openni2_driver.cpp @@ -394,6 +394,8 @@ void OpenNI2Driver::colorConnectCb() 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(); } @@ -907,7 +909,7 @@ void OpenNI2Driver::monitorConnection(const ros::TimerEvent &event) // 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_) + 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"