ZeroMQController.cpp

Go to the documentation of this file.

#include "ZeroMQController.h"
 
#include <Eigen/Dense>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
// for ARMARX_RT_LOGF_WARN
#include <chrono>
#include <string>
 
#include <boost/date_time/posix_time/posix_time.hpp>
 
#include <Ice/Current.h>
#include <Ice/Object.h>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h>
 
#include <armarx/control/common/type.h>
#include <armarx/control/common/utils.h>
#include <armarx/control/njoint_controller/task_space/CollisionAvoidanceImpedanceController.h>
#include <armarx/control/njoint_controller/task_space/CollisionAvoidanceMixedImpedanceVelocityController.h>
#include <armarx/control/njoint_controller/task_space/ImpedanceController.h>
#include <armarx/control/njoint_controller/task_space/MixedImpedanceVelocityController.h>
#include <armarx/control/njoint_controller/task_space/VelocityController.h>
 
#include <nlohmann/json_fwd.hpp>
#include <zmq.h>
#include <zmq.hpp>
 
namespace armarx::control::njoint_controller::task_space
{
    namespace base = armarx::control::njoint_controller::task_space;
 
    armarx::NJointControllerRegistration<
        NJointZeroMQTaskspaceController<base::NJointTaskspaceVelocityController>>
        registrationControllerNJointTSVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSVel));
 
    armarx::NJointControllerRegistration<
        NJointZeroMQTaskspaceController<base::NJointTaskspaceImpedanceController>>
        registrationControllerNJointTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSImp));
 
    armarx::NJointControllerRegistration<
        NJointZeroMQTaskspaceController<base::NJointTaskspaceMixedImpedanceVelocityController>>
        registrationControllerNJointTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSMixImpVel));
 
    armarx::NJointControllerRegistration<
        NJointZeroMQTaskspaceController<base::NJointTaskspaceCollisionAvoidanceImpedanceController>>
        registrationControllerNJointCollisionAvoidanceTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSImpCol));
 
    armarx::NJointControllerRegistration<NJointZeroMQTaskspaceController<
        base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController>>
        registrationControllerNJointCollisionAvoidanceTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSMixImpVelCol));
 
    // Implement getClassName() method for templated NJointZeroMQTaskspaceController
    template <>
    std::string
    NJointZeroMQTaskspaceController<class base::NJointTaskspaceVelocityController>::getClassName(
        const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSVel);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<class base::NJointTaskspaceImpedanceController>::getClassName(
        const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSImp);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<class base::NJointTaskspaceMixedImpedanceVelocityController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSMixImpVel);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceImpedanceController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSImpCol);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSMixImpVelCol);
    }
 
    template <typename NJointTaskspaceController>
    NJointZeroMQTaskspaceController<NJointTaskspaceController>::NJointZeroMQTaskspaceController(
        const armarx::RobotUnitPtr& robotUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceController{robotUnit, config, robot}, context(1), socket(context, ZMQ_PULL)
    {
        init();
    }
 
    template <>
    NJointZeroMQTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceImpedanceController>::
        NJointZeroMQTaskspaceController(const armarx::RobotUnitPtr& robotUnit,
                                        const armarx::NJointControllerConfigPtr& config,
                                        const VirtualRobot::RobotPtr& robot) :
        base::NJointTaskspaceImpedanceController{robotUnit, config, robot},
        base::NJointTaskspaceCollisionAvoidanceImpedanceController{robotUnit, config, robot},
        context(1),
        socket(context, ZMQ_PULL)
    {
        init();
    }
 
    template <>
    NJointZeroMQTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController>::
        NJointZeroMQTaskspaceController(const armarx::RobotUnitPtr& robotUnit,
                                        const armarx::NJointControllerConfigPtr& config,
                                        const VirtualRobot::RobotPtr& robot) :
        base::NJointTaskspaceMixedImpedanceVelocityController{robotUnit, config, robot},
        base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController{robotUnit,
                                                                                config,
                                                                                robot},
        context(1),
        socket(context, ZMQ_PULL)
    {
        init();
    }
 
    // Method(s) of NJointZeroMQTaskspaceController in general
    template <typename NJointTaskspaceController>
    void
    NJointZeroMQTaskspaceController<NJointTaskspaceController>::init()
    {
        // Bind to ZeroMQ socket
        int port = 5556; // TODO(@rietsch): Make configurable
        std::string ip_address = "*";
        std::string bind_address = "tcp://" + ip_address + ":" + std::to_string(port);
        try
        {
            socket.bind(bind_address);
            ARMARX_INFO << "Controller bound to " << bind_address << " through ZeroMQ";
        }
        catch (const zmq::error_t& e)
        {
            ARMARX_INFO << "ZeroMQ bind error: " << e.what();
            return;
        }
 
        time_last_packet = std::chrono::steady_clock::now();
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointZeroMQTaskspaceController<NJointTaskspaceController>::additionalTask()
    {
        zmq::message_t config_msg;
        bool received = socket.recv(&config_msg, ZMQ_DONTWAIT); // Try receive message, don't wait
        if (received)
        {
            // Measure time passed since last packet was received
            auto currentTime = std::chrono::steady_clock::now();
            std::chrono::duration<double> elapsed_seconds = currentTime - time_last_packet;
            frequency_ms = elapsed_seconds.count() * 1000;
            time_last_packet = currentTime;
 
            // Parse config message into nlohmann::json
            std::string config_json_str(static_cast<char*>(config_msg.data()), config_msg.size());
            nlohmann::json config_json = nlohmann::json::parse(config_json_str);
 
            // Calculate latency (might be imprecise due to clocks being not being synchronized?)
            if (config_json.contains("timestamp") && config_json["timestamp"].is_number())
            {
                auto arrival_time = std::chrono::duration<double>(
                                        std::chrono::system_clock::now().time_since_epoch())
                                        .count();
                double sender_time = config_json["timestamp"];
                latency_ms = (arrival_time - sender_time) * 1000; // Convert to milliseconds
                config_json.erase(
                    "timestamp"); // Important! Remove any data that is not part of the config
            }
 
            // Read into config data structure
            armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
            typename NJointTaskspaceController::ConfigDict receivedConfig;
            receivedConfig.read(reader, config_json);
            auto prevCfg = this->userConfig;
            for (auto& pair : this->limb)
            {
                auto limbName = pair.first;
                auto& limb = pair.second;
 
                auto limbConfig = prevCfg.limbs.at(limbName);
                auto limbConfigNew = receivedConfig.limbs.at(limbName);
                limbConfig.desiredPose = limbConfigNew.desiredPose;
                limb->bufferConfigUserToNonRt.getWriteBuffer() =
                    limbConfig; // Overwrite user config
                limb->bufferConfigUserToNonRt.commitWrite();
            }
 
            // Update hands
            if (this->hands)
            {
                this->hands->updateConfig(receivedConfig.hands);
            }
        }
 
        auto [rtTargetSafe, rtSafe] = this->additionalTaskUpdateStatus();
        this->additionalTaskSetTarget();
        if (not rtTargetSafe)
        {
            this->handleRTNotSafeInNonRT();
        }
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointZeroMQTaskspaceController<NJointTaskspaceController>::onPublish(
        const SensorAndControl&,
        const DebugDrawerInterfacePrx&,
        const DebugObserverInterfacePrx& debugObs)
    {
        // Log metrics
        StringVariantBaseMap datafields;
        datafields["frequency_ms"] = new Variant(frequency_ms);
        datafields["latency_ms"] = new Variant(latency_ms);
        debugObs->setDebugChannel(getClassName(), datafields);
 
        // Do limb publish
        for (auto& pair : this->limb)
        {
            if (not pair.second->rtReady.load())
                continue;
            this->limbPublish(pair.second, debugObs);
        }
    }
} // namespace armarx::control::njoint_controller::task_space