diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 5c4e0218d4..c3b84121e9 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -145,6 +145,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'By default, if loaned message can be used, messages are published as loaned ' 'message. It can help to reduce the number of data copies, so there is a greater ' 'benefit for sending big data.') + parser.add_argument( + '--log-level', type=str, default='info', + choices=['debug', 'info', 'warn', 'error', 'fatal'], + help='Logging level.') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -202,7 +206,7 @@ def main(self, *, args): # noqa: D102 play_options.wait_acked_timeout = args.wait_for_all_acked play_options.disable_loan_message = args.disable_loan_message - player = Player() + player = Player(args.log_level) try: player.play(storage_options, play_options) except KeyboardInterrupt: diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 60599d5197..9932ba7aa1 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -143,6 +143,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '--snapshot-mode', action='store_true', help='Enable snapshot mode. Messages will not be written to the bagfile until ' 'the "/rosbag2_recorder/snapshot" service is called.') + parser.add_argument( + '--log-level', type=str, default='info', + choices=['debug', 'info', 'warn', 'error', 'fatal'], + help='Logging level.') # Storage configuration add_writer_storage_plugin_extensions(parser) @@ -255,7 +259,7 @@ def main(self, *, args): # noqa: D102 record_options.ignore_leaf_topics = args.ignore_leaf_topics record_options.use_sim_time = args.use_sim_time - recorder = Recorder() + recorder = Recorder(args.log_level) try: recorder.record(storage_options, record_options, args.node_name) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 0a8a68f0ca..574519ac41 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -37,6 +38,36 @@ typedef std::unordered_map QoSMap; namespace { +class Arguments +{ +public: + explicit Arguments(const std::vector & args) + : arguments_(args) + { + std::for_each( + arguments_.begin(), arguments_.end(), + [this](const std::string & arg) { + pointers_.push_back(const_cast(arg.c_str())); + } + ); + pointers_.push_back(nullptr); + } + + char ** argv() + { + return arguments_.empty() ? nullptr : pointers_.data(); + } + + [[nodiscard]] int argc() const + { + return static_cast(arguments_.size()); + } + +private: + std::vector arguments_; + std::vector pointers_; +}; + rclcpp::QoS qos_from_handle(const py::handle source) { PyObject * raw_obj = PyObject_CallMethod(source.ptr(), "get_c_qos_profile", ""); @@ -130,9 +161,10 @@ namespace rosbag2_py class Player { public: - Player() + explicit Player(const std::string & log_level = "info") { - rclcpp::init(0, nullptr); + Arguments arguments({"--ros-args", "--log-level", log_level}); + rclcpp::init(arguments.argc(), arguments.argv()); } virtual ~Player() @@ -190,9 +222,10 @@ class Player class Recorder { public: - Recorder() + explicit Recorder(const std::string & log_level = "info") { - rclcpp::init(0, nullptr); + Arguments arguments({"--ros-args", "--log-level", log_level}); + rclcpp::init(arguments.argc(), arguments.argv()); } virtual ~Recorder() @@ -394,13 +427,15 @@ PYBIND11_MODULE(_transport, m) { ; py::class_(m, "Player") - .def(py::init()) + .def(py::init<>()) + .def(py::init()) .def("play", &rosbag2_py::Player::play, py::arg("storage_options"), py::arg("play_options")) .def("burst", &rosbag2_py::Player::burst) ; py::class_(m, "Recorder") - .def(py::init()) + .def(py::init<>()) + .def(py::init()) .def( "record", &rosbag2_py::Recorder::record, py::arg("storage_options"), py::arg("record_options"), py::arg("node_name") = "rosbag2_recorder") diff --git a/rosbag2_py/test/test_transport.py b/rosbag2_py/test/test_transport.py index 548708d409..614e39416c 100644 --- a/rosbag2_py/test/test_transport.py +++ b/rosbag2_py/test/test_transport.py @@ -42,6 +42,26 @@ def test_options_qos_conversion(): assert record_options.topic_qos_profile_overrides == simple_overrides +def test_player_log_level(): + rosbag2_py.Player() # Test for default constructor + valid_log_level = 'debug' + rosbag2_py.Player(valid_log_level) + + invalid_log_level = 'xxx' + with pytest.raises(RuntimeError): + rosbag2_py.Player(invalid_log_level) + + +def test_recoder_log_level(): + rosbag2_py.Recorder() # Test for default constructor + valid_log_level = 'debug' + rosbag2_py.Recorder(valid_log_level) + + invalid_log_level = 'xxx' + with pytest.raises(RuntimeError): + rosbag2_py.Recorder(invalid_log_level) + + @pytest.mark.parametrize('storage_id', TESTED_STORAGE_IDS) def test_record_cancel(tmp_path, storage_id): bag_path = str(tmp_path / 'test_record_cancel')