From c4192df29078c248ea105d4018ca16278495f889 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Mon, 12 Jul 2021 13:45:43 +0300 Subject: [PATCH] protect sensor frame handling --- src/media/playback/playback_device.cpp | 39 +++++++++---- src/media/playback/playback_sensor.h | 13 ++++- unit-tests/unit-tests-live.cpp | 78 ++++++++++++++++---------- 3 files changed, 85 insertions(+), 45 deletions(-) diff --git a/src/media/playback/playback_device.cpp b/src/media/playback/playback_device.cpp index 0a63f73f38..404442d7e1 100644 --- a/src/media/playback/playback_device.cpp +++ b/src/media/playback/playback_device.cpp @@ -83,16 +83,23 @@ std::map> playback_device::create_pla auto action = [this, id]() { - std::lock_guard locker(_active_sensors_mutex); - auto it = m_active_sensors.find(id); - if (it != m_active_sensors.end()) + bool need_to_stop_device = false; { - m_active_sensors.erase(it); - if (m_active_sensors.size() == 0) + std::lock_guard locker(_active_sensors_mutex); + auto it = m_active_sensors.find(id); + if (it != m_active_sensors.end()) { - stop_internal(); + m_active_sensors.erase(it); + if (m_active_sensors.size() == 0) + { + need_to_stop_device = true; + } } } + if (need_to_stop_device) + { + stop_internal(); + } }; if (invoke_required) { @@ -151,14 +158,19 @@ rs2_extrinsics playback_device::calc_extrinsic(const rs2_extrinsics& from, const playback_device::~playback_device() { + std::vector< std::shared_ptr< playback_sensor > > playback_sensors_copy; { - std::lock_guard locker(_active_sensors_mutex); - for (auto&& sensor : m_active_sensors) + std::lock_guard< std::mutex > locker(_active_sensors_mutex); + for (auto s : m_active_sensors) + playback_sensors_copy.push_back(s.second); + } + + + for (auto&& sensor : playback_sensors_copy) + { + if (sensor) { - if (sensor.second != nullptr) - { - sensor.second->stop(); - } + sensor->stop(); } } @@ -469,6 +481,7 @@ void playback_device::stop() void playback_device::stop_internal() { //stop_internal() is called from within the reading thread + LOG_DEBUG("stop_internal() called"); if (m_is_started == false) return; //nothing to do @@ -480,6 +493,7 @@ void playback_device::stop_internal() m_prev_timestamp = std::chrono::nanoseconds(0); catch_up(); playback_status_changed(RS2_PLAYBACK_STATUS_STOPPED); + LOG_DEBUG("stop_internal() end"); } template @@ -514,6 +528,7 @@ void playback_device::do_loop(T action) { if( psc ) { + psc->flush_pending_frames(); psc->stop( false ); } } diff --git a/src/media/playback/playback_sensor.h b/src/media/playback/playback_sensor.h index 52b2997421..58f406c005 100644 --- a/src/media/playback/playback_sensor.h +++ b/src/media/playback/playback_sensor.h @@ -115,20 +115,27 @@ namespace librealsense auto pf = std::make_shared(std::move(frame)); auto callback = [this, is_real_time, stream_id, pf, calc_sleep, is_paused, update_last_pushed_frame](dispatcher::cancellable_timer t) - { + { device_serializer::nanoseconds sleep_for = calc_sleep(); if (sleep_for.count() > 0) t.try_sleep( sleep_for ); LOG_DEBUG("callback--> "<< frame_holder_to_string(*pf)); - if(is_paused()) - return; frame_interface* pframe = nullptr; + + if (!is_streaming()) + return; + + if (is_paused()) + return; + std::swap((*pf).frame, pframe); + m_user_callback->on_frame((rs2_frame*)pframe); update_last_pushed_frame(); + }; m_dispatchers.at(stream_id)->invoke(callback, !is_real_time); } diff --git a/unit-tests/unit-tests-live.cpp b/unit-tests/unit-tests-live.cpp index 968232205a..44f07a3071 100644 --- a/unit-tests/unit-tests-live.cpp +++ b/unit-tests/unit-tests-live.cpp @@ -5238,6 +5238,12 @@ TEST_CASE("Projection from recording", "[software-device][using_pipeline][projec const double MAX_ERROR_PERCENTAGE = 0.1; CAPTURE(count); REQUIRE(count * 100 / (depth_intrin.width * depth_intrin.height) < MAX_ERROR_PERCENTAGE); + + for (auto s : sensors) + { + s.stop(); + s.close(); + } } TEST_CASE("software-device pose stream", "[software-device]") @@ -5304,45 +5310,54 @@ TEST_CASE("Record software-device", "[software-device][record][!mayfail]") std::string folder_name = get_folder_path(special_folder::temp_folder); const std::string filename = folder_name + "recording.bag"; + rs2_software_video_frame video_frame; + rs2_software_motion_frame motion_frame; + rs2_software_pose_frame pose_frame; + + std::vector pixels(W * H * BPP, 100); + float motion_data[3] = { 1, 1, 1 }; + rs2_software_pose_frame::pose_frame_info pose_info = { { 1, 1, 1 },{ 2, 2, 2 },{ 3, 3, 3 },{ 4, 4 ,4 ,4 },{ 5, 5, 5 },{ 6, 6 ,6 }, 0, 0 }; //Software device, streams and frames definition - rs2::software_device dev; + { + rs2::software_device dev; - auto sensor = dev.add_sensor("Synthetic"); - rs2_intrinsics depth_intrinsics = { W, H, (float)W / 2, H / 2, (float)W, (float)H, - RS2_DISTORTION_BROWN_CONRADY ,{ 0,0,0,0,0 } }; - rs2_video_stream video_stream = { RS2_STREAM_DEPTH, 0, 0, W, H, 60, BPP, RS2_FORMAT_Z16, depth_intrinsics }; - auto depth_stream_profile = sensor.add_video_stream(video_stream); + auto sensor = dev.add_sensor("Synthetic"); + rs2_intrinsics depth_intrinsics = { W, H, (float)W / 2, H / 2, (float)W, (float)H, + RS2_DISTORTION_BROWN_CONRADY ,{ 0,0,0,0,0 } }; + rs2_video_stream video_stream = { RS2_STREAM_DEPTH, 0, 0, W, H, 60, BPP, RS2_FORMAT_Z16, depth_intrinsics }; + auto depth_stream_profile = sensor.add_video_stream(video_stream); - rs2_motion_device_intrinsic motion_intrinsics = { { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },{ 2, 2, 2 },{ 3, 3 ,3 } }; - rs2_motion_stream motion_stream = { RS2_STREAM_ACCEL, 0, 1, 200, RS2_FORMAT_MOTION_RAW, motion_intrinsics }; - auto motion_stream_profile = sensor.add_motion_stream(motion_stream); + rs2_motion_device_intrinsic motion_intrinsics = { { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },{ 2, 2, 2 },{ 3, 3 ,3 } }; + rs2_motion_stream motion_stream = { RS2_STREAM_ACCEL, 0, 1, 200, RS2_FORMAT_MOTION_RAW, motion_intrinsics }; + auto motion_stream_profile = sensor.add_motion_stream(motion_stream); - rs2_pose_stream pose_stream = { RS2_STREAM_POSE, 0, 2, 200, RS2_FORMAT_6DOF }; - auto pose_stream_profile = sensor.add_pose_stream(pose_stream); + rs2_pose_stream pose_stream = { RS2_STREAM_POSE, 0, 2, 200, RS2_FORMAT_6DOF }; + auto pose_stream_profile = sensor.add_pose_stream(pose_stream); - rs2::syncer sync; - std::vector stream_profiles; - stream_profiles.push_back(depth_stream_profile); - stream_profiles.push_back(motion_stream_profile); - stream_profiles.push_back(pose_stream_profile); + rs2::syncer sync; + std::vector stream_profiles; + stream_profiles.push_back(depth_stream_profile); + stream_profiles.push_back(motion_stream_profile); + stream_profiles.push_back(pose_stream_profile); - std::vector pixels(W * H * BPP, 100); - rs2_software_video_frame video_frame = { pixels.data(), [](void*) {},W*BPP, BPP, 10000, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 0, depth_stream_profile }; - float motion_data[3] = { 1, 1, 1 }; - rs2_software_motion_frame motion_frame = { motion_data, [](void*) {}, 20000, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 0, motion_stream_profile }; - rs2_software_pose_frame::pose_frame_info pose_info = { { 1, 1, 1 },{ 2, 2, 2 },{ 3, 3, 3 },{ 4, 4 ,4 ,4 },{ 5, 5, 5 },{ 6, 6 ,6 }, 0, 0 }; - rs2_software_pose_frame pose_frame = { &pose_info, [](void*) {}, 30000, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK , 0, pose_stream_profile }; + video_frame = { pixels.data(), [](void*) {},W * BPP, BPP, 10000, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 0, depth_stream_profile }; + motion_frame = { motion_data, [](void*) {}, 20000, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK, 0, motion_stream_profile }; + pose_frame = { &pose_info, [](void*) {}, 30000, RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK , 0, pose_stream_profile }; - //Record software device - { - recorder recorder(filename, dev); - sensor.open(stream_profiles); - sensor.start(sync); - sensor.on_video_frame(video_frame); - sensor.on_motion_frame(motion_frame); - sensor.on_pose_frame(pose_frame); + //Record software device + { + recorder recorder(filename, dev); + sensor.open(stream_profiles); + sensor.start(sync); + sensor.on_video_frame(video_frame); + sensor.on_motion_frame(motion_frame); + sensor.on_pose_frame(pose_frame); + sensor.stop(); + sensor.close(); + } } + //Playback software device rs2::context ctx; @@ -5385,6 +5400,9 @@ TEST_CASE("Record software-device", "[software-device][record][!mayfail]") pose_frame.frame_number == recorded_pose.get_frame_number() && pose_frame.domain == recorded_pose.get_frame_timestamp_domain() && pose_frame.timestamp == recorded_pose.get_timestamp())); + + s.stop(); + s.close(); } void compare(filter first, filter second)