From d05b268d19377f91085f3563c442fd73123f7bee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 4 Aug 2018 15:15:56 +0200
Subject: [PATCH] posix shell: allow to run multiple instances
---
Tools/sitl_multiple_run.sh | 2 +-
cmake/posix/px4-alias.sh_in | 5 ++
launch/single_vehicle_spawn.launch | 2 +-
platforms/posix/cmake/px4_impl_os.cmake | 2 +-
platforms/posix/src/main.cpp | 69 +++++++++++++------
platforms/posix/src/px4_daemon/client.cpp | 7 +-
platforms/posix/src/px4_daemon/client.h | 3 +-
.../posix/src/px4_daemon/pipe_protocol.cpp | 10 ++-
.../posix/src/px4_daemon/pipe_protocol.h | 6 +-
platforms/posix/src/px4_daemon/server.cpp | 13 ++--
platforms/posix/src/px4_daemon/server.h | 4 +-
11 files changed, 84 insertions(+), 39 deletions(-)
diff --git a/Tools/sitl_multiple_run.sh b/Tools/sitl_multiple_run.sh
index a15ab1d3ec..826931f3f1 100755
--- a/Tools/sitl_multiple_run.sh
+++ b/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))
diff --git a/cmake/posix/px4-alias.sh_in b/cmake/posix/px4-alias.sh_in
index e0c361feb3..bd79461192 100644
--- a/cmake/posix/px4-alias.sh_in
+++ b/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}
diff --git a/launch/single_vehicle_spawn.launch b/launch/single_vehicle_spawn.launch
index 9c2c3fe7b0..c2f8bc5e39 100644
--- a/launch/single_vehicle_spawn.launch
+++ b/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)"/>
diff --git a/platforms/posix/cmake/px4_impl_os.cmake b/platforms/posix/cmake/px4_impl_os.cmake
index 217d1662a5..3c6faa944d 100644
--- a/platforms/posix/cmake/px4_impl_os.cmake
+++ b/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()
diff --git a/platforms/posix/src/main.cpp b/platforms/posix/src/main.cpp
index e2c391ed54..f305009c90 100644
--- a/platforms/posix/src/main.cpp
+++ b/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) {
+ 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()) {
+ 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(" -d daemon mode, don't start pxh shell\n");
+ printf(" -i <instance> px4 instance id to run multiple instances [0...N], default=0\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;
diff --git a/platforms/posix/src/px4_daemon/client.cpp b/platforms/posix/src/px4_daemon/client.cpp
index bd064fe34e..bbb3def133 100644
--- a/platforms/posix/src/px4_daemon/client.cpp
+++ b/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");
diff --git a/platforms/posix/src/px4_daemon/client.h b/platforms/posix/src/px4_daemon/client.h
index b0a606cfcb..2e454c4cc2 100644
--- a/platforms/posix/src/px4_daemon/client.h
+++ b/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
diff --git a/platforms/posix/src/px4_daemon/pipe_protocol.cpp b/platforms/posix/src/px4_daemon/pipe_protocol.cpp
index 0cdf49c94b..bbdc0e92a2 100644
--- a/platforms/posix/src/px4_daemon/pipe_protocol.cpp
+++ b/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
diff --git a/platforms/posix/src/px4_daemon/pipe_protocol.h b/platforms/posix/src/px4_daemon/pipe_protocol.h
index b0bcd0ba37..abc2b9b87a 100644
--- a/platforms/posix/src/px4_daemon/pipe_protocol.h
+++ b/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
diff --git a/platforms/posix/src/px4_daemon/server.cpp b/platforms/posix/src/px4_daemon/server.cpp
index cb6e2f1908..f8c544940e 100644
--- a/platforms/posix/src/px4_daemon/server.cpp
+++ b/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);
- int client_send_pipe_fd = open(CLIENT_SEND_PIPE_PATH, O_RDONLY);
+ mkfifo(client_send_pipe_path.c_str(), 0666);
+ 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);
}
}
diff --git a/platforms/posix/src/px4_daemon/server.h b/platforms/posix/src/px4_daemon/server.h
index 44bcf0918a..7ef8edd89f 100644
--- a/platforms/posix/src/px4_daemon/server.h
+++ b/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;
--
GitLab