Merge pull request IntelRealSense#16 from jonlwowski012/jrl_reset_fix…

…_cherry_upstream Added code to fix hardware reset issues
130s · Feb 15, 2019 · 797accb · 797accb
2 parents 926954a + b2ff0b7
commit 797accb
Show file tree

Hide file tree

Showing 8 changed files with 423 additions and 178 deletions.
diff --git a/.travis.yml b/.travis.yml
@@ -3,7 +3,12 @@ dist: xenial
 
   # - git clone -v --progress https://github.com/doronhi/realsense.git  # This is Done automatically by TravisCI
 before_install:
-  - sudo ./.install_dependency.sh
+  - echo 'deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main' || sudo tee /etc/apt/sources.list.d/realsense-public.list
+  # - sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keys.gnupg.net:80 --recv-key C8B3A55A6F3EFCDE
+  - sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main"
+  - sudo apt-get update -qq
+  - sudo apt-get install librealsense2-dkms --allow-unauthenticated -y 
+  - sudo apt-get install librealsense2-dev --allow-unauthenticated -y
 
 install:
   # install ROS:
@@ -48,4 +53,4 @@ cache:
   directories:
     - $HOME/.gradle/caches/
     - $HOME/.gradle/wrapper/
-    - $HOME/.android/build-cache
+    - $HOME/.android/build-cache
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt
@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(realsense2_camera)
+add_compile_options(-std=c++11)
 
 option(BUILD_WITH_OPENMP "Use OpenMP" OFF)
 option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OFF)

diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
@@ -177,9 +177,11 @@ namespace realsense2_camera
         bool _align_depth;
         bool _sync_frames;
         bool _pointcloud;
+
 #ifdef PLUS_ONE_ROBOTICS
         bool _enable_filter;
 #endif
+
 		PipelineSyncer _syncer;
 
         std::map<stream_index_pair, cv::Mat> _depth_aligned_image;

diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h
@@ -26,6 +26,9 @@
 #include <eigen3/Eigen/Geometry>
 #include <fstream>
 
+#include <mutex>
+#include <condition_variable>
+
 
 namespace realsense2_camera
 {
@@ -74,6 +77,7 @@ namespace realsense2_camera
         virtual ~RealSenseNodeFactory() {}
 
     private:
+        rs2::device getDevice(std::string& serial_no);
         virtual void onInit() override;
         void tryGetLogSeverity(rs2_log_severity& severity) const;
 

diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml
@@ -35,6 +35,7 @@
   <run_depend>tf</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>diagnostic_updater</run_depend>
+  <run_depend>rgbd_launch</run_depend>
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />
   </export>

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -533,70 +533,70 @@ void BaseRealSenseNode::publishAlignedDepthToOthers(rs2::frame depth_frame, cons
 
 void BaseRealSenseNode::enable_devices()
 {
-    for (auto& streams : IMAGE_STREAMS)
-    {
-        for (auto& elem : streams)
-        {
-            if (_enable[elem])
-            {
-                auto& sens = _sensors[elem];
-                auto profiles = sens.get_stream_profiles();
-                for (auto& profile : profiles)
-                {
-                    auto video_profile = profile.as<rs2::video_stream_profile>();
-                    ROS_DEBUG_STREAM("Sensor profile: " <<
-                                     "Format: " << video_profile.format() <<
-                                     ", Width: " << video_profile.width() <<
-                                     ", Height: " << video_profile.height() <<
-                                     ", FPS: " << video_profile.fps());
-
-                    if (video_profile.format() == _format[elem] &&
-                            (_width[elem] == 0 || video_profile.width() == _width[elem]) &&
-                            (_height[elem] == 0 || video_profile.height() == _height[elem]) &&
-                            (_fps[elem] == 0 || video_profile.fps() == _fps[elem]) &&
-                            video_profile.stream_index() == elem.second)
-                    {
-                        _width[elem] = video_profile.width();
-                        _height[elem] = video_profile.height();
-                        _fps[elem] = video_profile.fps();
-
-                        _enabled_profiles[elem].push_back(profile);
-
-                        _image[elem] = cv::Mat(_width[elem], _height[elem], _image_format[elem], cv::Scalar(0, 0, 0));
-
-                        ROS_INFO_STREAM(_stream_name[elem] << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem]);
-                        break;
-                    }
-                }
-                if (_enabled_profiles.find(elem) == _enabled_profiles.end())
-                {
-                    ROS_WARN_STREAM("Given stream configuration is not supported by the device! " <<
-                                    " Stream: " << rs2_stream_to_string(elem.first) <<
-                                    ", Stream Index: " << elem.second <<
-                                    ", Format: " << _format[elem] <<
-                                    ", Width: " << _width[elem] <<
-                                    ", Height: " << _height[elem] <<
-                                    ", FPS: " << _fps[elem]);
-                    _enable[elem] = false;
-                }
-            }
-        }
-    }
-    if (_align_depth)
-    {
-        for (auto& profiles : _enabled_profiles)
-        {
-            _depth_aligned_image[profiles.first] = cv::Mat(_width[DEPTH], _height[DEPTH], _image_format[DEPTH], cv::Scalar(0, 0, 0));
-        }
-    }
+	for (auto& streams : IMAGE_STREAMS)
+	{
+		for (auto& elem : streams)
+		{
+			if (_enable[elem])
+			{
+				auto& sens = _sensors[elem];
+				auto profiles = sens.get_stream_profiles();
+				for (auto& profile : profiles)
+				{
+					auto video_profile = profile.as<rs2::video_stream_profile>();
+					ROS_DEBUG_STREAM("Sensor profile: " <<
+									 "Format: " << video_profile.format() <<
+									 ", Width: " << video_profile.width() <<
+									 ", Height: " << video_profile.height() <<
+									 ", FPS: " << video_profile.fps());
+
+					if (video_profile.format() == _format[elem] &&
+						(_width[elem] == 0 || video_profile.width() == _width[elem]) &&
+						(_height[elem] == 0 || video_profile.height() == _height[elem]) &&
+						(_fps[elem] == 0 || video_profile.fps() == _fps[elem]) &&
+						video_profile.stream_index() == elem.second)
+					{
+						_width[elem] = video_profile.width();
+						_height[elem] = video_profile.height();
+						_fps[elem] = video_profile.fps();
+
+						_enabled_profiles[elem].push_back(profile);
+
+						_image[elem] = cv::Mat(_width[elem], _height[elem], _image_format[elem], cv::Scalar(0, 0, 0));
+
+						ROS_INFO_STREAM(_stream_name[elem] << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem]);
+						break;
+					}
+				}
+				if (_enabled_profiles.find(elem) == _enabled_profiles.end())
+				{
+					ROS_WARN_STREAM("Given stream configuration is not supported by the device! " <<
+						" Stream: " << rs2_stream_to_string(elem.first) <<
+						", Stream Index: " << elem.second <<
+						", Format: " << _format[elem] <<
+						", Width: " << _width[elem] <<
+						", Height: " << _height[elem] <<
+						", FPS: " << _fps[elem]);
+					_enable[elem] = false;
+				}
+			}
+		}
+	}
+	if (_align_depth)
+	{
+		for (auto& profiles : _enabled_profiles)
+		{
+			_depth_aligned_image[profiles.first] = cv::Mat(_width[DEPTH], _height[DEPTH], _image_format[DEPTH], cv::Scalar(0, 0, 0));
+		}
+	}
 }
 
 void BaseRealSenseNode::setupStreams()
 {
-        ROS_INFO("setupStreams...");
-        enable_devices();
+	ROS_INFO("setupStreams...");
+	enable_devices();
     try{
-            // Publish image stream info
+		// Publish image stream info
         for (auto& profiles : _enabled_profiles)
         {
             for (auto& profile : profiles.second)