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/client/interfaces.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.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 robot::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::Robot& obj,
                                       const armem::Time& timestamp) const override;
 
        std::optional<robot::Robot> get(const std::string& name,
                                        const armem::Time& timestamp) const override;
        robot::Robot get(const robot::RobotDescription& description,
                         const armem::Time& timestamp) const override;
 
        std::optional<robot::RobotDescription> queryDescription(const std::string& name,
                                                                const armem::Time& timestamp) const;
 
        std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp) const;
 
        std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
                                                    const armem::Time& timestamp) const;
 
        std::optional<armarx::armem::arondto::Proprioception>
        queryProprioception(const robot::RobotDescription& description,
                            const armem::Time& timestamp) const;
 
        using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
 
        JointTrajectory queryJointStates(const robot::RobotDescription& description,
                                         const armem::Time& begin,
                                         const armem::Time& end) const;
 
        std::optional<robot::RobotState::Pose>
        queryGlobalPose(const robot::RobotDescription& description,
                        const armem::Time& timestamp) const;
 
        std::optional<robot::PlatformState>
        queryPlatformState(const robot::RobotDescription& description,
                           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, robot::ForceTorque>>
        queryForceTorque(const robot::RobotDescription& description,
                         const armem::Time& timestamp) const;
 
        std::optional<std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>>
        queryForceTorques(const robot::RobotDescription& description,
                          const armem::Time& start,
                          const armem::Time& end) const;
 
 
        std::optional<std::map<Hand, robot::ToFArray>>
        queryToF(const robot::RobotDescription& description, 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<robot::RobotState::Pose> 
         */
        std::optional<::armarx::armem::robot_state::Transform>
        queryOdometryPose(const robot::RobotDescription& description,
                          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<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
                                                       const std::string& name) const;
 
        std::optional<robot::RobotDescription>
        getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
        std::vector<robot::RobotDescription>
        getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
 
        std::optional<armarx::armem::arondto::Proprioception>
        getRobotProprioception(const armarx::armem::wm::Memory& memory,
                               const std::string& name) const;
 
        JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
                                            const std::string& name) const;
 
        std::optional<robot::PlatformState>
        getRobotPlatformState(const armarx::armem::wm::Memory& memory,
                              const std::string& name) const;
 
        std::map<RobotReader::Hand, robot::ForceTorque>
        getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
 
        std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
        getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
 
        std::map<RobotReader::Hand, robot::ToFArray> 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::robot_state::localization::TransformReader transformReader;
    };
 
} // namespace armarx::armem::robot_state