Skip to content

Commit

Permalink
Add a Seek service (ros2#874)
Browse files Browse the repository at this point in the history 
Signed-off-by: Emerson Knapp <eknapp@amazon.com>
  • Loading branch information
@emersonknapp
emersonknapp authored Oct 28, 2021
1 parent 2e6ec0d commit 3581a16
Show file tree
Hide file tree
Showing 2 changed files with 75 additions and 65 deletions.
5 changes: 5 additions & 0 deletions rosbag2_transport/include/rosbag2_transport/player.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include "rosbag2_interfaces/srv/play_next.hpp"
#include "rosbag2_interfaces/srv/resume.hpp"
#include "rosbag2_interfaces/srv/set_rate.hpp"
#include "rosbag2_interfaces/srv/seek.hpp"
#include "rosbag2_interfaces/srv/toggle_paused.hpp"
#include "rosbag2_storage/serialized_bag_message.hpp"
#include "rosbag2_storage/storage_options.hpp"
Expand Down Expand Up @@ -169,6 +170,8 @@ class Player : public rclcpp::Node
const std::string & op_name);
void add_keyboard_callbacks();

void create_control_services();

rosbag2_storage::StorageOptions storage_options_;
rosbag2_transport::PlayOptions play_options_;
moodycamel::ReaderWriterQueue<rosbag2_storage::SerializedBagMessageSharedPtr> message_queue_;
Expand All @@ -182,13 +185,15 @@ class Player : public rclcpp::Node

rcutils_time_point_value_t starting_time_;

// control services
rclcpp::Service<rosbag2_interfaces::srv::Pause>::SharedPtr srv_pause_;
rclcpp::Service<rosbag2_interfaces::srv::Resume>::SharedPtr srv_resume_;
rclcpp::Service<rosbag2_interfaces::srv::TogglePaused>::SharedPtr srv_toggle_paused_;
rclcpp::Service<rosbag2_interfaces::srv::IsPaused>::SharedPtr srv_is_paused_;
rclcpp::Service<rosbag2_interfaces::srv::GetRate>::SharedPtr srv_get_rate_;
rclcpp::Service<rosbag2_interfaces::srv::SetRate>::SharedPtr srv_set_rate_;
rclcpp::Service<rosbag2_interfaces::srv::PlayNext>::SharedPtr srv_play_next_;
rclcpp::Service<rosbag2_interfaces::srv::Seek>::SharedPtr srv_seek_;

// defaults
std::shared_ptr<KeyboardHandler> keyboard_handler_;
Expand Down
135 changes: 70 additions & 65 deletions rosbag2_transport/src/rosbag2_transport/player.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,71 +147,7 @@ Player::Player(
topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides;
prepare_publishers();
}
// service callbacks
srv_pause_ = create_service<rosbag2_interfaces::srv::Pause>(
"~/pause",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::Pause::Request>/* request */,
const std::shared_ptr<rosbag2_interfaces::srv::Pause::Response>/* response */)
{
pause();
});
srv_resume_ = create_service<rosbag2_interfaces::srv::Resume>(
"~/resume",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::Resume::Request>/* request */,
const std::shared_ptr<rosbag2_interfaces::srv::Resume::Response>/* response */)
{
resume();
});
srv_toggle_paused_ = create_service<rosbag2_interfaces::srv::TogglePaused>(
"~/toggle_paused",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::TogglePaused::Request>/* request */,
const std::shared_ptr<rosbag2_interfaces::srv::TogglePaused::Response>/* response */)
{
toggle_paused();
});
srv_is_paused_ = create_service<rosbag2_interfaces::srv::IsPaused>(
"~/is_paused",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::IsPaused::Request>/* request */,
const std::shared_ptr<rosbag2_interfaces::srv::IsPaused::Response> response)
{
response->paused = is_paused();
});
srv_get_rate_ = create_service<rosbag2_interfaces::srv::GetRate>(
"~/get_rate",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::GetRate::Request>/* request */,
const std::shared_ptr<rosbag2_interfaces::srv::GetRate::Response> response)
{
response->rate = get_rate();
});
srv_set_rate_ = create_service<rosbag2_interfaces::srv::SetRate>(
"~/set_rate",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::SetRate::Request> request,
const std::shared_ptr<rosbag2_interfaces::srv::SetRate::Response> response)
{
response->success = set_rate(request->rate);
});
srv_play_next_ = create_service<rosbag2_interfaces::srv::PlayNext>(
"~/play_next",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::PlayNext::Request>/* request */,
const std::shared_ptr<rosbag2_interfaces::srv::PlayNext::Response> response)
{
response->success = play_next();
});
// keyboard callbacks
create_control_services();
add_keyboard_callbacks();
}

Expand Down Expand Up @@ -608,4 +544,73 @@ void Player::add_keyboard_callbacks()
);
}

void Player::create_control_services()
{
srv_pause_ = create_service<rosbag2_interfaces::srv::Pause>(
"~/pause",
[this](
rosbag2_interfaces::srv::Pause::Request::ConstSharedPtr,
rosbag2_interfaces::srv::Pause::Response::SharedPtr)
{
pause();
});
srv_resume_ = create_service<rosbag2_interfaces::srv::Resume>(
"~/resume",
[this](
rosbag2_interfaces::srv::Resume::Request::ConstSharedPtr,
rosbag2_interfaces::srv::Resume::Response::SharedPtr)
{
resume();
});
srv_toggle_paused_ = create_service<rosbag2_interfaces::srv::TogglePaused>(
"~/toggle_paused",
[this](
rosbag2_interfaces::srv::TogglePaused::Request::ConstSharedPtr,
rosbag2_interfaces::srv::TogglePaused::Response::SharedPtr)
{
toggle_paused();
});
srv_is_paused_ = create_service<rosbag2_interfaces::srv::IsPaused>(
"~/is_paused",
[this](
rosbag2_interfaces::srv::IsPaused::Request::ConstSharedPtr,
rosbag2_interfaces::srv::IsPaused::Response::SharedPtr response)
{
response->paused = is_paused();
});
srv_get_rate_ = create_service<rosbag2_interfaces::srv::GetRate>(
"~/get_rate",
[this](
rosbag2_interfaces::srv::GetRate::Request::ConstSharedPtr,
rosbag2_interfaces::srv::GetRate::Response::SharedPtr response)
{
response->rate = get_rate();
});
srv_set_rate_ = create_service<rosbag2_interfaces::srv::SetRate>(
"~/set_rate",
[this](
rosbag2_interfaces::srv::SetRate::Request::ConstSharedPtr request,
rosbag2_interfaces::srv::SetRate::Response::SharedPtr response)
{
response->success = set_rate(request->rate);
});
srv_play_next_ = create_service<rosbag2_interfaces::srv::PlayNext>(
"~/play_next",
[this](
rosbag2_interfaces::srv::PlayNext::Request::ConstSharedPtr,
rosbag2_interfaces::srv::PlayNext::Response::SharedPtr response)
{
response->success = play_next();
});
srv_seek_ = create_service<rosbag2_interfaces::srv::Seek>(
"~/seek",
[this](
rosbag2_interfaces::srv::Seek::Request::ConstSharedPtr request,
rosbag2_interfaces::srv::Seek::Response::SharedPtr response)
{
seek(rclcpp::Time(request->time).nanoseconds());
response->success = true;
});
}

} // namespace rosbag2_transport

0 comments on commit 3581a16

Please sign in to comment.