From 4538ba08027c782ba029129f74df68b456f4a2cc Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Wed, 24 Apr 2019 12:14:37 -0400 Subject: [PATCH 1/8] Refactor Driver * Get rid of the node factory and multiple derived nodes. Instead only params management is made polymorphic * Reset the device on depth callback timeout --- realsense2_camera/CMakeLists.txt | 23 +- .../{ => realsense2_camera}/constants.h | 0 .../realsense_node.h} | 102 ++++-- .../realsense2_camera/realsense_nodelet.h | 50 +++ .../rs415_param_manager.h} | 14 +- .../rs435_param_manager.h} | 13 +- .../sr300_param_manager.h} | 13 +- .../include/realsense_node_factory.h | 76 ---- realsense2_camera/nodelet_plugins.xml | 2 +- realsense2_camera/qt.cmake | 7 + ..._realsense_node.cpp => realsense_node.cpp} | 336 +++++++++++++----- .../src/realsense_node_factory.cpp | 241 ------------- realsense2_camera/src/realsense_nodelet.cpp | 78 ++++ realsense2_camera/src/rs415_node.cpp | 133 ------- realsense2_camera/src/rs435_node.cpp | 125 ------- ...sr300_node.cpp => sr300_param_manager.cpp} | 66 ++-- 16 files changed, 518 insertions(+), 761 deletions(-) rename realsense2_camera/include/{ => realsense2_camera}/constants.h (100%) rename realsense2_camera/include/{base_realsense_node.h => realsense2_camera/realsense_node.h} (75%) create mode 100644 realsense2_camera/include/realsense2_camera/realsense_nodelet.h rename realsense2_camera/include/{rs415_node.h => realsense2_camera/rs415_param_manager.h} (71%) rename realsense2_camera/include/{rs435_node.h => realsense2_camera/rs435_param_manager.h} (70%) rename realsense2_camera/include/{sr300_node.h => realsense2_camera/sr300_param_manager.h} (73%) delete mode 100644 realsense2_camera/include/realsense_node_factory.h create mode 100644 realsense2_camera/qt.cmake rename realsense2_camera/src/{base_realsense_node.cpp => realsense_node.cpp} (84%) delete mode 100644 realsense2_camera/src/realsense_node_factory.cpp create mode 100644 realsense2_camera/src/realsense_nodelet.cpp delete mode 100644 realsense2_camera/src/rs415_node.cpp delete mode 100644 realsense2_camera/src/rs435_node.cpp rename realsense2_camera/src/{sr300_node.cpp => sr300_param_manager.cpp} (53%) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index d53ea4154d..05ff5674d9 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -83,17 +83,11 @@ catkin_package( ) add_library(${PROJECT_NAME} - include/constants.h - include/realsense_node_factory.h - include/base_realsense_node.h - include/rs415_node.h - include/rs435_node.h - include/sr300_node.h - src/realsense_node_factory.cpp - src/base_realsense_node.cpp - src/rs415_node.cpp - src/rs435_node.cpp - src/sr300_node.cpp + src/realsense_nodelet.cpp + src/realsense_node.cpp + src/rs415_param_manager.cpp + src/rs435_param_manager.cpp + src/sr300_param_manager.cpp ) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) @@ -118,9 +112,9 @@ install(TARGETS ${PROJECT_NAME} ) # Install header files -install(DIRECTORY include/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - ) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) # Install launch files install(DIRECTORY launch/ @@ -136,3 +130,4 @@ install(DIRECTORY rviz/ install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +include(qt.cmake) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/realsense2_camera/constants.h similarity index 100% rename from realsense2_camera/include/constants.h rename to realsense2_camera/include/realsense2_camera/constants.h diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/realsense2_camera/realsense_node.h similarity index 75% rename from realsense2_camera/include/base_realsense_node.h rename to realsense2_camera/include/realsense2_camera/realsense_node.h index f8e69d660c..89a31eeab8 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/realsense2_camera/realsense_node.h @@ -3,19 +3,42 @@ #pragma once -#include "../include/realsense_node_factory.h" +#include +#include +#include +#include +#include +#include +#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include +#include namespace realsense2_camera { + class RealSenseParamManager; + enum base_depth_param{ base_depth_gain = 1, base_depth_enable_auto_exposure, @@ -76,38 +99,49 @@ namespace realsense2_camera class filter_options { public: - filter_options(const std::string name, rs2::process_interface& filter); + filter_options(const std::string name, rs2::process_interface &filter); filter_options(filter_options&& other); std::string filter_name; // Friendly name of the filter rs2::process_interface& filter; // The filter in use std::atomic_bool is_enabled; // A boolean controlled by the user that determines whether to apply the filter or not }; - class BaseRealSenseNode : public InterfaceRealSenseNode + class RealSenseNode { public: - BaseRealSenseNode(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, - const std::string& serial_no); + RealSenseNode(ros::NodeHandle& nodeHandle, ros::NodeHandle& privateNodeHandle); - virtual void publishTopics() override; - virtual void registerDynamicReconfigCb() override; - virtual ~BaseRealSenseNode() {} + void resetNode(); + void getDevice(); - protected: + void createParamsManager(); + + void publishTopics(); + ~RealSenseNode() {} + static constexpr stream_index_pair COLOR{RS2_STREAM_COLOR, 0}; + static constexpr stream_index_pair DEPTH{RS2_STREAM_DEPTH, 0}; + static constexpr stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1}; + static constexpr stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; + static constexpr stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0}; + static constexpr stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; + static constexpr stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; + + private: + rs2::context _ctx; const uint32_t set_default_dynamic_reconfig_values = 0xffffffff; rs2::device _dev; - ros::NodeHandle& _node_handle, _pnh; + std::string _rosbag_filename; + ros::NodeHandle _node_handle, _pnh; std::map _sensors; rs2::spatial_filter spat_filter; // Spatial - edge-preserving spatial smoothing rs2::temporal_filter temp_filter; // Temporal - reduces temporal noise rs2::disparity_transform depth_to_disparity{true}; rs2::disparity_transform disparity_to_depth{false}; std::vector filters; + std::mutex _mutex; + - private: struct float3 { float x, y, z; @@ -166,6 +200,9 @@ namespace realsense2_camera std::vector& out_vec); void TemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& stat); + + void setHealthTimers(); + std::string _json_file_path; std::string _serial_no; @@ -227,24 +264,43 @@ namespace realsense2_camera diagnostic_updater::Updater temp_diagnostic_updater_; ros::Timer temp_update_timer_; int temperature_; + ros::Timer depth_callback_timer_; + ros::Duration depth_callback_timeout_; + std::unique_ptr _params; + + const std::vector> IMAGE_STREAMS = {{{DEPTH, INFRA1, INFRA2}, + {COLOR}, + {FISHEYE}}}; + + const std::vector> HID_STREAMS = {{GYRO, ACCEL}}; + + friend class SR300ParamManager; + friend class RS415ParamManager; + friend class RS435ParamManager; + friend class D400ParamManager; };//end class - class BaseD400Node : public BaseRealSenseNode + + class RealSenseParamManager { + public: + virtual ~RealSenseParamManager() {}; + virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) = 0; + }; + + + class D400ParamManager : public RealSenseParamManager { public: - BaseD400Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - virtual void registerDynamicReconfigCb() override; + virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; protected: - void setParam(rs415_paramsConfig &config, base_depth_param param); - void setParam(rs435_paramsConfig &config, base_depth_param param); + void setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, base_depth_param param); + void setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, base_depth_param param); private: - void callback(base_d400_paramsConfig &config, uint32_t level); - void setOption(stream_index_pair sip, rs2_option opt, float val); - void setParam(base_d400_paramsConfig &config, base_depth_param param); + void callback(RealSenseNode* node_ptr,base_d400_paramsConfig &config, uint32_t level); + void setOption(RealSenseNode *node_ptr, stream_index_pair sip, rs2_option opt, float val); + void setParam(RealSenseNode* node_ptr,base_d400_paramsConfig &config, base_depth_param param); std::shared_ptr> _server; dynamic_reconfigure::Server::CallbackType _f; diff --git a/realsense2_camera/include/realsense2_camera/realsense_nodelet.h b/realsense2_camera/include/realsense2_camera/realsense_nodelet.h new file mode 100644 index 0000000000..0dbcbae640 --- /dev/null +++ b/realsense2_camera/include/realsense2_camera/realsense_nodelet.h @@ -0,0 +1,50 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2018 Intel Corporation. All Rights Reserved + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace realsense2_camera +{ + inline void signalHandler(int signum) + { + ROS_INFO_STREAM(strsignal(signum) << " Signal is received! Terminating RealSense Node..."); + ros::shutdown(); + exit(signum); + } + + class RealSenseNodelet : public nodelet::Nodelet + { + public: + RealSenseNodelet(); + virtual ~RealSenseNodelet() {} + + private: + virtual void onInit() override; + void tryGetLogSeverity(rs2_log_severity& severity) const; + std::unique_ptr _realSenseNode; + }; +}//end namespace diff --git a/realsense2_camera/include/rs415_node.h b/realsense2_camera/include/realsense2_camera/rs415_param_manager.h similarity index 71% rename from realsense2_camera/include/rs415_node.h rename to realsense2_camera/include/realsense2_camera/rs415_param_manager.h index ad1df12252..ceb982b34d 100644 --- a/realsense2_camera/include/rs415_node.h +++ b/realsense2_camera/include/realsense2_camera/rs415_param_manager.h @@ -3,7 +3,7 @@ #pragma once -#include "../include/base_realsense_node.h" +#include #include namespace realsense2_camera @@ -31,18 +31,14 @@ namespace realsense2_camera rs415_param_count }; - class RS415Node : public BaseD400Node + class RS415ParamManager : public D400ParamManager { public: - RS415Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - - virtual void registerDynamicReconfigCb() override; + virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; private: - void callback(rs415_paramsConfig &config, uint32_t level); - void setParam(rs415_paramsConfig &config, rs415_param param); + void callback(RealSenseNode* node_ptr, rs415_paramsConfig &config, uint32_t level); + void setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, rs415_param param); dynamic_reconfigure::Server _server; dynamic_reconfigure::Server::CallbackType _f; diff --git a/realsense2_camera/include/rs435_node.h b/realsense2_camera/include/realsense2_camera/rs435_param_manager.h similarity index 70% rename from realsense2_camera/include/rs435_node.h rename to realsense2_camera/include/realsense2_camera/rs435_param_manager.h index 95cf03a55a..abb44ef043 100644 --- a/realsense2_camera/include/rs435_node.h +++ b/realsense2_camera/include/realsense2_camera/rs435_param_manager.h @@ -3,7 +3,7 @@ #pragma once -#include "../include/base_realsense_node.h" +#include #include namespace realsense2_camera @@ -30,18 +30,15 @@ namespace realsense2_camera rs435_param_count }; - class RS435Node : public BaseD400Node + class RS435ParamManager : public D400ParamManager { public: - RS435Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - virtual void registerDynamicReconfigCb() override; + virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; private: - void callback(rs435_paramsConfig &config, uint32_t level); - void setParam(rs435_paramsConfig &config, rs435_param param); + void callback(RealSenseNode* node_ptr, rs435_paramsConfig &config, uint32_t level); + void setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, rs435_param param); dynamic_reconfigure::Server _server; dynamic_reconfigure::Server::CallbackType _f; diff --git a/realsense2_camera/include/sr300_node.h b/realsense2_camera/include/realsense2_camera/sr300_param_manager.h similarity index 73% rename from realsense2_camera/include/sr300_node.h rename to realsense2_camera/include/realsense2_camera/sr300_param_manager.h index 81d685e4fd..717ce364ca 100644 --- a/realsense2_camera/include/sr300_node.h +++ b/realsense2_camera/include/realsense2_camera/sr300_param_manager.h @@ -3,7 +3,7 @@ #pragma once -#include "../include/base_realsense_node.h" +#include #include namespace realsense2_camera @@ -32,18 +32,15 @@ namespace realsense2_camera sr300_param_count }; - class SR300Node : public BaseRealSenseNode + class SR300ParamManager : public RealSenseParamManager { public: - SR300Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - virtual void registerDynamicReconfigCb() override; + virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; private: - void callback(sr300_paramsConfig &config, uint32_t level); - void setParam(sr300_paramsConfig &config, sr300_param param); + void callback(RealSenseNode* node_ptr, sr300_paramsConfig &config, uint32_t level); + void setParam(RealSenseNode* node_ptr, sr300_paramsConfig &config, sr300_param param); dynamic_reconfigure::Server _server; dynamic_reconfigure::Server::CallbackType _f; diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h deleted file mode 100644 index 53544d463f..0000000000 --- a/realsense2_camera/include/realsense_node_factory.h +++ /dev/null @@ -1,76 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2018 Intel Corporation. All Rights Reserved - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace realsense2_camera -{ - const stream_index_pair COLOR{RS2_STREAM_COLOR, 0}; - const stream_index_pair DEPTH{RS2_STREAM_DEPTH, 0}; - const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1}; - const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; - const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0}; - const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; - const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; - - const std::vector> IMAGE_STREAMS = {{{DEPTH, INFRA1, INFRA2}, - {COLOR}, - {FISHEYE}}}; - - const std::vector> HID_STREAMS = {{GYRO, ACCEL}}; - - inline void signalHandler(int signum) - { - ROS_INFO_STREAM(strsignal(signum) << " Signal is received! Terminating RealSense Node..."); - ros::shutdown(); - exit(signum); - } - - class InterfaceRealSenseNode - { - public: - virtual void publishTopics() = 0; - virtual void registerDynamicReconfigCb() = 0; - virtual ~InterfaceRealSenseNode() = default; - }; - - class RealSenseNodeFactory : public nodelet::Nodelet - { - public: - RealSenseNodeFactory(); - virtual ~RealSenseNodeFactory() {} - - private: - rs2::device getDevice(std::string& serial_no); - virtual void onInit() override; - void tryGetLogSeverity(rs2_log_severity& severity) const; - - std::unique_ptr _realSenseNode; - rs2::device _device; - rs2::context _ctx; - }; -}//end namespace diff --git a/realsense2_camera/nodelet_plugins.xml b/realsense2_camera/nodelet_plugins.xml index 838d23a53e..ca73a3092d 100644 --- a/realsense2_camera/nodelet_plugins.xml +++ b/realsense2_camera/nodelet_plugins.xml @@ -1,5 +1,5 @@ - + Example camera nodelet using the Intel RealSense SDK 2.0 library for Intel RealSense SR300 and D400 cameras diff --git a/realsense2_camera/qt.cmake b/realsense2_camera/qt.cmake new file mode 100644 index 0000000000..2f354a3c62 --- /dev/null +++ b/realsense2_camera/qt.cmake @@ -0,0 +1,7 @@ +FILE( GLOB_RECURSE INCLUDES RELATIVE ${PROJECT_SOURCE_DIR} include/*.h ) +FILE( GLOB_RECURSE LAUNCH RELATIVE ${PROJECT_SOURCE_DIR} launch/*.launch ) +FILE( GLOB_RECURSE LAUNCH RELATIVE ${PROJECT_SOURCE_DIR} test/*.launch ) +FILE( GLOB_RECURSE LAUNCH RELATIVE ${PROJECT_SOURCE_DIR} test/*.cpp ) +FILE( GLOB_RECURSE LAUNCH RELATIVE ${PROJECT_SOURCE_DIR} scripts/*.* ) + +ADD_CUSTOM_TARGET(${PROJECT_NAME}_NonBuildingFiles SOURCES ${LAUNCH} ${INCLUDES} ) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/realsense_node.cpp similarity index 84% rename from realsense2_camera/src/base_realsense_node.cpp rename to realsense2_camera/src/realsense_node.cpp index 1bf0fdb51c..53fb9d7e5d 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/realsense_node.cpp @@ -1,25 +1,38 @@ -#include "../include/base_realsense_node.h" -#include "../include/sr300_node.h" +#include +#include +#include +#include +#include using namespace realsense2_camera; -std::string BaseRealSenseNode::getNamespaceStr() +constexpr stream_index_pair RealSenseNode::COLOR; +constexpr stream_index_pair RealSenseNode::DEPTH; +constexpr stream_index_pair RealSenseNode::INFRA1; +constexpr stream_index_pair RealSenseNode::INFRA2; +constexpr stream_index_pair RealSenseNode::FISHEYE; +constexpr stream_index_pair RealSenseNode::GYRO; +constexpr stream_index_pair RealSenseNode::ACCEL; + +std::string RealSenseNode::getNamespaceStr() { auto ns = ros::this_node::getNamespace(); ns.erase(std::remove(ns.begin(), ns.end(), '/'), ns.end()); return ns; } -BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, - const std::string& serial_no) : - _dev(dev), _node_handle(nodeHandle), - _pnh(privateNodeHandle), _json_file_path(""), - _serial_no(serial_no), _base_frame_id(""), +RealSenseNode::RealSenseNode(ros::NodeHandle& nodeHandle, + ros::NodeHandle& privateNodeHandle) : + _node_handle(nodeHandle), + _pnh(privateNodeHandle), + _json_file_path(""), + _base_frame_id(""), _intialize_time_base(false), _namespace(getNamespaceStr()) { + getDevice(); + createParamsManager(); + // Types for depth stream _is_frame_arrived[DEPTH] = false; _format[DEPTH] = RS2_FORMAT_Z16; // libRS type @@ -93,25 +106,154 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, filters[3].is_enabled = false; _prev_camera_time_stamp = 0; + + publishTopics(); } -void BaseRealSenseNode::publishTopics() +void RealSenseNode::createParamsManager() { + auto pid_str = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + uint16_t pid; + std::stringstream ss; + ss << std::hex << pid_str; + ss >> pid; + ROS_WARN_STREAM ("pid " << pid); + + switch(pid) + { + case SR300_PID: + _params = std::unique_ptr(new SR300ParamManager); + break; + case RS400_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS405_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS410_PID: + case RS460_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS415_PID: + _params = std::unique_ptr(new RS415ParamManager); + break; + case RS420_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS420_MM_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS430_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS430_MM_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS430_MM_RGB_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + case RS435_RGB_PID: + _params = std::unique_ptr(new RS435ParamManager); + break; + case RS_USB2_PID: + _params = std::unique_ptr(new D400ParamManager); + break; + default: + ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); + ros::Duration(20).sleep(); + resetNode(); + } + +} + +void RealSenseNode::resetNode() { + auto& nh = _node_handle; + auto& pnh = _pnh; + this->~RealSenseNode(); + new (this) RealSenseNode(nh, pnh); +} + +void RealSenseNode::getDevice() { + if (!_rosbag_filename.empty()) + { + 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 + { + namespace bi = boost::interprocess; + bi::named_mutex usb_mutex{bi::open_or_create, "usb_mutex"}; + usb_mutex.lock(); + auto list = _ctx.query_devices(); + usb_mutex.unlock(); + 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()) + { + _dev = dev; + _serial_no = sn; + found = true; + break; + } + else if (sn == _serial_no) + { + _dev = dev; + found = true; + break; + } + } + + if (!found) + { + ROS_ERROR_STREAM("The requested device with serial number " << _serial_no << " is NOT found!"); + ros::Duration(20).sleep(); + resetNode(); + } + + _ctx.set_devices_changed_callback([this](rs2::event_information& info) + { + if (info.was_removed(_dev)) + { + ROS_ERROR("The device has been disconnected!"); + } + }); + } +} + +void RealSenseNode::publishTopics() { getParameters(); setupDevice(); + setHealthTimers(); setupPublishers(); setupServices(); setupStreams(); publishStaticTransforms(); ROS_INFO_STREAM("RealSense Node Is Up!"); -} -void BaseRealSenseNode::registerDynamicReconfigCb() -{ - ROS_INFO("Dynamic reconfig parameters is not implemented in the base node."); + if (_params) + { + _params->registerDynamicReconfigCb(this); + } } -bool BaseRealSenseNode::enableStreams(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) + +bool RealSenseNode::enableStreams(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { res.success = true; for (auto& streams : IMAGE_STREAMS) @@ -137,6 +279,7 @@ bool BaseRealSenseNode::enableStreams(std_srvs::SetBool::Request& req, std_srvs: res.message += std::string("Failed to start stream: ") + e.what() + '\n'; res.success = false; } + depth_callback_timer_.start(); } else { @@ -156,7 +299,7 @@ bool BaseRealSenseNode::enableStreams(std_srvs::SetBool::Request& req, std_srvs: return true; } -void BaseRealSenseNode::getParameters() +void RealSenseNode::getParameters() { ROS_INFO("getParameters..."); @@ -227,9 +370,15 @@ void BaseRealSenseNode::getParameters() _pnh.param("aligned_depth_to_infra1_frame_id", _depth_aligned_frame_id[INFRA1], DEFAULT_ALIGNED_DEPTH_TO_INFRA1_FRAME_ID); _pnh.param("aligned_depth_to_infra2_frame_id", _depth_aligned_frame_id[INFRA2], DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID); _pnh.param("aligned_depth_to_fisheye_frame_id", _depth_aligned_frame_id[FISHEYE], DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID); + + _pnh.param("rosbag_filename", _rosbag_filename, _rosbag_filename); + + double depth_callback_timeout = 30; // seconds + _pnh.param("depth_callback_timeout", depth_callback_timeout, depth_callback_timeout); + depth_callback_timeout_ = ros::Duration(depth_callback_timeout); } -void BaseRealSenseNode::setupDevice() +void RealSenseNode::setupDevice() { ROS_INFO("setupDevice..."); try{ @@ -342,32 +491,39 @@ void BaseRealSenseNode::setupDevice() } } -void BaseRealSenseNode::TemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& stat) +void RealSenseNode::TemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& stat) { - auto dbg = _dev.as(); - std::vector cmd = { 0x14, 0, 0xab, 0xcd, 0x2a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + try + { + auto dbg = _dev.as(); + std::vector cmd = { 0x14, 0, 0xab, 0xcd, 0x2a, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - auto res = dbg.send_and_receive_raw_data(cmd); - temperature_ = res[4]; + auto res = dbg.send_and_receive_raw_data(cmd); + temperature_ = res[4]; - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK"); + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK"); - stat.add("Projector Temperature", temperature_); - if (temperature_ > 50) { - stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Temperature is Higher than 50 Degree Celsius"); + stat.add("Projector Temperature", temperature_); + if (temperature_ > 50) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Temperature is Higher than 50 Degree Celsius"); + } + } + catch (const rs2::error& e) + { + ROS_ERROR_STREAM_THROTTLE(3, "Can not check device temperature " << e.what()); } - } -void BaseRealSenseNode::setupServices() +void RealSenseNode::setupServices() { ROS_INFO("setupServices..."); - _enable_streams_service = _pnh.advertiseService("enable_streams", &BaseRealSenseNode::enableStreams, this); + _enable_streams_service = _pnh.advertiseService("enable_streams", &RealSenseNode::enableStreams, this); } -void BaseRealSenseNode::setupPublishers() +void RealSenseNode::setupPublishers() { ROS_INFO("setupPublishers..."); image_transport::ImageTransport image_transport(_node_handle); @@ -382,7 +538,7 @@ void BaseRealSenseNode::setupPublishers() } temp_diagnostic_updater_.setHardwareIDf("D435_temperature"); - temp_diagnostic_updater_.add("Temperature", this, &BaseRealSenseNode::TemperatureUpdate); + temp_diagnostic_updater_.add("Temperature", this, &RealSenseNode::TemperatureUpdate); temp_update_timer_ = _node_handle.createTimer(ros::Duration(0.1), boost::bind(&diagnostic_updater::Updater::update, &temp_diagnostic_updater_)); @@ -462,7 +618,7 @@ void BaseRealSenseNode::setupPublishers() } } -void BaseRealSenseNode::alignFrame(const rs2_intrinsics& from_intrin, +void RealSenseNode::alignFrame(const rs2_intrinsics& from_intrin, const rs2_intrinsics& other_intrin, rs2::frame from_image, uint32_t output_image_bytes_per_pixel, @@ -522,7 +678,7 @@ void BaseRealSenseNode::alignFrame(const rs2_intrinsics& from_intrin, } } -void BaseRealSenseNode::updateIsFrameArrived(std::map& is_frame_arrived, +void RealSenseNode::updateIsFrameArrived(std::map& is_frame_arrived, rs2_stream stream_type, int stream_index) { try @@ -535,7 +691,7 @@ void BaseRealSenseNode::updateIsFrameArrived(std::map& } } -void BaseRealSenseNode::publishAlignedDepthToOthers(rs2::frame depth_frame, const std::vector& frames, const ros::Time& t) +void RealSenseNode::publishAlignedDepthToOthers(rs2::frame depth_frame, const std::vector& frames, const ros::Time& t) { for (auto&& other_frame : frames) { @@ -569,7 +725,7 @@ void BaseRealSenseNode::publishAlignedDepthToOthers(rs2::frame depth_frame, cons } } -void BaseRealSenseNode::filterFrame(rs2::frame& frame) +void RealSenseNode::filterFrame(rs2::frame& frame) { for (auto&& filter : filters) { @@ -580,7 +736,7 @@ void BaseRealSenseNode::filterFrame(rs2::frame& frame) } } -void BaseRealSenseNode::enable_devices() +void RealSenseNode::enable_devices() { for (auto& streams : IMAGE_STREAMS) { @@ -640,7 +796,7 @@ void BaseRealSenseNode::enable_devices() } } -void BaseRealSenseNode::setupStreams() +void RealSenseNode::setupStreams() { ROS_INFO("setupStreams..."); enable_devices(); @@ -658,6 +814,7 @@ void BaseRealSenseNode::setupStreams() _frame_callback = [this](rs2::frame frame) { try{ + depth_callback_timer_.setPeriod(depth_callback_timeout_, true); // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, // and the incremental timestamp from the camera. // In sync mode the timestamp is based on ROS time @@ -801,6 +958,7 @@ void BaseRealSenseNode::setupStreams() { sens.start(_frame_callback); } + depth_callback_timer_.start(); } }//end for @@ -952,7 +1110,7 @@ void BaseRealSenseNode::setupStreams() } } -void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& video_profile) +void RealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& video_profile) { stream_index_pair stream_index{video_profile.stream_type(), video_profile.stream_index()}; auto intrinsic = video_profile.get_intrinsics(); @@ -1027,7 +1185,7 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v } } -tf::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotation[9]) const +tf::Quaternion RealSenseNode::rotationMatrixToQuaternion(const float rotation[9]) const { Eigen::Matrix3f m; // We need to be careful about the order, as RS2 rotation matrix is @@ -1039,7 +1197,7 @@ tf::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotatio return tf::Quaternion(q.x(), q.y(), q.z(), q.w()); } -void BaseRealSenseNode::publish_static_tf(const ros::Time& t, +void RealSenseNode::publish_static_tf(const ros::Time& t, const float3& trans, const quaternion& q, const std::string& from, @@ -1059,7 +1217,7 @@ void BaseRealSenseNode::publish_static_tf(const ros::Time& t, _static_tf_broadcaster.sendTransform(msg); } -void BaseRealSenseNode::publishStaticTransforms() +void RealSenseNode::publishStaticTransforms() { ROS_INFO("publishStaticTransforms..."); // Publish static transforms @@ -1177,7 +1335,7 @@ void BaseRealSenseNode::publishStaticTransforms() } } -void BaseRealSenseNode::publishDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived) +void RealSenseNode::publishDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived) { try { @@ -1246,7 +1404,7 @@ void BaseRealSenseNode::publishDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived) +void RealSenseNode::publishRgbToDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived) { try { @@ -1345,7 +1503,7 @@ void BaseRealSenseNode::publishRgbToDepthPCTopic(const ros::Time& t, const std:: _pointcloud_xyzrgb_publisher.publish(msg_pointcloud); } -Extrinsics BaseRealSenseNode::rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics, const std::string& frame_id) const +Extrinsics RealSenseNode::rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics, const std::string& frame_id) const { Extrinsics extrinsicsMsg; for (int i = 0; i < 9; ++i) @@ -1359,14 +1517,14 @@ Extrinsics BaseRealSenseNode::rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics return extrinsicsMsg; } -rs2_extrinsics BaseRealSenseNode::getRsExtrinsics(const stream_index_pair& from_stream, const stream_index_pair& to_stream) +rs2_extrinsics RealSenseNode::getRsExtrinsics(const stream_index_pair& from_stream, const stream_index_pair& to_stream) { auto& from = _enabled_profiles[from_stream].front(); auto& to = _enabled_profiles[to_stream].front(); return from.get_extrinsics_to(to); } -IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) +IMUInfo RealSenseNode::getImuInfo(const stream_index_pair& stream_index) { IMUInfo info{}; auto sp = _enabled_profiles[stream_index].front().as(); @@ -1394,7 +1552,7 @@ IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) return info; } -void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, +void RealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, const stream_index_pair& stream, std::map& images, const std::map& info_publishers, @@ -1456,7 +1614,16 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, } } -bool BaseRealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile) +void RealSenseNode::setHealthTimers() { + auto reset_this = [this](const ros::TimerEvent&) -> void + { + ROS_WARN_STREAM("RealSense " << _serial_no << " driver timeout! Resetting"); + resetNode(); + }; + depth_callback_timer_ = _node_handle.createTimer(depth_callback_timeout_, reset_this, false, false); +} + +bool RealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile) { // Assuming that all D400 SKUs have depth sensor auto profiles = _enabled_profiles[stream_index]; @@ -1471,38 +1638,31 @@ bool BaseRealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, } -BaseD400Node::BaseD400Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no) - : BaseRealSenseNode(nodeHandle, - privateNodeHandle, - dev, serial_no) -{} -void BaseD400Node::callback(base_d400_paramsConfig &config, uint32_t level) +void D400ParamManager::callback(RealSenseNode* node_ptr,base_d400_paramsConfig &config, uint32_t level) { ROS_DEBUG_STREAM("D400 - Level: " << level); - if (set_default_dynamic_reconfig_values == level) + if (node_ptr->set_default_dynamic_reconfig_values == level) { for (int i = 1 ; i < base_depth_count ; ++i) { ROS_DEBUG_STREAM("base_depth_param = " << i); - setParam(config ,(base_depth_param)i); + setParam(node_ptr, config ,(base_depth_param)i); } } else { - setParam(config, (base_depth_param)level); + setParam(node_ptr, config, (base_depth_param)level); } } -void BaseD400Node::setOption(stream_index_pair sip, rs2_option opt, float val) +void D400ParamManager::setOption(RealSenseNode* node_ptr,stream_index_pair sip, rs2_option opt, float val) { - _sensors[sip].set_option(opt, val); + node_ptr->_sensors[sip].set_option(opt, val); } -void BaseD400Node::setParam(rs435_paramsConfig &config, base_depth_param param) +void D400ParamManager::setParam(RealSenseNode* node_ptr,rs435_paramsConfig &config, base_depth_param param) { base_d400_paramsConfig base_config; base_config.base_depth_gain = config.rs435_depth_gain; @@ -1524,10 +1684,10 @@ void BaseD400Node::setParam(rs435_paramsConfig &config, base_depth_param param) base_config.base_temporal_filter_smooth_alpha = config.rs435_temporal_filter_smooth_alpha; base_config.base_temporal_filter_smooth_delta = config.rs435_temporal_filter_smooth_delta; base_config.base_temporal_filter_holes_fill = config.rs435_temporal_filter_holes_fill; - setParam(base_config, param); + setParam(node_ptr, base_config, param); } -void BaseD400Node::setParam(rs415_paramsConfig &config, base_depth_param param) +void D400ParamManager::setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, base_depth_param param) { base_d400_paramsConfig base_config; base_config.base_depth_gain = config.rs415_depth_gain; @@ -1549,10 +1709,10 @@ void BaseD400Node::setParam(rs415_paramsConfig &config, base_depth_param param) base_config.base_temporal_filter_smooth_alpha = config.rs415_temporal_filter_smooth_alpha; base_config.base_temporal_filter_smooth_delta = config.rs415_temporal_filter_smooth_delta; base_config.base_temporal_filter_holes_fill = config.rs415_temporal_filter_holes_fill; - setParam(base_config, param); + setParam(node_ptr, base_config, param); } -void BaseD400Node::setParam(base_d400_paramsConfig &config, base_depth_param param) +void D400ParamManager::setParam(RealSenseNode* node_ptr,base_d400_paramsConfig &config, base_depth_param param) { // W/O for zero param if (0 == param) @@ -1561,34 +1721,34 @@ void BaseD400Node::setParam(base_d400_paramsConfig &config, base_depth_param par switch (param) { case base_depth_gain: ROS_DEBUG_STREAM("base_depth_gain: " << config.base_depth_gain); - setOption(DEPTH, RS2_OPTION_GAIN, config.base_depth_gain); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_GAIN, config.base_depth_gain); break; case base_depth_enable_auto_exposure: ROS_DEBUG_STREAM("base_depth_enable_auto_exposure: " << config.base_depth_enable_auto_exposure); - setOption(DEPTH, RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.base_depth_enable_auto_exposure); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.base_depth_enable_auto_exposure); break; case base_depth_visual_preset: ROS_DEBUG_STREAM("base_depth_visual_preset: " << config.base_depth_visual_preset); - setOption(DEPTH, RS2_OPTION_VISUAL_PRESET, config.base_depth_visual_preset); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_VISUAL_PRESET, config.base_depth_visual_preset); break; case base_depth_frames_queue_size: ROS_DEBUG_STREAM("base_depth_frames_queue_size: " << config.base_depth_frames_queue_size); - setOption(DEPTH, RS2_OPTION_FRAMES_QUEUE_SIZE, config.base_depth_frames_queue_size); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_FRAMES_QUEUE_SIZE, config.base_depth_frames_queue_size); break; case base_depth_error_polling_enabled: ROS_DEBUG_STREAM("base_depth_error_polling_enabled: " << config.base_depth_error_polling_enabled); - setOption(DEPTH, RS2_OPTION_ERROR_POLLING_ENABLED, config.base_depth_error_polling_enabled); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ERROR_POLLING_ENABLED, config.base_depth_error_polling_enabled); break; case base_depth_output_trigger_enabled: ROS_DEBUG_STREAM("base_depth_output_trigger_enabled: " << config.base_depth_output_trigger_enabled); - setOption(DEPTH, RS2_OPTION_OUTPUT_TRIGGER_ENABLED, config.base_depth_output_trigger_enabled); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_OUTPUT_TRIGGER_ENABLED, config.base_depth_output_trigger_enabled); break; case base_depth_units: break; case base_JSON_file_path: { ROS_DEBUG_STREAM("base_JSON_file_path: " << config.base_JSON_file_path); - auto adv_dev = _dev.as(); + auto adv_dev = node_ptr->_dev.as(); if (!adv_dev) { ROS_WARN_STREAM("Device doesn't support Advanced Mode!"); @@ -1609,47 +1769,47 @@ void BaseD400Node::setParam(base_d400_paramsConfig &config, base_depth_param par } case base_enable_depth_to_disparity_filter: ROS_DEBUG_STREAM("base_enable_depth_to_disparity_filter: " << config.base_enable_depth_to_disparity_filter); - filters[DEPTH_TO_DISPARITY].is_enabled = config.base_enable_depth_to_disparity_filter; + node_ptr->filters[DEPTH_TO_DISPARITY].is_enabled = config.base_enable_depth_to_disparity_filter; break; case base_enable_spatial_filter: ROS_DEBUG_STREAM("base_enable_spatial_filter: " << config.base_enable_spatial_filter); - filters[SPATIAL].is_enabled = config.base_enable_spatial_filter; + node_ptr->filters[SPATIAL].is_enabled = config.base_enable_spatial_filter; break; case base_enable_temporal_filter: ROS_DEBUG_STREAM("base_enable_temporal_filter: " << config.base_enable_temporal_filter); - filters[TEMPORAL].is_enabled = config.base_enable_temporal_filter; + node_ptr->filters[TEMPORAL].is_enabled = config.base_enable_temporal_filter; break; case base_enable_disparity_to_depth_filter: ROS_DEBUG_STREAM("base_enable_disparity_to_depth_filter: " << config.base_enable_disparity_to_depth_filter); - filters[DISPARITY_TO_DEPTH].is_enabled = config.base_enable_disparity_to_depth_filter; + node_ptr->filters[DISPARITY_TO_DEPTH].is_enabled = config.base_enable_disparity_to_depth_filter; break; case base_spatial_filter_magnitude: ROS_DEBUG_STREAM("base_spatial_filter_magnitude: " << config.base_spatial_filter_magnitude); - filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, config.base_spatial_filter_magnitude); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, config.base_spatial_filter_magnitude); break; case base_spatial_filter_smooth_alpha: ROS_DEBUG_STREAM("base_spatial_filter_smooth_alpha: " << config.base_spatial_filter_smooth_alpha); - filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_spatial_filter_smooth_alpha); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_spatial_filter_smooth_alpha); break; case base_spatial_filter_smooth_delta: ROS_DEBUG_STREAM("base_spatial_filter_smooth_delta: " << config.base_spatial_filter_smooth_delta); - filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_spatial_filter_smooth_delta); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_spatial_filter_smooth_delta); break; case base_spatial_filter_holes_fill: ROS_DEBUG_STREAM("base_spatial_filter_holes_fill: " << config.base_spatial_filter_holes_fill); - filters[SPATIAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_spatial_filter_holes_fill); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_spatial_filter_holes_fill); break; case base_temporal_filter_smooth_alpha: ROS_DEBUG_STREAM("base_temporal_filter_smooth_alpha: " << config.base_temporal_filter_smooth_alpha); - filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_temporal_filter_smooth_alpha); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_temporal_filter_smooth_alpha); break; case base_temporal_filter_smooth_delta: ROS_DEBUG_STREAM("base_temporal_filter_smooth_delta: " << config.base_temporal_filter_smooth_delta); - filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_temporal_filter_smooth_delta); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_temporal_filter_smooth_delta); break; case base_temporal_filter_holes_fill: ROS_DEBUG_STREAM("base_temporal_filter_holes_fill: " << config.base_temporal_filter_holes_fill); - filters[TEMPORAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_temporal_filter_holes_fill); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_temporal_filter_holes_fill); break; default: ROS_WARN_STREAM("Unrecognized D400 param (" << param << ")"); @@ -1657,10 +1817,10 @@ void BaseD400Node::setParam(base_d400_paramsConfig &config, base_depth_param par } } -void BaseD400Node::registerDynamicReconfigCb() +void D400ParamManager::registerDynamicReconfigCb(RealSenseNode* node_ptr) { _server = std::make_shared>(); - _f = boost::bind(&BaseD400Node::callback, this, _1, _2); + _f = boost::bind(&D400ParamManager::callback, this, node_ptr, _1, _2); _server->setCallback(_f); } @@ -1675,4 +1835,4 @@ filter_options::filter_options(const std::string name, rs2::process_interface& f filter_options::filter_options(filter_options&& other) : filter_name(std::move(other.filter_name)), filter(other.filter), - is_enabled(other.is_enabled.load()) {} \ No newline at end of file + is_enabled(other.is_enabled.load()) {} diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp deleted file mode 100644 index 5724200ae5..0000000000 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ /dev/null @@ -1,241 +0,0 @@ -// 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 - -#include - -using namespace realsense2_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(realsense2_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{ -#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("")); - - std::string rosbag_filename(""); - privateNh.param("rosbag_filename", rosbag_filename, std::string("")); - if (!rosbag_filename.empty()) - { - 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 - { - namespace bi = boost::interprocess; - bi::named_mutex usb_mutex{bi::open_or_create, "usb_mutex"}; - usb_mutex.lock(); - auto list = _ctx.query_devices(); - usb_mutex.unlock(); - 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); - _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; - } - } - } -} diff --git a/realsense2_camera/src/realsense_nodelet.cpp b/realsense2_camera/src/realsense_nodelet.cpp new file mode 100644 index 0000000000..4d3170a10f --- /dev/null +++ b/realsense2_camera/src/realsense_nodelet.cpp @@ -0,0 +1,78 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved + +#include +#include +#include +#include + +#include + +using namespace realsense2_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(realsense2_camera::RealSenseNodelet, nodelet::Nodelet) + +RealSenseNodelet::RealSenseNodelet() +{ + 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); +} + + +void RealSenseNodelet::onInit() +{ + try{ +#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(); + _realSenseNode = std::unique_ptr(new RealSenseNode(nh, privateNh)); + } + 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 RealSenseNodelet::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; + } + } + } +} diff --git a/realsense2_camera/src/rs415_node.cpp b/realsense2_camera/src/rs415_node.cpp deleted file mode 100644 index f0050ebfd5..0000000000 --- a/realsense2_camera/src/rs415_node.cpp +++ /dev/null @@ -1,133 +0,0 @@ -#include "../include/rs415_node.h" - -using namespace realsense2_camera; - - -RS415Node::RS415Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no) - : BaseD400Node(nodeHandle, privateNodeHandle, dev, serial_no) -{} - -void RS415Node::registerDynamicReconfigCb() -{ - _f = boost::bind(&RS415Node::callback, this, _1, _2); - _server.setCallback(_f); -} - - -void RS415Node::setParam(rs415_paramsConfig &config, rs415_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case rs415_color_backlight_compensation: - ROS_DEBUG_STREAM("base_JSON_file_path: " << config.rs415_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs415_color_backlight_compensation); - break; - case rs415_color_brightness: - ROS_DEBUG_STREAM("rs415_color_brightness: " << config.rs415_color_brightness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs415_color_brightness); - break; - case rs415_color_contrast: - ROS_DEBUG_STREAM("rs415_color_contrast: " << config.rs415_color_contrast); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs415_color_contrast); - break; - case rs415_color_gain: - ROS_DEBUG_STREAM("rs415_color_gain: " << config.rs415_color_gain); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs415_color_gain); - break; - case rs415_color_gamma: - ROS_DEBUG_STREAM("rs415_color_gamma: " << config.rs415_color_gamma); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs415_color_gamma); - break; - case rs415_color_hue: - ROS_DEBUG_STREAM("rs415_color_hue: " << config.rs415_color_hue); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs415_color_hue); - break; - case rs415_color_saturation: - ROS_DEBUG_STREAM("rs415_color_saturation: " << config.rs415_color_saturation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs415_color_saturation); - break; - case rs415_color_sharpness: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs415_color_sharpness); - break; - case rs415_color_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_color_enable_auto_white_balance); - break; - case rs415_color_enable_auto_exposure: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_enable_auto_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs415_color_enable_auto_exposure); - break; - case rs415_color_exposure: - ROS_DEBUG_STREAM("rs415_color_exposure: " << config.rs415_color_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_color_exposure); - break; - case rs415_color_white_balance: - { - static const auto rs415_color_white_balance_factor = 10; - ROS_DEBUG_STREAM("rs415_color_white_balance: " << config.rs415_color_white_balance * rs415_color_white_balance_factor); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs415_color_white_balance * rs415_color_white_balance_factor); - } - break; - case rs415_color_frames_queue_size: - ROS_DEBUG_STREAM("rs415_color_frames_queue_size: " << config.rs415_color_frames_queue_size); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs415_color_frames_queue_size); - break; - case rs415_color_power_line_frequency: - ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_power_line_frequency); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs415_color_power_line_frequency); - break; - case rs415_color_auto_exposure_priority: - ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_auto_exposure_priority); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs415_color_auto_exposure_priority); - break; - case rs415_depth_exposure: - { - static const auto rs415_depth_exposure_factor = 20; - ROS_DEBUG_STREAM("rs415_depth_exposure: " << config.rs415_depth_exposure * rs415_depth_exposure_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_depth_exposure * rs415_depth_exposure_factor); - } - break; - case rs415_depth_laser_power: - { - static const auto rs415_depth_laser_power_factor = 30; - ROS_DEBUG_STREAM("rs415_depth_laser_power: " << config.rs415_depth_laser_power * rs415_depth_laser_power_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs415_depth_laser_power * rs415_depth_laser_power_factor); - } - break; - case rs415_depth_emitter_enabled: - ROS_DEBUG_STREAM("rs415_depth_emitter_enabled: " << config.rs415_depth_emitter_enabled); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs415_depth_emitter_enabled); - break; - case rs415_depth_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs415_depth_enable_auto_white_balance: " << config.rs415_depth_enable_auto_white_balance); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_depth_enable_auto_white_balance); - break; - default: - BaseD400Node::setParam(config, (base_depth_param)param); - break; - } -} - -void RS415Node::callback(rs415_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("RS415Node - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < rs415_param_count ; ++i) - { - ROS_DEBUG_STREAM("rs415_param = " << i); - setParam(config ,(rs415_param)i); - } - } - else - { - setParam(config, (rs415_param)level); - } -} diff --git a/realsense2_camera/src/rs435_node.cpp b/realsense2_camera/src/rs435_node.cpp deleted file mode 100644 index 4b0b0b26a6..0000000000 --- a/realsense2_camera/src/rs435_node.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include "../include/rs435_node.h" - -using namespace realsense2_camera; - - -RS435Node::RS435Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no) - : BaseD400Node(nodeHandle, privateNodeHandle, dev, serial_no) -{} - -void RS435Node::registerDynamicReconfigCb() -{ - _f = boost::bind(&RS435Node::callback, this, _1, _2); - _server.setCallback(_f); -} - -void RS435Node::setParam(rs435_paramsConfig &config, rs435_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case rs435_color_backlight_compensation: - ROS_DEBUG_STREAM("rs435_color_backlight_compensation: " << config.rs435_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs435_color_backlight_compensation); - break; - case rs435_color_brightness: - ROS_DEBUG_STREAM("rs435_color_brightness: " << config.rs435_color_brightness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs435_color_brightness); - break; - case rs435_color_contrast: - ROS_DEBUG_STREAM("rs435_color_contrast: " << config.rs435_color_contrast); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs435_color_contrast); - break; - case rs435_color_gain: - ROS_DEBUG_STREAM("rs435_color_gain: " << config.rs435_color_gain); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs435_color_gain); - break; - case rs435_color_gamma: - ROS_DEBUG_STREAM("rs435_color_gamma: " << config.rs435_color_gamma); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs435_color_gamma); - break; - case rs435_color_hue: - ROS_DEBUG_STREAM("rs435_color_hue: " << config.rs435_color_hue); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs435_color_hue); - break; - case rs435_color_saturation: - ROS_DEBUG_STREAM("rs435_color_saturation: " << config.rs435_color_saturation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs435_color_saturation); - break; - case rs435_color_sharpness: - ROS_DEBUG_STREAM("rs435_color_sharpness: " << config.rs435_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs435_color_sharpness); - break; - case rs435_color_enable_auto_exposure: - ROS_DEBUG_STREAM("rs435_color_enable_auto_exposure: " << config.rs435_color_enable_auto_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs435_color_enable_auto_exposure); - break; - case rs435_color_exposure: - ROS_DEBUG_STREAM("rs435_color_exposure: " << config.rs435_color_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_color_exposure); - break; - case rs435_color_white_balance: - ROS_DEBUG_STREAM("rs435_color_white_balance: " << config.rs435_color_white_balance * 10); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs435_color_white_balance * 10); - break; - case rs435_color_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs435_color_enable_auto_white_balance: " << config.rs435_color_enable_auto_white_balance); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs435_color_enable_auto_white_balance); - break; - case rs435_color_frames_queue_size: - ROS_DEBUG_STREAM("rs435_color_frames_queue_size: " << config.rs435_color_frames_queue_size); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs435_color_frames_queue_size); - break; - case rs435_color_power_line_frequency: - ROS_DEBUG_STREAM("rs435_color_power_line_frequency: " << config.rs435_color_power_line_frequency); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs435_color_power_line_frequency); - break; - case rs435_color_auto_exposure_priority: - ROS_DEBUG_STREAM("rs435_color_auto_exposure_priority: " << config.rs435_color_auto_exposure_priority); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs435_color_auto_exposure_priority); - break; - case rs435_depth_exposure: - { - static const auto rs435_depth_exposure_factor = 20; - ROS_DEBUG_STREAM("rs435_depth_exposure: " << config.rs435_depth_exposure * rs435_depth_exposure_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_depth_exposure * rs435_depth_exposure_factor); - } - break; - case rs435_depth_laser_power: - { - static const auto rs435_depth_laser_power_factor = 30; - ROS_DEBUG_STREAM("rs435_depth_laser_power: " << config.rs435_depth_laser_power * rs435_depth_laser_power_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs435_depth_laser_power * rs435_depth_laser_power_factor); - } - break; - case rs435_depth_emitter_enabled: - ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.rs435_depth_emitter_enabled); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs435_depth_emitter_enabled); - break; - default: - BaseD400Node::setParam(config, (base_depth_param)param); - break; - } -} - -void RS435Node::callback(rs435_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("RS435Node - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < rs435_param_count ; ++i) - { - ROS_DEBUG_STREAM("rs435_param = " << i); - setParam(config ,(rs435_param)i); - } - } - else - { - setParam(config, (rs435_param)level); - } -} diff --git a/realsense2_camera/src/sr300_node.cpp b/realsense2_camera/src/sr300_param_manager.cpp similarity index 53% rename from realsense2_camera/src/sr300_node.cpp rename to realsense2_camera/src/sr300_param_manager.cpp index a6590229b6..6d1ed33ef4 100644 --- a/realsense2_camera/src/sr300_node.cpp +++ b/realsense2_camera/src/sr300_param_manager.cpp @@ -1,18 +1,14 @@ -#include "../include/sr300_node.h" +#include using namespace realsense2_camera; -SR300Node::SR300Node(ros::NodeHandle& nodeHandle, ros::NodeHandle& privateNodeHandle, rs2::device dev, const std::string& serial_no) - : BaseRealSenseNode(nodeHandle, privateNodeHandle, dev, serial_no) -{} - -void SR300Node::registerDynamicReconfigCb() +void SR300ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) { - _f = boost::bind(&SR300Node::callback, this, _1, _2); + _f = boost::bind(&SR300ParamManager::callback, this, node_ptr, _1, _2); _server.setCallback(_f); } -void SR300Node::setParam(sr300_paramsConfig &config, sr300_param param) +void SR300ParamManager::setParam(RealSenseNode* node_ptr, sr300_paramsConfig &config, sr300_param param) { // W/O for zero param if (0 == param) @@ -21,85 +17,85 @@ void SR300Node::setParam(sr300_paramsConfig &config, sr300_param param) switch (param) { case sr300_param_color_backlight_compensation: ROS_DEBUG_STREAM("sr300_param_color_backlight_compensation: " << config.sr300_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.sr300_color_backlight_compensation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.sr300_color_backlight_compensation); break; case sr300_param_color_brightness: ROS_DEBUG_STREAM("sr300_color_backlight_compensation: " << config.sr300_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.sr300_color_brightness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.sr300_color_brightness); break; case sr300_param_color_contrast: ROS_DEBUG_STREAM("sr300_param_color_contrast: " << config.sr300_color_contrast); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.sr300_color_contrast); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.sr300_color_contrast); break; case sr300_param_color_gain: ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gain); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.sr300_color_gain); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.sr300_color_gain); break; case sr300_param_color_gamma: ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gamma); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.sr300_color_gamma); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.sr300_color_gamma); break; case sr300_param_color_hue: ROS_DEBUG_STREAM("sr300_param_color_hue: " << config.sr300_color_hue); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.sr300_color_hue); + node_ptr-> _sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.sr300_color_hue); break; case sr300_param_color_saturation: ROS_DEBUG_STREAM("sr300_param_color_saturation: " << config.sr300_color_saturation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.sr300_color_saturation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.sr300_color_saturation); break; case sr300_param_color_sharpness: ROS_DEBUG_STREAM("sr300_param_color_sharpness: " << config.sr300_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.sr300_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.sr300_color_sharpness); break; case sr300_param_color_white_balance: ROS_DEBUG_STREAM("sr300_param_color_white_balance: " << config.sr300_color_white_balance); - if (_sensors[COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)) - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 0); + if (node_ptr->_sensors[RealSenseNode::COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)) + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 0); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.sr300_color_white_balance); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.sr300_color_white_balance); break; case sr300_param_color_enable_auto_white_balance: ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.sr300_color_enable_auto_white_balance); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.sr300_color_enable_auto_white_balance); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.sr300_color_enable_auto_white_balance); break; case sr300_param_color_exposure: ROS_DEBUG_STREAM("sr300_param_color_exposure: " << config.sr300_color_exposure); - if (_sensors[COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE)) - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); + if (node_ptr->_sensors[RealSenseNode::COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE)) + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.sr300_color_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.sr300_color_exposure); break; case sr300_param_color_enable_auto_exposure: ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_color_enable_auto_white_balance); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.sr300_color_enable_auto_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.sr300_color_enable_auto_exposure); break; case sr300_param_depth_visual_preset: ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_visual_preset); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_VISUAL_PRESET, config.sr300_depth_visual_preset); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_VISUAL_PRESET, config.sr300_depth_visual_preset); break; case sr300_param_depth_laser_power: ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_laser_power); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.sr300_depth_laser_power); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.sr300_depth_laser_power); break; case sr300_param_depth_accuracy: ROS_DEBUG_STREAM("sr300_param_depth_accuracy: " << config.sr300_depth_accuracy); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_ACCURACY, config.sr300_depth_accuracy); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_ACCURACY, config.sr300_depth_accuracy); break; case sr300_param_depth_motion_range: ROS_DEBUG_STREAM("sr300_param_depth_motion_range: " << config.sr300_depth_motion_range); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_MOTION_RANGE, config.sr300_depth_motion_range); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_MOTION_RANGE, config.sr300_depth_motion_range); break; case sr300_param_depth_filter_option: ROS_DEBUG_STREAM("sr300_param_depth_filter_option: " << config.sr300_depth_filter_option); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_FILTER_OPTION, config.sr300_depth_filter_option); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_FILTER_OPTION, config.sr300_depth_filter_option); break; case sr300_param_depth_confidence_threshold: ROS_DEBUG_STREAM("sr300_param_depth_confidence_threshold: " << config.sr300_depth_confidence_threshold); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_CONFIDENCE_THRESHOLD, config.sr300_depth_confidence_threshold); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_CONFIDENCE_THRESHOLD, config.sr300_depth_confidence_threshold); break; case sr300_param_depth_frames_queue_size: ROS_DEBUG_STREAM("sr300_param_depth_frames_queue_size: " << config.sr300_depth_frames_queue_size); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.sr300_depth_frames_queue_size); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.sr300_depth_frames_queue_size); break; case sr300_param_depth_units: break; @@ -127,20 +123,20 @@ void SR300Node::setParam(sr300_paramsConfig &config, sr300_param param) // "sr300_auto_range_lower_threshold" } -void SR300Node::callback(sr300_paramsConfig &config, uint32_t level) +void SR300ParamManager::callback(RealSenseNode* node_ptr, sr300_paramsConfig &config, uint32_t level) { ROS_DEBUG_STREAM("SR300Node - Level: " << level); - if (set_default_dynamic_reconfig_values == level) + if (node_ptr->set_default_dynamic_reconfig_values == level) { for (int i = 1 ; i < sr300_param_count ; ++i) { ROS_DEBUG_STREAM("sr300_param = " << i); - setParam(config ,(sr300_param)i); + setParam(node_ptr, config ,(sr300_param)i); } } else { - setParam(config, (sr300_param)level); + setParam(node_ptr, config, (sr300_param)level); } } From 7b5cc264cdfeae98d0a60c4a6bc87570e7ef1b04 Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Mon, 29 Apr 2019 16:38:03 -0400 Subject: [PATCH 2/8] Add missing files --- realsense2_camera/src/rs415_param_manager.cpp | 126 ++++++++++++++++++ realsense2_camera/src/rs435_param_manager.cpp | 119 +++++++++++++++++ 2 files changed, 245 insertions(+) create mode 100644 realsense2_camera/src/rs415_param_manager.cpp create mode 100644 realsense2_camera/src/rs435_param_manager.cpp diff --git a/realsense2_camera/src/rs415_param_manager.cpp b/realsense2_camera/src/rs415_param_manager.cpp new file mode 100644 index 0000000000..c498edd5f1 --- /dev/null +++ b/realsense2_camera/src/rs415_param_manager.cpp @@ -0,0 +1,126 @@ +#include + +using namespace realsense2_camera; + +void RS415ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) +{ + _f = boost::bind(&RS415ParamManager::callback, this, node_ptr, _1, _2); + _server.setCallback(_f); +} + + +void RS415ParamManager::setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, rs415_param param) +{ + // W/O for zero param + if (0 == param) + return; + + switch (param) { + case rs415_color_backlight_compensation: + ROS_DEBUG_STREAM("base_JSON_file_path: " << config.rs415_color_backlight_compensation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs415_color_backlight_compensation); + break; + case rs415_color_brightness: + ROS_DEBUG_STREAM("rs415_color_brightness: " << config.rs415_color_brightness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs415_color_brightness); + break; + case rs415_color_contrast: + ROS_DEBUG_STREAM("rs415_color_contrast: " << config.rs415_color_contrast); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs415_color_contrast); + break; + case rs415_color_gain: + ROS_DEBUG_STREAM("rs415_color_gain: " << config.rs415_color_gain); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs415_color_gain); + break; + case rs415_color_gamma: + ROS_DEBUG_STREAM("rs415_color_gamma: " << config.rs415_color_gamma); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs415_color_gamma); + break; + case rs415_color_hue: + ROS_DEBUG_STREAM("rs415_color_hue: " << config.rs415_color_hue); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs415_color_hue); + break; + case rs415_color_saturation: + ROS_DEBUG_STREAM("rs415_color_saturation: " << config.rs415_color_saturation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs415_color_saturation); + break; + case rs415_color_sharpness: + ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs415_color_sharpness); + break; + case rs415_color_enable_auto_white_balance: + ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_color_enable_auto_white_balance); + break; + case rs415_color_enable_auto_exposure: + ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_enable_auto_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs415_color_enable_auto_exposure); + break; + case rs415_color_exposure: + ROS_DEBUG_STREAM("rs415_color_exposure: " << config.rs415_color_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_color_exposure); + break; + case rs415_color_white_balance: + { + static const auto rs415_color_white_balance_factor = 10; + ROS_DEBUG_STREAM("rs415_color_white_balance: " << config.rs415_color_white_balance * rs415_color_white_balance_factor); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs415_color_white_balance * rs415_color_white_balance_factor); + } + break; + case rs415_color_frames_queue_size: + ROS_DEBUG_STREAM("rs415_color_frames_queue_size: " << config.rs415_color_frames_queue_size); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs415_color_frames_queue_size); + break; + case rs415_color_power_line_frequency: + ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_power_line_frequency); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs415_color_power_line_frequency); + break; + case rs415_color_auto_exposure_priority: + ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_auto_exposure_priority); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs415_color_auto_exposure_priority); + break; + case rs415_depth_exposure: + { + static const auto rs415_depth_exposure_factor = 20; + ROS_DEBUG_STREAM("rs415_depth_exposure: " << config.rs415_depth_exposure * rs415_depth_exposure_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_depth_exposure * rs415_depth_exposure_factor); + } + break; + case rs415_depth_laser_power: + { + static const auto rs415_depth_laser_power_factor = 30; + ROS_DEBUG_STREAM("rs415_depth_laser_power: " << config.rs415_depth_laser_power * rs415_depth_laser_power_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs415_depth_laser_power * rs415_depth_laser_power_factor); + } + break; + case rs415_depth_emitter_enabled: + ROS_DEBUG_STREAM("rs415_depth_emitter_enabled: " << config.rs415_depth_emitter_enabled); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs415_depth_emitter_enabled); + break; + case rs415_depth_enable_auto_white_balance: + ROS_DEBUG_STREAM("rs415_depth_enable_auto_white_balance: " << config.rs415_depth_enable_auto_white_balance); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_depth_enable_auto_white_balance); + break; + default: + //D400ParamManager::setParam(config, (base_depth_param)param); + break; + } +} + +void RS415ParamManager::callback(RealSenseNode* node_ptr, rs415_paramsConfig &config, uint32_t level) +{ + ROS_DEBUG_STREAM("RS415ParamManager - Level: " << level); + + if (node_ptr->set_default_dynamic_reconfig_values == level) + { + for (int i = 1 ; i < rs415_param_count ; ++i) + { + ROS_DEBUG_STREAM("rs415_param = " << i); + setParam(node_ptr, config ,(rs415_param)i); + } + } + else + { + setParam(node_ptr, config, (rs415_param)level); + } +} diff --git a/realsense2_camera/src/rs435_param_manager.cpp b/realsense2_camera/src/rs435_param_manager.cpp new file mode 100644 index 0000000000..f14189da32 --- /dev/null +++ b/realsense2_camera/src/rs435_param_manager.cpp @@ -0,0 +1,119 @@ +#include + +using namespace realsense2_camera; + + +void RS435ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) +{ + _f = boost::bind(&RS435ParamManager::callback, this, node_ptr, _1, _2); + _server.setCallback(_f); +} + +void RS435ParamManager::setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, rs435_param param) +{ + // W/O for zero param + if (0 == param) + return; + + switch (param) { + case rs435_color_backlight_compensation: + ROS_DEBUG_STREAM("rs435_color_backlight_compensation: " << config.rs435_color_backlight_compensation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs435_color_backlight_compensation); + break; + case rs435_color_brightness: + ROS_DEBUG_STREAM("rs435_color_brightness: " << config.rs435_color_brightness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs435_color_brightness); + break; + case rs435_color_contrast: + ROS_DEBUG_STREAM("rs435_color_contrast: " << config.rs435_color_contrast); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs435_color_contrast); + break; + case rs435_color_gain: + ROS_DEBUG_STREAM("rs435_color_gain: " << config.rs435_color_gain); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs435_color_gain); + break; + case rs435_color_gamma: + ROS_DEBUG_STREAM("rs435_color_gamma: " << config.rs435_color_gamma); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs435_color_gamma); + break; + case rs435_color_hue: + ROS_DEBUG_STREAM("rs435_color_hue: " << config.rs435_color_hue); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs435_color_hue); + break; + case rs435_color_saturation: + ROS_DEBUG_STREAM("rs435_color_saturation: " << config.rs435_color_saturation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs435_color_saturation); + break; + case rs435_color_sharpness: + ROS_DEBUG_STREAM("rs435_color_sharpness: " << config.rs435_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs435_color_sharpness); + break; + case rs435_color_enable_auto_exposure: + ROS_DEBUG_STREAM("rs435_color_enable_auto_exposure: " << config.rs435_color_enable_auto_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs435_color_enable_auto_exposure); + break; + case rs435_color_exposure: + ROS_DEBUG_STREAM("rs435_color_exposure: " << config.rs435_color_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_color_exposure); + break; + case rs435_color_white_balance: + ROS_DEBUG_STREAM("rs435_color_white_balance: " << config.rs435_color_white_balance * 10); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs435_color_white_balance * 10); + break; + case rs435_color_enable_auto_white_balance: + ROS_DEBUG_STREAM("rs435_color_enable_auto_white_balance: " << config.rs435_color_enable_auto_white_balance); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs435_color_enable_auto_white_balance); + break; + case rs435_color_frames_queue_size: + ROS_DEBUG_STREAM("rs435_color_frames_queue_size: " << config.rs435_color_frames_queue_size); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs435_color_frames_queue_size); + break; + case rs435_color_power_line_frequency: + ROS_DEBUG_STREAM("rs435_color_power_line_frequency: " << config.rs435_color_power_line_frequency); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs435_color_power_line_frequency); + break; + case rs435_color_auto_exposure_priority: + ROS_DEBUG_STREAM("rs435_color_auto_exposure_priority: " << config.rs435_color_auto_exposure_priority); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs435_color_auto_exposure_priority); + break; + case rs435_depth_exposure: + { + static const auto rs435_depth_exposure_factor = 20; + ROS_DEBUG_STREAM("rs435_depth_exposure: " << config.rs435_depth_exposure * rs435_depth_exposure_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_depth_exposure * rs435_depth_exposure_factor); + } + break; + case rs435_depth_laser_power: + { + static const auto rs435_depth_laser_power_factor = 30; + ROS_DEBUG_STREAM("rs435_depth_laser_power: " << config.rs435_depth_laser_power * rs435_depth_laser_power_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs435_depth_laser_power * rs435_depth_laser_power_factor); + } + break; + case rs435_depth_emitter_enabled: + ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.rs435_depth_emitter_enabled); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs435_depth_emitter_enabled); + break; + default: + //D400ParamManager::setParam(config, (base_depth_param)param); + break; + } +} + +void RS435ParamManager::callback(RealSenseNode* node_ptr, rs435_paramsConfig &config, uint32_t level) +{ + ROS_DEBUG_STREAM("RS435Node - Level: " << level); + + if (node_ptr->set_default_dynamic_reconfig_values == level) + { + for (int i = 1 ; i < rs435_param_count ; ++i) + { + ROS_DEBUG_STREAM("rs435_param = " << i); + setParam(node_ptr, config ,(rs435_param)i); + } + } + else + { + setParam(node_ptr, config, (rs435_param)level); + } +} From b1886c6d7c2e80aa1fbefebe4affa15d2848027c Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Fri, 3 May 2019 16:58:23 -0400 Subject: [PATCH 3/8] bug fix --- .../realsense2_camera/realsense_node.h | 6 +-- .../realsense2_camera/rs415_param_manager.h | 4 +- .../realsense2_camera/rs435_param_manager.h | 10 ++-- .../realsense2_camera/sr300_param_manager.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_camera/src/realsense_node.cpp | 54 +------------------ realsense2_camera/src/rs415_param_manager.cpp | 4 +- realsense2_camera/src/rs435_param_manager.cpp | 3 +- realsense2_camera/src/sr300_param_manager.cpp | 3 +- 9 files changed, 19 insertions(+), 69 deletions(-) diff --git a/realsense2_camera/include/realsense2_camera/realsense_node.h b/realsense2_camera/include/realsense2_camera/realsense_node.h index 89a31eeab8..f2a32d6ca7 100644 --- a/realsense2_camera/include/realsense2_camera/realsense_node.h +++ b/realsense2_camera/include/realsense2_camera/realsense_node.h @@ -109,7 +109,7 @@ namespace realsense2_camera class RealSenseNode { public: - RealSenseNode(ros::NodeHandle& nodeHandle, ros::NodeHandle& privateNodeHandle); + RealSenseNode(const ros::NodeHandle& nodeHandle, const ros::NodeHandle& privateNodeHandle); void resetNode(); void getDevice(); @@ -293,10 +293,6 @@ namespace realsense2_camera public: virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; - protected: - void setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, base_depth_param param); - void setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, base_depth_param param); - private: void callback(RealSenseNode* node_ptr,base_d400_paramsConfig &config, uint32_t level); void setOption(RealSenseNode *node_ptr, stream_index_pair sip, rs2_option opt, float val); diff --git a/realsense2_camera/include/realsense2_camera/rs415_param_manager.h b/realsense2_camera/include/realsense2_camera/rs415_param_manager.h index ceb982b34d..42095b528a 100644 --- a/realsense2_camera/include/realsense2_camera/rs415_param_manager.h +++ b/realsense2_camera/include/realsense2_camera/rs415_param_manager.h @@ -31,7 +31,7 @@ namespace realsense2_camera rs415_param_count }; - class RS415ParamManager : public D400ParamManager + class RS415ParamManager : public RealSenseParamManager { public: virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; @@ -40,7 +40,7 @@ namespace realsense2_camera void callback(RealSenseNode* node_ptr, rs415_paramsConfig &config, uint32_t level); void setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, rs415_param param); - dynamic_reconfigure::Server _server; + std::shared_ptr> _server; dynamic_reconfigure::Server::CallbackType _f; }; } diff --git a/realsense2_camera/include/realsense2_camera/rs435_param_manager.h b/realsense2_camera/include/realsense2_camera/rs435_param_manager.h index abb44ef043..c47dbc91e7 100644 --- a/realsense2_camera/include/realsense2_camera/rs435_param_manager.h +++ b/realsense2_camera/include/realsense2_camera/rs435_param_manager.h @@ -30,17 +30,17 @@ namespace realsense2_camera rs435_param_count }; - class RS435ParamManager : public D400ParamManager + class RS435ParamManager : public RealSenseParamManager { public: virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; private: - void callback(RealSenseNode* node_ptr, rs435_paramsConfig &config, uint32_t level); - void setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, rs435_param param); + void callback(RealSenseNode* node_ptr, rs435_paramsConfig &config, uint32_t level); + void setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, rs435_param param); - dynamic_reconfigure::Server _server; - dynamic_reconfigure::Server::CallbackType _f; + std::shared_ptr> _server; + dynamic_reconfigure::Server::CallbackType _f; }; } diff --git a/realsense2_camera/include/realsense2_camera/sr300_param_manager.h b/realsense2_camera/include/realsense2_camera/sr300_param_manager.h index 717ce364ca..0ee9b492bb 100644 --- a/realsense2_camera/include/realsense2_camera/sr300_param_manager.h +++ b/realsense2_camera/include/realsense2_camera/sr300_param_manager.h @@ -42,7 +42,7 @@ namespace realsense2_camera void callback(RealSenseNode* node_ptr, sr300_paramsConfig &config, uint32_t level); void setParam(RealSenseNode* node_ptr, sr300_paramsConfig &config, sr300_param param); - dynamic_reconfigure::Server _server; + std::shared_ptr> _server; dynamic_reconfigure::Server::CallbackType _f; }; } diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 575a7b0803..4edb1dec5b 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.0.8 + 2.0.9 RealSense Camera package allowing access to Intel 3D D400 cameras Sergey Dorodnicov Itay Carpis diff --git a/realsense2_camera/src/realsense_node.cpp b/realsense2_camera/src/realsense_node.cpp index 53fb9d7e5d..6d9126cae2 100644 --- a/realsense2_camera/src/realsense_node.cpp +++ b/realsense2_camera/src/realsense_node.cpp @@ -21,8 +21,8 @@ std::string RealSenseNode::getNamespaceStr() return ns; } -RealSenseNode::RealSenseNode(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle) : +RealSenseNode::RealSenseNode(const ros::NodeHandle &nodeHandle, + const ros::NodeHandle &privateNodeHandle) : _node_handle(nodeHandle), _pnh(privateNodeHandle), _json_file_path(""), @@ -1662,56 +1662,6 @@ void D400ParamManager::setOption(RealSenseNode* node_ptr,stream_index_pair sip, node_ptr->_sensors[sip].set_option(opt, val); } -void D400ParamManager::setParam(RealSenseNode* node_ptr,rs435_paramsConfig &config, base_depth_param param) -{ - base_d400_paramsConfig base_config; - base_config.base_depth_gain = config.rs435_depth_gain; - base_config.base_depth_enable_auto_exposure = config.rs435_depth_enable_auto_exposure; - base_config.base_depth_visual_preset = config.rs435_depth_visual_preset; - base_config.base_depth_frames_queue_size = config.rs435_depth_frames_queue_size; - base_config.base_depth_error_polling_enabled = config.rs435_depth_error_polling_enabled; - base_config.base_depth_output_trigger_enabled = config.rs435_depth_output_trigger_enabled; - base_config.base_depth_units = config.rs435_depth_units; - base_config.base_JSON_file_path = config.rs435_JSON_file_path; - base_config.base_enable_depth_to_disparity_filter = config.rs435_enable_depth_to_disparity_filter; - base_config.base_enable_spatial_filter = config.rs435_enable_spatial_filter; - base_config.base_enable_temporal_filter = config.rs435_enable_temporal_filter; - base_config.base_enable_disparity_to_depth_filter = config.rs435_enable_disparity_to_depth_filter; - base_config.base_spatial_filter_magnitude = config.rs435_spatial_filter_magnitude; - base_config.base_spatial_filter_smooth_alpha = config.rs435_spatial_filter_smooth_alpha; - base_config.base_spatial_filter_smooth_delta = config.rs435_spatial_filter_smooth_delta; - base_config.base_spatial_filter_holes_fill = config.rs435_spatial_filter_holes_fill; - base_config.base_temporal_filter_smooth_alpha = config.rs435_temporal_filter_smooth_alpha; - base_config.base_temporal_filter_smooth_delta = config.rs435_temporal_filter_smooth_delta; - base_config.base_temporal_filter_holes_fill = config.rs435_temporal_filter_holes_fill; - setParam(node_ptr, base_config, param); -} - -void D400ParamManager::setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, base_depth_param param) -{ - base_d400_paramsConfig base_config; - base_config.base_depth_gain = config.rs415_depth_gain; - base_config.base_depth_enable_auto_exposure = config.rs415_depth_enable_auto_exposure; - base_config.base_depth_visual_preset = config.rs415_depth_visual_preset; - base_config.base_depth_frames_queue_size = config.rs415_depth_frames_queue_size; - base_config.base_depth_error_polling_enabled = config.rs415_depth_error_polling_enabled; - base_config.base_depth_output_trigger_enabled = config.rs415_depth_output_trigger_enabled; - base_config.base_depth_units = config.rs415_depth_units; - base_config.base_JSON_file_path = config.rs415_JSON_file_path; - base_config.base_enable_depth_to_disparity_filter = config.rs415_enable_depth_to_disparity_filter; - base_config.base_enable_spatial_filter = config.rs415_enable_spatial_filter; - base_config.base_enable_temporal_filter = config.rs415_enable_temporal_filter; - base_config.base_enable_disparity_to_depth_filter = config.rs415_enable_disparity_to_depth_filter; - base_config.base_spatial_filter_magnitude = config.rs415_spatial_filter_magnitude; - base_config.base_spatial_filter_smooth_alpha = config.rs415_spatial_filter_smooth_alpha; - base_config.base_spatial_filter_smooth_delta = config.rs415_spatial_filter_smooth_delta; - base_config.base_spatial_filter_holes_fill = config.rs415_spatial_filter_holes_fill; - base_config.base_temporal_filter_smooth_alpha = config.rs415_temporal_filter_smooth_alpha; - base_config.base_temporal_filter_smooth_delta = config.rs415_temporal_filter_smooth_delta; - base_config.base_temporal_filter_holes_fill = config.rs415_temporal_filter_holes_fill; - setParam(node_ptr, base_config, param); -} - void D400ParamManager::setParam(RealSenseNode* node_ptr,base_d400_paramsConfig &config, base_depth_param param) { // W/O for zero param diff --git a/realsense2_camera/src/rs415_param_manager.cpp b/realsense2_camera/src/rs415_param_manager.cpp index c498edd5f1..6799fc4931 100644 --- a/realsense2_camera/src/rs415_param_manager.cpp +++ b/realsense2_camera/src/rs415_param_manager.cpp @@ -1,11 +1,13 @@ #include +#include using namespace realsense2_camera; void RS415ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) { + _server = std::make_shared>(); _f = boost::bind(&RS415ParamManager::callback, this, node_ptr, _1, _2); - _server.setCallback(_f); + _server->setCallback(_f); } diff --git a/realsense2_camera/src/rs435_param_manager.cpp b/realsense2_camera/src/rs435_param_manager.cpp index f14189da32..39a23bd139 100644 --- a/realsense2_camera/src/rs435_param_manager.cpp +++ b/realsense2_camera/src/rs435_param_manager.cpp @@ -5,8 +5,9 @@ using namespace realsense2_camera; void RS435ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) { + _server = std::make_shared>(); _f = boost::bind(&RS435ParamManager::callback, this, node_ptr, _1, _2); - _server.setCallback(_f); + _server->setCallback(_f); } void RS435ParamManager::setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, rs435_param param) diff --git a/realsense2_camera/src/sr300_param_manager.cpp b/realsense2_camera/src/sr300_param_manager.cpp index 6d1ed33ef4..a67f54526a 100644 --- a/realsense2_camera/src/sr300_param_manager.cpp +++ b/realsense2_camera/src/sr300_param_manager.cpp @@ -4,8 +4,9 @@ using namespace realsense2_camera; void SR300ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) { + _server = std::make_shared>(); _f = boost::bind(&SR300ParamManager::callback, this, node_ptr, _1, _2); - _server.setCallback(_f); + _server->setCallback(_f); } void SR300ParamManager::setParam(RealSenseNode* node_ptr, sr300_paramsConfig &config, sr300_param param) From 5a95108146470456e4a4beaad9fae2b9c0ba8a6b Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Sat, 4 May 2019 17:48:53 -0400 Subject: [PATCH 4/8] Templatize RealSenseParamManager instead of inheritance --- realsense2_camera/CMakeLists.txt | 4 +- .../include/realsense2_camera/constants.h | 5 +- .../include/realsense2_camera/param_manager.h | 211 +++++++ .../realsense2_camera/realsense_node.h | 99 +--- .../realsense2_camera/realsense_nodelet.h | 31 +- .../realsense2_camera/rs415_param_manager.h | 46 -- .../realsense2_camera/rs435_param_manager.h | 46 -- .../realsense2_camera/sr300_param_manager.h | 48 -- realsense2_camera/package.xml | 2 +- realsense2_camera/src/param_manager.cpp | 556 ++++++++++++++++++ realsense2_camera/src/realsense_node.cpp | 201 +------ realsense2_camera/src/realsense_nodelet.cpp | 6 +- realsense2_camera/src/rs415_param_manager.cpp | 128 ---- realsense2_camera/src/rs435_param_manager.cpp | 120 ---- realsense2_camera/src/sr300_param_manager.cpp | 143 ----- 15 files changed, 827 insertions(+), 819 deletions(-) create mode 100644 realsense2_camera/include/realsense2_camera/param_manager.h delete mode 100644 realsense2_camera/include/realsense2_camera/rs415_param_manager.h delete mode 100644 realsense2_camera/include/realsense2_camera/rs435_param_manager.h delete mode 100644 realsense2_camera/include/realsense2_camera/sr300_param_manager.h create mode 100644 realsense2_camera/src/param_manager.cpp delete mode 100644 realsense2_camera/src/rs415_param_manager.cpp delete mode 100644 realsense2_camera/src/rs435_param_manager.cpp delete mode 100644 realsense2_camera/src/sr300_param_manager.cpp diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 05ff5674d9..9d015730a6 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -85,9 +85,7 @@ catkin_package( add_library(${PROJECT_NAME} src/realsense_nodelet.cpp src/realsense_node.cpp - src/rs415_param_manager.cpp - src/rs435_param_manager.cpp - src/sr300_param_manager.cpp + src/param_manager.cpp ) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) diff --git a/realsense2_camera/include/realsense2_camera/constants.h b/realsense2_camera/include/realsense2_camera/constants.h index 18c7b50186..aabd1ef4ef 100644 --- a/realsense2_camera/include/realsense2_camera/constants.h +++ b/realsense2_camera/include/realsense2_camera/constants.h @@ -1,7 +1,8 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2018 Intel Corporation. All Rights Reserved -#pragma once +#ifndef REALSENSE2_CAMERA_CONSTANTS_H +#define REALSENSE2_CAMERA_CONSTANTS_H #include @@ -94,3 +95,5 @@ namespace realsense2_camera using stream_index_pair = std::pair; } // namespace realsense2_camera + +#endif // REALSENSE2_CAMERA_CONSTANTS_H diff --git a/realsense2_camera/include/realsense2_camera/param_manager.h b/realsense2_camera/include/realsense2_camera/param_manager.h new file mode 100644 index 0000000000..a993d56158 --- /dev/null +++ b/realsense2_camera/include/realsense2_camera/param_manager.h @@ -0,0 +1,211 @@ +#ifndef REALSENSE2_CAMERA_PARAM_MANAGER_H +#define REALSENSE2_CAMERA_PARAM_MANAGER_H + +#include +#include +#include +#include +#include +#include + +namespace realsense2_camera +{ + +enum base_depth_param{ + base_depth_gain = 1, + base_depth_enable_auto_exposure, + base_depth_visual_preset, + base_depth_frames_queue_size, + base_depth_error_polling_enabled, + base_depth_output_trigger_enabled, + base_depth_units, + base_JSON_file_path, + base_enable_depth_to_disparity_filter, + base_enable_spatial_filter, + base_enable_temporal_filter, + base_enable_disparity_to_depth_filter, + base_spatial_filter_magnitude, + base_spatial_filter_smooth_alpha, + base_spatial_filter_smooth_delta, + base_spatial_filter_holes_fill, + base_temporal_filter_smooth_alpha, + base_temporal_filter_smooth_delta, + base_temporal_filter_holes_fill, + base_depth_count +}; + +enum rs435_param{ + rs435_depth_exposure = 20, + rs435_depth_laser_power, + rs435_depth_emitter_enabled, + rs435_color_backlight_compensation, + rs435_color_brightness, + rs435_color_contrast, + rs435_color_exposure, + rs435_color_gain, + rs435_color_gamma, + rs435_color_hue, + rs435_color_saturation, + rs435_color_sharpness, + rs435_color_white_balance, + rs435_color_enable_auto_exposure, + rs435_color_enable_auto_white_balance, + rs435_color_frames_queue_size, + rs435_color_power_line_frequency, + rs435_color_auto_exposure_priority, + rs435_param_count +}; + +enum rs415_param{ + rs415_depth_enable_auto_white_balance = 20, + rs415_depth_exposure, + rs415_depth_laser_power, + rs415_depth_emitter_enabled, + rs415_color_backlight_compensation, + rs415_color_brightness, + rs415_color_contrast, + rs415_color_exposure, + rs415_color_gain, + rs415_color_gamma, + rs415_color_hue, + rs415_color_saturation, + rs415_color_sharpness, + rs415_color_white_balance, + rs415_color_enable_auto_exposure, + rs415_color_enable_auto_white_balance, + rs415_color_frames_queue_size, + rs415_color_power_line_frequency, + rs415_color_auto_exposure_priority, + rs415_param_count +}; + +enum sr300_param{ + sr300_param_color_backlight_compensation = 1, + sr300_param_color_brightness, + sr300_param_color_contrast, + sr300_param_color_gain, + sr300_param_color_gamma, + sr300_param_color_hue, + sr300_param_color_saturation, + sr300_param_color_sharpness, + sr300_param_color_white_balance, + sr300_param_color_enable_auto_white_balance, + sr300_param_color_exposure, + sr300_param_color_enable_auto_exposure, + sr300_param_depth_visual_preset, + sr300_param_depth_laser_power, + sr300_param_depth_accuracy, + sr300_param_depth_motion_range, + sr300_param_depth_filter_option, + sr300_param_depth_confidence_threshold, + sr300_param_depth_frames_queue_size, + sr300_param_depth_units, + sr300_param_count +}; + + +class RealSenseParamManagerBase { +public: + virtual ~RealSenseParamManagerBase() {}; + virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) = 0; +}; + + +template +struct ModelTraits {}; + +template<> +struct ModelTraits { + using Config = base_d400_paramsConfig; + using Param = base_depth_param; + static const auto param_count = base_depth_param::base_depth_count; +}; + +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; +template<> struct ModelTraits : ModelTraits {}; + +template<> +struct ModelTraits { + using Config = rs415_paramsConfig; + using Param = rs415_param; + static const auto param_count = rs415_param::rs415_param_count; +}; + +template<> +struct ModelTraits { + using Config =rs435_paramsConfig; + using Param = rs435_param; + static const auto param_count = rs435_param::rs435_param_count; +}; + +template<> +struct ModelTraits { + using Config = sr300_paramsConfig; + using Param = sr300_param; + static const auto param_count = sr300_param::sr300_param_count; +}; + + +template +class RealSenseParamManager : public RealSenseParamManagerBase +{ +public: + using Param = typename ModelTraits::Param; + void setParam(RealSenseNode* node_ptr, typename ModelTraits::Config &config, Param param); + virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; + +private: + void callback(RealSenseNode* node_ptr, typename ModelTraits::Config &config, uint32_t level); + void setOption(RealSenseNode *node_ptr, stream_index_pair sip, rs2_option opt, float val); + + std::shared_ptr::Config>> _server; + typename dynamic_reconfigure::Server::Config>::CallbackType _f; +}; + + +template +void RealSenseParamManager::callback(RealSenseNode* node_ptr, typename ModelTraits::Config &config, uint32_t level) +{ + if (node_ptr->set_default_dynamic_reconfig_values == level) + { + for (int i = 1 ; i < ModelTraits::param_count ; ++i) + { + setParam(node_ptr, config , static_cast(i)); + } + } + else + { + setParam(node_ptr, config, static_cast(level)); + } +} + +using ParamManagerMaker = std::function()>; +template +using RSPM = RealSenseParamManager; + +const std::map param_makers = +{ + {RS400_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS405_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS410_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS460_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS420_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS420_MM_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS430_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS430_MM_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS430_MM_RGB_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS_USB2_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS415_PID, [](){return std::unique_ptr>(new RSPM);}}, + {RS435_RGB_PID, [](){return std::unique_ptr>(new RSPM);}}, + {SR300_PID, [](){return std::unique_ptr>(new RSPM);}} +}; + +} +#endif // REALSENSE2_CAMERA_PARAM_MANAGER_H diff --git a/realsense2_camera/include/realsense2_camera/realsense_node.h b/realsense2_camera/include/realsense2_camera/realsense_node.h index f2a32d6ca7..60d089a295 100644 --- a/realsense2_camera/include/realsense2_camera/realsense_node.h +++ b/realsense2_camera/include/realsense2_camera/realsense_node.h @@ -1,66 +1,48 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2018 Intel Corporation. All Rights Reserved -#pragma once +#ifndef REALSENSE2_CAMERA_REALSENSE_NODE_H +#define REALSENSE2_CAMERA_REALSENSE_NODE_H + +#include +#include +#include +#include + +#include -#include -#include -#include #include #include #include #include -#include -#include -#include -#include + #include +#include +#include +#include + +#include +#include +#include #include #include #include #include -#include +#include +#include + #include #include -#include -#include -#include -#include -#include -#include -#include #include #include -#include -#include namespace realsense2_camera { - class RealSenseParamManager; - - enum base_depth_param{ - base_depth_gain = 1, - base_depth_enable_auto_exposure, - base_depth_visual_preset, - base_depth_frames_queue_size, - base_depth_error_polling_enabled, - base_depth_output_trigger_enabled, - base_depth_units, - base_JSON_file_path, - base_enable_depth_to_disparity_filter, - base_enable_spatial_filter, - base_enable_temporal_filter, - base_enable_disparity_to_depth_filter, - base_spatial_filter_magnitude, - base_spatial_filter_smooth_alpha, - base_spatial_filter_smooth_delta, - base_spatial_filter_holes_fill, - base_temporal_filter_smooth_alpha, - base_temporal_filter_smooth_delta, - base_temporal_filter_holes_fill, - base_depth_count - }; + +class RealSenseParamManagerBase; +template +class RealSenseParamManager; enum filters{ DEPTH_TO_DISPARITY, @@ -266,7 +248,7 @@ namespace realsense2_camera int temperature_; ros::Timer depth_callback_timer_; ros::Duration depth_callback_timeout_; - std::unique_ptr _params; + std::unique_ptr _params; const std::vector> IMAGE_STREAMS = {{{DEPTH, INFRA1, INFRA2}, {COLOR}, @@ -274,31 +256,10 @@ namespace realsense2_camera const std::vector> HID_STREAMS = {{GYRO, ACCEL}}; - friend class SR300ParamManager; - friend class RS415ParamManager; - friend class RS435ParamManager; - friend class D400ParamManager; - };//end class + template + friend class RealSenseParamManager; + }; // end class +} // namespace realsense2_camera - class RealSenseParamManager { - public: - virtual ~RealSenseParamManager() {}; - virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) = 0; - }; - - - class D400ParamManager : public RealSenseParamManager - { - public: - virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; - - private: - void callback(RealSenseNode* node_ptr,base_d400_paramsConfig &config, uint32_t level); - void setOption(RealSenseNode *node_ptr, stream_index_pair sip, rs2_option opt, float val); - void setParam(RealSenseNode* node_ptr,base_d400_paramsConfig &config, base_depth_param param); - - std::shared_ptr> _server; - dynamic_reconfigure::Server::CallbackType _f; - }; -} +#endif REALSENSE2_CAMERA_REALSENSE_NODE_H diff --git a/realsense2_camera/include/realsense2_camera/realsense_nodelet.h b/realsense2_camera/include/realsense2_camera/realsense_nodelet.h index 0dbcbae640..7cfa8398aa 100644 --- a/realsense2_camera/include/realsense2_camera/realsense_nodelet.h +++ b/realsense2_camera/include/realsense2_camera/realsense_nodelet.h @@ -1,31 +1,14 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2018 Intel Corporation. All Rights Reserved -#pragma once +#ifndef REALSENSE2_CAMERA_REALSENSE_NODELET_H +#define REALSENSE2_CAMERA_REALSENSE_NODELET_H -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include + #include -#include -#include -#include namespace realsense2_camera { @@ -47,4 +30,6 @@ namespace realsense2_camera void tryGetLogSeverity(rs2_log_severity& severity) const; std::unique_ptr _realSenseNode; }; -}//end namespace +} // namespace realsense2_camera + +#endif // REALSENSE2_CAMERA_REALSENSE_NODE_H diff --git a/realsense2_camera/include/realsense2_camera/rs415_param_manager.h b/realsense2_camera/include/realsense2_camera/rs415_param_manager.h deleted file mode 100644 index 42095b528a..0000000000 --- a/realsense2_camera/include/realsense2_camera/rs415_param_manager.h +++ /dev/null @@ -1,46 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2018 Intel Corporation. All Rights Reserved - -#pragma once - -#include -#include - -namespace realsense2_camera -{ - enum rs415_param{ - rs415_depth_enable_auto_white_balance = 20, - rs415_depth_exposure, - rs415_depth_laser_power, - rs415_depth_emitter_enabled, - rs415_color_backlight_compensation, - rs415_color_brightness, - rs415_color_contrast, - rs415_color_exposure, - rs415_color_gain, - rs415_color_gamma, - rs415_color_hue, - rs415_color_saturation, - rs415_color_sharpness, - rs415_color_white_balance, - rs415_color_enable_auto_exposure, - rs415_color_enable_auto_white_balance, - rs415_color_frames_queue_size, - rs415_color_power_line_frequency, - rs415_color_auto_exposure_priority, - rs415_param_count - }; - - class RS415ParamManager : public RealSenseParamManager - { - public: - virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; - - private: - void callback(RealSenseNode* node_ptr, rs415_paramsConfig &config, uint32_t level); - void setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, rs415_param param); - - std::shared_ptr> _server; - dynamic_reconfigure::Server::CallbackType _f; - }; -} diff --git a/realsense2_camera/include/realsense2_camera/rs435_param_manager.h b/realsense2_camera/include/realsense2_camera/rs435_param_manager.h deleted file mode 100644 index c47dbc91e7..0000000000 --- a/realsense2_camera/include/realsense2_camera/rs435_param_manager.h +++ /dev/null @@ -1,46 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2018 Intel Corporation. All Rights Reserved - -#pragma once - -#include -#include - -namespace realsense2_camera -{ - enum rs435_param{ - rs435_depth_exposure = 20, - rs435_depth_laser_power, - rs435_depth_emitter_enabled, - rs435_color_backlight_compensation, - rs435_color_brightness, - rs435_color_contrast, - rs435_color_exposure, - rs435_color_gain, - rs435_color_gamma, - rs435_color_hue, - rs435_color_saturation, - rs435_color_sharpness, - rs435_color_white_balance, - rs435_color_enable_auto_exposure, - rs435_color_enable_auto_white_balance, - rs435_color_frames_queue_size, - rs435_color_power_line_frequency, - rs435_color_auto_exposure_priority, - rs435_param_count - }; - - class RS435ParamManager : public RealSenseParamManager - { - public: - - virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; - - private: - void callback(RealSenseNode* node_ptr, rs435_paramsConfig &config, uint32_t level); - void setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, rs435_param param); - - std::shared_ptr> _server; - dynamic_reconfigure::Server::CallbackType _f; - }; -} diff --git a/realsense2_camera/include/realsense2_camera/sr300_param_manager.h b/realsense2_camera/include/realsense2_camera/sr300_param_manager.h deleted file mode 100644 index 0ee9b492bb..0000000000 --- a/realsense2_camera/include/realsense2_camera/sr300_param_manager.h +++ /dev/null @@ -1,48 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2018 Intel Corporation. All Rights Reserved - -#pragma once - -#include -#include - -namespace realsense2_camera -{ - enum sr300_param{ - sr300_param_color_backlight_compensation = 1, - sr300_param_color_brightness, - sr300_param_color_contrast, - sr300_param_color_gain, - sr300_param_color_gamma, - sr300_param_color_hue, - sr300_param_color_saturation, - sr300_param_color_sharpness, - sr300_param_color_white_balance, - sr300_param_color_enable_auto_white_balance, - sr300_param_color_exposure, - sr300_param_color_enable_auto_exposure, - sr300_param_depth_visual_preset, - sr300_param_depth_laser_power, - sr300_param_depth_accuracy, - sr300_param_depth_motion_range, - sr300_param_depth_filter_option, - sr300_param_depth_confidence_threshold, - sr300_param_depth_frames_queue_size, - sr300_param_depth_units, - sr300_param_count - }; - - class SR300ParamManager : public RealSenseParamManager - { - public: - - virtual void registerDynamicReconfigCb(RealSenseNode *node_ptr) override; - - private: - void callback(RealSenseNode* node_ptr, sr300_paramsConfig &config, uint32_t level); - void setParam(RealSenseNode* node_ptr, sr300_paramsConfig &config, sr300_param param); - - std::shared_ptr> _server; - dynamic_reconfigure::Server::CallbackType _f; - }; -} diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 4edb1dec5b..41163bcb04 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.0.9 + 2.0.10 RealSense Camera package allowing access to Intel 3D D400 cameras Sergey Dorodnicov Itay Carpis diff --git a/realsense2_camera/src/param_manager.cpp b/realsense2_camera/src/param_manager.cpp new file mode 100644 index 0000000000..c11871bd51 --- /dev/null +++ b/realsense2_camera/src/param_manager.cpp @@ -0,0 +1,556 @@ +#include + +namespace realsense2_camera +{ + +template +void RealSenseParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) +{ + _server = std::make_shared::Config>>(); + _f = boost::bind(&RealSenseParamManager::callback, this, node_ptr, _1, _2); + _server->setCallback(_f); +} + +template +void RealSenseParamManager::setOption(RealSenseNode* node_ptr,stream_index_pair sip, rs2_option opt, float val) +{ + node_ptr->_sensors[sip].set_option(opt, val); +} + + +template<> +void RealSenseParamManager::setParam(RealSenseNode* node_ptr, typename ModelTraits::Config &config, Param param) +{ + // W/O for zero param + if (0 == param) + return; + + switch (param) { + case sr300_param_color_backlight_compensation: + ROS_DEBUG_STREAM("sr300_param_color_backlight_compensation: " << config.sr300_color_backlight_compensation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.sr300_color_backlight_compensation); + break; + case sr300_param_color_brightness: + ROS_DEBUG_STREAM("sr300_color_backlight_compensation: " << config.sr300_color_backlight_compensation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.sr300_color_brightness); + break; + case sr300_param_color_contrast: + ROS_DEBUG_STREAM("sr300_param_color_contrast: " << config.sr300_color_contrast); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.sr300_color_contrast); + break; + case sr300_param_color_gain: + ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gain); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.sr300_color_gain); + break; + case sr300_param_color_gamma: + ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gamma); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.sr300_color_gamma); + break; + case sr300_param_color_hue: + ROS_DEBUG_STREAM("sr300_param_color_hue: " << config.sr300_color_hue); + node_ptr-> _sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.sr300_color_hue); + break; + case sr300_param_color_saturation: + ROS_DEBUG_STREAM("sr300_param_color_saturation: " << config.sr300_color_saturation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.sr300_color_saturation); + break; + case sr300_param_color_sharpness: + ROS_DEBUG_STREAM("sr300_param_color_sharpness: " << config.sr300_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.sr300_color_sharpness); + break; + case sr300_param_color_white_balance: + ROS_DEBUG_STREAM("sr300_param_color_white_balance: " << config.sr300_color_white_balance); + if (node_ptr->_sensors[RealSenseNode::COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)) + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 0); + + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.sr300_color_white_balance); + break; + case sr300_param_color_enable_auto_white_balance: + ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.sr300_color_enable_auto_white_balance); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.sr300_color_enable_auto_white_balance); + break; + case sr300_param_color_exposure: + ROS_DEBUG_STREAM("sr300_param_color_exposure: " << config.sr300_color_exposure); + if (node_ptr->_sensors[RealSenseNode::COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE)) + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); + + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.sr300_color_exposure); + break; + case sr300_param_color_enable_auto_exposure: + ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_color_enable_auto_white_balance); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.sr300_color_enable_auto_exposure); + break; + case sr300_param_depth_visual_preset: + ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_visual_preset); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_VISUAL_PRESET, config.sr300_depth_visual_preset); + break; + case sr300_param_depth_laser_power: + ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_laser_power); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.sr300_depth_laser_power); + break; + case sr300_param_depth_accuracy: + ROS_DEBUG_STREAM("sr300_param_depth_accuracy: " << config.sr300_depth_accuracy); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_ACCURACY, config.sr300_depth_accuracy); + break; + case sr300_param_depth_motion_range: + ROS_DEBUG_STREAM("sr300_param_depth_motion_range: " << config.sr300_depth_motion_range); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_MOTION_RANGE, config.sr300_depth_motion_range); + break; + case sr300_param_depth_filter_option: + ROS_DEBUG_STREAM("sr300_param_depth_filter_option: " << config.sr300_depth_filter_option); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_FILTER_OPTION, config.sr300_depth_filter_option); + break; + case sr300_param_depth_confidence_threshold: + ROS_DEBUG_STREAM("sr300_param_depth_confidence_threshold: " << config.sr300_depth_confidence_threshold); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_CONFIDENCE_THRESHOLD, config.sr300_depth_confidence_threshold); + break; + case sr300_param_depth_frames_queue_size: + ROS_DEBUG_STREAM("sr300_param_depth_frames_queue_size: " << config.sr300_depth_frames_queue_size); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.sr300_depth_frames_queue_size); + break; + case sr300_param_depth_units: + break; + default: + ROS_WARN_STREAM("Unrecognized sr300 param (" << param << ")"); + break; + } +} + +template<> +void RealSenseParamManager::setParam(RealSenseNode* node_ptr, typename ModelTraits::Config &config, Param param) +{ + // W/O for zero param + if (0 == param) + return; + + switch (param) { + case base_depth_gain: + ROS_DEBUG_STREAM("base_depth_gain: " << config.base_depth_gain); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_GAIN, config.base_depth_gain); + break; + case base_depth_enable_auto_exposure: + ROS_DEBUG_STREAM("base_depth_enable_auto_exposure: " << config.base_depth_enable_auto_exposure); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.base_depth_enable_auto_exposure); + break; + case base_depth_visual_preset: + ROS_DEBUG_STREAM("base_depth_visual_preset: " << config.base_depth_visual_preset); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_VISUAL_PRESET, config.base_depth_visual_preset); + break; + case base_depth_frames_queue_size: + ROS_DEBUG_STREAM("base_depth_frames_queue_size: " << config.base_depth_frames_queue_size); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_FRAMES_QUEUE_SIZE, config.base_depth_frames_queue_size); + break; + case base_depth_error_polling_enabled: + ROS_DEBUG_STREAM("base_depth_error_polling_enabled: " << config.base_depth_error_polling_enabled); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ERROR_POLLING_ENABLED, config.base_depth_error_polling_enabled); + break; + case base_depth_output_trigger_enabled: + ROS_DEBUG_STREAM("base_depth_output_trigger_enabled: " << config.base_depth_output_trigger_enabled); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_OUTPUT_TRIGGER_ENABLED, config.base_depth_output_trigger_enabled); + break; + case base_depth_units: + break; + case base_JSON_file_path: + { + ROS_DEBUG_STREAM("base_JSON_file_path: " << config.base_JSON_file_path); + auto adv_dev = node_ptr->_dev.as(); + if (!adv_dev) + { + ROS_WARN_STREAM("Device doesn't support Advanced Mode!"); + return; + } + if (!config.base_JSON_file_path.empty()) + { + std::ifstream in(config.base_JSON_file_path); + if (!in.is_open()) + { + ROS_WARN_STREAM("JSON file provided doesn't exist!"); + return; + } + + adv_dev.load_json(config.base_JSON_file_path); + } + break; + } + case base_enable_depth_to_disparity_filter: + ROS_DEBUG_STREAM("base_enable_depth_to_disparity_filter: " << config.base_enable_depth_to_disparity_filter); + node_ptr->filters[DEPTH_TO_DISPARITY].is_enabled = config.base_enable_depth_to_disparity_filter; + break; + case base_enable_spatial_filter: + ROS_DEBUG_STREAM("base_enable_spatial_filter: " << config.base_enable_spatial_filter); + node_ptr->filters[SPATIAL].is_enabled = config.base_enable_spatial_filter; + break; + case base_enable_temporal_filter: + ROS_DEBUG_STREAM("base_enable_temporal_filter: " << config.base_enable_temporal_filter); + node_ptr->filters[TEMPORAL].is_enabled = config.base_enable_temporal_filter; + break; + case base_enable_disparity_to_depth_filter: + ROS_DEBUG_STREAM("base_enable_disparity_to_depth_filter: " << config.base_enable_disparity_to_depth_filter); + node_ptr->filters[DISPARITY_TO_DEPTH].is_enabled = config.base_enable_disparity_to_depth_filter; + break; + case base_spatial_filter_magnitude: + ROS_DEBUG_STREAM("base_spatial_filter_magnitude: " << config.base_spatial_filter_magnitude); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, config.base_spatial_filter_magnitude); + break; + case base_spatial_filter_smooth_alpha: + ROS_DEBUG_STREAM("base_spatial_filter_smooth_alpha: " << config.base_spatial_filter_smooth_alpha); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_spatial_filter_smooth_alpha); + break; + case base_spatial_filter_smooth_delta: + ROS_DEBUG_STREAM("base_spatial_filter_smooth_delta: " << config.base_spatial_filter_smooth_delta); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_spatial_filter_smooth_delta); + break; + case base_spatial_filter_holes_fill: + ROS_DEBUG_STREAM("base_spatial_filter_holes_fill: " << config.base_spatial_filter_holes_fill); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_spatial_filter_holes_fill); + break; + case base_temporal_filter_smooth_alpha: + ROS_DEBUG_STREAM("base_temporal_filter_smooth_alpha: " << config.base_temporal_filter_smooth_alpha); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_temporal_filter_smooth_alpha); + break; + case base_temporal_filter_smooth_delta: + ROS_DEBUG_STREAM("base_temporal_filter_smooth_delta: " << config.base_temporal_filter_smooth_delta); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_temporal_filter_smooth_delta); + break; + case base_temporal_filter_holes_fill: + ROS_DEBUG_STREAM("base_temporal_filter_holes_fill: " << config.base_temporal_filter_holes_fill); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_temporal_filter_holes_fill); + break; + default: + ROS_WARN_STREAM("Unrecognized D400 param (" << param << ")"); + break; + } +} + + +template<> +void RealSenseParamManager::setParam(RealSenseNode* node_ptr, typename ModelTraits::Config &config, Param param) +{ + // W/O for zero param + if (0 == param) + return; + switch (param) { + if (0 == param) + return; + + switch (param) { + case base_depth_gain: + ROS_DEBUG_STREAM("base_depth_gain: " << config.rs435_depth_gain); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_GAIN, config.rs435_depth_gain); + break; + case base_depth_enable_auto_exposure: + ROS_DEBUG_STREAM("base_depth_enable_auto_exposure: " << config.rs435_depth_enable_auto_exposure); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs435_depth_enable_auto_exposure); + break; + case base_depth_visual_preset: + ROS_DEBUG_STREAM("base_depth_visual_preset: " << config.rs435_depth_visual_preset); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_VISUAL_PRESET, config.rs435_depth_visual_preset); + break; + case base_depth_frames_queue_size: + ROS_DEBUG_STREAM("base_depth_frames_queue_size: " << config.rs435_depth_frames_queue_size); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs435_depth_frames_queue_size); + break; + case base_depth_error_polling_enabled: + ROS_DEBUG_STREAM("base_depth_error_polling_enabled: " << config.rs435_depth_error_polling_enabled); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ERROR_POLLING_ENABLED, config.rs435_depth_error_polling_enabled); + break; + case base_depth_output_trigger_enabled: + ROS_DEBUG_STREAM("base_depth_output_trigger_enabled: " << config.rs435_depth_output_trigger_enabled); + setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_OUTPUT_TRIGGER_ENABLED, config.rs435_depth_output_trigger_enabled); + break; + case base_depth_units: + break; + case base_JSON_file_path: + { + ROS_DEBUG_STREAM("base_JSON_file_path: " << config.rs435_JSON_file_path); + auto adv_dev = node_ptr->_dev.as(); + if (!adv_dev) + { + ROS_WARN_STREAM("Device doesn't support Advanced Mode!"); + return; + } + if (!config.rs435_JSON_file_path.empty()) + { + std::ifstream in(config.rs435_JSON_file_path); + if (!in.is_open()) + { + ROS_WARN_STREAM("JSON file provided doesn't exist!"); + return; + } + + adv_dev.load_json(config.rs435_JSON_file_path); + } + break; + } + case base_enable_depth_to_disparity_filter: + ROS_DEBUG_STREAM("base_enable_depth_to_disparity_filter: " << config.rs435_enable_depth_to_disparity_filter); + node_ptr->filters[DEPTH_TO_DISPARITY].is_enabled = config.rs435_enable_depth_to_disparity_filter; + break; + case base_enable_spatial_filter: + ROS_DEBUG_STREAM("base_enable_spatial_filter: " << config.rs435_enable_spatial_filter); + node_ptr->filters[SPATIAL].is_enabled = config.rs435_enable_spatial_filter; + break; + case base_enable_temporal_filter: + ROS_DEBUG_STREAM("base_enable_temporal_filter: " << config.rs435_enable_temporal_filter); + node_ptr->filters[TEMPORAL].is_enabled = config.rs435_enable_temporal_filter; + break; + case base_enable_disparity_to_depth_filter: + ROS_DEBUG_STREAM("base_enable_disparity_to_depth_filter: " << config.rs435_enable_disparity_to_depth_filter); + node_ptr->filters[DISPARITY_TO_DEPTH].is_enabled = config.rs435_enable_disparity_to_depth_filter; + break; + case base_spatial_filter_magnitude: + ROS_DEBUG_STREAM("base_spatial_filter_magnitude: " << config.rs435_spatial_filter_magnitude); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, config.rs435_spatial_filter_magnitude); + break; + case base_spatial_filter_smooth_alpha: + ROS_DEBUG_STREAM("base_spatial_filter_smooth_alpha: " << config.rs435_spatial_filter_smooth_alpha); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.rs435_spatial_filter_smooth_alpha); + break; + case base_spatial_filter_smooth_delta: + ROS_DEBUG_STREAM("base_spatial_filter_smooth_delta: " << config.rs435_spatial_filter_smooth_delta); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.rs435_spatial_filter_smooth_delta); + break; + case base_spatial_filter_holes_fill: + ROS_DEBUG_STREAM("base_spatial_filter_holes_fill: " << config.rs435_spatial_filter_holes_fill); + node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.rs435_spatial_filter_holes_fill); + break; + case base_temporal_filter_smooth_alpha: + ROS_DEBUG_STREAM("base_temporal_filter_smooth_alpha: " << config.rs435_temporal_filter_smooth_alpha); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.rs435_temporal_filter_smooth_alpha); + break; + case base_temporal_filter_smooth_delta: + ROS_DEBUG_STREAM("base_temporal_filter_smooth_delta: " << config.rs435_temporal_filter_smooth_delta); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.rs435_temporal_filter_smooth_delta); + break; + case base_temporal_filter_holes_fill: + ROS_DEBUG_STREAM("base_temporal_filter_holes_fill: " << config.rs435_temporal_filter_holes_fill); + node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.rs435_temporal_filter_holes_fill); + break; + } + case rs435_color_backlight_compensation: + ROS_DEBUG_STREAM("rs435_color_backlight_compensation: " << config.rs435_color_backlight_compensation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs435_color_backlight_compensation); + break; + case rs435_color_brightness: + ROS_DEBUG_STREAM("rs435_color_brightness: " << config.rs435_color_brightness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs435_color_brightness); + break; + case rs435_color_contrast: + ROS_DEBUG_STREAM("rs435_color_contrast: " << config.rs435_color_contrast); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs435_color_contrast); + break; + case rs435_color_gain: + ROS_DEBUG_STREAM("rs435_color_gain: " << config.rs435_color_gain); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs435_color_gain); + break; + case rs435_color_gamma: + ROS_DEBUG_STREAM("rs435_color_gamma: " << config.rs435_color_gamma); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs435_color_gamma); + break; + case rs435_color_hue: + ROS_DEBUG_STREAM("rs435_color_hue: " << config.rs435_color_hue); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs435_color_hue); + break; + case rs435_color_saturation: + ROS_DEBUG_STREAM("rs435_color_saturation: " << config.rs435_color_saturation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs435_color_saturation); + break; + case rs435_color_sharpness: + ROS_DEBUG_STREAM("rs435_color_sharpness: " << config.rs435_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs435_color_sharpness); + break; + case rs435_color_enable_auto_exposure: + ROS_DEBUG_STREAM("rs435_color_enable_auto_exposure: " << config.rs435_color_enable_auto_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs435_color_enable_auto_exposure); + break; + case rs435_color_exposure: + ROS_DEBUG_STREAM("rs435_color_exposure: " << config.rs435_color_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_color_exposure); + break; + case rs435_color_white_balance: + ROS_DEBUG_STREAM("rs435_color_white_balance: " << config.rs435_color_white_balance * 10); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs435_color_white_balance * 10); + break; + case rs435_color_enable_auto_white_balance: + ROS_DEBUG_STREAM("rs435_color_enable_auto_white_balance: " << config.rs435_color_enable_auto_white_balance); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs435_color_enable_auto_white_balance); + break; + case rs435_color_frames_queue_size: + ROS_DEBUG_STREAM("rs435_color_frames_queue_size: " << config.rs435_color_frames_queue_size); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs435_color_frames_queue_size); + break; + case rs435_color_power_line_frequency: + ROS_DEBUG_STREAM("rs435_color_power_line_frequency: " << config.rs435_color_power_line_frequency); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs435_color_power_line_frequency); + break; + case rs435_color_auto_exposure_priority: + ROS_DEBUG_STREAM("rs435_color_auto_exposure_priority: " << config.rs435_color_auto_exposure_priority); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs435_color_auto_exposure_priority); + break; + case rs435_depth_exposure: + { + static const auto rs435_depth_exposure_factor = 20; + ROS_DEBUG_STREAM("rs435_depth_exposure: " << config.rs435_depth_exposure * rs435_depth_exposure_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_depth_exposure * rs435_depth_exposure_factor); + } + break; + case rs435_depth_laser_power: + { + static const auto rs435_depth_laser_power_factor = 30; + ROS_DEBUG_STREAM("rs435_depth_laser_power: " << config.rs435_depth_laser_power * rs435_depth_laser_power_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs435_depth_laser_power * rs435_depth_laser_power_factor); + } + break; + case rs435_depth_emitter_enabled: + ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.rs435_depth_emitter_enabled); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs435_depth_emitter_enabled); + break; + default: + base_d400_paramsConfig base_config; + base_config.base_depth_gain = config.rs435_depth_gain; + base_config.base_depth_enable_auto_exposure = config.rs435_depth_enable_auto_exposure; + base_config.base_depth_visual_preset = config.rs435_depth_visual_preset; + base_config.base_depth_frames_queue_size = config.rs435_depth_frames_queue_size; + base_config.base_depth_error_polling_enabled = config.rs435_depth_error_polling_enabled; + base_config.base_depth_output_trigger_enabled = config.rs435_depth_output_trigger_enabled; + base_config.base_depth_units = config.rs435_depth_units; + base_config.base_JSON_file_path = config.rs435_JSON_file_path; + base_config.base_enable_depth_to_disparity_filter = config.rs435_enable_depth_to_disparity_filter; + base_config.base_enable_spatial_filter = config.rs435_enable_spatial_filter; + base_config.base_enable_temporal_filter = config.rs435_enable_temporal_filter; + base_config.base_enable_disparity_to_depth_filter = config.rs435_enable_disparity_to_depth_filter; + base_config.base_spatial_filter_magnitude = config.rs435_spatial_filter_magnitude; + base_config.base_spatial_filter_smooth_alpha = config.rs435_spatial_filter_smooth_alpha; + base_config.base_spatial_filter_smooth_delta = config.rs435_spatial_filter_smooth_delta; + base_config.base_spatial_filter_holes_fill = config.rs435_spatial_filter_holes_fill; + base_config.base_temporal_filter_smooth_alpha = config.rs435_temporal_filter_smooth_alpha; + base_config.base_temporal_filter_smooth_delta = config.rs435_temporal_filter_smooth_delta; + base_config.base_temporal_filter_holes_fill = config.rs435_temporal_filter_holes_fill; + RealSenseParamManager d400_param; + d400_param.setParam(node_ptr, base_config, static_cast::Param>(param)); + break; + } +} + + +template<> +void RealSenseParamManager::setParam(RealSenseNode* node_ptr, typename ModelTraits::Config &config, Param param) +{ + // W/O for zero param + if (0 == param) + return; + + switch (param) { + case rs415_color_backlight_compensation: + ROS_DEBUG_STREAM("base_JSON_file_path: " << config.rs415_color_backlight_compensation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs415_color_backlight_compensation); + break; + case rs415_color_brightness: + ROS_DEBUG_STREAM("rs415_color_brightness: " << config.rs415_color_brightness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs415_color_brightness); + break; + case rs415_color_contrast: + ROS_DEBUG_STREAM("rs415_color_contrast: " << config.rs415_color_contrast); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs415_color_contrast); + break; + case rs415_color_gain: + ROS_DEBUG_STREAM("rs415_color_gain: " << config.rs415_color_gain); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs415_color_gain); + break; + case rs415_color_gamma: + ROS_DEBUG_STREAM("rs415_color_gamma: " << config.rs415_color_gamma); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs415_color_gamma); + break; + case rs415_color_hue: + ROS_DEBUG_STREAM("rs415_color_hue: " << config.rs415_color_hue); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs415_color_hue); + break; + case rs415_color_saturation: + ROS_DEBUG_STREAM("rs415_color_saturation: " << config.rs415_color_saturation); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs415_color_saturation); + break; + case rs415_color_sharpness: + ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs415_color_sharpness); + break; + case rs415_color_enable_auto_white_balance: + ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_color_enable_auto_white_balance); + break; + case rs415_color_enable_auto_exposure: + ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_enable_auto_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs415_color_enable_auto_exposure); + break; + case rs415_color_exposure: + ROS_DEBUG_STREAM("rs415_color_exposure: " << config.rs415_color_exposure); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_color_exposure); + break; + case rs415_color_white_balance: + { + static const auto rs415_color_white_balance_factor = 10; + ROS_DEBUG_STREAM("rs415_color_white_balance: " << config.rs415_color_white_balance * rs415_color_white_balance_factor); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs415_color_white_balance * rs415_color_white_balance_factor); + } + break; + case rs415_color_frames_queue_size: + ROS_DEBUG_STREAM("rs415_color_frames_queue_size: " << config.rs415_color_frames_queue_size); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs415_color_frames_queue_size); + break; + case rs415_color_power_line_frequency: + ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_power_line_frequency); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs415_color_power_line_frequency); + break; + case rs415_color_auto_exposure_priority: + ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_auto_exposure_priority); + node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs415_color_auto_exposure_priority); + break; + case rs415_depth_exposure: + { + static const auto rs415_depth_exposure_factor = 20; + ROS_DEBUG_STREAM("rs415_depth_exposure: " << config.rs415_depth_exposure * rs415_depth_exposure_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_depth_exposure * rs415_depth_exposure_factor); + } + break; + case rs415_depth_laser_power: + { + static const auto rs415_depth_laser_power_factor = 30; + ROS_DEBUG_STREAM("rs415_depth_laser_power: " << config.rs415_depth_laser_power * rs415_depth_laser_power_factor); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs415_depth_laser_power * rs415_depth_laser_power_factor); + } + break; + case rs415_depth_emitter_enabled: + ROS_DEBUG_STREAM("rs415_depth_emitter_enabled: " << config.rs415_depth_emitter_enabled); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs415_depth_emitter_enabled); + break; + case rs415_depth_enable_auto_white_balance: + ROS_DEBUG_STREAM("rs415_depth_enable_auto_white_balance: " << config.rs415_depth_enable_auto_white_balance); + node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_depth_enable_auto_white_balance); + break; + default: + base_d400_paramsConfig base_config; + base_config.base_depth_gain = config.rs415_depth_gain; + base_config.base_depth_enable_auto_exposure = config.rs415_depth_enable_auto_exposure; + base_config.base_depth_visual_preset = config.rs415_depth_visual_preset; + base_config.base_depth_frames_queue_size = config.rs415_depth_frames_queue_size; + base_config.base_depth_error_polling_enabled = config.rs415_depth_error_polling_enabled; + base_config.base_depth_output_trigger_enabled = config.rs415_depth_output_trigger_enabled; + base_config.base_depth_units = config.rs415_depth_units; + base_config.base_JSON_file_path = config.rs415_JSON_file_path; + base_config.base_enable_depth_to_disparity_filter = config.rs415_enable_depth_to_disparity_filter; + base_config.base_enable_spatial_filter = config.rs415_enable_spatial_filter; + base_config.base_enable_temporal_filter = config.rs415_enable_temporal_filter; + base_config.base_enable_disparity_to_depth_filter = config.rs415_enable_disparity_to_depth_filter; + base_config.base_spatial_filter_magnitude = config.rs415_spatial_filter_magnitude; + base_config.base_spatial_filter_smooth_alpha = config.rs415_spatial_filter_smooth_alpha; + base_config.base_spatial_filter_smooth_delta = config.rs415_spatial_filter_smooth_delta; + base_config.base_spatial_filter_holes_fill = config.rs415_spatial_filter_holes_fill; + base_config.base_temporal_filter_smooth_alpha = config.rs415_temporal_filter_smooth_alpha; + base_config.base_temporal_filter_smooth_delta = config.rs415_temporal_filter_smooth_delta; + base_config.base_temporal_filter_holes_fill = config.rs415_temporal_filter_holes_fill; + RealSenseParamManager d400_param; + d400_param.setParam(node_ptr, base_config, static_cast::Param>(param)); + break; + } +} + +} diff --git a/realsense2_camera/src/realsense_node.cpp b/realsense2_camera/src/realsense_node.cpp index 6d9126cae2..3cc2dd91f9 100644 --- a/realsense2_camera/src/realsense_node.cpp +++ b/realsense2_camera/src/realsense_node.cpp @@ -1,8 +1,6 @@ #include -#include +#include #include -#include -#include using namespace realsense2_camera; @@ -113,56 +111,19 @@ RealSenseNode::RealSenseNode(const ros::NodeHandle &nodeHandle, void RealSenseNode::createParamsManager() { auto pid_str = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); uint16_t pid; - std::stringstream ss; - ss << std::hex << pid_str; - ss >> pid; - ROS_WARN_STREAM ("pid " << pid); - - switch(pid) - { - case SR300_PID: - _params = std::unique_ptr(new SR300ParamManager); - break; - case RS400_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS405_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS410_PID: - case RS460_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS415_PID: - _params = std::unique_ptr(new RS415ParamManager); - break; - case RS420_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS420_MM_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS430_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS430_MM_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS430_MM_RGB_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - case RS435_RGB_PID: - _params = std::unique_ptr(new RS435ParamManager); - break; - case RS_USB2_PID: - _params = std::unique_ptr(new D400ParamManager); - break; - default: - ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); - ros::Duration(20).sleep(); - resetNode(); - } - + std::stringstream ss; + ss << std::hex << pid_str; + ss >> pid; + try + { + _params = param_makers.at(pid)(); + } + catch (...) + { + ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); + ros::Duration(20).sleep(); + resetNode(); + } } void RealSenseNode::resetNode() { @@ -1639,140 +1600,6 @@ bool RealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, rs2 -void D400ParamManager::callback(RealSenseNode* node_ptr,base_d400_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("D400 - Level: " << level); - - if (node_ptr->set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < base_depth_count ; ++i) - { - ROS_DEBUG_STREAM("base_depth_param = " << i); - setParam(node_ptr, config ,(base_depth_param)i); - } - } - else - { - setParam(node_ptr, config, (base_depth_param)level); - } -} - -void D400ParamManager::setOption(RealSenseNode* node_ptr,stream_index_pair sip, rs2_option opt, float val) -{ - node_ptr->_sensors[sip].set_option(opt, val); -} - -void D400ParamManager::setParam(RealSenseNode* node_ptr,base_d400_paramsConfig &config, base_depth_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case base_depth_gain: - ROS_DEBUG_STREAM("base_depth_gain: " << config.base_depth_gain); - setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_GAIN, config.base_depth_gain); - break; - case base_depth_enable_auto_exposure: - ROS_DEBUG_STREAM("base_depth_enable_auto_exposure: " << config.base_depth_enable_auto_exposure); - setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.base_depth_enable_auto_exposure); - break; - case base_depth_visual_preset: - ROS_DEBUG_STREAM("base_depth_visual_preset: " << config.base_depth_visual_preset); - setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_VISUAL_PRESET, config.base_depth_visual_preset); - break; - case base_depth_frames_queue_size: - ROS_DEBUG_STREAM("base_depth_frames_queue_size: " << config.base_depth_frames_queue_size); - setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_FRAMES_QUEUE_SIZE, config.base_depth_frames_queue_size); - break; - case base_depth_error_polling_enabled: - ROS_DEBUG_STREAM("base_depth_error_polling_enabled: " << config.base_depth_error_polling_enabled); - setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_ERROR_POLLING_ENABLED, config.base_depth_error_polling_enabled); - break; - case base_depth_output_trigger_enabled: - ROS_DEBUG_STREAM("base_depth_output_trigger_enabled: " << config.base_depth_output_trigger_enabled); - setOption(node_ptr, RealSenseNode::DEPTH, RS2_OPTION_OUTPUT_TRIGGER_ENABLED, config.base_depth_output_trigger_enabled); - break; - case base_depth_units: - break; - case base_JSON_file_path: - { - ROS_DEBUG_STREAM("base_JSON_file_path: " << config.base_JSON_file_path); - auto adv_dev = node_ptr->_dev.as(); - if (!adv_dev) - { - ROS_WARN_STREAM("Device doesn't support Advanced Mode!"); - return; - } - if (!config.base_JSON_file_path.empty()) - { - std::ifstream in(config.base_JSON_file_path); - if (!in.is_open()) - { - ROS_WARN_STREAM("JSON file provided doesn't exist!"); - return; - } - - adv_dev.load_json(config.base_JSON_file_path); - } - break; - } - case base_enable_depth_to_disparity_filter: - ROS_DEBUG_STREAM("base_enable_depth_to_disparity_filter: " << config.base_enable_depth_to_disparity_filter); - node_ptr->filters[DEPTH_TO_DISPARITY].is_enabled = config.base_enable_depth_to_disparity_filter; - break; - case base_enable_spatial_filter: - ROS_DEBUG_STREAM("base_enable_spatial_filter: " << config.base_enable_spatial_filter); - node_ptr->filters[SPATIAL].is_enabled = config.base_enable_spatial_filter; - break; - case base_enable_temporal_filter: - ROS_DEBUG_STREAM("base_enable_temporal_filter: " << config.base_enable_temporal_filter); - node_ptr->filters[TEMPORAL].is_enabled = config.base_enable_temporal_filter; - break; - case base_enable_disparity_to_depth_filter: - ROS_DEBUG_STREAM("base_enable_disparity_to_depth_filter: " << config.base_enable_disparity_to_depth_filter); - node_ptr->filters[DISPARITY_TO_DEPTH].is_enabled = config.base_enable_disparity_to_depth_filter; - break; - case base_spatial_filter_magnitude: - ROS_DEBUG_STREAM("base_spatial_filter_magnitude: " << config.base_spatial_filter_magnitude); - node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, config.base_spatial_filter_magnitude); - break; - case base_spatial_filter_smooth_alpha: - ROS_DEBUG_STREAM("base_spatial_filter_smooth_alpha: " << config.base_spatial_filter_smooth_alpha); - node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_spatial_filter_smooth_alpha); - break; - case base_spatial_filter_smooth_delta: - ROS_DEBUG_STREAM("base_spatial_filter_smooth_delta: " << config.base_spatial_filter_smooth_delta); - node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_spatial_filter_smooth_delta); - break; - case base_spatial_filter_holes_fill: - ROS_DEBUG_STREAM("base_spatial_filter_holes_fill: " << config.base_spatial_filter_holes_fill); - node_ptr->filters[SPATIAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_spatial_filter_holes_fill); - break; - case base_temporal_filter_smooth_alpha: - ROS_DEBUG_STREAM("base_temporal_filter_smooth_alpha: " << config.base_temporal_filter_smooth_alpha); - node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_temporal_filter_smooth_alpha); - break; - case base_temporal_filter_smooth_delta: - ROS_DEBUG_STREAM("base_temporal_filter_smooth_delta: " << config.base_temporal_filter_smooth_delta); - node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_temporal_filter_smooth_delta); - break; - case base_temporal_filter_holes_fill: - ROS_DEBUG_STREAM("base_temporal_filter_holes_fill: " << config.base_temporal_filter_holes_fill); - node_ptr->filters[TEMPORAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_temporal_filter_holes_fill); - break; - default: - ROS_WARN_STREAM("Unrecognized D400 param (" << param << ")"); - break; - } -} - -void D400ParamManager::registerDynamicReconfigCb(RealSenseNode* node_ptr) -{ - _server = std::make_shared>(); - _f = boost::bind(&D400ParamManager::callback, this, node_ptr, _1, _2); - _server->setCallback(_f); -} /** Constructor for filter_options, takes a name and a filter. diff --git a/realsense2_camera/src/realsense_nodelet.cpp b/realsense2_camera/src/realsense_nodelet.cpp index 4d3170a10f..3dfb5520b3 100644 --- a/realsense2_camera/src/realsense_nodelet.cpp +++ b/realsense2_camera/src/realsense_nodelet.cpp @@ -2,11 +2,9 @@ // Copyright(c) 2017 Intel Corporation. All Rights Reserved #include -#include -#include -#include +#include -#include +#include using namespace realsense2_camera; diff --git a/realsense2_camera/src/rs415_param_manager.cpp b/realsense2_camera/src/rs415_param_manager.cpp deleted file mode 100644 index 6799fc4931..0000000000 --- a/realsense2_camera/src/rs415_param_manager.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include -#include - -using namespace realsense2_camera; - -void RS415ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) -{ - _server = std::make_shared>(); - _f = boost::bind(&RS415ParamManager::callback, this, node_ptr, _1, _2); - _server->setCallback(_f); -} - - -void RS415ParamManager::setParam(RealSenseNode* node_ptr, rs415_paramsConfig &config, rs415_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case rs415_color_backlight_compensation: - ROS_DEBUG_STREAM("base_JSON_file_path: " << config.rs415_color_backlight_compensation); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs415_color_backlight_compensation); - break; - case rs415_color_brightness: - ROS_DEBUG_STREAM("rs415_color_brightness: " << config.rs415_color_brightness); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs415_color_brightness); - break; - case rs415_color_contrast: - ROS_DEBUG_STREAM("rs415_color_contrast: " << config.rs415_color_contrast); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs415_color_contrast); - break; - case rs415_color_gain: - ROS_DEBUG_STREAM("rs415_color_gain: " << config.rs415_color_gain); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs415_color_gain); - break; - case rs415_color_gamma: - ROS_DEBUG_STREAM("rs415_color_gamma: " << config.rs415_color_gamma); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs415_color_gamma); - break; - case rs415_color_hue: - ROS_DEBUG_STREAM("rs415_color_hue: " << config.rs415_color_hue); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs415_color_hue); - break; - case rs415_color_saturation: - ROS_DEBUG_STREAM("rs415_color_saturation: " << config.rs415_color_saturation); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs415_color_saturation); - break; - case rs415_color_sharpness: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs415_color_sharpness); - break; - case rs415_color_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_color_enable_auto_white_balance); - break; - case rs415_color_enable_auto_exposure: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_enable_auto_exposure); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs415_color_enable_auto_exposure); - break; - case rs415_color_exposure: - ROS_DEBUG_STREAM("rs415_color_exposure: " << config.rs415_color_exposure); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_color_exposure); - break; - case rs415_color_white_balance: - { - static const auto rs415_color_white_balance_factor = 10; - ROS_DEBUG_STREAM("rs415_color_white_balance: " << config.rs415_color_white_balance * rs415_color_white_balance_factor); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs415_color_white_balance * rs415_color_white_balance_factor); - } - break; - case rs415_color_frames_queue_size: - ROS_DEBUG_STREAM("rs415_color_frames_queue_size: " << config.rs415_color_frames_queue_size); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs415_color_frames_queue_size); - break; - case rs415_color_power_line_frequency: - ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_power_line_frequency); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs415_color_power_line_frequency); - break; - case rs415_color_auto_exposure_priority: - ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_auto_exposure_priority); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs415_color_auto_exposure_priority); - break; - case rs415_depth_exposure: - { - static const auto rs415_depth_exposure_factor = 20; - ROS_DEBUG_STREAM("rs415_depth_exposure: " << config.rs415_depth_exposure * rs415_depth_exposure_factor); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_depth_exposure * rs415_depth_exposure_factor); - } - break; - case rs415_depth_laser_power: - { - static const auto rs415_depth_laser_power_factor = 30; - ROS_DEBUG_STREAM("rs415_depth_laser_power: " << config.rs415_depth_laser_power * rs415_depth_laser_power_factor); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs415_depth_laser_power * rs415_depth_laser_power_factor); - } - break; - case rs415_depth_emitter_enabled: - ROS_DEBUG_STREAM("rs415_depth_emitter_enabled: " << config.rs415_depth_emitter_enabled); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs415_depth_emitter_enabled); - break; - case rs415_depth_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs415_depth_enable_auto_white_balance: " << config.rs415_depth_enable_auto_white_balance); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_depth_enable_auto_white_balance); - break; - default: - //D400ParamManager::setParam(config, (base_depth_param)param); - break; - } -} - -void RS415ParamManager::callback(RealSenseNode* node_ptr, rs415_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("RS415ParamManager - Level: " << level); - - if (node_ptr->set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < rs415_param_count ; ++i) - { - ROS_DEBUG_STREAM("rs415_param = " << i); - setParam(node_ptr, config ,(rs415_param)i); - } - } - else - { - setParam(node_ptr, config, (rs415_param)level); - } -} diff --git a/realsense2_camera/src/rs435_param_manager.cpp b/realsense2_camera/src/rs435_param_manager.cpp deleted file mode 100644 index 39a23bd139..0000000000 --- a/realsense2_camera/src/rs435_param_manager.cpp +++ /dev/null @@ -1,120 +0,0 @@ -#include - -using namespace realsense2_camera; - - -void RS435ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) -{ - _server = std::make_shared>(); - _f = boost::bind(&RS435ParamManager::callback, this, node_ptr, _1, _2); - _server->setCallback(_f); -} - -void RS435ParamManager::setParam(RealSenseNode* node_ptr, rs435_paramsConfig &config, rs435_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case rs435_color_backlight_compensation: - ROS_DEBUG_STREAM("rs435_color_backlight_compensation: " << config.rs435_color_backlight_compensation); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs435_color_backlight_compensation); - break; - case rs435_color_brightness: - ROS_DEBUG_STREAM("rs435_color_brightness: " << config.rs435_color_brightness); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs435_color_brightness); - break; - case rs435_color_contrast: - ROS_DEBUG_STREAM("rs435_color_contrast: " << config.rs435_color_contrast); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs435_color_contrast); - break; - case rs435_color_gain: - ROS_DEBUG_STREAM("rs435_color_gain: " << config.rs435_color_gain); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs435_color_gain); - break; - case rs435_color_gamma: - ROS_DEBUG_STREAM("rs435_color_gamma: " << config.rs435_color_gamma); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs435_color_gamma); - break; - case rs435_color_hue: - ROS_DEBUG_STREAM("rs435_color_hue: " << config.rs435_color_hue); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs435_color_hue); - break; - case rs435_color_saturation: - ROS_DEBUG_STREAM("rs435_color_saturation: " << config.rs435_color_saturation); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs435_color_saturation); - break; - case rs435_color_sharpness: - ROS_DEBUG_STREAM("rs435_color_sharpness: " << config.rs435_color_sharpness); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs435_color_sharpness); - break; - case rs435_color_enable_auto_exposure: - ROS_DEBUG_STREAM("rs435_color_enable_auto_exposure: " << config.rs435_color_enable_auto_exposure); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs435_color_enable_auto_exposure); - break; - case rs435_color_exposure: - ROS_DEBUG_STREAM("rs435_color_exposure: " << config.rs435_color_exposure); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_color_exposure); - break; - case rs435_color_white_balance: - ROS_DEBUG_STREAM("rs435_color_white_balance: " << config.rs435_color_white_balance * 10); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs435_color_white_balance * 10); - break; - case rs435_color_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs435_color_enable_auto_white_balance: " << config.rs435_color_enable_auto_white_balance); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs435_color_enable_auto_white_balance); - break; - case rs435_color_frames_queue_size: - ROS_DEBUG_STREAM("rs435_color_frames_queue_size: " << config.rs435_color_frames_queue_size); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs435_color_frames_queue_size); - break; - case rs435_color_power_line_frequency: - ROS_DEBUG_STREAM("rs435_color_power_line_frequency: " << config.rs435_color_power_line_frequency); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs435_color_power_line_frequency); - break; - case rs435_color_auto_exposure_priority: - ROS_DEBUG_STREAM("rs435_color_auto_exposure_priority: " << config.rs435_color_auto_exposure_priority); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs435_color_auto_exposure_priority); - break; - case rs435_depth_exposure: - { - static const auto rs435_depth_exposure_factor = 20; - ROS_DEBUG_STREAM("rs435_depth_exposure: " << config.rs435_depth_exposure * rs435_depth_exposure_factor); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_depth_exposure * rs435_depth_exposure_factor); - } - break; - case rs435_depth_laser_power: - { - static const auto rs435_depth_laser_power_factor = 30; - ROS_DEBUG_STREAM("rs435_depth_laser_power: " << config.rs435_depth_laser_power * rs435_depth_laser_power_factor); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs435_depth_laser_power * rs435_depth_laser_power_factor); - } - break; - case rs435_depth_emitter_enabled: - ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.rs435_depth_emitter_enabled); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs435_depth_emitter_enabled); - break; - default: - //D400ParamManager::setParam(config, (base_depth_param)param); - break; - } -} - -void RS435ParamManager::callback(RealSenseNode* node_ptr, rs435_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("RS435Node - Level: " << level); - - if (node_ptr->set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < rs435_param_count ; ++i) - { - ROS_DEBUG_STREAM("rs435_param = " << i); - setParam(node_ptr, config ,(rs435_param)i); - } - } - else - { - setParam(node_ptr, config, (rs435_param)level); - } -} diff --git a/realsense2_camera/src/sr300_param_manager.cpp b/realsense2_camera/src/sr300_param_manager.cpp deleted file mode 100644 index a67f54526a..0000000000 --- a/realsense2_camera/src/sr300_param_manager.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include - -using namespace realsense2_camera; - -void SR300ParamManager::registerDynamicReconfigCb(RealSenseNode *node_ptr) -{ - _server = std::make_shared>(); - _f = boost::bind(&SR300ParamManager::callback, this, node_ptr, _1, _2); - _server->setCallback(_f); -} - -void SR300ParamManager::setParam(RealSenseNode* node_ptr, sr300_paramsConfig &config, sr300_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case sr300_param_color_backlight_compensation: - ROS_DEBUG_STREAM("sr300_param_color_backlight_compensation: " << config.sr300_color_backlight_compensation); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.sr300_color_backlight_compensation); - break; - case sr300_param_color_brightness: - ROS_DEBUG_STREAM("sr300_color_backlight_compensation: " << config.sr300_color_backlight_compensation); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.sr300_color_brightness); - break; - case sr300_param_color_contrast: - ROS_DEBUG_STREAM("sr300_param_color_contrast: " << config.sr300_color_contrast); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.sr300_color_contrast); - break; - case sr300_param_color_gain: - ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gain); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.sr300_color_gain); - break; - case sr300_param_color_gamma: - ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gamma); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.sr300_color_gamma); - break; - case sr300_param_color_hue: - ROS_DEBUG_STREAM("sr300_param_color_hue: " << config.sr300_color_hue); - node_ptr-> _sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.sr300_color_hue); - break; - case sr300_param_color_saturation: - ROS_DEBUG_STREAM("sr300_param_color_saturation: " << config.sr300_color_saturation); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.sr300_color_saturation); - break; - case sr300_param_color_sharpness: - ROS_DEBUG_STREAM("sr300_param_color_sharpness: " << config.sr300_color_sharpness); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.sr300_color_sharpness); - break; - case sr300_param_color_white_balance: - ROS_DEBUG_STREAM("sr300_param_color_white_balance: " << config.sr300_color_white_balance); - if (node_ptr->_sensors[RealSenseNode::COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)) - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 0); - - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.sr300_color_white_balance); - break; - case sr300_param_color_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.sr300_color_enable_auto_white_balance); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.sr300_color_enable_auto_white_balance); - break; - case sr300_param_color_exposure: - ROS_DEBUG_STREAM("sr300_param_color_exposure: " << config.sr300_color_exposure); - if (node_ptr->_sensors[RealSenseNode::COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE)) - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); - - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.sr300_color_exposure); - break; - case sr300_param_color_enable_auto_exposure: - ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_color_enable_auto_white_balance); - node_ptr->_sensors[RealSenseNode::COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.sr300_color_enable_auto_exposure); - break; - case sr300_param_depth_visual_preset: - ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_visual_preset); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_VISUAL_PRESET, config.sr300_depth_visual_preset); - break; - case sr300_param_depth_laser_power: - ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_laser_power); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.sr300_depth_laser_power); - break; - case sr300_param_depth_accuracy: - ROS_DEBUG_STREAM("sr300_param_depth_accuracy: " << config.sr300_depth_accuracy); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_ACCURACY, config.sr300_depth_accuracy); - break; - case sr300_param_depth_motion_range: - ROS_DEBUG_STREAM("sr300_param_depth_motion_range: " << config.sr300_depth_motion_range); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_MOTION_RANGE, config.sr300_depth_motion_range); - break; - case sr300_param_depth_filter_option: - ROS_DEBUG_STREAM("sr300_param_depth_filter_option: " << config.sr300_depth_filter_option); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_FILTER_OPTION, config.sr300_depth_filter_option); - break; - case sr300_param_depth_confidence_threshold: - ROS_DEBUG_STREAM("sr300_param_depth_confidence_threshold: " << config.sr300_depth_confidence_threshold); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_CONFIDENCE_THRESHOLD, config.sr300_depth_confidence_threshold); - break; - case sr300_param_depth_frames_queue_size: - ROS_DEBUG_STREAM("sr300_param_depth_frames_queue_size: " << config.sr300_depth_frames_queue_size); - node_ptr->_sensors[RealSenseNode::DEPTH].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.sr300_depth_frames_queue_size); - break; - case sr300_param_depth_units: - break; - default: - ROS_WARN_STREAM("Unrecognized sr300 param (" << param << ")"); - break; - } - - // TODO -// // Auto range options -// "sr300_auto_range_enable_motion_versus_range" -// "sr300_auto_range_enable_laser", - -// // Set only if sr300_auto_range_enable_motion_versus_range is set to 1 -// "sr300_auto_range_min_motion_versus_range", -// "sr300_auto_range_max_motion_versus_range", -// "sr300_auto_range_start_motion_versus_range", - -// // Set only if sr300_auto_range_enable_laser is enabled -// "sr300_auto_range_min_laser" -// "sr300_auto_range_max_laser" -// "sr300_auto_range_start_laser" - -// "sr300_auto_range_upper_threshold" -// "sr300_auto_range_lower_threshold" -} - -void SR300ParamManager::callback(RealSenseNode* node_ptr, sr300_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("SR300Node - Level: " << level); - - if (node_ptr->set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < sr300_param_count ; ++i) - { - ROS_DEBUG_STREAM("sr300_param = " << i); - setParam(node_ptr, config ,(sr300_param)i); - } - } - else - { - setParam(node_ptr, config, (sr300_param)level); - } -} From aa73992b4459ca4aefe818e35c955b9d67e009d8 Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Sun, 5 May 2019 03:19:54 -0400 Subject: [PATCH 5/8] bump version --- realsense2_camera/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 41163bcb04..840d7cf0aa 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.0.10 + 2.0.11 RealSense Camera package allowing access to Intel 3D D400 cameras Sergey Dorodnicov Itay Carpis From 5a1ca0ed6c6ab20de287c9a7f7d7325071d1c075 Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Mon, 6 May 2019 13:07:18 -0400 Subject: [PATCH 6/8] bug fix --- realsense2_camera/src/realsense_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/realsense_node.cpp b/realsense2_camera/src/realsense_node.cpp index 3cc2dd91f9..0d4170f554 100644 --- a/realsense2_camera/src/realsense_node.cpp +++ b/realsense2_camera/src/realsense_node.cpp @@ -28,6 +28,7 @@ RealSenseNode::RealSenseNode(const ros::NodeHandle &nodeHandle, _intialize_time_base(false), _namespace(getNamespaceStr()) { + getParameters(); getDevice(); createParamsManager(); @@ -198,7 +199,6 @@ void RealSenseNode::getDevice() { void RealSenseNode::publishTopics() { - getParameters(); setupDevice(); setHealthTimers(); setupPublishers(); @@ -332,6 +332,7 @@ void RealSenseNode::getParameters() _pnh.param("aligned_depth_to_infra2_frame_id", _depth_aligned_frame_id[INFRA2], DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID); _pnh.param("aligned_depth_to_fisheye_frame_id", _depth_aligned_frame_id[FISHEYE], DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID); + _pnh.param("serial_no", _serial_no, _serial_no); _pnh.param("rosbag_filename", _rosbag_filename, _rosbag_filename); double depth_callback_timeout = 30; // seconds From eb1026109aa3c2cbf86506c591c4174b5731e219 Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Mon, 6 May 2019 13:12:44 -0400 Subject: [PATCH 7/8] bump version --- realsense2_camera/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 840d7cf0aa..4d68061079 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.0.11 + 2.0.12 RealSense Camera package allowing access to Intel 3D D400 cameras Sergey Dorodnicov Itay Carpis From ab867cf6086b02129b133c35527a27ae62fb3785 Mon Sep 17 00:00:00 2001 From: Adel Fakih Date: Tue, 7 May 2019 01:12:25 -0400 Subject: [PATCH 8/8] bug fixes --- realsense2_camera/package.xml | 2 +- realsense2_camera/src/realsense_node.cpp | 36 ++++++++++++++---------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 4d68061079..67dbf249b1 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.0.12 + 2.0.13 RealSense Camera package allowing access to Intel 3D D400 cameras Sergey Dorodnicov Itay Carpis diff --git a/realsense2_camera/src/realsense_node.cpp b/realsense2_camera/src/realsense_node.cpp index 0d4170f554..f85dd409f0 100644 --- a/realsense2_camera/src/realsense_node.cpp +++ b/realsense2_camera/src/realsense_node.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -30,7 +30,6 @@ RealSenseNode::RealSenseNode(const ros::NodeHandle &nodeHandle, { getParameters(); getDevice(); - createParamsManager(); // Types for depth stream _is_frame_arrived[DEPTH] = false; @@ -106,7 +105,13 @@ RealSenseNode::RealSenseNode(const ros::NodeHandle &nodeHandle, _prev_camera_time_stamp = 0; - publishTopics(); + setHealthTimers(); + + if (_dev) + { + createParamsManager(); + publishTopics(); + } } void RealSenseNode::createParamsManager() { @@ -128,13 +133,14 @@ void RealSenseNode::createParamsManager() { } void RealSenseNode::resetNode() { - auto& nh = _node_handle; - auto& pnh = _pnh; + auto nh = _node_handle; + auto pnh = _pnh; this->~RealSenseNode(); new (this) RealSenseNode(nh, pnh); } void RealSenseNode::getDevice() { + if (!_rosbag_filename.empty()) { ROS_INFO_STREAM("publish topics from rosbag file: " << _rosbag_filename.c_str()); @@ -155,9 +161,8 @@ void RealSenseNode::getDevice() { usb_mutex.unlock(); if (0 == list.size()) { - ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); - ros::shutdown(); - exit(1); + ROS_ERROR("No RealSense devices were found!."); + return; } bool found = false; @@ -183,8 +188,7 @@ void RealSenseNode::getDevice() { if (!found) { ROS_ERROR_STREAM("The requested device with serial number " << _serial_no << " is NOT found!"); - ros::Duration(20).sleep(); - resetNode(); + return; } _ctx.set_devices_changed_callback([this](rs2::event_information& info) @@ -200,17 +204,15 @@ void RealSenseNode::getDevice() { void RealSenseNode::publishTopics() { setupDevice(); - setHealthTimers(); setupPublishers(); setupServices(); setupStreams(); publishStaticTransforms(); - ROS_INFO_STREAM("RealSense Node Is Up!"); - if (_params) { _params->registerDynamicReconfigCb(this); } + ROS_INFO_STREAM("RealSense Node Is Up!"); } @@ -253,6 +255,7 @@ bool RealSenseNode::enableStreams(std_srvs::SetBool::Request& req, std_srvs::Set res.message += std::string("Failed to stop stream: ") + e.what() + '\n'; res.success = false; } + depth_callback_timer_.stop(); } } } @@ -278,6 +281,7 @@ void RealSenseNode::getParameters() _pnh.param("depth_width", _width[DEPTH], DEPTH_WIDTH); _pnh.param("depth_height", _height[DEPTH], DEPTH_HEIGHT); _pnh.param("depth_fps", _fps[DEPTH], DEPTH_FPS); + _pnh.param("enable_depth", _enable[DEPTH], ENABLE_DEPTH); _aligned_depth_images[DEPTH].resize(_width[DEPTH] * _height[DEPTH] * _unit_step_size[DEPTH]); @@ -332,12 +336,13 @@ void RealSenseNode::getParameters() _pnh.param("aligned_depth_to_infra2_frame_id", _depth_aligned_frame_id[INFRA2], DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID); _pnh.param("aligned_depth_to_fisheye_frame_id", _depth_aligned_frame_id[FISHEYE], DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID); - _pnh.param("serial_no", _serial_no, _serial_no); _pnh.param("rosbag_filename", _rosbag_filename, _rosbag_filename); double depth_callback_timeout = 30; // seconds _pnh.param("depth_callback_timeout", depth_callback_timeout, depth_callback_timeout); depth_callback_timeout_ = ros::Duration(depth_callback_timeout); + + _pnh.param("serial_no", _serial_no, _serial_no); } void RealSenseNode::setupDevice() @@ -1582,7 +1587,8 @@ void RealSenseNode::setHealthTimers() { ROS_WARN_STREAM("RealSense " << _serial_no << " driver timeout! Resetting"); resetNode(); }; - depth_callback_timer_ = _node_handle.createTimer(depth_callback_timeout_, reset_this, false, false); + depth_callback_timer_ = _node_handle.createTimer(depth_callback_timeout_, reset_this, false, !_dev); + } bool RealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile)