Improvements to SITL to make paths more flexible. (#5181) (#5255)

* Path cleanup for SITL.

* Restructured sitl scripts dir.

* Set integration tests to use ekf2 for vtol.

* Fix sitl paths for mac.

src/firmware/posix/CMakeLists.txt

@@ -118,17 +118,43 @@ else()
 	endif()
 endif()
 
+#=============================================================================
+# sitl run targets
+#
+
+set(SITL_WORKING_DIR ${PX4_BINARY_DIR}/tmp)
+file(MAKE_DIRECTORY ${SITL_WORKING_DIR})
+
 add_custom_target(run_config
-	COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}"
-	"${config_sitl_viewer}" "${config_sitl_model}" "${PX4_BINARY_DIR}"
-	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+	COMMAND Tools/sitl_run.sh
+	$<TARGET_FILE:px4>
+	${config_sitl_rcS_dir}
+	${config_sitl_debugger}
+	${config_sitl_viewer}
+	${config_sitl_model} 
+	${PX4_SOURCE_DIR}
+	${PX4_BINARY_DIR}
+	WORKING_DIRECTORY ${SITL_WORKING_DIR}
 	USES_TERMINAL
 	)
 add_dependencies(run_config px4)
 
-foreach(viewer none jmavsim gazebo replay)
-	foreach(debugger none gdb lldb ddd valgrind)
-		foreach(model none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480)
+# project to build sitl_gazebo if necessary
+ExternalProject_Add(sitl_gazebo
+	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/sitl_gazebo
+	CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+	BINARY_DIR ${PX4_BINARY_DIR}/build_gazebo
+	INSTALL_COMMAND ""
+	)
+set_target_properties(sitl_gazebo PROPERTIES EXCLUDE_FROM_ALL TRUE)
+
+# create targets for each viewer/model/debugger combination
+set(viewers none jmavsim gazebo replay)
+set(debuggers none gdb lldb ddd valgrind)
+set(models none iris iris_opt_flow tailsitter standard_vtol plane solo typhoon_h480)
+foreach(viewer ${viewers})
+	foreach(debugger ${debuggers})
+		foreach(model ${models})
 			if (debugger STREQUAL "none")
 				if (model STREQUAL "none")
 					set(_targ_name "${viewer}")
@@ -143,13 +169,20 @@ foreach(viewer none jmavsim gazebo replay)
 				endif()
 			endif()
 			add_custom_target(${_targ_name}
-				COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
-				"${debugger}"
-				"${viewer}" "${model}" "${PX4_BINARY_DIR}"
-				WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+				COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+				$<TARGET_FILE:px4>
+				${config_sitl_rcS_dir}
+				${debugger}
+				${viewer}
+				${model}
+				${PX4_SOURCE_DIR}
+				${PX4_BINARY_DIR}
+				WORKING_DIRECTORY ${SITL_WORKING_DIR}
 				USES_TERMINAL
 				)
-			add_dependencies(${_targ_name} px4)
+			if (viewer STREQUAL "gazebo")
+				add_dependencies(${_targ_name} sitl_gazebo)
+			endif()
 		endforeach()
 	endforeach()
 endforeach()
@@ -158,8 +191,25 @@ endforeach()
 # install
 #
 
-install(TARGETS px4 DESTINATION bin)
+install(TARGETS px4 DESTINATION ${CMAKE_INSTALL_BINDIR})
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/ROMFS DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/posix-configs DESTINATION ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
 
+#=============================================================================
+# tests
+#
+
+add_test(NAME rcS_tests
+	COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+		$<TARGET_FILE:px4>
+		posix-configs/SITL/init/test
+		none
+		none
+		none
+		${PX4_SOURCE_DIR}
+		${PX4_BINARY_DIR}
+		WORKING_DIRECTORY ${SITL_WORKING_DIR})
+set_tests_properties(rcS_tests PROPERTIES
+	PASS_REGULAR_EXPRESSION "All tests passed")
+
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/lib/rc/rc_tests/RCTest.cpp

@@ -16,7 +16,7 @@
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 #define TEST_DATA_PATH "/fs/microsd"
 #else
-#define TEST_DATA_PATH "../../../../test_data/"
+#define TEST_DATA_PATH "./test_data/"
 #endif
 
 extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]);

src/platforms/posix/main.cpp

@@ -50,6 +50,7 @@
 #include "px4_middleware.h"
 #include "DriverFramework.hpp"
 #include <termios.h>
+#include <sys/stat.h>
 
 namespace px4
 {
@@ -62,6 +63,12 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
 
 #define CMD_BUFF_SIZE	100
 
+#ifdef PATH_MAX
+const unsigned path_max_len = PATH_MAX;
+#else
+const unsigned path_max_len = 1024;
+#endif
+
 static bool _ExitFlag = false;
 
 static struct termios orig_term;
@@ -85,6 +92,96 @@ extern "C" {
 	}
 }
 
+static inline bool fileExists(const string &name)
+{
+	struct stat buffer;
+	return (stat(name.c_str(), &buffer) == 0);
+}
+
+static inline bool dirExists(const string &path)
+{
+	struct stat info;
+
+	if (stat(path.c_str(), &info) != 0) {
+		return false;
+
+	} else if (info.st_mode & S_IFDIR) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+static inline void touch(const string &name)
+{
+	fstream fs;
+	fs.open(name, ios::out);
+	fs.close();
+}
+
+static int mkpath(const char *path, mode_t mode);
+
+static int do_mkdir(const char *path, mode_t mode)
+{
+	struct stat             st;
+	int             status = 0;
+
+	if (stat(path, &st) != 0) {
+		/* Directory does not exist. EEXIST for race condition */
+		if (mkdir(path, mode) != 0 && errno != EEXIST) {
+			status = -1;
+		}
+
+	} else if (!S_ISDIR(st.st_mode)) {
+		errno = ENOTDIR;
+		status = -1;
+	}
+
+	return (status);
+}
+
+/**
+** mkpath - ensure all directories in path exist
+** Algorithm takes the pessimistic view and works top-down to ensure
+** each directory in path exists, rather than optimistically creating
+** the last element and working backwards.
+*/
+static int mkpath(const char *path, mode_t mode)
+{
+	char           *pp;
+	char           *sp;
+	int             status;
+	char           *copypath = strdup(path);
+
+	status = 0;
+	pp = copypath;
+
+	while (status == 0 && (sp = strchr(pp, '/')) != 0) {
+		if (sp != pp) {
+			/* Neither root nor double slash in path */
+			*sp = '\0';
+			status = do_mkdir(copypath, mode);
+			*sp = '/';
+		}
+
+		pp = sp + 1;
+	}
+
+	if (status == 0) {
+		status = do_mkdir(path, mode);
+	}
+
+	free(copypath);
+	return (status);
+}
+
+static string pwd()
+{
+	char temp[path_max_len];
+	return (getcwd(temp, path_max_len) ? string(temp) : string(""));
+}
+
 static void print_prompt()
 {
 	cout.flush();
@@ -134,12 +231,13 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silen
 static void usage()
 {
 
-	cout << "./px4 [-d] [startup_config] -h" << std::endl;
+	cout << "./px4 [-d] data_directory startup_config [-h]" << endl;
 	cout << "   -d            - Optional flag to run the app in daemon mode and does not listen for user input." <<
-	     std::endl;
-	cout << "                   This is needed if px4 is intended to be run as a upstart job on linux" << std::endl;
-	cout << "<startup_config> - config file for starting/stopping px4 modules" << std::endl;
-	cout << "   -h            - help/usage information" << std::endl;
+	     endl;
+	cout << "                   This is needed if px4 is intended to be run as a upstart job on linux" << endl;
+	cout << "<data_directory> - directory where romfs and posix-configs are located" << endl;
+	cout << "<startup_config> - config file for starting/stopping px4 modules" << endl;
+	cout << "   -h            - help/usage information" << endl;
 }
 
 static void process_line(string &line, bool exit_on_fail)
@@ -202,52 +300,131 @@ int main(int argc, char **argv)
 	set_cpu_scaling();
 
 	int index = 1;
-	char *commands_file = nullptr;
+	string  commands_file = "";
+	int positional_arg_count = 0;
+	string data_path = "";
+	string node_name = "";
 
+	// parse arguments
 	while (index < argc) {
+		//cout << "arg: " << index << " : " << argv[index] << endl;
+
 		if (argv[index][0] == '-') {
 			// the arg starts with -
-			if (strcmp(argv[index], "-d") == 0) {
+			if (strncmp(argv[index], "-d", 2) == 0) {
 				daemon_mode = true;
 
-			} else if (strcmp(argv[index], "-h") == 0) {
+			} else if (strncmp(argv[index], "-h", 2) == 0) {
 				usage();
 				return 0;
 
-			} else if (strcmp(argv[index], "-c") == 0) {
+			} else if (strncmp(argv[index], "-c", 2) == 0) {
 				chroot_on = true;
 
 			} else {
-				PX4_WARN("Unknown/unhandled parameter: %s", argv[index]);
+				PX4_ERR("Unknown/unhandled parameter: %s", argv[index]);
 				return 1;
 			}
 
+		} else if (!strncmp(argv[index], "__", 2)) {
+			//cout << "ros argument" << endl;
+
+			// ros arguments
+			if (!strncmp(argv[index], "__name:=", 8)) {
+				string name_arg = argv[index];
+				node_name = name_arg.substr(8);
+				cout << "node name: " << node_name << endl;
+			}
+
 		} else {
-			// this is an argument that does not have '-' prefix; treat it like a file name
-			ifstream infile(argv[index]);
+			//cout << "positional argument" << endl;
 
-			if (infile.good()) {
-				infile.close();
+			positional_arg_count += 1;
+
+			if (positional_arg_count == 1) {
+				data_path = argv[index];
+				cout << "data path: " << data_path << endl;
+
+			} else if (positional_arg_count == 2) {
 				commands_file = argv[index];
-
-			} else {
-				PX4_WARN("Error opening file: %s", argv[index]);
-				return -1;
+				cout << "commands file: " << commands_file << endl;
 			}
 		}
 
 		++index;
 	}
 
+	if (positional_arg_count != 2) {
+		PX4_ERR("Error expected 2 position arguments, got %d", positional_arg_count);
+		usage();
+		return -1;
+	}
 
+	if (commands_file.size() < 1) {
+		PX4_ERR("Error commands file not specified");
+		return -1;
+	}
+
+	if (!fileExists(commands_file)) {
+		PX4_ERR("Error opening commands file, does not exist: %s", commands_file.c_str());
+		return -1;
+	}
+
+	// create sym-links
+	vector<string> path_sym_links;
+	path_sym_links.push_back("ROMFS");
+	path_sym_links.push_back("posix-configs");
+	path_sym_links.push_back("test_data");
+
+	for (int i = 0; i < path_sym_links.size(); i++) {
+		string path_sym_link = path_sym_links[i];
+		//cout << "path sym link: " << path_sym_link << endl;
+		string src_path = data_path + "/" + path_sym_link;
+		string dest_path =  pwd() + "/" +  path_sym_link;
+
+		PX4_DEBUG("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str());
+
+		if (dirExists(path_sym_link)) { continue; }
+
+		// create sym-links
+		int ret = symlink(src_path.c_str(), dest_path.c_str());
+
+		if (ret != 0) {
+			PX4_ERR("Error creating symlink %s -> %s",
+				src_path.c_str(), dest_path.c_str());
+			return ret;
+
+		} else {
+			PX4_DEBUG("Successfully created symlink %s -> %s",
+				  src_path.c_str(), dest_path.c_str());
+		}
+	}
+
+	// setup rootfs
+	const string eeprom_path = "./rootfs/eeprom/";
+	const string microsd_path = "./rootfs/fs/microsd/";
+
+	if (!fileExists(eeprom_path + "parameters")) {
+		cout << "creating new parameters file" << endl;
+		mkpath(eeprom_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR);
+		touch(eeprom_path + "parameters");
+	}
+
+	if (!fileExists(microsd_path + "dataman")) {
+		cout << "creating new dataman file" << endl;
+		mkpath(microsd_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR);
+		touch(microsd_path + "dataman");
+	}
+
+	// initialize
 	DriverFramework::Framework::initialize();
 	px4::init_once();
 
 	px4::init(argc, argv, "px4");
 
 	// if commandfile is present, process the commands from the file
-	if (commands_file != nullptr) {
-		ifstream infile(commands_file);
+	if (commands_file.size() != 0) {
+		ifstream infile(commands_file.c_str());
 
 		if (infile.is_open()) {
 			for (string line; getline(infile, line, '\n');) {
@@ -261,7 +438,7 @@ int main(int argc, char **argv)
 			}
 
 		} else {
-			PX4_WARN("Error opening file: %s", commands_file);
+			PX4_ERR("Error opening commands file: %s", commands_file.c_str());
 		}
 	}
 
@@ -270,12 +447,6 @@ int main(int argc, char **argv)
 		// this is not an attempt to secure the environment,
 		// rather, to replicate a deployed file system.
 
-#ifdef PATH_MAX
-		const unsigned path_max_len = PATH_MAX;
-#else
-		const unsigned path_max_len = 1024;
-#endif
-
 		char pwd_path[path_max_len];
 		const char *folderpath = "/rootfs/";
 

src/systemcmds/tests/test_mixer.cpp

@@ -96,7 +96,7 @@ int test_mixer(int argc, char *argv[])
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 	const char *filename = "/etc/mixers/IO_pass.mix";
 #else
-	const char *filename = "../../../../ROMFS/px4fmu_test/mixers/IO_pass.mix";
+	const char *filename = "ROMFS/px4fmu_test/mixers/IO_pass.mix";
 #endif
 
 	//PX4_INFO("loading: %s", filename);
@@ -406,7 +406,7 @@ int test_mixer(int argc, char *argv[])
 #if !defined(CONFIG_ARCH_BOARD_SITL)
 	filename = "/etc/mixers/quad_test.mix";
 #else
-	filename = "../../../../ROMFS/px4fmu_test/mixers/quad_test.mix";
+	filename = "ROMFS/px4fmu_test/mixers/quad_test.mix";
 #endif
 
 	load_mixer_file(filename, &buf[0], sizeof(buf));