ResolveLandmark.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    RobotSkillTemplates::PlatformGroup
 * @author     Fabian PK ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ResolveLandmark.h"
 
#include "PlatformContext.h"
 
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
#include <VirtualRobot/MathTools.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/armem_locations/client/Reader.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
namespace armarx::PlatformGroup
{
    // DO NOT EDIT NEXT LINE
    ResolveLandmark::SubClassRegistry ResolveLandmark::Registry(ResolveLandmark::GetName(),
                                                                &ResolveLandmark::CreateInstance);
 
    void
    ResolveLandmark::onEnter()
    {
 
    }
 
    void ResolveLandmark::run()
    {
        PlatformContext* context = getContext<PlatformContext>();
 
        ARMARX_IMPORTANT << "ON ENTER!";
        auto poseMatToLandmarkPose =
            [this](Eigen::Matrix4f poseMat, const std::string& landmarkName)
        {
            Eigen::Vector3f rpy;
            VirtualRobot::MathTools::eigen4f2rpy(poseMat, rpy);
            const float goalYaw = rpy[2];
 
            Vector3Ptr goalPose(new Vector3(poseMat(0, 3), poseMat(1, 3), goalYaw));
            ARMARX_INFO << "Landmark: '" << landmarkName << "' => position: ["
                        << goalPose->toEigen().transpose() << "]";
            return goalPose;
        };
 
 
        auto lookupLandmarkNamesWithNavigationMemory =
            [this, poseMatToLandmarkPose, context](const std::string& sceneName,
                                                   const std::vector<std::string>& landmarkNames,
                                                   std::map<std::string, Vector3Ptr>& outPoseMap,
                                                   std::vector<Vector3Ptr>& outPoseList) -> bool
        {
            ARMARX_INFO << "Getting the MNS";
            armem::client::MemoryNameSystem mns(context->memoryNameSystemPrx);
            armarx::armem::locations::client::Reader locReader;
            mns.setComponent(getContext<armarx::PlatformContext>());
            locReader.connect(mns);
 
            const auto locations = locReader.getAllLocationsInGlobalFrame();
            ARMARX_INFO << "Got " << locations.size() << " locations";
 
            std::vector<std::string> locationNames;
            for (const auto& [n, l] : locations)
            {
                ARMARX_CHECK(l.framedPose.header.frame == armarx::GlobalFrame);
                locationNames.push_back(n);
            }
 
            for (const std::string& landmarkName : landmarkNames)
            {
                const std::string landmarkMemoryName = sceneName + "/" + landmarkName;
                if (locations.count(landmarkMemoryName) == 0)
                {
                    std::stringstream ss;
                    ss << "Could not find location with name '" << landmarkMemoryName
                       << "'. Available are: ";
                    for (const auto& location : locations)
                    {
                        ss << "'" << location.first << "', \n";
                    }
                    ARMARX_ERROR << ss.str();
                    return false;
                }
                auto location = locations.at(landmarkMemoryName);
                Vector3Ptr landmarkPose =
                    poseMatToLandmarkPose(location.framedPose.pose, landmarkName);
                outPoseMap[landmarkName] = landmarkPose;
                outPoseList.push_back(landmarkPose);
            }
 
            return true;
        };
 
 
        const std::string sceneName = in.getSceneName();
        ARMARX_IMPORTANT << "Got scene name: '" << sceneName << "'";
 
        std::vector<std::string> landmarkNames;
        if (in.isLandmarkNamesSet())
        {
            landmarkNames = in.getLandmarkNames();
        }
        else
        {
            landmarkNames = {in.getLandmarkName()};
        }
 
        ARMARX_INFO << "landmark names: " << landmarkNames;
 
        std::map<std::string, Vector3Ptr> poseMap;
        std::vector<Vector3Ptr> poseList;
        bool success = false;
 
        if (true)
        {
            ARMARX_INFO << "Calling function lookupLandmarkNamesWithNavigationMemory()";
            success = lookupLandmarkNamesWithNavigationMemory(
                sceneName, landmarkNames, poseMap, poseList);
            ARMARX_INFO << "Calling function lookupLandmarkNamesWithNavigationMemory() Success? "
                        << success;
        }
        else
        {
            // TODO: Only get old memory stuff if necessary. (Should be removed in the future)
            auto priorKnowledge = context->priorKnowledgePrx;
            auto graphSegment = priorKnowledge->getGraphSegment();
 
            auto resolveLandmark = [this, sceneName, poseMatToLandmarkPose](
                                       ::memoryx::GraphMemorySegmentBasePrx graphSegment,
                                       const std::string& landmarkName) -> Vector3Ptr
            {
                if (!graphSegment->hasNodeWithName(sceneName, landmarkName))
                {
                    ARMARX_WARNING << "Target landmark '" << landmarkName
                                   << "' doesn't exist in graph " << sceneName;
                    auto nodes = graphSegment->getNodesByScene(sceneName);
                    for (auto& node : nodes)
                    {
                        ARMARX_INFO << "node: " << node->getName();
                    }
                    return nullptr;
                }
 
                auto node = graphSegment->getNodeFromSceneByName(sceneName, landmarkName);
                ARMARX_CHECK_NOT_NULL(node);
                ARMARX_CHECK_NOT_NULL(node->getPose());
 
                PosePtr pose = PosePtr::dynamicCast(node->getPose());
                ARMARX_CHECK_NOT_NULL(pose);
                ARMARX_CHECK_NOT_NULL(pose->position);
 
                Eigen::Matrix4f poseMat = pose->toEigen();
                Vector3Ptr goalPose = poseMatToLandmarkPose(poseMat, landmarkName);
 
                return goalPose;
            };
 
 
            for (const std::string& landmarkName : landmarkNames)
            {
                Vector3Ptr pose = resolveLandmark(graphSegment, landmarkName);
                if (pose)
                {
                    poseMap[landmarkName] = pose;
                    poseList.push_back(pose);
                }
                else
                {
                    success = true;
                }
            }
        }
 
        if (success)
        {
            if (in.isLandmarkNamesSet())
            {
                ARMARX_INFO << "Setting output maps";
                // Set output in any case so user can still use a failure.
                out.setLandmarkPoseMap(poseMap);
                out.setLandmarkPoseList(poseList);
            }
            else
            {
                ARMARX_INFO << "Setting output value";
                ARMARX_CHECK_EQUAL(poseList.size(), 1);
                out.setlandmarkPosition(poseList.front());
            }
            emitSuccess();
        }
        else
        {
            emitFailure();
        }
    }
 
    //void ResolveLandmark::onBreak()
    //{
    //    // put your user code for the breaking point here
    //    // execution time should be short (<100ms)
    //}
 
    void
    ResolveLandmark::onExit()
    {
        // put your user code for the exit point here
        // execution time should be short (<100ms)
    }
 
    // DO NOT EDIT NEXT FUNCTION
    XMLStateFactoryBasePtr
    ResolveLandmark::CreateInstance(XMLStateConstructorParams stateData)
    {
        return XMLStateFactoryBasePtr(new ResolveLandmark(stateData));
    }
} // namespace armarx::PlatformGroup