diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp index ff413dd475886dc1436a4a492b28668c17e167b7..b8b3190d1bae586226c60b490bb399abe5799272 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp @@ -84,7 +84,7 @@ namespace armarx::armem::server::robot_state // clang-format off viz::Robot robotVisu = viz::Robot(robot.description.name) - .file(xmlPath.package, xmlPath.path) + .file(xmlPath.package, xmlPath.package + "/" + xmlPath.path) .joints(robot.config.jointMap) .pose(robot.config.globalPose); diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp index e285967fdcf1d5df56dd483c2a298a1b908d7116..af2381c78d334e62ea39eb22978492ad66a32f1a 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp @@ -1,4 +1,6 @@ #include "Segment.h" +#include <filesystem> +#include <SimoxUtility/algorithm/string/string_tools.h> #include <ArmarXCore/core/application/properties/PluginAll.h> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> @@ -83,13 +85,29 @@ namespace armarx::armem::server::robot_state::description const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages(); const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename); - ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package; + // make sure that the relative path is without the 'package/' prefix + const std::string robotFileRelPath = [&robotFilename, &package]()-> std::string { + + if(simox::alg::starts_with(robotFilename, package)) + { + // remove "package" + "/" + const std::string fixedRobotFilename = robotFilename.substr(package.size() + 1, -1); + return fixedRobotFilename; + } + + return robotFilename; + }(); + + ARMARX_INFO << "Robot description '" << VAROUT(robotFileRelPath) << "' found in package " << package; const robot::RobotDescription robotDescription { .name = kinematicUnit->getRobotName(), - .xml = {package, kinematicUnit->getRobotFilename()} - }; // FIXME + .xml = {package, robotFileRelPath} + }; + + // make sure that the package path is valid + ARMARX_CHECK(std::filesystem::exists(robotDescription.xml.toSystemPath())); commitRobotDescription(robotDescription); }