added no-gui for play with status info and start/stop/pause services.

xqms · Feb 19, 2024 · cabfb4b · cabfb4b
1 parent 342c002
commit cabfb4b
Show file tree

Hide file tree

Showing 3 changed files with 94 additions and 4 deletions.
diff --git a/rosbag_fancy/src/cmd_play.cpp b/rosbag_fancy/src/cmd_play.cpp
@@ -11,6 +11,8 @@
 
 #include <ros/node_handle.h>
 #include <ros/publisher.h>
+#include <rosbag_fancy_msgs/StatusPlay.h>
+#include <std_srvs/Trigger.h>
 
 #include <rosgraph_msgs/Clock.h>
 
@@ -22,6 +24,7 @@
 
 namespace po = boost::program_options;
 using namespace rosbag_fancy;
+using namespace rosbag_fancy_msgs;
 
 int play(const std::vector<std::string>& options)
 {
@@ -33,6 +36,7 @@ int play(const std::vector<std::string>& 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<std::string>& options)
 	struct Bag
 	{
 		explicit Bag(const std::string& filename)
-		 : reader{filename}
+                 : reader{filename}, filename_ ( filename )
 		{}
-
+                std::string filename_;
 		BagReader reader;
 		std::unordered_map<int, ros::Publisher> publishers;
 	};
@@ -195,6 +199,73 @@ int play(const std::vector<std::string>& options)
 		});
 	}
 
+        // Start/Stop service calls
+        ros::ServiceServer srv_start = nh.advertiseService("start", boost::function<bool(std_srvs::TriggerRequest&, std_srvs::TriggerResponse&)>([&](auto&, auto& resp){
+                paused = false;
+                resp.success = !paused;
+                if ( ui )
+                    ui->setPaused(paused);
+                return true;
+        }));
+        ros::ServiceServer srv_pause = nh.advertiseService("pause", boost::function<bool(std_srvs::TriggerRequest&, std_srvs::TriggerResponse&)>([&](auto&, auto& resp){
+                paused = true;
+                resp.success = paused;
+                if ( ui )
+                    ui->setPaused(paused);
+                return true;
+        }));
+        ros::ServiceServer srv_toggle = nh.advertiseService("toggle", boost::function<bool(std_srvs::TriggerRequest&, std_srvs::TriggerResponse&)>([&](auto&, auto& resp){
+                paused = !paused;
+                if ( ui )
+                    ui->setPaused(paused);
+                resp.success = true;
+                return true;
+        }));
+
+        // Status publisher
+        ros::Publisher pub_status = nh.advertise<StatusPlay>("status", 1);
+        ros::SteadyTimer timer_status = nh.createSteadyTimer(ros::WallDuration(0.1), boost::function<void(const ros::SteadyTimerEvent&)>([&](auto&){
+                ros::WallTime now = ros::WallTime::now();
+
+                StatusPlayPtr msg = boost::make_shared<StatusPlay>();
+                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<std::string>& 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<std::string>& 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
@@ -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
@@ -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
+