Add ROS Service logic to start/stop with regex

rosmon_core/src/ros_interface.cpp

@@ -6,6 +6,7 @@
 #include <rosmon_msgs/State.h>
 
 #include <algorithm>
+#include <regex>
 
 namespace rosmon
 {
@@ -74,25 +75,82 @@ void ROSInterface::update()
 
 bool ROSInterface::handleStartStop(rosmon_msgs::StartStopRequest& req, rosmon_msgs::StartStopResponse&)
 {
-	auto it = std::find_if(
-		m_monitor->nodes().begin(), m_monitor->nodes().end(),
-		[&](const monitor::NodeMonitor::ConstPtr& n){ return (n->name() == req.node) && (n->namespaceString() == req.ns); }
-	);
-
-	if(it == m_monitor->nodes().end())
-		return false;
-
-	switch(req.action)
-	{
+	const auto start_stop = [&](monitor::NodeMonitor& node) {
+		switch (req.action) {
 		case rosmon_msgs::StartStopRequest::START:
-			(*it)->start();
+			node.start();
 			break;
 		case rosmon_msgs::StartStopRequest::STOP:
-			(*it)->stop();
+			node.stop();
 			break;
 		case rosmon_msgs::StartStopRequest::RESTART:
-			(*it)->restart();
+			node.restart();
 			break;
+		}
+	};
+
+	// remove all slashes from start and end of string
+	const auto trim = [](std::string& str) {
+		size_t start = str.find_first_not_of('/');
+		size_t end = str.find_last_not_of('/');
+		if(start == std::string::npos || end == std::string::npos || start > end)
+		{
+			str = "";
+			return;
+		}
+		str = str.substr(start, end - start + 1);
+	};
+
+	trim(req.ns);
+	trim(req.node);
+
+	if(req.node.empty())
+	{
+		ROS_ERROR("Got a StartStopRequest with empty node field");
+		return false;
+	}
+
+	std::string strPattern;
+
+	if(!req.ns.empty())
+	{
+		if(req.ns[0] != '/')
+			strPattern += "/";
+
+		strPattern += req.ns;
+	}
+
+	if(req.node[0] != '/')
+		strPattern += "/";
+
+	strPattern += req.node;
+
+	std::regex reg;
+	try
+	{
+		reg = std::regex(strPattern);
+	}
+	catch(const std::regex_error& e)
+	{
+		ROS_ERROR("Invalid regular expression: %s", e.what());
+		return false;
+	}
+
+	bool found = false;
+	for(auto& node : m_monitor->nodes())
+	{
+		const std::string str_name = node->namespaceString() + "/" + node->name();
+		if(std::regex_match(str_name, reg))
+		{
+			found = true;
+			start_stop(*node);
+		}
+	}
+
+	if(!found)
+	{
+		ROS_ERROR("No node matching '%s' found", strPattern.c_str());
+		return false;
 	}
 
 	return true;

rosmon_core/test/basic.py

@@ -18,6 +18,7 @@
 from std_msgs.msg import String
 
 from rosmon_msgs.msg import State, NodeState
+from rosmon_msgs.srv import StartStop, StartStopRequest
 
 rospack = rospkg.RosPack()
 
@@ -135,6 +136,65 @@ def test_global_remapping(self):
 	def test_nested(self):
 		self.assertEqual(rospy.get_param("/nested/nested_param"), "hello")
 
+	def getNode(self, state, name):
+		lst = list(filter(lambda n: n.name == name, state.nodes))
+		self.assertEqual(len(lst), 1)
+		return lst[0]
+
+	def test_startstop_one(self):
+		srv = rospy.ServiceProxy('/rosmon_uut/start_stop', StartStop)
+
+		srv(node=r'test1', action=StartStopRequest.STOP)
+
+		time.sleep(2)
+
+		try:
+			state = rospy.client.wait_for_message('/rosmon_uut/state', State, timeout=5.0)
+		except rospy.ROSException:
+			self.fail('Did not get state msg on /rosmon_uut/state' + repr(rospy.client.get_published_topics()))
+
+		self.assertEqual(self.getNode(state, 'test1').state, NodeState.IDLE)
+		self.assertEqual(self.getNode(state, 'test2').state, NodeState.RUNNING)
+
+		srv(node=r'test1', action=StartStopRequest.START)
+
+		time.sleep(2)
+
+		try:
+			state = rospy.client.wait_for_message('/rosmon_uut/state', State, timeout=5.0)
+		except rospy.ROSException:
+			self.fail('Did not get state msg on /rosmon_uut/state' + repr(rospy.client.get_published_topics()))
+
+		self.assertEqual(self.getNode(state, 'test1').state, NodeState.RUNNING)
+		self.assertEqual(self.getNode(state, 'test2').state, NodeState.RUNNING)
+
+	def test_startstop_regex(self):
+		srv = rospy.ServiceProxy('/rosmon_uut/start_stop', StartStop)
+
+		srv(node=r'test\d+', action=StartStopRequest.STOP)
+
+		time.sleep(2)
+
+		try:
+			state = rospy.client.wait_for_message('/rosmon_uut/state', State, timeout=5.0)
+		except rospy.ROSException:
+			self.fail('Did not get state msg on /rosmon_uut/state' + repr(rospy.client.get_published_topics()))
+
+		self.assertEqual(self.getNode(state, 'test1').state, NodeState.IDLE)
+		self.assertEqual(self.getNode(state, 'test2').state, NodeState.IDLE)
+
+		srv(node=r'test\d+', action=StartStopRequest.START)
+
+		time.sleep(2)
+
+		try:
+			state = rospy.client.wait_for_message('/rosmon_uut/state', State, timeout=5.0)
+		except rospy.ROSException:
+			self.fail('Did not get state msg on /rosmon_uut/state' + repr(rospy.client.get_published_topics()))
+
+		self.assertEqual(self.getNode(state, 'test1').state, NodeState.RUNNING)
+		self.assertEqual(self.getNode(state, 'test2').state, NodeState.RUNNING)
+
 if __name__ == '__main__':
 	rospy.init_node('basic_test')
 

rosmon_msgs/srv/StartStop.srv

@@ -2,6 +2,13 @@ uint8 START = 1
 uint8 STOP = 2
 uint8 RESTART = 3
 
+# The "ns" field is provided for compatibility. To figure out the full path,
+# "ns" and "node" are joined together. So you can provide a full path
+# including namespaces in "node".
+
+# Supplying a regex in "node" is also supported. For example, setting
+# node=".*" will influence all nodes.
+
 string node     # ROS node name
 string ns       # ROS node namespace
 uint8 action