Skip to content

Commit

Permalink
Move TimeControllerClock into separate file
Browse files Browse the repository at this point in the history
Signed-off-by: Emerson Knapp <[email protected]>
  • Loading branch information
Emerson Knapp committed Apr 2, 2021
1 parent 20f37f3 commit 3173b87
Show file tree
Hide file tree
Showing 7 changed files with 88 additions and 58 deletions.
10 changes: 5 additions & 5 deletions rosbag2_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -53,9 +53,9 @@ add_library(${PROJECT_NAME} SHARED
src/rosbag2_cpp/cache/cache_consumer.cpp
src/rosbag2_cpp/cache/message_cache_buffer.cpp
src/rosbag2_cpp/cache/message_cache.cpp
src/rosbag2_cpp/clocks/time_controller_clock.cpp
src/rosbag2_cpp/converter.cpp
src/rosbag2_cpp/info.cpp
src/rosbag2_cpp/player_clock.cpp
src/rosbag2_cpp/reader.cpp
src/rosbag2_cpp/readers/sequential_reader.cpp
src/rosbag2_cpp/rmw_implemented_serialization_format_converter.cpp
Expand Down Expand Up @@ -224,11 +224,11 @@ if(BUILD_TESTING)
target_link_libraries(test_multifile_reader ${PROJECT_NAME})
endif()

ament_add_gmock(test_player_clock
test/rosbag2_cpp/test_player_clock.cpp)
if(TARGET test_player_clock)
ament_add_gmock(test_time_controller_clock
test/rosbag2_cpp/test_time_controller_clock.cpp)
if(TARGET test_time_controller_clock)
# ament_target_dependencies(test_player_clock rosbag2_cpp)
target_link_libraries(test_player_clock ${PROJECT_NAME})
target_link_libraries(test_time_controller_clock ${PROJECT_NAME})
endif()
endif()

Expand Down
74 changes: 74 additions & 0 deletions rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
// Copyright 2021 Amazon.com, Inc. or its affiliates. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ROSBAG2_CPP__CLOCKS__PLAYER_CLOCK_HPP_
#define ROSBAG2_CPP__CLOCKS__PLAYER_CLOCK_HPP_

#include <chrono>
#include <functional>
#include <memory>

#include "rcutils/time.h"
#include "rosbag2_cpp/visibility_control.hpp"

namespace rosbag2_cpp
{

/**
* Virtual interface used to drive the timing of bag playback.
* This clock should be used to query times and sleep between message playing,
* so that the complexity involved around time control and time sources
* is encapsulated in this one place.
*/
class PlayerClock
{
public:
// Type representing the current time as according to the playback
typedef rcutils_time_point_value_t ROSTimePoint;
/**
* Type representing an arbitrary steady time, used to measure real-time durations
* This type is never exposed by the PlayerClock - it is only used as input to the PlayerClock.
*/
typedef std::chrono::time_point<std::chrono::steady_clock> SteadyTimePoint;
typedef std::function<SteadyTimePoint()> NowFunction;

ROSBAG2_CPP_PUBLIC
virtual ~PlayerClock() = default;

/**
* Calculate and return current ROSTimePoint based on starting time, playback rate, pause state.
*/
ROSBAG2_CPP_PUBLIC
virtual ROSTimePoint now() const = 0;

/**
* Try to sleep (non-busy) the current thread until the provided time is reached - according to this Clock
*
* Return true if the time has been reached, false if it was not successfully reached after sleeping
* for the appropriate duration.
* The user should not take action based on this sleep until it returns true.
*/
ROSBAG2_CPP_PUBLIC
virtual bool sleep_until(ROSTimePoint until) = 0;

/**
* Return the current playback rate.
*/
ROSBAG2_CPP_PUBLIC
virtual double get_rate() const = 0;
};

} // namespace rosbag2_cpp

#endif // ROSBAG2_CPP__CLOCKS__PLAYER_CLOCK_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,63 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ROSBAG2_CPP__PLAYER_CLOCK_HPP_
#define ROSBAG2_CPP__PLAYER_CLOCK_HPP_
#ifndef ROSBAG2_CPP__CLOCKS__TIME_CONTROLLER_CLOCK_HPP_
#define ROSBAG2_CPP__CLOCKS__TIME_CONTROLLER_CLOCK_HPP_

#include <chrono>
#include <functional>
#include <memory>

#include "rcutils/time.h"
#include "rosbag2_cpp/visibility_control.hpp"
#include "rosbag2_cpp/clocks/player_clock.hpp"

namespace rosbag2_cpp
{

/**
* Virtual interface used to drive the timing of bag playback.
* This clock should be used to query times and sleep between message playing,
* so that the complexity involved around time control and time sources
* is encapsulated in this one place.
*/
class PlayerClock
{
public:
// Type representing the current time as according to the playback
typedef rcutils_time_point_value_t ROSTimePoint;
/**
* Type representing an arbitrary steady time, used to measure real-time durations
* This type is never exposed by the PlayerClock - it is only used as input to the PlayerClock.
*/
typedef std::chrono::time_point<std::chrono::steady_clock> SteadyTimePoint;
typedef std::function<SteadyTimePoint()> NowFunction;

ROSBAG2_CPP_PUBLIC
virtual ~PlayerClock() = default;

/**
* Calculate and return current ROSTimePoint based on starting time, playback rate, pause state.
*/
ROSBAG2_CPP_PUBLIC
virtual ROSTimePoint now() const = 0;

/**
* Try to sleep (non-busy) the current thread until the provided time is reached - according to this Clock
*
* Return true if the time has been reached, false if it was not successfully reached after sleeping
* for the appropriate duration.
* The user should not take action based on this sleep until it returns true.
*/
ROSBAG2_CPP_PUBLIC
virtual bool sleep_until(ROSTimePoint until) = 0;

/**
* Return the current playback rate.
*/
ROSBAG2_CPP_PUBLIC
virtual double get_rate() const = 0;
};

/**
* Version of the PlayerClock interface that has control over time.
* It does not listen to any external ROS Time Source and can optionally publish /clock
Expand Down Expand Up @@ -123,6 +77,7 @@ class TimeControllerClock : public PlayerClock
std::unique_ptr<TimeControllerClockImpl> impl_;
};


} // namespace rosbag2_cpp

#endif // ROSBAG2_CPP__PLAYER_CLOCK_HPP_
#endif // ROSBAG2_CPP__CLOCKS__TIME_CONTROLLER_CLOCK_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#include <thread>

#include "rcpputils/thread_safety_annotations.hpp"
#include "rosbag2_cpp/player_clock.hpp"
#include "rosbag2_cpp/clocks/time_controller_clock.hpp"
#include "rosbag2_cpp/types.hpp"

namespace rosbag2_cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

#include <gmock/gmock.h>

#include "rosbag2_cpp/player_clock.hpp"
#include "rosbag2_cpp/clocks/time_controller_clock.hpp"

using namespace testing; // NOLINT
using SteadyTimePoint = rosbag2_cpp::PlayerClock::SteadyTimePoint;
Expand Down
1 change: 1 addition & 0 deletions rosbag2_transport/src/rosbag2_transport/player.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

#include "rcutils/time.h"

#include "rosbag2_cpp/clocks/time_controller_clock.hpp"
#include "rosbag2_cpp/reader.hpp"
#include "rosbag2_cpp/typesupport_helpers.hpp"

Expand Down
2 changes: 1 addition & 1 deletion rosbag2_transport/src/rosbag2_transport/player.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@

#include "rclcpp/qos.hpp"

#include "rosbag2_cpp/player_clock.hpp"
#include "rosbag2_cpp/clocks/player_clock.hpp"
#include "rosbag2_storage/serialized_bag_message.hpp"
#include "rosbag2_transport/play_options.hpp"

Expand Down

0 comments on commit 3173b87

Please sign in to comment.