diff --git a/kuka_experimental/CMakeLists.txt b/kuka_experimental/CMakeLists.txt index 8dbd255ce..2c84d20f4 100644 --- a/kuka_experimental/CMakeLists.txt +++ b/kuka_experimental/CMakeLists.txt @@ -1,4 +1,7 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.16) + project(kuka_experimental) -find_package(catkin REQUIRED) -catkin_metapackage() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/kuka_experimental/package.xml b/kuka_experimental/package.xml index c9c20c0ce..811f572ff 100644 --- a/kuka_experimental/package.xml +++ b/kuka_experimental/package.xml @@ -1,5 +1,5 @@ - + kuka_experimental 0.1.0 Experimental packages for KUKA manipulators within ROS-Industrial. @@ -11,22 +11,14 @@ https://github.com/ros-industrial/kuka_experimental/issues https://github.com/ros-industrial/kuka_experimental - catkin + ament_cmake - kuka_kr10_support - kuka_kr120_support - kuka_kr150_support - kuka_kr16_support - kuka_kr210_support - kuka_kr5_support 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_kr16_support/CMakeLists.txt b/kuka_kr16_support/CMakeLists.txt index 2e76269e4..770599dc6 100644 --- a/kuka_kr16_support/CMakeLists.txt +++ b/kuka_kr16_support/CMakeLists.txt @@ -1,17 +1,16 @@ -cmake_minimum_required(VERSION 3.16) +cmake_minimum_required(VERSION 3.5) -project(kuka_kr16_support LANGUAGES CXX) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) -endif() +project(kuka_kr16_support) find_package(ament_cmake REQUIRED) +if (CATKIN_ENABLE_TESTING) + find_package(roslaunch REQUIRED) + roslaunch_add_file_check(test/roslaunch_test.xml) +endif() -install( - DIRECTORY config launch meshes urdf +install(DIRECTORY config launch meshes urdf DESTINATION share/${PROJECT_NAME} - ) +) - ament_package() +ament_package() \ No newline at end of file diff --git a/kuka_kr16_support/config/rviz/view_robot_kr16_2.rviz b/kuka_kr16_support/config/rviz/view_robot_kr16_2.rviz new file mode 100644 index 000000000..77b895b52 --- /dev/null +++ b/kuka_kr16_support/config/rviz/view_robot_kr16_2.rviz @@ -0,0 +1,249 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 745 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + base_link: + Value: true + flange: + Value: true + link_1: + Value: true + link_2: + Value: true + link_3: + Value: true + link_4: + Value: true + link_5: + Value: true + link_6: + Value: true + tool0: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + base: + {} + link_1: + link_2: + link_3: + link_4: + link_5: + link_6: + flange: + tool0: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.382266521453857 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.4607224464416504 + Y: 0.24497589468955994 + Z: 0.1281789243221283 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.155398428440094 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.303589820861816 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1036 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000372fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000372000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000372000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077c0000003efc0100000002fb0000000800540069006d006501000000000000077c0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050b0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1916 + X: 0 + Y: 20 diff --git a/kuka_kr16_support/launch/view_kr16_2.launch.py b/kuka_kr16_support/launch/view_kr16_2.launch.py new file mode 100644 index 000000000..7ef0cb4eb --- /dev/null +++ b/kuka_kr16_support/launch/view_kr16_2.launch.py @@ -0,0 +1,71 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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 RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("kuka_kr16_support"), "urdf", + "kr16_2.xacro"] + ), + ] + ) + + robot_description = {"robot_description": robot_description_content} + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("kuka_kr16_support"), "config/rviz", + "view_robot_kr16_2.rviz"] + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file] + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node + ] + + return LaunchDescription(nodes) diff --git a/kuka_kr16_support/package.xml b/kuka_kr16_support/package.xml index 7cae6bd27..fcf9a147c 100644 --- a/kuka_kr16_support/package.xml +++ b/kuka_kr16_support/package.xml @@ -1,5 +1,4 @@ - kuka_kr16_support 0.1.0 @@ -40,14 +39,7 @@ ament_cmake - roslaunch - - industrial_robot_client - joint_state_publisher kuka_resources - robot_state_publisher - rviz - xacro ament_cmake diff --git a/kuka_kr16_support/urdf/kr16_2.xacro b/kuka_kr16_support/urdf/kr16_2.xacro index 8a7d6f66a..893a73044 100644 --- a/kuka_kr16_support/urdf/kr16_2.xacro +++ b/kuka_kr16_support/urdf/kr16_2.xacro @@ -1,13 +1,6 @@ - - - - - - - diff --git a/kuka_kr6_support/CMakeLists.txt b/kuka_kr6_support/CMakeLists.txt index 8fdd61091..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/config/rviz/view_robot.rviz b/kuka_kr6_support/config/rviz/view_robot.rviz new file mode 100644 index 000000000..6a83d7c07 --- /dev/null +++ b/kuka_kr6_support/config/rviz/view_robot.rviz @@ -0,0 +1,244 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 749 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + base_link: + Value: true + link_1: + Value: true + link_2: + Value: true + link_3: + Value: true + link_4: + Value: true + link_5: + Value: true + link_6: + Value: true + tool0: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + base: + {} + link_1: + link_2: + link_3: + link_4: + link_5: + link_6: + tool0: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.1647839546203613 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.33039820194244385 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.143582820892334 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1040 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000376fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000376000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000376fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000376000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077c0000003efc0100000002fb0000000800540069006d006501000000000000077c0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050b0000037600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1916 + X: 0 + Y: 18 diff --git a/kuka_kr6_support/launch/view_kr6.launch.py b/kuka_kr6_support/launch/view_kr6.launch.py new file mode 100644 index 000000000..ec685cf84 --- /dev/null +++ b/kuka_kr6_support/launch/view_kr6.launch.py @@ -0,0 +1,71 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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 RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("kuka_kr6_support"), "urdf", + "kr6r900sixx.xacro"] + ), + ] + ) + + robot_description = {"robot_description": robot_description_content} + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("kuka_kr6_support"), "config/rviz", + "view_robot.rviz"] + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file] + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node + ] + + return LaunchDescription(nodes) 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/CMakeLists.txt b/kuka_resources/CMakeLists.txt index 25a9644d4..ecaaa9604 100644 --- a/kuka_resources/CMakeLists.txt +++ b/kuka_resources/CMakeLists.txt @@ -12,6 +12,6 @@ find_package(ament_cmake REQUIRED) install( DIRECTORY urdf config launch DESTINATION share/${PROJECT_NAME} - ) +) - ament_package() +ament_package() 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..9b77ba7d0 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,15 @@ 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/kuka_rsi_simulator/__init__.py b/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py b/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py new file mode 100755 index 000000000..c7f3b5704 --- /dev/null +++ b/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 + +import sys +import socket +import numpy as np +import time +import xml.etree.ElementTree as ET + +import errno +# import rospy +# 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 + return desired_joint_correction, int(IPOC) + + +# if __name__ == '__main__': +def main(): + 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) + cmd_joint_pos = act_joint_pos.copy() + des_joint_correction_absolute = np.zeros(6) + timeout_count = 0 + ipoc = 0 + + # import argparse + # parser = argparse.ArgumentParser(description='KUKA RSI Simulation') + # 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() + # host = args.rsi_hw_iface_ip + # port = int(args.rsi_hw_iface_port) + # sen_type = args.sen + + host = "127.0.0.1" + port = 49152 + sen_type = 'ImFree' + + + # rospy.init_node(node_name) + # rospy.loginfo('{}: Started'.format(node_name)) + + try: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + # rospy.loginfo('{}, Successfully created socket'.format(node_name)) + print('{}, Successfully created socket'.format(node_name)) + s.settimeout(1) + except socket.error as e: + # rospy.logfatal('{}Could not create socket'.format(node_name)) + print('{}Could not create socket'.format(node_name)) + sys.exit() + + def shutdown_hook(): + # rospy.loginfo('{}: Shutting down'.format(node_name)) + print('{}: Shutting down'.format(node_name)) + s.close() + + # rospy.on_shutdown(shutdown_hook) + + # while not rospy.is_shutdown(): + while True: + 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(str(msg)) + s.sendto(msg, (host, port)) + recv_msg, addr = s.recvfrom(1024) + # 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 + time.sleep(cycle_time / 2) + except socket.timeout: + # rospy.logwarn('{}: Socket timed out'.format(node_name)) + print('{}: Socket timed out'.format(node_name)) + timeout_count += 1 + except socket.error as e: + if e.errno != errno.EINTR: + raise + + +if __name__ == '__main__': + main() 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', + ], + }, +)