diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py b/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py new file mode 100755 index 00000000..5335d2aa --- /dev/null +++ b/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() diff --git a/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py b/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py new file mode 100644 index 00000000..a683f4ce --- /dev/null +++ b/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'), + }] + ) + ]) diff --git a/kuka_rsi_simulator/package.xml b/kuka_rsi_simulator/package.xml new file mode 100644 index 00000000..d3b944b0 --- /dev/null +++ b/kuka_rsi_simulator/package.xml @@ -0,0 +1,19 @@ + + + + kuka_rsi_simulator + 0.0.0 + Simple package for simulating the KUKA RSI interface + Marton Antal + BSD + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + ros2launch + + + ament_python + + diff --git a/kuka_rsi_simulator/resource/kuka_rsi_simulator b/kuka_rsi_simulator/resource/kuka_rsi_simulator new file mode 100644 index 00000000..e69de29b diff --git a/kuka_rsi_simulator/setup.cfg b/kuka_rsi_simulator/setup.cfg new file mode 100644 index 00000000..2922234f --- /dev/null +++ b/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 diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py new file mode 100644 index 00000000..21a11608 --- /dev/null +++ b/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' + ], + }, +) diff --git a/kuka_rsi_simulator/test/test_copyright.py b/kuka_rsi_simulator/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/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' diff --git a/kuka_rsi_simulator/test/test_flake8.py b/kuka_rsi_simulator/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/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) diff --git a/kuka_rsi_simulator/test/test_pep257.py b/kuka_rsi_simulator/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/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'