Commit 1d846c06 authored by Adkins, Cameron's avatar Adkins, Cameron
Browse files

wip: callback is hopefully in order

parent 7ad855ba

include/opcua_bridge/node.hpp

+1 −0
Original line number Diff line number Diff line
@@ -30,6 +30,7 @@ namespace obridge { 
        struct OpcuaData {

            std::shared_ptr<opcua::Node<opcua::Client>> node;

            rclcpp::PublisherBase::SharedPtr publisher;

            std::function<void(opcua::Variant)> callback;

        };



        std::map<std::string, OpcuaData> m_publishers;

src/node.cpp

+47 −28
Original line number Diff line number Diff line
@@ -44,12 +44,10 @@ namespace obridge { 


            m_logger.info("Established connection to {}:{}", host, port);

        }

    }



    auto OpcuaNode::poll() -> void {

        int space = this->get_parameter("bridge.namespace").as_int();



        auto read_value = [&]<typename value_t>(int space, std::string object) -> void {

        auto subscribe = [&]<typename value_t>(int space, std::string object) -> void {

            if (!m_publishers.contains(object)) {

                m_publishers[object].publisher = this->create_publisher<value_t>(

                    fmt::format("opcua/{}", object),
@@ -62,38 +60,59 @@ namespace obridge { 
                        object

                    }

                );

            }



            auto& node = m_publishers[object].node;



            value_t value = node->readValue().to<value_t>();

                m_publishers[object].callback = [&](opcua::Variant variant) -> void {

                    value_t value = variant.to<value_t>();



            m_logger.debug("{}:{} - {}", space, object, value);

                    m_logger.debug("read: {} - {}", object, value);



                    rclcpp::PublisherBase::SharedPtr pub_base = m_publishers[object].publisher;

                    auto pub = std::static_pointer_cast<rclcpp::Publisher<value_t>>(pub_base);



                    pub->publish(value);

                };

            }



            m_logger.info("- {}", object);

        };



        m_logger.info("Subscribed to:");



        subscribe.template operator()<float>(space, "ACTUAL_CURRENT");

        subscribe.template operator()<float>(space, "ACTUAL_GASFLOW");

        subscribe.template operator()<float>(space, "ACTUAL_POWER");

        subscribe.template operator()<float>(space, "ACTUAL_VOLTAGE");

        subscribe.template operator()<float>(space, "ACTUAL_WELDINGTIME");

        subscribe.template operator()<float>(space, "ACTUAL_WFS");

        subscribe.template operator()<float>(space, "DISPLAY_CURRENT");

        subscribe.template operator()<float>(space, "DISPLAY_ENERGY");

        subscribe.template operator()<float>(space, "DISPLAY_POWER");

        subscribe.template operator()<float>(space, "DISPLAY_VOLTAGE");

        subscribe.template operator()<float>(space, "DISPLAY_WFS");

        subscribe.template operator()<float>(space, "WIREBUFFER_VALUE");

        subscribe.template operator()<float>(space, "ERROR");



        read_value.template operator()<float>(space, "ACTUAL_CURRENT");

        read_value.template operator()<float>(space, "ACTUAL_GASFLOW");

        read_value.template operator()<float>(space, "ACTUAL_POWER");

        read_value.template operator()<float>(space, "ACTUAL_VOLTAGE");

        read_value.template operator()<float>(space, "ACTUAL_WELDINGTIME");

        read_value.template operator()<float>(space, "ACTUAL_WFS");

        read_value.template operator()<float>(space, "DISPLAY_CURRENT");

        read_value.template operator()<float>(space, "DISPLAY_ENERGY");

        read_value.template operator()<float>(space, "DISPLAY_POWER");

        read_value.template operator()<float>(space, "DISPLAY_VOLTAGE");

        read_value.template operator()<float>(space, "DISPLAY_WFS");

        read_value.template operator()<float>(space, "WIREBUFFER_VALUE");

        read_value.template operator()<float>(space, "ERROR");



        read_value.template operator()<bool>(space, "PROCESS_ACTIVE");



        read_value.template operator()<std::string>(space, "FIRMWAREVERSION");

        read_value.template operator()<std::string>(space, "SERIALNUMBER");

        subscribe.template operator()<bool>(space, "PROCESS_ACTIVE");



        subscribe.template operator()<std::string>(space, "FIRMWAREVERSION");

        subscribe.template operator()<std::string>(space, "SERIALNUMBER");



    }



    auto OpcuaNode::poll() -> void {

        std::vector<opcua::ReadValueId> request;

        std::vector<std::function<void(opcua::Variant)>> callbacks;



        for (auto& [name, data] : m_publishers) {

            request.emplace_back(data.node->id(), opcua::AttributeId::Value);

            callbacks.push_back(data.callback);

        }



        auto response = opcua::services::read(m_client, request, opcua::TimestampsToReturn::Server);



        for (int i = 0; auto& data : response.results()) {

            callbacks[i](data.value());

            i++;

        }

    }

}