diff --git a/kuka_experimental/package.xml b/kuka_experimental/package.xml
index c9509b205..de653bf63 100644
--- a/kuka_experimental/package.xml
+++ b/kuka_experimental/package.xml
@@ -23,11 +23,12 @@
kuka_kr6_support
kuka_lbr_iiwa_support
kuka_resources
+
kuka_eki_hw_interface
kuka_rsi_hw_interface
kuka_rsi_simulator
-
+ ament_cmake
diff --git a/kuka_kr6_support/CMakeLists.txt b/kuka_kr6_support/CMakeLists.txt
index 562c0cdd3..8e412f826 100644
--- a/kuka_kr6_support/CMakeLists.txt
+++ b/kuka_kr6_support/CMakeLists.txt
@@ -8,10 +8,9 @@ endif()
find_package(ament_cmake REQUIRED)
-
install(
DIRECTORY config launch meshes urdf
DESTINATION share/${PROJECT_NAME}
)
- ament_package()
+ament_package()
diff --git a/kuka_kr6_support/package.xml b/kuka_kr6_support/package.xml
index 81baead4a..1d8b1b0c4 100644
--- a/kuka_kr6_support/package.xml
+++ b/kuka_kr6_support/package.xml
@@ -48,8 +48,6 @@
ament_cmake
- roslaunch
-
industrial_robot_client
joint_state_publisher
kuka_kr10_support
diff --git a/kuka_resources/package.xml b/kuka_resources/package.xml
index cade58a7a..83d9db37a 100644
--- a/kuka_resources/package.xml
+++ b/kuka_resources/package.xml
@@ -22,8 +22,6 @@
ament_cmake
- roslaunch
-
industrial_robot_client
joint_state_publisher
joint_state_broadcaster
diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml
index 1e6547c56..445299a9a 100644
--- a/kuka_rsi_hw_interface/package.xml
+++ b/kuka_rsi_hw_interface/package.xml
@@ -1,5 +1,5 @@
-
+
kuka_rsi_hw_interface
0.1.0
A ROS-Control hardware interface for use with KUKA RSI
@@ -12,14 +12,14 @@
https://github.com/ros-industrial/kuka_experimental/issues
https://github.com/ros-industrial/kuka_experimental
- catkin
+ ament_cmake
- cmake_modules
-
- controller_manager
- hardware_interface
- joint_limits_interface
- realtime_tools
- roscpp
+
+
+
+ rclcpp
std_msgs
+
+ ament_cmake
+
diff --git a/kuka_rsi_simulator/COLCON_IGNORE b/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py
similarity index 100%
rename from kuka_rsi_simulator/COLCON_IGNORE
rename to kuka_rsi_simulator/kuka_rsi_simulator/__init__.py
diff --git a/kuka_rsi_simulator/package.xml b/kuka_rsi_simulator/package.xml
index e32385ba2..de196cd7f 100644
--- a/kuka_rsi_simulator/package.xml
+++ b/kuka_rsi_simulator/package.xml
@@ -1,5 +1,5 @@
-
+
kuka_rsi_simulator
0.1.0
Python node that implements a minimal RSI interface simulator.
@@ -12,8 +12,10 @@
https://github.com/ros-industrial/kuka_experimental/issues
https://github.com/ros-industrial/kuka_experimental
- catkin
-
- rospy
+ rclpy
std_msgs
+
+
+ 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 000000000..e69de29bb
diff --git a/kuka_rsi_simulator/scripts/kuka_rsi_simulator b/kuka_rsi_simulator/scripts/kuka_rsi_simulator
index 15131e167..db1b47f34 100755
--- a/kuka_rsi_simulator/scripts/kuka_rsi_simulator
+++ b/kuka_rsi_simulator/scripts/kuka_rsi_simulator
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
import sys
import socket
@@ -7,7 +7,7 @@ import time
import xml.etree.ElementTree as ET
import errno
-import rospy
+import rclpy
from std_msgs.msg import String
def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc):
@@ -34,9 +34,8 @@ def parse_rsi_xml_sen(data):
IPOC = root.find('IPOC').text
return desired_joint_correction, int(IPOC)
+
node_name = 'kuka_rsi_simulation'
-rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
-rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
cycle_time = 0.004
act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
@@ -48,8 +47,8 @@ ipoc = 0
if __name__ == '__main__':
import argparse
parser = argparse.ArgumentParser(description='KUKA RSI Simulation')
- parser.add_argument('rsi_hw_iface_ip', help='The ip address of the RSI control interface')
- parser.add_argument('rsi_hw_iface_port', help='The port of the RSI control interface')
+ parser.add_argument('--rsi_hw_iface_ip', default="127.0.0.1", help='The ip address of the RSI control interface (default=127.0.0.1)')
+ parser.add_argument('--rsi_hw_iface_port', default=49152, help='The port of the RSI control interface (default=49152)')
parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. ')
# Only parse known arguments
args, _ = parser.parse_known_args()
@@ -57,38 +56,43 @@ if __name__ == '__main__':
port = int(args.rsi_hw_iface_port)
sen_type = args.sen
- rospy.init_node(node_name)
- rospy.loginfo('{}: Started'.format(node_name))
+ rclpy.init(args=args)
+ node = rclpy.create_node(node_name)
+
+ node.get_logger().info(f"Started '{node_name}'")
+
+ rsi_act_pub = node.create_publisher(String, '~/rsi/state')
+ rsi_cmd_pub = node.create_publisher(String, '~/rsi/command')
try:
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
- rospy.loginfo('{}, Successfully created socket'.format(node_name))
+ node.get_logger().info(f"[{node_name}] Successfully created socket.")
s.settimeout(1)
except socket.error as e:
- rospy.logfatal('{}Could not create socket'.format(node_name))
+ node.get_logger().fatal(f"[{node_name}] Could not create socket.")
sys.exit()
- def shutdown_hook():
- rospy.loginfo('{}: Shutting down'.format(node_name))
- s.close()
-
- rospy.on_shutdown(shutdown_hook)
-
- while not rospy.is_shutdown():
+ while rclpy.ok():
+ time.sleep(0.001) # this is a hack, make this a ros2 node
try:
msg = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc)
- rsi_act_pub.publish(msg)
+ rsi_act_pub.publish(str(msg))
s.sendto(msg, (host, port))
recv_msg, addr = s.recvfrom(1024)
- rsi_cmd_pub.publish(recv_msg)
+ rsi_cmd_pub.publish(str(recv_msg))
des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg)
act_joint_pos = cmd_joint_pos + des_joint_correction_absolute
ipoc += 1
+ rclpy.spin_once(node)
time.sleep(cycle_time / 2)
- except socket.timeout, msg:
- rospy.logwarn('{}: Socket timed out'.format(node_name))
+ except socket.timeout:
+ node.get_logger().warn(f"[{node_name}] Socket timed out.")
timeout_count += 1
- except socket.error, e:
+ except socket.error as e:
if e.errno != errno.EINTR:
raise
+ node.get_logger().info(f"Shutting down '{node_name}'")
+ node.destroy_node()
+ rclpy.shutdown()
+ s.close()
diff --git a/kuka_rsi_simulator/setup.cfg b/kuka_rsi_simulator/setup.cfg
new file mode 100644
index 000000000..5cb6c94a3
--- /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 000000000..cf4d2de54
--- /dev/null
+++ b/kuka_rsi_simulator/setup.py
@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'kuka_rsi_simulator'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='Denis Štogl',
+ maintainer_email='denis@stoglrobotics.de',
+ description='Python node that implements a minimal RSI interface simulator.',
+ license='BSD',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'kuka_rsi_simulator = kuka_rsi_simulator.kuka_rsi_simulator:main',
+ ],
+ },
+)