diff --git a/README.md b/README.md index 6a6384e..a971d9c 100644 --- a/README.md +++ b/README.md @@ -29,12 +29,6 @@ Github CI ## What data is verifyed? -## Running the RSI simulator - -``` -ros2 launch kuka_rsi_simulator kuka_rsi_simulator_launch.py -``` - ## Starting the move group server with fake hardware To start the driver with fake hardware and the motion planning rviz plugin, the following launch files can be used: diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py b/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py b/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py deleted file mode 100755 index 5335d2a..0000000 --- a/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py +++ /dev/null @@ -1,139 +0,0 @@ -# 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/__pycache__/kuka_rsi_simulator_launch.cpython-38.pyc b/kuka_rsi_simulator/launch/__pycache__/kuka_rsi_simulator_launch.cpython-38.pyc deleted file mode 100644 index 82e22a4..0000000 Binary files a/kuka_rsi_simulator/launch/__pycache__/kuka_rsi_simulator_launch.cpython-38.pyc and /dev/null differ diff --git a/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py b/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py deleted file mode 100644 index a683f4c..0000000 --- a/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py +++ /dev/null @@ -1,43 +0,0 @@ -# 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 deleted file mode 100644 index d3b944b..0000000 --- a/kuka_rsi_simulator/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - 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 deleted file mode 100644 index e69de29..0000000 diff --git a/kuka_rsi_simulator/setup.cfg b/kuka_rsi_simulator/setup.cfg deleted file mode 100644 index 2922234..0000000 --- a/kuka_rsi_simulator/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[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 deleted file mode 100644 index 21a1160..0000000 --- a/kuka_rsi_simulator/setup.py +++ /dev/null @@ -1,33 +0,0 @@ -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 deleted file mode 100644 index cc8ff03..0000000 --- a/kuka_rsi_simulator/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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 deleted file mode 100644 index 27ee107..0000000 --- a/kuka_rsi_simulator/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# 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 deleted file mode 100644 index b234a38..0000000 --- a/kuka_rsi_simulator/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# 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'