[RSI] Port Python Simulator (backport #5) (#26)

Co-authored-by: Dmitri <dmitri@dmitri.com>
(cherry picked from commit bdb28eb)

Co-authored-by: Dr. Denis <denis@stoglrobotics.de>

kuka_experimental/package.xml

@@ -23,11 +23,12 @@
   <exec_depend>kuka_kr6_support</exec_depend>
   <exec_depend>kuka_lbr_iiwa_support</exec_depend>
   <exec_depend>kuka_resources</exec_depend>
+  <!--<exec_depend>kuka_rsi_hw_interface</exec_depend>-->
   <exec_depend>kuka_eki_hw_interface</exec_depend>
   <exec_depend>kuka_rsi_hw_interface</exec_depend>
   <exec_depend>kuka_rsi_simulator</exec_depend>
 
   <export>
-    <metapackage/>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>

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()

kuka_kr6_support/package.xml

@@ -48,8 +48,6 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>roslaunch</test_depend>
-
   <exec_depend>industrial_robot_client</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>kuka_kr10_support</exec_depend>

kuka_resources/package.xml

@@ -22,8 +22,6 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>roslaunch</test_depend>
-
   <exec_depend>industrial_robot_client</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>

kuka_rsi_hw_interface/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>kuka_rsi_hw_interface</name>
   <version>0.1.0</version>
   <description>A ROS-Control hardware interface for use with KUKA RSI</description>
@@ -12,14 +12,14 @@
   <url type="bugtracker">https://github.com/ros-industrial/kuka_experimental/issues</url>
   <url type="repository">https://github.com/ros-industrial/kuka_experimental</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>cmake_modules</build_depend>
-
-  <depend>controller_manager</depend>
-  <depend>hardware_interface</depend>
-  <depend>joint_limits_interface</depend>
-  <depend>realtime_tools</depend>
-  <depend>roscpp</depend>
+  <!--<depend>controller_manager</depend>-->
+  <!--<depend>hardware_interface</depend>-->
+  <!--<depend>realtime_tools</depend>-->
+  <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

kuka_rsi_simulator/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>kuka_rsi_simulator</name>
   <version>0.1.0</version>
   <description>Python node that implements a minimal RSI interface simulator.</description>
@@ -12,8 +12,10 @@
   <url type="bugtracker">https://github.com/ros-industrial/kuka_experimental/issues</url>
   <url type="repository">https://github.com/ros-industrial/kuka_experimental</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <exec_depend>rospy</exec_depend>
+  <exec_depend>rclpy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
 </package>

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,47 +47,52 @@ 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. <Sen Type:"ImFree">')
     # Only parse known arguments
     args, _ = parser.parse_known_args()
     host = args.rsi_hw_iface_ip
     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()

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,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',
+        ],
+    },
+)