diff --git a/examples/loaders/urdf_loader.cpp b/examples/loaders/urdf_loader.cpp index 650eb142..8e831d65 100644 --- a/examples/loaders/urdf_loader.cpp +++ b/examples/loaders/urdf_loader.cpp @@ -8,9 +8,16 @@ using namespace threepp; int main(int argc, char** argv) { - if (argc != 2) return 1; + if (argc != 2) { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + return 1; + } - std::string urdfPath = argv[1]; + std::filesystem::path urdfPath = argv[1]; + if (!std::filesystem::exists(urdfPath)) { + std::cerr << "File not found: " << urdfPath << std::endl; + return 1; + } Canvas canvas{"URDF loader", {{"aa", 4}}}; GLRenderer renderer(canvas.size()); @@ -38,7 +45,7 @@ int main(int argc, char** argv) { Vector3 size; bb.getSize(size); - camera->position.copy(size).multiplyScalar(1.5).setX(0); + camera->position.set(0, size.y * 1.5f, size.z * 3.f); controls.update(); canvas.onWindowResize([&](WindowSize size) { @@ -51,10 +58,10 @@ int main(int argc, char** argv) { canvas.animate([&]() { const auto dt = clock.getDelta(); - model->getObjectByName("joint_1")->rotation.y += dt * 0.1; - model->getObjectByName("joint_2")->rotation.z += dt * 0.1; - model->getObjectByName("joint_3")->rotation.z += dt * 0.1; - model->getObjectByName("joint_4")->rotation.x += dt * 0.1; + model->getObjectByName("joint_1")->rotation.y += dt * 0.1f; + model->getObjectByName("joint_2")->rotation.z += dt * 0.1f; + model->getObjectByName("joint_3")->rotation.z += dt * 0.1f; + model->getObjectByName("joint_4")->rotation.x += dt * 0.1f; // model->getObjectByName("joint_5")->rotation.y += dt * 0.1; // model->getObjectByName("joint_6")->rotation.y += dt * 0.1; @@ -62,5 +69,4 @@ int main(int argc, char** argv) { renderer.render(*scene, *camera); }); - -} \ No newline at end of file +} diff --git a/include/threepp/loaders/Loader.hpp b/include/threepp/loaders/Loader.hpp new file mode 100644 index 00000000..b5f43303 --- /dev/null +++ b/include/threepp/loaders/Loader.hpp @@ -0,0 +1,20 @@ + +#ifndef THREEPP_LOADER_HPP +#define THREEPP_LOADER_HPP + +#include +#include + +namespace threepp { + + template + class Loader { + public: + virtual std::shared_ptr load(const std::filesystem::path& path) = 0; + + virtual ~Loader() = default; + }; + +}// namespace threepp + +#endif//THREEPP_LOADER_HPP diff --git a/include/threepp/loaders/URDFLoader.hpp b/include/threepp/loaders/URDFLoader.hpp index 77d22fd3..d39fde50 100644 --- a/include/threepp/loaders/URDFLoader.hpp +++ b/include/threepp/loaders/URDFLoader.hpp @@ -2,21 +2,15 @@ #ifndef THREEPP_URDFLOADER_HPP #define THREEPP_URDFLOADER_HPP -#include +#include "threepp/loaders/Loader.hpp" + #include +#include namespace threepp { class Group; - template - class Loader { - public: - virtual std::shared_ptr load(const std::filesystem::path& path) = 0; - - virtual ~Loader() = default; - }; - class URDFLoader { public: @@ -31,6 +25,6 @@ namespace threepp { std::unique_ptr pimpl_; }; -} +}// namespace threepp #endif//THREEPP_URDFLOADER_HPP diff --git a/src/threepp/loaders/URDFLoader.cpp b/src/threepp/loaders/URDFLoader.cpp index dca8e81f..519d73a9 100644 --- a/src/threepp/loaders/URDFLoader.cpp +++ b/src/threepp/loaders/URDFLoader.cpp @@ -31,6 +31,32 @@ namespace { -utils::parseFloat(xyz[2])); } + std::filesystem::path getModelPath(const std::filesystem::path& basePath, std::string fileName) { + + if (fileName.find("package://") != std::string::npos) { + + fileName = fileName.substr(10); + + //find parent path with package.xml + bool packageFound = false; + auto packagePath = basePath.parent_path(); + for (int i = 0; i < 10; ++i) { + if (exists(packagePath / "package.xml")) { + packageFound = true; + break; + } + packagePath = packagePath.parent_path(); + } + if (!packageFound) { + return ""; + } + + return packagePath.parent_path().string() + "/" + fileName; + } + + return fileName; + } + }// namespace struct URDFLoader::Impl { @@ -50,7 +76,7 @@ struct URDFLoader::Impl { object->name = root.attribute("name").as_string("robot"); std::vector> links; - for (auto link : root.children("link")) { + for (const auto link : root.children("link")) { auto linkObject = std::make_shared(); linkObject->name = link.attribute("name").value(); @@ -59,7 +85,7 @@ struct URDFLoader::Impl { // object->add(linkObject); // } - for (auto visual : link.children("visual")) { + for (const auto visual : link.children("visual")) { auto visualObject = std::make_shared(); visualObject->name = visual.attribute("name").value(); @@ -67,32 +93,12 @@ struct URDFLoader::Impl { visualObject->position.copy(getXYZ(visual)); visualObject->rotation.copy(getRPY(visual)); - for (auto geometry : visual.children("geometry")) { + for (const auto geometry : visual.children("geometry")) { auto mesh = geometry.child("mesh"); - auto fileName = std::string(mesh.attribute("filename").value()); - - if (fileName.find("package://") == 0) { - fileName = fileName.substr(10); - //find parent path with package.xml - bool packageFound = false; - auto packagePath = path.parent_path(); - for (int i = 0; i < 10; ++i) { - if (exists(packagePath / "package.xml")) { - packageFound = true; - break; - } - packagePath = packagePath.parent_path(); - } - if (!packageFound) { - return nullptr; - } - - fileName = packagePath.parent_path().string() + "/" + fileName; - } + auto fileName = getModelPath(path, mesh.attribute("filename").value()); - auto load = loader.load(fileName); - if (load) { + if (auto load = loader.load(fileName)) { visualObject->add(load); } } @@ -116,7 +122,7 @@ struct URDFLoader::Impl { links.emplace_back(linkObject); } - for (auto joint : root.children("joint")) { + for (const auto joint : root.children("joint")) { auto jointObject = std::make_shared(); jointObject->name = joint.attribute("name").value();