ArticulatedObjectLocalizerDynamicSimulation.cpp

Go to the documentation of this file.

/*
 * 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/>.
 *
 * @package    ArmarXSimulation::ArmarXObjects::ArticulatedObjectLocalizerDynamicSimulation
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ArticulatedObjectLocalizerDynamicSimulation.h"
 
// Include headers you only need in function definitions in the .cpp.
#include <algorithm>
#include <mutex>
 
#include <Eigen/Core>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr
    ArticulatedObjectLocalizerDynamicSimulation::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->component(simulator);
 
        defs->optional(p.cycleTime, "cycleTime");
        defs->optional(p.cycleTimeUpdateObjectsInSimulator, "cycleTimeUpdateObjectList");
        defs->optional(p.objects, "objects", "The objects to (fake) localize.");
 
        return defs;
    }
 
    ArticulatedObjectLocalizerDynamicSimulation::ArticulatedObjectLocalizerDynamicSimulation()
    {
        addPlugin(articulatedObjectWriterPlugin);
        addPlugin(articulatedObjectReaderPlugin);
    }
 
    void
    ArticulatedObjectLocalizerDynamicSimulation::onInitComponent()
    {
    }
 
    void
    ArticulatedObjectLocalizerDynamicSimulation::onConnectComponent()
    {
        setupObjects();
 
        ARMARX_INFO << "Starting `localizeArticulatedObjects` task";
        task = new PeriodicTask<ArticulatedObjectLocalizerDynamicSimulation>(
            this,
            &ArticulatedObjectLocalizerDynamicSimulation::localizeObjects,
            p.cycleTime,
            false,
            "localizeArticulatedObjects");
        task->start();
        task->setDelayWarningTolerance(100);
 
        ARMARX_INFO << "Starting `setupObjects` task";
        taskUpdateObjects = new PeriodicTask<ArticulatedObjectLocalizerDynamicSimulation>(
            this,
            &ArticulatedObjectLocalizerDynamicSimulation::setupObjects,
            p.cycleTimeUpdateObjectsInSimulator,
            false,
            "setupObjects");
        taskUpdateObjects->start();
        taskUpdateObjects->setDelayWarningTolerance(100);
    }
 
    void
    ArticulatedObjectLocalizerDynamicSimulation::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    ArticulatedObjectLocalizerDynamicSimulation::onExitComponent()
    {
    }
 
    std::string
    ArticulatedObjectLocalizerDynamicSimulation::getDefaultName() const
    {
        return "ArticulatedObjectLocalizerDynamicSimulation";
    }
 
    void
    ArticulatedObjectLocalizerDynamicSimulation::setupObjects()
    {
        const auto descriptions = articulatedObjectReaderPlugin->get().queryDescriptions(
            armarx::DateTime::Now(), std::nullopt);
 
        std::unordered_map<std::string, std::shared_ptr<VirtualRobot::Robot>> newKnownObjects;
 
        // read access to shared variable "knownObjects"
        std::unique_lock lck{knownObjectsMtx};
        std::unordered_map<std::string, std::shared_ptr<VirtualRobot::Robot>> oldKnownObjects =
            knownObjects;
        lck.unlock();
 
        const auto tryRegister = [&](const std::string& name)
        {
            ARMARX_DEBUG << "Checking element '" << name << "' ...";
 
            const auto isKnownObject = [&name, this](const auto description) -> bool
            {
                if (description.name == name)
                {
                    return true;
                }
 
                const auto splits = simox::alg::split(description.name, "/");
                if (not splits.empty())
                {
                    if (splits.back() == name)
                    {
                        ARMARX_DEBUG << "Relaxed filter successful: Object description '"
                                     << description.name << "' matches '" << name;
                        return true;
                    }
                }
 
                return false;
            };
 
            const auto it = std::find_if(descriptions.begin(), descriptions.end(), isKnownObject);
            if (it == descriptions.end())
            {
                ARMARX_DEBUG << "Element '" << name << "' is either a robot or an unknown object";
                return;
            }
 
            ARMARX_DEBUG << "Found articulated object '" << name << "'";
 
            // Avoid loading of already existing robots as it takes some time ...
            if (oldKnownObjects.count(name) > 0)
            {
                newKnownObjects[name] = oldKnownObjects.at(name);
                return;
            }
 
            // New robot. Load it.
            const auto robot = VirtualRobot::RobotIO::loadRobot(
                ArmarXDataPath::resolvePath(it->xml.serialize().path),
                VirtualRobot::RobotIO::eStructure);
 
            if (robot)
            {
                ARMARX_CHECK(newKnownObjects.count(name) == 0)
                    << "At the moment, only one object per class supported.";
                robot->setName(robot->getName() +
                               "/0"); // mark object as instance '0', similar to scene snapshot
                newKnownObjects[name] = robot;
            }
        };
 
        // robots can be either active or passive
        const auto robots = simulator->getRobotNames();
        std::for_each(robots.begin(), robots.end(), tryRegister);
 
        {
            std::lock_guard g{knownObjectsMtx};
            knownObjects = newKnownObjects;
        }
    }
 
    armem::articulated_object::ArticulatedObject
    convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
    {
        ARMARX_DEBUG << "Filename is " << obj.getFilename();
 
        return armem::articulated_object::ArticulatedObject{
            .description = {.name = obj.getType(),
                            .xml = PackagePath(armarx::ArmarXDataPath::getProject(
                                                   {"PriorKnowledgeData"}, obj.getFilename()),
                                               obj.getFilename()),
                            .visualization = {},
                            .info = {}},
            .instance = "", // TODO(fabian.reister):
            .config =
                {
                    .timestamp = timestamp,
                    .globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
                    .jointMap = obj.getJointValues(),
                    .proprioception = std::nullopt,
                },
            .timestamp = timestamp,
        };
    }
 
    void
    ArticulatedObjectLocalizerDynamicSimulation::localizeObjects()
    {
        // remove objects if the no longer exist in the scene
        // knownObjects.erase(std::remove_if(knownObjects.begin(), knownObjects.end(), [&](const auto&p) -> bool{
        //     const auto& name = p.first;
        //     if(not simulator->hasRobot(name))
        //     {
        //         ARMARX_INFO << "Articulated object '" << name << "' was removed from the scene.";
        //         return true;
        //     }
 
        //     return false;
        // }), knownObjects.end());
        std::lock_guard g{knownObjectsMtx};
 
        for (const auto& [name, robot] : knownObjects)
        {
            // it is possible that the object is no longer in the scene
            const PoseBasePtr poseBase = [this, &name]() -> PoseBasePtr
            {
                try
                {
                    const auto robotPose = simulator->getRobotPose(name);
                    return robotPose;
                }
                catch (...)
                {
                    return nullptr;
                }
            }();
 
 
            if (not poseBase)
            {
                ARMARX_WARNING << deactivateSpam(10)
                               << "No articulatd object pose from simulator for object " << name
                               << " ...";
                continue;
            }
 
            robot->setGlobalPose(PosePtr::dynamicCast(poseBase)->toEigen());
 
            const auto jointMap = simulator->getRobotJointAngles(name);
            robot->setJointValues(jointMap);
 
            armarx::armem::articulated_object::ArticulatedObject obj =
                convert(*robot, armarx::DateTime::Now());
            obj.instance =
                static_cast<std::string>("0"); // we assume that there is only one object of a type
 
            ARMARX_DEBUG << "Publishing new state for object `" << name << "`, instance id: `"
                         << obj.instance << "`";
 
            articulatedObjectWriterPlugin->get().store(obj, true);
        }
    }
 
 
} // namespace armarx