diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 2198987e1e..c8be6492ea 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -27,6 +27,13 @@ from rosbag2_py import StorageOptions import yaml +def positive_float(arg: str) -> float: + value = float(arg) + if value <= 0: + raise ValueError(f'Value {value} is less than or equal to zero.') + return value + + class PlayVerb(VerbExtension): """Play back ROS data from a bag.""" @@ -71,6 +78,10 @@ def add_arguments(self, parser, cli_name): # noqa: D102 ' pragmas: [\"\" = ]' 'Note that applicable settings are limited to read-only for ros2 bag play.' 'For a list of sqlite3 settings, refer to sqlite3 documentation') + parser.add_argument( + '--clock', type=positive_float, default=0, + help='Publish to /clock at a specific frequency in Hz, to act as a ROS Time Source. ' + 'Value must be positive. Defaults to not publishing.') def main(self, *, args): # noqa: D102 qos_profile_overrides = {} # Specify a valid default @@ -104,6 +115,7 @@ def main(self, *, args): # noqa: D102 play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = args.loop play_options.topic_remapping_options = topic_remapping + play_options.clock_publish_frequency = args.clock player = Player() player.play(storage_options, play_options) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index e6c98f262b..22fc39e9be 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -190,6 +190,7 @@ PYBIND11_MODULE(_transport, m) { &PlayOptions::setTopicQoSProfileOverrides) .def_readwrite("loop", &PlayOptions::loop) .def_readwrite("topic_remapping_options", &PlayOptions::topic_remapping_options) + .def_readwrite("clock_publish_frequency", &PlayOptions::clock_publish_frequency) ; py::class_(m, "RecordOptions") diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 36fd956292..b9ceddda11 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -229,16 +229,8 @@ void Player::prepare_clock(const PlayOptions & options) // should not own transport-based functionality std::chrono::duration clock_period{1.0 / options.clock_publish_frequency}; auto nanos = std::chrono::duration_cast(clock_period); - ROSBAG2_TRANSPORT_LOG_WARN("Nanos is %lu", nanos.count()); - rclcpp::QoS clock_qos = rclcpp::QoS(1) - // offer as strict as possible, will match if only best_effort requested - .reliable() - // new joiner can get a clock sample immediately no matter when it was published - .transient_local() - // offer a deadline based on the current rate so that subscriptions can use it if wanted - .deadline(clock_period); clock_publisher_ = rosbag2_transport_->create_publisher( - "/clock", clock_qos); + "/clock", rclcpp::ClockQoS()); clock_publish_timer_ = rosbag2_transport_->create_wall_timer( nanos, [this]() { auto msg = rosgraph_msgs::msg::Clock();