TransformReader.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/>.
 *
 * @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 "TransformReader.h"
 
#include <algorithm>
#include <iterator>
#include <numeric>
 
// Ice
#include <IceUtil/Time.h>
 
// Eigen
#include <Eigen/Geometry>
 
// Simox
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/color/cmaps.h>
#include <SimoxUtility/math/pose/interpolate.h>
 
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
// this package
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx::armem::robot_state::client::localization
{
    TransformReader::TransformReader(const TransformReader& t) :
        memoryReader(t.memoryReader), properties(t.properties), propertyPrefix(t.propertyPrefix)
    {
 
    }
 
    TransformReader::~TransformReader() = default;
 
    void
    TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
 
        const std::string prefix = propertyPrefix;
 
        def->optional(properties.localizationSegment,
                      prefix + "localizationSegment",
                      "Name of the localization memory core segment to use.");
 
        def->optional(properties.memoryName, prefix + "Memory");
    }
 
    void
    TransformReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_INFO << "TransformReader: Waiting for memory '" << properties.memoryName << "' ...";
        try
        {
            memoryReader = memoryNameSystem.useReader(properties.memoryName);
            ARMARX_INFO << "TransformReader: Connected to memory '" << properties.memoryName;
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
    }
 
    TransformResult
    TransformReader::getGlobalPose(const std::string& agentName,
                                   const std::string& robotRootFrame,
                                   const armem::Time& timestamp) const
    {
        const TransformQuery query{.header = {.parentFrame = GlobalFrame,
                                              .frame = robotRootFrame,
                                              .agent = agentName,
                                              .timestamp = timestamp}};
 
        return lookupTransform(query);
    }
 
    // std::vector<std::string> buildTransformChainArbitrary(const
    // std::map<std::string, armem::ProviderSegment>& providerSegments, const
    // std::string& parentFrame, const std::string& frame){
 
    //     std::map<std::string, std::string> tf;
 
    //     // build string representation of transforms
    //     for(const auto& [name, segment]: providerSegments){
    //         const auto frames = simox::alg::split(name, ",");
    //         if (frames.size() != 2){
    //             ARMARX_WARNING << "Segment name " << name << " not understood";
    //             continue;
    //         }
 
    //         tf.insert(frame[0], frame[1]);
    //     }
 
    //     // find
 
    // }
 
 
    TransformResult
    TransformReader::lookupTransform(const TransformQuery& query) const
    {
        const auto& timestamp = query.header.timestamp;
        ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
 
        const IceUtil::Time durationEpsilon = IceUtil::Time::milliSeconds(-1);
        (void)durationEpsilon;
 
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties.localizationSegment)
        .providerSegments().withName(query.header.agent) // agent
        .entities().all() // parentFrame,frame
        .snapshots().beforeOrAtTime(query.header.timestamp);
        // clang-format on
 
        try
        {
            std::scoped_lock l(memoryReaderMutex);
            const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
            ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
            if (not qResult.success)
            {
                return {.transform =
                            {
                                .header = query.header,
                            },
                        .status = TransformResult::Status::ErrorFrameNotAvailable,
                        .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
                                        query.header.frame + "' : " + qResult.errorMessage};
            }
 
            const auto& localizationCoreSegment =
                qResult.memory.getCoreSegment(properties.localizationSegment);
 
            using armarx::armem::robot_state::localization::TransformHelper;
            return TransformHelper::lookupTransform(localizationCoreSegment, query);
        }
        catch (...)
        {
            ARMARX_VERBOSE << "Lookup Transform failure" << GetHandledExceptionString();
            return TransformResult{.transform =
                                       {
                                           .header = query.header,
                                       },
                                   .status = TransformResult::Status::Error,
                                   .errorMessage = "Error in tf lookup '" +
                                                   query.header.parentFrame + " -> " +
                                                   query.header.frame + "' : Memory exception."};
        }
    }
 
} // namespace armarx::armem::client::robot_state::localization