add rsi_simulator package

kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py

@@ -0,0 +1,139 @@
+# Copyright 2022 Márton Antal
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.from launch import LaunchDescription
+
+import sys
+import socket
+import xml.etree.ElementTree as ET
+import numpy as np
+
+
+import errno
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+
+def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc):
+    q = act_joint_pos
+    qd = setpoint_joint_pos
+    root = ET.Element('Rob', {'TYPE': 'KUKA'})
+    ET.SubElement(root, 'RIst', {'X': '0.0', 'Y': '0.0', 'Z': '0.0',
+                                 'A': '0.0', 'B': '0.0', 'C': '0.0'})
+    ET.SubElement(root, 'RSol', {'X': '0.0', 'Y': '0.0', 'Z': '0.0',
+                                 'A': '0.0', 'B': '0.0', 'C': '0.0'})
+    ET.SubElement(root, 'AIPos', {'A1': str(q[0]), 'A2': str(q[1]), 'A3': str(q[2]),
+                                  'A4': str(q[3]), 'A5': str(q[4]), 'A6': str(q[5])})
+    ET.SubElement(root, 'ASPos', {'A1': str(qd[0]), 'A2': str(qd[1]), 'A3': str(qd[2]),
+                                  'A4': str(qd[3]), 'A5': str(qd[4]), 'A6': str(qd[5])})
+    ET.SubElement(root, 'Delay', {'D': str(timeout_count)})
+    ET.SubElement(root, 'IPOC').text = str(ipoc)
+    return ET.tostring(root)
+
+
+def parse_rsi_xml_sen(data):
+    root = ET.fromstring(data)
+    AK = root.find('AK').attrib
+    desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'],
+                                        AK['A4'], AK['A5'], AK['A6']]).astype(np.float64)
+    IPOC = root.find('IPOC').text
+    stop_flag = root.find('Stop').text
+
+    return desired_joint_correction, int(IPOC), bool(int(stop_flag))
+
+
+class RSISimulator(Node):
+    cycle_time = 0.04
+    act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
+    initial_joint_pos = act_joint_pos.copy()
+    des_joint_correction_absolute = np.zeros(6)
+    timeout_count = 0
+    ipoc = 0
+    rsi_ip_address_ = '127.0.0.1'
+    rsi_port_address_ = 59152
+    rsi_send_name_ = 'IamFree'
+    rsi_act_pub_ = None
+    rsi_cmd_pub_ = None
+    node_name_ = 'rsi_simulator_node'
+    socket_ = None
+
+    def __init__(self, node_name):
+        super().__init__(node_name)
+        self.node_name_ = node_name
+        self.timer = self.create_timer(self.cycle_time, self.timer_callback)
+        self.declare_parameter('rsi_ip_address', '127.0.0.1')
+        self.declare_parameter('rsi_port', 59152)
+        self.declare_parameter('rsi_send_name', "IamFree")
+        self.rsi_ip_address_ = (self.get_parameter('rsi_ip_address').get_parameter_value()
+                                .string_value)
+        self.rsi_port_address_ = self.get_parameter('rsi_port').get_parameter_value().integer_value
+        self.rsi_send_name_ = (self.get_parameter('rsi_send_name').get_parameter_value()
+                               .string_value)
+        self.rsi_act_pub_ = self.create_publisher(String, self.node_name_ + '/rsi/state', 1)
+        self.rsi_cmd_pub_ = self.create_publisher(String, self.node_name_ + '/rsi/command', 1)
+        self.get_logger().info('rsi_ip_address: {}'.format(self.rsi_ip_address_))
+        self.get_logger().info('rsi_port: {}'.format(self.rsi_port_address_))
+
+        self.socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.get_logger().info('{}, Successfully created socket'.format(self.node_name_))
+        self.socket_.settimeout(self.cycle_time)
+
+    def timer_callback(self):
+        if self.timeout_count == 100:
+            self.get_logger().fatal('{} Timeout count of 100 exceeded'.format(self.node_name_))
+            sys.exit()
+        try:
+            msg = create_rsi_xml_rob(self.act_joint_pos, self.initial_joint_pos,
+                                     self.timeout_count, self.ipoc)
+            self.rsi_act_pub_.publish(msg)
+            self.socket_.sendto(msg, (self.rsi_ip_address_, self.rsi_port_address_))
+            recv_msg, addr = self.socket_.recvfrom(1024)
+            self.rsi_cmd_pub_.publish(recv_msg)
+            self.get_logger().warn('msg: {}'.format(recv_msg))
+            des_joint_correction_absolute, ipoc_recv, stop_flag = parse_rsi_xml_sen(recv_msg)
+            if ipoc_recv == self.ipoc:
+                self.act_joint_pos = self.initial_joint_pos + des_joint_correction_absolute
+            else:
+                self.get_logger().warn('{}: Packet is late'.format(self.node_name_))
+                self.get_logger().warn('{}: sent ipoc: {}, received: {}'.
+                                       format(self.node_name_, self.ipoc, ipoc_recv))
+                if self.ipoc != 0:
+                    self.timeout_count += 1
+            self.ipoc += 1
+            if stop_flag:
+                self.on_shutdown()
+                sys.exit()
+        except OSError:
+            if self.ipoc != 0:
+                self.timeout_count += 1
+                self.get_logger().warn('{}: Socket timed out'.format(self.node_name_))
+
+    def on_shutdown(self):
+        self.socket_.close()
+        self.get_logger().info('Socket closed.')
+
+
+def main():
+    node_name = 'rsi_simulator_node'
+
+    rclpy.init()
+    node = RSISimulator(node_name)
+    node.get_logger().info('{}: Started'.format(node_name))
+
+    rclpy.spin(node)
+    node.on_shutdown()
+    node.get_logger().info('{}: Shutting down'.format(node_name))
+
+
+if __name__ == '__main__':
+    main()

kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py

@@ -0,0 +1,43 @@
+# Copyright 2022 Márton Antal
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.from launch import LaunchDescription
+
+
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch import LaunchDescription
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    rsi_ip_address = DeclareLaunchArgument(
+        'rsi_ip_address', default_value=TextSubstitution(text='127.0.0.1')
+    )
+    rsi_port = DeclareLaunchArgument(
+        'rsi_port', default_value=TextSubstitution(text='59152')
+    )
+
+    return LaunchDescription([
+        rsi_ip_address,
+        rsi_port,
+        Node(
+            package='kuka_rsi_simulator',
+            executable='rsi_simulator',
+            name='kuka_rsi_simulator',
+            parameters=[{
+                'rsi_ip_address': LaunchConfiguration('rsi_ip_address'),
+                'rsi_port': LaunchConfiguration('rsi_port'),
+            }]
+        )
+    ])

kuka_rsi_simulator/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>kuka_rsi_simulator</name>
+  <version>0.0.0</version>
+  <description>Simple package for simulating the KUKA RSI interface</description>
+  <maintainer email="antal.marci@gmail.com">Marton Antal</maintainer>
+  <license>BSD</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

kuka_rsi_simulator/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/kuka_rsi_simulator
+[install]
+install_scripts=$base/lib/kuka_rsi_simulator

kuka_rsi_simulator/setup.py

@@ -0,0 +1,33 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'kuka_rsi_simulator'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'),
+            glob(os.path.join('launch', '*.launch.py'))),
+        (os.path.join('share', package_name, 'launch'),
+            glob(os.path.join('launch', '*_launch.py'))),
+        (os.path.join('share', package_name, 'config'),
+            glob(os.path.join('config', '*.yaml'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Marton Antal',
+    maintainer_email='antal.marci@gmail.com',
+    description='Simple package for simulating the KUKA RSI interface.',
+    license='BSD',
+    entry_points={
+        'console_scripts': [
+            'rsi_simulator = kuka_rsi_simulator.rsi_simulator:main'
+        ],
+    },
+)

kuka_rsi_simulator/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

kuka_rsi_simulator/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

kuka_rsi_simulator/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'