diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index ec80088c34..3e6500495b 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -15,7 +15,7 @@ import os from ros2bag.verb import VerbExtension - +from ros2cli.node import NODE_NAME_PREFIX from rosbag2_transport import rosbag2_transport_py @@ -42,4 +42,5 @@ def main(self, *, args): # noqa: D102 rosbag2_transport_py.play( uri=bag_file, storage_id=args.storage, + node_prefix=NODE_NAME_PREFIX, read_ahead_queue_size=args.read_ahead_queue_size) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 970eb6398e..23d8e2f496 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -19,7 +19,7 @@ from ros2cli.node.strategy import NodeStrategy from ros2cli.node.strategy import add_arguments - +from ros2cli.node import NODE_NAME_PREFIX from rosbag2_transport import rosbag2_transport_py @@ -75,6 +75,7 @@ def main(self, *, args): # noqa: D102 uri=uri, storage_id=args.storage, serialization_format=args.serialization_format, + node_prefix=NODE_NAME_PREFIX, all=True, no_discovery=args.no_discovery, polling_interval=args.polling_interval) @@ -83,6 +84,7 @@ def main(self, *, args): # noqa: D102 uri=uri, storage_id=args.storage, serialization_format=args.serialization_format, + node_prefix=NODE_NAME_PREFIX, no_discovery=args.no_discovery, polling_interval=args.polling_interval, topics=args.topics) diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index e4259ae78b..bcf113e14d 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -16,6 +16,7 @@ #define ROSBAG2_TRANSPORT__PLAY_OPTIONS_HPP_ #include +#include namespace rosbag2_transport { @@ -24,6 +25,7 @@ struct PlayOptions { public: size_t read_ahead_queue_size; + std::string node_prefix = ""; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index e2cc6b8084..9b2dd4de53 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -29,6 +29,7 @@ struct RecordOptions std::vector topics; std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval; + std::string node_prefix = ""; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp b/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp index 09bde70cc9..4ccf7b8fef 100644 --- a/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp +++ b/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp @@ -85,7 +85,7 @@ class Rosbag2Transport void print_bag_info(const std::string & uri, const std::string & storage_id); private: - std::shared_ptr setup_node(); + std::shared_ptr setup_node(std::string node_prefix = ""); std::shared_ptr reader_; std::shared_ptr writer_; diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp index 115fd3b0b5..bb4bd10570 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp @@ -67,7 +67,7 @@ void Rosbag2Transport::record( writer_->open( storage_options, {rmw_get_serialization_format(), record_options.rmw_serialization_format}); - auto transport_node = setup_node(); + auto transport_node = setup_node(record_options.node_prefix); Recorder recorder(writer_, transport_node); recorder.record(record_options); @@ -76,10 +76,10 @@ void Rosbag2Transport::record( } } -std::shared_ptr Rosbag2Transport::setup_node() +std::shared_ptr Rosbag2Transport::setup_node(std::string node_prefix) { if (!transport_node_) { - transport_node_ = std::make_shared("rosbag2"); + transport_node_ = std::make_shared(node_prefix + "_rosbag2"); } return transport_node_; } @@ -90,7 +90,7 @@ void Rosbag2Transport::play( try { reader_->open(storage_options, {"", rmw_get_serialization_format()}); - auto transport_node = setup_node(); + auto transport_node = setup_node(play_options.node_prefix); Player player(reader_, transport_node); player.play(play_options); diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index 8e9185277f..ce4d6e0586 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -32,6 +32,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * "uri", "storage_id", "serialization_format", + "node_prefix", "all", "no_discovery", "polling_interval", @@ -41,14 +42,16 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * char * uri = nullptr; char * storage_id = nullptr; char * serilization_format = nullptr; + char * node_prefix = nullptr; bool all = false; bool no_discovery = false; uint64_t polling_interval_ms = 100; PyObject * topics = nullptr; - if (!PyArg_ParseTupleAndKeywords(args, kwargs, "sss|bbKO", const_cast(kwlist), + if (!PyArg_ParseTupleAndKeywords(args, kwargs, "ssss|bbKO", const_cast(kwlist), &uri, &storage_id, &serilization_format, + &node_prefix, &all, &no_discovery, &polling_interval_ms, @@ -62,6 +65,7 @@ rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * record_options.all = all; record_options.is_discovery_disabled = no_discovery; record_options.topic_polling_interval = std::chrono::milliseconds(polling_interval_ms); + record_options.node_prefix = std::string(node_prefix); if (topics) { PyObject * topic_iterator = PyObject_GetIter(topics); @@ -93,14 +97,22 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k rosbag2_transport::PlayOptions play_options{}; rosbag2_transport::StorageOptions storage_options{}; - static const char * kwlist[] = {"uri", "storage_id", "read_ahead_queue_size", nullptr}; + static const char * kwlist[] = { + "uri", + "storage_id", + "node_prefix", + "read_ahead_queue_size", + nullptr + }; char * uri; char * storage_id; + char * node_prefix; size_t read_ahead_queue_size; - if (!PyArg_ParseTupleAndKeywords(args, kwargs, "ss|k", const_cast(kwlist), + if (!PyArg_ParseTupleAndKeywords(args, kwargs, "sss|k", const_cast(kwlist), &uri, &storage_id, + &node_prefix, &read_ahead_queue_size)) { return nullptr; @@ -109,6 +121,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k storage_options.uri = std::string(uri); storage_options.storage_id = std::string(storage_id); + play_options.node_prefix = std::string(node_prefix); play_options.read_ahead_queue_size = read_ahead_queue_size; rosbag2_transport::Rosbag2Transport transport;