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 <cstring>
#include <mutex>
#include <string>
 
#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/Base.h>
#include <armarx/control/njoint_controller/task_space/ColAvoid.h>
 
#include <nlohmann/json.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::NJointTSVelController>>
        registrationControllerNJointTSVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSVel));
 
    armarx::NJointControllerRegistration<
        NJointZeroMQTaskspaceController<base::NJointTSImpController>>
        registrationControllerNJointTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSImp));
 
    armarx::NJointControllerRegistration<
        NJointZeroMQTaskspaceController<base::NJointTSMixImpVelController>>
        registrationControllerNJointTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSMixImpVel));
 
    armarx::NJointControllerRegistration<
        NJointZeroMQTaskspaceController<base::NJointTSImpColController>>
        registrationControllerNJointCollisionAvoidanceTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSImpCol));
 
    armarx::NJointControllerRegistration<NJointZeroMQTaskspaceController<
        base::NJointTSMixImpVelColController>>
        registrationControllerNJointCollisionAvoidanceTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZMQTSMixImpVelCol));
 
    // Implement getClassName() method for templated NJointZeroMQTaskspaceController
    template <>
    std::string
    NJointZeroMQTaskspaceController<class base::NJointTSVelController>::getClassName(
        const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSVel);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<class base::NJointTSImpController>::getClassName(
        const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSImp);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<class base::NJointTSMixImpVelController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSMixImpVel);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<
        class base::NJointTSImpColController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZMQTSImpCol);
    }
 
    template <>
    std::string
    NJointZeroMQTaskspaceController<
        class base::NJointTSMixImpVelColController>::
        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),
        publishSocket_(context, ZMQ_PUSH)
    {
        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),
    //     publishSocket_(context, ZMQ_PUSH)
    // {
    //     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),
    //     publishSocket_(context, ZMQ_PUSH)
    // {
    //     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>::setPublishIPAddress(
        const std::string& ipAddress)
    {
        ARMARX_INFO << "Trying to set publisher socket";
        int publishPort = 5557; // TODO(@rietsch): Make configurable
        std::string pub_bind_address = "tcp://" + ipAddress + ":" + std::to_string(publishPort);
        try
        {
            std::unique_lock<std::mutex> lock(publishSocketMutex_);
            // publishSocket_.bind(pub_bind_address);
            publishSocket_.connect(pub_bind_address);
            publishSocketBound_.store(true);
            publishSocket_.set(zmq::sockopt::linger, 0);  // Important for socket to be non-blocking on deletion
            ARMARX_INFO << "Controller publish socket bound to " << pub_bind_address
                        << " through ZeroMQ";
        }
        catch (const zmq::error_t& e)
        {
            ARMARX_WARNING << "ZeroMQ publish bind error: " << e.what();
        }
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointZeroMQTaskspaceController<NJointTaskspaceController>::additionalTask()
    {
        zmq::message_t config_msg;
        auto received = socket.recv(config_msg, zmq::recv_flags::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
            }
 
            // If the sender send "publish_ip", the controller will send out robot data to that address
            if (config_json.contains("publish_ip") && !publishSocketBound_.load())
            {
                setPublishIPAddress(config_json["publish_ip"].get<std::string>());
                config_json.erase("publish_ip");
            }
 
            // 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);
        }
 
        // If there's no publisher socket, skip rest
        if (!publishSocketBound_.load())
        {
            return;
        }
 
        nlohmann::json stateJson;
        stateJson["timestamp"] =
            std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch())
                .count();
 
        auto mat4ToJson = [](const Eigen::Matrix4f& m)
        {
            nlohmann::json j = nlohmann::json::array();
            for (int r = 0; r < 4; ++r)
                for (int c = 0; c < 4; ++c)
                    j.push_back(m(r, c));
            return j;
        };
 
        // Extract limb data
        for (auto& pair : this->limb)
        {
            if (not pair.second->rtReady.load())
                continue;
            auto rtData = pair.second->bufferConfigRtToOnPublish.getUpToDateReadBuffer();
            auto rtStatus = pair.second->bufferRtStatusToOnPublish.getUpToDateReadBuffer();
            const auto& nodeSetName = pair.second->kinematicChainName;
            const auto& jointNames = pair.second->jointNames;
            auto jointPosition = rtStatus.jointPosition;
            auto jointVelocity = rtStatus.jointVelocity;
            auto jointTorque = rtStatus.jointTorque;
            auto desiredJointTorque = rtStatus.desiredJointTorque;
            auto desiredJointVelocity = rtStatus.desiredJointVelocity;
            auto desiredJointPosition = rtStatus.desiredJointPosition;
            Eigen::Matrix4f currentPose = rtStatus.currentPose;
            Eigen::Matrix4f desiredPose = rtStatus.desiredPose;
 
            for (size_t i = 0; i < jointNames.size(); ++i)
            {
                stateJson[nodeSetName]["jointPosition"][jointNames[i]] = jointPosition(i);
                stateJson[nodeSetName]["jointVelocity"][jointNames[i]] = jointVelocity(i);
                stateJson[nodeSetName]["jointTorque"][jointNames[i]] = jointTorque(i);
                stateJson[nodeSetName]["desiredJointTorque"][jointNames[i]] = desiredJointTorque(i);
                stateJson[nodeSetName]["desiredJointVelocity"][jointNames[i]] = desiredJointVelocity(i);
                stateJson[nodeSetName]["desiredJointPosition"][jointNames[i]] = desiredJointPosition(i);
            }
            stateJson[nodeSetName]["currentPose"] = mat4ToJson(currentPose);
            stateJson[nodeSetName]["desiredPose"] = mat4ToJson(desiredPose);
        }
 
        // Extract hand data
        if (this->hands)
        {
            for (auto& [handName, hand] : this->hands->hands)
            {
                const auto& jointNames = hand->jointNames;
                const auto desiredConfig =
                    hand->bufferConfigNonRTToOnPublish.getUpToDateReadBuffer();
                const auto currentJointPositionMap = hand->ctrl->getJointValuesMap();
 
                for (size_t i = 0; i < jointNames.size(); ++i)
                {
                    const auto it = currentJointPositionMap.find(jointNames[i]);
                    stateJson[handName]["jointPosition"][jointNames[i]] =
                        (it != currentJointPositionMap.end()) ? it->second : 0.F;
                    if (desiredConfig.jointPosition.has_value())
                    {
                        stateJson[handName]["desiredJointPosition"][jointNames[i]] =
                            desiredConfig.jointPosition.value()(i);
                    }
                }
            }
        }
 
        nlohmann::json metadata_json;
        metadata_json["name"] = "robot_state";
        metadata_json["type"] = "json";
        metadata_json["timestamp_processed"] = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        std::string metadata_string = metadata_json.dump();
 
        // Send data via ZMQ
        const std::string stateStr = stateJson.dump();
        zmq::message_t msg(stateStr.size());
        std::memcpy(msg.data(), stateStr.data(), stateStr.size());
        zmq::message_t metadata_msg(metadata_string.size());
        memcpy(metadata_msg.data(), metadata_string.data(), metadata_string.size());
        try
        {
            std::unique_lock<std::mutex> lock(publishSocketMutex_);
            publishSocket_.send(metadata_msg, zmq::send_flags::sndmore);
            publishSocket_.send(msg, zmq::send_flags::dontwait);
        }
        catch (const zmq::error_t& e)
        {
            ARMARX_RT_LOGF_WARN("ZeroMQ joint state publish error: %s", e.what());
        }
    }
 
    template <typename NJointTaskspaceController>
    void NJointZeroMQTaskspaceController<NJointTaskspaceController>::onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
    {
        if (publishSocketBound_.load())
        {
            std::unique_lock<std::mutex> lock(publishSocketMutex_);
            publishSocket_.close();
            publishSocketBound_.store(false);
        }
    }
 
} // namespace armarx::control::njoint_controller::task_space