Add ROS service for executing shell commands (#210)

* Add ROS service for executing shell commands

* Show image version on index web page

* Add test for exec service

* Add shell node to clover.launch

* Remake exec handling, consider exit code and exec failures

clover/CMakeLists.txt

@@ -81,6 +81,7 @@ add_service_files(
   SetAttitude.srv
   SetRates.srv
   SetLEDEffect.srv
+  Execute.srv
 )
 
 ## Generate actions in the 'action' folder
@@ -167,6 +168,8 @@ add_executable(vpe_publisher src/vpe_publisher.cpp)
 
 add_executable(led src/led.cpp)
 
+add_executable(shell src/shell.cpp)
+
 target_link_libraries(simple_offboard
   ${catkin_LIBRARIES}
   ${GeographicLib_LIBRARIES}
@@ -180,10 +183,14 @@ target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
 
 target_link_libraries(led ${catkin_LIBRARIES})
 
+target_link_libraries(shell ${catkin_LIBRARIES})
+
 add_dependencies(simple_offboard ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(led ${PROJECT_NAME}_generate_messages_cpp)
 
+add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use

clover/launch/clover.launch

@@ -10,6 +10,7 @@
     <arg name="rangefinder_vl53l1x" default="true"/>
     <arg name="led" default="true"/>
     <arg name="rc" default="true"/>
+    <arg name="shell" default="true"/>
 
     <!-- log formatting -->
     <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
@@ -76,4 +77,7 @@
         <!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
         <param name="use_fake_gcs" value="false"/>
     </node>
+
+    <!-- Shell access through ROS service -->
+    <node name="shell" pkg="clover" type="shell" output="screen" if="$(arg shell)"/>
 </launch>

clover/src/shell.cpp

@@ -0,0 +1,50 @@
+#include <ros/ros.h>
+#include <cstdio>
+#include <iostream>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <array>
+#include <std_msgs/String.h>
+
+#include <clover/Execute.h>
+
+ros::Duration timeout;
+
+// TODO: handle timeout
+bool handle(clover::Execute::Request& req, clover::Execute::Response& res)
+{
+	ROS_INFO("Execute: %s", req.cmd.c_str());
+
+	std::array<char, 128> buffer;
+	std::string result;
+
+	FILE *fp = popen(req.cmd.c_str(), "r");
+
+	if (fp == NULL) {
+		res.code = clover::Execute::Request::CODE_FAIL;
+		res.output = "popen() failed";
+		return true;
+	}
+
+	while (fgets(buffer.data(), buffer.size(), fp) != nullptr) {
+		res.output += buffer.data();
+	}
+
+	res.code = pclose(fp);
+	return true;
+}
+
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "shell");
+	ros::NodeHandle nh, nh_priv("~");
+
+	timeout = ros::Duration(nh_priv.param("timeout", 3.0));
+
+	auto gt_serv = nh.advertiseService("exec", &handle);
+
+	ROS_INFO("shell: ready");
+	ros::spin();
+}

clover/srv/Execute.srv

@@ -0,0 +1,7 @@
+int32 CODE_FAIL = -1
+int32 CODE_TIMEOUT = -2
+
+string cmd
+---
+string output
+int32 code

clover/test/basic.py

@@ -2,6 +2,7 @@
 import rospy
 import pytest
 from mavros_msgs.msg import State
+from clover import srv
 
 @pytest.fixture()
 def node():
@@ -27,3 +28,19 @@ def test_simple_offboard_services_available():
 def test_web_video_server(node):
     import urllib2
     urllib2.urlopen("http://localhost:8080").read()
+
+def test_shell(node):
+    execute = rospy.ServiceProxy('exec', srv.Execute)
+    execute.wait_for_service(5)
+
+    res = execute(cmd='echo foo')
+    assert res.code == 0
+    assert res.output == 'foo\n'
+
+    res = execute(cmd='foo')
+    assert res.code == 32512
+    assert res.output == ''
+
+    res = execute(cmd='ls foo')
+    assert res.code == 512
+    assert res.output == ''

clover/test/basic.test

@@ -32,6 +32,8 @@
 
     <node name="rc" pkg="clover" type="rc" required="true" output="screen"/>
 
+    <node name="shell" pkg="clover" type="shell" required="true" output="screen"/>
+
     <node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" required="true">
         <rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
     </node>

clover/www/index.html

@@ -8,7 +8,17 @@
 	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 </ul>
 
+<div class="version"></div>
+
+<script src="js/roslib.js"></script>
 <script type="text/javascript">
 	document.querySelector("#wvs").href = location.origin + ':8080';
 	document.querySelector("#butterfly").href = location.origin + ':57575';
+
+	// Determine image version
+	var ros = new ROSLIB.Ros({ url: 'ws://' + location.hostname + ':9090' });
+	var exec = new ROSLIB.Service({ ros: ros, name : '/exec', serviceType : 'clover/Execute' });
+	exec.callService(new ROSLIB.ServiceRequest({ cmd: 'cat /etc/clover_version' }), function(result) {
+		document.querySelector('.version').innerHTML = 'Version: ' + result.output;
+	});
 </script>