From 5a8a0a7c7edd3ea921dd7825faa8058c23b32b70 Mon Sep 17 00:00:00 2001 From: Geoffrey Biggs Date: Wed, 1 Jun 2022 17:55:55 +0900 Subject: [PATCH] Correct errors introduced by rebase Signed-off-by: Geoffrey Biggs --- .../include/rosbag2_transport/player.hpp | 1 - .../src/rosbag2_transport/player.cpp | 19 ++----------------- 2 files changed, 2 insertions(+), 18 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index e68f0e1093..66821e2382 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -242,7 +242,6 @@ class Player : public rclcpp::Node rclcpp::Service::SharedPtr srv_set_rate_; rclcpp::Service::SharedPtr srv_play_; rclcpp::Service::SharedPtr srv_play_next_; - rclcpp::Service::SharedPtr srv_play_for_; rclcpp::Service::SharedPtr srv_burst_; rclcpp::Service::SharedPtr srv_seek_; diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 866808f2e6..8599455ed3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -479,13 +479,12 @@ void Player::play_messages_from_queue(const rcutils_duration_value_t & play_unti ready_to_play_from_queue_cv_.notify_all(); } while (message_ptr != nullptr && rclcpp::ok()) { - rosbag2_storage::SerializedBagMessageSharedPtr message = *message_ptr; - if (play_until_time >= starting_time_ && message->time_stamp > play_until_time) { + if (play_until_time >= starting_time_ && message_ptr->time_stamp > play_until_time) { break; } // Do not move on until sleep_until returns true // It will always sleep, so this is not a tight busy loop on pause - while (rclcpp::ok() && !clock_->sleep_until(message->time_stamp)) { + while (rclcpp::ok() && !clock_->sleep_until(message_ptr->time_stamp)) { if (std::atomic_exchange(&cancel_wait_for_next_message_, false)) { break; } @@ -723,20 +722,6 @@ void Player::create_control_services() { response->success = play_next(); }); - srv_play_for_ = create_service( - "~/play_for", - [this]( - const std::shared_ptr/* request_header */, - const std::shared_ptr request, - const std::shared_ptr response) - { - const rcutils_duration_value_t duration = - static_cast(request->duration.sec) * - static_cast(1000000000) + - static_cast(request->duration.nanosec); - play({duration}); - response->success = true; - }); srv_burst_ = create_service( "~/burst", [this](