From 1334fc865e853602ec64eecec35a615b75f48fdf Mon Sep 17 00:00:00 2001 From: Jan Quenzel Date: Thu, 1 Feb 2024 14:01:25 +0100 Subject: [PATCH 1/4] added no-gui for play with status info and start/stop/pause services. --- rosbag_fancy/src/cmd_play.cpp | 80 ++++++++++++++++++++++++++-- rosbag_fancy_msgs/CMakeLists.txt | 1 + rosbag_fancy_msgs/msg/StatusPlay.msg | 17 ++++++ 3 files changed, 94 insertions(+), 4 deletions(-) create mode 100644 rosbag_fancy_msgs/msg/StatusPlay.msg diff --git a/rosbag_fancy/src/cmd_play.cpp b/rosbag_fancy/src/cmd_play.cpp index d3a49e3..5ef660e 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,9 +89,9 @@ int play(const std::vector& options) struct Bag { explicit Bag(const std::string& filename) - : reader{filename} + : reader{filename}, filename_ ( filename ) {} - + std::string filename_; BagReader reader; std::unordered_map publishers; }; @@ -195,6 +199,73 @@ 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&){ + ros::WallTime now = ros::WallTime::now(); + + StatusPlayPtr msg = boost::make_shared(); + msg->header.stamp = ros::Time::now(); + + msg->status = paused ? StatusPlay::STATUS_PAUSED : StatusPlay::STATUS_RUNNING; + + msg->currentTime = currentTime; + msg->startTime = startTime; + msg->endTime = endTime; + msg->current = currentTime-startTime; + msg->duration = endTime-startTime; + + // TODO: fill some of this information + for ( const auto & bag : bags ) + msg->bagfiles.emplace_back(bag.filename_); + +// auto& counts = writer.messageCounts(); + +// for(auto& topic : topicManager.topics()) +// { +// msg->topics.emplace_back(); +// auto& topicMsg = msg->topics.back(); + +// msg->bandwidth += topic.bandwidth; + +// topicMsg.name = topic.name; +// topicMsg.publishers = topic.numPublishers; +// topicMsg.bandwidth = topic.bandwidth; +// topicMsg.bytes = topic.totalBytes; +// topicMsg.messages = topic.totalMessages; + +// if(topic.id < counts.size()) +// topicMsg.messages_in_current_bag = counts[topic.id]; + +// topicMsg.rate = topic.messageRateAt(now); +// } + + pub_status.publish(msg); + })); + while(ros::ok()) { if(paused) @@ -213,7 +284,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); @@ -252,9 +324,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..7c171ca 100644 --- a/rosbag_fancy_msgs/CMakeLists.txt +++ b/rosbag_fancy_msgs/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files(FILES Status.msg TopicStatus.msg + StatusPlay.msg ) generate_messages(DEPENDENCIES diff --git a/rosbag_fancy_msgs/msg/StatusPlay.msg b/rosbag_fancy_msgs/msg/StatusPlay.msg new file mode 100644 index 0000000..42ef9fc --- /dev/null +++ b/rosbag_fancy_msgs/msg/StatusPlay.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 + +TopicStatus[] topics + From f5b0ab9e3511cfa442ff1c3d2dba840a3aaa6eb5 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Mon, 19 Feb 2024 15:43:26 +0100 Subject: [PATCH 2/4] StatusPlay -> PlayStatus, fix compiler warnings & coding style --- rosbag_fancy/src/cmd_play.cpp | 141 +++++++++--------- rosbag_fancy_msgs/CMakeLists.txt | 2 +- .../msg/{StatusPlay.msg => PlayStatus.msg} | 0 3 files changed, 72 insertions(+), 71 deletions(-) rename rosbag_fancy_msgs/msg/{StatusPlay.msg => PlayStatus.msg} (100%) diff --git a/rosbag_fancy/src/cmd_play.cpp b/rosbag_fancy/src/cmd_play.cpp index 5ef660e..9e8056f 100644 --- a/rosbag_fancy/src/cmd_play.cpp +++ b/rosbag_fancy/src/cmd_play.cpp @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include @@ -36,7 +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") + ("no-ui", "Disable terminal UI") ; po::options_description hidden("Hidden"); @@ -89,10 +89,11 @@ int play(const std::vector& options) struct Bag { explicit Bag(const std::string& filename) - : reader{filename}, filename_ ( filename ) + : reader{filename}, filename_ ( filename ) {} - std::string filename_; + BagReader reader; + std::string filename_; std::unordered_map publishers; }; @@ -199,72 +200,72 @@ 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&){ - ros::WallTime now = ros::WallTime::now(); - - StatusPlayPtr msg = boost::make_shared(); - msg->header.stamp = ros::Time::now(); - - msg->status = paused ? StatusPlay::STATUS_PAUSED : StatusPlay::STATUS_RUNNING; - - msg->currentTime = currentTime; - msg->startTime = startTime; - msg->endTime = endTime; - msg->current = currentTime-startTime; - msg->duration = endTime-startTime; - - // TODO: fill some of this information - for ( const auto & bag : bags ) - msg->bagfiles.emplace_back(bag.filename_); - -// auto& counts = writer.messageCounts(); - -// for(auto& topic : topicManager.topics()) -// { -// msg->topics.emplace_back(); -// auto& topicMsg = msg->topics.back(); - -// msg->bandwidth += topic.bandwidth; - -// topicMsg.name = topic.name; -// topicMsg.publishers = topic.numPublishers; -// topicMsg.bandwidth = topic.bandwidth; -// topicMsg.bytes = topic.totalBytes; -// topicMsg.messages = topic.totalMessages; - -// if(topic.id < counts.size()) -// topicMsg.messages_in_current_bag = counts[topic.id]; - -// topicMsg.rate = topic.messageRateAt(now); -// } - - pub_status.publish(msg); - })); + // 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; + + for(const auto & bag : bags) + msg->bagfiles.emplace_back(bag.filename_); + + + // TODO: fill some of this information + +// auto& counts = writer.messageCounts(); + +// for(auto& topic : topicManager.topics()) +// { +// msg->topics.emplace_back(); +// auto& topicMsg = msg->topics.back(); + +// msg->bandwidth += topic.bandwidth; + +// topicMsg.name = topic.name; +// topicMsg.publishers = topic.numPublishers; +// topicMsg.bandwidth = topic.bandwidth; +// topicMsg.bytes = topic.totalBytes; +// topicMsg.messages = topic.totalMessages; + +// if(topic.id < counts.size()) +// topicMsg.messages_in_current_bag = counts[topic.id]; + +// topicMsg.rate = topic.messageRateAt(now); +// } + + pub_status.publish(msg); + })); while(ros::ok()) { diff --git a/rosbag_fancy_msgs/CMakeLists.txt b/rosbag_fancy_msgs/CMakeLists.txt index 7c171ca..be617fc 100644 --- a/rosbag_fancy_msgs/CMakeLists.txt +++ b/rosbag_fancy_msgs/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files(FILES Status.msg TopicStatus.msg - StatusPlay.msg + PlayStatus.msg ) generate_messages(DEPENDENCIES diff --git a/rosbag_fancy_msgs/msg/StatusPlay.msg b/rosbag_fancy_msgs/msg/PlayStatus.msg similarity index 100% rename from rosbag_fancy_msgs/msg/StatusPlay.msg rename to rosbag_fancy_msgs/msg/PlayStatus.msg From ce04faad55b458df4a2dfd2ebb027ab21fe816f7 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Mon, 19 Feb 2024 15:45:57 +0100 Subject: [PATCH 3/4] further coding style fixes --- rosbag_fancy/src/cmd_play.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rosbag_fancy/src/cmd_play.cpp b/rosbag_fancy/src/cmd_play.cpp index 9e8056f..789348a 100644 --- a/rosbag_fancy/src/cmd_play.cpp +++ b/rosbag_fancy/src/cmd_play.cpp @@ -285,8 +285,8 @@ int play(const std::vector& options) ros::SteadyTime::sleepUntil(wallStamp); currentTime = msg.stamp; - if(ui) - ui->setPositionInBag(msg.stamp); + if(ui) + ui->setPositionInBag(msg.stamp); bags[it->bagIndex].publishers[msg.connection->id].publish(msg); @@ -310,9 +310,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); @@ -325,9 +325,9 @@ int play(const std::vector& options) } else if(ret != 0) ui->handleInput(); - ui->setPaused(paused); - } + ui->setPaused(paused); + } } return 0; From 70441ca66af67dfbbd57dee905ab3fd74ea8bc55 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Mon, 19 Feb 2024 15:53:08 +0100 Subject: [PATCH 4/4] cmd_play: basic topic information in status message --- rosbag_fancy/src/cmd_play.cpp | 34 ++++++++--------------- rosbag_fancy_msgs/CMakeLists.txt | 1 + rosbag_fancy_msgs/msg/PlayStatus.msg | 2 +- rosbag_fancy_msgs/msg/PlayTopicStatus.msg | 5 ++++ 4 files changed, 18 insertions(+), 24 deletions(-) create mode 100644 rosbag_fancy_msgs/msg/PlayTopicStatus.msg diff --git a/rosbag_fancy/src/cmd_play.cpp b/rosbag_fancy/src/cmd_play.cpp index 789348a..3c02611 100644 --- a/rosbag_fancy/src/cmd_play.cpp +++ b/rosbag_fancy/src/cmd_play.cpp @@ -237,32 +237,20 @@ int play(const std::vector& options) msg->current = currentTime-startTime; msg->duration = endTime-startTime; - for(const auto & bag : bags) + 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(); - // TODO: fill some of this information - -// auto& counts = writer.messageCounts(); - -// for(auto& topic : topicManager.topics()) -// { -// msg->topics.emplace_back(); -// auto& topicMsg = msg->topics.back(); - -// msg->bandwidth += topic.bandwidth; - -// topicMsg.name = topic.name; -// topicMsg.publishers = topic.numPublishers; -// topicMsg.bandwidth = topic.bandwidth; -// topicMsg.bytes = topic.totalBytes; -// topicMsg.messages = topic.totalMessages; - -// if(topic.id < counts.size()) -// topicMsg.messages_in_current_bag = counts[topic.id]; - -// topicMsg.rate = topic.messageRateAt(now); -// } + topicMsg.name = topic.name; + topicMsg.bandwidth = topic.bandwidth; + topicMsg.rate = topic.messageRate; + } pub_status.publish(msg); })); diff --git a/rosbag_fancy_msgs/CMakeLists.txt b/rosbag_fancy_msgs/CMakeLists.txt index be617fc..de87545 100644 --- a/rosbag_fancy_msgs/CMakeLists.txt +++ b/rosbag_fancy_msgs/CMakeLists.txt @@ -11,6 +11,7 @@ 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 index 42ef9fc..b44f5a4 100644 --- a/rosbag_fancy_msgs/msg/PlayStatus.msg +++ b/rosbag_fancy_msgs/msg/PlayStatus.msg @@ -13,5 +13,5 @@ time startTime time endTime string[] bagfiles -TopicStatus[] topics +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