Add ros2_control support for RSI (#95)

* initial portation of the HW to ROS2_CONTROL * adding setup launch file, not yet working example * some fixes to pluginazation, still not working * change CI to humble * hwinterface to SHARED library * fix parameter names * changed to joint_trajectory controller, will fix for forward controller, too * launch file rework * uncrustify * add custom controller_manager, remove unused node * lint * logging * start controller as active * remove unnecessary logs * lint, move launch file to launch folder * uncrustify * initialize with 0-s * lint cmake * fix humble exclude * fix sonar code smells * clean up unnecessary class members * move configuration to init * uncrustify * disable writing to hwif before successful read * uncrustify * fix merge * add manager node * fix deactivation * update rsi context files * fix ros_rsi.src * update ip log * update rsi start instructions * update readme * uncrustify * default destrc code smell * control node and launch file cleanup * add robot_model param to manager * update plugin description * sleep in read if inactive * lint * rename robot manager nodes * use no-capital resource names * launch file robot name adaptation * remove ptp-s * add cybertech support * move control node to kroshu core * recv timeout for activation in hwif * update package.xml-s * add versions to readme * revert robot manager name changes * remove unused config files * rename sender * add simulation how-to for rsi * return error in case of timeout --------- Co-authored-by: Marton Antal <[email protected]> Co-authored-by: Gergely Kovacs <[email protected]> Co-authored-by: Aron Svastits <[email protected]>
kroshu · Jul 25, 2023 · d94587d · d94587d
1 parent 89893a2
commit d94587d
Show file tree

Hide file tree

Showing 38 changed files with 3,452 additions and 1,182 deletions.
diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md
@@ -64,7 +64,21 @@ The following features of the FRI are exposed to ROS2:
 
 ## KUKA KSS driver (RSI)
 
-Another project in the repo centers on the development of a ROS2 driver for KSS robots through Robot Sensor Interface (RSI). It is in an experimental state, with only joint angle states and commands available. The structure of the driver is similiar to that of the Sunrise driver and the same interfaces are opened, so the same joint controller can be used to move robots. Howewer this controller should be improved, as it allows big torques that stop the robot for machine protection reasons.
+Another project in the repo centers on the development of a ROS2 driver for KSS robots through Robot Sensor Interface (RSI). It is in an experimental state, with only joint angle states and commands available. The guide to set up this driver on a real robot can be found in kuka_rsi_hw_interface\krl for both KCR4 and KRC5 controllers.
+
+### Simulation
+
+To try out the driver with an open-loop simulation the driver and the kuka_rsi_simulator must be started, (at first only a "collapsed" robot will be visible in rviz):
+
+**`ros2 launch kuka_rsi_hw_interface startup_with_rviz.launch.py`**
+
+**`ros2 launch kuka_rsi_simulator kuka_rsi_simulator_launch.py`**
+
+After all components have started successfully, the system needs to be configured and activated to start the simulation, the robot will be visible in rviz after activation:
+
+**`ros2 lifecycle set robot_manager configure`**
+
+**`ros2 lifecycle set robot_manager activate`**
 
 ## iiQKA driver (ECI)
 

diff --git a/kuka_rox_hardware_interface/CMakeLists.txt b/kuka_rox_hardware_interface/CMakeLists.txt
@@ -28,7 +28,6 @@ find_package(sensor_msgs REQUIRED)
 find_package(kroshu_ros2_core REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(controller_manager REQUIRED)
 find_package(controller_manager_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
@@ -76,19 +75,15 @@ ament_target_dependencies(kuka_rox_hw_interface rclcpp sensor_msgs hardware_inte
 target_link_libraries(kuka_rox_hw_interface motion-external-proto-api-nanopb motion-services-ecs-proto-api-cpp
   motion-services-ecs-proto-api-nanopb yaml-cpp os-core-udp-communication nanopb-helpers)
 
-add_executable(rox_control_node
-  src/rox_control_node.cpp)
-ament_target_dependencies(rox_control_node rclcpp kroshu_ros2_core sensor_msgs controller_manager controller_manager_msgs)
-target_link_libraries(rox_control_node kuka_rox_hw_interface)
 
 add_executable(robot_manager_node
   src/robot_manager_node.cpp)
-ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager controller_manager_msgs)
+ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager_msgs)
 target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers motion-services-ecs-proto-api-cpp)
 
 pluginlib_export_plugin_description_file(hardware_interface kuka_rox_hw_interface.xml)
 
-install(TARGETS kuka_rox_hw_interface rox_control_node robot_manager_node
+install(TARGETS kuka_rox_hw_interface robot_manager_node
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY config launch

diff --git a/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp b/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp
@@ -56,12 +56,10 @@ class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_deactivate(const rclcpp_lifecycle::State &) override;
 
-  bool onControlModeChangeRequest(int control_mode);
-  bool onRobotModelChangeRequest(const std::string & robot_model);
-
 private:
   void ObserveControl();
-
+  bool onControlModeChangeRequest(int control_mode);
+  bool onRobotModelChangeRequest(const std::string & robot_model);
 
   rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
     change_hardware_state_client_;

diff --git a/kuka_rox_hardware_interface/launch/startup.launch.py b/kuka_rox_hardware_interface/launch/startup.launch.py
@@ -15,12 +15,10 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch_ros.actions import LifecycleNode
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
-from launch.substitutions import LaunchConfiguration
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+from launch_ros.actions import Node, LifecycleNode
+from launch_ros.substitutions import FindPackageShare
 
 
 def launch_setup(context, *args, **kwargs):
@@ -70,8 +68,8 @@ def launch_setup(context, *args, **kwargs):
     controller_manager_node = '/controller_manager'
 
     control_node = Node(
-        package='kuka_rox_hw_interface',
-        executable='rox_control_node',
+        package='kroshu_ros2_core',
+        executable='control_node',
         parameters=[robot_description, controller_config]
     )
     robot_manager_node = LifecycleNode(

diff --git a/kuka_rox_hardware_interface/package.xml b/kuka_rox_hardware_interface/package.xml
@@ -20,7 +20,6 @@
   <depend>tinyxml_vendor</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
-  <depend>controller_manager</depend>
 
   <depend>yaml-cpp</depend>
   <depend>protobuf-dev</depend>

diff --git a/kuka_rox_hardware_interface/src/rox_control_node.cpp b/kuka_rox_hardware_interface/src/rox_control_node.cpp
diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt
@@ -20,25 +20,38 @@ find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(kroshu_ros2_core REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
 
 find_package(tinyxml_vendor REQUIRED)
 find_package(TinyXML REQUIRED)
 
 include_directories(include ${TinyXML2_INCLUDE_DIRS})
 
-add_library(kuka_hardware_interface
+add_library(${PROJECT_NAME} SHARED
   src/kuka_hardware_interface.cpp
 )
-ament_target_dependencies(kuka_hardware_interface rclcpp sensor_msgs)
-target_link_libraries(kuka_hardware_interface tinyxml)
 
-add_executable(robot_control_node
-  src/robot_control_node.cpp)
-ament_target_dependencies(robot_control_node rclcpp kroshu_ros2_core sensor_msgs)
-target_link_libraries(robot_control_node kuka_hardware_interface)
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL")
+# prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
+ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface)
+target_link_libraries(${PROJECT_NAME} tinyxml)
 
-install(TARGETS kuka_hardware_interface robot_control_node
+
+
+add_executable(robot_manager_node
+  src/robot_manager_node.cpp)
+ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager_msgs)
+target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers)
+
+pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml)
+
+install(TARGETS ${PROJECT_NAME} robot_manager_node
   DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
@@ -60,4 +73,21 @@ if(BUILD_TESTING)
   ament_xmllint(--exclude ros_rsi.rsi.xml)
 endif()
 
+## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+install(DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME})
+
 ament_package()
diff --git a/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml b/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml
@@ -0,0 +1,16 @@
+joint_trajectory_controller:
+  ros__parameters:
+    joints: 
+    - joint_a1
+    - joint_a2
+    - joint_a3
+    - joint_a4
+    - joint_a5
+    - joint_a6
+
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
diff --git a/kuka_rsi_hw_interface/config/ros2_controller_config.yaml b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml
@@ -0,0 +1,11 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 250  # Hz
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    configure_components_on_start: [""]
diff --git a/kuka_rsi_hw_interface/config/rsi_config.yaml b/kuka_rsi_hw_interface/config/rsi_config.yaml
@@ -0,0 +1,2 @@
+client_ip: "0.0.0.0"
+client_port: 59152