posix shell: allow to run multiple instances

Tools/sitl_multiple_run.sh

@@ -43,7 +43,7 @@ while [ $n -le $sitl_num ]; do
 
 	pushd "$working_dir" &>/dev/null
 	echo "starting instance $n in $(pwd)"
-	sudo -b -u $user ../bin/px4 -d "$src_path" rcS >out.log 2>err.log
+	sudo -b -u $user ../bin/px4 -i $n -d "$src_path" -s rcS >out.log 2>err.log
 	popd &>/dev/null
 
 	n=$(($n + 1))

cmake/posix/px4-alias.sh_in

@@ -18,4 +18,9 @@ set() {
 # Don't stop on errors.
 #set -e
 
+# Arguments passed to this script:
+# $1: optional instance id
+px4_instance=0
+[[ -n "$1" ]] && px4_instance=$1
+
 ${alias_string}

launch/single_vehicle_spawn.launch

@@ -23,7 +23,7 @@
     <!-- PX4 SITL -->
     <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
     <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
-    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4) $(arg rcS) $(arg px4_command_arg1)">
+    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4) -s $(arg rcS) -i $(arg ID) $(arg px4_command_arg1)">
     </node>
     <!-- spawn vehicle -->
     <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model" args="-urdf -param rotors_description -model $(arg vehicle)_$(arg ID) -package_to_model -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>

platforms/posix/cmake/px4_impl_os.cmake

@@ -123,7 +123,7 @@ function(px4_posix_generate_alias)
 		endforeach()
 		if (MAIN)
 			set(alias_string
-				"${alias_string}alias ${MAIN}='${PREFIX}${MAIN}'\n"
+				"${alias_string}alias ${MAIN}='${PREFIX}${MAIN} --instance $px4_instance'\n"
 			)
 		endif()
 	endforeach()

platforms/posix/src/main.cpp

@@ -99,10 +99,11 @@ static void register_sig_handler();
 static void set_cpu_scaling();
 static int create_symlinks_if_needed(std::string &data_path);
 static int create_dirs();
-static int run_startup_bash_script(const std::string &commands_file, const std::string &absolute_binary_path);
+static int run_startup_bash_script(const std::string &commands_file, const std::string &absolute_binary_path,
+				   int instance);
 static std::string get_absolute_binary_path(const std::string &argv0);
 static void wait_to_exit();
-static bool is_already_running();
+static bool is_already_running(int instance);
 static void print_usage();
 static bool dir_exists(const std::string &path);
 static bool file_exists(const std::string &name);
@@ -141,9 +142,23 @@ int main(int argc, char **argv)
 		absolute_binary_path = get_absolute_binary_path(full_binary_name);
 	}
 
-	if (is_client) {
 
-		if (!is_already_running()) {
+	if (is_client) {
+		int instance = 0;
+
+		if (argc >= 3 && strcmp(argv[1], "--instance") == 0) {
+			instance = strtoul(argv[2], nullptr, 10);
+			/* update argv so that "--instance <instance>" is not visible anymore */
+			argc -= 2;
+
+			for (int i = 1; i < argc; ++i) {
+				argv[i] = argv[i + 2];
+			}
+		}
+
+		PX4_DEBUG("instance: %i", instance);
+
+		if (!is_already_running(instance)) {
 			PX4_ERR("PX4 daemon not running yet");
 			return -1;
 		}
@@ -151,28 +166,24 @@ int main(int argc, char **argv)
 		/* Remove the path and prefix. */
 		argv[0] += path_length + strlen(prefix);
 
-		px4_daemon::Client client;
+		px4_daemon::Client client(instance);
 		client.generate_uuid();
 		client.register_sig_handler();
 		return client.process_args(argc, (const char **)argv);
 
 	} else {
-		if (is_already_running()) {
-			PX4_ERR("PX4 daemon already running");
-			return -1;
-		}
-
 		/* Server/daemon apps need to parse the command line arguments. */
 
 		std::string data_path = "";
 		std::string commands_file = "etc/init.d/rcS";
 		std::string test_data_path = "";
+		int instance = 0;
 
 		int myoptind = 1;
 		int ch;
 		const char *myoptarg = nullptr;
 
-		while ((ch = px4_getopt(argc, argv, "hdt:s:", &myoptind, &myoptarg)) != EOF) {
+		while ((ch = px4_getopt(argc, argv, "hdt:s:i:", &myoptind, &myoptarg)) != EOF) {
 			switch (ch) {
 			case 'h':
 				print_usage();
@@ -190,6 +201,10 @@ int main(int argc, char **argv)
 				commands_file = myoptarg;
 				break;
 
+			case 'i':
+				instance = strtoul(myoptarg, nullptr, 10);
+				break;
+
 			default:
 				PX4_ERR("unrecognized flag");
 				print_usage();
@@ -197,10 +212,19 @@ int main(int argc, char **argv)
 			}
 		}
 
+		PX4_DEBUG("instance: %i", instance);
+
 		if (myoptind < argc) {
 			data_path = argv[myoptind];
 		}
 
+		if (is_already_running(instance)) {
+			// allow running multiple instances, but the server is only started for the first
+			PX4_INFO("PX4 daemon already running for instance %i", instance);
+			return -1;
+		}
+
+
 		int ret = create_symlinks_if_needed(data_path);
 
 		if (ret != PX4_OK) {
@@ -223,7 +247,7 @@ int main(int argc, char **argv)
 		register_sig_handler();
 		set_cpu_scaling();
 
-		px4_daemon::Server server;
+		px4_daemon::Server server(instance);
 		server.start();
 
 		ret = create_dirs();
@@ -237,7 +261,7 @@ int main(int argc, char **argv)
 		px4::init_once();
 		px4::init(argc, argv, "px4");
 
-		ret = run_startup_bash_script(commands_file, absolute_binary_path);
+		ret = run_startup_bash_script(commands_file, absolute_binary_path, instance);
 
 		// We now block here until we need to exit.
 		if (pxh_off) {
@@ -432,11 +456,12 @@ std::string get_absolute_binary_path(const std::string &argv0)
 	return pwd() + "/" + base;
 }
 
-int run_startup_bash_script(const std::string &commands_file, const std::string &absolute_binary_path)
+int run_startup_bash_script(const std::string &commands_file, const std::string &absolute_binary_path,
+			    int instance)
 {
 	std::string bash_command("bash ");
 
-	bash_command += commands_file;
+	bash_command += commands_file + ' ' + std::to_string(instance);
 
 	// Update the PATH variable to include the absolute_binary_path
 	// (required for the px4-alias.sh script and px4-* commands).
@@ -506,24 +531,26 @@ void print_usage()
 {
 	printf("Usage for Server/daemon process: \n");
 	printf("\n");
-	printf("    px4 [-h|-d] [-s <startup_file>] [-d <test_data_directory>] [<rootfs_directory>]\n");
+	printf("    px4 [-h|-d] [-s <startup_file>] [-t <test_data_directory>] [<rootfs_directory>] [-i <instance>]\n");
 	printf("\n");
-	printf("    <startup_file>     bash start script to be used as startup (default=etc/init.d/rcS)\n");
+	printf("    -s <startup_file>  bash start script to be used as startup (default=etc/init.d/rcS)\n");
 	printf("    <rootfs_directory> directory where startup files and mixers are located,\n");
 	printf("                       (if not given, CWD is used)\n");
-	printf("        -h             help/usage information\n");
+	printf("    -i <instance>      px4 instance id to run multiple instances [0...N], default=0\n");
-	printf("        -d             daemon mode, don't start pxh shell\n");
+	printf("    -h                 help/usage information\n");
+	printf("    -d                 daemon mode, don't start pxh shell\n");
 	printf("\n");
 	printf("Usage for client: \n");
 	printf("\n");
-	printf("    px4-MODULE command using symlink.\n");
+	printf("    px4-MODULE [--instance <instance>] command using symlink.\n");
 	printf("        e.g.: px4-commander status\n");
 }
 
-bool is_already_running()
+bool is_already_running(int instance)
 {
+	const std::string file_lock_path = std::string(LOCK_FILE_PATH) + '-' + std::to_string(instance);
 	struct flock fl;
-	int fd = open(LOCK_FILE_PATH, O_RDWR | O_CREAT, 0666);
+	int fd = open(file_lock_path.c_str(), O_RDWR | O_CREAT, 0666);
 
 	if (fd < 0) {
 		return false;

platforms/posix/src/px4_daemon/client.cpp

@@ -59,9 +59,10 @@ namespace client
 static Client *_instance;
 }
 
-Client::Client() :
+Client::Client(int instance_id) :
 	_uuid(0),
-	_client_send_pipe_fd(-1)
+	_client_send_pipe_fd(-1),
+	_instance_id(instance_id)
 {
 	client::_instance = this;
 }
@@ -166,7 +167,7 @@ Client::_send_cmds(const int argc, const char **argv)
 	// The size is +1 because we want to include the null termination.
 	packet.header.payload_length = cmd_buf.size() + 1;
 
-	_client_send_pipe_fd = open(CLIENT_SEND_PIPE_PATH, O_WRONLY);
+	_client_send_pipe_fd = open(get_client_send_pipe_path(_instance_id).c_str(), O_WRONLY);
 
 	if (_client_send_pipe_fd < 0) {
 		PX4_ERR("pipe open fail");

platforms/posix/src/px4_daemon/client.h

@@ -56,7 +56,7 @@ namespace px4_daemon
 class Client
 {
 public:
-	Client();
+	Client(int instance_id = 0);
 	~Client();
 
 	/**
@@ -97,6 +97,7 @@ private:
 	uint64_t _uuid;
 	int _client_send_pipe_fd;
 	char _recv_pipe_path[RECV_PIPE_PATH_LEN];
+	int _instance_id; ///< instance ID for running multiple instances of the px4 server
 };
 
 } // namespace px4_daemon

platforms/posix/src/px4_daemon/pipe_protocol.cpp

@@ -43,6 +43,10 @@
 
 #include "pipe_protocol.h"
 
+static const char CLIENT_SEND_PIPE_PATH[] = "/tmp/px4_client_send_pipe-";
+static const char CLIENT_RECV_PIPE_PATH[] = "/tmp/px4_client_recv_pipe";
+
+
 namespace px4_daemon
 {
 
@@ -59,9 +63,13 @@ unsigned get_client_recv_packet_length(const client_recv_packet_s *packet)
 
 int get_client_recv_pipe_path(const uint64_t uuid, char *path, const size_t path_len)
 {
-	return snprintf(path, path_len, "%s_%016" PRIx64, CLIENT_RECV_PIPE_PATH, uuid);
+	return snprintf(path, path_len, "%s-%016" PRIx64, CLIENT_RECV_PIPE_PATH, uuid);
 }
 
+std::string get_client_send_pipe_path(int instance_id)
+{
+	return std::string(CLIENT_SEND_PIPE_PATH) + std::to_string(instance_id);
+}
 
 } // namespace px4_daemon
 

platforms/posix/src/px4_daemon/pipe_protocol.h

@@ -39,14 +39,11 @@
 #pragma once
 
 #include <stdint.h>
+#include <string>
 
 namespace px4_daemon
 {
 
-
-static const char CLIENT_SEND_PIPE_PATH[] = "/tmp/px4_client_send_pipe";
-static const char CLIENT_RECV_PIPE_PATH[] = "/tmp/px4_client_recv_pipe";
-
 static const unsigned RECV_PIPE_PATH_LEN = 64;
 
 struct client_send_packet_s {
@@ -93,6 +90,7 @@ struct client_recv_packet_s {
 unsigned get_client_send_packet_length(const client_send_packet_s *packet);
 unsigned get_client_recv_packet_length(const client_recv_packet_s *packet);
 int get_client_recv_pipe_path(const uint64_t uuid, char *path, const size_t path_len);
+std::string get_client_send_pipe_path(int instance_id);
 
 } // namespace px4_daemon
 

platforms/posix/src/px4_daemon/server.cpp

@@ -57,7 +57,8 @@ namespace px4_daemon
 
 Server *Server::_instance = nullptr;
 
-Server::Server()
+Server::Server(int instance_id)
+	: _instance_id(instance_id)
 {
 	_instance = this;
 }
@@ -109,12 +110,14 @@ Server::_server_main(void *arg)
 		return;
 	}
 
+	std::string client_send_pipe_path = get_client_send_pipe_path(_instance_id);
+
 	// Delete pipe in case it exists already.
-	unlink(CLIENT_SEND_PIPE_PATH);
+	unlink(client_send_pipe_path.c_str());
 
 	// Create new pipe to listen to clients.
-	mkfifo(CLIENT_SEND_PIPE_PATH, 0666);
+	mkfifo(client_send_pipe_path.c_str(), 0666);
-	int client_send_pipe_fd = open(CLIENT_SEND_PIPE_PATH, O_RDONLY);
+	int client_send_pipe_fd = open(client_send_pipe_path.c_str(), O_RDONLY);
 
 	while (true) {
 
@@ -129,7 +132,7 @@ Server::_server_main(void *arg)
 			// 0 means the pipe has been closed by all clients
 			// and we need to re-open it.
 			close(client_send_pipe_fd);
-			client_send_pipe_fd = open(CLIENT_SEND_PIPE_PATH, O_RDONLY);
+			client_send_pipe_fd = open(client_send_pipe_path.c_str(), O_RDONLY);
 		}
 	}
 

platforms/posix/src/px4_daemon/server.h

@@ -64,7 +64,7 @@ namespace px4_daemon
 class Server
 {
 public:
-	Server();
+	Server(int instance_id = 0);
 	~Server();
 
 	/**
@@ -118,6 +118,8 @@ private:
 
 	pthread_key_t _key;
 
+	int _instance_id; ///< instance ID for running multiple instances of the px4 server
+
 	static void _pthread_key_destructor(void *arg);
 
 	static Server *_instance;