Find nodes & files in associated source directories of workspaces

rosmon_core/src/package_registry.cpp

@@ -11,20 +11,79 @@
 #include <boost/filesystem.hpp>
 #include <boost/tokenizer.hpp>
 
+#include <tinyxml.h>
+
 #include <sys/types.h>
 #include <sys/wait.h>
 
+#include <fmt/format.h>
+
+namespace fs = boost::filesystem;
+
 namespace rosmon
 {
 
+struct CatkinWorkspace
+{
+	explicit CatkinWorkspace(const fs::path& path)
+	 : path{path}
+	{}
+
+	fs::path path;
+
+	bool sourcePackagesCrawled = false;
+	std::map<std::string, fs::path> packageSourcePaths;
+
+	void crawlSourcePackage(const fs::path& packageXMLPath)
+	{
+		TiXmlDocument document(packageXMLPath.string());
+
+		TiXmlBase::SetCondenseWhiteSpace(false);
+
+		if(!document.LoadFile())
+			return;
+
+		if(document.RootElement()->ValueStr() != "package")
+			return;
+
+		auto name = document.RootElement()->FirstChildElement("name");
+		if(!name)
+			return;
+
+		packageSourcePaths[name->GetText()] = packageXMLPath.parent_path();
+	}
+
+	void crawlSourcePackages()
+	{
+		if(sourcePackagesCrawled)
+			return;
+
+		fs::path catkinPath = path / ".catkin";
+		std::ifstream file{catkinPath.string()};
+
+		for(std::string path; std::getline(file, path, ';');)
+		{
+			if(!fs::exists(path))
+			{
+				fmt::print(stderr, "Warning: source path '{}' found in '{}' does not exist.\n", path, catkinPath.string());
+				continue;
+			}
+
+			for(fs::recursive_directory_iterator it(path); it != fs::recursive_directory_iterator(); ++it)
+			{
+				if(it->path().filename() == "package.xml")
+					crawlSourcePackage(it->path());
+			}
+		}
+	}
+};
+
 static std::map<std::string, std::string> g_cache;
 static rospack::Rospack g_pack;
-static std::vector<std::string> g_catkin_workspaces;
+static std::vector<CatkinWorkspace> g_catkin_workspaces;
 static std::map<std::pair<std::string, std::string>, std::string> g_executableCache;
 static bool g_initialized = false;
 
-namespace fs = boost::filesystem;
-
 static void init()
 {
 	if(g_initialized)
@@ -49,7 +108,7 @@ static void init()
 				continue;
 
 // 			printf("Found catkin workspace: '%s'\n", path.string().c_str());
-			g_catkin_workspaces.push_back(path.string());
+			g_catkin_workspaces.emplace_back(path);
 		}
 	}
 }
@@ -97,17 +156,22 @@ static std::string _getExecutable(const std::string& package, const std::string&
 		init();
 
 	// Try catkin libexec & catkin share first
-	for(const auto& workspace : g_catkin_workspaces)
+	for(auto& workspace : g_catkin_workspaces)
 	{
-		fs::path workspacePath(workspace);
-
-		fs::path execPath = workspacePath / "lib" / package / name;
+		fs::path execPath = workspace.path / "lib" / package / name;
 		if(fs::exists(execPath) && access(execPath.c_str(), X_OK) == 0)
 			return execPath.string();
 
-		std::string sharePath = getExecutableInPath(workspacePath / "share" / package, name);
+		std::string sharePath = getExecutableInPath(workspace.path / "share" / package, name);
 		if(!sharePath.empty())
 			return sharePath;
+
+		// Look in associated source directories of the workspace
+		workspace.crawlSourcePackages();
+
+		auto it = workspace.packageSourcePaths.find(package);
+		if(it != workspace.packageSourcePaths.end())
+			return getExecutableInPath(it->second, name);
 	}
 
 	// Crawl package directory for an appropriate executable
@@ -139,19 +203,28 @@ std::string PackageRegistry::findPathToFile(const std::string& package, const st
 		init();
 
 	// Try catkin libexec & catkin share first
-	for(const auto& workspace : g_catkin_workspaces)
+	for(auto& workspace : g_catkin_workspaces)
 	{
-		fs::path workspacePath(workspace);
-
-		fs::path execPath = workspacePath / "lib" / package;
+		fs::path execPath = workspace.path / "lib" / package;
 		fs::path filePath = execPath / name;
 		if(fs::exists(filePath) && access(filePath.c_str(), X_OK) == 0)
 			return execPath.string();
 
-		fs::path sharePath = workspacePath / "share" / package;
+		fs::path sharePath = workspace.path / "share" / package;
 		filePath = sharePath / name;
 		if(fs::exists(filePath) && access(filePath.c_str(), X_OK) == 0)
 			return sharePath.string();
+
+		// Look in associated source directories of the workspace
+		workspace.crawlSourcePackages();
+
+		auto it = workspace.packageSourcePaths.find(package);
+		if(it != workspace.packageSourcePaths.end())
+		{
+			fs::path filePath = it->second / name;
+			if(fs::exists(filePath) && access(filePath.c_str(), X_OK) == 0)
+				return it->second.string();
+		}
 	}
 
 	// Try package directory (src)