diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt index 20f30486fd3249168f6f80a9353d6dd78cc7b2ea..2b2fb5f9d4ea899930351b1234873b2354cb50bf 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt +++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt @@ -30,6 +30,8 @@ set(LIB_FILES plugins/ObjectPoseProviderPlugin.cpp plugins/ObjectPoseClientPlugin.cpp plugins/RequestedObjects.cpp + + util.cpp ) set(LIB_HEADERS ArmarXObjects.h @@ -54,6 +56,8 @@ set(LIB_HEADERS plugins/ObjectPoseProviderPlugin.h plugins/ObjectPoseClientPlugin.h plugins/RequestedObjects.h + + util.h ) armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}") diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.cpp b/source/RobotAPI/libraries/ArmarXObjects/util.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c1d057c5c33c1085c35307fc7eb4208b14f1042a --- /dev/null +++ b/source/RobotAPI/libraries/ArmarXObjects/util.cpp @@ -0,0 +1,97 @@ +/** + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2022 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#include "util.h" + +#include <string> + +#include <Eigen/Geometry> + +#include <VirtualRobot/CollisionDetection/CollisionModel.h> +#include <VirtualRobot/ManipulationObject.h> +#include <VirtualRobot/Primitive.h> +#include <VirtualRobot/SceneObjectSet.h> +#include <VirtualRobot/VirtualRobot.h> +#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h> +#include <VirtualRobot/Visualization/VisualizationNode.h> + +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> + +#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h> +#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h> +#include <RobotAPI/libraries/core/FramedPose.h> + +namespace armarx::objpose +{ + + objpose::ObjectPoseSeq + filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlacklist) + { + const auto isBlacklisted = [&datasetBlacklist](const objpose::ObjectPose& objectPose) + { + const auto dataset = objectPose.objectID.dataset(); + + return std::find(datasetBlacklist.begin(), datasetBlacklist.end(), dataset) != + datasetBlacklist.end(); + }; + + objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end()); + return objects; + } + + + VirtualRobot::ManipulationObjectPtr + asManipulationObject(const objpose::ObjectPose& objectPose) + { + ObjectFinder finder; + + VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet); + if (auto obstacle = finder.loadManipulationObject(objectPose)) + { + obstacle->setGlobalPose(objectPose.objectPoseGlobal); + return obstacle; + } + + ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`"; + return nullptr; + } + + + VirtualRobot::SceneObjectSetPtr + asSceneObjects(const objpose::ObjectPoseSeq& objectPoses) + { + ObjectFinder finder; + + VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet); + for (const auto& objectPose : objectPoses) + { + if (auto obstacle = finder.loadManipulationObject(objectPose)) + { + obstacle->setGlobalPose(objectPose.objectPoseGlobal); + sceneObjects->addSceneObject(obstacle); + } + } + + return sceneObjects; + } + + +} // namespace armarx::objpose diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.h b/source/RobotAPI/libraries/ArmarXObjects/util.h new file mode 100644 index 0000000000000000000000000000000000000000..9c54736b0fcb2f1315d7a9189b53319eaa18a278 --- /dev/null +++ b/source/RobotAPI/libraries/ArmarXObjects/util.h @@ -0,0 +1,36 @@ +/** + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2022 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <VirtualRobot/VirtualRobot.h> + +#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h> + +namespace armarx::objpose +{ + objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects, + const std::vector<std::string>& datasetBlacklist); + + VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose); + VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses); + +} // namespace armarx::objpose