diff --git a/rosbag_fancy/src/cmd_play.cpp b/rosbag_fancy/src/cmd_play.cpp index d3a49e3..3c02611 100644 --- a/rosbag_fancy/src/cmd_play.cpp +++ b/rosbag_fancy/src/cmd_play.cpp @@ -11,6 +11,8 @@ #include #include +#include +#include #include @@ -22,6 +24,7 @@ namespace po = boost::program_options; using namespace rosbag_fancy; +using namespace rosbag_fancy_msgs; int play(const std::vector& options) { @@ -33,6 +36,7 @@ int play(const std::vector& options) desc.add_options() ("help", "Display this help message") ("clock", "Publish clock (requires use_sim_time)") + ("no-ui", "Disable terminal UI") ; po::options_description hidden("Hidden"); @@ -85,10 +89,11 @@ int play(const std::vector& options) struct Bag { explicit Bag(const std::string& filename) - : reader{filename} + : reader{filename}, filename_ ( filename ) {} BagReader reader; + std::string filename_; std::unordered_map publishers; }; @@ -195,6 +200,61 @@ int play(const std::vector& options) }); } + // Start/Stop service calls + ros::ServiceServer srv_start = nh.advertiseService("start", boost::function([&](auto&, auto& resp) { + paused = false; + resp.success = !paused; + if(ui) + ui->setPaused(paused); + return true; + })); + ros::ServiceServer srv_pause = nh.advertiseService("pause", boost::function([&](auto&, auto& resp) { + paused = true; + resp.success = paused; + if(ui) + ui->setPaused(paused); + return true; + })); + ros::ServiceServer srv_toggle = nh.advertiseService("toggle", boost::function([&](auto&, auto& resp) { + paused = !paused; + if(ui) + ui->setPaused(paused); + resp.success = true; + return true; + })); + + // Status publisher + ros::Publisher pub_status = nh.advertise("status", 1); + ros::SteadyTimer timer_status = nh.createSteadyTimer(ros::WallDuration(0.1), boost::function([&](auto&) { + auto msg = boost::make_shared(); + msg->header.stamp = ros::Time::now(); + + msg->status = paused ? PlayStatus::STATUS_PAUSED : PlayStatus::STATUS_RUNNING; + + msg->currentTime = currentTime; + msg->startTime = startTime; + msg->endTime = endTime; + msg->current = currentTime-startTime; + msg->duration = endTime-startTime; + + msg->bagfiles.reserve(bags.size()); + for(const auto& bag : bags) + msg->bagfiles.emplace_back(bag.filename_); + + msg->topics.reserve(topicManager.topics().size()); + for(const auto& topic : topicManager.topics()) + { + msg->topics.emplace_back(); + auto& topicMsg = msg->topics.back(); + + topicMsg.name = topic.name; + topicMsg.bandwidth = topic.bandwidth; + topicMsg.rate = topic.messageRate; + } + + pub_status.publish(msg); + })); + while(ros::ok()) { if(paused) @@ -213,7 +273,8 @@ int play(const std::vector& options) ros::SteadyTime::sleepUntil(wallStamp); currentTime = msg.stamp; - ui->setPositionInBag(msg.stamp); + if(ui) + ui->setPositionInBag(msg.stamp); bags[it->bagIndex].publishers[msg.connection->id].publish(msg); @@ -237,9 +298,9 @@ int play(const std::vector& options) ros::spinOnce(); - // Handle key input if(ui) { + // Handle key input fd_set fds{}; FD_ZERO(&fds); FD_SET(STDIN_FILENO, &fds); @@ -252,9 +313,9 @@ int play(const std::vector& options) } else if(ret != 0) ui->handleInput(); - } - ui->setPaused(paused); + ui->setPaused(paused); + } } return 0; diff --git a/rosbag_fancy_msgs/CMakeLists.txt b/rosbag_fancy_msgs/CMakeLists.txt index 2e132d0..de87545 100644 --- a/rosbag_fancy_msgs/CMakeLists.txt +++ b/rosbag_fancy_msgs/CMakeLists.txt @@ -10,6 +10,8 @@ find_package(catkin REQUIRED COMPONENTS add_message_files(FILES Status.msg TopicStatus.msg + PlayStatus.msg + PlayTopicStatus.msg ) generate_messages(DEPENDENCIES diff --git a/rosbag_fancy_msgs/msg/PlayStatus.msg b/rosbag_fancy_msgs/msg/PlayStatus.msg new file mode 100644 index 0000000..b44f5a4 --- /dev/null +++ b/rosbag_fancy_msgs/msg/PlayStatus.msg @@ -0,0 +1,17 @@ + +uint8 STATUS_PAUSED = 0 +uint8 STATUS_RUNNING = 1 + + +Header header + +uint8 status +duration duration +duration current +time currentTime +time startTime +time endTime +string[] bagfiles + +PlayTopicStatus[] topics + diff --git a/rosbag_fancy_msgs/msg/PlayTopicStatus.msg b/rosbag_fancy_msgs/msg/PlayTopicStatus.msg new file mode 100644 index 0000000..d5710de --- /dev/null +++ b/rosbag_fancy_msgs/msg/PlayTopicStatus.msg @@ -0,0 +1,5 @@ + +string name + +float32 rate +float32 bandwidth