From f01d776fc502ed51d4fe18fe71b0b2dd003d0d4d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 13 Jul 2022 15:53:32 +0300 Subject: [PATCH 1/2] fix rosbag device loading by preventing set_option to HDR/Gain/Exposure --- realsense2_camera/include/base_realsense_node.h | 4 +++- realsense2_camera/include/realsense_node_factory.h | 2 +- realsense2_camera/include/ros_sensor.h | 5 +++-- realsense2_camera/include/t265_realsense_node.h | 3 ++- realsense2_camera/src/base_realsense_node.cpp | 6 ++++-- realsense2_camera/src/realsense_node_factory.cpp | 8 ++++---- realsense2_camera/src/ros_sensor.cpp | 14 ++++++++++---- realsense2_camera/src/rs_node_setup.cpp | 6 +++--- realsense2_camera/src/t265_realsense_node.cpp | 5 +++-- 9 files changed, 33 insertions(+), 20 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 27024b14f8..3d8a9380ef 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -97,7 +97,8 @@ namespace realsense2_camera BaseRealSenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process = false); + bool use_intra_process = false, + bool is_rosbag_file = false); ~BaseRealSenseNode(); void publishTopics(); @@ -293,6 +294,7 @@ namespace realsense2_camera mutable std::condition_variable _cv_temp, _cv_mpc, _cv_tf; bool _is_profile_changed; bool _is_align_depth_changed; + bool _is_rosbag_file; std::shared_ptr _diagnostics_updater; rs2::stream_profile _base_profile; diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index e133814860..8453bb74ea 100755 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -35,7 +35,7 @@ namespace realsense2_camera private: void init(); void closeDevice(); - void startDevice(); + void startDevice(bool is_rosbag_file = false); void changeDeviceCallback(rs2::event_information& info); void getDevice(rs2::device_list list); void tryGetLogSeverity(rs2_log_severity& severity) const; diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index aba44f5c19..673ec1c57f 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -70,7 +70,8 @@ namespace realsense2_camera std::function hardware_reset_func, std::shared_ptr diagnostics_updater, rclcpp::Logger logger, - bool force_image_default_qos = false); + bool force_image_default_qos = false, + bool is_rosbag_file = false); ~RosSensor(); void registerSensorParameters(); bool getUpdatedProfiles(std::vector& wanted_profiles); @@ -88,7 +89,7 @@ namespace realsense2_camera private: void setupErrorCallback(); - void setParameters(); + void setParameters(bool is_rosbag_file = false); void clearParameters(); void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); diff --git a/realsense2_camera/include/t265_realsense_node.h b/realsense2_camera/include/t265_realsense_node.h index c8e1f312ca..0e84807dff 100644 --- a/realsense2_camera/include/t265_realsense_node.h +++ b/realsense2_camera/include/t265_realsense_node.h @@ -14,7 +14,8 @@ namespace realsense2_camera T265RealsenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process = false); + bool use_intra_process = false, + bool is_rosbag_file = false); void publishTopics(); protected: diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9170b0c823..413a118b9f 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -73,7 +73,8 @@ size_t SyncedImuPublisher::getNumSubscribers() BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process) : + bool use_intra_process, + bool is_rosbag_file) : _is_running(true), _node(node), _logger(node.get_logger()), @@ -85,7 +86,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_initialized_time_base(false), _sync_frames(SYNC_FRAMES), _is_profile_changed(false), - _is_align_depth_changed(false) + _is_align_depth_changed(false), + _is_rosbag_file(is_rosbag_file) { if ( use_intra_process ) { diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 2a8fc31aa1..2328a98b20 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -285,7 +285,7 @@ void RealSenseNodeFactory::init() } if (_device) { - startDevice(); + startDevice(true); } } else @@ -347,7 +347,7 @@ void RealSenseNodeFactory::init() } } -void RealSenseNodeFactory::startDevice() +void RealSenseNodeFactory::startDevice(bool is_rosbag_file) { if (_realSenseNode) _realSenseNode.reset(); std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); @@ -376,10 +376,10 @@ void RealSenseNodeFactory::startDevice() case RS_L515_PID_PRE_PRQ: case RS_L515_PID: case RS_L535_PID: - _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); + _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), is_rosbag_file)); break; case RS_T265_PID: - _realSenseNode = std::unique_ptr(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); + _realSenseNode = std::unique_ptr(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), is_rosbag_file)); break; default: ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 3f367e320c..a74b3e1da8 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -33,7 +33,8 @@ RosSensor::RosSensor(rs2::sensor sensor, std::function hardware_reset_func, std::shared_ptr diagnostics_updater, rclcpp::Logger logger, - bool force_image_default_qos): + bool force_image_default_qos, + bool is_rosbag_file): rs2::sensor(sensor), _logger(logger), _origin_frame_callback(frame_callback), @@ -54,7 +55,7 @@ RosSensor::RosSensor(rs2::sensor sensor, _origin_frame_callback(frame); }; - setParameters(); + setParameters(is_rosbag_file); } RosSensor::~RosSensor() @@ -63,11 +64,16 @@ RosSensor::~RosSensor() stop(); } -void RosSensor::setParameters() +void RosSensor::setParameters(bool is_rosbag_file) { std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); _params.registerDynamicOptions(*this, module_name); - UpdateSequenceIdCallback(); + + // for rosbag files, don't set hdr(sequence_id) / gain / exposure options + // since these options can be changed only in real devices + if(!is_rosbag_file) + UpdateSequenceIdCallback(); + registerSensorParameters(); } diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index a36ed842e2..6d0b40b59e 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -135,17 +135,17 @@ void BaseRealSenseNode::setAvailableSensors() sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor."); - rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process); + rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _is_rosbag_file); } else if (sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as ImuSensor."); - rosSensor = std::make_unique(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger); + rosSensor = std::make_unique(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _is_rosbag_file); } else if (sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as PoseSensor."); - rosSensor = std::make_unique(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger); + rosSensor = std::make_unique(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _is_rosbag_file); } else { diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp index 20b198e1e0..955bbe0d11 100644 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ b/realsense2_camera/src/t265_realsense_node.cpp @@ -9,8 +9,9 @@ using namespace realsense2_camera; T265RealsenseNode::T265RealsenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process) : - BaseRealSenseNode(node, dev, parameters, use_intra_process), + bool use_intra_process, + bool is_rosbag_file) : + BaseRealSenseNode(node, dev, parameters, use_intra_process, is_rosbag_file), _wo_snr(dev.first()), _use_odom_in(false) { From 7ffa714107eb36b385b9f79ba0063c71a022b475 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 13 Jul 2022 16:55:10 +0300 Subject: [PATCH 2/2] use is instead --- realsense2_camera/include/base_realsense_node.h | 4 +--- realsense2_camera/include/realsense_node_factory.h | 2 +- realsense2_camera/include/t265_realsense_node.h | 3 +-- realsense2_camera/src/base_realsense_node.cpp | 6 ++---- realsense2_camera/src/realsense_node_factory.cpp | 8 ++++---- realsense2_camera/src/rs_node_setup.cpp | 8 ++++---- realsense2_camera/src/t265_realsense_node.cpp | 5 ++--- 7 files changed, 15 insertions(+), 21 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 3d8a9380ef..27024b14f8 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -97,8 +97,7 @@ namespace realsense2_camera BaseRealSenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process = false, - bool is_rosbag_file = false); + bool use_intra_process = false); ~BaseRealSenseNode(); void publishTopics(); @@ -294,7 +293,6 @@ namespace realsense2_camera mutable std::condition_variable _cv_temp, _cv_mpc, _cv_tf; bool _is_profile_changed; bool _is_align_depth_changed; - bool _is_rosbag_file; std::shared_ptr _diagnostics_updater; rs2::stream_profile _base_profile; diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 8453bb74ea..e133814860 100755 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -35,7 +35,7 @@ namespace realsense2_camera private: void init(); void closeDevice(); - void startDevice(bool is_rosbag_file = false); + void startDevice(); void changeDeviceCallback(rs2::event_information& info); void getDevice(rs2::device_list list); void tryGetLogSeverity(rs2_log_severity& severity) const; diff --git a/realsense2_camera/include/t265_realsense_node.h b/realsense2_camera/include/t265_realsense_node.h index 0e84807dff..c8e1f312ca 100644 --- a/realsense2_camera/include/t265_realsense_node.h +++ b/realsense2_camera/include/t265_realsense_node.h @@ -14,8 +14,7 @@ namespace realsense2_camera T265RealsenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process = false, - bool is_rosbag_file = false); + bool use_intra_process = false); void publishTopics(); protected: diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 413a118b9f..9170b0c823 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -73,8 +73,7 @@ size_t SyncedImuPublisher::getNumSubscribers() BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process, - bool is_rosbag_file) : + bool use_intra_process) : _is_running(true), _node(node), _logger(node.get_logger()), @@ -86,8 +85,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_initialized_time_base(false), _sync_frames(SYNC_FRAMES), _is_profile_changed(false), - _is_align_depth_changed(false), - _is_rosbag_file(is_rosbag_file) + _is_align_depth_changed(false) { if ( use_intra_process ) { diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 2328a98b20..2a8fc31aa1 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -285,7 +285,7 @@ void RealSenseNodeFactory::init() } if (_device) { - startDevice(true); + startDevice(); } } else @@ -347,7 +347,7 @@ void RealSenseNodeFactory::init() } } -void RealSenseNodeFactory::startDevice(bool is_rosbag_file) +void RealSenseNodeFactory::startDevice() { if (_realSenseNode) _realSenseNode.reset(); std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID)); @@ -376,10 +376,10 @@ void RealSenseNodeFactory::startDevice(bool is_rosbag_file) case RS_L515_PID_PRE_PRQ: case RS_L515_PID: case RS_L535_PID: - _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), is_rosbag_file)); + _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; case RS_T265_PID: - _realSenseNode = std::unique_ptr(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms(), is_rosbag_file)); + _realSenseNode = std::unique_ptr(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; default: ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 6d0b40b59e..e35758d5c8 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -97,7 +97,7 @@ void BaseRealSenseNode::setAvailableSensors() ROS_INFO_STREAM("Device Product ID: 0x" << pid); ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off")); - + std::function frame_callback_function = [this](rs2::frame frame){ bool is_filter(_filters.end() != find_if(_filters.begin(), _filters.end(), [](std::shared_ptr f){return (f->is_enabled()); })); if (_sync_frames || is_filter) @@ -135,17 +135,17 @@ void BaseRealSenseNode::setAvailableSensors() sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor."); - rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _is_rosbag_file); + rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is()); } else if (sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as ImuSensor."); - rosSensor = std::make_unique(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _is_rosbag_file); + rosSensor = std::make_unique(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is()); } else if (sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as PoseSensor."); - rosSensor = std::make_unique(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _is_rosbag_file); + rosSensor = std::make_unique(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is()); } else { diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp index 955bbe0d11..20b198e1e0 100644 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ b/realsense2_camera/src/t265_realsense_node.cpp @@ -9,9 +9,8 @@ using namespace realsense2_camera; T265RealsenseNode::T265RealsenseNode(rclcpp::Node& node, rs2::device dev, std::shared_ptr parameters, - bool use_intra_process, - bool is_rosbag_file) : - BaseRealSenseNode(node, dev, parameters, use_intra_process, is_rosbag_file), + bool use_intra_process) : + BaseRealSenseNode(node, dev, parameters, use_intra_process), _wo_snr(dev.first()), _use_odom_in(false) {