Skip to content

Commit 96c11b0

Browse files
Redesign Player class with PIMPL idiom (#1447)
* Player PIMPL Signed-off-by: Patrick Roncagliolo <[email protected]> * Fix Signed-off-by: Patrick Roncagliolo <[email protected]> * Fix Signed-off-by: Patrick Roncagliolo <[email protected]> * Fix Signed-off-by: Patrick Roncagliolo <[email protected]> * Fix Signed-off-by: Patrick Roncagliolo <[email protected]> * Fix Signed-off-by: Patrick Roncagliolo <[email protected]> * Fix Signed-off-by: Patrick Roncagliolo <[email protected]> * WIP: make test compile again Signed-off-by: Patrick Roncagliolo <[email protected]> * WIP: make test compile again Signed-off-by: Patrick Roncagliolo <[email protected]> * WIP: make test compile again Signed-off-by: Patrick Roncagliolo <[email protected]> * WIP: make test compile again Signed-off-by: Patrick Roncagliolo <[email protected]> * WIP: make test compile again Signed-off-by: Patrick Roncagliolo <[email protected]> * Uncrustify Signed-off-by: Patrick Roncagliolo <[email protected]> * WIP: make test compile again Signed-off-by: Patrick Roncagliolo <[email protected]> * WIP: make test compile again Signed-off-by: Patrick Roncagliolo <[email protected]> * Clean Signed-off-by: Patrick Roncagliolo <[email protected]> * Revert wrong reformat Signed-off-by: Patrick Roncagliolo <[email protected]> * Some renames and code formatting Signed-off-by: Michael Orlov <[email protected]> * Fixes for failing keyboard_handler tests Use upper level public API from owner class for callbacks to facilitate unit tests Signed-off-by: Michael Orlov <[email protected]> --------- Signed-off-by: Patrick Roncagliolo <[email protected]> Signed-off-by: Michael Orlov <[email protected]> Co-authored-by: Michael Orlov <[email protected]>
1 parent 64e2972 commit 96c11b0

File tree

3 files changed

+558
-267
lines changed

3 files changed

+558
-267
lines changed

rosbag2_transport/include/rosbag2_transport/player.hpp

+27-109
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,9 @@ class Reader;
6262

6363
namespace rosbag2_transport
6464
{
65+
66+
class PlayerImpl;
67+
6568
class Player : public rclcpp::Node
6669
{
6770
public:
@@ -200,118 +203,33 @@ class Player : public rclcpp::Node
200203
void delete_on_play_message_callback(const callback_handle_t & handle);
201204

202205
protected:
203-
struct play_msg_callback_data
204-
{
205-
callback_handle_t handle;
206-
play_msg_callback_t callback;
207-
};
208-
209-
std::mutex on_play_msg_callbacks_mutex_;
210-
std::forward_list<play_msg_callback_data> on_play_msg_pre_callbacks_;
211-
std::forward_list<play_msg_callback_data> on_play_msg_post_callbacks_;
212-
213-
class PlayerPublisher final
214-
{
215-
public:
216-
explicit PlayerPublisher(
217-
std::shared_ptr<rclcpp::GenericPublisher> pub,
218-
bool disable_loan_message)
219-
: publisher_(std::move(pub))
220-
{
221-
using std::placeholders::_1;
222-
if (disable_loan_message || !publisher_->can_loan_messages()) {
223-
publish_func_ = std::bind(&rclcpp::GenericPublisher::publish, publisher_, _1);
224-
} else {
225-
publish_func_ = std::bind(&rclcpp::GenericPublisher::publish_as_loaned_msg, publisher_, _1);
226-
}
227-
}
228-
229-
~PlayerPublisher() {}
230-
231-
void publish(const rclcpp::SerializedMessage & message)
232-
{
233-
publish_func_(message);
234-
}
235-
236-
std::shared_ptr<rclcpp::GenericPublisher> generic_publisher()
237-
{
238-
return publisher_;
239-
}
206+
/// \brief Getter for publishers corresponding to each topic
207+
/// \return Hashtable representing topic to publisher map excluding inner clock_publisher
208+
ROSBAG2_TRANSPORT_PUBLIC
209+
std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericPublisher>> get_publishers();
240210

241-
private:
242-
std::shared_ptr<rclcpp::GenericPublisher> publisher_;
243-
std::function<void(const rclcpp::SerializedMessage &)> publish_func_;
244-
};
245-
bool is_ready_to_play_from_queue_{false};
246-
std::mutex ready_to_play_from_queue_mutex_;
247-
std::condition_variable ready_to_play_from_queue_cv_;
248-
rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_publisher_;
249-
std::unordered_map<std::string, std::shared_ptr<PlayerPublisher>> publishers_;
211+
/// \brief Getter for inner clock_publisher
212+
/// \return Shared pointer to the inner clock_publisher
213+
ROSBAG2_TRANSPORT_PUBLIC
214+
rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr get_clock_publisher();
215+
216+
/// \brief Blocks and wait on condition variable until first message will be taken from read
217+
/// queue
218+
ROSBAG2_TRANSPORT_PUBLIC
219+
void wait_for_playback_to_start();
220+
221+
/// \brief Getter for the number of registered on_play_msg_pre_callbacks
222+
/// \return Number of registered on_play_msg_pre_callbacks
223+
ROSBAG2_TRANSPORT_PUBLIC
224+
size_t get_number_of_registered_on_play_msg_pre_callbacks();
225+
226+
/// \brief Getter for the number of registered on_play_msg_post_callbacks
227+
/// \return Number of registered on_play_msg_post_callbacks
228+
ROSBAG2_TRANSPORT_PUBLIC
229+
size_t get_number_of_registered_on_play_msg_post_callbacks();
250230

251231
private:
252-
rosbag2_storage::SerializedBagMessageSharedPtr peek_next_message_from_queue();
253-
void load_storage_content();
254-
bool is_storage_completely_loaded() const;
255-
void enqueue_up_to_boundary(size_t boundary) RCPPUTILS_TSA_REQUIRES(reader_mutex_);
256-
void wait_for_filled_queue() const;
257-
void play_messages_from_queue();
258-
void prepare_publishers();
259-
bool publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message);
260-
static callback_handle_t get_new_on_play_msg_callback_handle();
261-
void add_key_callback(
262-
KeyboardHandler::KeyCode key,
263-
const std::function<void()> & cb,
264-
const std::string & op_name);
265-
void add_keyboard_callbacks();
266-
void create_control_services();
267-
void configure_play_until_timestamp();
268-
bool shall_stop_at_timestamp(const rcutils_time_point_value_t & msg_timestamp) const;
269-
270-
static constexpr double read_ahead_lower_bound_percentage_ = 0.9;
271-
static const std::chrono::milliseconds queue_read_wait_period_;
272-
std::atomic_bool cancel_wait_for_next_message_{false};
273-
std::atomic_bool stop_playback_{false};
274-
275-
std::mutex reader_mutex_;
276-
std::unique_ptr<rosbag2_cpp::Reader> reader_ RCPPUTILS_TSA_GUARDED_BY(reader_mutex_);
277-
278-
void publish_clock_update();
279-
void publish_clock_update(const rclcpp::Time & time);
280-
281-
rosbag2_storage::StorageOptions storage_options_;
282-
rosbag2_transport::PlayOptions play_options_;
283-
rcutils_time_point_value_t play_until_timestamp_ = -1;
284-
moodycamel::ReaderWriterQueue<rosbag2_storage::SerializedBagMessageSharedPtr> message_queue_;
285-
mutable std::future<void> storage_loading_future_;
286-
std::atomic_bool load_storage_content_{true};
287-
std::unordered_map<std::string, rclcpp::QoS> topic_qos_profile_overrides_;
288-
std::unique_ptr<rosbag2_cpp::PlayerClock> clock_;
289-
std::shared_ptr<rclcpp::TimerBase> clock_publish_timer_;
290-
std::mutex skip_message_in_main_play_loop_mutex_;
291-
bool skip_message_in_main_play_loop_ RCPPUTILS_TSA_GUARDED_BY(
292-
skip_message_in_main_play_loop_mutex_) = false;
293-
std::atomic_bool is_in_playback_{false};
294-
295-
rcutils_time_point_value_t starting_time_;
296-
297-
// control services
298-
rclcpp::Service<rosbag2_interfaces::srv::Pause>::SharedPtr srv_pause_;
299-
rclcpp::Service<rosbag2_interfaces::srv::Resume>::SharedPtr srv_resume_;
300-
rclcpp::Service<rosbag2_interfaces::srv::TogglePaused>::SharedPtr srv_toggle_paused_;
301-
rclcpp::Service<rosbag2_interfaces::srv::IsPaused>::SharedPtr srv_is_paused_;
302-
rclcpp::Service<rosbag2_interfaces::srv::GetRate>::SharedPtr srv_get_rate_;
303-
rclcpp::Service<rosbag2_interfaces::srv::SetRate>::SharedPtr srv_set_rate_;
304-
rclcpp::Service<rosbag2_interfaces::srv::Play>::SharedPtr srv_play_;
305-
rclcpp::Service<rosbag2_interfaces::srv::PlayNext>::SharedPtr srv_play_next_;
306-
rclcpp::Service<rosbag2_interfaces::srv::Burst>::SharedPtr srv_burst_;
307-
rclcpp::Service<rosbag2_interfaces::srv::Seek>::SharedPtr srv_seek_;
308-
rclcpp::Service<rosbag2_interfaces::srv::Stop>::SharedPtr srv_stop_;
309-
310-
rclcpp::Publisher<rosbag2_interfaces::msg::ReadSplitEvent>::SharedPtr split_event_pub_;
311-
312-
// defaults
313-
std::shared_ptr<KeyboardHandler> keyboard_handler_;
314-
std::vector<KeyboardHandler::callback_handle_t> keyboard_callbacks_;
232+
std::unique_ptr<PlayerImpl> pimpl_;
315233
};
316234

317235
} // namespace rosbag2_transport

0 commit comments

Comments
 (0)