From e1928c711923e45e7fe65d79936bac793102df22 Mon Sep 17 00:00:00 2001 From: Max Schwarz Date: Mon, 19 Feb 2024 15:43:26 +0100 Subject: [PATCH] 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