rosmon_core: Adding verbosity filter feature

rosmon_core/src/launch/launch_config.cpp

@@ -483,18 +483,18 @@ void LaunchConfig::parseNode(TiXmlElement* element, ParseContext& attr_ctx)
 
 	if(m_outputAttrMode == OutputAttr::Obey)
 	{
-		node->setMuted(true); // output=log is default
+		node->setStdoutDisplayed(false); // output=log is default
+	}
 
-		if(output)
-		{
-			std::string outputStr = attr_ctx.evaluate(output);
-			if(outputStr == "screen")
-				node->setMuted(false);
-			else if(outputStr == "log")
-				node->setMuted(true);
-			else
-				throw ctx.error("Invalid output attribute value: '{}'", outputStr);
-		}
+	if(output)
+	{
+		std::string outputStr = attr_ctx.evaluate(output);
+		if(outputStr == "screen")
+			node->setStdoutDisplayed(true);
+		else if(outputStr == "log")
+			node->setStdoutDisplayed(false);
+		else
+			throw ctx.error("Invalid output attribute value: '{}'", outputStr);
 	}
 
 	node->setRemappings(ctx.remappings());

rosmon_core/src/launch/node.cpp

@@ -42,6 +42,7 @@ Node::Node(std::string name, std::string package, std::string type)
  , m_memoryLimitByte(15e6)
  , m_cpuLimit(0.05)
  , m_muted(false)
+ , m_stdoutDisplayed(true)
 {
 	m_executable = PackageRegistry::getExecutable(m_package, m_type);
 }
@@ -161,6 +162,11 @@ void Node::setMuted(bool muted)
 	m_muted = muted;
 }
 
+void Node::setStdoutDisplayed(bool displayed)
+{
+	m_stdoutDisplayed = displayed;
+}
+
 }
 
 }

rosmon_core/src/launch/node.h

@@ -47,6 +47,7 @@ class Node
 	void setCPULimit(float cpuLimit);
 
 	void setMuted(bool muted);
+	void setStdoutDisplayed(bool showStdout);
 
 	std::string name() const
 	{ return m_name; }
@@ -106,6 +107,9 @@ class Node
 
 	bool isMuted() const
 	{ return m_muted; }
+
+	bool stdoutDisplayed() const
+	{ return m_stdoutDisplayed; }
 private:
 	std::string m_name;
 	std::string m_package;
@@ -139,6 +143,7 @@ class Node
 	float m_cpuLimit;
 
 	bool m_muted;
+	bool m_stdoutDisplayed;
 };
 
 }

rosmon_core/src/log_event.h

@@ -24,6 +24,13 @@ struct LogEvent
 		Error
 	};
 
+	enum class Channel
+	{
+		NotApplicable,
+		Stdout,
+		Stderr
+	};
+
 	LogEvent(std::string source, std::string message, Type type = Type::Raw)
 	 : source{std::move(source)}, message{std::move(message)}, type{type}
 	{}
@@ -32,6 +39,8 @@ struct LogEvent
 	std::string message;
 	Type type;
 	bool muted = false;
+	Channel channel = Channel::NotApplicable;
+	bool showStdout = true;
 };
 
 }

rosmon_core/src/monitor/node_monitor.cpp

@@ -94,6 +94,7 @@ NodeMonitor::NodeMonitor(launch::Node::ConstPtr launchNode, FDWatcher::Ptr fdWat
  : m_launchNode(std::move(launchNode))
  , m_fdWatcher(std::move(fdWatcher))
  , m_rxBuffer(4096)
+ , m_stderrBuffer(4096)
  , m_exitCode(0)
  , m_command(CMD_STOP) // we start in stopped state
  , m_restarting(false)
@@ -215,6 +216,11 @@ void NodeMonitor::start()
 	if(openpty(&master, &slave, nullptr, nullptr, nullptr) == -1)
 		throw error("Could not open pseudo terminal for child process: {}", strerror(errno));
 
+	// For stderr, we open a separate pipe
+	int stderr_pipe[2];
+	if(pipe(stderr_pipe) != 0)
+		throw error("Could not create stderr pipe: {}", strerror(errno));
+
 	// Compose args
 	{
 		args.push_back(strdup("rosrun"));
@@ -224,6 +230,9 @@ void NodeMonitor::start()
 		args.push_back(strdup("--tty"));
 		args.push_back(strdup(fmt::format("{}", slave).c_str()));
 
+		args.push_back(strdup("--stderr"));
+		args.push_back(strdup(fmt::format("{}", stderr_pipe[1]).c_str()));
+
 		if(!m_launchNode->namespaceString().empty())
 		{
 			args.push_back(strdup("--namespace"));
@@ -263,6 +272,7 @@ void NodeMonitor::start()
 	if(pid == 0)
 	{
 		close(master);
+		close(stderr_pipe[0]);
 
 		if(execvp("rosrun", args.data()) != 0)
 		{
@@ -279,10 +289,13 @@ void NodeMonitor::start()
 
 	// Parent
 	close(slave);
+	close(stderr_pipe[1]);
 
 	m_fd = master;
+	m_stderrFD = stderr_pipe[0];
 	m_pid = pid;
 	m_fdWatcher->registerFD(m_fd, boost::bind(&NodeMonitor::communicate, this));
+	m_fdWatcher->registerFD(m_stderrFD, boost::bind(&NodeMonitor::communicateStderr, this));
 }
 
 void NodeMonitor::stop(bool restart)
@@ -363,6 +376,49 @@ NodeMonitor::State NodeMonitor::state() const
 	return STATE_CRASHED;
 }
 
+void NodeMonitor::communicateStderr()
+{
+	auto handleByte = [&](char c){
+		m_stderrBuffer.push_back(c);
+		if(c == '\n')
+		{
+			m_stderrBuffer.push_back(0);
+			m_stderrBuffer.linearize();
+
+			auto one = m_stderrBuffer.array_one();
+
+			LogEvent event{name(), one.first};
+			event.muted = isMuted();
+			event.channel = LogEvent::Channel::Stderr;
+
+			logMessageSignal(std::move(event));
+
+			m_stderrBuffer.clear();
+		}
+	};
+
+	char buf[1024];
+	int bytes = read(m_stderrFD, buf, sizeof(buf));
+
+	if(bytes == 0)
+	{
+		// Flush out any remaining stdout
+		if(!m_stderrBuffer.empty())
+			handleByte('\n');
+
+		m_fdWatcher->removeFD(m_stderrFD);
+		return; // handled in communicate()
+	}
+
+	if(bytes < 0)
+		throw error("{}: Could not read: {}", name(), strerror(errno));
+
+	for(int i = 0; i < bytes; ++i)
+	{
+		handleByte(buf[i]);
+	}
+}
+
 void NodeMonitor::communicate()
 {
 	auto handleByte = [&](char c){
@@ -376,6 +432,8 @@ void NodeMonitor::communicate()
 
 			LogEvent event{name(), one.first};
 			event.muted = isMuted();
+			event.channel = LogEvent::Channel::Stdout;
+			event.showStdout = m_launchNode->stdoutDisplayed();
 
 			logMessageSignal(std::move(event));
 

rosmon_core/src/monitor/node_monitor.h

@@ -198,6 +198,7 @@ class NodeMonitor
 	std::vector<std::string> composeCommand() const;
 
 	void communicate();
+	void communicateStderr();
 
 	template<typename... Args>
 	void log(const char* format, Args&& ... args);
@@ -213,9 +214,11 @@ class NodeMonitor
 	FDWatcher::Ptr m_fdWatcher;
 
 	boost::circular_buffer<char> m_rxBuffer;
+	boost::circular_buffer<char> m_stderrBuffer;
 
 	int m_pid = -1;
 	int m_fd = -1;
+	int m_stderrFD = -1;
 	int m_exitCode;
 
 	ros::WallTimer m_stopCheckTimer;

rosmon_core/src/monitor/shim.cpp

@@ -23,6 +23,7 @@ static const struct option OPTIONS[] = {
 	{"coredump-relative", required_argument, nullptr, 'C'},
 	{"tty", required_argument, nullptr, 't'},
 	{"run", required_argument, nullptr, 'r'},
+	{"stderr", required_argument, nullptr, 's'},
 
 	{nullptr, 0, nullptr, 0}
 };
@@ -41,6 +42,8 @@ This is an internal tool for rosmon. You should not need to call it yourself.
   --coredump               Enable coredump collection
   --coredump-relative=DIR  Coredumps should go to DIR
   --run <executable>       All arguments after this one are passed on
+  --tty TTY                TTY fd for stdout
+  --stderr FD              file descriptor for stderr
 )EOS");
 }
 
@@ -53,6 +56,7 @@ int main(int argc, char** argv)
 	int nodeOptionsBegin = -1;
 
 	int tty = -1;
+	int stderr_fd = -1;
 
 	while(true)
 	{
@@ -97,6 +101,9 @@ int main(int argc, char** argv)
 				nodeExecutable = optarg;
 				nodeOptionsBegin = optind;
 				break;
+			case 's':
+				stderr_fd = atoi(optarg);
+				break;
 		}
 
 		if(nodeExecutable)
@@ -109,12 +116,21 @@ int main(int argc, char** argv)
 	if(tty < 0)
 		throw std::invalid_argument("Need --tty option");
 
+	if(stderr_fd < 0)
+		throw std::invalid_argument("Need --stderr option");
+
 	if(login_tty(tty) != 0)
 	{
 		perror("Could not call login_tty()");
 		std::abort();
 	}
 
+	if(dup2(stderr_fd, STDERR_FILENO) < 0)
+	{
+		perror("Could not call dup2() for stderr");
+		std::abort();
+	}
+
 	// Try to enable core dumps
 	if(coredumpsEnabled)
 	{

rosmon_core/src/ui.cpp

@@ -209,10 +209,20 @@ void UI::drawStatusLine()
 		else
 		{
 			printKey("A-Z", "Node actions");
+			printKey("F8", "Toggle WARN+ only");
 			printKey("F9", "Mute all");
 			printKey("F10", "Unmute all");
 			printKey("/", "Node search");
 
+			if(stderrOnly())
+			{
+				print("      ");
+				m_term.setSimpleForeground(Terminal::Black);
+				m_term.setSimpleBackground(Terminal::Magenta);
+				print("! WARN+ output only !");
+				m_style_bar.use();
+			}
+
 			if(anyMuted())
 			{
 				print("      ");
@@ -397,6 +407,10 @@ void UI::log(const LogEvent& event)
 	if(event.muted)
 		return;
 
+	// Are we supposed to show stdout?
+	if(event.channel == LogEvent::Channel::Stdout && (!event.showStdout || stderrOnly()))
+		return;
+
 	const std::string& clean = event.message;
 
 	auto it = m_nodeColorMap.find(event.source);
@@ -649,6 +663,13 @@ void UI::handleKey(int c)
 			return;
 		}
 
+		// Check for Stderr Only Toggle
+		if(c == Terminal::SK_F8)
+		{
+			toggleStderrOnly();
+			return;
+		}
+
 		// Search
 		if(c == '/')
 		{
@@ -723,4 +744,14 @@ void UI::scheduleUpdate()
 	m_refresh_required = true;
 }
 
+bool UI::stderrOnly()
+{
+	return m_stderr_only;
+}
+
+void UI::toggleStderrOnly()
+{
+	m_stderr_only = !m_stderr_only;
+}
+
 }

rosmon_core/src/ui.h

@@ -59,11 +59,16 @@ class UI
 
 	void unmuteAll();
 
+	bool stderrOnly();
+
+	void toggleStderrOnly();
+
 	void scheduleUpdate();
 
 	monitor::Monitor* m_monitor;
 	FDWatcher::Ptr m_fdWatcher;
 	bool m_refresh_required = true;
+	bool m_stderr_only = false;
 
 	Terminal m_term;