esmini · johschmitz · Feb 28, 2023
@@ -449,7 +449,7 @@ int main(int argc, char** argv)
 		return -1;
 	}
 
-	std::string modelFilename = opt.GetOptionArg("model");
+	std::string modelFilePath = opt.GetOptionArg("model");
 
 	if (opt.GetOptionArg("density") != "")
 	{
@@ -497,7 +497,7 @@ int main(int argc, char** argv)
 		osg::ArgumentParser arguments(&argc, argv);
 		viewer::Viewer *viewer = new viewer::Viewer(
 			odrManager,
-			modelFilename.c_str(),
+			modelFilePath.c_str(),
 			NULL,
 			argv[0],
 			arguments,

@@ -8,7 +8,7 @@ message(STATUS "CMake version: " ${CMAKE_VERSION})
 cmake_minimum_required (VERSION 3.5.0 FATAL_ERROR)
 
 set(TARGET esmini)
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 # set(CMAKE_BUILD_TYPE "Debug")
 # set(CMAKE_CXX_FLAGS_DEBUG  "$ENV{CXXFLAGS} -O0 -Wall -g")             # 调试包不优化
 

@@ -697,7 +697,7 @@ extern "C"
 		{
 			return 0;
 		}
-		returnString = player->scenarioEngine->getOdrFilename().c_str();
+		returnString = player->scenarioEngine->getOdrFilePath().c_str();
 		return returnString.c_str();
 	}
 

@@ -18,6 +18,7 @@
 #include <sstream>
 #include <locale>
 #include <array>
+#include <filesystem>
 
 // UDP network includes
 #ifndef _WIN32
@@ -189,6 +190,20 @@ std::string CombineDirectoryPathAndFilepath(std::string dir_path, std::string fi
 	return path;
 }
 
+std::string ConcatenatePathVectorForLogging(const std::vector<std::filesystem::path>& vec)
+{
+	std::stringstream ss;
+	for (size_t i = 0; i < vec.size(); i++)
+8000
	{
+		if (i > 0)
+		{
+			ss << ", ";
+		}
+		ss << vec[i].string();
+	}
+	return ss.str();
+}
+
 double GetAngleOfVector(double x, double y)
 {
 	double angle;

@@ -22,6 +22,7 @@
 #include <condition_variable>
 #include <cstring>
 #include <map>
+#include <filesystem>
 
 #ifndef _WIN32
 	#include <inttypes.h>
@@ -269,6 +270,11 @@ bool FileExists(const char* fileName);
 */
 std::string CombineDirectoryPathAndFilepath(std::string dir_path, std::string file_path);
 
+/**
+ 	Concatenates a vector of strings into a single comma separated string
+*/
+std::string ConcatenatePathVectorForLogging(const std::vector<std::filesystem::path>& vec);
+
 /**
 	Retrieve the angle of a vector
 */

@@ -605,7 +605,7 @@ int ScenarioPlayer::InitViewer()
 	viewer_ = new viewer::Viewer(
 		roadmanager::Position::GetOpenDrive(),
 		scenarioEngine->getSceneGraphFilename().c_str(),
-		scenarioEngine->getScenarioFilename().c_str(),
+		scenarioEngine->getScenarioFilePath().c_str(),
 		exe_path_.c_str(),
 		arguments, &opt);
 
@@ -1404,7 +1404,7 @@ int ScenarioPlayer::Init()
 	// Save xml
 	if (opt.GetOptionSet("save_xosc"))
 	{
-		std::string filename = FileNameOf(scenarioEngine->getScenarioFilename());
+		std::string filename = FileNameOf(scenarioEngine->getScenarioFilePath());
 		pugi::xml_document* xml_doc = scenarioEngine->scenarioReader->GetDXMLDocument();
 
 		if (xml_doc)
@@ -1463,7 +1463,7 @@ int ScenarioPlayer::Init()
 				filename = dist.AddInfoToFilename(filename);
 			}
 
-			CSV_Log->Open(scenarioEngine->getScenarioFilename(),
+			CSV_Log->Open(scenarioEngine->getScenarioFilePath(),
 				static_cast<int>(scenarioEngine->entities_.object_.size()), filename);
 			LOG("Log all vehicle data in csv file");
 		}
@@ -1482,7 +1482,7 @@ int ScenarioPlayer::Init()
 		{
 			if (IsDirectoryName(arg_str))
 			{
-				filename = arg_str + FileNameWithoutExtOf(scenarioEngine->getScenarioFilename()) + ".dat";
+				filename = arg_str + FileNameWithoutExtOf(scenarioEngine->getScenarioFilePath()) + ".dat";
 			}
 			else
 			{
@@ -1500,7 +1500,7 @@ int ScenarioPlayer::Init()
 		}
 
 		LOG("Recording data to file %s", filename.c_str());
-		scenarioGateway->RecordToFile(filename, scenarioEngine->getOdrFilename(), scenarioEngine->getSceneGraphFilename());
+		scenarioGateway->RecordToFile(filename, scenarioEngine->getOdrFilePath(), scenarioEngine->getSceneGraphFilename());
 	}
 
 	if (launch_server)

@@ -18,9 +18,13 @@
 #include "ControllerFollowRoute.hpp"
 #include "OSCParameterDistribution.hpp"
 
+#include <filesystem>
+
 #define WHEEL_RADIUS 0.35
 #define STAND_STILL_THRESHOLD 1e-3  // meter per second
 
+namespace fs = std::filesystem;
+
 using namespace scenarioengine;
 
 static CallBack paramDeclCallback = {0, 0};
@@ -765,7 +769,8 @@ int ScenarioEngine::parseScenario()
 		LOG_AND_QUIT("OpenSCENARIO v%d.%d not supported. Please migrate scenario to v1.0 or v1.1 and try again.",
 			scenarioReader->GetVersionMajor(), scenarioReader->GetVersionMinor());
 	}
-	LOG("Loading %s (v%d.%d)", scenarioReader->getScenarioFilename().c_str(), scenarioReader->GetVersionMajor(), scenarioReader->GetVersionMinor());
+	LOG("Loading OpenSCENARIO from %s (v%d.%d)", scenarioReader->getScenarioFilePath().c_str(),
+		scenarioReader->GetVersionMajor(), scenarioReader->GetVersionMinor());
 
 	scenarioReader->parseGlobalParameterDeclarations();
 	scenarioReader->variables.Print("variables");  // All variables parsed at this point (not the case with parameters)
@@ -779,51 +784,73 @@ int ScenarioEngine::parseScenario()
 	// Init road manager
 	scenarioReader->parseRoadNetwork(roadNetwork);
 
-	if (getOdrFilename().empty())
+	if (getOdrFilePath().empty())
 	{
 		LOG("No OpenDRIVE file specified, continue without");
 	}
 	else
 	{
-		std::vector<std::string> file_name_candidates;
-		// absolute path or relative to current directory
-		file_name_candidates.push_back(getOdrFilename());
-		// relative path to scenario directory
-		file_name_candidates.push_back(CombineDirectoryPathAndFilepath(DirNameOf(scenarioReader->getScenarioFilename()), getOdrFilename()));
-		// Remove all directories from path and look in current directory
-		file_name_candidates.push_back(FileNameOf(getOdrFilename()));
-		// Finally check registered paths
-		for (size_t i = 0; i < SE_Env::Inst().GetPaths().size(); i++)
+		fs::path xodrPathOsc(getOdrFilePath());
+		std::vector<fs::path> xodrPathCandidates;
+		// Absolute path or relative to current directory
+		if (xodrPathOsc.is_absolute())
 		{
-			file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[i], getOdrFilename()));
-			file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[i], FileNameOf(getOdrFilename())));
+			// Absolute path
+			xodrPathCandidates.push_back(xodrPathOsc);
 		}
-		size_t i;
-		bool located = false;
-		for (i = 0; i < file_name_candidates.size(); i++)
+		else {
+			// Relative path (relative to .xosc file) - this should be the normal case
+			fs::path osc_path = fs::path(getScenarioFilePath()).parent_path();
+			xodrPathCandidates.push_back(osc_path / xodrPathOsc);
+		}
+		// Also relative to registered paths
+		for (size_t j = 0; j < SE_Env::Inst().GetPaths().size(); j++)
 		{
-			if (FileExists(file_name_candidates[i].c_str()))
-			{
-				located = true;
-				if (roadmanager::Position::LoadOpenDrive(file_name_candidates[i].c_str()) == true)
-				{
-					LOG("Loaded OpenDRIVE: %s", file_name_candidates[i].c_str());
-					break;
+			// Take the relative path from the LogicFile OpenSCENARIO XML
+			// element into account when looking for .xodr files. Note that this
+			// is different for catalog files due to the inability of
+			// OpenSCENARIO to specify explicit catalog file paths. Do not
+			// search for .xodr files only via their names to avoid
+			// unpredictable side effects.
+			fs::path registeredPath(SE_Env::Inst().GetPaths()[j]);
+			xodrPathCandidates.push_back(registeredPath / xodrPathOsc);
+		}
+		// Make all paths absolute and canonical
+		for (auto &&candidate_path : xodrPathCandidates) {
+			if (not candidate_path.is_absolute()) {
+				if (fs::exists(candidate_path)) {
+					// Canonical requires file to exist
+					candidate_path = fs::canonical(
+						fs::current_path() / candidate_path);
+				}
+				else {
+					// Weakly canonical does not require file to exist
+					candidate_path = fs::weakly_canonical(candidate_path);
 				}
-				else
+			}
+		}
+		bool result = false;
+		for (size_t j = 0; j < xodrPathCandidates.size() && result == false; j++)
+		{
+			if (fs::exists(xodrPathCandidates[j]))
+			{
+				// Try to load the .xodr file
+				result = roadmanager::Position::LoadOpenDrive(xodrPathCandidates[j].c_str());
+				if (result == true)
 				{
-					LOG("Failed to load OpenDRIVE file: %s", file_name_candidates[i].c_str());
-					if (i < file_name_candidates.size() - 1)
-					{
-						LOG("  -> trying: %s", file_name_candidates[i + 1].c_str());
-					}
+					LOG("Successfully loaded OpenDRIVE road network from %s",
+						xodrPathCandidates[j].c_str());
 				}
 			}
 		}
-
-		if (i == file_name_candidates.size())
+		if (result == false)
 		{
-			LOG((std::string("Failed to ") + (located ? "load" : "find") + " OpenDRIVE file " + getOdrFilename().c_str()).c_str());
+			std::string candidatesLogStr = ConcatenatePathVectorForLogging(xodrPathCandidates);
+			LOG("Couldn't locate OpenDRIVE road network file at either of the "
+				" following locations: %s. Check the path in the .xosc file "
+				"or specify additional asset paths for search with the --path"
+				" option.",
+				candidatesLogStr.c_str());
 			return -1;
 		}
 	}

@@ -74,9 +74,9 @@ namespace scenarioengine
 		void ResetEvents();
 		int DetectCollisions();
 
-		std::string getScenarioFilename() { return scenarioReader->getScenarioFilename(); }
+		std::string getScenarioFilePath() { return scenarioReader->getScenarioFilePath(); }
 		std::string getSceneGraphFilename() { return roadNetwork.sceneGraphFile.filepath; }
-		std::string getOdrFilename() { return roadNetwork.logicFile.filepath; }
+		std::string getOdrFilePath() { return roadNetwork.logicFile.filepath; }
 		roadmanager::OpenDrive *getRoadManager() { return odrManager; }
 
 		ScenarioGateway *getScenarioGateway();

@@ -29,6 +29,9 @@
 #include "ControllerALKS_R157SM.hpp"
 
 #include <cstdlib>
+#include <filesystem>
+
+namespace fs = std::filesystem;
 
 using namespace scenarioengine;
 
@@ -153,7 +156,7 @@ int ScenarioReader::loadOSCFile(const char *path)
 		}
 	}
 
-	oscFilename_ = path;
+	oscFilePath_ = path;
 
 	return 0;
 }
@@ -176,7 +179,7 @@ int ScenarioReader::loadOSCMem(const pugi::xml_document &xml_doc)
 		throw std::runtime_error("Couldn't find OpenSCENARIO or OpenScenario element - check XML!");
 	}
 
-	oscFilename_ = "inline";
+	oscFilePath_ = "inline";
 
 	return 0;
 }
@@ -259,35 +262,68 @@ Catalog *ScenarioReader::LoadCatalog(std::string name)
 	// Not found, try to locate it in one the registered catalog directories
 	pugi::xml_document catalog_doc;
 	pugi::xml_parse_result result;
-	std::vector<std::string> file_name_candidates;
+	std::vector<fs::path> catalogPathCandidates;
 	for (size_t i = 0; i < catalogs_->catalog_dirs_.size() && !result; i++)
 	{
-		file_name_candidates.clear();
-		// absolute path or relative to current directory
-		file_name_candidates.push_back(catalogs_->catalog_dirs_[i].dir_name_ + "/" + name + ".xosc");
-		// Then assume relative path to scenario directory - which perhaps should be the expected location
-		file_name_candidates.push_back(CombineDirectoryPathAndFilepath(DirNameOf(oscFilename_), catalogs_->catalog_dirs_[i].dir_name_) + "/" + name + ".xosc");
-		// Check registered paths
+		catalogPathCandidates.clear();
+		fs::path catalogPathOsc(catalogs_->catalog_dirs_[i].dir_name_);
+		if (catalogPathOsc.is_absolute())
+		{
+			// Absolute path
+			catalogPathCandidates.push_back(catalogPathOsc);
+		}
+		else {
+			// Relative path (relative to .xosc file) - this should be the normal case
+			fs::path oscPath = fs::path(oscFilePath_).parent_path();
+			catalogPathCandidates.push_back(
+				oscPath / catalogPathOsc / fs::path(name + ".xosc"));
+		}
+		// Also relative to registered paths
 		for (size_t j = 0; j < SE_Env::Inst().GetPaths().size(); j++)
 		{
-			file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[j], catalogs_->catalog_dirs_[i].dir_name_ + "/" + name + ".xosc"));
-			file_name_candidates.push_back(CombineDirectoryPathAndFilepath(SE_Env::Inst().GetPaths()[j], name + ".xosc"));
+			fs::path registeredPath(SE_Env::Inst().GetPaths()[j]);
+			catalogPathCandidates.push_back(
+				registeredPath / fs::path(name + ".xosc"));
+		}
+		// Make all paths absolute and canonical
+		for (auto &&catalogPath : catalogPathCandidates) {
+			if (not catalogPath.is_absolute()) {
+				if (fs::exists(catalogPath)) {
+					// Canonical requires file to exist
+					catalogPath = fs::canonical(
+						fs::current_path() / catalogPath);
+				}
+				else {
+					// Weakly canonical does not require file to exist
+					catalogPath = fs::weakly_canonical(catalogPath);
+				}
+			}
 		}
-		for (size_t j = 0; j < file_name_candidates.size() && !result; j++)
+		for (size_t j = 0; j < catalogPathCandidates.size() && !result; j++)
 		{
-			if (FileExists(file_name_candidates[j].c_str()))
+			if (fs::exists(catalogPathCandidates[j]))
 			{
-				// Load it
-				result = catalog_doc.load_file(file_name_candidates[j].c_str());
+				// Try to load the catalog file
+				result = catalog_doc.load_file(catalogPathCandidates[j].c_str());
+				if (result)
+				{
+					LOG("Successfully loaded %s from %s", name.c_str(),
+						catalogPathCandidates[j].c_str());
+				}
 			}
 		}
 	}
 	if (!result)
 	{
-		throw std::runtime_error("Couldn't locate catalog file: " + name + ". " + result.description());
+		throw std::runtime_error("Couldn't locate " + name
+			+ " file at either of the following locations: "
+			+ ConcatenatePathVectorForLogging(catalogPathCandidates)
+			+ ". Check catalog paths in .xosc file or specify additional "
+			  " asset paths for search with the --path option."
+			+ result.description());
 	}
 
-	pugi::xml_node osc_node_ = catalog_doc.child("OpenSCENARIO");
+    pugi::xml_node osc_node_ = catalog_doc.child("OpenSCENARIO");
 	if (!osc_node_)
 	{
 		osc_node_ = catalog_doc.child("OpenScenario");
@@ -901,7 +937,7 @@ Controller *ScenarioReader::parseOSCObjectController(pugi::xml_node controllerNo
 		if (!FileExists(filename.c_str()))
 		{
 			// Then assume relative path to scenario directory - which perhaps should be the expected location
-			std::string filename2 = CombineDirectoryPathAndFilepath(DirNameOf(oscFilename_), filename);
+			std::string filename2 = CombineDirectoryPathAndFilepath(DirNameOf(oscFilePath_), filename);
 
 			if (!FileExists(filename2.c_str()))
 			{