From 65a48e99928f75bd50ad064429d526f5cccfa136 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 10 May 2024 04:56:33 +0300 Subject: [PATCH 1/2] Add --log-level to ros2 bag play and record Co-authored-by: Michael Orlov Signed-off-by: Roman Sokolkov --- ros2bag/ros2bag/verb/play.py | 6 +++- ros2bag/ros2bag/verb/record.py | 6 +++- rosbag2_py/src/rosbag2_py/_transport.cpp | 45 +++++++++++++++++++++--- rosbag2_py/test/test_transport.py | 20 +++++++++++ 4 files changed, 70 insertions(+), 7 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 4ee8b567ce..8143668735 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -104,6 +104,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 main(self, *, args): # noqa: D102 qos_profile_overrides = {} # Specify a valid default @@ -145,7 +149,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 65d8dd2ea2..a95b553f03 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -164,6 +164,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '--use-sim-time', action='store_true', default=False, help='Use simulation time for message timestamps by subscribing to the /clock topic. ' 'Until first /clock message is received, no messages will be written to bag.') + parser.add_argument( + '--log-level', type=str, default='info', + choices=['debug', 'info', 'warn', 'error', 'fatal'], + help='Logging level.') self._subparser = parser def main(self, *, args): # noqa: D102 @@ -247,7 +251,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) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 1b8129e794..cc2d128657 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", ""); @@ -108,9 +139,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() @@ -142,9 +174,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() @@ -322,11 +355,13 @@ PYBIND11_MODULE(_transport, m) { py::class_(m, "Player") .def(py::init()) + .def(py::init()) .def("play", &rosbag2_py::Player::play) ; 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")) .def_static("cancel", &rosbag2_py::Recorder::cancel) diff --git a/rosbag2_py/test/test_transport.py b/rosbag2_py/test/test_transport.py index d5d3f01a2d..aabbfe7628 100644 --- a/rosbag2_py/test/test_transport.py +++ b/rosbag2_py/test/test_transport.py @@ -48,6 +48,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) + + def test_record_cancel(tmp_path): bag_path = str(tmp_path / 'test_record_cancel') storage_options, converter_options = get_rosbag_options(bag_path) From 86f6bd578543787d11f7a50f86d76a04c2c351c2 Mon Sep 17 00:00:00 2001 From: Roman Sokolkov Date: Tue, 14 May 2024 14:15:22 +0300 Subject: [PATCH 2/2] Fix missing import Signed-off-by: Roman Sokolkov --- rosbag2_py/test/test_transport.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rosbag2_py/test/test_transport.py b/rosbag2_py/test/test_transport.py index aabbfe7628..2006ff44d5 100644 --- a/rosbag2_py/test/test_transport.py +++ b/rosbag2_py/test/test_transport.py @@ -20,6 +20,7 @@ from common import get_rosbag_options, wait_for +import pytest import rclpy from rclpy.qos import QoSProfile import rosbag2_py