diff --git a/.gitignore b/.gitignore index e69de29..6380766 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1,6 @@ +*~ +*.pyc +.idea +venv +ros_opcua_impl_python_opcua/scripts/*.xml +ros_opcua_impl_python_opcua/export/*.xml diff --git a/.travis.before.bash b/.travis.before.bash deleted file mode 100755 index 76b35a7..0000000 --- a/.travis.before.bash +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash - -git submodule update --init -export PYTHONPATH=$PWD/ros_opcua_impl_python_opcua/python-opcua:$PYTHONPATH diff --git a/.travis.yml b/.travis.yml index 5f433d1..07c2b02 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,7 +13,6 @@ env: - export PYTHONPATH=$PWD/ros_opcua_impl_python_opcua/python-opcua:$PYTHONPATH matrix: - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu -# - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: diff --git a/README.md b/README.md index c61b53c..7e6582a 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,15 @@ Clone the repository with: git clone --recursive https://github.com/iirob/ros_opcua_communication.git ``` + +## Getting started + +Please check readme files of the packages: + +* [ros_opcua_impl_freeopcua](ros_opcua_impl_freeopcua/README.md) +* [ros_opcua_impl_python_opcua](ros_opcua_impl_python_opcua/README.md) + + ## ROS Distro Support | | Indigo | Kinetic | @@ -13,3 +22,4 @@ git clone --recursive https://github.com/iirob/ros_opcua_communication.git | Branch | [`indigo-devel`](https://github.com/iirob/ros_opcua_communication/tree/indigo-devel) | [`kinetic_devel`](https://github.com/iirob/ros_opcua_communication/tree/kinetic-devel) | | Status | [![Build Status](https://travis-ci.org/iirob/ros_opcua_communication.svg?branch=indigo-devel)](https://travis-ci.org/iirob/ros_opcua_communication) | [![Build Status](https://travis-ci.org/iirob/ros_opcua_communication.svg?branch=kinetic-devel)](https://travis-ci.org/iirob/ros_opcua_communication) | | Version | [version](http://repositories.ros.org/status_page/ros_indigo_default.html?q=ros_opcua_communication) | [version](http://repositories.ros.org/status_page/ros_kinetic_default.html?q=ros_opcua_communication) | + diff --git a/ros_opcua_impl_python_opcua/README.md b/ros_opcua_impl_python_opcua/README.md new file mode 100644 index 0000000..f058be3 --- /dev/null +++ b/ros_opcua_impl_python_opcua/README.md @@ -0,0 +1,111 @@ +# ros_opcua_impl_python_opcua + +This package provides a python implementation of OPC UA server, which models ROS in OPC UA protocol. The server provides access to all ROS topics, services, actions, parameters and nodes, besides an OPC UA client is provided to explore the contents of the server. + +## Getting started with ros_opcua_impl_python_opcua package + +### Start up + +1. After cloning the package locally, an initial build is needed. As a standard ROS package, of course `catkin_make` is the ideal tool to do this, if you are new to ROS, please check [here](http://wiki.ros.org/ROS/Tutorials/BuildingPackages) to figure out how to build the package. After building, please do not forget to **source the package**. + +2. Start the server (the starting will take some time, wait until you see the output `server started`) using: + +``` +roslaunch ros_opcua_impl_python_opcua rosopcua.launch +``` + +Then a ROS node named `rosopcua` could be seen in ROS, this node takes related ROS contents and models them in OPC UA protocol. + +3. Run some ROS Nodes to test, they should afterward visible in the server. For example one can start `turtlesim_node`: + +``` +rosrun turtlesim turtlesim_node +``` + +4. Start the UaExpert as OPC UA client and connect to the server, if you have not installed it locally, please check [here](https://www.unified-automation.com/products/development-tools/uaexpert.html) to download it. The initial address you should type in UaExpert is `opc.tcp://0.0.0.0:21554/ROSServer`, afterward if you want to test with server and client located in different computers, this address can be found in `ros_opcua_impl_python_opcua/basic_server_client.py` under the variant `server_end_point`. + +After a successful connection, all ROS topics, services, actions, parameters and nodes mapped to the OPC UA can be seen. + +### Basic operations + +1. Call a service. To move the turtle from example, choose `Objects->Services->turtle1/teleport_relative`, right click and choose call. Enter the new position (for instance 1, 1) of the turtle and then the turtle should move. + +2. Interact with topics. In `Objects->Topics->turtle1/pose` one can follow the position of the turtle in real time. To check the full effect of this try to move turtle using [Robot Steering](https://wiki.ros.org/rqt_robot_steering) rqt-Plugin, or run: + +``` +rosrun turtlesim turtle_teleop_key +``` + +and move the turtle there. To publish messages under a ROS topic, you may choose `Objects->Topics->Topic publish->/turtle1/color_sensor`, call the method and the background color should be changed. + +3. ROS msgs and srvs. They are modeled internally and linked under `Types->DataTypes->ROSDataType`. + +## Rough project structure + +Below is an overview of the folders and files in this project: + ++ config, include the configuration file, will be loaded while launching the server node, a more specific explanation can be found in chapter "Configurations". ++ export, OPC UA nodes could be exported there, not interested for using ++ launch, includes 3 launch files ++ python-opcua, the foundation framework of this project ++ scripts, the main code folder of this project ++ known_issues.md, records the solved and unsolved problems of this project + +... + +## Interacting with the OPC UA server without `UaExpert` + +`scripts/ros_opc_ua_client.py` is a ROS implementation of OPC UA client, which allows to connect with any OPC UA server, to start it, run: + +``` +roslaunch ros_opcua_impl_python_opcua opcuaclient.launch +``` + +After this 8 different services available under the ROS node `opcuaclient` can be seen, they can be called with `rosservice` (please **first call connect** service before calling other services) or more leisurely, with python scripts, there are two examples in file `scripts/opcua_client_application_example.py` and `scripts/opcua_client_application_example_ros.py` respectively. + +## Configurations + +Some parameters are designed in the file `config/params.yaml`, which supports advanced functions, below is a list of them: + +| Parameter name | Default value| Comment| +| :---: | :---: |:----------------| +|namespace|'/'|filters of the ros topics, services, actions, parameters and nodes, default '/' means no filter| +|automatic_refresh|true|True will start automatic refresh of ros nodes, otherwise the user should call refresh manually| +|refresh_cycle_time|0.5|The cycle time of the automatic refresh, the time unit is second| +|show_nodes|true|Switch for masking the display of ROS nodes| +|show_topics|true|Switch for masking the display of ROS topics| +|show_services|true|Switch for masking the display of ROS services| +|show_params|true|Switch for masking the display of ROS parameters| +|import_xml_msgs|false|Controls if data types will be imported from an xml file or generate on-the-fly, its address is written in `/scripts/ros_opc_ua_comm.py` under the variable `message_export_path`, the nodes to be imported should be generated with the script `scripts/ros_server_export_message.py`, do not set it to true until the bug mentioned there is fixed| +|parameter_writable|true|Controls if the variables in parameters are writable| + +## For developers + +### Project hierarchy + +The project is organized in a 5 layer hierarchy: + +| Layer | Correspondent files | Comment| +| :---: | :---: |:----------------| +|Application|`scripts/opcua_client_application_example.py`, `scripts/opcua_client_application_example_ros.py`|possible application examples| +|Top|`scripts/ros_server.py`, `scripts/ros_server_export_message.py`, `scripts/ros_opc_ua_client.py`|built OPC UA server, manage ROS information with different information managers| +|High|`scripts/ros_info_manage.py`|created ROS information managers to manage the ROS services set, topic set, and ROS node set| +|Middle|`scripts/ros_opc_ua_comm.py` other classes|build the communication proxy of ROS topics, services based on the UA objects| +|Low|`scripts/ros_opc_ua_comm.py` class `OpcUaROSMessage`|created correspondent OPC UA extension object of rosmsg, rossrv| +|Bottom|`python-opcua/opcua/common/type_dictionary_buider.py`| extension object support in python-opcua| + +### Msgs and Srvs used in `scripts/ros_opc_ua_client.py` + +They can be found under the root folder under `ros_opcua_communication/ros_opcua_msgs` and `ros_opcua_communication/ros_opcua_srvs`. + +The msg `TypeValue.msg` is designed for transferring arbitrary data structure between ROS and OPC UA in ROS world, to transfer an ROS object as data, the name of the object should be written in `type` string, the complete object should be serialized and written in the string field `string_d`. Please check `scripts/opcua_client_application_example_ros.py` to figure out detailed usage. + +### Tricks that may help + +Inheriting the two classes in `scripts/basic_server_client.py` can save time for further developing. + +To implement filters in static modeling, it would be better to modify the low layer; in dynamic modeling, it would be better to modify the class `ROSInfoAgent` in the high layer, so that the server does not "see" a specific node, topic, service or parameter. + +### Under the hood + +Please check [this file](https://github.com/iirob/ros_opcua_communication/blob/interactive_ros/ros_opcua_impl_python_opcua/ROS-OPC%20UA%20Connector%203.0.pdf) for detailed information. diff --git a/ros_opcua_impl_python_opcua/ROS-OPC UA Connector 3.0.pdf b/ros_opcua_impl_python_opcua/ROS-OPC UA Connector 3.0.pdf new file mode 100644 index 0000000..d18c14e Binary files /dev/null and b/ros_opcua_impl_python_opcua/ROS-OPC UA Connector 3.0.pdf differ diff --git a/ros_opcua_impl_python_opcua/config/params.yaml b/ros_opcua_impl_python_opcua/config/params.yaml index 2d3157e..0ec3733 100644 --- a/ros_opcua_impl_python_opcua/config/params.yaml +++ b/ros_opcua_impl_python_opcua/config/params.yaml @@ -1 +1,23 @@ +# endpoint of the server +end_point: 'opc.tcp://141.3.80.78:18801/ROSServer' +# range of the ros topics, services and actions, default '/' means no filter namespace: '/' +# True will start automatic refresh of ros nodes +automatic_refresh: true +# The automatic refresh time, if automatic_refresh is false this will not be considered +# The unit is second +refresh_cycle_time: 0.5 +# Switches for specific ros information +show_nodes: true +show_topics: true +show_services: true +show_params: true +# True means data types will be imported from an xml file, its address is written in +# ./scripts/ros_opc_ua_comm under the variable message_export_path. +# False means the data types will be generated on the fly. +# FIXME: Please notice, do not set it to true before the bug in ./scripts/ros_server fixed. +import_xml_msgs: false +# Control if the variables in parameters are writable +parameter_writable: true +# TODO: filters for specific topics, services +#topics: [/turle1/cmd_vel, /turtle1/color_sensor] diff --git a/ros_opcua_impl_python_opcua/known_issues.md b/ros_opcua_impl_python_opcua/known_issues.md index 808ee36..cfd962c 100644 --- a/ros_opcua_impl_python_opcua/known_issues.md +++ b/ros_opcua_impl_python_opcua/known_issues.md @@ -1,32 +1,22 @@ # Known Issues or problems -## Actions -* Wrong Argument Parsing, as Int32 type arguments are wrongly parsed to Int16 ua.Arguments() -(See BlinkyAction, PoliceAction on SR2 Robot in Lab for example) +## ROS Topic deleting problem -* Move_Base_Simple action has special case as to fix Errors that were thrown, possibly it is a malformed Action Definition -```python - if 'move_base_simple' in self.name: - self.goal_instance = self.goal_class() - else: - self.goal_instance = self.goal_class().goal -``` +- [ ] Error message: `[WARN] [1535063519.464253]: Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.` -* Actions are seen as Actions when */goal*, */feedback*, */result* or */status* is in the topic name, better heuristic possibly needed +Problem lies on ROS side, please check [here](https://github.com/Microsoft/WSL/issues/1391#issuecomment-300076193). -* We ignore "header" in the goal call message instance to create our Arguments (and back the other way). Possible mistake See: -```python - while cur_slot == 'header': - rospy.logdebug("ignoring header") - object_counter += 1 - if object_counter < len(sample.__slots__): - cur_slot = sample.__slots__[object_counter] -``` +The [pull request](https://github.com/ros/ros_comm/pull/1050/files) to repair it in ros-lunar. -## Topics: +This problem happens sometimes, not always. -* Better Idea for publishing OPC UA Data ? +## Import xml instead of directly generate messages -## Services -* Methods return value Strings are not correctly transfered to OPC UA somehow (maybe an UAExpert issue, as they can be printed in the call method onto the console correctly) +- [ ] The created extension objects can not be displayed correctly in UAExpert by importing xml, the import function seem to have mixed OPC UA dictionary and data type nodes, the created data became one specific node. +Besides, for a test case with 395 ros messages: + +direct generation takes about 1.6s +xml import takes about 2.4s but works incorrectly + +A ros parameter `import_xml_msgs` is imported to support both xml import and create on-the-fly. diff --git a/ros_opcua_impl_python_opcua/launch/opcuaclient.launch b/ros_opcua_impl_python_opcua/launch/opcuaclient.launch new file mode 100644 index 0000000..4d6734f --- /dev/null +++ b/ros_opcua_impl_python_opcua/launch/opcuaclient.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/ros_opcua_impl_python_opcua/launch/rosopcua.launch b/ros_opcua_impl_python_opcua/launch/rosopcua.launch index c53d09a..f472fe0 100644 --- a/ros_opcua_impl_python_opcua/launch/rosopcua.launch +++ b/ros_opcua_impl_python_opcua/launch/rosopcua.launch @@ -2,5 +2,4 @@ - diff --git a/ros_opcua_impl_python_opcua/launch/rosopcua_parse_and_export_ros_message.launch b/ros_opcua_impl_python_opcua/launch/rosopcua_parse_and_export_ros_message.launch new file mode 100644 index 0000000..a9e3009 --- /dev/null +++ b/ros_opcua_impl_python_opcua/launch/rosopcua_parse_and_export_ros_message.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ros_opcua_impl_python_opcua/package.xml b/ros_opcua_impl_python_opcua/package.xml index ef48887..1023b49 100644 --- a/ros_opcua_impl_python_opcua/package.xml +++ b/ros_opcua_impl_python_opcua/package.xml @@ -14,6 +14,7 @@ Denis Štogl Daniel Draper + Peiren Yang catkin diff --git a/ros_opcua_impl_python_opcua/python-opcua b/ros_opcua_impl_python_opcua/python-opcua index 1bd0559..36a0ee9 160000 --- a/ros_opcua_impl_python_opcua/python-opcua +++ b/ros_opcua_impl_python_opcua/python-opcua @@ -1 +1 @@ -Subproject commit 1bd05593a7a1fa2b4a6a7439bc5e4bbee40937f3 +Subproject commit 36a0ee9a700460e3fe9e7f63f4c33099c87ff1bc diff --git a/ros_opcua_impl_python_opcua/scripts/basic_server_client.py b/ros_opcua_impl_python_opcua/scripts/basic_server_client.py new file mode 100644 index 0000000..fe5573f --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/basic_server_client.py @@ -0,0 +1,266 @@ +import rospy + +from opcua import ua, Server, Client +from opcua.ua import UaError +from ros_opc_ua_comm import OpcUaROSMessage, expand_ua_class + +message_export_path = 'message.xml' +server_end_point = 'opc.tcp://0.0.0.0:21554/ROSServer' +ros_idx_name = 'http://ros.org/rosopcua' + + +class ROSBasicServer: + + def __init__(self): + self._server = Server() + + self._end_point = rospy.get_param('rosopcua/end_point', server_end_point) + + self._server.set_endpoint(self._end_point) + self._server.set_server_name('ROS UA Server') + self._idx_name = ros_idx_name + self._idx = self._server.register_namespace(self._idx_name) + self._ros_node_name = 'rosopcua' + self._message_path = message_export_path + self._server_started = False + + self._namespace_ros = rospy.get_param('/rosopcua/namespace') + self._auto_refresh = rospy.get_param('/rosopcua/automatic_refresh') + self._refresh_cycle_time = rospy.get_param('/rosopcua/refresh_cycle_time') + self._show_nodes = rospy.get_param('/rosopcua/show_nodes') + self._show_topics = rospy.get_param('/rosopcua/show_topics') + self._show_services = rospy.get_param('/rosopcua/show_services') + self._show_params = rospy.get_param('/rosopcua/show_params') + self._import_xml_msgs = rospy.get_param('/rosopcua/import_xml_msgs') + self._para_writable = rospy.get_param('/rosopcua/parameter_writable') + + self._type_dict = {} + self._msgs_dict = {} + self._srvs_dict = {} + + def __enter__(self): + rospy.init_node(self._ros_node_name, log_level=rospy.INFO) + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + if self._server_started: + self._server.stop() + rospy.loginfo(' ----- Server stopped! ------ ') + + def _nodeid_generator(self): + return ua.NodeId(namespaceidx=self._idx) + + def _start_server(self): + self._server.start() + self._server_started = True + rospy.loginfo(' ----- Server started! ------ ') + + def import_messages(self): + rospy.loginfo(' ----- start importing node message to xml ------ ') + nodes = self._server.import_xml(self._message_path) + rospy.loginfo(' ----- {} nodes are imported ------ '.format(len(nodes))) + type_dict = {self._server.get_node(node).get_display_name().Text: node for node in nodes} + return type_dict + + def load_messages(self): + rospy.loginfo(' ----- Creating messages ------ ') + ros_type_creator = OpcUaROSMessage(self._server, self._idx, self._idx_name) + self._type_dict, self._msgs_dict, self._srvs_dict = ros_type_creator.create_ros_data_types() + rospy.loginfo(' ----- {} messages created------ '.format(str(len(self._type_dict)))) + + +class ROSBasicClient: + + def __init__(self): + self._client = Client(server_end_point) + self._ros_node_name = 'opcuaclient' + self._idx = None + self._node_root = None + self._topics = {} + self._pub_topics = {} + self._pub_parent = None + self._services = {} + self._service_parent = None + self._params = {} + self._nodes = {} + + def __enter__(self): + rospy.init_node(self._ros_node_name, log_level=rospy.INFO) + self._client.connect() + self._idx = self._client.get_namespace_index(ros_idx_name) + self._client.load_type_definitions() + self._root = self._client.get_objects_node() + rospy.loginfo(' ----- Client connected! ------ ') + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self._client.disconnect() + rospy.loginfo(' ----- Client disconnected! ------ ') + + @staticmethod + def expand_value(obj): + return expand_ua_class(obj) + + def _retrieve_ua_nodes(self, folder_name, refs=ua.ObjectIds.Organizes): + temp_dict = {} + msg_folder = self._root.get_child(folder_name) + msgs = msg_folder.get_children(refs) + for msg in msgs: + msg_name = msg.get_display_name().Text + temp_dict[msg_name] = msg + return temp_dict + + def _refresh_topics(self): + topic_folder = 'Topics' + self._topics = self._retrieve_ua_nodes(topic_folder, refs=ua.ObjectIds.HasProperty) + if not self._pub_parent: + self._pub_parent = self._retrieve_ua_nodes(topic_folder).values()[0] + pub_methods = self._pub_parent.get_children(refs=ua.ObjectIds.HasComponent) + for method in pub_methods: + msg_name = method.get_display_name().Text + self._pub_topics[msg_name] = method + + def _refresh_services(self): + services_folder_name = 'Services' + if not self._service_parent: + self._service_parent = self._root.get_child(services_folder_name) + self._services = self._retrieve_ua_nodes(services_folder_name, refs=ua.ObjectIds.HasComponent) + + def _refresh_nodes(self): + self._nodes = self._retrieve_ua_nodes('Nodes') + + def _refresh_params(self): + self._params = self._retrieve_ua_nodes('Parameters', refs=ua.ObjectIds.HasProperty) + + def list_services(self): + self._refresh_services() + for service in self._services: + rospy.loginfo(service) + + def list_topics(self): + self._refresh_topics() + for topic in self._topics: + rospy.loginfo(topic) + + def show_topic(self, topic_name): + if topic_name not in self._topics: + self._refresh_topics() + if topic_name not in self._topics: + raise Exception('Topic {} does not exist in server side!'.format(topic_name)) + try: + topic_value = self._topics[topic_name].get_value() + rospy.loginfo('{}:{}'.format(topic_name, self.expand_value(topic_value))) + except UaError as e: + rospy.logerr(e.message) + del self._topics[topic_name] + + def show_topics(self): + # In case the dict changed in iteration + if not self._topics: + self._refresh_topics() + for topic in self._topics.keys(): + self.show_topic(topic) + + def list_params(self): + self._refresh_params() + for param in self._params: + rospy.loginfo(param) + + def show_param(self, param_name): + if not self._params: + self._refresh_params() + if param_name not in self._params: + self._refresh_params() + if param_name not in self._params: + raise Exception('Parameter {} does not exist in server side!'.format(param_name)) + try: + param_value = self._params[param_name].get_value() + rospy.loginfo('{}:{}'.format(param_name, param_value)) + except UaError as e: + rospy.logerr(e.message) + del self._params[param_name] + + def show_params(self): + if not self._params: + self._refresh_params() + # In case the dict changed in iteration + for param in self._params.keys(): + self.show_param(param) + + def list_ros_nodes(self): + self._refresh_nodes() + for node in self._nodes: + rospy.loginfo(node) + + def show_ros_node(self, node_name): + if node_name not in self._nodes: + self._refresh_nodes() + if node_name not in self._nodes: + raise Exception('Node {} does not exist in server side!'.format(node_name)) + + rospy.loginfo('------ rosnode {} ------'.format(node_name)) + try: + pub_node = self._nodes[node_name].get_child('Publications') + rospy.loginfo('\tPublications:') + pubs = pub_node.get_children(refs=ua.ObjectIds.Organizes) + for pub in pubs: + rospy.loginfo('\t\t{}'.format(pub.get_display_name().Text)) + except UaError: + pass + try: + sub_node = self._nodes[node_name].get_child('Subscriptions') + rospy.loginfo('\tSubscriptions:') + subs = sub_node.get_children(refs=ua.ObjectIds.Organizes) + for sub in subs: + sub_name = sub.get_display_name().Text + if sub_name != 'Topic publish': + rospy.loginfo('\t\t{}'.format(sub_name)) + except UaError: + pass + try: + srv_node = self._nodes[node_name].get_child('Services') + rospy.loginfo('\tServices:') + srvs = srv_node.get_children(refs=ua.ObjectIds.Organizes) + for srv in srvs: + rospy.loginfo('\t\t{}'.format(srv.get_display_name().Text)) + except UaError: + pass + try: + act_node = self._nodes[node_name].get_child('Action') + rospy.loginfo('\tAction:') + acts = act_node.get_children(refs=ua.ObjectIds.Organizes) + for act in acts: + rospy.loginfo('\t\t{}'.format(act.get_display_name().Text)) + except UaError: + pass + + def show_ros_nodes(self): + if not self._nodes: + self._refresh_nodes() + # In case the dict changed in iteration + for node in self._nodes.keys(): + self.show_ros_node(node) + + def publish_topic(self, topic_name, ext_obj): + if not self._pub_topics: + self._refresh_topics() + if topic_name not in self._pub_topics: + raise Exception('Topic {} does not exist in server side!'.format(topic_name)) + topic_node = self._pub_topics[topic_name] + result = self._pub_parent.call_method(topic_node, ext_obj) + rospy.loginfo('Publish message of topic {} successful!'.format(topic_name)) + if result: + rospy.loginfo(self.expand_value(result)) + + def call_service(self, service_name, ext_obj=None): + if not self._services: + self._refresh_services() + if service_name not in self._services: + raise Exception('Topic {} does not exist in server side!'.format(service_name)) + service_node = self._services[service_name] + if not ext_obj: + ext_obj = [] + result = self._service_parent.call_method(service_node, ext_obj) + rospy.loginfo('Call service {} successful!'.format(service_name)) + if result: + rospy.loginfo(self.expand_value(result)) diff --git a/ros_opcua_impl_python_opcua/scripts/opcua_client_application_example.py b/ros_opcua_impl_python_opcua/scripts/opcua_client_application_example.py new file mode 100644 index 0000000..f0a8c41 --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/opcua_client_application_example.py @@ -0,0 +1,159 @@ +import time +import rospy +from ros_opcua_msgs.msg import * +from ros_opcua_srvs.srv import * + + +def connect_server(): + # Call connect + proxy = rospy.ServiceProxy('/opcuaclient/connect', Connect) + address = ConnectRequest() + address.endpoint = 'opc.tcp://0.0.0.0:4840/freeopcua/server/' + result = proxy(address) + if result.success: + print('\nConnected!') + else: + raise Exception(result.error_message) + proxy.close() + + +def disconnect_server(): + # call disconnect ua server + proxy = rospy.ServiceProxy('/opcuaclient/disconnect', Disconnect) + result = proxy() + if result.success: + print('\nDisconnected!') + else: + raise Exception(result.error_message) + proxy.close() + + +def generate_multiply_request(): + # Make ua service request + req = CallMethodRequest() + # make sure the node id here are correct + # list node service may help here. + req.node.nodeId = 'ns=2;i=7' + req.method.nodeId = 'ns=2;i=18' + # here is our special msg class to handle any kinds of data types + data1 = TypeValue() + data1.type = 'int64' + data1.int64_d = 2 + data2 = TypeValue() + data2.type = 'int64' + data2.int64_d = 3 + req.data.append(data1) + req.data.append(data2) + + return req + + +def list_node(): + proxy = rospy.ServiceProxy('/opcuaclient/list_node', ListNode) + node_to_list = ListNodeRequest() + node_to_list.node.nodeId = 'ns=2;i=7' + response = proxy(node_to_list) + if response.success: + for node in response.children: + print('\nNode ID: ' + node.nodeId) + print('Node Name: ' + node.qualifiedName) + print('Additional Information:') + for info in node.nodeInfo: + print(info) + else: + raise Exception(response.error_message) + proxy.close() + + +def execute_method_call(req): + # call method + proxy = rospy.ServiceProxy('/opcuaclient/call_method', CallMethod) + response = proxy(req) + if response.success: + print('\nThe result of the multiplication is: ' + str(response.data[0].int64_d)) + else: + raise Exception(response.error_message) + proxy.close() + + +def read(): + node = ReadRequest() + node.node.nodeId = 'ns=2;i=10' + + proxy = rospy.ServiceProxy('/opcuaclient/read', Read) + response = proxy(node) + if response.success: + print('\nValue of the node is: ' + str(getattr(response.data, response.data.type + '_d'))) + else: + raise Exception(response.error_message) + proxy.close() + + +def write(): + node = WriteRequest() + node.node.nodeId = 'ns=2;i=10' + node.data.type = 'string' + node.data.string_d = 'new string' + + proxy = rospy.ServiceProxy('/opcuaclient/write', Write) + response = proxy(node) + if response.success: + print('Write in successful') + else: + raise Exception(response.error_message) + proxy.close() + + +def subscribe(): + node = SubscribeRequest() + node.node.nodeId = 'ns=2;i=8' + node.callback_topic = 'test' + + proxy = rospy.ServiceProxy('/opcuaclient/subscribe', Subscribe) + response = proxy(node) + if response.success: + print('\nSubscription successful, data change will be published under ROS topic ' + node.callback_topic) + else: + raise Exception(response.error_message) + proxy.close() + + +def unsubscribe(): + node = UnsubscribeRequest() + node.node.nodeId = 'ns=2;i=8' + + proxy = rospy.ServiceProxy('/opcuaclient/unsubscribe', Unsubscribe) + response = proxy(node) + if response.success: + print('\nUnsubscribe successful') + else: + raise Exception(response.error_message) + proxy.close() + + +if __name__ == '__main__': + try: + connect_server() + time.sleep(1) + + list_node() + time.sleep(1) + + ua_req = generate_multiply_request() + execute_method_call(ua_req) + time.sleep(1) + + read() + time.sleep(1) + write() + time.sleep(1) + read() + time.sleep(1) + + subscribe() + time.sleep(10) + unsubscribe() + + disconnect_server() + except Exception as e: + print(str(e)) diff --git a/ros_opcua_impl_python_opcua/scripts/opcua_client_application_example_ros.py b/ros_opcua_impl_python_opcua/scripts/opcua_client_application_example_ros.py new file mode 100644 index 0000000..876d56a --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/opcua_client_application_example_ros.py @@ -0,0 +1,225 @@ +import time +from io import BytesIO +import rospy +import roslib.message +from ros_opcua_msgs.msg import * +from ros_opcua_srvs.srv import * + + +def serialize(ros_obj): + buff = BytesIO() + ros_obj.serialize(buff) + return str(buff.getvalue()) + + +def deserialize(ros_obj, data): + return ros_obj.deserialize(data) + + +def connect_server(): + # Call connect + proxy = rospy.ServiceProxy('/opcuaclient/connect', Connect) + address = ConnectRequest() + address.endpoint = 'opc.tcp://0.0.0.0:21554/ROSServer' + result = proxy(address) + if result.success: + print('\nConnected!') + else: + raise Exception(result.error_message) + proxy.close() + + +def disconnect_server(): + # call disconnect ua server + proxy = rospy.ServiceProxy('/opcuaclient/disconnect', Disconnect) + result = proxy() + if result.success: + print('\nDisconnected!') + else: + raise Exception(result.error_message) + proxy.close() + + +def list_node(): + proxy = rospy.ServiceProxy('/opcuaclient/list_node', ListNode) + node_to_list = ListNodeRequest() + node_to_list.node.nodeId = 'ns=2;i=53' + response = proxy(node_to_list) + if response.success: + for node in response.children: + print('\nNode ID: ' + node.nodeId) + print('Node Name: ' + node.qualifiedName) + print('Additional Information:') + for info in node.nodeInfo: + print(info) + else: + raise Exception(response.error_message) + proxy.close() + + +def generate_spawn_request(): + # Make sure the turtlesim node is started. + # Sample of calling spawn and kill. + # Get service class + req_type = 'turtlesim/SpawnRequest' + spawn_req = roslib.message.get_service_class(req_type)() + # Set value + spawn_req.name = 'hello_world' + spawn_req.x = 1 + spawn_req.y = 1 + spawn_req.theta = 1 + + # Make ua service request + req = CallMethodRequest() + # make sure the node id here are correct + # list node service may help here. + req.node.nodeId = 'ns=2;i=53' + req.method.nodeId = 'ns=2;i=61' + # here is our special msg class to handle any kinds of data types + data = TypeValue() + # set the value and serialized object + data.type = req_type + data.string_d = serialize(spawn_req) + req.data.append(data) + return req + + +def generate_kill_request(): + req_type = 'turtlesim/KillRequest' + kill_req = roslib.message.get_service_class(req_type)() + kill_req.name = 'hello_world' + + # Make ua service request + req = CallMethodRequest() + req.node.nodeId = 'ns=2;i=53' + req.method.nodeId = 'ns=2;i=58' + data = TypeValue() + data.type = req_type + data.string_d = serialize(kill_req) + req.data.append(data) + return req + + +def execute_method_call(req): + proxy = rospy.ServiceProxy('/opcuaclient/call_method', CallMethod) + response = proxy(req) + if response.success: + # deserialize the response + if response.data: + result = roslib.message.get_service_class(response.data[0].type)() + result = deserialize(result, response.data[0].string_d) + else: + result = [] + print('\nCall method succeeded, result is:' + str(result)) + else: + raise Exception(response.error_message) + proxy.close() + + +def read(): + """ + read one node with extension object + :return: + """ + node = ReadRequest() + node.node.nodeId = 'ns=2;i=81' + + proxy = rospy.ServiceProxy('/opcuaclient/read', Read) + response = proxy(node) + if response.success: + method_class = roslib.message.get_service_class(response.data.type) + if not method_class: + method_class = roslib.message.get_message_class(response.data.type) + result = deserialize(method_class(), response.data.string_d) + print('\nValue of the node is:\n' + str(result)) + else: + raise Exception(response.error_message) + proxy.close() + + +def write(): + """ + normally the node with extension objects in server are not writeable + if one node with extension object should be written, the serialize method above should be called. + :return: + """ + node = WriteRequest() + node.node.nodeId = 'ns=2;i=95' + node.data.type = 'int64' + node.data.int64_d = 200 + + proxy = rospy.ServiceProxy('/opcuaclient/write', Write) + response = proxy(node) + if response.success: + print('Write in successful') + else: + raise Exception(response.error_message) + proxy.close() + + +def subscribe(): + """ + for parsing extension object, the serialize method above should be called. + :return: + """ + node = SubscribeRequest() + node.node.nodeId = 'ns=2;i=81' + node.callback_topic = 'test' + + proxy = rospy.ServiceProxy('/opcuaclient/subscribe', Subscribe) + response = proxy(node) + if response.success: + print('\nSubscription successful, data change will be published under ROS topic ' + node.callback_topic) + else: + raise Exception(response.error_message) + proxy.close() + + +def unsubscribe(): + node = UnsubscribeRequest() + node.node.nodeId = 'ns=2;i=81' + + proxy = rospy.ServiceProxy('/opcuaclient/unsubscribe', Unsubscribe) + response = proxy(node) + if response.success: + print('\nUnsubscribe successful') + else: + raise Exception(response.error_message) + proxy.close() + + +if __name__ == '__main__': + """ + make sure the node ids are correct + make sure the turtlesim node are started + make sure the ros_opc_ua_client is running + """ + try: + connect_server() + time.sleep(1) + + list_node() + time.sleep(1) + + # call spawn service + ua_req = generate_spawn_request() + execute_method_call(ua_req) + time.sleep(1) + # call kill service + ua_req = generate_kill_request() + execute_method_call(ua_req) + time.sleep(1) + + read() + time.sleep(1) + write() + time.sleep(1) + + subscribe() + time.sleep(10) + unsubscribe() + + disconnect_server() + + except Exception as e: + print(str(e)) diff --git a/ros_opcua_impl_python_opcua/scripts/ros_actions.py b/ros_opcua_impl_python_opcua/scripts/ros_actions.py deleted file mode 100644 index 63953c1..0000000 --- a/ros_opcua_impl_python_opcua/scripts/ros_actions.py +++ /dev/null @@ -1,433 +0,0 @@ -# !/usr/bin/python -# thanks to https://github.com/ros-visualization/rqt_common_plugins/blob/groovy-devel/rqt_action/src/rqt_action/action_plugin.py -import random -from pydoc import locate - -import actionlib -import roslib -import rospy -from opcua import ua, common -from opcua import uamethod -from opcua.ua.uaerrors import UaError -from roslib import message - -import ros_server -import ros_services -import ros_topics - - -class OpcUaROSAction: - def __init__(self, server, parent, idx, name, action_type, feedback_type): - self.server = server - self.idx = idx - self.name = name - self.type = action_type.split("/")[0] - self.feedback_type = feedback_type - self._feedback_nodes = {} - goal_name = "_" + action_type.split("/")[-1] - msg_name = goal_name.replace("Goal", "") - class_name = msg_name.replace("_", "", 1) - rospy.logdebug("Trying to find module with name: " + self.type + ".msg." + goal_name.replace("Goal", "")) - actionspec = locate(self.type + ".msg." + msg_name) - rospy.logdebug("We are creating action: " + self.name) - rospy.logdebug("We have type: " + self.type) - rospy.logdebug("We have msg name: " + msg_name) - rospy.logdebug("We have class name: " + class_name) - rospy.logdebug("We have goal name: " + goal_name) - rospy.logdebug("We have goal class name: " + goal_name.replace("_", "", 1)) - - goalspec = locate(self.type + ".msg." + goal_name) - rospy.logdebug("found goalspec") - self.goal_class = getattr(goalspec, goal_name.replace("_", "", 1)) - # malformed move_base_simple Action hack - if 'move_base_simple' in self.name: - self.goal_instance = self.goal_class() - else: - self.goal_instance = self.goal_class().goal - rospy.logdebug("found goal_instance " + str(self.goal_instance)) - try: - self.client = actionlib.SimpleActionClient(self.get_ns_name(), getattr(actionspec, class_name)) - rospy.logdebug("We have created a SimpleActionClient for action " + self.name) - except actionlib.ActionException as e: - rospy.logerr("Error when creating ActionClient for action " + self.name, e) - rospy.logdebug("Creating parent objects for action " + str(self.name)) - self.parent = self.recursive_create_objects(name, idx, parent) - rospy.logdebug("Found parent for action: " + str(self.parent)) - rospy.logdebug("Creating main node with name " + self.name.split("/")[-1]) - # parent is our main node, this means our parent in log message above was actionsObject - if self.name.split("/")[-1] == self.parent.nodeid.Identifier: - self.main_node = self.parent - else: - self.main_node = self.parent.add_object( - ua.NodeId(self.name.split("/")[-1], self.parent.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName(self.name.split("/")[-1], self.parent.nodeid.NamespaceIndex)) - rospy.logdebug("Created Main Node under parent!") - self.result = self.main_node.add_object( - ua.NodeId(self.name + "_result", self.main_node.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName("result", self.main_node.nodeid.NamespaceIndex)) - self.result_node = ros_topics._create_node_with_type(self.result, self.idx, self.name + "_result_value", - self.name + "_result_value", - "string", -1) - - self.result_node.set_value("No goal completed yet") - rospy.logdebug("Created result node") - self.goal = self.main_node.add_object( - ua.NodeId(self.name + "_goal", self.main_node.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName("goal", parent.nodeid.NamespaceIndex)) - self.goal_node = self.goal.add_method(idx, self.name + "_send_goal", self.send_goal, - getargarray(self.goal_instance), []) - - self.goal_cancel = self.goal.add_method(idx, self.name + "_cancel_goal", self.cancel_goal, [], []) - - rospy.logdebug("Created goal node") - - self.feedback = self.main_node.add_object( - ua.NodeId(self.name + "feedback", self.main_node.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName("feedback", self.main_node.nodeid.NamespaceIndex)) - rospy.logdebug("Created feedback node") - if self.feedback_type is not None: - try: - rospy.logdebug("We are trying to create Feedback for feedback type: " + self.feedback_type) - self.feedback_message_class = roslib.message.get_message_class(self.feedback_type) - self.feedback_message_instance = self.feedback_message_class() - rospy.logdebug("Created feedback message instance") - - except rospy.ROSException: - self.message_class = None - rospy.logerror("Didn't find feedback message class for type " + self.feedback_type) - - self._recursive_create_feedback_items(self.feedback, self.name + "/feedback", self.feedback_type, - getattr(self.feedback_message_instance, "feedback")) - - self.status = self.main_node.add_object( - ua.NodeId(self.name + "status", self.main_node.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName("status", self.main_node.nodeid.NamespaceIndex)) - self.status_node = ros_topics._create_node_with_type(self.status, self.idx, self.name + "_status", - self.name + "_status", - "string", -1) - self.status_node.set_value("No goal sent yet") - rospy.loginfo("Created ROS Action with name: %s", self.name) - - def message_callback(self, message): - self.update_value(self.name + "/feedback", message) - - def update_value(self, topic_name, message): - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name in message.__slots__: - self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name)) - - elif type(message) in (list, tuple): - if (len(message) > 0) and hasattr(message[0], '__slots__'): - for index, slot in enumerate(message): - if topic_name + '[%d]' % index in self._feedback_nodes: - self.update_value(topic_name + '[%d]' % index, slot) - else: - if topic_name in self._feedback_nodes: - base_type_str, _ = ros_topics._extract_array_info( - self._feedback_nodes[topic_name].text(self.feedback_type)) - self._recursive_create_items(self._feedback_nodes[topic_name], topic_name + '[%d]' % index, - base_type_str, - slot, None) - # remove obsolete children - if topic_name in self._feedback_nodes: - if len(message) < len(self._feedback_nodes[topic_name].get_children()): - for i in range(len(message), self._feedback_nodes[topic_name].childCount()): - item_topic_name = topic_name + '[%d]' % i - self.recursive_delete_items(self._feedback_nodes[item_topic_name]) - del self._feedback_nodes[item_topic_name] - else: - if topic_name in self._feedback_nodes and self._feedback_nodes[topic_name] is not None: - self._feedback_nodes[topic_name].set_value(repr(message)) - - @uamethod - def cancel_goal(self, parent, *inputs): - # rospy.logdebug("cancelling goal " + self.name) - try: - self.client.cancel_all_goals() - self.update_state() - except (rospy.ROSException, UaError) as e: - rospy.logerr("Error when cancelling a goal for " + self.name, e) - - def recursive_create_objects(self, topic_name, idx, parent): - rospy.logdebug("reached parent object creation! current parent: " + str(parent)) - hierachy = topic_name.split('/') - rospy.logdebug("Current hierachy: " + str(hierachy)) - if len(hierachy) == 0 or len(hierachy) == 1: - return parent - for name in hierachy: - rospy.logdebug("current name: " + str(name)) - if name != '': - try: - nodewithsamename = self.server.find_action_node_with_same_name(name, idx) - if nodewithsamename is not None: - rospy.logdebug("Found node with same name, is now new parent") - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - nodewithsamename) - else: - # if for some reason 2 services with exactly same name are created use hack>: add random int, prob to hit two - # same ints 1/10000, should be sufficient - newparent = parent.add_object( - ua.NodeId(name, parent.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName(name, parent.nodeid.NamespaceIndex)) - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - newparent) - # thrown when node with parent name is not existent in server - except IndexError, UaError: - newparent = parent.add_object( - ua.NodeId(name + str(random.randint(0, 10000)), parent.nodeid.NamespaceIndex, - ua.NodeIdType.String), - ua.QualifiedName(name, parent.nodeid.NamespaceIndex)) - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - newparent) - - return parent - - def _recursive_create_feedback_items(self, parent, feedback_topic_name, type_name, feedback_message): - idx = self.idx - topic_text = feedback_topic_name.split('/')[-1] - if '[' in topic_text: - topic_text = topic_text[topic_text.index('['):] - - # This here are 'complex data' - if hasattr(feedback_message, '__slots__') and hasattr(feedback_message, '_slot_types'): - complex_type = True - new_node = parent.add_object( - ua.NodeId(feedback_topic_name, parent.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName(feedback_topic_name, parent.nodeid.NamespaceIndex)) - - for slot_name, type_name_child in zip(feedback_message.__slots__, feedback_message._slot_types): - self._recursive_create_feedback_items(new_node, feedback_topic_name + '/' + slot_name, type_name_child, - getattr(feedback_message, slot_name)) - self._feedback_nodes[feedback_topic_name] = new_node - - else: - # This are arrays - base_type_str, array_size = ros_topics._extract_array_info(type_name) - try: - base_instance = roslib.message.get_message_class(base_type_str)() - except (ValueError, TypeError): - base_instance = None - - if array_size is not None and hasattr(base_instance, '__slots__'): - for index in range(array_size): - self._recursive_create_feedback_items(parent, feedback_topic_name + '[%d]' % index, base_type_str, - base_instance) - else: - new_node = ros_topics._create_node_with_type(parent, idx, feedback_topic_name, topic_text, type_name, - array_size) - new_node.set_writable(True) - self._feedback_nodes[feedback_topic_name] = new_node - return - - # namespace - def get_ns_name(self): - splits = self.name.split("/") - ns = splits[1:] - res = "" - for split in ns: - res += split + "/" - rospy.logdebug("Created ns name: " + res[:-1]) - return str(res[:-1]) - - @uamethod - def send_goal(self, parent, *inputs): - rospy.loginfo("Sending Goal for " + self.name) - try: - goal_msg = self.create_message_instance(inputs, self.goal_instance) - if 'move_base' in self.name: - rospy.loginfo("setting frame_id for move_base malformation") - try: - target_pose = getattr(goal_msg, "target_pose") - header = getattr(target_pose, "header") - setattr(header, "frame_id", "map") - setattr(target_pose, "header", header) - except AttributeError as e: - rospy.logerr("Error occured when setting frame_id", e) - rospy.loginfo("Created Message Instance for goal-send: " + str(goal_msg)) - self.client.send_goal(goal_msg, done_cb=self.update_result, feedback_cb=self.update_feedback, - active_cb=self.update_state) - return - except Exception as e: - rospy.logerr("Error occured during goal sending for Action " + str(self.name)) - print(e) - - def create_message_instance(self, inputs, sample): - rospy.logdebug("Creating message for goal call") - already_set = [] - if isinstance(inputs, tuple): - arg_counter = 0 - object_counter = 0 - while arg_counter < len(inputs) and object_counter < len(sample.__slots__): - cur_arg = inputs[arg_counter] - cur_slot = sample.__slots__[object_counter] - # ignore header for malformed move_base_goal, as header shouldnt be in sent message - while cur_slot == 'header': - rospy.logdebug("ignoring header") - object_counter += 1 - if object_counter < len(sample.__slots__): - cur_slot = sample.__slots__[object_counter] - real_slot = getattr(sample, cur_slot) - rospy.lodebug( - "cur_arg: " + str(cur_arg) + " cur_slot_name: " + str(cur_slot) + " real slot content: " + str( - real_slot)) - if hasattr(real_slot, '_type'): - rospy.logdebug("We found an object with name " + str(cur_slot) + ", creating it recursively") - arg_counter_before = arg_counter - already_set, arg_counter = self.create_object_instance(already_set, real_slot, cur_slot, - arg_counter, inputs, sample) - if arg_counter != arg_counter_before: - object_counter += 1 - rospy.logdebug("completed object, object counter: " + str(object_counter) + " len(object): " + str( - len(sample.__slots__))) - else: - already_set.append(cur_slot) - # set the attribute in the request - setattr(sample, cur_slot, cur_arg) - arg_counter += 1 - object_counter += 1 - - return sample - - def create_object_instance(self, already_set, object, name, counter, inputs, parent): - rospy.logdebug("Create Object Instance Notify") - object_counter = 0 - while object_counter < len(object.__slots__) and counter < len(inputs): - cur_arg = inputs[counter] - cur_slot = object.__slots__[object_counter] - # ignore header for malformed move_base_goal, as header shouldnt be in sent message - while cur_slot == 'header': - rospy.logdebug("ignoring header") - object_counter += 1 - if object_counter < len(object.__slots__): - cur_slot = object.__slots__[object_counter] - else: - return already_set, counter - real_slot = getattr(object, cur_slot) - rospy.logdebug( - "cur_arg: " + str(cur_arg) + " cur_slot_name: " + str(cur_slot) + " real slot content: " + str( - real_slot)) - if hasattr(real_slot, '_type'): - rospy.logdebug("Recursive Object found in request/response of service call") - already_set, counter = self.create_object_instance(already_set, real_slot, cur_slot, counter, inputs, - object) - object_counter += 1 - else: - already_set.append(cur_slot) - setattr(object, cur_slot, cur_arg) - object_counter += 1 - counter += 1 - # sets the object as an attribute in the request were trying to build - setattr(parent, name, object) - return already_set, counter - - def recursive_delete_items(self, item): - self.client.cancel_all_goals() - for child in item.get_children(): - self.recursive_delete_items(child) - self.server.server.delete_nodes([child]) - self.server.server.delete_nodes([self.result, self.result_node, self.goal_node, self.goal, self.parent]) - ros_server.own_rosnode_cleanup() - - def update_result(self, state, result): - rospy.logdebug("updated result cb reached") - self.status_node.set_value(map_status_to_string(state)) - self.result_node.set_value(repr(result)) - - def update_state(self): - rospy.logdebug("updated state cb reached") - self.status_node.set_value(repr(self.client.get_goal_status_text())) - - def update_feedback(self, feedback): - rospy.logdebug("updated feedback cb reached") - self.message_callback(feedback) - - -def get_correct_name(topic_name): - rospy.logdebug("getting correct name for: " + str(topic_name)) - splits = topic_name.split("/") - counter = 0 - counter2 = 0 - result = "" - while counter < len(splits): - if splits[-1] == splits[counter] and not counter == 1: - while counter2 <= counter - 1: - if counter2 != counter - 1: - result += splits[counter2] + '/' - else: - result += splits[counter2] - counter2 += 1 - return result - counter += 1 - - -def getargarray(goal_class): - array = [] - for slot_name in goal_class.__slots__: - if slot_name != 'header': - slot = getattr(goal_class, slot_name) - if hasattr(slot, '_type'): - array_to_merge = getargarray(slot) - array.extend(array_to_merge) - else: - if isinstance(slot, list): - rospy.logdebug("Found an Array Argument!") - arg = ua.Argument() - arg.Name = slot_name - arg.DataType = ua.NodeId(ros_services.getobjectidfromtype("array")) - arg.ValueRank = -1 - arg.ArrayDimensions = [] - arg.Description = ua.LocalizedText("Array") - else: - arg = ua.Argument() - if hasattr(goal_class, "__name__"): - arg.Name = goal_class.__name__ + slot_name - else: - arg.Name = slot_name - arg.DataType = ua.NodeId(ros_services.getobjectidfromtype(type(slot).__name__)) - arg.ValueRank = -1 - arg.ArrayDimensions = [] - arg.Description = ua.LocalizedText(slot_name) - array.append(arg) - - return array - - -def map_status_to_string(param): - if param == 9: - return "Goal LOST" - elif param == 8: - return "Goal RECALLED" - elif param == 7: - return "Goal RECALLING" - elif param == 6: - return "Goal PREEMPTING" - elif param == 5: - return "Goal REJECTED" - elif param == 4: - return "Goal ABORTED" - elif param == 3: - return "Goal SUCEEDED" - elif param == 2: - return "Goal PREEMPTED" - elif param == 1: - return "Goal ACTIVE" - elif param == 0: - return "Goal PENDING" - - -def refresh_dict(namespace_ros, actionsdict, topicsdict, server, idx_actions): - topics = rospy.get_published_topics(namespace_ros) - tobedeleted = [] - for actionNameOPC in actionsdict: - found = False - for topicROS, topic_type in topics: - ros_server.own_rosnode_cleanup() - if actionNameOPC in topicROS: - found = True - if not found: - actionsdict[actionNameOPC].recursive_delete_items(actionsdict[actionNameOPC].parent) - tobedeleted.append(actionNameOPC) - rospy.logdebug("deleting action: " + actionNameOPC) - ros_server.own_rosnode_cleanup() - for name in tobedeleted: - del actionsdict[name] diff --git a/ros_opcua_impl_python_opcua/scripts/ros_info_manage.py b/ros_opcua_impl_python_opcua/scripts/ros_info_manage.py new file mode 100644 index 0000000..dc460c0 --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/ros_info_manage.py @@ -0,0 +1,338 @@ +import rospy +import rosgraph +import rosnode + +from opcua import ua +from ros_opc_ua_comm import OpcUaROSService, OpcUaROSTopic + +_action_feature_list = ('cancel', 'goal', 'result', 'feedback', 'status') +# keep the name update with the basic ros server and client +_server_name = 'rosopcua' +_client_name = 'opcuaclient' +_exclude_list = ('rosout', '/rosout', _server_name, '/' + _server_name, _client_name, '/' + _client_name) + + +def _nodeid_generator(idx): + return ua.NodeId(namespaceidx=idx) + + +class ROSServiceManager: + + def __init__(self, idx, node_root, type_dict, reverse_dict): + self._idx = idx + self._type_dict = type_dict + self._root = node_root.add_folder(_nodeid_generator(self._idx), 'Services') + self._services = {} + self._ua_nodes = {} + self._reverse_dict = reverse_dict + + def _create_service(self, name): + try: + service = OpcUaROSService(name, self._root, _nodeid_generator(self._idx), + self._type_dict, self._reverse_dict) + self._services[name] = service + self._ua_nodes[name] = service.get_node() + except Exception as e: + rospy.logerr(str(e)) + + def _delete_service(self, name): + self._services[name].delete_node() + del self._services[name] + del self._ua_nodes[name] + + def refresh_services(self, service_list): + if service_list != self._services.keys(): + delete_list = [name for name in self._services if name not in service_list] + add_list = [name for name in service_list if name not in self._services] + for node in delete_list: + self._delete_service(node) + for node in add_list: + self._create_service(node) + + def get_node_dict(self): + return self._ua_nodes + + +class ROSTopicManager: + + def __init__(self, idx, node_root, type_dict, reverse_dict): + self._idx = idx + self._type_dict = type_dict + self._root = node_root.add_folder(_nodeid_generator(self._idx), 'Topics') + self._pub_root = None + + self._topics = {} + self._status_ua_node = {} + self._publish_ua_node = {} + + self._reverse_dict = reverse_dict + + def _create_topic(self, name): + try: + if not self._pub_root: + self._pub_root = self._root.add_folder(_nodeid_generator(self._idx), 'Topic publish') + new_topic = OpcUaROSTopic(name, self._root, self._pub_root, self._type_dict, self._reverse_dict, + _nodeid_generator(self._idx), _nodeid_generator(self._idx)) + self._topics[name] = new_topic + self._status_ua_node[name] = new_topic.get_status_node() + self._publish_ua_node[name] = new_topic.get_publish_node() + except Exception as e: + rospy.logerr(str(e)) + + def _delete_topic(self, name): + self._topics[name].delete_node() + del self._topics[name] + del self._status_ua_node[name] + del self._publish_ua_node[name] + + def refresh_topics(self, topic_list): + if topic_list != self._topics.keys(): + delete_list = [name for name in self._topics if name not in topic_list] + add_list = [name for name in topic_list if name not in self._topics] + for node in delete_list: + self._delete_topic(node) + for node in add_list: + self._create_topic(node) + + def get_status_ua_node(self): + return self._status_ua_node + + def get_publish_ua_node(self): + return self._publish_ua_node + + +class ROSNodeManager: + + def __init__(self, idx, node_root, type_dict, service_nodes, topic_status_nodes, topic_publish_nodes): + self._idx = idx + self._type_dict = type_dict + self._root = node_root.add_folder(_nodeid_generator(self._idx), 'Nodes') + self._service_nodes = service_nodes + self._topic_status_nodes = topic_status_nodes + self._topic_publish_nodes = topic_publish_nodes + self._ua_nodes = {} + self._nodes_previous = {} + + def _link_services(self, ua_node, node_name, content): + if content: + srv_node = ua_node.add_folder(_nodeid_generator(self._idx), 'Services') + for service in content: + srv_node.add_reference(self._service_nodes[service], ua.ObjectIds.Organizes) + self._ua_nodes[node_name].append(srv_node) + + def _link_topics(self, ua_node, node_name, content_pub, content_sub): + if content_pub: + pub_node = ua_node.add_folder(_nodeid_generator(self._idx), 'Publications') + for publish in content_pub: + pub_node.add_reference(self._topic_status_nodes[publish], ua.ObjectIds.Organizes) + self._ua_nodes[node_name].append(pub_node) + if content_sub: + sub_node = ua_node.add_folder(_nodeid_generator(self._idx), 'Subscriptions') + sub_node_publish = sub_node.add_folder(_nodeid_generator(self._idx), 'Topic publish') + for subscribe in content_sub: + sub_node.add_reference(self._topic_status_nodes[subscribe], ua.ObjectIds.Organizes) + sub_node_publish.add_reference(self._topic_publish_nodes[subscribe], ua.ObjectIds.Organizes) + self._ua_nodes[node_name].append(sub_node) + self._ua_nodes[node_name].append(sub_node_publish) + + def _link_action(self, ua_node, node_name, content): + if content: + action_node = ua_node.add_folder(_nodeid_generator(self._idx), 'Action') + for topic in content['topics']: + action_node.add_reference(self._topic_status_nodes[topic], ua.ObjectIds.Organizes) + self._ua_nodes[node_name].append(action_node) + # action server, with extra publish + if content['is_action_server']: + action_publish = action_node.add_folder(_nodeid_generator(self._idx), 'Topic publish') + for topic in content['topics']: + if 'goal' in topic or 'cancel' in topic: + action_publish.add_reference(self._topic_publish_nodes[topic], ua.ObjectIds.Organizes) + self._ua_nodes[node_name].append(action_publish) + + def _create_node(self, node_name, node_content): + try: + ua_node = self._root.add_folder(_nodeid_generator(self._idx), node_name) + self._ua_nodes[node_name] = [ua_node] + self._link_services(ua_node, node_name, node_content['srvs']) + self._link_topics(ua_node, node_name, node_content['pubs'], node_content['subs']) + self._link_action(ua_node, node_name, node_content['acts']) + rospy.loginfo('Created ROS Node: ' + node_name) + except Exception as e: + rospy.logerr(str(e)) + + def _delete_node(self, node_name): + for node in self._ua_nodes[node_name]: + node.delete() + del self._ua_nodes[node_name] + rospy.loginfo('Deleted ROS Node: ' + node_name) + + def refresh_nodes(self, current_nodes): + if current_nodes != self._nodes_previous: + delete_list = [name for name in self._nodes_previous if name not in current_nodes] + add_dict = {name: content for name, content in current_nodes.items() if name not in self._nodes_previous} + for node in delete_list: + self._delete_node(node) + for name, content in add_dict.items(): + self._create_node(name, content) + self._nodes_previous = current_nodes + + +class SubHandler: + + def __init__(self, name, init_val): + self.old_val = init_val + self._name = name + + def datachange_notification(self, *value): + val = value[1] + if val != self.old_val: + rospy.set_param(self._name, val) + rospy.loginfo('ROS Parameter {} set to {}'.format(self._name, val)) + self.old_val = val + + +class ROSParamManager: + + def __init__(self, idx, node_root, writable=False, server=None): + self._server = server + self._idx = idx + self._root = node_root.add_folder(_nodeid_generator(self._idx), 'Parameters') + + self._ua_nodes = {} + self._sub_handler = {} + self._previous = {} + self._writable = writable + + def _create_param(self, param_name, param_value): + try: + new_param = self._root.add_property(_nodeid_generator(self._idx), param_name, param_value) + if self._writable: + new_param.set_writable() + sub = self._server.create_subscription(500, SubHandler(param_name, param_value)) + sub.subscribe_data_change(new_param) + self._sub_handler[param_name] = sub + self._ua_nodes[param_name] = new_param + rospy.loginfo('Created ROS Parameter: ' + param_name) + except Exception as e: + rospy.logerr(str(e)) + + def _delete_param(self, param_name): + self._ua_nodes[param_name].delete() + del self._ua_nodes[param_name] + if self._writable: + self._sub_handler[param_name].delete() + del self._sub_handler[param_name] + rospy.loginfo('Deleted ROS Parameter: ' + param_name) + + def refresh_params(self, param_list): + if param_list != self._previous: + add_dict = {name: content for name, content in param_list.items() if name not in self._previous} + delete_list = [name for name in self._previous if name not in param_list] + update_dict = {name: content for name, content in param_list.items() + if name in self._previous and name not in delete_list and content != self._previous[name]} + for name, content in add_dict.items(): + self._create_param(name, content) + for name in delete_list: + self._delete_param(name) + for name, content in update_dict.items(): + self._ua_nodes[name].set_value(content) + self._previous = param_list + + +def _is_action(name_list, feature_list): + # at least two features should match + feature_counter = 0 + for name in name_list: + if name.split('/')[-1] in feature_list: + feature_counter = feature_counter + 1 + if feature_counter >= 2: + return True + return False + + +def _add_action(pub_list, sub_list): + name_list = pub_list + sub_list + action_list = {'is_action_server': True if _is_action(sub_list, ('goal', 'cancel')) else False, + 'topics': [n for n in name_list if n.split('/')[-1] in _action_feature_list]} + return action_list + + +def _del_action(name_list): + return [n for n in name_list if n.split('/')[-1] not in _action_feature_list] + + +def _expand_params(param_dict, expand_dict, param_path=''): + + for k, v in param_dict.items(): + curr_path = param_path + '/' + k + if isinstance(v, dict): + _expand_params(v, expand_dict, curr_path) + else: + expand_dict[curr_path] = v + + +class ROSInfoAgent: + + def __init__(self, ros_namespace, show_nodes, show_topics, show_services, show_params): + self._exclude_list = _exclude_list + self._namespace = ros_namespace + self._ns = rosgraph.names.make_global_ns(self._namespace) + self._master = rosgraph.Master(_server_name) + self._show_nodes = show_nodes + self._show_topics = show_topics + self._show_services = show_services + self._show_params = show_params + + def _extract_info(self, info): + return sorted([t for t, l in info if (t == self._namespace or t.startswith(self._ns)) + and t not in self._exclude_list and not (len(l) == 1 and l[0] in self._exclude_list)]) + + def _extract_node_info(self, info, node): + return sorted([t for t, l in info if t not in self._exclude_list + and (t == self._namespace or t.startswith(self._ns)) + and node in l and not (len(l) == 1 and l[0] in self._exclude_list)]) + + def get_ros_info(self): + state = self._master.getSystemState() + + topics = [] + if self._show_topics: + topics = self._extract_info(state[0]) + topics += self._extract_info(state[1]) + topics = list(set(topics)) + + services = [] + if self._show_services: + services = self._extract_info(state[2]) + + nodes_info_dict = {} + if self._show_nodes: + nodes = [] + for s in state: + for t, l in s: + nodes.extend(l) + nodes = [node for node in list(set(nodes)) if node.split('/')[-1] not in self._exclude_list] + + for node in nodes: + node_info = {'pubs': self._extract_node_info(state[0], node) if self._show_topics else [], + 'subs': self._extract_node_info(state[1], node) if self._show_topics else [], + 'srvs': self._extract_node_info(state[2], node) if self._show_services else [], + 'acts': {}} + if _is_action(node_info['pubs'], _action_feature_list): + node_info['acts'] = _add_action(node_info['pubs'], node_info['subs']) + node_info['pubs'] = _del_action(node_info['pubs']) + node_info['subs'] = _del_action(node_info['subs']) + nodes_info_dict[node] = node_info + + expanded_params = {} + if self._show_params: + params = {key: value for key, value in self._master.getParam(self._namespace).items() + if key not in self._exclude_list} + _expand_params(params, expanded_params) + + return nodes_info_dict, services, topics, expanded_params + + def node_cleanup(self): + _, unpinged = rosnode.rosnode_ping_all() + if unpinged: + rosnode.cleanup_master_blacklist(self._master, unpinged) diff --git a/ros_opcua_impl_python_opcua/scripts/ros_opc_ua_client.py b/ros_opcua_impl_python_opcua/scripts/ros_opc_ua_client.py new file mode 100755 index 0000000..c9f40d0 --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/ros_opc_ua_client.py @@ -0,0 +1,294 @@ +#!/usr/bin/python +import io +import codecs + +import rospy +import roslib.message +from ros_opcua_msgs.msg import * +from ros_opcua_srvs.srv import * + +from opcua import ua, Client +from opcua.common.type_dictionary_buider import get_ua_class + +from ros_opc_ua_comm import ros_build_in_types, to_ua_class, to_ros_msg + + +def _get_value(data): + if data.type in ros_build_in_types: + return getattr(data, data.type + '_d') + elif not data.type: + return + else: + data_class = roslib.message.get_message_class(data.type) + if not data_class: + data_class = roslib.message.get_service_class(data.type) + escaped_value = codecs.escape_decode(data.string_d)[0] + ros_obj = data_class().deserialize(escaped_value) + return to_ua_class(ros_obj, get_ua_class(data.type)()) + + +def _set_value(data, msg_dict, ros_msg_name=None): + value = TypeValue() + if hasattr(data, 'ua_types'): + ros_class = roslib.message.get_message_class(ros_msg_name) + if not ros_class: + ros_class = roslib.message.get_service_class(ros_msg_name) + ros_msg = to_ros_msg(data, ros_class(), msg_dict) + value.type = ros_msg_name + serializer = io.BytesIO() + ros_msg.serialize(serializer) + value.string_d = str(serializer.getvalue()) + else: + var_type = ua.Variant(data).VariantType + for k, v in ros_build_in_types.items(): + if v == var_type: + value.type = k + setattr(value, k + '_d', data) + break + return value + + +def _get_name_in_argument(node): + for child in node.get_children(): + if child.get_browse_name().Name == 'OutputArguments': + return child.get_value()[0].Name + + +class SubHandler: + def __init__(self, pub, ros_msg_name, msg_dict): + self._pub = pub + self._ros_msg_name = ros_msg_name + self._msg_dict = msg_dict + + def datachange_notification(self, *val): + self._pub.publish(_set_value(val[1], self._msg_dict, self._ros_msg_name)) + + +class ROSOPCClient: + """ + Note that the name opcuaclient is in exclude list, launch this ros node makes no change on ua server. + """ + def __init__(self): + self._client = None + self._connected = False + self._node_name = 'opcuaclient' + self._sub_handlers = {} + self._topics = {} + self._ros_type_dict = {} + + def __enter__(self): + rospy.init_node(self._node_name, log_level=rospy.INFO) + self._setup_services() + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self._disconnect(None) + + def _retrieve_data_types(self, node): + for msg_node in node.get_children(refs=ua.ObjectIds.Organizes): + for msg_folder in msg_node.get_children(refs=ua.ObjectIds.Organizes): + for msg in msg_folder.get_children(refs=ua.ObjectIds.Organizes): + self._ros_type_dict[msg.get_browse_name().Name] = msg.get_display_name().Text + + def _setup_ros_server(self): + data_types = self._client.get_node(ua.TwoByteNodeId(ua.ObjectIds.DataTypesFolder)) + for node in data_types.get_children(): + if node.get_browse_name().Name == 'ROSDataType': + rospy.loginfo(' ----- ROS UA server detected! ------ ') + self._retrieve_data_types(node) + break + + def _setup_services(self): + rospy.Service(self._node_name + '/connect', Connect, self._connect) + rospy.Service(self._node_name + '/disconnect', Disconnect, self._disconnect) + rospy.Service(self._node_name + '/list_node', ListNode, self._list_node) + rospy.Service(self._node_name + '/call_method', CallMethod, self._call_method) + rospy.Service(self._node_name + '/read', Read, self._read) + rospy.Service(self._node_name + '/write', Write, self._write) + rospy.Service(self._node_name + '/subscribe', Subscribe, self._subscribe) + rospy.Service(self._node_name + '/unsubscribe', Unsubscribe, self._unsubscribe) + + def _connect(self, request): + response = ConnectResponse() + try: + self._client = Client(request.endpoint) + self._client.connect() + self._client.load_type_definitions() + self._root = self._client.get_objects_node() + self._setup_ros_server() + rospy.loginfo(' ----- Client {} connected! ------ '.format(request.endpoint)) + self._connected = True + response.success = True + except Exception as error: + rospy.logerr(' ----- Connection failed! ------ ') + response.success = False + response.error_message = str(error) + finally: + return response + + def _disconnect(self, _): + response = DisconnectResponse() + if self._connected: + try: + self._client.disconnect() + rospy.loginfo(' ----- Client disconnected! ------ ') + response.success = True + except Exception as error: + rospy.logerr(' ----- Connection failed! ------ ') + response.success = False + response.error_message = str(error) + else: + response.success = False + response.error_message = 'Client is not connected yet!' + return response + + def _add_addition_info(self, node, address): + node_type_id = node.get_type_definition() + if node_type_id: + node_type = self._client.get_node(node.get_type_definition()).get_browse_name() + address.nodeInfo.append('NodeType:' + node_type.Name.replace('Type', '')) + if node.get_node_class() == ua.NodeClass.Variable: + data_type = self._client.get_node(node.get_data_type()).get_display_name().Text + address.nodeInfo.append('DataType:' + data_type) + elif node.get_node_class() == ua.NodeClass.Method: + address.nodeInfo.append('NodeType:Method') + for child in node.get_children(): + if child.get_browse_name().Name == 'InputArguments': + for arg in child.get_value(): + if arg.Name: + address.nodeInfo.append('InputDataType:' + arg.Name) + else: + address.nodeInfo.append('InputDataType:' + ua.VariantType(arg.DataType.Identifier).name) + if child.get_browse_name().Name == 'OutputArguments': + for arg in child.get_value(): + if arg.Name: + address.nodeInfo.append('OutputDataType:' + arg.Name) + else: + address.nodeInfo.append('OutputDataType:' + ua.VariantType(arg.DataType.Identifier).name) + + def _list_node(self, request): + response = ListNodeResponse() + try: + if request.node.nodeId == '': + root = self._root + else: + root = self._client.get_node(request.node.nodeId) + for node in root.get_children(): + address = Address() + address.nodeId = node.nodeid.to_string() + address.qualifiedName = node.get_browse_name().Name + self._add_addition_info(node, address) + response.children.append(address) + rospy.loginfo(' ----- List Node "{}" successful! ------ '.format(root.get_browse_name().Name)) + response.success = True + except Exception as error: + rospy.logerr(' ----- List Node failed! ------ ') + response.success = False + response.error_message = str(error) + finally: + return response + + def _call_method(self, request): + response = CallMethodResponse() + try: + node = self._client.get_node(request.node.nodeId) + method_node = self._client.get_node(request.method.nodeId) + ros_msg_name = _get_name_in_argument(method_node) + inputs = [] + for data in request.data: + data = _get_value(data) + if data: + inputs.append(data) + outputs = node.call_method(method_node, *inputs) + if outputs: + if isinstance(outputs, list): + for output in outputs: + response.data.append(_set_value(output, self._ros_type_dict, ros_msg_name)) + else: + response.data.append(_set_value(outputs, self._ros_type_dict, ros_msg_name)) + rospy.loginfo(' ----- Call Method {} successful! ------ '.format(node.get_browse_name().Name)) + response.success = True + except Exception as error: + rospy.logerr(' ----- Call Method failed! ------ ') + response.success = False + response.error_message = str(error) + finally: + return response + + def _get_name_in_data_type(self, node): + return self._client.get_node(node.get_data_type()).get_display_name().Text + + def _read(self, request): + response = ReadResponse() + try: + node = self._client.get_node(request.node.nodeId) + response.data = _set_value(node.get_value(), self._ros_type_dict, self._get_name_in_data_type(node)) + rospy.loginfo(' ----- Read value of Node {} successful! ------ '.format(node.get_browse_name().Name)) + response.success = True + except Exception as error: + rospy.logerr(' ----- Read value failed! ------ ') + response.success = False + response.error_message = str(error) + finally: + return response + + def _write(self, request): + response = WriteResponse() + try: + node = self._client.get_node(request.node.nodeId) + node.set_value(_get_value(request.data)) + rospy.loginfo(' ----- Write value of Node {} successful! ------ '.format(node.get_browse_name().Name)) + response.success = True + except Exception as error: + rospy.logerr(' ----- Write value failed! ------ ') + response.success = False + response.error_message = str(error) + finally: + return response + + def _subscribe(self, request): + response = SubscribeResponse() + try: + node = self._client.get_node(request.node.nodeId) + pub = rospy.Publisher(request.callback_topic, TypeValue, queue_size=10) + self._topics[request.node.nodeId] = pub + sub_handler = self._client.create_subscription(500, SubHandler(pub, self._get_name_in_data_type(node), + self._ros_type_dict)) + sub_handler.subscribe_data_change(node) + self._sub_handlers[request.node.nodeId] = sub_handler + rospy.loginfo(' ----- Subscribe to Node {} successful! ------ '.format(node.get_browse_name().Name)) + response.success = True + except Exception as error: + rospy.logerr(' ----- Subscribe failed! ------ ') + response.success = False + response.error_message = str(error) + finally: + return response + + def _unsubscribe(self, request): + response = UnsubscribeResponse() + if request.node.nodeId in self._topics and request.node.nodeId in self._sub_handlers: + try: + self._topics[request.node.nodeId].unregister() + self._sub_handlers[request.node.nodeId].delete() + del self._topics[request.node.nodeId] + del self._sub_handlers[request.node.nodeId] + rospy.loginfo(' ----- Unsubscribe successful! ------ ') + response.success = True + except Exception as error: + rospy.logerr(' ----- Unsubscribe failed! ------ ') + response.success = False + response.error_message = str(error) + else: + response.success = False + response.error_message = 'No such a subscription!' + return response + + +if __name__ == '__main__': + try: + with ROSOPCClient(): + rospy.loginfo(' ----- ROS OPC UA Client started! ------ ') + rospy.spin() + except Exception as e: + print(e.message) diff --git a/ros_opcua_impl_python_opcua/scripts/ros_opc_ua_comm.py b/ros_opcua_impl_python_opcua/scripts/ros_opc_ua_comm.py new file mode 100644 index 0000000..19059ee --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/ros_opc_ua_comm.py @@ -0,0 +1,292 @@ +import rospy +import roslib.message +import rostopic +import rosservice +import rosmsg +import rospkg + +from opcua import ua, uamethod +from opcua.common.type_dictionary_buider import DataTypeDictionaryBuilder, get_ua_class + +ros_build_in_types = {'bool': ua.VariantType.Boolean, + 'int8': ua.VariantType.SByte, + 'byte': ua.VariantType.SByte, # deprecated int8 + 'uint8': ua.VariantType.Byte, + 'char': ua.VariantType.Byte, # deprecated uint8 + 'int16': ua.VariantType.Int16, + 'uint16': ua.VariantType.UInt16, + 'int32': ua.VariantType.Int32, + 'uint32': ua.VariantType.UInt32, + 'int64': ua.VariantType.Int64, + 'uint64': ua.VariantType.UInt64, + 'float32': ua.VariantType.Float, + 'float64': ua.VariantType.Float, + 'string': ua.VariantType.String} + +ua_basic_types = [item.name for item in ros_build_in_types.values()] + + +def expand_ua_class(obj, level=0): + """expand ua class to plain text""" + if not obj: + return None + buff = '\n{}ua_object, type: {}'.format(level * '\t', obj.__class__.__name__) + for var, data_type in obj.ua_types: + if data_type in ua_basic_types: + buff += '\n\t{}:{}'.format(level * '\t' + var, getattr(obj, var)) + else: + if isinstance(getattr(obj, var), list): + for member in getattr(obj, var): + if hasattr(member, 'ua_types'): + buff += expand_ua_class(member, level=level + 1) + else: + buff += '\n\t{}:{}'.format(level * '\t' + type(member).__name__, member) + else: + buff += expand_ua_class(getattr(obj, var), level=level + 1) + return buff + + +def _lookup_type(type_name): + if type_name in ros_build_in_types: + return ros_build_in_types[type_name] + return type_name + + +def _get_ros_packages(mode): + """ + same as the command line 'rosmsg packages' + :return: ROS messages as a list + """ + return sorted([x for x in rosmsg.iterate_packages(rospkg.RosPack(), mode)]) + + +def _get_ros_msg(mode): + ret = [] + if mode == rosmsg.MODE_MSG: + suffix = 'msg' + else: + suffix = 'srv' + ros_packages = _get_ros_packages(mode) + for (p, directory) in ros_packages: + for file_name in getattr(rosmsg, '_list_types')(directory, suffix, mode): + ret.append(p + '/' + file_name) + return ret + + +def _extract_ua_array_info(type_str): + is_array = False + if type_str.startswith('ListOf'): + type_str = type_str.split('ListOf', 1)[-1] + is_array = True + + return type_str, is_array + + +def _extract_ros_array_info(type_str): + """ROS only support 1 dimensional array""" + is_array = False + if '[' in type_str: + type_str = type_str.split('[', 1)[0] + is_array = True + + return type_str, is_array + + +def to_ros_msg(ua_obj, ros_msg, msg_dict): + for attr in ua_obj.ua_types: + base_type_str, is_array = _extract_ua_array_info(attr[1]) + if base_type_str in ua_basic_types: + setattr(ros_msg, attr[0], getattr(ua_obj, attr[0])) + else: + if is_array: + for member in getattr(ua_obj, attr[0]): + new_ros_msg = roslib.message.get_message_class(msg_dict[base_type_str])() + getattr(ros_msg, attr[0]).append(new_ros_msg) + to_ros_msg(member, new_ros_msg, msg_dict) + else: + to_ros_msg(getattr(ua_obj, attr[0]), getattr(ros_msg, attr[0]), msg_dict) + return ros_msg + + +def to_ua_class(ros_msg, ua_obj): + for attr, data_type in zip(ros_msg.__slots__, getattr(ros_msg, '_slot_types')): + base_type_str, is_array = _extract_ros_array_info(data_type) + if base_type_str in ros_build_in_types: + setattr(ua_obj, attr, getattr(ros_msg, attr)) + else: + if is_array: + for member in getattr(ros_msg, attr): + new_ua_class = get_ua_class(getattr(member, '_type'))() + getattr(ua_obj, attr).append(new_ua_class) + to_ua_class(member, new_ua_class) + else: + to_ua_class(getattr(ros_msg, attr), getattr(ua_obj, attr)) + return ua_obj + + +def _create_args(msg_class, data_type): + """one extension object contains all info""" + if not len(getattr(msg_class, '__slots__')): + return [] + msg_class_name = getattr(msg_class, '_type') + arg = ua.Argument() + arg.Name = msg_class_name + arg.DataType = data_type + arg.ValueRank = -1 + arg.ArrayDimensions = [] + arg.Description = ua.LocalizedText(msg_class_name) + return [arg] + + +class OpcUaROSMessage: + def __init__(self, server, idx, idx_name): + self._server = server + self._idx = idx + + self._created_struct_nodes = {} + self._dict_builder = DataTypeDictionaryBuilder(server, idx, idx_name, 'ROSDictionary') + + def _is_new_type(self, message): + if not isinstance(message, str): + message = message.name + return message not in ros_build_in_types and message not in self._created_struct_nodes + + def _create_data_type(self, type_name): + new_dt = self._dict_builder.create_data_type(type_name) + self._created_struct_nodes[type_name] = new_dt + return new_dt + + def _recursively_create_message(self, msg): + msg = self._create_data_type(msg) if self._is_new_type(msg) else self._created_struct_nodes[msg] + message = roslib.message.get_message_class(msg.name) + if not message: # Broken packages + return + for variable_type, data_type in zip(message.__slots__, getattr(message, '_slot_types')): + base_type_str, is_array = _extract_ros_array_info(data_type) + if self._is_new_type(base_type_str): + self._create_data_type(base_type_str) + self._recursively_create_message(base_type_str) + + msg.add_field(variable_type, _lookup_type(base_type_str), is_array) + + def _create_messages(self): + # TODO: maybe apply filter here for partial loading + messages = _get_ros_msg(rosmsg.MODE_MSG) + for msg in messages: + if msg not in self._created_struct_nodes: + self._recursively_create_message(msg) + + def _process_service_classes(self, srv): + msg_name = getattr(srv, '_type') + new_struct = self._create_data_type(msg_name) + for variable_type, data_type in zip(srv.__slots__, getattr(srv, '_slot_types')): + base_type_str, is_array = _extract_ros_array_info(data_type) + new_struct.add_field(variable_type, _lookup_type(base_type_str), is_array) + + def _create_services(self): + """since srv can not embed another .srv, no recursion is needed""" + # TODO: maybe apply filter here for partial loading + services = _get_ros_msg(rosmsg.MODE_SRV) + for srv in services: + service = roslib.message.get_service_class(srv) + if not service: # Broken packages + continue + self._process_service_classes(getattr(service, '_request_class')) + self._process_service_classes(getattr(service, '_response_class')) + + def create_ros_data_types(self): + self._create_messages() + created_msgs = {key: value.data_type for key, value in self._created_struct_nodes.items()} + self._create_services() + self._dict_builder.set_dict_byte_string() + data_types = {key: value.data_type for key, value in self._created_struct_nodes.items()} + created_srvs = {key: value for key, value in data_types.items() if key not in created_msgs} + return data_types, created_msgs, created_srvs + + +class OpcUaROSService: + + def __init__(self, service_name, node_root, service_node_id, msg_dict, reverse_dict): + self._service_class = rosservice.get_service_class_by_name(service_name) + self._service_name = service_name + self._proxy = rospy.ServiceProxy(service_name, self._service_class) + self._node_root = node_root + + self._ros_service_req = getattr(self._service_class, '_request_class') + self._req_name = getattr(self._ros_service_req, '_type') + self._ros_service_resp = getattr(self._service_class, '_response_class') + self._resp_name = getattr(self._ros_service_resp, '_type') + + in_dt_node_id = msg_dict[self._req_name] + out_dt_node_id = msg_dict[self._resp_name] + input_arg = _create_args(self._ros_service_req, in_dt_node_id) + output_arg = _create_args(self._ros_service_resp, out_dt_node_id) + + self._method = node_root.add_method(service_node_id, service_name, self._call_service, input_arg, output_arg) + self._msg_dict = reverse_dict + rospy.loginfo('Created ROS Service: ' + service_name) + + @uamethod + def _call_service(self, _, *inputs): + rospy.loginfo('Calling service {0} under ROS node: {1}'.format(self._service_name, + self._node_root.get_display_name().Text)) + try: + ua_obj = inputs[0] if inputs else get_ua_class(self._req_name)() + response = self._proxy(to_ros_msg(ua_obj, self._ros_service_req(), self._msg_dict)) + rospy.loginfo('Calling service succeeded!') + if response.__slots__: + resp_ua_class = to_ua_class(response, get_ua_class(self._resp_name)()) + return ua.Variant(resp_ua_class) + except Exception as e: + rospy.logerr('Error when calling service ' + self._service_name, e) + + def get_node(self): + return self._method + + def delete_node(self): + self._proxy.close() + self._method.delete() + rospy.loginfo('Deleted ROS Service: ' + self._service_name) + + +class OpcUaROSTopic: + + def __init__(self, topic_name, status_root, pub_root, msg_dict, reverse_dict, *node_id): + self._topic_name = topic_name + self._topic_class, _, _ = rostopic.get_topic_class(self._topic_name) + self._msg_name = getattr(self._topic_class, '_type') + self._topic = status_root.add_property(node_id[0], self._topic_name, ua.Variant(None, ua.VariantType.Null), + datatype=msg_dict[self._msg_name]) + self._pub_handler = rospy.Publisher(self._topic_name, self._topic_class, queue_size=10) + self._sub_handler = rospy.Subscriber(self._topic_name, self._topic_class, self._message_callback) + + input_arg = _create_args(self._topic_class, msg_dict[self._msg_name]) + self._method = pub_root.add_method(node_id[1], self._topic_name, self._call_publish, input_arg, []) + self._msg_dict = reverse_dict + rospy.loginfo('Created ROS Topic: ' + self._topic_name) + + def _message_callback(self, message): + self._topic.set_value(to_ua_class(message, get_ua_class(self._msg_name)())) + + @uamethod + def _call_publish(self, _, *inputs): + rospy.loginfo('Publishing data under ROS Topic: ' + self._topic_name) + try: + ua_obj = inputs[0] if inputs else get_ua_class(self._topic_class)() + self._pub_handler.publish(to_ros_msg(ua_obj, self._topic_class(), self._msg_dict)) + rospy.loginfo('Topic publishing succeeded!') + except Exception as e: + rospy.logerr('Error when publishing topic ' + self._topic, e) + + def get_status_node(self): + return self._topic + + def get_publish_node(self): + return self._method + + def delete_node(self): + self._sub_handler.unregister() + self._method.delete() + self._topic.delete() + self._pub_handler.unregister() + rospy.loginfo('Deleted ROS Topic: ' + self._topic_name) diff --git a/ros_opcua_impl_python_opcua/scripts/ros_server.py b/ros_opcua_impl_python_opcua/scripts/ros_server.py index ca77115..8b9eb7b 100755 --- a/ros_opcua_impl_python_opcua/scripts/ros_server.py +++ b/ros_opcua_impl_python_opcua/scripts/ros_server.py @@ -1,103 +1,107 @@ #!/usr/bin/python -import sys import time - -import rosgraph -import rosnode import rospy -from opcua import Server, ua -import ros_services -import ros_topics +from opcua import ua +from ros_info_manage import ROSServiceManager, ROSTopicManager, ROSNodeManager, ROSParamManager, ROSInfoAgent +from basic_server_client import ROSBasicServer -# Returns the hierachy as one string from the first remaining part on. -def nextname(hierachy, index_of_last_processed): - try: - output = "" - counter = index_of_last_processed + 1 - while counter < len(hierachy): - output += hierachy[counter] - counter += 1 - return output - except Exception as e: - rospy.logerr("Error encountered ", e) +class ROSServer(ROSBasicServer): -def own_rosnode_cleanup(): - pinged, unpinged = rosnode.rosnode_ping_all() - if unpinged: - master = rosgraph.Master(rosnode.ID) - # noinspection PyTypeChecker - rosnode.cleanup_master_blacklist(master, unpinged) + def __init__(self): + ROSBasicServer.__init__(self) + self._root = self._server.nodes.objects + self._type_root = self._server.nodes.data_types.add_folder(self._nodeid_generator(), 'ROSDataType') + self._msg_folder = self._type_root.add_folder(self._nodeid_generator(), 'MsgType') + self._srv_folder = self._type_root.add_folder(self._nodeid_generator(), 'SrvType') + self._agent = ROSInfoAgent(self._namespace_ros, self._show_nodes, self._show_topics, + self._show_services, self._show_params) -class ROSServer: - def __init__(self): - self.namespace_ros = rospy.get_param("/rosopcua/namespace") - self.topicsDict = {} - self.servicesDict = {} - self.actionsDict = {} - rospy.init_node("rosopcua") - self.server = Server() - self.server.set_endpoint("opc.tcp://0.0.0.0:21554/") - self.server.set_server_name("ROS ua Server") - self.server.start() - # setup our own namespaces, this is expected - uri_topics = "http://ros.org/topics" - # two different namespaces to make getting the correct node easier for get_node (otherwise had object for service and topic with same name - uri_services = "http://ros.org/services" - uri_actions = "http://ros.org/actions" - idx_topics = self.server.register_namespace(uri_topics) - idx_services = self.server.register_namespace(uri_services) - idx_actions = self.server.register_namespace(uri_actions) - # get Objects node, this is where we should put our custom stuff - objects = self.server.get_objects_node() - # one object per type we are watching - topics_object = objects.add_object(idx_topics, "ROS-Topics") - services_object = objects.add_object(idx_services, "ROS-Services") - actions_object = objects.add_object(idx_actions, "ROS_Actions") + def _load_messages(self): + # FIXME: 1. bugs after xml import, extension object can not be used + # 2. should compare in system to get correct msgs and srvs after xml loaded. + if self._import_xml_msgs: + self._type_dict = self.import_messages() + else: + self.load_messages() + + self._server.load_type_definitions() + self._create_static_info() + + def _link_msg(self, msg_folder, msg_dict): + lib_folders = {} + for msg in msg_dict: + lib_name = msg.split('/')[0] + if lib_name not in lib_folders: + lib_folders[lib_name] = msg_folder.add_folder(self._nodeid_generator(), lib_name) + lib_folders[lib_name].add_reference(msg_dict[msg], ua.ObjectIds.Organizes) + + def _create_static_info(self): + """ + show rosmsg and rossrv in ua server + :return: + """ + self._link_msg(self._msg_folder, self._msgs_dict) + self._link_msg(self._srv_folder, self._srvs_dict) + + def _create_reverse_dict(self): + reverse_dict = {} + for msg in self._type_dict: + node = self._server.get_node(self._type_dict[msg]) + reverse_dict[node.get_browse_name().Name] = msg + return reverse_dict + + def _initialize_info_managers(self): + reverse_dict = self._create_reverse_dict() + self._service_manager = ROSServiceManager(self._idx, self._root, self._type_dict, reverse_dict) + self._topic_manager = ROSTopicManager(self._idx, self._root, self._type_dict, reverse_dict) + + service_ua_nodes = self._service_manager.get_node_dict() + topic_status_nodes = self._topic_manager.get_status_ua_node() + topic_publish_nodes = self._topic_manager.get_publish_ua_node() + self._node_manager = ROSNodeManager(self._idx, self._root, self._type_dict, service_ua_nodes, + topic_status_nodes, topic_publish_nodes) + self._param_manager = ROSParamManager(self._idx, self._root, self._para_writable, self._server) + + def _refresh_info(self, initial_refresh=False): + self._agent.node_cleanup() + current_nodes, current_services, current_topics, current_params = self._agent.get_ros_info() + self._service_manager.refresh_services(current_services) + self._topic_manager.refresh_topics(current_topics) + self._node_manager.refresh_nodes(current_nodes) + if initial_refresh and self._para_writable: + return + self._param_manager.refresh_params(current_params) + + def _auto_refresh_info(self): while not rospy.is_shutdown(): - # ros_topics starts a lot of publisher/subscribers, might slow everything down quite a bit. - ros_services.refresh_services(self.namespace_ros, self, self.servicesDict, idx_services, services_object) - ros_topics.refresh_topics_and_actions(self.namespace_ros, self, self.topicsDict, self.actionsDict, - idx_topics, idx_actions, topics_object, actions_object) - # Don't clog cpu - time.sleep(60) - self.server.stop() - quit() - - def find_service_node_with_same_name(self, name, idx): - rospy.logdebug("Reached ServiceCheck for name " + name) - for service in self.servicesDict: - rospy.logdebug("Found name: " + str(self.servicesDict[service].parent.nodeid.Identifier)) - if self.servicesDict[service].parent.nodeid.Identifier == name: - rospy.logdebug("Found match for name: " + name) - return self.servicesDict[service].parent - return None - - def find_topics_node_with_same_name(self, name, idx): - rospy.logdebug("Reached TopicCheck for name " + name) - for topic in self.topicsDict: - rospy.logdebug("Found name: " + str(self.topicsDict[topic].parent.nodeid.Identifier)) - if self.topicsDict[topic].parent.nodeid.Identifier == name: - rospy.logdebug("Found match for name: " + name) - return self.topicsDict[topic].parent - return None - - def find_action_node_with_same_name(self, name, idx): - rospy.logdebug("Reached ActionCheck for name " + name) - for topic in self.actionsDict: - rospy.logdebug("Found name: " + str(self.actionsDict[topic].parent.nodeid.Identifier)) - if self.actionsDict[topic].parent.nodeid.Identifier == name: - rospy.logdebug("Found match for name: " + name) - return self.actionsDict[topic].parent - return None - - -def main(args): - rosserver = ROSServer() - - -if __name__ == "__main__": - main(sys.argv) + self._refresh_info() + time.sleep(self._refresh_cycle_time) + + def initialize_server(self): + self._load_messages() + self._initialize_info_managers() + self._refresh_info(True) + self._start_server() + + def refresh(self): + """ + For manual refresh please call this method + :return: + """ + if self._auto_refresh: + self._auto_refresh_info() + else: + self._refresh_info() + + +if __name__ == '__main__': + try: + with ROSServer() as ua_server: + ua_server.initialize_server() + ua_server.refresh() + except Exception as e: + print(e.message) diff --git a/ros_opcua_impl_python_opcua/scripts/ros_server_export_message.py b/ros_opcua_impl_python_opcua/scripts/ros_server_export_message.py new file mode 100755 index 0000000..87f92ee --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/ros_server_export_message.py @@ -0,0 +1,23 @@ +#!/usr/bin/python +import rospy + +from opcua.common.ua_utils import get_nodes_of_namespace +from basic_server_client import ROSBasicServer + + +class ROSServer(ROSBasicServer): + + def export_messages(self): + rospy.loginfo(' ----- Exporting node message to xml ------ ') + node_to_export = get_nodes_of_namespace(self._server, [self._idx]) + self._server.export_xml(node_to_export, self._message_path) + rospy.loginfo(' ----- {0} nodes exported to {1}------ '.format(len(node_to_export), self._message_path)) + + +if __name__ == '__main__': + try: + with ROSServer() as ua_server: + ua_server.load_messages() + ua_server.export_messages() + except Exception as e: + print(e.message) diff --git a/ros_opcua_impl_python_opcua/scripts/ros_services.py b/ros_opcua_impl_python_opcua/scripts/ros_services.py deleted file mode 100644 index e32eb8d..0000000 --- a/ros_opcua_impl_python_opcua/scripts/ros_services.py +++ /dev/null @@ -1,247 +0,0 @@ -#!/usr/bin/env python - -# Thanks to: -# https://github.com/ros-visualization/rqt_common_plugins/blob/groovy-devel/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py -import math -import random -import time - -import genpy -import rospy -import rosservice -from opcua import ua, uamethod, common - -import ros_server - - -class OpcUaROSService: - def __init__(self, server, parent, idx, service_name, service_class): - self.server = server - self.name = service_name - self.parent = self.recursive_create_objects(service_name, idx, parent) - self._class = service_class - self.proxy = rospy.ServiceProxy(self.name, self._class) - self.counter = 0 - self._nodes = {} - self.expressions = {} - self._eval_locals = {} - - for module in (math, random, time): - self._eval_locals.update(module.__dict__) - self._eval_locals['genpy'] = genpy - del self._eval_locals['__name__'] - del self._eval_locals['__doc__'] - # Build the Array of inputs - self.sample_req = self._class._request_class() - self.sample_resp = self._class._response_class() - inputs = getargarray(self.sample_req) - self.outputs = getargarray(self.sample_resp) - self.method = self.parent.add_method(idx, self.name, self.call_service, inputs, self.outputs) - rospy.loginfo("Created ROS Service with name: %s", self.name) - - @uamethod - def call_service(self, parent, *inputs): - try: - rospy.loginfo("Calling Service with name: " + self.name) - input_msg = self.create_message_instance(inputs, self.sample_req) - rospy.logdebug("Created Input Request for Service " + self.name + " : " + str(input_msg)) - response = self.proxy(input_msg) - rospy.logdebug("got response: " + str(response)) - rospy.logdebug("Creating response message object") - return_values = [] - for slot in response.__slots__: - rospy.logdebug("Converting slot: " + str(getattr(response, slot))) - return_values.append(getattr(response, slot)) - rospy.logdebug("Current Response list: " + str(return_values)) - return return_values - except (TypeError, rospy.ROSException, rospy.ROSInternalException, rospy.ROSSerializationException, - common.uaerrors.UaError, rosservice.ROSServiceException) as e: - rospy.logerr("Error when calling service " + self.name, e) - - def create_message_instance(self, inputs, sample): - rospy.logdebug("Creating message for goal call") - already_set = [] - if isinstance(inputs, tuple): - arg_counter = 0 - object_counter = 0 - while (arg_counter < len(inputs) and object_counter < len(sample.__slots__)): - cur_arg = inputs[arg_counter] - cur_slot = sample.__slots__[object_counter] - real_slot = getattr(sample, cur_slot) - rospy.logdebug( - "cur_arg: " + str(cur_arg) + " cur_slot_name: " + str(cur_slot) + " real slot content: " + str( - real_slot)) - if hasattr(real_slot, '_type'): - rospy.logdebug("We found an object with name " + str(cur_slot) + ", creating it recursively") - already_set, arg_counter = self.create_object_instance(already_set, real_slot, cur_slot, - arg_counter, inputs, sample) - object_counter += 1 - else: - already_set.append(cur_slot) - # set the attribute in the request - setattr(sample, cur_slot, cur_arg) - arg_counter += 1 - object_counter += 1 - - return sample - - def create_object_instance(self, already_set, object, name, counter, inputs, sample): - rospy.loginfo("Create Object Instance Notify") - object_counter = 0 - while (object_counter < len(object.__slots__) and counter < len(inputs)): - cur_arg = inputs[counter] - cur_slot = object.__slots__[object_counter] - real_slot = getattr(object, cur_slot) - rospy.loginfo( - "cur_arg: " + str(cur_arg) + " cur_slot_name: " + str(cur_slot) + " real slot content: " + str( - real_slot)) - if hasattr(real_slot, '_type'): - rospy.logdebug("Recursive Object found in request/response of service call") - already_set, counter = self.create_object_instance(already_set, real_slot, cur_slot, counter, inputs, - sample) - object_counter += 1 - else: - already_set.append(cur_slot) - setattr(object, cur_slot, cur_arg) - object_counter += 1 - counter += 1 - # sets the object as an attribute in the request were trying to build - setattr(sample, name, object) - return already_set, counter - - def recursive_delete_items(self, item): - self.proxy.close() - for child in item.get_children(): - self.recursive_delete_items(child) - if child in self._nodes: - del self._nodes[child] - self.server.server.delete_nodes([child]) - self.server.server.delete_nodes([self.method]) - ros_server.own_rosnode_cleanup() - - def recursive_create_objects(self, name, idx, parent): - hierachy = name.split('/') - if len(hierachy) == 0 or len(hierachy) == 1: - return parent - for name in hierachy: - if name != '': - try: - nodewithsamename = self.server.find_service_node_with_same_name(name, idx) - rospy.logdebug("nodewithsamename for name: " + str(name) + " is : " + str(nodewithsamename)) - if nodewithsamename is not None: - rospy.logdebug("recursive call for same name for: " + name) - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - nodewithsamename) - else: - newparent = parent.add_object( - ua.NodeId(name, parent.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName(name, parent.nodeid.NamespaceIndex)) - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - newparent) - except IndexError, common.uaerrors.UaError: - newparent = parent.add_object( - ua.NodeId(name + str(random.randint(0, 10000)), parent.nodeid.NamespaceIndex, - ua.NodeIdType.String), - ua.QualifiedName(name, parent.nodeid.NamespaceIndex)) - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - newparent) - return parent - - -def getargarray(sample_req): - array = [] - for slot_name in sample_req.__slots__: - slot = getattr(sample_req, slot_name) - if hasattr(slot, '_type'): - array_to_merge = getargarray(slot) - array.extend(array_to_merge) - else: - if isinstance(slot, list): - arg = ua.Argument() - arg.Name = slot_name - arg.DataType = ua.NodeId(getobjectidfromtype("array")) - arg.ValueRank = -1 - arg.ArrayDimensions = [1] - arg.Description = ua.LocalizedText("Array") - else: - arg = ua.Argument() - arg.Name = slot_name - arg.DataType = ua.NodeId(getobjectidfromtype(type(slot).__name__)) - arg.ValueRank = -1 - arg.ArrayDimensions = [] - arg.Description = ua.LocalizedText(slot_name) - array.append(arg) - - return array - - -def refresh_services(namespace_ros, server, servicesdict, idx, services_object_opc): - rosservices = rosservice.get_service_list(namespace=namespace_ros) - - for service_name_ros in rosservices: - try: - if service_name_ros not in servicesdict or servicesdict[service_name_ros] is None: - service = OpcUaROSService(server, services_object_opc, idx, service_name_ros, - rosservice.get_service_class_by_name(service_name_ros)) - servicesdict[service_name_ros] = service - except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e: - try: - rospy.logerr("Error when trying to refresh services", e) - except TypeError as e2: - rospy.logerr("Error when logging an Exception, can't convert everything to string") - # use extra iteration as to not get "dict changed during iteration" errors - tobedeleted = [] - rosservices = rosservice.get_service_list() - for service_nameOPC in servicesdict: - found = False - for rosservice_name in rosservices: - if service_nameOPC == rosservice_name: - found = True - if not found and servicesdict[service_nameOPC] is not None: - servicesdict[service_nameOPC].recursive_delete_items( - server.server.get_node(ua.NodeId(service_nameOPC, idx))) - tobedeleted.append(service_nameOPC) - if len(servicesdict[service_nameOPC].parent.get_children()) == 0: - server.server.delete_nodes([servicesdict[service_nameOPC].parent]) - for name in tobedeleted: - del servicesdict[name] - - -def getobjectidfromtype(type_name): - if type_name == 'bool': - dv = ua.ObjectIds.Boolean - elif type_name == 'byte': - dv = ua.ObjectIds.Byte - elif type_name == 'int': - dv = ua.ObjectIds.Int16 - elif type_name == 'int8': - dv = ua.ObjectIds.SByte - elif type_name == 'uint8': - dv = ua.ObjectIds.Byte - elif type_name == 'int16': - dv = ua.ObjectIds.Int16 - rospy.roswarn("Int16??") - elif type_name == 'uint16': - dv = ua.ObjectIds.UInt16 - elif type_name == 'int32': - dv = ua.ObjectIds.Int32 - elif type_name == 'uint32': - dv = ua.ObjectIds.UInt32 - elif type_name == 'int64': - dv = ua.ObjectIds.Int64 - elif type_name == 'uint64': - dv = ua.ObjectIds.UInt64 - elif type_name == 'float' or type_name == 'float32' or type_name == 'float64': - dv = ua.ObjectIds.Float - elif type_name == 'double': - dv = ua.ObjectIds.Double - elif type_name == 'string' or type_name == 'str': - dv = ua.ObjectIds.String - elif type_name == 'array': - dv = ua.ObjectIds.Enumeration - elif type_name == 'Time' or type_name == 'time': - dv = ua.ObjectIds.Time - else: - rospy.logerr("Can't create type with name " + type_name) - return None - return dv diff --git a/ros_opcua_impl_python_opcua/scripts/ros_topics.py b/ros_opcua_impl_python_opcua/scripts/ros_topics.py deleted file mode 100644 index 7410fb7..0000000 --- a/ros_opcua_impl_python_opcua/scripts/ros_topics.py +++ /dev/null @@ -1,339 +0,0 @@ -#!/usr/bin/env python - -# Thanks to: -# https://github.com/ros-visualization/rqt_common_plugins/blob/groovy-devel/rqt_topic/src/rqt_topic/topic_widget.py -import numpy -import random - -import roslib -import roslib.message -import rospy -from opcua import ua, uamethod - -import ros_actions -import ros_server -import rostopic - - -class OpcUaROSTopic: - def __init__(self, server, parent, idx, topic_name, topic_type): - self.server = server - self.parent = self.recursive_create_objects(topic_name, idx, parent) - self.type_name = topic_type - self.name = topic_name - self._nodes = {} - self.idx = idx - - self.message_class = None - try: - self.message_class = roslib.message.get_message_class(topic_type) - self.message_instance = self.message_class() - - except rospy.ROSException: - self.message_class = None - rospy.logfatal("Couldn't find message class for type " + topic_type) - - self._recursive_create_items(self.parent, idx, topic_name, topic_type, self.message_instance, True) - - self._subscriber = rospy.Subscriber(self.name, self.message_class, self.message_callback) - self._publisher = rospy.Publisher(self.name, self.message_class, queue_size=1) - rospy.loginfo("Created ROS Topic with name: " + str(self.name)) - - def _recursive_create_items(self, parent, idx, topic_name, type_name, message, top_level=False): - topic_text = topic_name.split('/')[-1] - if '[' in topic_text: - topic_text = topic_text[topic_text.index('['):] - - # This here are 'complex datatypes' - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - complex_type = True - new_node = parent.add_object(ua.NodeId(topic_name, parent.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName(topic_name, parent.nodeid.NamespaceIndex)) - new_node.add_property(ua.NodeId(topic_name + ".Type", idx), - ua.QualifiedName("Type", parent.nodeid.NamespaceIndex), type_name) - if top_level: - new_node.add_method(ua.NodeId(topic_name + ".Update", parent.nodeid.NamespaceIndex), - ua.QualifiedName("Update", parent.nodeid.NamespaceIndex), - self.opcua_update_callback, [], []) - for slot_name, type_name_child in zip(message.__slots__, message._slot_types): - self._recursive_create_items(new_node, idx, topic_name + '/' + slot_name, type_name_child, - getattr(message, slot_name)) - self._nodes[topic_name] = new_node - - else: - # This are arrays - base_type_str, array_size = _extract_array_info(type_name) - try: - base_instance = roslib.message.get_message_class(base_type_str)() - except (ValueError, TypeError): - base_instance = None - - if array_size is not None and hasattr(base_instance, '__slots__'): - for index in range(array_size): - self._recursive_create_items(parent, idx, topic_name + '[%d]' % index, base_type_str, base_instance) - else: - new_node = _create_node_with_type(parent, idx, topic_name, topic_text, type_name, array_size) - self._nodes[topic_name] = new_node - - if topic_name in self._nodes and self._nodes[topic_name].get_node_class() == ua.NodeClass.Variable: - self._nodes[topic_name].set_writable(True) - return - - def message_callback(self, message): - self.update_value(self.name, message) - - @uamethod - def opcua_update_callback(self, parent): - try: - for nodeName in self._nodes: - child = self._nodes[nodeName] - name = child.get_display_name().Text - if hasattr(self.message_instance, name): - if child.get_node_class() == ua.NodeClass.Variable: - setattr(self.message_instance, name, - correct_type(child, type(getattr(self.message_instance, name)))) - elif child.get_node_class == ua.NodeClass.Object: - setattr(self.message_instance, name, self.create_message_instance(child)) - self._publisher.publish(self.message_instance) - except rospy.ROSException as e: - rospy.logerr("Error when updating node " + self.name, e) - self.server.server.delete_nodes([self.parent]) - - def update_value(self, topic_name, message): - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name in message.__slots__: - self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name)) - - elif type(message) in (list, tuple): - if (len(message) > 0) and hasattr(message[0], '__slots__'): - for index, slot in enumerate(message): - if topic_name + '[%d]' % index in self._nodes: - self.update_value(topic_name + '[%d]' % index, slot) - else: - if topic_name in self._nodes: - base_type_str, _ = _extract_array_info( - self._nodes[topic_name].text(self.type_name)) - self._recursive_create_items(self._nodes[topic_name], topic_name + '[%d]' % index, - base_type_str, - slot, None) - # remove obsolete children - if topic_name in self._nodes: - if len(message) < len(self._nodes[topic_name].get_children()): - for i in range(len(message), self._nodes[topic_name].childCount()): - item_topic_name = topic_name + '[%d]' % i - self.recursive_delete_items(self._nodes[item_topic_name]) - del self._nodes[item_topic_name] - else: - if topic_name in self._nodes and self._nodes[topic_name] is not None: - self._nodes[topic_name].set_value(repr(message)) - - def recursive_delete_items(self, item): - self._publisher.unregister() - self._subscriber.unregister() - for child in item.get_children(): - self.recursive_delete_items(child) - if child in self._nodes: - del self._nodes[child] - self.server.server.delete_nodes([child]) - self.server.server.delete_nodes([item]) - if len(self.parent.get_children()) == 0: - self.server.server.delete_nodes([self.parent]) - - def create_message_instance(self, node): - for child in node.get_children(): - name = child.get_display_name().Text - if hasattr(self.message_instance, name): - if child.get_node_class() == ua.NodeClass.Variable: - setattr(self.message_instance, name, - correct_type(child, type(getattr(self.message_instance, name)))) - elif child.get_node_class == ua.NodeClass.Object: - setattr(self.message_instance, name, self.create_message_instance(child)) - return self.message_instance # Converts the value of the node to that specified in the ros message we are trying to fill. Casts python ints - - def recursive_create_objects(self, topic_name, idx, parent): - hierachy = topic_name.split('/') - if len(hierachy) == 0 or len(hierachy) == 1: - return parent - for name in hierachy: - if name != '': - try: - nodewithsamename = self.server.find_topics_node_with_same_name(name, idx) - if nodewithsamename is not None: - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - nodewithsamename) - else: - # if for some reason 2 services with exactly same name are created use hack>: add random int, prob to hit two - # same ints 1/10000, should be sufficient - newparent = parent.add_object( - ua.NodeId(name, parent.nodeid.NamespaceIndex, ua.NodeIdType.String), - ua.QualifiedName(name, parent.nodeid.NamespaceIndex)) - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - newparent) - # thrown when node with parent name is not existent in server - except IndexError, common.UaError: - newparent = parent.add_object( - ua.NodeId(name + str(random.randint(0, 10000)), parent.nodeid.NamespaceIndex, - ua.NodeIdType.String), - ua.QualifiedName(name, parent.nodeid.NamespaceIndex)) - return self.recursive_create_objects(ros_server.nextname(hierachy, hierachy.index(name)), idx, - newparent) - - return parent - - -# to unsigned integers as to fulfill ros specification. Currently only uses a few different types, -# no other types encountered so far. -def correct_type(node, typemessage): - data_value = node.get_data_value() - result = node.get_value() - if isinstance(data_value, ua.DataValue): - if typemessage.__name__ == "float": - result = numpy.float(result) - if typemessage.__name__ == "double": - result = numpy.double(result) - if typemessage.__name__ == "int": - result = int(result) & 0xff - else: - rospy.logerr("can't convert: " + str(node.get_data_value.Value)) - return None - return result - - -def _extract_array_info(type_str): - array_size = None - if '[' in type_str and type_str[-1] == ']': - type_str, array_size_str = type_str.split('[', 1) - array_size_str = array_size_str[:-1] - if len(array_size_str) > 0: - array_size = int(array_size_str) - else: - array_size = 0 - - return type_str, array_size - - -def _create_node_with_type(parent, idx, topic_name, topic_text, type_name, array_size): - if '[' in type_name: - type_name = type_name[:type_name.index('[')] - - if type_name == 'bool': - dv = ua.Variant(False, ua.VariantType.Boolean) - elif type_name == 'byte': - dv = ua.Variant(0, ua.VariantType.Byte) - elif type_name == 'int': - dv = ua.Variant(0, ua.VariantType.Int32) - elif type_name == 'int8': - dv = ua.Variant(0, ua.VariantType.SByte) - elif type_name == 'uint8': - dv = ua.Variant(0, ua.VariantType.Byte) - elif type_name == 'int16': - dv = ua.Variant(0, ua.VariantType.Int16) - elif type_name == 'uint16': - dv = ua.Variant(0, ua.VariantType.UInt16) - elif type_name == 'int32': - dv = ua.Variant(0, ua.VariantType.Int32) - elif type_name == 'uint32': - dv = ua.Variant(0, ua.VariantType.UInt32) - elif type_name == 'int64': - dv = ua.Variant(0, ua.VariantType.Int64) - elif type_name == 'uint64': - dv = ua.Variant(0, ua.VariantType.UInt64) - elif type_name == 'float' or type_name == 'float32' or type_name == 'float64': - dv = ua.Variant(0.0, ua.VariantType.Float) - elif type_name == 'double': - dv = ua.Variant(0.0, ua.VariantType.Double) - elif type_name == 'string': - dv = ua.Variant('', ua.VariantType.String) - else: - rospy.logerr("can't create node with type" + str(type_name)) - return None - - if array_size is not None: - value = [] - for i in range(array_size): - value.append(i) - return parent.add_variable(ua.NodeId(topic_name, parent.nodeid.NamespaceIndex), - ua.QualifiedName(topic_text, parent.nodeid.NamespaceIndex), dv.Value) - - -# Used to delete obsolete topics -def numberofsubscribers(nametolookfor, topicsDict): - # rosout only has one subscriber/publisher at all times, so ignore. - if nametolookfor != "/rosout": - ret = topicsDict[nametolookfor]._subscriber.get_num_connections() - else: - ret = 2 - return ret - - -def refresh_topics_and_actions(namespace_ros, server, topicsdict, actionsdict, idx_topics, idx_actions, topics, - actions): - ros_topics = rospy.get_published_topics(namespace_ros) - rospy.logdebug(str(ros_topics)) - rospy.logdebug(str(rospy.get_published_topics('/move_base_simple'))) - for topic_name, topic_type in ros_topics: - if topic_name not in topicsdict or topicsdict[topic_name] is None: - if "cancel" in topic_name or "result" in topic_name or "feedback" in topic_name or "goal" in topic_name or "status" in topic_name: - rospy.logdebug("Found an action: " + str(topic_name)) - correct_name = ros_actions.get_correct_name(topic_name) - if correct_name not in actionsdict: - rospy.loginfo("Creating Action with name: " + correct_name) - try: - actionsdict[correct_name] = ros_actions.OpcUaROSAction(server, actions, idx_actions, - correct_name, - get_goal_type(correct_name), - get_feedback_type( - correct_name)) - except (ValueError, TypeError, AttributeError) as e: - print(e) - rospy.logerr("Error while creating Action Objects for Action " + topic_name) - - else: - # rospy.loginfo("Ignoring normal topics for debugging...") - topic = OpcUaROSTopic(server, topics, idx_topics, topic_name, topic_type) - topicsdict[topic_name] = topic - elif numberofsubscribers(topic_name, topicsdict) <= 1 and "rosout" not in topic_name: - topicsdict[topic_name].recursive_delete_items(server.server.get_node(ua.NodeId(topic_name, idx_topics))) - del topicsdict[topic_name] - ros_server.own_rosnode_cleanup() - - ros_topics = rospy.get_published_topics(namespace_ros) - # use to not get dict changed during iteration errors - tobedeleted = [] - for topic_nameOPC in topicsdict: - found = False - for topicROS, topic_type in ros_topics: - if topic_nameOPC == topicROS: - found = True - if not found: - topicsdict[topic_nameOPC].recursive_delete_items(server.get_node(ua.NodeId(topic_nameOPC, idx_topics))) - tobedeleted.append(topic_nameOPC) - for name in tobedeleted: - del topicsdict[name] - ros_actions.refresh_dict(namespace_ros, actionsdict, topicsdict, server, idx_actions) - - -def get_feedback_type(action_name): - try: - type, name, fn = rostopic.get_topic_type(action_name + "/feedback") - return type - except rospy.ROSException as e: - try: - type, name, fn = rostopic.get_topic_type(action_name + "/Feedback", e) - return type - except rospy.ROSException as e2: - rospy.logerr("Couldnt find feedback type for action " + action_name, e2) - return None - - -def get_goal_type(action_name): - try: - type, name, fn = rostopic.get_topic_type(action_name + "/goal") - return type - except rospy.ROSException as e: - try: - type, name, fn = rostopic.get_topic_type(action_name + "/Goal", e) - return type - except rospy.ROSException as e2: - rospy.logerr("Couldnt find feedback type for action " + action_name, e2) - return None diff --git a/ros_opcua_impl_python_opcua/scripts/ros_ua_client_example.py b/ros_opcua_impl_python_opcua/scripts/ros_ua_client_example.py new file mode 100755 index 0000000..369840b --- /dev/null +++ b/ros_opcua_impl_python_opcua/scripts/ros_ua_client_example.py @@ -0,0 +1,72 @@ +#!/usr/bin/python +""" +This client is only specifically designed for ros_server +""" +import time +import rospy + +from basic_server_client import ROSBasicClient +# from opcua.common.type_dictionary_buider import get_ua_class + + +class SubHandler(object): + + @staticmethod + def datachange_notification(node, val, _): + if not val: + rospy.loginfo('Empty extension object received in node' + node.nodeid) + else: + rospy.loginfo(ROSBasicClient.expand_value(val)) + + +class ROSClient(ROSBasicClient): + + def subscribe_topics(self): + if not self._topics: + self._refresh_topics() + publishing_interval = 500 + sub = self._client.create_subscription(publishing_interval, SubHandler()) + sub.subscribe_data_change(self._topics.values()) + + +if __name__ == '__main__': + with ROSClient() as client: + rospy.loginfo(' ----- rosservice ------ ') + client.list_services() + time.sleep(1) + rospy.loginfo(' ----- rostopic ------ ') + client.list_topics() + time.sleep(1) + rospy.loginfo(' ----- rosnode ------ ') + client.list_ros_nodes() + time.sleep(1) + rospy.loginfo(' ----- rosparam ------ ') + client.list_params() + time.sleep(1) + rospy.loginfo(' ----- rostopic values ------ ') + client.show_topics() + time.sleep(1) + rospy.loginfo(' ----- rosparam values ------ ') + client.show_params() + time.sleep(1) + rospy.loginfo(' ----- rosnodes ------ ') + client.show_ros_nodes() + rospy.loginfo(' ----- static information end------ ') + + # Call Methods + # time.sleep(1) + # # Test case for turtlesim + # obj = get_ua_class('turtlesim/TeleportRelativeRequest')() + # obj.linear = 1 + # obj.angular = 1 + # client.call_service('/turtle1/teleport_relative', obj) + # time.sleep(1) + # # Test case for turtlesim + # obj = get_ua_class('geometry_msgs/Twist')() + # obj.linear.x = 1 + # client.publish_topic('/turtle1/cmd_vel', obj) + + # subscribe to topic publications + time.sleep(1) + client.subscribe_topics() + rospy.spin() diff --git a/ros_opcua_msgs/msg/Address.msg b/ros_opcua_msgs/msg/Address.msg index 2bb715c..1491067 100644 --- a/ros_opcua_msgs/msg/Address.msg +++ b/ros_opcua_msgs/msg/Address.msg @@ -1,2 +1,3 @@ string nodeId -string qualifiedName \ No newline at end of file +string qualifiedName # Only for display information +string[] nodeInfo # Only for display information \ No newline at end of file diff --git a/ros_opcua_msgs/msg/TypeValue.msg b/ros_opcua_msgs/msg/TypeValue.msg index 7c17aaa..e9ac380 100644 --- a/ros_opcua_msgs/msg/TypeValue.msg +++ b/ros_opcua_msgs/msg/TypeValue.msg @@ -1,4 +1,5 @@ # This represents OPC-UA type-value pair +# type should be the basic types or rosmsg/rossrv class name string type bool bool_d int8 int8_d @@ -11,4 +12,4 @@ int64 int64_d uint64 uint64_d float32 float_d float64 double_d -string string_d \ No newline at end of file +string string_d # Can be serialized ROS Message, if type is not in basic types \ No newline at end of file