diff --git a/rosbag2_compression/CMakeLists.txt b/rosbag2_compression/CMakeLists.txt index 1862fa2eb2..9bb9a9e795 100644 --- a/rosbag2_compression/CMakeLists.txt +++ b/rosbag2_compression/CMakeLists.txt @@ -104,7 +104,8 @@ if(BUILD_TESTING) ) ament_add_gmock(test_sequential_compression_writer - test/rosbag2_compression/test_sequential_compression_writer.cpp) + test/rosbag2_compression/test_sequential_compression_writer.cpp + test/rosbag2_compression/fake_compressor.cpp) target_link_libraries(test_sequential_compression_writer ${PROJECT_NAME} rosbag2_cpp::rosbag2_cpp diff --git a/rosbag2_compression/test/rosbag2_compression/fake_compression_factory.hpp b/rosbag2_compression/test/rosbag2_compression/fake_compression_factory.hpp new file mode 100644 index 0000000000..d023789462 --- /dev/null +++ b/rosbag2_compression/test/rosbag2_compression/fake_compression_factory.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_COMPRESSION__FAKE_COMPRESSION_FACTORY_HPP_ +#define ROSBAG2_COMPRESSION__FAKE_COMPRESSION_FACTORY_HPP_ + +#include +#include + +#include "rosbag2_compression/compression_factory.hpp" +#include "fake_compressor.hpp" + +class FakeCompressionFactory + : public rosbag2_compression::CompressionFactory +{ +public: + FakeCompressionFactory() = delete; + + ~FakeCompressionFactory() override = default; + + explicit FakeCompressionFactory(int & detected_thread_priority) + : detected_thread_priority_(detected_thread_priority) {} + + std::shared_ptr + create_compressor(const std::string & /*compression_format*/) override + { + return std::make_shared(detected_thread_priority_); + } + +private: + int & detected_thread_priority_; +}; + +#endif // ROSBAG2_COMPRESSION__FAKE_COMPRESSION_FACTORY_HPP_ diff --git a/rosbag2_compression/test/rosbag2_compression/fake_compressor.cpp b/rosbag2_compression/test/rosbag2_compression/fake_compressor.cpp index 8c97ccfa23..01a5e9f0fd 100644 --- a/rosbag2_compression/test/rosbag2_compression/fake_compressor.cpp +++ b/rosbag2_compression/test/rosbag2_compression/fake_compressor.cpp @@ -13,11 +13,27 @@ // limitations under the License. #include +#ifdef _WIN32 +#include +#else +#include +#endif #include "pluginlib/class_list_macros.hpp" - #include "fake_compressor.hpp" +FakeCompressor::FakeCompressor(int & detected_thread_priority) +{ +#ifndef _WIN32 + int cur_nice_value = getpriority(PRIO_PROCESS, 0); + if (cur_nice_value != -1 && errno == 0) { + detected_thread_priority = cur_nice_value; + } +#else + detected_thread_priority = GetThreadPriority(GetCurrentThread()); +#endif +} + std::string FakeCompressor::compress_uri(const std::string & uri) { return uri + "." + get_compression_identifier(); diff --git a/rosbag2_compression/test/rosbag2_compression/fake_compressor.hpp b/rosbag2_compression/test/rosbag2_compression/fake_compressor.hpp index 01c6bb5be0..492958c3ec 100644 --- a/rosbag2_compression/test/rosbag2_compression/fake_compressor.hpp +++ b/rosbag2_compression/test/rosbag2_compression/fake_compressor.hpp @@ -25,6 +25,8 @@ class FakeCompressor : public rosbag2_compression::BaseCompressorInterface public: FakeCompressor() = default; + explicit FakeCompressor(int & detected_thread_priority); + std::string compress_uri(const std::string & uri) override; void compress_serialized_bag_message( diff --git a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp index 8877223cbb..75f33c8914 100644 --- a/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp +++ b/rosbag2_compression/test/rosbag2_compression/test_sequential_compression_writer.cpp @@ -36,12 +36,10 @@ #include "mock_storage_factory.hpp" #include "mock_compression_factory.hpp" +#include "fake_compression_factory.hpp" #ifdef _WIN32 #include -#else -#include -#include #endif using namespace testing; // NOLINT @@ -401,82 +399,37 @@ TEST_P(SequentialCompressionWriterTest, writer_writes_with_compression_queue_siz EXPECT_EQ(fake_storage_size_, kNumMessagesToWrite); } - -TEST_P(SequentialCompressionWriterTest, writer_sets_nice_value) +TEST_P(SequentialCompressionWriterTest, writer_sets_threads_priority) { const std::string test_topic_name = "test_topic"; const std::string test_topic_type = "test_msgs/BasicTypes"; const uint64_t kCompressionQueueSize = GetParam(); - const int wanted_nice_value = 10; - - class TestCompressor : public rosbag2_compression::BaseCompressorInterface - { - int & detected_nice_value; - -public: - TestCompressor(int & detected_nice_value) - : detected_nice_value(detected_nice_value) {} - virtual std::string compress_uri(const std::string & uri) - { - return uri; - } - - virtual void compress_serialized_bag_message( - const rosbag2_storage::SerializedBagMessage * bag_message, - rosbag2_storage::SerializedBagMessage * compressed_message) - { -#ifdef _WIN32 - int cur_nice_value = getpriority(PRIO_PROCESS, 0); - if (cur_nice_value != -1 && errno == 0) { - - detected_nice_value = cur_nice_value; - } +#ifndef _WIN32 + const int wanted_thread_priority = 10; #else - //FIXME implement windows version + const int wanted_thread_priority = THREAD_MODE_BACKGROUND_BEGIN; #endif - *compressed_message = *bag_message; - } - - /** - * Get the identifier of the compression algorithm. - * This is appended to the extension of the compressed file. - */ - virtual std::string get_compression_identifier() const - { - return "niceTest"; - } - }; - - class FakeFactory : public rosbag2_compression::CompressionFactory - { - int & detected_nice_value; - -public: - FakeFactory(int & detected_nice_value) - : detected_nice_value(detected_nice_value) {} - - virtual std::shared_ptr - create_compressor(const std::string & /*compression_format*/) - { - return std::make_shared(detected_nice_value); - } - }; - // queue size should be 0 or at least the number of remaining messages to prevent message loss rosbag2_compression::CompressionOptions compression_options { DefaultTestCompressor, rosbag2_compression::CompressionMode::MESSAGE, kCompressionQueueSize, kDefaultCompressionQueueThreads, - wanted_nice_value + wanted_thread_priority }; +#ifndef _WIN32 // nice values are in the range from -20 to +19, so this value will never be read - int detected_nice_value = 100; + int detected_thread_priority = 100; +#else + int detected_thread_priority = THREAD_PRIORITY_ERROR_RETURN; +#endif initializeFakeFileStorage(); - initializeWriter(compression_options, std::make_unique(detected_nice_value)); + initializeWriter( + compression_options, + std::make_unique(detected_thread_priority)); writer_->open(tmp_dir_storage_options_); writer_->create_topic({test_topic_name, test_topic_type, "", "", ""}); @@ -490,8 +443,7 @@ TEST_P(SequentialCompressionWriterTest, writer_sets_nice_value) } writer_.reset(); // reset will call writer destructor - EXPECT_EQ(detected_nice_value, *compression_options.thread_nice_value); - + EXPECT_EQ(detected_thread_priority, *compression_options.thread_nice_value); EXPECT_EQ(fake_storage_size_, kNumMessagesToWrite); }