ZenohController.cpp

Go to the documentation of this file.

#include "ZenohController.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/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.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/aron/Aron.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 <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.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 <zenoh_constants.h>
 
namespace armarx::control::njoint_controller::task_space
{
    namespace base = armarx::control::njoint_controller::task_space;
 
    armarx::NJointControllerRegistration<
        NJointZenohTaskspaceController<base::NJointTaskspaceVelocityController>>
        registrationControllerNJointTSVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZenohTSVel));
 
    armarx::NJointControllerRegistration<
        NJointZenohTaskspaceController<base::NJointTaskspaceImpedanceController>>
        registrationControllerNJointTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZenohTSImp));
 
    armarx::NJointControllerRegistration<
        NJointZenohTaskspaceController<base::NJointTaskspaceMixedImpedanceVelocityController>>
        registrationControllerNJointTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZenohTSMixImpVel));
 
    armarx::NJointControllerRegistration<
        NJointZenohTaskspaceController<base::NJointTaskspaceCollisionAvoidanceImpedanceController>>
        registrationControllerNJointCollisionAvoidanceTSImpedanceController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZenohTSImpCol));
 
    armarx::NJointControllerRegistration<NJointZenohTaskspaceController<
        base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController>>
        registrationControllerNJointCollisionAvoidanceTSMixedImpedanceVelocityController(
            armarx::control::common::ControllerTypeNames.to_name(
                armarx::control::common::ControllerType::ZenohTSMixImpVelCol));
 
    // Implement getClassName() method for templated NJointZenohTaskspaceController
    template <>
    std::string
    NJointZenohTaskspaceController<class base::NJointTaskspaceVelocityController>::getClassName(
        const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZenohTSVel);
    }
 
    template <>
    std::string
    NJointZenohTaskspaceController<class base::NJointTaskspaceImpedanceController>::getClassName(
        const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZenohTSImp);
    }
 
    template <>
    std::string
    NJointZenohTaskspaceController<class base::NJointTaskspaceMixedImpedanceVelocityController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZenohTSMixImpVel);
    }
 
    template <>
    std::string
    NJointZenohTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceImpedanceController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZenohTSImpCol);
    }
 
    template <>
    std::string
    NJointZenohTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController>::
        getClassName(const Ice::Current&) const
    {
        return armarx::control::common::ControllerTypeNames.to_name(
            armarx::control::common::ControllerType::ZenohTSMixImpVelCol);
    }
 
    template <typename NJointTaskspaceController>
    NJointZenohTaskspaceController<NJointTaskspaceController>::NJointZenohTaskspaceController(
        const armarx::RobotUnitPtr& robotUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr& robot) :
        NJointTaskspaceController{robotUnit, config, robot}
    {
        init();
    }
 
    template <>
    NJointZenohTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceImpedanceController>::
        NJointZenohTaskspaceController(const armarx::RobotUnitPtr& robotUnit,
                                       const armarx::NJointControllerConfigPtr& config,
                                       const VirtualRobot::RobotPtr& robot) :
        base::NJointTaskspaceImpedanceController{robotUnit, config, robot},
        base::NJointTaskspaceCollisionAvoidanceImpedanceController{robotUnit, config, robot}
    {
        init();
    }
 
    template <>
    NJointZenohTaskspaceController<
        class base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController>::
        NJointZenohTaskspaceController(const armarx::RobotUnitPtr& robotUnit,
                                       const armarx::NJointControllerConfigPtr& config,
                                       const VirtualRobot::RobotPtr& robot) :
        base::NJointTaskspaceMixedImpedanceVelocityController{robotUnit, config, robot},
        base::NJointTaskspaceCollisionAvoidanceMixedImpedanceVelocityController{robotUnit,
                                                                                config,
                                                                                robot}
    {
        init();
    }
 
    // Method(s) of NJointZenohTaskspaceController in general
    template <typename NJointTaskspaceController>
    void
    NJointZenohTaskspaceController<NJointTaskspaceController>::init()
    {
        ARMARX_TRACE;
        auto config = zenoh::Config::create_default();
        //        config.insert_json5(zenoh::Z_CONFIG_CONNECT_KEY, "[\"" + _connect_endpoint + "\"]");
        config.insert_json5("connect/endpoints", "[\"" + _connect_endpoint + "\"]");
 
 
        _session = std::make_unique<zenoh::Session>(zenoh::Session::open(std::move(config)));
        ARMARX_CHECK(_session) << "Failed to create Zenoh session unique pointer";
 
 
        ARMARX_TRACE;
        ARMARX_INFO << "setting up publisher and subscriber";
 
        _publisher = std::make_unique<zenoh::Publisher>(
            _session->declare_publisher(zenoh::KeyExpr(_state_key)));
 
        auto queryable_expr = zenoh::KeyExpr(_config_key);
        auto on_drop_queryable = []() { ARMARX_INFO << "Destroying queryable"; };
        zenoh::Session::QueryableOptions opts;
        opts.complete = true;
        auto queryable = _session->declare_queryable(
            queryable_expr,
            [&queryable_expr](const zenoh::Query& query)
            {
                ARMARX_INFO << "Received Query '" << query.get_keyexpr().as_string_view();
                query.reply(queryable_expr, zenoh::Bytes("42"));
            },
            std::move(on_drop_queryable),
            std::move(opts));
        //        _subscriber = std::make_unique<zenoh::Subscriber>(_session->declare_subscriber(
        //            _config_key, [this](const zenoh::Sample& s) { this->updateConfigZenoh(s); }));
 
        ARMARX_INFO << "Zenoh session and endpoints initialized";
        time_last_packet = std::chrono::steady_clock::now();
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointZenohTaskspaceController<NJointTaskspaceController>::additionalTask()
    {
        auto [rtTargetSafe, ftSafe] = this->additionalTaskUpdateStatus();
 
        auto sendTime = std::chrono::duration_cast<std::chrono::microseconds>(
                            std::chrono::system_clock::now().time_since_epoch())
                            .count();
        nlohmann::json j;
 
        j["timestamp"] = static_cast<double>(sendTime) / 1e6;
        j["msg"] = R"({"matrix":[[1,0],[0,1]],"float":3.14,"string":"hello","bool":true})";
        //        std::string msg = R"({"matrix":[[1,0],[0,1]],"float":3.14,"string":"hello","bool":true})";
 
        for (auto& pair : this->limb)
        {
            this->userConfig.limbs.at(pair.first) = pair.second->nonRtConfig;
        }
        armarx::aron::data::writer::NlohmannJSONWriter writer;
        auto json_msg = this->userConfig.write(writer);
 
        ARMARX_TRACE;
        ARMARX_INFO << "convert json";
        armarx::armem::gui::instance::TreeTypedJSONConverter conv;
        ARMARX_INFO << this->limb.at("LeftArm")->nonRtConfig.desiredNullspaceJointAngles.value();
 
        try
        {
            armarx::aron::data::visitRecursive(conv,
                                               this->limb.at("LeftArm")->nonRtConfig.toAron(),
                                               this->limb.at("LeftArm")->nonRtConfig.ToAronType());
        }
        catch (const aron::error::AronException& e)
        {
            ARMARX_WARNING << "In controller additional task, failed to Aron data to JSON: "
                           << e.getReason();
        }
        ARMARX_INFO << "dump json";
        _publisher->put(conv.getJSON().dump(2));
 
        //        std::string msg = j.dump();
        //        _publisher->put(json_msg.dump());
 
        //        Zenoh::message_t config_msg;
        //        bool received = socket.recv(&config_msg, Zenoh_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);
        //            for (auto& pair : this->limb)
        //            {
        //                auto limbName = pair.first;
        //                auto& arm = pair.second;
 
        //                auto limbConfigNew = receivedConfig.limbs.at(limbName);
        //                arm->nonRtConfig.desiredPose =
        //                    limbConfigNew.desiredPose; // Update desiredPose with new configuration
        //                // TODO: Other config parameters can be updated here as needed
        //                arm->bufferConfigUserToNonRt.getWriteBuffer() =
        //                    arm->nonRtConfig; // Overwrite user config
        //                arm->bufferConfigUserToNonRt.commitWrite();
        //            }
 
        //            // Update hands
        //            if (this->hands)
        //            {
        //                this->hands->updateConfig(receivedConfig.hands);
        //            }
        //        }
 
        this->additionalTaskSetTarget();
        if (not rtTargetSafe)
        {
            this->handleRTNotSafeInNonRT();
        }
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointZenohTaskspaceController<NJointTaskspaceController>::updateConfigZenoh(
        const zenoh::Sample& sample)
    {
 
        std::string payload = sample.get_payload().as_string();
        _config_json = payload;
        ARMARX_INFO << "[CONFIG] Updated: " << _config_json;
    }
 
    template <typename NJointTaskspaceController>
    void
    NJointZenohTaskspaceController<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