From fed8521a17dfa939b3980b8b3fc70b1da5ca0bc2 Mon Sep 17 00:00:00 2001 From: icarpis Date: Thu, 8 Feb 2018 14:24:58 +0200 Subject: [PATCH 1/5] Added hardware reset as a w/o of FW issue --- .../include/realsense_node_factory.h | 1 + .../src/realsense_node_factory.cpp | 201 ++++++++++++++++++ 2 files changed, 202 insertions(+) create mode 100644 realsense_ros_camera/src/realsense_node_factory.cpp diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 0132bf5667..71d0797a6b 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -74,6 +74,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/realsense_ros_camera/src/realsense_node_factory.cpp b/realsense_ros_camera/src/realsense_node_factory.cpp new file mode 100644 index 0000000000..0c3b0b9efc --- /dev/null +++ b/realsense_ros_camera/src/realsense_node_factory.cpp @@ -0,0 +1,201 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved + +#include "../include/realsense_node_factory.h" +#include "../include/sr300_node.h" +#include "../include/rs415_node.h" +#include "../include/rs435_node.h" +#include +#include + +using namespace realsense_ros_camera; + +#define REALSENSE_ROS_EMBEDDED_VERSION_STR (VAR_ARG_STRING(VERSION: REALSENSE_ROS_MAJOR_VERSION.REALSENSE_ROS_MINOR_VERSION.REALSENSE_ROS_PATCH_VERSION)) +constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR; + +PLUGINLIB_EXPORT_CLASS(realsense_ros_camera::RealSenseNodeFactory, nodelet::Nodelet) + +RealSenseNodeFactory::RealSenseNodeFactory() +{ + ROS_INFO("RealSense ROS v%s", REALSENSE_ROS_VERSION_STR); + ROS_INFO("Running with LibRealSense v%s", RS2_API_VERSION_STR); + + signal(SIGINT, signalHandler); + auto severity = rs2_log_severity::RS2_LOG_SEVERITY_WARN; + tryGetLogSeverity(severity); + if (rs2_log_severity::RS2_LOG_SEVERITY_DEBUG == severity) + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + + rs2::log_to_console(severity); +} + +rs2::device RealSenseNodeFactory::getDevice(std::string& serial_no) +{ + auto list = _ctx.query_devices(); + if (0 == list.size()) + { + ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); + ros::shutdown(); + exit(1); + } + + bool found = false; + rs2::device retDev; + + for (auto&& dev : list) + { + auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); + ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); + if (serial_no.empty()) + { + retDev = dev; + serial_no = sn; + found = true; + break; + } + else if (sn == serial_no) + { + retDev = dev; + found = true; + break; + } + } + + if (!found) + { + ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); + ros::shutdown(); + exit(1); + } + + return retDev; +} + +void RealSenseNodeFactory::onInit() +{ + try{ + std::mutex mtx; + std::condition_variable cv; + rs2::device dev; + _ctx.set_devices_changed_callback([&dev, &cv](rs2::event_information& info) + { + if (info.was_removed(dev)) + { + cv.notify_one(); + } + }); + + auto privateNh = getPrivateNodeHandle(); + std::string serial_no(""); + privateNh.param("serial_no", serial_no, std::string("")); + dev = getDevice(serial_no); + ROS_INFO("Resetting device..."); + dev.hardware_reset(); + + { + std::unique_lock lk(mtx); + cv.wait(lk); + } + + _device = getDevice(serial_no); + + _ctx.set_devices_changed_callback([this](rs2::event_information& info) + { + if (info.was_removed(_device)) + { + ROS_FATAL("The device has been disconnected! Terminating RealSense Node..."); + ros::shutdown(); + exit(1); + } + }); + + // TODO + auto nh = getNodeHandle(); + auto pid_str = _device.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + uint16_t pid; + std::stringstream ss; + ss << std::hex << pid_str; + ss >> pid; + switch(pid) + { + case SR300_PID: + _realSenseNode = std::unique_ptr(new SR300Node(nh, privateNh, _device, serial_no)); + break; + case RS400_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS405_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS410_PID: + case RS460_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS415_PID: + _realSenseNode = std::unique_ptr(new RS415Node(nh, privateNh, _device, serial_no)); + break; + case RS420_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS420_MM_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_MM_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_MM_RGB_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS435_RGB_PID: + _realSenseNode = std::unique_ptr(new RS435Node(nh, privateNh, _device, serial_no)); + break; + case RS_USB2_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + default: + ROS_FATAL_STREAM("Unsupported device!" << "Product ID: " << pid_str); + ros::shutdown(); + exit(1); + } + + assert(_realSenseNode); + _realSenseNode->publishTopics(); + _realSenseNode->registerDynamicReconfigCb(); + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM("An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM("Unknown exception has occured!"); + throw; + } +} + +void RealSenseNodeFactory::tryGetLogSeverity(rs2_log_severity& severity) const +{ + static const char* severity_var_name = "LRS_LOG_LEVEL"; + auto content = getenv(severity_var_name); + + if (content) + { + std::string content_str(content); + std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::toupper); + + for (uint32_t i = 0; i < RS2_LOG_SEVERITY_COUNT; i++) + { + auto current = std::string(rs2_log_severity_to_string((rs2_log_severity)i)); + std::transform(current.begin(), current.end(), current.begin(), ::toupper); + if (content_str == current) + { + severity = (rs2_log_severity)i; + break; + } + } + } +} From 8776d3b5d02c6e88629c89045eaf4f20a999e007 Mon Sep 17 00:00:00 2001 From: Jamie Cho Date: Mon, 6 Aug 2018 13:03:02 -0400 Subject: [PATCH 2/5] Merge remote-tracking branch 'reset_dev/reset_dev' into development --- .../src/realsense_node_factory.cpp | 243 +++++++++--------- 1 file changed, 128 insertions(+), 115 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 15a4f80132..05c096c5a3 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -29,6 +29,48 @@ RealSenseNodeFactory::RealSenseNodeFactory() rs2::log_to_console(severity); } +rs2::device RealSenseNodeFactory::getDevice(std::string& serial_no) +{ + auto list = _ctx.query_devices(); + if (0 == list.size()) + { + ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); + ros::shutdown(); + exit(1); + } + + bool found = false; + rs2::device retDev; + + for (auto&& dev : list) + { + auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); + ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); + if (serial_no.empty()) + { + retDev = dev; + serial_no = sn; + found = true; + break; + } + else if (sn == serial_no) + { + retDev = dev; + found = true; + break; + } + } + + if (!found) + { + ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); + ros::shutdown(); + exit(1); + } + + return retDev; +} + void RealSenseNodeFactory::onInit() { try{ @@ -37,123 +79,94 @@ void RealSenseNodeFactory::onInit() std::cout << "Press key to continue." << std::endl; std::cin.get(); #endif - auto nh = getNodeHandle(); - auto privateNh = getPrivateNodeHandle(); - std::string serial_no(""); - privateNh.param("serial_no", serial_no, std::string("")); - - std::string rosbag_filename(""); - privateNh.param("rosbag_filename", rosbag_filename, std::string("")); - if (!rosbag_filename.empty()) + std::mutex mtx; + std::condition_variable cv; + rs2::device dev; + _ctx.set_devices_changed_callback([&dev, &cv](rs2::event_information& info) { - ROS_INFO_STREAM("publish topics from rosbag file: " << rosbag_filename.c_str()); - auto pipe = std::make_shared(); - rs2::config cfg; - cfg.enable_device_from_file(rosbag_filename.c_str(), false); - cfg.enable_all_streams(); - pipe->start(cfg); //File will be opened in read mode at this point - auto _device = pipe->get_active_profile().get_device(); - _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, serial_no)); - } - else + if (info.was_removed(dev)) + { + cv.notify_one(); + } + }); + + auto privateNh = getPrivateNodeHandle(); + std::string serial_no(""); + privateNh.param("serial_no", serial_no, std::string("")); + dev = getDevice(serial_no); + ROS_INFO("Resetting device..."); + dev.hardware_reset(); + { - auto list = _ctx.query_devices(); - if (0 == list.size()) - { - ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); - ros::shutdown(); - exit(1); - } - - bool found = false; - for (auto&& dev : list) - { - auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); - if (serial_no.empty()) - { - _device = dev; - serial_no = sn; - found = true; - break; - } - else if (sn == serial_no) - { - _device = dev; - found = true; - break; - } - } - - if (!found) - { - ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); - ros::shutdown(); - exit(1); - } - - _ctx.set_devices_changed_callback([this](rs2::event_information& info) - { - if (info.was_removed(_device)) - { - ROS_FATAL("The device has been disconnected! Terminating RealSense Node..."); - ros::shutdown(); - exit(1); - } - }); - - // TODO - auto pid_str = _device.get_info(RS2_CAMERA_INFO_PRODUCT_ID); - uint16_t pid; - std::stringstream ss; - ss << std::hex << pid_str; - ss >> pid; - switch(pid) - { - case SR300_PID: - _realSenseNode = std::unique_ptr(new SR300Node(nh, privateNh, _device, serial_no)); - break; - case RS400_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS405_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS410_PID: - case RS460_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS415_PID: - _realSenseNode = std::unique_ptr(new RS415Node(nh, privateNh, _device, serial_no)); - break; - case RS420_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS420_MM_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS430_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS430_MM_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS430_MM_RGB_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS435_RGB_PID: - _realSenseNode = std::unique_ptr(new RS435Node(nh, privateNh, _device, serial_no)); - break; - case RS_USB2_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - default: - ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); - ros::shutdown(); - exit(1); - } - } - assert(_realSenseNode); + std::unique_lock lk(mtx); + cv.wait(lk); + } + + _device = getDevice(serial_no); + + _ctx.set_devices_changed_callback([this](rs2::event_information& info) + { + if (info.was_removed(_device)) + { + ROS_FATAL("The device has been disconnected! Terminating RealSense Node..."); + ros::shutdown(); + exit(1); + } + }); + + // TODO + auto nh = getNodeHandle(); + auto pid_str = _device.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + uint16_t pid; + std::stringstream ss; + ss << std::hex << pid_str; + ss >> pid; + switch(pid) + { + case SR300_PID: + _realSenseNode = std::unique_ptr(new SR300Node(nh, privateNh, _device, serial_no)); + break; + case RS400_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS405_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS410_PID: + case RS460_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS415_PID: + _realSenseNode = std::unique_ptr(new RS415Node(nh, privateNh, _device, serial_no)); + break; + case RS420_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS420_MM_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_MM_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_MM_RGB_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS435_RGB_PID: + _realSenseNode = std::unique_ptr(new RS435Node(nh, privateNh, _device, serial_no)); + break; + case RS_USB2_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + default: + ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); + ros::shutdown(); + exit(1); + } + + assert(_realSenseNode); _realSenseNode->publishTopics(); _realSenseNode->registerDynamicReconfigCb(); } From 2b4a798a6a763e49063ad60f5f8ebd7e9e4c4812 Mon Sep 17 00:00:00 2001 From: Jamie Cho Date: Mon, 6 Aug 2018 13:11:28 -0400 Subject: [PATCH 3/5] possible fix --- realsense2_camera/CMakeLists.txt | 1 + realsense2_camera/include/realsense_node_factory.h | 3 +++ 2 files changed, 4 insertions(+) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 761c6c2b86..c0e199b47f 100644 --- 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/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 71d0797a6b..a2248a149e 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -26,6 +26,9 @@ #include #include +#include +#include + namespace realsense2_camera { From 1e93b1cefa8bf5f24c635a032c8f1a90846b0827 Mon Sep 17 00:00:00 2001 From: Jamie Cho Date: Thu, 30 Aug 2018 17:04:02 -0400 Subject: [PATCH 4/5] fixed merge conflict while retaining hardware reset during initialization; added exec_depends to rgbd_launch --- .travis.yml | 9 +- .../include/base_realsense_node.h | 2 + realsense2_camera/package.xml | 1 + realsense2_camera/src/base_realsense_node.cpp | 118 +++++------ .../src/realsense_node_factory.cpp | 198 +++++++++++------- 5 files changed, 195 insertions(+), 133 deletions(-) diff --git a/.travis.yml b/.travis.yml index 05b063a6cc..0a59c7e1d8 100644 --- 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: - - ./.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 \ No newline at end of file + - $HOME/.android/build-cache diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 81b63856eb..60d041e91b 100644 --- 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 _depth_aligned_image; diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index e55cfd8ad4..a92f440020 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -35,6 +35,7 @@ tf dynamic_reconfigure diagnostic_updater + rgbd_launch diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index f00c4cd9b6..7468d0a62d 100644 --- 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(); - 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(); + 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) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 05c096c5a3..072a4b2cf1 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -83,93 +83,147 @@ void RealSenseNodeFactory::onInit() std::condition_variable cv; rs2::device dev; _ctx.set_devices_changed_callback([&dev, &cv](rs2::event_information& info) - { - if (info.was_removed(dev)) - { + { + if (info.was_removed(dev)) + { cv.notify_one(); - } - }); + } + }); + +#ifdef BPDEBUG + std::cout << "Attach to Process: " << getpid() << std::endl; + std::cout << "Press key to continue." << std::endl; + std::cin.get(); +#endif + auto nh = getNodeHandle(); auto privateNh = getPrivateNodeHandle(); std::string serial_no(""); privateNh.param("serial_no", serial_no, std::string("")); - dev = getDevice(serial_no); - ROS_INFO("Resetting device..."); - dev.hardware_reset(); + std::string rosbag_filename(""); + privateNh.param("rosbag_filename", rosbag_filename, std::string("")); + if (!rosbag_filename.empty()) { - std::unique_lock lk(mtx); - cv.wait(lk); - } + ROS_INFO_STREAM("publish topics from rosbag file: " << rosbag_filename.c_str()); + auto pipe = std::make_shared(); + rs2::config cfg; + cfg.enable_device_from_file(rosbag_filename.c_str(), false); + cfg.enable_all_streams(); + pipe->start(cfg); //File will be opened in read mode at this point + auto _device = pipe->get_active_profile().get_device(); + _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, serial_no)); + }else{ + + auto list = _ctx.query_devices(); + if (0 == list.size()) + { + ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); + ros::shutdown(); + exit(1); + } - _device = getDevice(serial_no); + bool found = false; + for (auto&& dev : list) + { + auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); + ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); + if (serial_no.empty()) + { + _device = dev; + serial_no = sn; + found = true; + break; + } + else if (sn == serial_no) + { + _device = dev; + found = true; + break; + } + } - _ctx.set_devices_changed_callback([this](rs2::event_information& info) - { - if (info.was_removed(_device)) + if (!found) { - ROS_FATAL("The device has been disconnected! Terminating RealSense Node..."); + ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); ros::shutdown(); exit(1); } - }); - // TODO - auto nh = getNodeHandle(); - auto pid_str = _device.get_info(RS2_CAMERA_INFO_PRODUCT_ID); - uint16_t pid; - std::stringstream ss; - ss << std::hex << pid_str; - ss >> pid; - switch(pid) - { - case SR300_PID: - _realSenseNode = std::unique_ptr(new SR300Node(nh, privateNh, _device, serial_no)); - break; - case RS400_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS405_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS410_PID: - case RS460_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS415_PID: - _realSenseNode = std::unique_ptr(new RS415Node(nh, privateNh, _device, serial_no)); - break; - case RS420_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS420_MM_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS430_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS430_MM_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS430_MM_RGB_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - case RS435_RGB_PID: - _realSenseNode = std::unique_ptr(new RS435Node(nh, privateNh, _device, serial_no)); - break; - case RS_USB2_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; - default: - ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); - ros::shutdown(); - exit(1); - } + ROS_INFO("Resetting device..."); + dev = getDevice(serial_no); + dev.hardware_reset(); + { + std::unique_lock lk(mtx); + cv.wait(lk); + } + + _device = getDevice(serial_no); + + _ctx.set_devices_changed_callback([this](rs2::event_information& info) + { + if (info.was_removed(_device)) + { + ROS_FATAL("The device has been disconnected! Terminating RealSense Node..."); + ros::shutdown(); + exit(1); + } + }); - assert(_realSenseNode); - _realSenseNode->publishTopics(); - _realSenseNode->registerDynamicReconfigCb(); - } + // TODO + auto pid_str = _device.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + uint16_t pid; + std::stringstream ss; + ss << std::hex << pid_str; + ss >> pid; + switch(pid) + { + case SR300_PID: + _realSenseNode = std::unique_ptr(new SR300Node(nh, privateNh, _device, serial_no)); + break; + case RS400_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS405_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS410_PID: + case RS460_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS415_PID: + _realSenseNode = std::unique_ptr(new RS415Node(nh, privateNh, _device, serial_no)); + break; + case RS420_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS420_MM_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_MM_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS430_MM_RGB_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + case RS435_RGB_PID: + _realSenseNode = std::unique_ptr(new RS435Node(nh, privateNh, _device, serial_no)); + break; + case RS_USB2_PID: + _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + break; + default: + ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); + ros::shutdown(); + exit(1); + } + } + assert(_realSenseNode); _realSenseNode->publishTopics(); + _realSenseNode->registerDynamicReconfigCb(); + } catch(const std::exception& ex) { ROS_ERROR_STREAM("An exception has been thrown: " << ex.what()); From d01c9957b67d41a7deaaafc5406375a753a8e268 Mon Sep 17 00:00:00 2001 From: Jamie Cho Date: Thu, 30 Aug 2018 17:11:23 -0400 Subject: [PATCH 5/5] removed unnecessary device query (artifact from merge) --- .../src/realsense_node_factory.cpp | 36 ------------------- 1 file changed, 36 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 072a4b2cf1..9c6a65c083 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -114,42 +114,6 @@ void RealSenseNodeFactory::onInit() auto _device = pipe->get_active_profile().get_device(); _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, serial_no)); }else{ - - auto list = _ctx.query_devices(); - if (0 == list.size()) - { - ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); - ros::shutdown(); - exit(1); - } - - bool found = false; - for (auto&& dev : list) - { - auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); - if (serial_no.empty()) - { - _device = dev; - serial_no = sn; - found = true; - break; - } - else if (sn == serial_no) - { - _device = dev; - found = true; - break; - } - } - - if (!found) - { - ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); - ros::shutdown(); - exit(1); - } - ROS_INFO("Resetting device..."); dev = getDevice(serial_no); dev.hardware_reset();