RobotReader.h

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
 */
 
#pragma once
 
#include <mutex>
#include <optional>
#include <vector>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/ReaderInterface.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
 
namespace armarx::armem::robot_state
{
    /**
     * @brief The RobotReader class.
     *
     * The purpose of this class is to synchronize the armem data structure armem::Robot
     * with the memory.
     */
    class RobotReader : virtual public ReaderInterface
    {
    public:
        RobotReader() = default;
        RobotReader(const RobotReader&);
        virtual ~RobotReader() = default;
 
        virtual void connect(armem::client::MemoryNameSystem& memoryNameSystem);
 
        virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
 
        [[nodiscard]] bool synchronize(Robot& obj, const armem::Time& timestamp) const override;
 
        std::optional<Robot> get(const std::string& name,
                                 const armem::Time& timestamp) const override;
        Robot get(const description::RobotDescription& description,
                  const armem::Time& timestamp) const override;
 
        std::optional<description::RobotDescription>
        queryDescription(const std::string& name, const armem::Time& timestamp) const;
 
        std::vector<description::RobotDescription>
        queryDescriptions(const armem::Time& timestamp) const;
 
        std::optional<RobotState> queryState(const std::string& robotName,
                                             const armem::Time& timestamp) const;
 
        std::optional<RobotState> queryJointState(const std::string& robotName,
                                                  const armem::Time& timestamp) const;
 
        /**
         * @param referencedTime If given, receives the time the returned data actually refers to,
         *                       which is the newest snapshot at or before `timestamp` and may be
         *                       arbitrarily older than it.
         */
        std::optional<armarx::armem::arondto::Proprioception>
        queryProprioception(const std::string& robotName,
                            const armem::Time& timestamp,
                            armem::Time* referencedTime = nullptr) const;
 
        using JointTrajectory = std::map<armem::Time, RobotState::JointMap>;
 
        JointTrajectory queryJointStates(const std::string& robotName,
                                         const armem::Time& begin,
                                         const armem::Time& end) const;
 
        std::optional<RobotState::Pose> queryGlobalPose(const std::string& robotName,
                                                        const armem::Time& timestamp) const;
 
        std::optional<PlatformState> queryPlatformState(const std::string& robotName,
                                                        const armem::Time& timestamp) const;
 
        void setSyncTimeout(const armem::Duration& duration);
        void setSleepAfterSyncFailure(const armem::Duration& duration);
 
        enum class Hand
        {
            Left,
            Right
        };
 
        std::optional<std::map<Hand, proprioception::ForceTorque>>
        queryForceTorque(const std::string& robotName, const armem::Time& timestamp) const;
 
        std::optional<
            std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
        queryForceTorques(const std::string& robotName,
                          const armem::Time& start,
                          const armem::Time& end) const;
 
 
        std::optional<std::map<Hand, exteroception::ToF>>
        queryToF(const std::string& robotName, const armem::Time& timestamp) const;
 
        /**
         * @brief retrieve the robot's pose in the odometry frame. 
         *
         * This pose is an integration of the robot's platform velocity and undergoes a significant drift.
         * 
         * @param description 
         * @param timestamp 
         * @return std::optional<RobotState::Pose> 
         */
        std::optional<::armarx::armem::robot_state::localization::Transform>
        queryOdometryPose(const std::string& robotName, const armem::Time& timestamp) const;
 
    protected:
        // by default, no timeout mechanism
        armem::Duration syncTimeout = armem::Duration::MicroSeconds(0);
        armem::Duration sleepAfterFailure = armem::Duration::MicroSeconds(0);
 
    private:
        std::optional<RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
                                                const std::string& name) const;
 
        std::optional<description::RobotDescription>
        getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
        std::vector<description::RobotDescription>
        getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
 
    protected:
        /// Protected rather than private so the referenced-time reporting can be exercised
        /// directly: everything above it needs a live memory server, this needs only a wm::Memory.
        std::optional<armarx::armem::arondto::Proprioception>
        getRobotProprioception(const armarx::armem::wm::Memory& memory,
                               const std::string& name,
                               armem::Time* referencedTime = nullptr) const;
 
    private:
 
        JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
                                            const std::string& name) const;
 
        std::optional<PlatformState> getRobotPlatformState(const armarx::armem::wm::Memory& memory,
                                                           const std::string& name) const;
 
        std::map<RobotReader::Hand, proprioception::ForceTorque>
        getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
 
        std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
        getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
        std::map<RobotReader::Hand, exteroception::ToF>
        getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
        struct Properties
        {
        } properties;
 
        const std::string propertyPrefix = "mem.robot_state.";
 
        armem::client::Reader memoryReader;
        mutable std::mutex memoryReaderMutex;
 
        client::localization::TransformReader transformReader;
    };
 
} // namespace armarx::armem::robot_state