opcua_client.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <signal.h>

#include <opcua_helpers.h>

#include "ros_opcua_msgs/Address.h"
#include "ros_opcua_msgs/TypeValue.h"

#include "ros_opcua_srvs/CallMethod.h"
#include "ros_opcua_srvs/Connect.h"
#include "ros_opcua_srvs/Disconnect.h"
#include "ros_opcua_srvs/ListNode.h"
#include "ros_opcua_srvs/Read.h"
#include "ros_opcua_srvs/Subscribe.h"
#include "ros_opcua_srvs/Write.h"
#include "ros_opcua_srvs/Unsubscribe.h"

#include <opc/ua/client/client.h>
#include <opc/ua/subscription.h>

OpcUa::UaClient _client(false);

std::map<std::string, std::unique_ptr<OpcUa::Subscription>> _subscriptions;

std::map<std::string, ros::Publisher> _callback_publishers;

std::map<std::string, uint32_t> _subscription_handles;
bool _connected = false;


class SubClient : public OpcUa::SubscriptionHandler
{
  void DataChange(uint32_t handle, const OpcUa::Node& node, const
OpcUa::Variant& value, OpcUa::AttributeId attr) override
  {
    ROS_DEBUG("Callback....");
    _callback_publishers[OpcUa::ToString(node.GetId())].publish(convertVariantToTypeValue(value));
  }
};


SubClient _sclt;

bool connect(ros_opcua_srvs::Connect::Request &req, ros_opcua_srvs::Connect::Response &res)
{
    // TODO: set connect status

    ROS_DEBUG("Establishing connection to OPC-UA server on address: %s", req.endpoint.c_str());
    try {
        _client.Connect(req.endpoint);
        ROS_INFO("Connection to OPC-UA server on address '%s' established!", req.endpoint.c_str());
        res.success = true;
        _connected = true;
    }
    catch (const std::exception& exc){
        ROS_ERROR("OPC-UA client node %s: Connection to OPC-UA server on address %s failed! Message: %s", ros::this_node::getName().c_str(), req.endpoint.c_str(), exc.what());

        res.success = false;
        char err_string[100];
        sprintf(err_string, "Connection to OPC-UA server on address %s failed! Message: %s", req.endpoint.c_str(), exc.what());
        res.error_message = err_string;
    }
    catch (...) {
        ROS_ERROR("Connection to OPC-UA server on address %s failed with unknown exception", req.endpoint.c_str());
        res.success = false;
        res.error_message = "'Connect' service failed with Unknown exception";
    }
    return true;
}

bool disconnect(ros_opcua_srvs::Disconnect::Request &req, ros_opcua_srvs::Disconnect::Response &res)
{
    ROS_DEBUG("Disconnecting from OPC-UA server...");
    try {
        _client.Disconnect();
        ROS_INFO("Disconnection succeded!");
        _connected = false;
        res.success = true;
    }
    catch (const std::exception& exc){
        ROS_ERROR("OPC-UA client node %s: Disconnection failed! (maybe client was not connected before?). Message: %s", ros::this_node::getName().c_str(), exc.what());

        res.success = false;
        char err_string[100];
        sprintf(err_string, "Dissconect service failed with exception: %s", exc.what());
        res.error_message = err_string;
    }
    catch (...) {
        ROS_ERROR("'Disconnect' service failed with unknown exception");
        res.success = false;
        res.error_message = "'Disconnect' service failed with unknown exception";
    }
    return true;
}

bool list_node(ros_opcua_srvs::ListNode::Request &req, ros_opcua_srvs::ListNode::Response &res)
{
    ROS_DEBUG("OPC-UA client node %s: 'ListNode' service called with node Id: %s parameters", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

    OpcUa::Node node;
    try {
      if (req.node.nodeId == "") {
          node = _client.GetObjectsNode();
      }
      else {
        node = _client.GetNode(req.node.nodeId);
      }

      for (OpcUa::Node child : node.GetChildren()){
          ros_opcua_msgs::Address address;
          address.nodeId = OpcUa::ToString(child.GetId());
          address.qualifiedName = child.GetBrowseName().Name;
          res.children.push_back(address);
      }
      res.success = true;
    }
    catch (const std::exception& exc){
            ROS_ERROR("OPC-UA client node %s: 'ListNode' service called with node Id: %s, parameters failed! Exception: %s", ros::this_node::getName().c_str(), req.node.nodeId.c_str(), exc.what());

            res.success = false;
            char err_string[100];
            sprintf(err_string, "ListNode service failed with exception: %s", exc.what());
            res.error_message = err_string;
        }
        catch (...)
        {
            ROS_ERROR("OPC-UA client node %s: 'ListNode' service called with node Id: %s parameters failed! Unknown exception!", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

            res.success = false;
            res.error_message = "ListNode service failed with Unknown exception";
        }
        return true;
}

bool call_method(ros_opcua_srvs::CallMethod::Request &req, ros_opcua_srvs::CallMethod::Response &res)
{
    ROS_DEBUG("OPC-UA client node %s: 'CallMethod' service called with node Id: %s and %s parameters", ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.method.nodeId.c_str());

    try {
        OpcUa::Node node = _client.GetNode(req.node.nodeId);
        OpcUa::NodeId method_id = OpcUa::ToNodeId(req.method.nodeId);
        std::vector<OpcUa::Variant> inputArguments;

        for (ros_opcua_msgs::TypeValue typeValue : req.data) {
            std::cout << "Variant: " <<
convertTypeValueToVariant(typeValue).ToString() << std::endl;
            inputArguments.push_back(convertTypeValueToVariant(typeValue));
        }

        std::vector<OpcUa::Variant> outputArguments = node.CallMethod(method_id, inputArguments);

        for (OpcUa::Variant variant : outputArguments) {
            res.data.push_back(convertVariantToTypeValue(variant));
        }

        res.success = true;

    }
    catch (const std::exception& exc){
        ROS_ERROR("OPC-UA client node %s: 'CallMethod' service called with node Id: %s and %s parameters failed! Exception: %s", ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.method.nodeId.c_str(), exc.what());

        res.success = false;
        char err_string[100];
        sprintf(err_string, "Call method service failed with exception: %s", exc.what());
        res.error_message = err_string;
    }
    catch (...)
    {
        ROS_ERROR("OPC-UA client node %s: 'CallMethod' service called with node Id: %s parameters failed! Unknown exception!", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

        res.success = false;
        res.error_message = "Call method service failed with Unknown exception";
    }
    return true;
}

bool read(ros_opcua_srvs::Read::Request &req, ros_opcua_srvs::Read::Response &res)
{
    ROS_DEBUG("OPC-UA client node %s: 'Read' service called with node Id: %s parameters", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

    try {
        OpcUa::Node variable = _client.GetNode(req.node.nodeId);

        res.success = true;
        res.data = convertVariantToTypeValue(variable.GetValue());

        if (res.data.type == "Unknown") {
            res.success = false;
            char err_string[100];
            sprintf(err_string, "Unknon data type!!");
            res.error_message = err_string;
            ROS_DEBUG("Reading failed!");
        }
    }
    catch (const std::exception& exc){
        ROS_ERROR("OPC-UA client node %s: 'Read' service called with node Id: %s, parameters failed! Exception: %s", ros::this_node::getName().c_str(), req.node.nodeId.c_str(), exc.what());

        res.success = false;
        char err_string[100];
        sprintf(err_string, "Read service failed with exception: %s", exc.what());
        res.error_message = err_string;
    }
    catch (...)
    {
        ROS_ERROR("OPC-UA client node %s: 'Read' service called with node Id: %s parameters failed! Unknown exception!", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

        res.success = false;
        res.error_message = "Read service failed with Unknown exception";
    }
    return true;
}

bool write(ros_opcua_srvs::Write::Request &req, ros_opcua_srvs::Write::Response &res)
{
    ROS_DEBUG("OPC-UA client node %s: 'Write' service called with node Id: %s, parameters", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

    try {
        OpcUa::Node variable = _client.GetNode(req.node.nodeId);

        OpcUa::Variant variant = convertTypeValueToVariant(req.data);

        if (!variant.IsNul()) {
//           if (variant.Type() == OpcUa::VariantType::STRING) {
//             if (((std::string)variant).length() == 0) {
//               variant = std::string(" ");
//             }
//           }
          variable.SetValue(variant);
          res.success = true;
        }
        else {
            res.success = false;
            char err_string[100];
            sprintf(err_string, "Unknon data type %s", req.data.type.c_str());
            res.error_message = err_string;
            ROS_DEBUG("Writing failed!");
        }
    }
    catch (const std::exception& exc){
        ROS_ERROR("OPC-UA client node %s: 'Write' service called with node Id: %s, type: '%s' parameters failed! Exception: %s", ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.data.type.c_str(), exc.what());

        res.success = false;
        char err_string[100];
        sprintf(err_string, "Write service failed with exception: %s", exc.what());
        res.error_message = err_string;
    }
    catch (...)
    {
        ROS_ERROR("OPC-UA client node %s: 'Write' service called with node Id: %s, type: '%s' parameters failed! Unknown exception!", ros::this_node::getName().c_str(), req.node.nodeId.c_str(), req.data.type.c_str());

        res.success = false;
        res.error_message = "Write service failed with Unknown exception";
    }

    return true;
}

bool subscribe(ros_opcua_srvs::Subscribe::Request &req, ros_opcua_srvs::Subscribe::Response &res)
{
    ROS_DEBUG("OPC-UA client node %s: 'Subscribe' service called with node Id: %s parameters", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

    try {
        // TODO: Check if already subscribed to node

        OpcUa::Node variable = _client.GetNode(req.node.nodeId);
        std::string node_string = OpcUa::ToString(variable.GetId());

        ros::NodeHandle nodeHandle("~");
        _callback_publishers[node_string] =  nodeHandle.advertise<ros_opcua_msgs::TypeValue>(req.callback_topic, 1);

        _subscriptions[node_string] = _client.CreateSubscription(100, _sclt);
        _subscription_handles[node_string] =  _subscriptions[node_string]->SubscribeDataChange(variable);

        ROS_INFO("Node successfully subscribed!");
        res.success = true;
    }
    catch (const std::exception& exc){
        ROS_ERROR("OPC-UA client node %s: 'Subscribe' service called with node Id: %s parameters failed!", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

        res.success = false;
        char err_string[100];
        sprintf(err_string, "Subscribe service failed with exception: %s", exc.what());
        res.error_message = err_string;
    }
    catch (...)
    {
        ROS_ERROR("OPC-UA client node %s: 'Subscribe' service called with node Id: %s parameters failed!", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

        res.success = false;
        res.error_message = "Subscribe service failed with Unknown exception";
    }

    return true;
}

bool unsubscribe(ros_opcua_srvs::Unsubscribe::Request &req, ros_opcua_srvs::Unsubscribe::Response &res)
{
    ROS_DEBUG("OPC-UA client node %s: 'Unsubscribe' service called with node Id: %s parameters", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

    try {
        // TODO: Check if already subscribed to node

        // TODO: Why is here return status false even the subscription was ok?

        OpcUa::Node variable = _client.GetNode(req.node.nodeId);
        std::string node_string = OpcUa::ToString(variable.GetId());

        _subscriptions[node_string]->UnSubscribe(_subscription_handles[node_string]);
        _callback_publishers[node_string].shutdown();

        _subscriptions.erase(node_string);
        _subscription_handles.erase(node_string);
        _callback_publishers.erase(node_string);

        ROS_INFO("Node successfully unsubscribed!");
        res.success = true;
    }
    catch (const std::exception& exc){
       ROS_ERROR("OPC-UA client node %s: 'Unsubscribe' service called with node Id: %s parameters failed!", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

        res.success = false;
        char err_string[100];
        sprintf(err_string, "Unsubscribe service failed with exception: %s", exc.what());
        res.error_message = err_string;
    }
    catch (...)
    {
        ROS_ERROR("OPC-UA client node %s: 'Unsubscribe' service called with node Id: %s parameters failed!", ros::this_node::getName().c_str(), req.node.nodeId.c_str());

        res.success = false;
        res.error_message = "Unsubscribe service failed with Unknown exception";
    }

    return true;
}

void on_shutdown(int sig)
{
  _client.Disconnect();

  ros::shutdown();
}

int main (int argc, char** argv)
{
    ros::init(argc, argv, "opcua_client_node");
    ros::NodeHandle nodeHandle("~");
    signal(SIGINT, on_shutdown);

    ros::ServiceServer connect_service = nodeHandle.advertiseService("connect", connect);
    ROS_DEBUG("OPC-UA client node %s: 'Connect' service available on on: %s", ros::this_node::getName().c_str(), connect_service.getService().c_str());
    ros::ServiceServer disconnect_service = nodeHandle.advertiseService("disconnect", disconnect);
    ROS_DEBUG("OPC-UA client node %s: 'Disconnect' service available on: %s", ros::this_node::getName().c_str(), disconnect_service.getService().c_str());

    // List node
    ros::ServiceServer list_node_service = nodeHandle.advertiseService("list_node", list_node);
    ROS_DEBUG("OPC-UA client node %s: 'ListNode' service available on on: %s", ros::this_node::getName().c_str(), connect_service.getService().c_str());

     // Method Call
     ros::ServiceServer call_method_service = nodeHandle.advertiseService("call_method", call_method);
     ROS_DEBUG("OPC-UA client node %s: 'CallMethod' service available on: %s", ros::this_node::getName().c_str(), call_method_service.getService().c_str());

    // Reading of data
    ros::ServiceServer read_service = nodeHandle.advertiseService("read", read);
    ROS_DEBUG("OPC-UA client node %s: 'Read' service available on: %s", ros::this_node::getName().c_str(), read_service.getService().c_str());

    // Writing of data
    ros::ServiceServer write_service = nodeHandle.advertiseService("write", write);
    ROS_DEBUG("OPC-UA client node %s: 'Write' service available on: %s", ros::this_node::getName().c_str(), write_service.getService().c_str());

    // Subscriptions
    ros::ServiceServer subscribe_service = nodeHandle.advertiseService("subscribe", subscribe);
    ROS_DEBUG("OPC-UA client node %s: 'Subscribe' service available on: %s", ros::this_node::getName().c_str(), subscribe_service.getService().c_str());
    ros::ServiceServer unsubscribe_service = nodeHandle.advertiseService("unsubscribe", unsubscribe);
    ROS_DEBUG("OPC-UA client node %s: 'Unsubscribe' service available on: %s", ros::this_node::getName().c_str(), unsubscribe_service.getService().c_str());


    ROS_INFO("OPCUA client node: %s is ready!", ros::this_node::getName().c_str());

    ros::spin();

    return 0;
}