From 4fbd6d18a7b686a21cb9a3ec733d12e7a2b918ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=81ron=20Svastits?= <49677296+Svastits@users.noreply.github.com> Date: Tue, 30 Apr 2024 08:44:09 +0200 Subject: [PATCH 1/2] Fix topic name for control_mode_handler (#164) * fix topic name * add dimensions to xyz rpy doc --------- Co-authored-by: Svastits --- doc/wiki/1_iiQKA_EAC.md | 4 ++-- doc/wiki/2_KSS_RSI.md | 4 ++-- doc/wiki/3_Sunrise_FRI.md | 4 ++-- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/doc/wiki/1_iiQKA_EAC.md b/doc/wiki/1_iiQKA_EAC.md index 674a743c..139fc5b7 100644 --- a/doc/wiki/1_iiQKA_EAC.md +++ b/doc/wiki/1_iiQKA_EAC.md @@ -75,8 +75,8 @@ Both launch files support the following arguments: - `robot_model`: defines which LBR iisy robot to use. Available options: `lbr_iisy3_r760` (default), `lbr_iisy11_r1300`, `lbr_iisy15_r930` - `use_fake_hardware`: if true, the `KukaMockHardwareInterface` will be used instead of the `KukaEACHardwareInterface`. This enables trying out the driver without actual hardware. - `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_` -- `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame (default: [0, 0, 0]) -- `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame (default: [0, 0, 0]) +- `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame in meters (default: [0, 0, 0]) +- `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame in radians (default: [0, 0, 0]) - `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 2500 us, only used if `use_fake_hardware` is true) - `qos_config`: the location of the QoS configuration file (defaults to `kuka_iiqka_eac_driver/config/qos_config.yaml`) - `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_iiqka_eac_driver/config/ros2_controller_config.yaml`) diff --git a/doc/wiki/2_KSS_RSI.md b/doc/wiki/2_KSS_RSI.md index 02c5b58b..ea57b6a7 100644 --- a/doc/wiki/2_KSS_RSI.md +++ b/doc/wiki/2_KSS_RSI.md @@ -114,8 +114,8 @@ Both launch files support the following arguments: - `robot_model` and `robot_family`: defines which robot to use. The available options for the valid model and family combinations can be found in the [readme](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#what-data-is-verified) of the `kuka_robot_descriptions` repository. - `use_fake_hardware`: if true, the `KukaMockHardwareInterface` will be used instead of the `KukaRSIHardwareInterface`. This enables trying out the driver without actual hardware. - `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_` -- `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame (default: [0, 0, 0]) -- `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame (default: [0, 0, 0]) +- `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame in meters (default: [0, 0, 0]) +- `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame in radians (default: [0, 0, 0]) - `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 4000 us, only used if `use_fake_hardware` is true) - `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_kss_rsi_driver/config/ros2_controller_config.yaml`) - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml`) diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md index d2b5c524..790d2e62 100644 --- a/doc/wiki/3_Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -63,8 +63,8 @@ Both launch files support the following argument: - `robot_model`: defines which LBR iiwa robot to use. Available options: `lbr_iiwa14_r820` (default) - `use_fake_hardware`: if true, the `KukaMockHardwareInterface` will be used instead of the `KukaFRIHardwareInterface`. This enables trying out the driver without actual hardware. - `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_` -- `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame (default: [0, 0, 0]) -- `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame (default: [0, 0, 0]) +- `x`, `y`, `z`: define the position of `base_link` relative to the `world` frame in meters (default: [0, 0, 0]) +- `roll`, `pitch`, `yaw`: define the orientation of `base_link` relative to the `world` frame in radians (default: [0, 0, 0]) - `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 5000 us, only used if `use_fake_hardware` is true) - `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_sunrise_fri_driver/config/ros2_controller_config.yaml`) - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml`) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 50c09517..936f6be9 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -46,7 +46,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ this->create_publisher("robot_manager/is_configured", is_configured_qos); control_mode_pub_ = this->create_publisher( - "kuka_control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); + "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = event_cbg_; From e33b7c00635f96d9f50580d4f8d2cfd689c830e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=81ron=20Svastits?= <49677296+Svastits@users.noreply.github.com> Date: Tue, 30 Apr 2024 13:40:22 +0200 Subject: [PATCH 2/2] Restructure Sunrise driver (#141) * controller_ip to launch * use controller_ip * define setfriconfig srv * move hw-related stuff to hwif step1 * format * cleanup * configuration manager cleanup * hw if cleanup * extend launch and config * remove config manager * clean up access rights * remove configuring from rights * if and controller cleanup * change control and command modes in hwif * add new args to robot app * fix relative links in wiki * fix file * extend serialization * client ip and port params * remove params * launch args * update mds * control mode handler * extend md * update params * damping and stiffness as params * update md * update comment * fix controller config * fix multi-install * friction compensation * format * fix merge * fix doc merge * merge controllers branch * add event_broadcaster * add activation test * fix linking * fix ip transmission * fix ip on server side * app folder * format * is_active and controller fixes * fix kss doc * add sleep necessary for jtc * fix impedance settings * extend log * fri config fix * rework fri configuration * fix comparison * remove unnecessary check + add comments * add comment * format + update md * string_view * str_view 2 * always allow unconfigured param changes * fix build * udpate package xml * sonar fixes --------- Co-authored-by: Aron Svastits Co-authored-by: Gergely Kovacs --- .../fri_configuration_controller.hpp | 6 +- .../src/fri_configuration_controller.cpp | 29 +- doc/wiki/2_KSS_RSI.md | 1 - doc/wiki/3_Sunrise_FRI.md | 32 +- doc/wiki/4_Controllers.md | 2 +- doc/wiki/Home.md | 4 +- kuka_driver_interfaces/CMakeLists.txt | 4 +- .../msg/FriConfiguration.msg | 3 + kuka_driver_interfaces/srv/GetInt.srv | 3 - kuka_driver_interfaces/srv/SetDouble.srv | 5 - kuka_driver_interfaces/srv/SetInt.srv | 6 - kuka_drivers_core/README.md | 8 +- .../kuka_drivers_core/controller_names.hpp | 1 + .../hardware_interface_types.hpp | 1 + .../kuka_drivers_core/parameter_handler.hpp | 9 +- .../src/robot_manager_node.cpp | 14 +- .../src/robot_manager_node.cpp | 3 +- kuka_sunrise_fri_driver/CMakeLists.txt | 23 +- .../config/driver_config.yaml | 5 +- .../config/effort_controller_config.yaml | 10 + .../joint_impedance_controller_config.yaml | 10 + .../config/ros2_controller_config.yaml | 15 +- .../configuration_manager.hpp | 80 -- .../fri_connection.hpp | 6 +- .../hardware_interface.hpp | 51 +- .../robot_manager_node.hpp | 47 +- .../kuka_sunrise_fri_driver/serialization.hpp | 7 +- .../launch/startup.launch.py | 35 + kuka_sunrise_fri_driver/package.xml | 3 + .../src/application/ROS2_Control.java | 134 +-- .../ioAccess/MediaFlangeIOGroup.java | 200 +++-- .../src/ros2/modules/FRIManager.java | 533 ++++++------ .../src/ros2/modules/ROS2Connection.java | 792 +++++++++--------- .../src/ros2/modules/TCPConnection.java | 373 ++++----- .../ros2/serialization/ControlModeParams.java | 194 ++--- .../serialization/FRIConfigurationParams.java | 120 +-- ...intImpedanceControlModeExternalizable.java | 118 +-- .../ros2/serialization/MessageEncoding.java | 78 +- .../src/ros2/tools/Conversions.java | 26 +- .../configuration_manager.cpp | 384 --------- .../src/connection_helpers/fri_connection.cpp | 36 +- .../src/hardware_interface.cpp | 340 ++++++-- .../src/robot_manager_node.cpp | 458 ++++++---- .../test/test_driver_activation.py | 88 ++ 44 files changed, 2155 insertions(+), 2142 deletions(-) create mode 100644 kuka_driver_interfaces/msg/FriConfiguration.msg delete mode 100644 kuka_driver_interfaces/srv/GetInt.srv delete mode 100644 kuka_driver_interfaces/srv/SetDouble.srv delete mode 100644 kuka_driver_interfaces/srv/SetInt.srv create mode 100644 kuka_sunrise_fri_driver/config/effort_controller_config.yaml create mode 100644 kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml delete mode 100644 kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/application/ROS2_Control.java (96%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java (52%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/modules/FRIManager.java (96%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/modules/ROS2Connection.java (96%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/modules/TCPConnection.java (95%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/serialization/ControlModeParams.java (96%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/serialization/FRIConfigurationParams.java (72%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/serialization/JointImpedanceControlModeExternalizable.java (96%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/serialization/MessageEncoding.java (97%) mode change 100644 => 100755 rename kuka_sunrise_fri_driver/robot_application/{ROS2_Control => }/src/ros2/tools/Conversions.java (96%) mode change 100644 => 100755 delete mode 100644 kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp create mode 100644 kuka_sunrise_fri_driver/test/test_driver_activation.py diff --git a/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp b/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp index c33058fe..598e855c 100644 --- a/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp +++ b/controllers/fri_configuration_controller/include/fri_configuration_controller/fri_configuration_controller.hpp @@ -20,7 +20,7 @@ #include #include "controller_interface/controller_interface.hpp" -#include "kuka_driver_interfaces/srv/set_int.hpp" +#include "kuka_driver_interfaces/msg/fri_configuration.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" @@ -53,9 +53,9 @@ class FRIConfigurationController : public controller_interface::ControllerInterf FRI_CONFIGURATION_CONTROLLER_PUBLIC controller_interface::CallbackReturn on_init() override; private: - rclcpp::Service::SharedPtr receive_multiplier_service_; + rclcpp::Subscription::SharedPtr fri_config_sub_; int receive_multiplier_ = 1; - bool resend_multiplier_ = false; + int send_period_ms_ = 10; }; } // namespace kuka_controllers #endif // FRI_CONFIGURATION_CONTROLLER__FRI_CONFIGURATION_CONTROLLER_HPP_ diff --git a/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp b/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp index 91144568..35bf218d 100644 --- a/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp +++ b/controllers/fri_configuration_controller/src/fri_configuration_controller.cpp @@ -20,19 +20,13 @@ namespace kuka_controllers { controller_interface::CallbackReturn FRIConfigurationController::on_init() { - auto callback = [this]( - kuka_driver_interfaces::srv::SetInt::Request::SharedPtr request, - kuka_driver_interfaces::srv::SetInt::Response::SharedPtr response) + auto callback = [this](const kuka_driver_interfaces::msg::FriConfiguration::SharedPtr msg) { - resend_multiplier_ = true; - receive_multiplier_ = request->data; - response->success = true; + receive_multiplier_ = msg->receive_multiplier; + send_period_ms_ = msg->send_period_ms; }; - receive_multiplier_service_ = get_node()->create_service( - "~/set_receive_multiplier", callback); - // TODO(Svastits): create service to get multiplier changes (or perpaps - // parameter??) - // and set resend_multiplier_ to true in the callback + fri_config_sub_ = get_node()->create_subscription( + "~/set_fri_config", rclcpp::SystemDefaultsQoS(), callback); return controller_interface::CallbackReturn::SUCCESS; } @@ -43,6 +37,8 @@ FRIConfigurationController::command_interface_configuration() const config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.emplace_back( std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::RECEIVE_MULTIPLIER); + config.names.emplace_back( + std::string(hardware_interface::CONFIG_PREFIX) + "/" + hardware_interface::SEND_PERIOD); return config; } @@ -75,14 +71,9 @@ controller_interface::return_type FRIConfigurationController::update( const rclcpp::Time &, const rclcpp::Duration &) { // TODO(Svastits): disable changes if HWIF is active - if (resend_multiplier_) - { - RCLCPP_INFO( - get_node()->get_logger(), "Changing receive multiplier of hardware interface to %i", - receive_multiplier_); - command_interfaces_[0].set_value(receive_multiplier_); - resend_multiplier_ = false; - } + command_interfaces_[0].set_value(receive_multiplier_); + command_interfaces_[1].set_value(send_period_ms_); + return controller_interface::return_type::OK; } diff --git a/doc/wiki/2_KSS_RSI.md b/doc/wiki/2_KSS_RSI.md index ea57b6a7..949241f0 100644 --- a/doc/wiki/2_KSS_RSI.md +++ b/doc/wiki/2_KSS_RSI.md @@ -64,7 +64,6 @@ Method 2: #### Startup configuration The following configuration files are available in the `config` directory of the package: -- `driver_config.yaml`: contains the IP address of the client machine - `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. - configuration files for specific controllers, for further information, see the documentation of the given controller diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md index 790d2e62..19f8314f 100644 --- a/doc/wiki/3_Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -8,7 +8,6 @@ #### Controller side -- Modify the `_remoteIP` variable of the [FRIConfigurationParams.java](https://github.com/kroshu/kuka_drivers/blob/master/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java) file to the IP address of the client machine - Upload the robot application under `robot_application/src` to the controller using Sunrise Workbench ### Configuration @@ -28,8 +27,8 @@ The IP address of robot controller must be provided as a launch argument. For fu The parameters in the driver configuration file can be also changed during runtime using the parameter interface of the `robot_manager` node: - `send_period_ms` (integer): this parameter defines the send rate in milliseconds (with which the controller sends robot state updates). It must be between 1 and 10 for control and can be only changed in `inactive` and `configuring` states. - `receive_multiplier` (integer): this parameter defines the answer rate factor (the client should sends commands in every `receive_multiplier`*`send_period_ms` milliseconds). It must be at least 1 and can be only changed in `inactive` and `configuring` states. -- `control_mode`, `command_mode` (strings): control mode related parameters, which will be combined to support the defined enums. They cannot be changed in active state. -- `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. They will be removed after changing to using the `joint_group_impedance_controller` to adapt to conventions. +- `control_mode`: The enum value of the control mode should be given, which updates the `ControlMode` and `ClientCommandMode` parameters of FRI. It cannot be changed in active state. +- `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. The updated values are sent to the hardware interface using the `joint_group_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `joint_group_impedance_controller` is deactivated at driver activation.) - `position_controller_name`: The name of the controller (string) that controls the `position` interface of the robot. It can't be changed in active state. - `torque_controller_name`: The name of the controller (string) that controls the `effort` interface of the robot. It can't be changed in active state. @@ -39,14 +38,17 @@ The parameters in the driver configuration file can be also changed during runti 1. On the controller, start the uploaded robot application (ROS2_Control). 2. To start the driver, two launch file are available, with and without `rviz`. To launch (without `rviz`), run - ``` - ros2 launch kuka_sunrise_fri_driver startup.launch.py controller_ip:=0.0.0.0 - ``` - - This starts the 3 core components of every driver (described in the [Non-real-time interface](https://github.com/kroshu/kuka_drivers/wiki#non-real-time-interface) section of the project overview) and the following controllers: - - `joint_state_broadcaster` (no configuration file, all state interfaces are published) - - `joint_trajectory_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml)) - - [`fri_configuration_controller`](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#fri_configuration_controller) (no configuration file) - - [`fri_state_broadcaster`](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#fri_state_broadcaster) (no configuration file) +``` +ros2 launch kuka_sunrise_fri_driver startup.launch.py controller_ip:=0.0.0.0 client_ip:=0.0.0.0 +``` +This starts the 3 core components of every driver (described in the [Non-real-time interface](https://github.com/kroshu/kuka_drivers/wiki#non-real-time-interface) section of the project overview) and the following controllers: +- `joint_state_broadcaster` (no configuration file, all state interfaces are published) +- `joint_trajectory_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml)) +- [`fri_configuration_controller`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_configuration_controller) (no configuration file) +- [`fri_state_broadcaster`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_state_broadcaster) (no configuration file) +- `joint_group_impedance_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml)) +- `effort_controller` (of type `JointGroupEffortController`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/effort_controller_config.yaml)) +- [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file) 3. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`): ``` @@ -60,6 +62,8 @@ On successful activation the brakes of the robot will be released and external c Both launch files support the following argument: - `controller_ip`: IP address of the robot controller +- `client_ip`: IP address of the client PC +- `client_port`: port of the client machine (default: 30200) - `robot_model`: defines which LBR iiwa robot to use. Available options: `lbr_iiwa14_r820` (default) - `use_fake_hardware`: if true, the `KukaMockHardwareInterface` will be used instead of the `KukaFRIHardwareInterface`. This enables trying out the driver without actual hardware. - `namespace`: adds a namespace to all nodes and controllers of the driver, and modifies the `prefix` argument of the robot description macro to `namespace_` @@ -68,6 +72,8 @@ Both launch files support the following argument: - `roundtrip_time`: The roundtrip time (in microseconds) to be enforced by the [KUKA mock hardware interface](https://github.com/kroshu/kuka_robot_descriptions?tab=readme-ov-file#custom-mock-hardware), (defaults to 5000 us, only used if `use_fake_hardware` is true) - `controller_config`: the location of the `ros2_control` configuration file (defaults to `kuka_sunrise_fri_driver/config/ros2_controller_config.yaml`) - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml`) +- `jic_config`: the location of the configuration file for the `joint_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml`) +- `ec_config`: the location of the configuration file for the `effort_controller` (defaults to `kuka_sunrise_fri_driver/config/effort_controller_config.yaml`) The `startup_with_rviz.launch.py` additionally contains one argument: @@ -83,9 +89,5 @@ BEWARE, that this is a non-realtime process including lifecycle management, so t ### Known issues and limitations -- Not all hardware-related communication is implemented in the hardware interface, therefore the mock hardware option is not working properly -- The control mode handling for the driver is not the one defined in the `kuka_drivers_core` package - - enum definition and controller switching logic is not used - - joint impedance control is not implemented properly using command interfaces - I/O control was not tested - Cartesian modes are not yet supported diff --git a/doc/wiki/4_Controllers.md b/doc/wiki/4_Controllers.md index 4be12a7a..145d357f 100644 --- a/doc/wiki/4_Controllers.md +++ b/doc/wiki/4_Controllers.md @@ -53,6 +53,6 @@ __Required parameters__: None #### `fri_configuration_controller` -The `receive_multiplier` parameter of FRI defines the answer rate factor (ratio of receiving states and sending commands). This is a parameter of the hardware interface, which can be modified in connected state, when control is not active. To support changing this parameter after startup, the `FRIConfigurationController` implements a service named `~/set_receive_multiplier`. Sending a request containing the desired integer value of the `receive_multiplier` updates the parameter of the hardware interface. +The `SendPeriodMilliSec` parameter of FRI defines the period with which the controller sends state updates, while the `ReceiveMultiplier` defines the answer rate factor (ratio of receiving states and sending commands). These are parameters of the hardware interface, which can be modified in connected state, when control is not active. To support changing these parameters after startup, the `FRIConfigurationController` advertises the topic `~/set_fri_config`. Sending a message containing the desired integer values of `send_period_ms` and `receive_multiplier` updates the parameters of the hardware interface. __Required parameters__: None diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index 70689253..d9f5e59c 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -34,7 +34,7 @@ The startup procedure for any system in ROS can be defined using a launch file, The last issue should be certainly prevented from happening, therefore it was decided to extend the default startup procedure with a [lifecycle interface](https://design.ros2.org/articles/node_lifecycle.html), that synchronizes all components of the driver. The hardware interfaces and controllers already have a lifecycle interface, but by default they are loaded and activated at startup. This configuration was modified to only load the hardware interfaces and controllers, configuration and activation is handled by a custom a lifecycle node, called `robot_manager`. The 3 states of the `robot_manager` node have the following meaning: - `unconfigured`: all necessary components are started, but no connection is needed to the robot -- `configured`: The driver has configured the parameters necessary to start external control. Connection to the robot might be needed. (All of the parameters have default values in the driver, which are set on the robot controller during configuration.) A few [configuration controllers](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#configuration-controllers) might be active, that handle the runtime parameters of the hardware interface. +- `configured`: The driver has valid parameters configured, external control can be initiated. It is possible to change most parameters (with the exception of IP addresses and robot model) in this state without having to clean up the `robot_manager` node. Connection to the robot might be needed. (All of the parameters have default values in the driver, which are set on the robot controller during configuration.) A few [configuration controllers](https://github.com/kroshu/kuka_drivers/wiki/4_Controllers#configuration-controllers) might be active, that handle the runtime parameters of the hardware interface. - `active`: external control is running with cyclic real-time communication, controllers are active To achieve these synchronized states, the state transitions of the system do the following steps (implemented by the launch file and the `robot_manager` node): @@ -61,7 +61,7 @@ The control mode specifications are also part of the common API. They are define - joint position control: the driver streams cyclic position updates for every joint. - Needed command interface(s): `position` -- joint impedance control: the driver streams cyclic position updates for every joint and additionally stiffness [Nm/rad] and normalized damping [-] attributes, which define how the joint reacts to external effects (around the setpoint position). The effect of gravity is compensated internally. +- joint impedance control: the driver streams cyclic position updates for every joint and additionally stiffness [Nm/rad] and normalized damping [-] attributes, which define how the joint reacts to external effects (around the setpoint position). The effect of gravity is compensated internally. (FRI does not allow changing the impedance attributes in runtime, therefore the initial damping and stiffness values are valid for the whole motion.) - Needed command interface(s): `position`, `stiffness`, `damping` - joint velocity control: the driver streams cyclic velocity updates for every joint. - Needed command interface(s): `velocity` diff --git a/kuka_driver_interfaces/CMakeLists.txt b/kuka_driver_interfaces/CMakeLists.txt index d42a11d6..5c7d8056 100644 --- a/kuka_driver_interfaces/CMakeLists.txt +++ b/kuka_driver_interfaces/CMakeLists.txt @@ -20,9 +20,7 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "srv/SetInt.srv" - "srv/GetInt.srv" - "srv/SetDouble.srv" + "msg/FriConfiguration.msg" "msg/FRIState.msg" ) diff --git a/kuka_driver_interfaces/msg/FriConfiguration.msg b/kuka_driver_interfaces/msg/FriConfiguration.msg new file mode 100644 index 00000000..22486e17 --- /dev/null +++ b/kuka_driver_interfaces/msg/FriConfiguration.msg @@ -0,0 +1,3 @@ +# Message with the modifiable values of FRI configuration +int32 receive_multiplier +int32 send_period_ms diff --git a/kuka_driver_interfaces/srv/GetInt.srv b/kuka_driver_interfaces/srv/GetInt.srv deleted file mode 100644 index e360ca99..00000000 --- a/kuka_driver_interfaces/srv/GetInt.srv +++ /dev/null @@ -1,3 +0,0 @@ -# Get an integer value from the server ---- -int64 data diff --git a/kuka_driver_interfaces/srv/SetDouble.srv b/kuka_driver_interfaces/srv/SetDouble.srv deleted file mode 100644 index 7f22bfa5..00000000 --- a/kuka_driver_interfaces/srv/SetDouble.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Attempt to set a double value and indicate and explain if RPC failed -float64 data ---- -bool success -string reason diff --git a/kuka_driver_interfaces/srv/SetInt.srv b/kuka_driver_interfaces/srv/SetInt.srv deleted file mode 100644 index dd7854c9..00000000 --- a/kuka_driver_interfaces/srv/SetInt.srv +++ /dev/null @@ -1,6 +0,0 @@ -# Attempt to set an integer value and indicate and explain if RPC failed -# 32 bit int is used, because that is the default size for most compilers -int32 data ---- -bool success -string reason diff --git a/kuka_drivers_core/README.md b/kuka_drivers_core/README.md index b0dd4f0a..f7530b2e 100644 --- a/kuka_drivers_core/README.md +++ b/kuka_drivers_core/README.md @@ -80,7 +80,7 @@ Improvements: - The parameter type is enforced automatically. - The registered callback has access over all of the registered parameters, therefore the parameter server and the node is always in sync - It is easy to register a callback for additional checks before requested parameter value is accepted. -- The user can define the lifecycle states, in which parameter changes are allowed. +- The user can define the lifecycle states, in which parameter changes are allowed using the `ParameterSetAccessRights` structure. - There is a different endpoint for static parameters, which cannot be changed after initialization. The `Parameter` class supports the following parameter types (identical to the types supported by `rclcpp::Parameter`): @@ -98,7 +98,7 @@ The `Parameter` class supports the following parameter types (identical to th The nodes provide the `registerParameter()` and `registerStaticParameter()` endpoints with the following arguments: - `name` [std::string]: name of the parameter - `value` [T]: default value of the parameter - - `rights` [ParameterAccessRights]: structure defining in which states is the setting of the parameter allowed (only for ROS2BaseLCNode) + - `rights` [ParameterSetAccessRights]: structure defining whether modifying the parameter value is allowed in `inactive` and `active` states (only for `ROS2BaseLCNode`, value changes are always allowed in `unconfigured` state) - `on_change_callback` [std::function]: the callback to call when determining the validity of a parameter change request Both methods register a parameter with the given `name` and `type`, and the `on_change_callback` is called if a parameter change is requested to check validity. In case of the `registerStaticParameter()`, the callback always returns false after initializing the value. @@ -107,8 +107,8 @@ Example code for registering an integer parameter for both base nodes (`onRateCh ```C++ // For class derived from ROS2BaseLCNode registerParameter( - "rate", 2, kuka_drivers_core::ParameterSetAccessRights {true, true, - false, false}, [this](int rate) { + "rate", 2, kuka_drivers_core::ParameterSetAccessRights {true, false}, + [this](int rate) { return this->onRateChangeRequest(rate); }); diff --git a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp index 58f4b0ae..41a0bcf6 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp @@ -26,6 +26,7 @@ static constexpr char FRI_STATE_BROADCASTER[] = "fri_state_broadcaster"; // Controller names with default values static constexpr char JOINT_TRAJECTORY_CONTROLLER[] = "joint_trajectory_controller"; +static constexpr char JOINT_GROUP_IMPEDANCE_CONTROLLER[] = "joint_group_impedance_controller"; } // namespace kuka_drivers_core #endif // KUKA_DRIVERS_CORE__CONTROLLER_NAMES_HPP_ diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index c82fb07e..3338082c 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -40,6 +40,7 @@ static constexpr char STATE_PREFIX[] = "state"; static constexpr char CONTROL_MODE[] = "control_mode"; // Constant defining the receive multiplier interface needed for FRI static constexpr char RECEIVE_MULTIPLIER[] = "receive_multiplier"; +static constexpr char SEND_PERIOD[] = "send_period_ms"; /* FRI state interfaces */ static constexpr char SESSION_STATE[] = "session_state"; diff --git a/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp index f5db7a02..7be3743c 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/parameter_handler.hpp @@ -28,25 +28,18 @@ namespace kuka_drivers_core { struct ParameterSetAccessRights { - bool unconfigured; bool inactive; bool active; - bool finalized; - bool configuring = false; bool isSetAllowed(std::uint8_t current_state) const { switch (current_state) { case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: - return unconfigured; + return true; case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: return inactive; case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: return active; - case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: - return finalized; - case lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING: - return configuring; default: return false; } diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 936f6be9..0c7b15ad 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -59,7 +59,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ // Register parameters this->registerParameter( "position_controller_name", kuka_drivers_core::JOINT_TRAJECTORY_CONTROLLER, - kuka_drivers_core::ParameterSetAccessRights{true, true, false, false, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->controller_handler_.UpdateControllerName( @@ -67,7 +67,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); this->registerParameter( "impedance_controller_name", "joint_group_impedance_controllers", - kuka_drivers_core::ParameterSetAccessRights{true, true, false, false, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->controller_handler_.UpdateControllerName( @@ -75,7 +75,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); this->registerParameter( "torque_controller_name", "effort_controller", - kuka_drivers_core::ParameterSetAccessRights{true, true, false, false, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false}, [this](const std::string & controller_name) { return this->controller_handler_.UpdateControllerName( @@ -83,15 +83,13 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ }); this->registerParameter( "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), - kuka_drivers_core::ParameterSetAccessRights{true, true, true, false, false}, + kuka_drivers_core::ParameterSetAccessRights{true, true}, [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); this->registerStaticParameter( - "controller_ip", "", - kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string &) { return true; }); this->registerStaticParameter( - "robot_model", "lbr_iisy3_r760", - kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + "robot_model", "lbr_iisy3_r760", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); } diff --git a/kuka_kss_rsi_driver/src/robot_manager_node.cpp b/kuka_kss_rsi_driver/src/robot_manager_node.cpp index 837003e1..4f61a4c1 100644 --- a/kuka_kss_rsi_driver/src/robot_manager_node.cpp +++ b/kuka_kss_rsi_driver/src/robot_manager_node.cpp @@ -40,8 +40,7 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ this->create_publisher("robot_manager/is_configured", is_configured_qos); this->registerStaticParameter( - "robot_model", "kr6_r700_sixx", - kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + "robot_model", "kr6_r700_sixx", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); } diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt index ad40f508..7f0c85fe 100644 --- a/kuka_sunrise_fri_driver/CMakeLists.txt +++ b/kuka_sunrise_fri_driver/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++14 +# C++17 needed for string_view if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -77,36 +77,29 @@ add_library(${PROJECT_NAME} SHARED target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_SUNRISE_FRI_DRIVER_BUILDING_LIBRARY") ament_target_dependencies(${PROJECT_NAME} kuka_driver_interfaces hardware_interface kuka_drivers_core) -target_link_libraries(${PROJECT_NAME} fri_client_sdk) - -add_library(configuration_manager SHARED - src/connection_helpers/configuration_manager.cpp) -ament_target_dependencies(configuration_manager std_srvs kuka_drivers_core kuka_driver_interfaces controller_manager_msgs) +target_link_libraries(${PROJECT_NAME} fri_client_sdk fri_connection) add_executable(robot_manager_node src/robot_manager_node.cpp) -ament_target_dependencies(robot_manager_node std_msgs kuka_drivers_core) -target_link_libraries(robot_manager_node - fri_connection - configuration_manager) +ament_target_dependencies(robot_manager_node kuka_driver_interfaces kuka_drivers_core std_msgs std_srvs + controller_manager_msgs) + pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) install(TARGETS ${PROJECT_NAME} robot_manager_node DESTINATION lib/${PROJECT_NAME}) -install(TARGETS fri_connection fri_client_sdk configuration_manager +install(TARGETS fri_connection fri_client_sdk LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ) install(DIRECTORY launch config test DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME}) - if(BUILD_TESTING) find_package(launch_testing_ament_cmake) + add_launch_test(test/test_driver_activation.py) add_launch_test(test/test_driver_startup.py) add_launch_test(test/test_multi_robot_startup.py) endif() diff --git a/kuka_sunrise_fri_driver/config/driver_config.yaml b/kuka_sunrise_fri_driver/config/driver_config.yaml index 0631c7e9..8d550527 100644 --- a/kuka_sunrise_fri_driver/config/driver_config.yaml +++ b/kuka_sunrise_fri_driver/config/driver_config.yaml @@ -1,9 +1,8 @@ robot_manager: ros__parameters: - control_mode: "position" - command_mode: "position" + control_mode: 1 position_controller_name: "joint_trajectory_controller" - torque_controller_name: "" + torque_controller_name: "effort_controller" receive_multiplier: 1 send_period_ms: 10 joint_damping: [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] diff --git a/kuka_sunrise_fri_driver/config/effort_controller_config.yaml b/kuka_sunrise_fri_driver/config/effort_controller_config.yaml new file mode 100644 index 00000000..7dd1d73b --- /dev/null +++ b/kuka_sunrise_fri_driver/config/effort_controller_config.yaml @@ -0,0 +1,10 @@ +effort_controller: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - joint_7 diff --git a/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml b/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml new file mode 100644 index 00000000..78e99c5f --- /dev/null +++ b/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml @@ -0,0 +1,10 @@ +joint_group_impedance_controller: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - joint_7 diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index 9948d169..d340c2b7 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -4,12 +4,19 @@ controller_manager: joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController - + effort_controller: + type: effort_controllers/JointGroupEffortController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + fri_state_broadcaster: + type: kuka_controllers/FRIStateBroadcaster + event_broadcaster: + type: kuka_controllers/EventBroadcaster + # Configuration controllers + joint_group_impedance_controller: + type: kuka_controllers/JointGroupImpedanceController fri_configuration_controller: type: kuka_controllers/FRIConfigurationController - - fri_state_broadcaster: - type: kuka_controllers/FRIStateBroadcaster + control_mode_handler: + type: kuka_controllers/ControlModeHandler diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp deleted file mode 100644 index 5f287bed..00000000 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/configuration_manager.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2020 Zoltán Rési -// -// 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. - -#ifndef KUKA_SUNRISE_FRI_DRIVER__CONFIGURATION_MANAGER_HPP_ -#define KUKA_SUNRISE_FRI_DRIVER__CONFIGURATION_MANAGER_HPP_ - -#include -#include -#include -#include - -#include "controller_manager_msgs/srv/list_controllers.hpp" -#include "kuka_driver_interfaces/srv/set_int.hpp" -#include "kuka_sunrise_fri_driver/fri_connection.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include "kuka_drivers_core/ros2_base_lc_node.hpp" - -namespace kuka_sunrise_fri_driver -{ - -class RobotManager; - -class ConfigurationManager -{ -public: - ConfigurationManager( - std::shared_ptr robot_manager_node, - std::shared_ptr fri_connection); - -private: - bool configured_ = false; - bool position_controller_available_ = false; - bool torque_controller_available_ = false; - std::shared_ptr robot_manager_node_; - std::shared_ptr fri_connection_; - rclcpp::CallbackGroup::SharedPtr cbg_; - rclcpp::CallbackGroup::SharedPtr param_cbg_; - rclcpp::Client::SharedPtr receive_multiplier_client_; - rclcpp::Client::SharedPtr get_controllers_client_; - rclcpp::Service::SharedPtr set_parameter_service_; - - std::vector joint_stiffness_ = std::vector(7, 1000.0); - std::vector joint_damping_ = std::vector(7, 0.7); - - const std::string POSITION_COMMAND = "position"; - const std::string TORQUE_COMMAND = "torque"; - const std::string POSITION_CONTROL = "position"; - const std::string IMPEDANCE_CONTROL = "joint_impedance"; - - bool onCommandModeChangeRequest(const std::string & command_mode) const; - bool onControlModeChangeRequest(const std::string & control_mode) const; - bool onJointStiffnessChangeRequest(const std::vector & joint_stiffness); - bool onJointDampingChangeRequest(const std::vector & joint_damping); - bool onSendPeriodChangeRequest(const int & send_period) const; - bool onReceiveMultiplierChangeRequest(const int & receive_multiplier) const; - bool onControllerIpChangeRequest(const std::string & controller_ip) const; - bool onControllerNameChangeRequest(const std::string & controller_name, bool position); - bool setCommandMode(const std::string & control_mode) const; - bool setReceiveMultiplier(int receive_multiplier) const; - void setParameters(std_srvs::srv::Trigger::Response::SharedPtr response); -}; -} // namespace kuka_sunrise_fri_driver - -#endif // KUKA_SUNRISE_FRI_DRIVER__CONFIGURATION_MANAGER_HPP_ diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp index bca1c5c0..a895e06c 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/fri_connection.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include namespace kuka_sunrise_fri_driver @@ -68,7 +69,7 @@ enum ClientCommandModeID : std::uint8_t TORQUE_COMMAND_MODE = 3 }; -static const std::vector FRI_CONFIG_HEADER = {0xAC, 0xED, 0x00, 0x05, 0x77, 0x0C}; +static const std::vector FRI_CONFIG_HEADER = {0xAC, 0xED, 0x00, 0x05, 0x77, 0x10}; static const std::vector CONTROL_MODE_HEADER = {0xAC, 0xED, 0x00, 0x05, 0x77, 0x70}; class FRIConnection @@ -89,7 +90,8 @@ class FRIConnection const std::vector & joint_stiffness, const std::vector & joint_damping); bool setClientCommandMode(ClientCommandModeID client_command_mode); // bool getControlMode(); - bool setFRIConfig(int remote_port, int send_period_ms, int receive_multiplier); + bool setFRIConfig( + const std::string & client_ip, int remote_port, int send_period_ms, int receive_multiplier); // bool getFRIConfig(); bool isConnected(); diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index 3278388b..661f67d5 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -25,12 +25,14 @@ #include "rclcpp/rclcpp.hpp" #include "hardware_interface/system_interface.hpp" -#include "kuka_driver_interfaces/srv/set_int.hpp" +#include "kuka_drivers_core/control_mode.hpp" +#include "kuka_drivers_core/hardware_event.hpp" #include "fri_client_sdk/HWIFClientApplication.hpp" #include "fri_client_sdk/friClientIf.h" #include "fri_client_sdk/friLBRClient.h" #include "fri_client_sdk/friUdpConnection.h" +#include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "kuka_sunrise_fri_driver/visibility_control.h" using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -54,6 +56,7 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, public: RCLCPP_SHARED_PTR_DEFINITIONS(KukaFRIHardwareInterface) + // Set UDP timeout to 10 ms to enable checking return value of client_app_read() KUKA_SUNRISE_FRI_DRIVER_PUBLIC KukaFRIHardwareInterface() : client_application_(udp_connection_, *this) { @@ -61,6 +64,10 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn + on_configure(const rclcpp_lifecycle::State & previous_state) override; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn + on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; @@ -92,26 +99,47 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, }; private: - bool is_active_ = false; + KUKA_SUNRISE_FRI_DRIVER_LOCAL bool FRIConfigChanged(); + bool active_read_ = false; - KUKA::FRI::UdpConnection udp_connection_; + std::string controller_ip_; + KUKA::FRI::UdpConnection udp_connection_ = KUKA::FRI::UdpConnection(10); KUKA::FRI::HWIFClientApplication client_application_; - - rclcpp::Service::SharedPtr set_receive_multiplier_service_; + std::shared_ptr fri_connection_; rclcpp::Clock ros_clock_; // Command interface must be of type double, but controller can set only integers // this is a temporary solution, until runtime parameters are supported for hardware interfaces + double control_mode_ = 0; // default to undefined double receive_multiplier_ = 1; + double send_period_ms_ = 10; + int client_port_ = 30200; + std::string client_ip_ = "0.0.0.0"; int receive_counter_ = 0; bool torque_command_mode_ = false; + int prev_period_ = 0; + int prev_multiplier_ = 0; + // State and command interfaces - std::vector hw_commands_; - std::vector hw_states_; - std::vector hw_torques_ext_; - std::vector hw_torques_; - std::vector hw_effort_command_; + std::vector hw_position_commands_; + std::vector hw_torque_commands_; + std::vector hw_stiffness_commands_; + std::vector hw_damping_commands_; + + std::vector hw_position_states_; + std::vector hw_torque_states_; + std::vector hw_ext_torque_states_; + + double server_state_ = 0; + + std::mutex event_mutex_; + + kuka_drivers_core::HardwareEvent last_event_ = + kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED; + + static const int TCP_SERVER_PORT = 30000; + static const int DOF = 7; struct RobotState { @@ -129,6 +157,9 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, RobotState robot_state_; + void activateFrictionCompensation(double * values) const; + void onError(); + KUKA_SUNRISE_FRI_DRIVER_LOCAL IOTypes getType(const std::string & type_string) const { auto it = types.find(type_string); diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 657b06c3..7bf4c44e 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -17,18 +17,23 @@ #include #include -#include +#include +#include #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" #include "rclcpp/client.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" -#include "std_srvs/srv/trigger.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "std_msgs/msg/u_int32.hpp" +#include "std_msgs/msg/u_int8.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "kuka_driver_interfaces/msg/fri_configuration.hpp" +#include "kuka_drivers_core/control_mode.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" -#include "kuka_sunrise_fri_driver/configuration_manager.hpp" #include "kuka_sunrise_fri_driver/fri_connection.hpp" namespace kuka_sunrise_fri_driver @@ -51,28 +56,44 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State &); - bool activateControl(); - bool deactivateControl(); - private: std::shared_ptr fri_connection_; - std::unique_ptr configuration_manager_; - rclcpp::Client::SharedPtr set_parameter_client_; rclcpp::Client::SharedPtr change_hardware_state_client_; rclcpp::Client::SharedPtr change_controller_state_client_; rclcpp::CallbackGroup::SharedPtr cbg_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - command_state_changed_publisher_; + rclcpp::CallbackGroup::SharedPtr event_cbg_; std::shared_ptr> is_configured_pub_; std_msgs::msg::Bool is_configured_msg_; - std::string controller_name_; + rclcpp::Publisher::SharedPtr fri_config_pub_; + rclcpp::Publisher::SharedPtr control_mode_pub_; + rclcpp::Publisher::SharedPtr joint_imp_pub_; + rclcpp::Subscription::SharedPtr event_subscriber_; + std_msgs::msg::UInt32 control_mode_msg_; + + int receive_multiplier_ = 0; + int send_period_ms_ = 0; std::string robot_model_; + std::string joint_pos_controller_name_; + std::string joint_torque_controller_name_; + std::vector joint_stiffness_ = std::vector(7, 100.0); + std::vector joint_damping_ = std::vector(7, 0.7); - void handleControlEndedError(); - void handleFRIEndedError(); + std::string GetControllerName() const; + bool onControlModeChangeRequest(int control_mode); bool onRobotModelChangeRequest(const std::string & robot_model); + bool onSendPeriodChangeRequest(int send_period); + bool setReceiveMultiplier(int receive_multiplier); + bool onReceiveMultiplierChangeRequest(const int & receive_multiplier); + bool ValidateIPAdress(std::string_view controller_ip) const; + bool onControllerNameChangeRequest( + std::string_view controller_name, kuka_drivers_core::ControllerType controller_type); + bool onJointDampingChangeRequest(const std::vector & joint_damping); + bool onJointStiffnessChangeRequest(const std::vector & joint_stiffness); + void setFriConfiguration(int send_period_ms, int receive_multiplier) const; + + void EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg); }; } // namespace kuka_sunrise_fri_driver diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/serialization.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/serialization.hpp index 1eae70cd..d1272036 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/serialization.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/serialization.hpp @@ -15,11 +15,13 @@ #ifndef KUKA_SUNRISE_FRI_DRIVER__SERIALIZATION_HPP_ #define KUKA_SUNRISE_FRI_DRIVER__SERIALIZATION_HPP_ +#include #include #include +#include #include -namespace kuka_drivers_core +namespace kuka_sunrise_fri_driver { int serializeNext(int integer_in, std::vector & serialized_out) @@ -70,7 +72,6 @@ int deserializeNext(const std::vector & serialized_in, double & do double_out = *(reinterpret_cast(serialized_copy.data())); return sizeof(int); } - -} // namespace kuka_drivers_core +} // namespace kuka_sunrise_fri_driver #endif // KUKA_SUNRISE_FRI_DRIVER__SERIALIZATION_HPP_ diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 81d088f0..7ab660f0 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -25,6 +25,8 @@ def launch_setup(context, *args, **kwargs): robot_model = LaunchConfiguration("robot_model") use_fake_hardware = LaunchConfiguration("use_fake_hardware") controller_ip = LaunchConfiguration("controller_ip") + client_ip = LaunchConfiguration("client_ip") + client_port = LaunchConfiguration("client_port") ns = LaunchConfiguration("namespace") x = LaunchConfiguration("x") y = LaunchConfiguration("y") @@ -35,6 +37,8 @@ def launch_setup(context, *args, **kwargs): roundtrip_time = LaunchConfiguration("roundtrip_time") controller_config = LaunchConfiguration("controller_config") jtc_config = LaunchConfiguration("jtc_config") + jic_config = LaunchConfiguration("jic_config") + ec_config = LaunchConfiguration("ec_config") if ns.perform(context) == "": tf_prefix = "" else: @@ -53,6 +57,15 @@ def launch_setup(context, *args, **kwargs): ] ), " ", + "controller_ip:=", + controller_ip, + " ", + "client_ip:=", + client_ip, + " ", + "client_port:=", + client_port, + " ", "use_fake_hardware:=", use_fake_hardware, " ", @@ -101,6 +114,8 @@ def launch_setup(context, *args, **kwargs): robot_description, controller_config, jtc_config, + jic_config, + ec_config, { "hardware_components_initial_state": { "unconfigured": [tf_prefix + robot_model.perform(context)] @@ -147,6 +162,10 @@ def controller_spawner(controller_names, activate=False): "joint_trajectory_controller", "fri_configuration_controller", "fri_state_broadcaster", + "joint_group_impedance_controller", + "effort_controller", + "control_mode_handler", + "event_broadcaster", ] controller_spawners = [controller_spawner(name) for name in controller_names] @@ -164,6 +183,8 @@ def generate_launch_description(): launch_arguments = [] launch_arguments.append(DeclareLaunchArgument("robot_model", default_value="lbr_iiwa14_r820")) launch_arguments.append(DeclareLaunchArgument("controller_ip", default_value="0.0.0.0")) + launch_arguments.append(DeclareLaunchArgument("client_ip", default_value="0.0.0.0")) + launch_arguments.append(DeclareLaunchArgument("client_port", default_value="30200")) launch_arguments.append(DeclareLaunchArgument("use_fake_hardware", default_value="false")) launch_arguments.append(DeclareLaunchArgument("namespace", default_value="")) launch_arguments.append(DeclareLaunchArgument("x", default_value="0")) @@ -187,4 +208,18 @@ def generate_launch_description(): + "/config/joint_trajectory_controller_config.yaml", ) ) + launch_arguments.append( + DeclareLaunchArgument( + "jic_config", + default_value=get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/joint_impedance_controller_config.yaml", + ) + ) + launch_arguments.append( + DeclareLaunchArgument( + "ec_config", + default_value=get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/effort_controller_config.yaml", + ) + ) return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index 7deb6ce7..7a1966c0 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -25,6 +25,9 @@ joint_state_broadcaster fri_configuration_controller fri_state_broadcaster + kuka_control_mode_handler + kuka_event_broadcaster + joint_group_impedance_controller kuka_lbr_iiwa_support diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java old mode 100644 new mode 100755 similarity index 96% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/application/ROS2_Control.java rename to kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java index 447b2f94..d5fc6c6a --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/application/ROS2_Control.java +++ b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java @@ -1,67 +1,67 @@ -package application; - -import javax.inject.Inject; - -import ros2.modules.FRIManager; -import ros2.modules.ROS2Connection; -import ros2.modules.TCPConnection; - -import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; -import com.kuka.roboticsAPI.deviceModel.LBR; - -/** - * Implementation of a robot application. - *

- * The application provides a {@link RoboticsAPITask#initialize()} and a - * {@link RoboticsAPITask#run()} method, which will be called successively in - * the application lifecycle. The application will terminate automatically after - * the {@link RoboticsAPITask#run()} method has finished or after stopping the - * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an - * exception is thrown during initialization or run. - *

- * It is imperative to call super.dispose() when overriding the - * {@link RoboticsAPITask#dispose()} method. - * - * @see UseRoboticsAPIContext - * @see #initialize() - * @see #run() - * @see #dispose() - */ -public class ROS2_Control extends RoboticsAPIApplication { - @Inject - private LBR _lbr; - - private TCPConnection _TCPConnection; - private ROS2Connection _ROS2Connection; - private FRIManager _FRIManager; - - @Override - public void initialize() { - // initialize your application here - _TCPConnection = new TCPConnection(30000); - _ROS2Connection = new ROS2Connection(); - _FRIManager = new FRIManager(_lbr, getApplicationControl()); - - _FRIManager.registerROS2ConnectionModule(_ROS2Connection); - _TCPConnection.registerROS2ConnectionModule(_ROS2Connection); - _ROS2Connection.registerTCPConnectionModule(_TCPConnection); - _ROS2Connection.registerFRIManagerModule(_FRIManager); - } - - @Override - public void run() { - // your application execution starts here - _ROS2Connection.acceptCommands(); - _TCPConnection.openConnection(); - _TCPConnection.waitUntilDisconnected(); - _ROS2Connection.rejectCommands(); - _FRIManager.close(); - } - - @Override - public void dispose() { - getLogger().info("disposes"); - _TCPConnection.closeConnection(); - _FRIManager.close(); - } -} +package application; + +import javax.inject.Inject; + +import ros2.modules.FRIManager; +import ros2.modules.ROS2Connection; +import ros2.modules.TCPConnection; + +import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; +import com.kuka.roboticsAPI.deviceModel.LBR; + +/** + * Implementation of a robot application. + *

+ * The application provides a {@link RoboticsAPITask#initialize()} and a + * {@link RoboticsAPITask#run()} method, which will be called successively in + * the application lifecycle. The application will terminate automatically after + * the {@link RoboticsAPITask#run()} method has finished or after stopping the + * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an + * exception is thrown during initialization or run. + *

+ * It is imperative to call super.dispose() when overriding the + * {@link RoboticsAPITask#dispose()} method. + * + * @see UseRoboticsAPIContext + * @see #initialize() + * @see #run() + * @see #dispose() + */ +public class ROS2_Control extends RoboticsAPIApplication { + @Inject + private LBR _lbr; + + private TCPConnection _TCPConnection; + private ROS2Connection _ROS2Connection; + private FRIManager _FRIManager; + + @Override + public void initialize() { + // initialize your application here + _TCPConnection = new TCPConnection(30000); + _ROS2Connection = new ROS2Connection(); + _FRIManager = new FRIManager(_lbr, getApplicationControl()); + + _FRIManager.registerROS2ConnectionModule(_ROS2Connection); + _TCPConnection.registerROS2ConnectionModule(_ROS2Connection); + _ROS2Connection.registerTCPConnectionModule(_TCPConnection); + _ROS2Connection.registerFRIManagerModule(_FRIManager); + } + + @Override + public void run() { + // your application execution starts here + _ROS2Connection.acceptCommands(); + _TCPConnection.openConnection(); + _TCPConnection.waitUntilDisconnected(); + _ROS2Connection.rejectCommands(); + _FRIManager.close(); + } + + @Override + public void dispose() { + getLogger().info("disposes"); + _TCPConnection.closeConnection(); + _FRIManager.close(); + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java b/kuka_sunrise_fri_driver/robot_application/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java old mode 100644 new mode 100755 similarity index 52% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java rename to kuka_sunrise_fri_driver/robot_application/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java index 40eef573..1fc50a4b --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java +++ b/kuka_sunrise_fri_driver/robot_application/src/com/kuka/generated/ioAccess/MediaFlangeIOGroup.java @@ -12,7 +12,7 @@ * Please, do not modify! *

* I/O group description:
- * This I/O Group contains the In-/Outputs for the Media-Flange IO. + * This I/O Group contains the In-/Outputs for the Media-Flange Touch. */ @Singleton public class MediaFlangeIOGroup extends AbstractIOGroup @@ -29,22 +29,22 @@ public MediaFlangeIOGroup(Controller controller) { super(controller, "MediaFlange"); - addInput("Input1", IOTypes.BOOLEAN, 1); - addInput("Input2", IOTypes.BOOLEAN, 1); - addInput("Input3", IOTypes.BOOLEAN, 1); - addInput("Input4", IOTypes.BOOLEAN, 1); - addInput("Input5", IOTypes.BOOLEAN, 1); - addInput("Input6", IOTypes.BOOLEAN, 1); - addInput("Input7", IOTypes.BOOLEAN, 1); - addInput("Input8", IOTypes.BOOLEAN, 1); - addDigitalOutput("Output1", IOTypes.BOOLEAN, 1); - addDigitalOutput("Output2", IOTypes.BOOLEAN, 1); - addDigitalOutput("Output3", IOTypes.BOOLEAN, 1); - addDigitalOutput("Output4", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin3", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin4", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin10", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin13", IOTypes.BOOLEAN, 1); + addInput("InputX3Pin16", IOTypes.BOOLEAN, 1); + addInput("UserButton", IOTypes.BOOLEAN, 1); + addDigitalOutput("LEDBlue", IOTypes.BOOLEAN, 1); + addDigitalOutput("SwitchOffX3Voltage", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin1", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin2", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin11", IOTypes.BOOLEAN, 1); + addDigitalOutput("OutputX3Pin12", IOTypes.BOOLEAN, 1); } /** - * Gets the value of the digital input 'Input1'.
+ * Gets the value of the digital input 'InputX3Pin3'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -56,15 +56,15 @@ public MediaFlangeIOGroup(Controller controller) * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input1' + * @return current value of the digital input 'InputX3Pin3' */ - public boolean getInput1() + public boolean getInputX3Pin3() { - return getBooleanIOValue("Input1", false); + return getBooleanIOValue("InputX3Pin3", false); } /** - * Gets the value of the digital input 'Input2'.
+ * Gets the value of the digital input 'InputX3Pin4'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -76,15 +76,15 @@ public boolean getInput1() * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input2' + * @return current value of the digital input 'InputX3Pin4' */ - public boolean getInput2() + public boolean getInputX3Pin4() { - return getBooleanIOValue("Input2", false); + return getBooleanIOValue("InputX3Pin4", false); } /** - * Gets the value of the digital input 'Input3'.
+ * Gets the value of the digital input 'InputX3Pin10'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -96,15 +96,15 @@ public boolean getInput2() * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input3' + * @return current value of the digital input 'InputX3Pin10' */ - public boolean getInput3() + public boolean getInputX3Pin10() { - return getBooleanIOValue("Input3", false); + return getBooleanIOValue("InputX3Pin10", false); } /** - * Gets the value of the digital input 'Input4'.
+ * Gets the value of the digital input 'InputX3Pin13'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -116,15 +116,15 @@ public boolean getInput3() * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input4' + * @return current value of the digital input 'InputX3Pin13' */ - public boolean getInput4() + public boolean getInputX3Pin13() { - return getBooleanIOValue("Input4", false); + return getBooleanIOValue("InputX3Pin13", false); } /** - * Gets the value of the digital input 'Input5'.
+ * Gets the value of the digital input 'InputX3Pin16'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -136,15 +136,15 @@ public boolean getInput4() * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input5' + * @return current value of the digital input 'InputX3Pin16' */ - public boolean getInput5() + public boolean getInputX3Pin16() { - return getBooleanIOValue("Input5", false); + return getBooleanIOValue("InputX3Pin16", false); } /** - * Gets the value of the digital input 'Input6'.
+ * Gets the value of the digital input 'UserButton'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -156,19 +156,19 @@ public boolean getInput5() * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input6' + * @return current value of the digital input 'UserButton' */ - public boolean getInput6() + public boolean getUserButton() { - return getBooleanIOValue("Input6", false); + return getBooleanIOValue("UserButton", false); } /** - * Gets the value of the digital input 'Input7'.
+ * Gets the value of the digital output 'LEDBlue'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
- * digital input + * digital output *

* User description of the I/O:
* ./. @@ -176,19 +176,40 @@ public boolean getInput6() * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input7' + * @return current value of the digital output 'LEDBlue' */ - public boolean getInput7() + public boolean getLEDBlue() { - return getBooleanIOValue("Input7", false); + return getBooleanIOValue("LEDBlue", true); } /** - * Gets the value of the digital input 'Input8'.
+ * Sets the value of the digital output 'LEDBlue'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
- * digital input + * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'LEDBlue' + */ + public void setLEDBlue(java.lang.Boolean value) + { + setDigitalOutput("LEDBlue", value); + } + + /** + * Gets the value of the digital output 'SwitchOffX3Voltage'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output *

* User description of the I/O:
* ./. @@ -196,15 +217,36 @@ public boolean getInput7() * Range of the I/O value:
* [false; true] * - * @return current value of the digital input 'Input8' + * @return current value of the digital output 'SwitchOffX3Voltage' + */ + public boolean getSwitchOffX3Voltage() + { + return getBooleanIOValue("SwitchOffX3Voltage", true); + } + + /** + * Sets the value of the digital output 'SwitchOffX3Voltage'.
+ * This method is automatically generated. Please, do not modify! + *

+ * I/O direction and type:
+ * digital output + *

+ * User description of the I/O:
+ * ./. + *

+ * Range of the I/O value:
+ * [false; true] + * + * @param value + * the value, which has to be written to the digital output 'SwitchOffX3Voltage' */ - public boolean getInput8() + public void setSwitchOffX3Voltage(java.lang.Boolean value) { - return getBooleanIOValue("Input8", false); + setDigitalOutput("SwitchOffX3Voltage", value); } /** - * Gets the value of the digital output 'Output1'.
+ * Gets the value of the digital output 'OutputX3Pin1'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -216,15 +258,15 @@ public boolean getInput8() * Range of the I/O value:
* [false; true] * - * @return current value of the digital output 'Output1' + * @return current value of the digital output 'OutputX3Pin1' */ - public boolean getOutput1() + public boolean getOutputX3Pin1() { - return getBooleanIOValue("Output1", true); + return getBooleanIOValue("OutputX3Pin1", true); } /** - * Sets the value of the digital output 'Output1'.
+ * Sets the value of the digital output 'OutputX3Pin1'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -237,15 +279,15 @@ public boolean getOutput1() * [false; true] * * @param value - * the value, which has to be written to the digital output 'Output1' + * the value, which has to be written to the digital output 'OutputX3Pin1' */ - public void setOutput1(java.lang.Boolean value) + public void setOutputX3Pin1(java.lang.Boolean value) { - setDigitalOutput("Output1", value); + setDigitalOutput("OutputX3Pin1", value); } /** - * Gets the value of the digital output 'Output2'.
+ * Gets the value of the digital output 'OutputX3Pin2'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -257,15 +299,15 @@ public void setOutput1(java.lang.Boolean value) * Range of the I/O value:
* [false; true] * - * @return current value of the digital output 'Output2' + * @return current value of the digital output 'OutputX3Pin2' */ - public boolean getOutput2() + public boolean getOutputX3Pin2() { - return getBooleanIOValue("Output2", true); + return getBooleanIOValue("OutputX3Pin2", true); } /** - * Sets the value of the digital output 'Output2'.
+ * Sets the value of the digital output 'OutputX3Pin2'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -278,15 +320,15 @@ public boolean getOutput2() * [false; true] * * @param value - * the value, which has to be written to the digital output 'Output2' + * the value, which has to be written to the digital output 'OutputX3Pin2' */ - public void setOutput2(java.lang.Boolean value) + public void setOutputX3Pin2(java.lang.Boolean value) { - setDigitalOutput("Output2", value); + setDigitalOutput("OutputX3Pin2", value); } /** - * Gets the value of the digital output 'Output3'.
+ * Gets the value of the digital output 'OutputX3Pin11'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -298,15 +340,15 @@ public void setOutput2(java.lang.Boolean value) * Range of the I/O value:
* [false; true] * - * @return current value of the digital output 'Output3' + * @return current value of the digital output 'OutputX3Pin11' */ - public boolean getOutput3() + public boolean getOutputX3Pin11() { - return getBooleanIOValue("Output3", true); + return getBooleanIOValue("OutputX3Pin11", true); } /** - * Sets the value of the digital output 'Output3'.
+ * Sets the value of the digital output 'OutputX3Pin11'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -319,15 +361,15 @@ public boolean getOutput3() * [false; true] * * @param value - * the value, which has to be written to the digital output 'Output3' + * the value, which has to be written to the digital output 'OutputX3Pin11' */ - public void setOutput3(java.lang.Boolean value) + public void setOutputX3Pin11(java.lang.Boolean value) { - setDigitalOutput("Output3", value); + setDigitalOutput("OutputX3Pin11", value); } /** - * Gets the value of the digital output 'Output4'.
+ * Gets the value of the digital output 'OutputX3Pin12'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -339,15 +381,15 @@ public void setOutput3(java.lang.Boolean value) * Range of the I/O value:
* [false; true] * - * @return current value of the digital output 'Output4' + * @return current value of the digital output 'OutputX3Pin12' */ - public boolean getOutput4() + public boolean getOutputX3Pin12() { - return getBooleanIOValue("Output4", true); + return getBooleanIOValue("OutputX3Pin12", true); } /** - * Sets the value of the digital output 'Output4'.
+ * Sets the value of the digital output 'OutputX3Pin12'.
* This method is automatically generated. Please, do not modify! *

* I/O direction and type:
@@ -360,11 +402,11 @@ public boolean getOutput4() * [false; true] * * @param value - * the value, which has to be written to the digital output 'Output4' + * the value, which has to be written to the digital output 'OutputX3Pin12' */ - public void setOutput4(java.lang.Boolean value) + public void setOutputX3Pin12(java.lang.Boolean value) { - setDigitalOutput("Output4", value); + setDigitalOutput("OutputX3Pin12", value); } } diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java old mode 100644 new mode 100755 similarity index 96% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/FRIManager.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java index 299b9362..4dc69e58 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java @@ -1,267 +1,266 @@ -package ros2.modules; - -import java.util.Arrays; -import java.util.List; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.TimeoutException; - -import ros2.serialization.FRIConfigurationParams; - -import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; -import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation.FRISessionState; -import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation; -import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; -import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay; -import com.kuka.connectivity.fastRobotInterface.FRISession; -import com.kuka.roboticsAPI.applicationModel.IApplicationControl; -import com.kuka.roboticsAPI.deviceModel.Device; -import com.kuka.roboticsAPI.deviceModel.LBR; -import com.kuka.roboticsAPI.motionModel.ErrorHandlingAction; -import com.kuka.roboticsAPI.motionModel.IErrorHandler; -import com.kuka.roboticsAPI.motionModel.IMotionContainer; -import com.kuka.roboticsAPI.motionModel.PositionHold; -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; -import com.kuka.roboticsAPI.requestModel.GetApplicationOverrideRequest; - -public class FRIManager{ - public enum CommandResult{ - EXECUTED, - REJECTED, - ERRORED; - } - private ROS2Connection _ROS2Connection; - - private State _currentState; - private LBR _lbr; - private FRISession _FRISession; - private FRIConfiguration _FRIConfiguration; - private IMotionControlMode _controlMode; - private ClientCommandMode _clientCommandMode; - private IMotionContainer _motionContainer; - private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); - //private IApplicationControl _applicationControl; - - private static double[] stiffness_ = new double[7]; - - public FRIManager(LBR lbr, IApplicationControl applicationControl){ - _currentState = new InactiveState(); - _lbr = lbr; - _FRISession = null; - _FRIConfiguration = new FRIConfigurationParams().toFRIConfiguration(_lbr); - Arrays.fill(stiffness_, 200); - _controlMode = new JointImpedanceControlMode(stiffness_); - //_controlMode = new PositionControlMode(); - _clientCommandMode = ClientCommandMode.POSITION; - //_applicationControl = applicationControl; - applicationControl.registerMoveAsyncErrorHandler(_friMotionErrorHandler); - } - - public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ - _ROS2Connection = ros2ConnectionModule; - } - - public void close(){ - if(_currentState instanceof ControlActiveState){ - deactivateControl(); - } - if(_currentState instanceof FRIActiveState){ - endFRI();//TODO: handle error - } - } - - public FRIConfigurationParams getFRIConfig(){ - FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(_FRIConfiguration); - return friConfigurationParams; - } - - public ClientCommandMode getClientCommandMode(){ - return _clientCommandMode; - } - - public IMotionControlMode getControlMode(){ - return _controlMode; - } - - public FRISessionState getFRISessionState(){ - return _FRISession.getFRIChannelInformation().getFRISessionState(); - } - - /* - * The Following commands change the state of the FRI Manager, and thus are forwarded to the state machine for validation - * */ - - public CommandResult setControlMode(IMotionControlMode controlMode){ - return _currentState.setControlMode(controlMode); - } - - public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ - return _currentState.setCommandMode(clientCommandMode); - } - - public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ - return _currentState.setFRIConfig(friConfigurationParams); - } - - public CommandResult startFRI(){ - CommandResult commandResult = _currentState.startFRI(); - if(commandResult == CommandResult.EXECUTED){ - _currentState = new FRIActiveState(); - } - return commandResult; - } - - public CommandResult endFRI(){ - CommandResult commandResult = _currentState.endFRI(); - if(commandResult == CommandResult.EXECUTED){ - _currentState = new InactiveState(); - } - return commandResult; - } - - public CommandResult activateControl(){ - CommandResult commandResult = _currentState.activateControl(); - if(commandResult == CommandResult.EXECUTED){ - _currentState = new ControlActiveState(); - } - return commandResult; - } - - public CommandResult deactivateControl(){ - CommandResult commandResult = _currentState.deactivateControl(); - if(commandResult == CommandResult.EXECUTED){ - _currentState = new FRIActiveState(); - } - return commandResult; - } - - private class State{ - /* By default reject all commands */ - public CommandResult startFRI(){ - return CommandResult.REJECTED; - } - - public CommandResult endFRI(){ - return CommandResult.REJECTED; - } - - public CommandResult activateControl(){ - return CommandResult.REJECTED; - } - - public CommandResult deactivateControl(){ - return CommandResult.REJECTED; - } - - public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ - return CommandResult.REJECTED; - } - - public CommandResult setControlMode(IMotionControlMode controlMode){ - return CommandResult.REJECTED; - } - - public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ - return CommandResult.REJECTED; - } - } - - private class InactiveState extends State{ - @Override - public CommandResult startFRI(){ - FRIManager.this._FRISession = new FRISession(FRIManager.this._FRIConfiguration); - try { - FRIManager.this._FRISession.await(10, TimeUnit.SECONDS); - } catch (TimeoutException e) { - FRIManager.this._FRISession.close(); - return CommandResult.ERRORED; - } - - return CommandResult.EXECUTED; - } - @Override - public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ - FRIManager.this._FRIConfiguration = friConfigurationParams.toFRIConfiguration(FRIManager.this._lbr); - return CommandResult.EXECUTED; - } - @Override - public CommandResult setControlMode(IMotionControlMode controlMode){ - FRIManager.this._controlMode = controlMode; - return CommandResult.EXECUTED; - } - @Override - public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ - FRIManager.this._clientCommandMode = clientCommandMode; - return CommandResult.EXECUTED; - } - } - - private class FRIActiveState extends State { - @Override - public CommandResult endFRI(){ - try{ - FRIManager.this._FRISession.close(); - } catch(IllegalStateException e){ - return CommandResult.ERRORED; - } - return CommandResult.EXECUTED; - } - @Override - public CommandResult activateControl(){ - FRIJointOverlay friJointOverlay = - new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode); - //friJointOverlay.overrideJointAcceleration(20.0); - PositionHold motion = - new PositionHold(FRIManager.this._controlMode, -1, null); - FRIManager.this._motionContainer = - FRIManager.this._lbr.moveAsync(motion.addMotionOverlay(friJointOverlay)); - return CommandResult.EXECUTED; - } - @Override - public CommandResult setControlMode(IMotionControlMode controlMode){ - FRIManager.this._controlMode = controlMode; - return CommandResult.EXECUTED; - } - @Override - public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ - FRIManager.this._clientCommandMode = clientCommandMode; - return CommandResult.EXECUTED; - } - } - - private class ControlActiveState extends State { - @Override - public CommandResult deactivateControl(){ - FRIManager.this._motionContainer.cancel(); - return CommandResult.EXECUTED; - } - } - - private class FRIMotionErrorHandler implements IErrorHandler{ - - @Override - public ErrorHandlingAction handleError(Device device, - IMotionContainer failedContainer, - List canceledContainers) { - FRISessionState sessionState = _FRISession.getFRIChannelInformation().getFRISessionState(); - switch(sessionState){ - case IDLE: - FRIManager.this._ROS2Connection.handleFRIEndedError(); - //FRIManager.this._currentState = new InactiveState(); - break; - default: - FRIManager.this._ROS2Connection.handleControlEndedError(); - //FRIManager.this._currentState = new FRIActiveState(); - break; - } - System.out.println("Failed container: " + failedContainer.toString() + "."); - System.out.println("Error: " + failedContainer.getErrorMessage()); - System.out.println("Runtime data: " + failedContainer.getRuntimeData()); - System.out.println("Cancelled containers: " + canceledContainers.toString()); - return ErrorHandlingAction.Ignore; - } - - } - -} +package ros2.modules; + +import java.util.Arrays; +import java.util.List; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.TimeoutException; + +import ros2.serialization.FRIConfigurationParams; + +import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; +import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation.FRISessionState; +import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation; +import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; +import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay; +import com.kuka.connectivity.fastRobotInterface.FRISession; +import com.kuka.roboticsAPI.applicationModel.IApplicationControl; +import com.kuka.roboticsAPI.deviceModel.Device; +import com.kuka.roboticsAPI.deviceModel.LBR; +import com.kuka.roboticsAPI.motionModel.ErrorHandlingAction; +import com.kuka.roboticsAPI.motionModel.IErrorHandler; +import com.kuka.roboticsAPI.motionModel.IMotionContainer; +import com.kuka.roboticsAPI.motionModel.PositionHold; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; +import com.kuka.roboticsAPI.requestModel.GetApplicationOverrideRequest; + +public class FRIManager{ + public enum CommandResult{ + EXECUTED, + REJECTED, + ERRORED; + } + private ROS2Connection _ROS2Connection; + + private State _currentState; + private LBR _lbr; + private FRISession _FRISession; + private FRIConfiguration _FRIConfiguration; + private IMotionControlMode _controlMode; + private ClientCommandMode _clientCommandMode; + private IMotionContainer _motionContainer; + private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); + //private IApplicationControl _applicationControl; + + private static double[] stiffness_ = new double[7]; + + public FRIManager(LBR lbr, IApplicationControl applicationControl){ + _currentState = new InactiveState(); + _lbr = lbr; + _FRISession = null; + _FRIConfiguration = new FRIConfigurationParams().toFRIConfiguration(_lbr); + Arrays.fill(stiffness_, 200); + _controlMode = new JointImpedanceControlMode(stiffness_); + //_controlMode = new PositionControlMode(); + _clientCommandMode = ClientCommandMode.POSITION; + //_applicationControl = applicationControl; + applicationControl.registerMoveAsyncErrorHandler(_friMotionErrorHandler); + } + + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ + _ROS2Connection = ros2ConnectionModule; + } + + public void close(){ + if(_currentState instanceof ControlActiveState){ + deactivateControl(); + } + if(_currentState instanceof FRIActiveState){ + endFRI();//TODO: handle error + } + } + + public FRIConfigurationParams getFRIConfig(){ + FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(_FRIConfiguration); + return friConfigurationParams; + } + + public ClientCommandMode getClientCommandMode(){ + return _clientCommandMode; + } + + public IMotionControlMode getControlMode(){ + return _controlMode; + } + + public FRISessionState getFRISessionState(){ + return _FRISession.getFRIChannelInformation().getFRISessionState(); + } + + /* + * The Following commands change the state of the FRI Manager, and thus are forwarded to the state machine for validation + * */ + + public CommandResult setControlMode(IMotionControlMode controlMode){ + return _currentState.setControlMode(controlMode); + } + + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + return _currentState.setCommandMode(clientCommandMode); + } + + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + return _currentState.setFRIConfig(friConfigurationParams); + } + + public CommandResult startFRI(){ + CommandResult commandResult = _currentState.startFRI(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new FRIActiveState(); + } + return commandResult; + } + + public CommandResult endFRI(){ + CommandResult commandResult = _currentState.endFRI(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new InactiveState(); + } + return commandResult; + } + + public CommandResult activateControl(){ + CommandResult commandResult = _currentState.activateControl(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new ControlActiveState(); + } + return commandResult; + } + + public CommandResult deactivateControl(){ + CommandResult commandResult = _currentState.deactivateControl(); + if(commandResult == CommandResult.EXECUTED){ + _currentState = new FRIActiveState(); + } + return commandResult; + } + + private class State{ + /* By default reject all commands */ + public CommandResult startFRI(){ + return CommandResult.REJECTED; + } + + public CommandResult endFRI(){ + return CommandResult.REJECTED; + } + + public CommandResult activateControl(){ + return CommandResult.REJECTED; + } + + public CommandResult deactivateControl(){ + return CommandResult.REJECTED; + } + + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + return CommandResult.REJECTED; + } + + public CommandResult setControlMode(IMotionControlMode controlMode){ + return CommandResult.REJECTED; + } + + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + return CommandResult.REJECTED; + } + } + + private class InactiveState extends State{ + @Override + public CommandResult startFRI(){ + FRIManager.this._FRISession = new FRISession(FRIManager.this._FRIConfiguration); + try { + FRIManager.this._FRISession.await(10, TimeUnit.SECONDS); + } catch (TimeoutException e) { + FRIManager.this._FRISession.close(); + return CommandResult.ERRORED; + } + + return CommandResult.EXECUTED; + } + @Override + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ + FRIManager.this._FRIConfiguration = friConfigurationParams.toFRIConfiguration(FRIManager.this._lbr); + return CommandResult.EXECUTED; + } + @Override + public CommandResult setControlMode(IMotionControlMode controlMode){ + FRIManager.this._controlMode = controlMode; + return CommandResult.EXECUTED; + } + @Override + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + FRIManager.this._clientCommandMode = clientCommandMode; + return CommandResult.EXECUTED; + } + } + + private class FRIActiveState extends State { + @Override + public CommandResult endFRI(){ + try{ + FRIManager.this._FRISession.close(); + } catch(IllegalStateException e){ + return CommandResult.ERRORED; + } + return CommandResult.EXECUTED; + } + @Override + public CommandResult activateControl(){ + FRIJointOverlay friJointOverlay = + new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode); + PositionHold motion = + new PositionHold(FRIManager.this._controlMode, -1, null); + FRIManager.this._motionContainer = + FRIManager.this._lbr.moveAsync(motion.addMotionOverlay(friJointOverlay)); + return CommandResult.EXECUTED; + } + @Override + public CommandResult setControlMode(IMotionControlMode controlMode){ + FRIManager.this._controlMode = controlMode; + return CommandResult.EXECUTED; + } + @Override + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ + FRIManager.this._clientCommandMode = clientCommandMode; + return CommandResult.EXECUTED; + } + } + + private class ControlActiveState extends State { + @Override + public CommandResult deactivateControl(){ + FRIManager.this._motionContainer.cancel(); + return CommandResult.EXECUTED; + } + } + + private class FRIMotionErrorHandler implements IErrorHandler{ + + @Override + public ErrorHandlingAction handleError(Device device, + IMotionContainer failedContainer, + List canceledContainers) { + FRISessionState sessionState = _FRISession.getFRIChannelInformation().getFRISessionState(); + switch(sessionState){ + case IDLE: + FRIManager.this._ROS2Connection.handleFRIEndedError(); + //FRIManager.this._currentState = new InactiveState(); + break; + default: + FRIManager.this._ROS2Connection.handleControlEndedError(); + //FRIManager.this._currentState = new FRIActiveState(); + break; + } + System.out.println("Failed container: " + failedContainer.toString() + "."); + System.out.println("Error: " + failedContainer.getErrorMessage()); + System.out.println("Runtime data: " + failedContainer.getRuntimeData()); + System.out.println("Cancelled containers: " + canceledContainers.toString()); + return ErrorHandlingAction.Ignore; + } + + } + +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java old mode 100644 new mode 100755 similarity index 96% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/ROS2Connection.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java index be495f8d..c44e569c --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java @@ -1,397 +1,395 @@ -package ros2.modules; - -import java.io.Externalizable; -import java.util.Arrays; - -import ros2.modules.FRIManager; -import ros2.serialization.ControlModeParams; -import ros2.serialization.FRIConfigurationParams; -import ros2.serialization.JointImpedanceControlModeExternalizable; -import ros2.serialization.MessageEncoding; - -import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; - -public class ROS2Connection { - - private TCPConnection _TCPConnection; - private FRIManager _FRIManager; - private boolean _acceptingCommands = false; - private boolean _disconnect = false; - - public void registerTCPConnectionModule(TCPConnection tcpConnectionModule){ - _TCPConnection = tcpConnectionModule; - } - - public void registerFRIManagerModule(FRIManager friManagerModule){ - _FRIManager = friManagerModule; - } - - public void acceptCommands(){ - _acceptingCommands = true; - } - - public void rejectCommands(){ - _acceptingCommands = false; - } - - private enum CommandState{ - ACCEPTED((byte)1), - REJECTED((byte)2), - UNKNOWN((byte)3), - ERROR_CONTROL_ENDED((byte)4), - ERROR_FRI_ENDED((byte)5); - - private final byte value; - - private CommandState(byte value){ - this.value = value; - } - } - - private enum CommandID{ - CONNECT( (byte)1), - DISCONNECT( (byte)2), - START_FRI( (byte)3), - END_FRI( (byte)4), - ACTIVATE_CONTROL( (byte)5), - DEACTIVATE_CONTROL( (byte)6), - GET_FRI_CONFIG( (byte)7), - SET_FRI_CONFIG( (byte)8), - GET_CONTROL_MODE( (byte)9), - SET_CONTROL_MODE( (byte)10), - GET_COMMAND_MODE( (byte)11), - SET_COMMAND_MODE( (byte)12); - - private final byte value; - - private CommandID(byte value){ - this.value = value; - } - - - public static CommandID fromByte(byte value){ - for(CommandID id : CommandID.values()){ - if(value == id.value){ - return id; - } - } - throw new RuntimeException("Byte " + value + " does not represent an InMessageID"); - } - } - - private enum SuccessSignalID { - SUCCESS((byte)1), - NO_SUCCESS((byte)2); - - private final byte value; - - private SuccessSignalID(byte value){ - this.value = value; - } - } - - private enum ControlModeID{ - POSITION( (byte)1), - JOINT_IMPEDANCE((byte)2); - - public final byte value; - - ControlModeID(byte value){ - this.value = value; - } - - public static ControlModeID fromByte(byte value){ - for(ControlModeID id : ControlModeID.values()){ - if(value == id.value){ - return id; - } - } - throw new RuntimeException("Byte " + value + " does not represent a ControlModeID"); - } - } - - public void handleMessageFromROS(byte[] inMessage){ - CommandID command = null; - try{ - ensureArrayLength(inMessage, 1); - command = CommandID.fromByte(inMessage[0]); - if(!_acceptingCommands){ - feedbackCommandRejected(command, new String("Not accepting commands").getBytes()); - return; - } - - } catch(RuntimeException e){ - System.out.println(e.getMessage()); - feedbackCommandUnknown(e.getMessage().getBytes()); - return; - } - - try{ - byte[] inMessageData = Arrays.copyOfRange(inMessage, 1, inMessage.length); - byte[] feedbackData = null; - System.out.println("Command received: " + command.toString()); - - switch(command){ - case CONNECT: - feedbackData = connect(inMessageData); - break; - case DISCONNECT: - feedbackData = disconnect(inMessageData); - break; - case START_FRI: - feedbackData = startFRI(inMessageData); - break; - case END_FRI: - feedbackData = endFRI(inMessageData); - break; - case ACTIVATE_CONTROL: - feedbackData = activateControl(inMessageData); - break; - case DEACTIVATE_CONTROL: - feedbackData = deactivateControl(inMessageData); - break; - case GET_FRI_CONFIG: - feedbackData = getFRIConfig(inMessageData); - break; - case SET_FRI_CONFIG: - feedbackData = setFRIConfig(inMessageData); - break; - case GET_CONTROL_MODE: - feedbackData = getControlMode(inMessageData); - break; - case SET_CONTROL_MODE: - feedbackData = setControlMode(inMessageData); - break; - case GET_COMMAND_MODE: - feedbackData = getCommandMode(inMessageData); - break; - case SET_COMMAND_MODE: - feedbackData = setCommandMode(inMessageData); - break; - } - System.out.println("Command executed."); - feedbackCommandSuccess(command, feedbackData); - /*if(_disconnect == true){ - _TCPConnection.closeConnection(); - }*/ - } catch(RuntimeException e){ - System.out.println(e.getMessage()); - feedbackCommandNoSuccess(command, e.getMessage().getBytes()); - return; - } - } - - public void handleConnectionLost(){ - _FRIManager.deactivateControl(); - _FRIManager.endFRI(); - _FRIManager.close(); - System.out.println("Error: connection lost. FRI ended."); - } - - public void handleControlEndedError(){ - byte[] message = {CommandState.ERROR_CONTROL_ENDED.value}; - System.out.println("Error: control ended"); - _TCPConnection.sendBytes(message); - } - - public void handleFRIEndedError(){ - byte[] message = {CommandState.ERROR_FRI_ENDED.value}; - System.out.println("Error: session ended"); - _TCPConnection.sendBytes(message); - } - - private void feedbackCommandRejected(CommandID command, byte[] feedbackData){ - byte[] message = appendByte(command.value, feedbackData); - message = appendByte(CommandState.REJECTED.value, message); - _TCPConnection.sendBytes(message); - } - - private void feedbackCommandUnknown(byte[] feedbackData){ - byte[] message = appendByte(CommandState.UNKNOWN.value, feedbackData); - _TCPConnection.sendBytes(message); - } - - private void feedbackCommandSuccess(CommandID command, byte[] feedbackData){ - byte[] message = appendByte(SuccessSignalID.SUCCESS.value, feedbackData); - message = appendByte(command.value, message); - message = appendByte(CommandState.ACCEPTED.value, message); - _TCPConnection.sendBytes(message); - } - - private void feedbackCommandNoSuccess(CommandID command, byte[] feedbackData){ - byte[] message = appendByte(SuccessSignalID.NO_SUCCESS.value, feedbackData); - message = appendByte(command.value, message); - message = appendByte(CommandState.ACCEPTED.value, message); - _TCPConnection.sendBytes(message); - } - - private byte[] appendByte(byte id, byte[] data){ - byte[] message = null; - if(data == null){ - message = new byte[]{id}; - } else { - message = new byte[data.length + 1]; - message[0] = id; - System.arraycopy(data, 0, message, 1, data.length); - } - return message; - } - - private byte[] connect(byte[] cmdData){ - return null; - } - - private byte[] disconnect(byte[] cmdData){ - _FRIManager.close(); - _disconnect = true; - //_TCPConnection.closeConnection(); //TODO: close connection after feedback was sent - return null; - } - - private byte[] startFRI(byte[] cmdData){ - FRIManager.CommandResult commandResult = _FRIManager.startFRI(); - if(commandResult == FRIManager.CommandResult.REJECTED){ - throw new RuntimeException("Command rejected"); - } - if(commandResult == FRIManager.CommandResult.ERRORED){ - throw new RuntimeException("Command errored"); - } - return null; - } - - private byte[] endFRI(byte[] cmdData){ - FRIManager.CommandResult commandResult = _FRIManager.endFRI(); - if(commandResult == FRIManager.CommandResult.REJECTED){ - throw new RuntimeException("Command rejected"); - } - if(commandResult == FRIManager.CommandResult.ERRORED){ - throw new RuntimeException("Command errored"); - } - return null; - } - - private byte[] activateControl(byte[] cmdData){ - FRIManager.CommandResult commandResult = _FRIManager.activateControl(); - if(commandResult == FRIManager.CommandResult.REJECTED){ - throw new RuntimeException("Command rejected"); - } - if(commandResult == FRIManager.CommandResult.ERRORED){ - throw new RuntimeException("Command errored"); - } - return null; - } - - private byte[] deactivateControl(byte[] cmdData){ - FRIManager.CommandResult commandResult = _FRIManager.deactivateControl(); - if(commandResult == FRIManager.CommandResult.REJECTED){ - throw new RuntimeException("Command rejected"); - } - if(commandResult == FRIManager.CommandResult.ERRORED){ - throw new RuntimeException("Command errored"); - } - return null; - } - - private byte[] getFRIConfig(byte[] cmdData){ - FRIConfigurationParams friConfigurationParams = _FRIManager.getFRIConfig(); - byte[] feedbackData = MessageEncoding.Encode(friConfigurationParams, FRIConfigurationParams.length); - return feedbackData; - } - - private byte[] setFRIConfig(byte[] cmdData) { - ensureArrayLength(cmdData, FRIConfigurationParams.length + 6); - FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(); - MessageEncoding.Decode(cmdData, friConfigurationParams); - FRIManager.CommandResult commandResult = _FRIManager.setFRIConfig(friConfigurationParams); - if(commandResult == FRIManager.CommandResult.REJECTED){ - throw new RuntimeException("Command rejected"); - } - if(commandResult == FRIManager.CommandResult.ERRORED){ - throw new RuntimeException("Command errored"); - } - return null; - } - - private byte[] getControlMode(byte[] cmdData){ - IMotionControlMode controlMode = _FRIManager.getControlMode(); - ControlModeID controlModeID; - byte[] controlModeData; - if(controlMode instanceof PositionControlMode){ - controlModeID = ControlModeID.POSITION; - controlModeData = null; - } else if (controlMode instanceof JointImpedanceControlMode){ - controlModeID = ControlModeID.JOINT_IMPEDANCE; - controlModeData = MessageEncoding.Encode(new JointImpedanceControlModeExternalizable((JointImpedanceControlMode)controlMode), JointImpedanceControlModeExternalizable.length); - } else { - throw new RuntimeException("Control mode not supported"); - } - byte[] message = appendByte(controlModeID.value, controlModeData); - return message; - } - - private byte[] setControlMode(byte[] cmdData){ - ensureArrayLength(cmdData, 1); - ControlModeID controlModeID = ControlModeID.fromByte(cmdData[0]); - - byte[] controlModeData = Arrays.copyOfRange(cmdData, 1, cmdData.length); - - IMotionControlMode controlMode = null; - switch(controlModeID){ - case POSITION: - controlMode = new PositionControlMode(); - break; - case JOINT_IMPEDANCE: - ensureArrayLength(controlModeData, JointImpedanceControlModeExternalizable.length + 6); - JointImpedanceControlModeExternalizable externalizable = new JointImpedanceControlModeExternalizable(); - System.out.println("Decoding params"); - MessageEncoding.Decode(controlModeData, externalizable); - controlMode = externalizable.toControlMode(); - break; - } - System.out.println("Control mode decoded."); - FRIManager.CommandResult commandResult = _FRIManager.setControlMode(controlMode); - if(commandResult == FRIManager.CommandResult.REJECTED){ - throw new RuntimeException("Command rejected"); - } - if(commandResult == FRIManager.CommandResult.ERRORED){ - throw new RuntimeException("Command errored"); - } - return null; - } - - private byte[] getCommandMode(byte[] cmdData){ - ClientCommandMode clientCommandMode = _FRIManager.getClientCommandMode(); - byte[] commandModeData = new byte[1]; - commandModeData[0] = (byte)clientCommandMode.ordinal();//TODO: check if ordinal == value - return commandModeData; - } - - private byte[] setCommandMode(byte[] cmdData){ - ensureArrayLength(cmdData, 1); - ClientCommandMode clientCommandMode = ClientCommandMode.intToVal((int)cmdData[0]); - if(clientCommandMode == ClientCommandMode.NO_COMMAND_MODE){ - throw new RuntimeException("Byte " + cmdData[0] + " does not represent a ClientCommandMode."); - } - FRIManager.CommandResult commandResult = _FRIManager.setCommandMode(clientCommandMode); - if(commandResult == FRIManager.CommandResult.REJECTED){ - throw new RuntimeException("Command rejected"); - } - if(commandResult == FRIManager.CommandResult.ERRORED){ - throw new RuntimeException("Command errored"); - } - return null; - } - - private void ensureArrayLength(byte[] array, int requiredLength){ - if(array == null){ - throw new RuntimeException("Array is null"); - } - if(array.length < requiredLength){ - throw new RuntimeException("Array does not satisfy length requirement. Required length: " + requiredLength + ", actual length: " + array.length); - } - } - -} +package ros2.modules; + +import java.io.Externalizable; +import java.util.Arrays; + +import ros2.modules.FRIManager; +import ros2.serialization.ControlModeParams; +import ros2.serialization.FRIConfigurationParams; +import ros2.serialization.JointImpedanceControlModeExternalizable; +import ros2.serialization.MessageEncoding; + +import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; + +public class ROS2Connection { + + private TCPConnection _TCPConnection; + private FRIManager _FRIManager; + private boolean _acceptingCommands = false; + private boolean _disconnect = false; + + public void registerTCPConnectionModule(TCPConnection tcpConnectionModule){ + _TCPConnection = tcpConnectionModule; + } + + public void registerFRIManagerModule(FRIManager friManagerModule){ + _FRIManager = friManagerModule; + } + + public void acceptCommands(){ + _acceptingCommands = true; + } + + public void rejectCommands(){ + _acceptingCommands = false; + } + + private enum CommandState{ + ACCEPTED((byte)1), + REJECTED((byte)2), + UNKNOWN((byte)3), + ERROR_CONTROL_ENDED((byte)4), + ERROR_FRI_ENDED((byte)5); + + private final byte value; + + private CommandState(byte value){ + this.value = value; + } + } + + private enum CommandID{ + CONNECT( (byte)1), + DISCONNECT( (byte)2), + START_FRI( (byte)3), + END_FRI( (byte)4), + ACTIVATE_CONTROL( (byte)5), + DEACTIVATE_CONTROL( (byte)6), + GET_FRI_CONFIG( (byte)7), + SET_FRI_CONFIG( (byte)8), + GET_CONTROL_MODE( (byte)9), + SET_CONTROL_MODE( (byte)10), + GET_COMMAND_MODE( (byte)11), + SET_COMMAND_MODE( (byte)12); + + private final byte value; + + private CommandID(byte value){ + this.value = value; + } + + + public static CommandID fromByte(byte value){ + for(CommandID id : CommandID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent an InMessageID"); + } + } + + private enum SuccessSignalID { + SUCCESS((byte)1), + NO_SUCCESS((byte)2); + + private final byte value; + + private SuccessSignalID(byte value){ + this.value = value; + } + } + + private enum ControlModeID{ + POSITION( (byte)1), + JOINT_IMPEDANCE((byte)2); + + public final byte value; + + ControlModeID(byte value){ + this.value = value; + } + + public static ControlModeID fromByte(byte value){ + for(ControlModeID id : ControlModeID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent a ControlModeID"); + } + } + + public void handleMessageFromROS(byte[] inMessage){ + CommandID command = null; + try{ + ensureArrayLength(inMessage, 1); + command = CommandID.fromByte(inMessage[0]); + if(!_acceptingCommands){ + feedbackCommandRejected(command, new String("Not accepting commands").getBytes()); + return; + } + + } catch(RuntimeException e){ + System.out.println(e.getMessage()); + feedbackCommandUnknown(e.getMessage().getBytes()); + return; + } + + try{ + byte[] inMessageData = Arrays.copyOfRange(inMessage, 1, inMessage.length); + byte[] feedbackData = null; + System.out.println("Command received: " + command.toString()); + + switch(command){ + case CONNECT: + feedbackData = connect(inMessageData); + break; + case DISCONNECT: + feedbackData = disconnect(inMessageData); + break; + case START_FRI: + feedbackData = startFRI(inMessageData); + break; + case END_FRI: + feedbackData = endFRI(inMessageData); + break; + case ACTIVATE_CONTROL: + feedbackData = activateControl(inMessageData); + break; + case DEACTIVATE_CONTROL: + feedbackData = deactivateControl(inMessageData); + break; + case GET_FRI_CONFIG: + feedbackData = getFRIConfig(inMessageData); + break; + case SET_FRI_CONFIG: + feedbackData = setFRIConfig(inMessageData); + break; + case GET_CONTROL_MODE: + feedbackData = getControlMode(inMessageData); + break; + case SET_CONTROL_MODE: + feedbackData = setControlMode(inMessageData); + break; + case GET_COMMAND_MODE: + feedbackData = getCommandMode(inMessageData); + break; + case SET_COMMAND_MODE: + feedbackData = setCommandMode(inMessageData); + break; + } + System.out.println("Command executed."); + feedbackCommandSuccess(command, feedbackData); + } catch(RuntimeException e){ + System.out.println(e.getMessage()); + feedbackCommandNoSuccess(command, e.getMessage().getBytes()); + return; + } + } + + public void handleConnectionLost(){ + _FRIManager.deactivateControl(); + _FRIManager.endFRI(); + _FRIManager.close(); + System.out.println("Error: connection lost. FRI ended."); + } + + public void handleControlEndedError(){ + byte[] message = {CommandState.ERROR_CONTROL_ENDED.value}; + System.out.println("Error: control ended"); + _TCPConnection.sendBytes(message); + } + + public void handleFRIEndedError(){ + byte[] message = {CommandState.ERROR_FRI_ENDED.value}; + System.out.println("Error: session ended"); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandRejected(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(command.value, feedbackData); + message = appendByte(CommandState.REJECTED.value, message); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandUnknown(byte[] feedbackData){ + byte[] message = appendByte(CommandState.UNKNOWN.value, feedbackData); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandSuccess(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(SuccessSignalID.SUCCESS.value, feedbackData); + message = appendByte(command.value, message); + message = appendByte(CommandState.ACCEPTED.value, message); + _TCPConnection.sendBytes(message); + } + + private void feedbackCommandNoSuccess(CommandID command, byte[] feedbackData){ + byte[] message = appendByte(SuccessSignalID.NO_SUCCESS.value, feedbackData); + message = appendByte(command.value, message); + message = appendByte(CommandState.ACCEPTED.value, message); + _TCPConnection.sendBytes(message); + } + + private byte[] appendByte(byte id, byte[] data){ + byte[] message = null; + if(data == null){ + message = new byte[]{id}; + } else { + message = new byte[data.length + 1]; + message[0] = id; + System.arraycopy(data, 0, message, 1, data.length); + } + return message; + } + + private byte[] connect(byte[] cmdData){ + return null; + } + + private byte[] disconnect(byte[] cmdData){ + _FRIManager.close(); + _disconnect = true; + //_TCPConnection.closeConnection(); //TODO: close connection after feedback was sent + return null; + } + + private byte[] startFRI(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.startFRI(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] endFRI(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.endFRI(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] activateControl(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.activateControl(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] deactivateControl(byte[] cmdData){ + FRIManager.CommandResult commandResult = _FRIManager.deactivateControl(); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getFRIConfig(byte[] cmdData){ + FRIConfigurationParams friConfigurationParams = _FRIManager.getFRIConfig(); + byte[] feedbackData = MessageEncoding.Encode(friConfigurationParams, FRIConfigurationParams.length); + return feedbackData; + } + + private byte[] setFRIConfig(byte[] cmdData) { + ensureArrayLength(cmdData, FRIConfigurationParams.length + 6); + + FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(); + MessageEncoding.Decode(cmdData, friConfigurationParams); + FRIManager.CommandResult commandResult = _FRIManager.setFRIConfig(friConfigurationParams); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getControlMode(byte[] cmdData){ + IMotionControlMode controlMode = _FRIManager.getControlMode(); + ControlModeID controlModeID; + byte[] controlModeData; + if(controlMode instanceof PositionControlMode){ + controlModeID = ControlModeID.POSITION; + controlModeData = null; + } else if (controlMode instanceof JointImpedanceControlMode){ + controlModeID = ControlModeID.JOINT_IMPEDANCE; + controlModeData = MessageEncoding.Encode(new JointImpedanceControlModeExternalizable((JointImpedanceControlMode)controlMode), JointImpedanceControlModeExternalizable.length); + } else { + throw new RuntimeException("Control mode not supported"); + } + byte[] message = appendByte(controlModeID.value, controlModeData); + return message; + } + + private byte[] setControlMode(byte[] cmdData){ + ensureArrayLength(cmdData, 1); + ControlModeID controlModeID = ControlModeID.fromByte(cmdData[0]); + + byte[] controlModeData = Arrays.copyOfRange(cmdData, 1, cmdData.length); + + IMotionControlMode controlMode = null; + switch(controlModeID){ + case POSITION: + controlMode = new PositionControlMode(); + break; + case JOINT_IMPEDANCE: + ensureArrayLength(controlModeData, JointImpedanceControlModeExternalizable.length + 6); + JointImpedanceControlModeExternalizable externalizable = new JointImpedanceControlModeExternalizable(); + System.out.println("Decoding params"); + MessageEncoding.Decode(controlModeData, externalizable); + controlMode = externalizable.toControlMode(); + break; + } + System.out.println("Control mode decoded."); + FRIManager.CommandResult commandResult = _FRIManager.setControlMode(controlMode); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private byte[] getCommandMode(byte[] cmdData){ + ClientCommandMode clientCommandMode = _FRIManager.getClientCommandMode(); + byte[] commandModeData = new byte[1]; + commandModeData[0] = (byte)clientCommandMode.ordinal();//TODO: check if ordinal == value + return commandModeData; + } + + private byte[] setCommandMode(byte[] cmdData){ + ensureArrayLength(cmdData, 1); + ClientCommandMode clientCommandMode = ClientCommandMode.intToVal((int)cmdData[0]); + if(clientCommandMode == ClientCommandMode.NO_COMMAND_MODE){ + throw new RuntimeException("Byte " + cmdData[0] + " does not represent a ClientCommandMode."); + } + FRIManager.CommandResult commandResult = _FRIManager.setCommandMode(clientCommandMode); + if(commandResult == FRIManager.CommandResult.REJECTED){ + throw new RuntimeException("Command rejected"); + } + if(commandResult == FRIManager.CommandResult.ERRORED){ + throw new RuntimeException("Command errored"); + } + return null; + } + + private void ensureArrayLength(byte[] array, int requiredLength){ + if(array == null){ + throw new RuntimeException("Array is null"); + } + if(array.length < requiredLength){ + throw new RuntimeException("Array does not satisfy length requirement. Required length: " + requiredLength + ", actual length: " + array.length); + } + } + +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/TCPConnection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java old mode 100644 new mode 100755 similarity index 95% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/TCPConnection.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java index 58ec404e..f39018f5 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/modules/TCPConnection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java @@ -1,187 +1,186 @@ -package ros2.modules; - -import java.net.*; -import java.util.Arrays; -import java.io.*; - -import javax.xml.bind.DatatypeConverter; -//import org.apache.xerces.jaxp.datatype.DatatypeFactoryImpl; - -public class TCPConnection{ - private int _tcpServerPort; - private ServerSocket _tcpServer; - private Socket _tcpClient; - private Thread _tcpConnectionThread; - private boolean _closeRequested; - private byte[] _incomingData; - private ROS2Connection _ROS2Connection; - - public TCPConnection(int tcpServerPort){ - _tcpServerPort = tcpServerPort; - _tcpServer = null; - _tcpClient = null; - _tcpConnectionThread = new Thread(new ConnectionHandler()); - _closeRequested = false; - _incomingData = null; - } - - public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ - _ROS2Connection = ros2ConnectionModule; - } - - public void waitUntilDisconnected(){ - try { - _tcpConnectionThread.join(); - } catch (InterruptedException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - - private class ConnectionHandler implements Runnable{ - public void run(){ - try{ - while(true){ - waitForConnection(); - if(_closeRequested) { - System.out.println("Stopped waiting for connection."); - break; - } - handleIncomingData(); - if(_closeRequested) { - System.out.println("TCP Read interrupted."); - break; - } - } - }catch (IOException e){ - e.printStackTrace(); - }catch(Exception e){ - e.printStackTrace(); - } - - if(_tcpServer.isClosed() == false){ - try{ - _tcpServer.close(); - }catch(IOException e){ - e.printStackTrace(); - } - } - _closeRequested = false; - } - } - - public void openConnection() { - try { - _tcpServer = new ServerSocket(_tcpServerPort); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - _tcpConnectionThread.start(); - } - - public void sendString(String s){ - if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ - try{ - DataOutputStream outToClient = new DataOutputStream(_tcpClient.getOutputStream()); - outToClient.writeBytes(s); - outToClient.close(); - } - catch(IOException e){ - e.printStackTrace(); - } - } else{ - System.out.println("TCP client not connected."); - } - } - - public void sendBytes(byte[] message){ - if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ - try{ - DataOutputStream outToClient = new DataOutputStream(_tcpClient.getOutputStream()); - outToClient.write(message); - //outToClient.close(); - }catch(IOException e){ - e.printStackTrace(); - } - } else{ - System.out.println("TCP client not connected."); - } - } - - public byte[] getReceivedData(){ - byte[] dataCopy = _incomingData; - _incomingData = null; - return dataCopy; - } - - public void closeConnection(){ - _closeRequested = true; - try{ - if(_tcpServer.isClosed() == false){ - _tcpServer.close(); - } - if(_tcpClient.isClosed() == false){ - _tcpClient.close(); - } - } - catch(IOException e) - { - e.printStackTrace(); - } - } - - private void waitForConnection() throws Exception { - System.out.println("Waiting for connection..."); - try { - _tcpClient = _tcpServer.accept(); - } catch (SocketException e) { - if(_closeRequested){ - return; - } else { - // closing of connection wasn't requested, error - throw e; - } - - } - System.out.println("Connection established."); - } - - private void handleIncomingData() throws IOException{ - BufferedReader inFromClient = new BufferedReader(new InputStreamReader(_tcpClient.getInputStream())); - while(_tcpClient.isClosed() == false){ - int dataLength = -2; - char[] charArray = new char[1024]; - try{ - dataLength = inFromClient.read(charArray); - } catch (SocketException e) { - if(_closeRequested){ - break; - } else { - // closing of connection wasn't requested, error - throw e; - } - } - if(dataLength < 0){ - _ROS2Connection.handleConnectionLost(); - break; - } - - byte[] byteArray = Arrays.copyOf(new String(charArray).getBytes(), dataLength); - String byteHexString = DatatypeConverter.printHexBinary(byteArray); - - if(_incomingData != null){ - System.out.println("ERROR: Previous data not yet processed! Skipping data: " + byteHexString); - } - else{ - _incomingData = byteArray; - //System.out.println("New data received: " + byteHexString + ", " + _incomingData); - _ROS2Connection.handleMessageFromROS(_incomingData); - _incomingData = null; - } - } - } - - - -} +package ros2.modules; + +import java.net.*; +import java.util.Arrays; +import java.io.*; + +import javax.xml.bind.DatatypeConverter; + +public class TCPConnection{ + private int _tcpServerPort; + private ServerSocket _tcpServer; + private Socket _tcpClient; + private Thread _tcpConnectionThread; + private boolean _closeRequested; + private byte[] _incomingData; + private ROS2Connection _ROS2Connection; + + public TCPConnection(int tcpServerPort){ + _tcpServerPort = tcpServerPort; + _tcpServer = null; + _tcpClient = null; + _tcpConnectionThread = new Thread(new ConnectionHandler()); + _closeRequested = false; + _incomingData = null; + } + + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ + _ROS2Connection = ros2ConnectionModule; + } + + public void waitUntilDisconnected(){ + try { + _tcpConnectionThread.join(); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + private class ConnectionHandler implements Runnable{ + public void run(){ + try{ + while(true){ + waitForConnection(); + if(_closeRequested) { + System.out.println("Stopped waiting for connection."); + break; + } + handleIncomingData(); + if(_closeRequested) { + System.out.println("TCP Read interrupted."); + break; + } + } + }catch (IOException e){ + e.printStackTrace(); + }catch(Exception e){ + e.printStackTrace(); + } + + if(_tcpServer.isClosed() == false){ + try{ + _tcpServer.close(); + }catch(IOException e){ + e.printStackTrace(); + } + } + _closeRequested = false; + } + } + + public void openConnection() { + try { + _tcpServer = new ServerSocket(_tcpServerPort); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + _tcpConnectionThread.start(); + } + + public void sendString(String s){ + if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ + try{ + DataOutputStream outToClient = new DataOutputStream(_tcpClient.getOutputStream()); + outToClient.writeBytes(s); + outToClient.close(); + } + catch(IOException e){ + e.printStackTrace(); + } + } else{ + System.out.println("TCP client not connected."); + } + } + + public void sendBytes(byte[] message){ + if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ + try{ + DataOutputStream outToClient = new DataOutputStream(_tcpClient.getOutputStream()); + outToClient.write(message); + //outToClient.close(); + }catch(IOException e){ + e.printStackTrace(); + } + } else{ + System.out.println("TCP client not connected."); + } + } + + public byte[] getReceivedData(){ + byte[] dataCopy = _incomingData; + _incomingData = null; + return dataCopy; + } + + public void closeConnection(){ + _closeRequested = true; + try{ + if(_tcpServer.isClosed() == false){ + _tcpServer.close(); + } + if(_tcpClient.isClosed() == false){ + _tcpClient.close(); + } + } + catch(IOException e) + { + e.printStackTrace(); + } + } + + private void waitForConnection() throws Exception { + System.out.println("Waiting for connection..."); + try { + _tcpClient = _tcpServer.accept(); + } catch (SocketException e) { + if(_closeRequested){ + return; + } else { + // closing of connection wasn't requested, error + throw e; + } + + } + System.out.println("Connection established."); + } + + private void handleIncomingData() throws IOException{ + BufferedReader inFromClient = new BufferedReader(new InputStreamReader(_tcpClient.getInputStream())); + while(_tcpClient.isClosed() == false){ + int dataLength = -2; + char[] charArray = new char[1024]; + try{ + dataLength = inFromClient.read(charArray); + } catch (SocketException e) { + if(_closeRequested){ + break; + } else { + // closing of connection wasn't requested, error + throw e; + } + } + if(dataLength < 0){ + _ROS2Connection.handleConnectionLost(); + break; + } + + byte[] byteArray = Arrays.copyOf(new String(charArray).getBytes(), dataLength); + String byteHexString = DatatypeConverter.printHexBinary(byteArray); + + if(_incomingData != null){ + System.out.println("ERROR: Previous data not yet processed! Skipping data: " + byteHexString); + } + else{ + _incomingData = byteArray; + //System.out.println("New data received: " + byteHexString + ", " + _incomingData); + _ROS2Connection.handleMessageFromROS(_incomingData); + _incomingData = null; + } + } + } + + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java old mode 100644 new mode 100755 similarity index 96% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/ControlModeParams.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java index e308c9c0..4872d1ca --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java @@ -1,97 +1,97 @@ -package ros2.serialization; - -import java.io.Externalizable; -import java.io.IOException; -import java.io.ObjectInput; -import java.io.ObjectOutput; -import java.util.Arrays; - - -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; - -public abstract class ControlModeParams implements Externalizable{ - public static int length = 0; - - private enum ControlModeID{ - POSITION( (byte)1), - JOINT_IMPEDANCE((byte)2); - - public final byte value; - - ControlModeID(byte value){ - this.value = value; - } - - public static ControlModeID fromByte(byte value){ - for(ControlModeID id : ControlModeID.values()){ - if(value == id.value){ - return id; - } - } - throw new RuntimeException("Byte " + value + " does not represent an ControlModeID"); - } - } - - public static ControlModeParams fromSerialData(byte[] serialData){ - if(serialData.length == 0){ - throw new RuntimeException("serialData is empty"); - } - ControlModeID controlModeID = ControlModeID.fromByte(serialData[0]); - ControlModeParams controlModeParams = null; - switch(controlModeID){ - case POSITION: - controlModeParams = new PositionControlModeParams(); - break; - case JOINT_IMPEDANCE: - controlModeParams = new JointImpedanceControlModeParams(); - break; - } - serialData = Arrays.copyOfRange(serialData, 1, serialData.length); - MessageEncoding.Decode(serialData, controlModeParams); - return controlModeParams; - } - - public static ControlModeParams fromMotionControlMode(IMotionControlMode controlMode){ - if(controlMode == null){ - throw new RuntimeException("ControlMode is null"); - } - ControlModeParams controlModeParams; - if(controlMode instanceof PositionControlMode){ - controlModeParams = new PositionControlModeParams(); - } else if (controlMode instanceof JointImpedanceControlMode){ - controlModeParams = new JointImpedanceControlModeParams((JointImpedanceControlMode) controlMode); - } else { - throw new RuntimeException("Control mode not supported"); - } - return controlModeParams; - } - - @Override - public void writeExternal(ObjectOutput out) throws IOException { - - - } - - @Override - public void readExternal(ObjectInput in) throws IOException, - ClassNotFoundException { - - } - -} - -class PositionControlModeParams extends ControlModeParams{ - - -} - -class JointImpedanceControlModeParams extends ControlModeParams{ - public JointImpedanceControlModeParams(){ - - } - public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ - - } -} +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; +import java.util.Arrays; + + +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode; + +public abstract class ControlModeParams implements Externalizable{ + public static int length = 0; + + private enum ControlModeID{ + POSITION( (byte)1), + JOINT_IMPEDANCE((byte)2); + + public final byte value; + + ControlModeID(byte value){ + this.value = value; + } + + public static ControlModeID fromByte(byte value){ + for(ControlModeID id : ControlModeID.values()){ + if(value == id.value){ + return id; + } + } + throw new RuntimeException("Byte " + value + " does not represent an ControlModeID"); + } + } + + public static ControlModeParams fromSerialData(byte[] serialData){ + if(serialData.length == 0){ + throw new RuntimeException("serialData is empty"); + } + ControlModeID controlModeID = ControlModeID.fromByte(serialData[0]); + ControlModeParams controlModeParams = null; + switch(controlModeID){ + case POSITION: + controlModeParams = new PositionControlModeParams(); + break; + case JOINT_IMPEDANCE: + controlModeParams = new JointImpedanceControlModeParams(); + break; + } + serialData = Arrays.copyOfRange(serialData, 1, serialData.length); + MessageEncoding.Decode(serialData, controlModeParams); + return controlModeParams; + } + + public static ControlModeParams fromMotionControlMode(IMotionControlMode controlMode){ + if(controlMode == null){ + throw new RuntimeException("ControlMode is null"); + } + ControlModeParams controlModeParams; + if(controlMode instanceof PositionControlMode){ + controlModeParams = new PositionControlModeParams(); + } else if (controlMode instanceof JointImpedanceControlMode){ + controlModeParams = new JointImpedanceControlModeParams((JointImpedanceControlMode) controlMode); + } else { + throw new RuntimeException("Control mode not supported"); + } + return controlModeParams; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + + + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + + } + +} + +class PositionControlModeParams extends ControlModeParams{ + + +} + +class JointImpedanceControlModeParams extends ControlModeParams{ + public JointImpedanceControlModeParams(){ + + } + public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ + + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java old mode 100644 new mode 100755 similarity index 72% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java index 683a6bce..6e2bb022 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/FRIConfigurationParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java @@ -1,57 +1,63 @@ -package ros2.serialization; - -import java.io.Externalizable; -import java.io.IOException; -import java.io.ObjectInput; -import java.io.ObjectOutput; -import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; -import com.kuka.roboticsAPI.deviceModel.Device; - -public class FRIConfigurationParams implements Externalizable { - - public static final int length = 12; //remoteIP not included - - - private String _remoteIP = "192.168.37.70"; - private int _remotePort; - private int _sendPeriodMilliSec; - private int _receiveMultiplier; - - @Override - public void writeExternal(ObjectOutput out) throws IOException { - //out.writeBytes(_remoteIP); - out.writeInt(_remotePort); - out.writeInt(_sendPeriodMilliSec); - out.writeInt(_receiveMultiplier); - } - - @Override - public void readExternal(ObjectInput in) throws IOException, - ClassNotFoundException { - //_remoteIP = in.readUTF();//TODO: check for validity! eventually transfer as ints - _remotePort = in.readInt(); - _sendPeriodMilliSec = in.readInt(); - _receiveMultiplier = in.readInt(); - } - - public FRIConfigurationParams() { - _remotePort = 30200; - _sendPeriodMilliSec = 10; - _receiveMultiplier = 1; - } - - public FRIConfigurationParams(FRIConfiguration friConfiguration){ - //_remoteIP = friConfiguration.getHostName(); - _remotePort = friConfiguration.getPortOnRemote(); - _sendPeriodMilliSec = friConfiguration.getSendPeriodMilliSec(); - _receiveMultiplier = friConfiguration.getReceiveMultiplier(); - } - - public FRIConfiguration toFRIConfiguration(Device device){ - FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(device, _remoteIP); - friConfiguration.setPortOnRemote(_remotePort); - friConfiguration.setSendPeriodMilliSec(_sendPeriodMilliSec); - friConfiguration.setReceiveMultiplier(_receiveMultiplier); - return friConfiguration; - } -} +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; +import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; +import com.kuka.roboticsAPI.deviceModel.Device; + +public class FRIConfigurationParams implements Externalizable { + + public static final int length = 16; // 4 integers + + + private String _remoteIP; + private int _remotePort; + private int _sendPeriodMilliSec; + private int _receiveMultiplier; + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + out.writeBytes(_remoteIP); + out.writeInt(_remotePort); + out.writeInt(_sendPeriodMilliSec); + out.writeInt(_receiveMultiplier); + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + _remotePort = in.readInt(); + _sendPeriodMilliSec = in.readInt(); + _receiveMultiplier = in.readInt(); + _remoteIP = "192.168.38.6"; + + int ip = in.readInt(); + _remoteIP = String.format("%d.%d.%d.%d", (ip & 0xff),(ip >> 8 & 0xff), (ip >> 16 & 0xff), (ip >> 24 & 0xff)); + + System.out.println("FRI configuration: client IP: " + _remoteIP + ":" + _remotePort + ", send_period [ms]: " + _sendPeriodMilliSec + ", receive multiplier: " + _receiveMultiplier); + } + + public FRIConfigurationParams() { + _remoteIP = "0.0.0.0"; + _remotePort = 30200; + _sendPeriodMilliSec = 10; + _receiveMultiplier = 1; + } + + public FRIConfigurationParams(FRIConfiguration friConfiguration){ + _remoteIP = friConfiguration.getHostName(); + _remotePort = friConfiguration.getPortOnRemote(); + _sendPeriodMilliSec = friConfiguration.getSendPeriodMilliSec(); + _receiveMultiplier = friConfiguration.getReceiveMultiplier(); + } + + public FRIConfiguration toFRIConfiguration(Device device){ + FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(device, _remoteIP); + friConfiguration.setPortOnRemote(_remotePort); + friConfiguration.setSendPeriodMilliSec(_sendPeriodMilliSec); + friConfiguration.setReceiveMultiplier(_receiveMultiplier); + return friConfiguration; + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/JointImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java old mode 100644 new mode 100755 similarity index 96% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/JointImpedanceControlModeExternalizable.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java index bd933b6c..618205a2 --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/JointImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java @@ -1,59 +1,59 @@ -package ros2.serialization; - -import java.io.Externalizable; -import java.io.IOException; -import java.io.ObjectInput; -import java.io.ObjectOutput; - -import ros2.tools.Conversions; - -import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; -import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; - -public class JointImpedanceControlModeExternalizable extends JointImpedanceControlMode implements Externalizable{ - - public final static int length = 112; - - public JointImpedanceControlModeExternalizable(){ - super(1000, 1000, 1000, 1000, 1000, 1000, 1000); - } - - public JointImpedanceControlModeExternalizable(JointImpedanceControlMode other){ - super(other); - } - - public IMotionControlMode toControlMode(){ - JointImpedanceControlMode controlMode = new JointImpedanceControlMode((JointImpedanceControlMode)this); - return (IMotionControlMode)controlMode; - } - - @Override - public void writeExternal(ObjectOutput out) throws IOException { - for(double jointStiffness : getStiffness()){ - out.writeDouble(jointStiffness); - } - for(double jointDamping : getDamping()){ - out.writeDouble(jointDamping); - } - } - - @Override - public void readExternal(ObjectInput in) throws IOException, - ClassNotFoundException { - double[] jointStiffness = new double[getStiffness().length]; - for(int i = 0; i < getStiffness().length; i++){ - jointStiffness[i] = in.readDouble(); - } - setStiffness(jointStiffness); - - double[] jointDamping = new double[getDamping().length]; - for(int i = 0; i < getDamping().length; i++){ - jointDamping[i] = in.readDouble(); - } - setDamping(jointDamping); - - } - - - -} +package ros2.serialization; + +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectInput; +import java.io.ObjectOutput; + +import ros2.tools.Conversions; + +import com.kuka.roboticsAPI.motionModel.controlModeModel.IMotionControlMode; +import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; + +public class JointImpedanceControlModeExternalizable extends JointImpedanceControlMode implements Externalizable{ + + public final static int length = 112; + + public JointImpedanceControlModeExternalizable(){ + super(1000, 1000, 1000, 1000, 1000, 1000, 1000); + } + + public JointImpedanceControlModeExternalizable(JointImpedanceControlMode other){ + super(other); + } + + public IMotionControlMode toControlMode(){ + JointImpedanceControlMode controlMode = new JointImpedanceControlMode((JointImpedanceControlMode)this); + return (IMotionControlMode)controlMode; + } + + @Override + public void writeExternal(ObjectOutput out) throws IOException { + for(double jointStiffness : getStiffness()){ + out.writeDouble(jointStiffness); + } + for(double jointDamping : getDamping()){ + out.writeDouble(jointDamping); + } + } + + @Override + public void readExternal(ObjectInput in) throws IOException, + ClassNotFoundException { + double[] jointStiffness = new double[getStiffness().length]; + for(int i = 0; i < getStiffness().length; i++){ + jointStiffness[i] = in.readDouble(); + } + setStiffness(jointStiffness); + + double[] jointDamping = new double[getDamping().length]; + for(int i = 0; i < getDamping().length; i++){ + jointDamping[i] = in.readDouble(); + } + setDamping(jointDamping); + + } + + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/MessageEncoding.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java old mode 100644 new mode 100755 similarity index 97% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/MessageEncoding.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java index 13d77813..7083509d --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/serialization/MessageEncoding.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java @@ -1,39 +1,39 @@ -package ros2.serialization; - -import java.io.ByteArrayInputStream; -import java.io.ByteArrayOutputStream; -import java.io.Externalizable; -import java.io.IOException; -import java.io.ObjectOutputStream; -import java.io.ObjectInputStream; - -public class MessageEncoding { - public static byte[] Encode(Externalizable objectIn, int serialLength){ - ByteArrayOutputStream serialDataStream = new ByteArrayOutputStream(); - byte[] serialDataOut = new byte[serialLength + 4]; - try{ - ObjectOutputStream objectStream = new ObjectOutputStream(serialDataStream); - objectIn.writeExternal(objectStream); - objectStream.flush(); - objectStream.close(); - serialDataOut = serialDataStream.toByteArray(); - }catch(IOException e){ - e.printStackTrace(); - } - return serialDataOut; - } - - public static void Decode(byte[] serialDataIn, Externalizable objectOut) throws RuntimeException{ - try{ - ByteArrayInputStream serialDataStream = new ByteArrayInputStream(serialDataIn); - ObjectInputStream objectStream = new ObjectInputStream(serialDataStream); - objectOut.readExternal(objectStream); - objectStream.close(); - } catch(IOException e){ - e.printStackTrace(); - throw new RuntimeException("IO Exception occurred"); - } catch (ClassNotFoundException e) { - throw new RuntimeException("Message could not be decoded"); - } - } -} +package ros2.serialization; + +import java.io.ByteArrayInputStream; +import java.io.ByteArrayOutputStream; +import java.io.Externalizable; +import java.io.IOException; +import java.io.ObjectOutputStream; +import java.io.ObjectInputStream; + +public class MessageEncoding { + public static byte[] Encode(Externalizable objectIn, int serialLength){ + ByteArrayOutputStream serialDataStream = new ByteArrayOutputStream(); + byte[] serialDataOut = new byte[serialLength + 4]; + try{ + ObjectOutputStream objectStream = new ObjectOutputStream(serialDataStream); + objectIn.writeExternal(objectStream); + objectStream.flush(); + objectStream.close(); + serialDataOut = serialDataStream.toByteArray(); + }catch(IOException e){ + e.printStackTrace(); + } + return serialDataOut; + } + + public static void Decode(byte[] serialDataIn, Externalizable objectOut) throws RuntimeException{ + try{ + ByteArrayInputStream serialDataStream = new ByteArrayInputStream(serialDataIn); + ObjectInputStream objectStream = new ObjectInputStream(serialDataStream); + objectOut.readExternal(objectStream); + objectStream.close(); + } catch(IOException e){ + e.printStackTrace(); + throw new RuntimeException("IO Exception occurred"); + } catch (ClassNotFoundException e) { + throw new RuntimeException("Message could not be decoded"); + } + } +} diff --git a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/tools/Conversions.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/tools/Conversions.java old mode 100644 new mode 100755 similarity index 96% rename from kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/tools/Conversions.java rename to kuka_sunrise_fri_driver/robot_application/src/ros2/tools/Conversions.java index 0d31689b..2635dd1b --- a/kuka_sunrise_fri_driver/robot_application/ROS2_Control/src/ros2/tools/Conversions.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/tools/Conversions.java @@ -1,13 +1,13 @@ -package ros2.tools; - -public class Conversions { - public static class Arrays{ - public static float[] DoubleToFloat(double[] doubleArray){ - float[] floatArray = new float[doubleArray.length]; - for(int i = 0; i < doubleArray.length; i++){ - floatArray[i] = (float)doubleArray[i]; - } - return floatArray; - } - } -} +package ros2.tools; + +public class Conversions { + public static class Arrays{ + public static float[] DoubleToFloat(double[] doubleArray){ + float[] floatArray = new float[doubleArray.length]; + for(int i = 0; i < doubleArray.length; i++){ + floatArray[i] = (float)doubleArray[i]; + } + return floatArray; + } + } +} diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp deleted file mode 100644 index 99d8b929..00000000 --- a/kuka_sunrise_fri_driver/src/connection_helpers/configuration_manager.cpp +++ /dev/null @@ -1,384 +0,0 @@ -// Copyright 2020 Zoltán Rési -// -// 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. - -#include -#include -#include - -#include "communication_helpers/service_tools.hpp" -#include "kuka_sunrise_fri_driver/configuration_manager.hpp" - -namespace kuka_sunrise_fri_driver -{ -ConfigurationManager::ConfigurationManager( - std::shared_ptr robot_manager_node, - std::shared_ptr fri_connection) -: robot_manager_node_(robot_manager_node), fri_connection_(fri_connection) -{ - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); - qos.reliable(); - cbg_ = robot_manager_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - param_cbg_ = - robot_manager_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - receive_multiplier_client_ = - robot_manager_node->create_client( - "fri_configuration_controller/set_receive_multiplier", qos.get_rmw_qos_profile(), cbg_); - - robot_manager_node_->registerParameter( - "controller_ip", "", - kuka_drivers_core::ParameterSetAccessRights{false, false, false, false, true}, - [this](const std::string & controller_ip) - { return this->onControllerIpChangeRequest(controller_ip); }); - - set_parameter_service_ = robot_manager_node->create_service( - "configuration_manager/set_params", - [this]( - std_srvs::srv::Trigger::Request::SharedPtr, - std_srvs::srv::Trigger::Response::SharedPtr response) { this->setParameters(response); }, - ::rmw_qos_profile_default, param_cbg_); - - get_controllers_client_ = - robot_manager_node->create_client( - "controller_manager/list_controllers", qos.get_rmw_qos_profile(), cbg_); -} - -bool ConfigurationManager::onCommandModeChangeRequest(const std::string & command_mode) const -{ - if (command_mode == POSITION_COMMAND) - { - if (!position_controller_available_ || !setCommandMode(POSITION_COMMAND)) - { - return false; - } - } - else if (command_mode == TORQUE_COMMAND) - { - if (robot_manager_node_->get_parameter("control_mode").as_string() != IMPEDANCE_CONTROL) - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), - "Unable to set torque command mode, if control mode is not 'joint impedance'"); - return false; - } - if (robot_manager_node_->get_parameter("send_period_ms").as_int() > 5) - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), - "Unable to set torque command mode, if send period is bigger than 5 [ms]"); - return false; - } - if (!torque_controller_available_ || !setCommandMode(TORQUE_COMMAND)) - { - return false; - } - } - else - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), "Command mode should be '%s' or '%s'", - POSITION_COMMAND.c_str(), TORQUE_COMMAND.c_str()); - return false; - } - RCLCPP_INFO(robot_manager_node_->get_logger(), "Successfully set command mode"); - return true; -} - -bool ConfigurationManager::onControlModeChangeRequest(const std::string & control_mode) const -{ - if (control_mode == POSITION_CONTROL) - { - return fri_connection_->setPositionControlMode(); - } - else if (control_mode == IMPEDANCE_CONTROL) - { - try - { - return fri_connection_->setJointImpedanceControlMode(joint_stiffness_, joint_damping_); - } - catch (const std::exception & e) - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), e.what()); - } - return false; - } - else - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), "Control mode should be '%s' or '%s'", - POSITION_CONTROL.c_str(), IMPEDANCE_CONTROL.c_str()); - return false; - } -} - -bool ConfigurationManager::onJointStiffnessChangeRequest( - const std::vector & joint_stiffness) -{ - if (joint_stiffness.size() != 7) - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), - "Invalid parameter array length for parameter joint stiffness"); - return false; - } - for (double js : joint_stiffness) - { - if (js < 0) - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Joint stiffness values must be >=0"); - return false; - } - } - joint_stiffness_ = joint_stiffness; - return true; -} - -bool ConfigurationManager::onJointDampingChangeRequest(const std::vector & joint_damping) -{ - if (joint_damping.size() != 7) - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), - "Invalid parameter array length for parameter joint damping"); - return false; - } - for (double jd : joint_damping) - { - if (jd < 0 || jd > 1) - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Joint damping values must be >=0 && <=1"); - return false; - } - } - joint_damping_ = joint_damping; - return true; -} - -bool ConfigurationManager::onSendPeriodChangeRequest(const int & send_period) const -{ - if (send_period < 1 || send_period > 100) - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), "Send period milliseconds must be >=1 && <=100"); - return false; - } - return true; -} - -bool ConfigurationManager::onReceiveMultiplierChangeRequest(const int & receive_multiplier) const -{ - if (receive_multiplier < 1) - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Receive multiplier must be >=1"); - return false; - } - if (!setReceiveMultiplier(receive_multiplier)) - { - return false; - } - return true; -} - -bool ConfigurationManager::onControllerIpChangeRequest(const std::string & controller_ip) const -{ - // Check IP validity - size_t i = 0; - std::vector split_ip; - auto pos = controller_ip.find('.'); - while (pos != std::string::npos) - { - split_ip.push_back(controller_ip.substr(i, pos - i)); - i = ++pos; - pos = controller_ip.find('.', pos); - } - split_ip.push_back(controller_ip.substr(i, controller_ip.length())); - - if (split_ip.size() != 4) - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Valid IP must have 3 '.' delimiters"); - return false; - } - - for (const auto & ip : split_ip) - { - if ( - ip.empty() || (ip.find_first_not_of("[0123456789]") != std::string::npos) || stoi(ip) > 255 || - stoi(ip) < 0) - { - RCLCPP_ERROR( - robot_manager_node_->get_logger(), "Valid IP must contain only numbers between 0 and 255"); - return false; - } - } - return true; -} - -bool ConfigurationManager::onControllerNameChangeRequest( - const std::string & controller_name, bool position) -{ - auto request = std::make_shared(); - auto response = - kuka_drivers_core::sendRequest( - get_controllers_client_, request, 0, 1000); - - if (!response) - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Could not get controller names"); - return false; - } - - if (controller_name == "") - { - RCLCPP_WARN( - robot_manager_node_->get_logger(), "Controller for %s command mode not available", - position ? POSITION_COMMAND.c_str() : TORQUE_COMMAND.c_str()); - if (position) - { - position_controller_available_ = false; - } - else - { - torque_controller_available_ = false; - } - return true; - } - - for (const auto & controller : response->controller) - { - if (controller_name == controller.name) - { - if (position) - { - position_controller_available_ = true; - } - else - { - torque_controller_available_ = true; - } - return true; - } - } - RCLCPP_ERROR( - robot_manager_node_->get_logger(), "Controller name '%s' not available", - controller_name.c_str()); - return false; -} - -bool ConfigurationManager::setCommandMode(const std::string & command_mode) const -{ - ClientCommandModeID client_command_mode; - if (command_mode == POSITION_COMMAND) - { - client_command_mode = POSITION_COMMAND_MODE; - } - else if (command_mode == TORQUE_COMMAND) - { - client_command_mode = TORQUE_COMMAND_MODE; - } - else - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Invalid control mode"); - return false; - } - if (fri_connection_) - { - fri_connection_->setClientCommandMode(client_command_mode); - } - else - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Robot Manager not available"); - return false; - } - return true; -} - -bool ConfigurationManager::setReceiveMultiplier(int receive_multiplier) const -{ - // Set receive multiplier of hardware interface through controller manager service - auto request = std::make_shared(); - request->data = receive_multiplier; - auto response = kuka_drivers_core::sendRequest( - receive_multiplier_client_, request, 0, 1000); - - if (!response || !response->success) - { - RCLCPP_ERROR(robot_manager_node_->get_logger(), "Could not set receive_multiplier"); - return false; - } - return true; -} - -void ConfigurationManager::setParameters(std_srvs::srv::Trigger::Response::SharedPtr response) -{ - if (configured_) - { - RCLCPP_WARN(robot_manager_node_->get_logger(), "Parameters already registered"); - response->success = true; - return; - } - // The parameter callbacks are called on this thread - // Response is sent only after all parameters are declared (or error occurs) - // Parameter exceptions are intentionally not caught, because in case of an invalid - // parameter type (or value), the nodes must be launched again with changed parameters - // because they could not be declared, therefore change is not possible in runtime - robot_manager_node_->registerParameter( - "send_period_ms", 10, - kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, - [this](const int & send_period) { return this->onSendPeriodChangeRequest(send_period); }); - - robot_manager_node_->registerParameter>( - "joint_stiffness", joint_stiffness_, - kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, - [this](const std::vector & joint_stiffness) - { return this->onJointStiffnessChangeRequest(joint_stiffness); }); - - robot_manager_node_->registerParameter>( - "joint_damping", joint_damping_, - kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, - [this](const std::vector & joint_damping) - { return this->onJointDampingChangeRequest(joint_damping); }); - - robot_manager_node_->registerParameter( - "control_mode", POSITION_CONTROL, - kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, - [this](const std::string & control_mode) - { return this->onControlModeChangeRequest(control_mode); }); - - robot_manager_node_->registerParameter( - "position_controller_name", "", - kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, - [this](const std::string & controller_name) - { return this->onControllerNameChangeRequest(controller_name, true); }); - - robot_manager_node_->registerParameter( - "torque_controller_name", "", - kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, - [this](const std::string & controller_name) - { return this->onControllerNameChangeRequest(controller_name, false); }); - - robot_manager_node_->registerParameter( - "command_mode", POSITION_COMMAND, - kuka_drivers_core::ParameterSetAccessRights{false, true, true, false, true}, - [this](const std::string & command_mode) - { return this->onCommandModeChangeRequest(command_mode); }); - - robot_manager_node_->registerParameter( - "receive_multiplier", 1, - kuka_drivers_core::ParameterSetAccessRights{false, true, false, false, true}, - [this](const int & receive_multiplier) - { return this->onReceiveMultiplierChangeRequest(receive_multiplier); }); - - configured_ = true; - response->success = true; -} -} // namespace kuka_sunrise_fri_driver diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp index 97b08fd5..45958ca9 100644 --- a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp +++ b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp @@ -17,6 +17,10 @@ #include #include +#include "rclcpp/logging.hpp" + +#include + #include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "kuka_sunrise_fri_driver/serialization.hpp" #include "kuka_sunrise_fri_driver/tcp_connection.hpp" @@ -97,29 +101,20 @@ bool FRIConnection::setPositionControlMode() bool FRIConnection::setJointImpedanceControlMode( const std::vector & joint_stiffness, const std::vector & joint_damping) { - int msg_size = 0; - printf("Sizeof(double) = %lu\n", sizeof(double)); - printf( - "Joint_stiffness size: %lu, joint damping size: %lu\n", joint_stiffness.size(), - joint_damping.size()); std::vector serialized; serialized.reserve(1 + CONTROL_MODE_HEADER.size() + 2 * 7 * sizeof(double)); serialized.emplace_back(JOINT_IMPEDANCE_CONTROL_MODE); - msg_size++; for (std::uint8_t byte : CONTROL_MODE_HEADER) { serialized.emplace_back(byte); - msg_size++; } for (double js : joint_stiffness) { - printf("js = %lf\n", js); - msg_size += kuka_drivers_core::serializeNext(js, serialized); + serializeNext(js, serialized); } for (double jd : joint_damping) { - printf("jd = %lf\n", jd); - msg_size += kuka_drivers_core::serializeNext(jd, serialized); + serializeNext(jd, serialized); } return sendCommandAndWait(SET_CONTROL_MODE, serialized); } @@ -130,19 +125,22 @@ bool FRIConnection::setClientCommandMode(ClientCommandModeID client_command_mode return sendCommandAndWait(SET_COMMAND_MODE, command_data); } -bool FRIConnection::setFRIConfig(int remote_port, int send_period_ms, int receive_multiplier) +bool FRIConnection::setFRIConfig( + const std::string & client_ip, int remote_port, int send_period_ms, int receive_multiplier) { std::vector serialized; - serialized.reserve(FRI_CONFIG_HEADER.size() + 3 * sizeof(int)); - int msg_size = 0; + serialized.reserve(FRI_CONFIG_HEADER.size() + 4 * sizeof(int)); for (std::uint8_t byte : FRI_CONFIG_HEADER) { serialized.emplace_back(byte); - msg_size++; } - msg_size += kuka_drivers_core::serializeNext(remote_port, serialized); - msg_size += kuka_drivers_core::serializeNext(send_period_ms, serialized); - msg_size += kuka_drivers_core::serializeNext(receive_multiplier, serialized); + serializeNext(remote_port, serialized); + serializeNext(send_period_ms, serialized); + serializeNext(receive_multiplier, serialized); + + int ip = inet_addr(client_ip.c_str()); + serializeNext(ip, serialized); + return sendCommandAndWait(SET_FRI_CONFIG, serialized); } @@ -272,7 +270,7 @@ void FRIConnection::handleReceivedTCPData(const std::vector & data void FRIConnection::connectionLostCallback(const char * server_addr, int server_port) { - printf("Connection lost, trying to reconnect"); + RCLCPP_ERROR(rclcpp::get_logger("fri_connection"), "Connection lost, trying to reconnect"); connect(server_addr, server_port); } diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 443136ed..4557e65a 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -24,15 +24,24 @@ namespace kuka_sunrise_fri_driver CallbackReturn KukaFRIHardwareInterface::on_init( const hardware_interface::HardwareInfo & system_info) { + fri_connection_ = + std::make_shared([this] { this->onError(); }, [this] { this->onError(); }); + if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } - hw_states_.resize(info_.joints.size()); - hw_commands_.resize(info_.joints.size()); - hw_torques_.resize(info_.joints.size()); - hw_torques_ext_.resize(info_.joints.size()); - hw_effort_command_.resize(info_.joints.size()); + controller_ip_ = info_.hardware_parameters.at("controller_ip"); + client_ip_ = info_.hardware_parameters.at("client_ip"); + client_port_ = std::stoi(info_.hardware_parameters.at("client_port")); + + hw_position_states_.resize(info_.joints.size()); + hw_position_commands_.resize(info_.joints.size()); + hw_stiffness_commands_.resize(info_.joints.size()); + hw_damping_commands_.resize(info_.joints.size()); + hw_torque_states_.resize(info_.joints.size()); + hw_ext_torque_states_.resize(info_.joints.size()); + hw_torque_commands_.resize(info_.joints.size()); if (info_.gpios.size() != 1) { @@ -62,10 +71,10 @@ CallbackReturn KukaFRIHardwareInterface::on_init( for (const hardware_interface::ComponentInfo & joint : info_.joints) { - if (joint.command_interfaces.size() != 2) + if (joint.command_interfaces.size() != 4) { RCLCPP_FATAL( - rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 2 command interface"); + rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 4 command interface"); return CallbackReturn::ERROR; } @@ -73,15 +82,28 @@ CallbackReturn KukaFRIHardwareInterface::on_init( { RCLCPP_FATAL( rclcpp::get_logger("KukaFRIHardwareInterface"), - "expecting POSITION command interface as first"); + "expecting 'POSITION' command interface as first"); return CallbackReturn::ERROR; } - - if (joint.command_interfaces[1].name != hardware_interface::HW_IF_EFFORT) + if (joint.command_interfaces[1].name != hardware_interface::HW_IF_STIFFNESS) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "expecting 'STIFFNESS' command interface as second"); + return CallbackReturn::ERROR; + } + if (joint.command_interfaces[2].name != hardware_interface::HW_IF_DAMPING) { RCLCPP_FATAL( rclcpp::get_logger("KukaFRIHardwareInterface"), - "expecting EFFORT command interface as second"); + "expecting 'DAMPING' command interface as third"); + return CallbackReturn::ERROR; + } + if (joint.command_interfaces[3].name != hardware_interface::HW_IF_EFFORT) + { + RCLCPP_FATAL( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "expecting 'EFFORT' command interface as fourth"); return CallbackReturn::ERROR; } @@ -96,7 +118,7 @@ CallbackReturn KukaFRIHardwareInterface::on_init( { RCLCPP_FATAL( rclcpp::get_logger("KukaFRIHardwareInterface"), - "expecting POSITION state interface as first"); + "expecting 'POSITION' state interface as first"); return CallbackReturn::ERROR; } @@ -104,7 +126,7 @@ CallbackReturn KukaFRIHardwareInterface::on_init( { RCLCPP_FATAL( rclcpp::get_logger("KukaFRIHardwareInterface"), - "expecting EFFORT state interface as second"); + "expecting 'EFFORT' state interface as second"); return CallbackReturn::ERROR; } @@ -117,38 +139,118 @@ CallbackReturn KukaFRIHardwareInterface::on_init( } } + RCLCPP_INFO( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "Init successful with controller ip: %s and client ip: %s:%i", controller_ip_.c_str(), + client_ip_.c_str(), client_port_); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn KukaFRIHardwareInterface::on_configure(const rclcpp_lifecycle::State &) +{ + // Set up UDP connection (UDP replier on client) + if (!client_application_.connect(30200, controller_ip_.c_str())) + { + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not set up UDP connection"); + return CallbackReturn::FAILURE; + } + + // Set up TCP connection necessary for configuration + if (!fri_connection_->connect(controller_ip_.c_str(), TCP_SERVER_PORT)) + { + RCLCPP_ERROR( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "Could not initialize TCP connection to controller"); + return CallbackReturn::FAILURE; + } + RCLCPP_INFO( + rclcpp::get_logger("KukaFRIHardwareInterface"), "Successfully connected to FRI application"); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn KukaFRIHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &) +{ + client_application_.disconnect(); + + if (!fri_connection_->disconnect()) + { + RCLCPP_ERROR( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "Could not close TCP connection to controller"); + return CallbackReturn::ERROR; + } return CallbackReturn::SUCCESS; } CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - if (!client_application_.connect(30200, nullptr)) + // Set control mode before starting motion - not even the impedance attributes can be changed in + // active state + switch (static_cast(control_mode_)) + { + case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: + fri_connection_->setPositionControlMode(); + fri_connection_->setClientCommandMode(ClientCommandModeID::POSITION_COMMAND_MODE); + break; + case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: + fri_connection_->setJointImpedanceControlMode(hw_stiffness_commands_, hw_damping_commands_); + fri_connection_->setClientCommandMode(ClientCommandModeID::POSITION_COMMAND_MODE); + break; + case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: + fri_connection_->setJointImpedanceControlMode( + std::vector(DOF, 0.0), std::vector(DOF, 0.0)); + fri_connection_->setClientCommandMode(ClientCommandModeID::TORQUE_COMMAND_MODE); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Unsupported control mode"); + return CallbackReturn::ERROR; + } + + // Start FRI (in monitoring mode) + if (!fri_connection_->startFRI()) { - RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not connect"); + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not start FRI"); + return CallbackReturn::FAILURE; + } + RCLCPP_INFO(rclcpp::get_logger("KukaFRIHardwareInterface"), "Started FRI"); + + // Switch to commanding mode + if (!fri_connection_->activateControl()) + { + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not activate control"); return CallbackReturn::FAILURE; } - is_active_ = true; return CallbackReturn::SUCCESS; } CallbackReturn KukaFRIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { - client_application_.disconnect(); - is_active_ = false; + if (!fri_connection_->deactivateControl()) + { + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not deactivate control"); + return CallbackReturn::ERROR; + } + + if (!fri_connection_->endFRI()) + { + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not end FRI"); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; } void KukaFRIHardwareInterface::waitForCommand() { - hw_commands_ = hw_states_; - hw_effort_command_ = hw_torques_; - // TODO(Svastits): is this really the purpose of waitForCommand? + // In COMMANDING_WAIT state, the controller and client sync commanded positions + // Therefore the control signal should not be modified in this callback + // TODO(Svastits): check for torque/impedance mode, where state can change + hw_position_commands_ = hw_position_states_; rclcpp::Time stamp = ros_clock_.now(); - if (++receive_counter_ == receive_multiplier_) - { - updateCommand(stamp); - receive_counter_ = 0; - } + updateCommand(stamp); } void KukaFRIHardwareInterface::command() @@ -164,64 +266,68 @@ void KukaFRIHardwareInterface::command() hardware_interface::return_type KukaFRIHardwareInterface::read( const rclcpp::Time &, const rclcpp::Duration &) { - // Read is called in inactive state, check is necessary - if (!is_active_) - { - active_read_ = false; - RCLCPP_DEBUG(rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware interface not active"); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - return hardware_interface::return_type::OK; - } - active_read_ = true; - - if (!client_application_.client_app_read()) - { - RCLCPP_ERROR( - rclcpp::get_logger("KukaFRIHardwareInterface"), "Failed to read data from controller"); - return hardware_interface::return_type::ERROR; - } - - // get the position and efforts and share them with exposed state interfaces - const double * position = robotState().getMeasuredJointPosition(); - hw_states_.assign(position, position + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - const double * torque = robotState().getMeasuredTorque(); - hw_torques_.assign(torque, torque + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - const double * external_torque = robotState().getExternalTorque(); - hw_torques_ext_.assign(external_torque, external_torque + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - - robot_state_.tracking_performance_ = robotState().getTrackingPerformance(); - robot_state_.session_state_ = robotState().getSessionState(); - robot_state_.connection_quality_ = robotState().getConnectionQuality(); - robot_state_.command_mode_ = robotState().getClientCommandMode(); - robot_state_.safety_state_ = robotState().getSafetyState(); - robot_state_.control_mode_ = robotState().getControlMode(); - robot_state_.operation_mode_ = robotState().getOperationMode(); - robot_state_.drive_state_ = robotState().getDriveState(); - robot_state_.overlay_type_ = robotState().getOverlayType(); - - for (auto & output : gpio_outputs_) + if ((active_read_ = client_application_.client_app_read() == true)) { - output.getValue(); + // get the position and efforts and share them with exposed state interfaces + const double * position = robotState().getMeasuredJointPosition(); + hw_position_states_.assign(position, position + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + const double * torque = robotState().getMeasuredTorque(); + hw_torque_states_.assign(torque, torque + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + const double * external_torque = robotState().getExternalTorque(); + hw_ext_torque_states_.assign( + external_torque, external_torque + KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + + robot_state_.tracking_performance_ = robotState().getTrackingPerformance(); + robot_state_.session_state_ = robotState().getSessionState(); + robot_state_.connection_quality_ = robotState().getConnectionQuality(); + robot_state_.command_mode_ = robotState().getClientCommandMode(); + robot_state_.safety_state_ = robotState().getSafetyState(); + robot_state_.control_mode_ = robotState().getControlMode(); + robot_state_.operation_mode_ = robotState().getOperationMode(); + robot_state_.drive_state_ = robotState().getDriveState(); + robot_state_.overlay_type_ = robotState().getOverlayType(); + + for (auto & output : gpio_outputs_) + { + output.getValue(); + } } + // Modify state interface only in read + std::lock_guard lk(event_mutex_); + server_state_ = static_cast(last_event_); return hardware_interface::return_type::OK; } hardware_interface::return_type KukaFRIHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration &) { - // Client app update and read must be called if read has been called in current cycle + // Client app update and read must be called only if read has been called in current cycle + // FRI configuration can be modified only before cyclic communication is started if (!active_read_) { - RCLCPP_DEBUG(rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware interface not active"); + if (FRIConfigChanged()) + { + // FRI config cannot be set during hardware interface configuration, as the controller cannot + // modify the cmd interface until the hardware reached the configured state + if (!fri_connection_->setFRIConfig( + client_ip_, client_port_, static_cast(send_period_ms_), + static_cast(receive_multiplier_))) + { + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not set FRI config"); + return hardware_interface::return_type::ERROR; + } + RCLCPP_INFO(rclcpp::get_logger("KukaFRIHardwareInterface"), "Successfully set FRI config"); + } + return hardware_interface::return_type::OK; } // Call the appropriate callback for the actual state (e.g. updateCommand) - // this updates the command to be sent based on the output of the controller update + // in active state this updates the command to be sent based on the command interfaces client_application_.client_app_update(); - if (!client_application_.client_app_write() && is_active_) + if (!client_application_.client_app_write()) { RCLCPP_ERROR( rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not send command to controller"); @@ -233,27 +339,34 @@ hardware_interface::return_type KukaFRIHardwareInterface::write( void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) { - if (!is_active_) - { - RCLCPP_ERROR( - rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware inactive, exiting updateCommand"); - return; - } - if (robot_state_.command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) - { - const double * joint_torques_ = hw_effort_command_.data(); - robotCommand().setJointPosition(robotState().getIpoJointPosition()); - robotCommand().setTorque(joint_torques_); - } - else if (robot_state_.command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) - { - const double * joint_positions_ = hw_commands_.data(); - robotCommand().setJointPosition(joint_positions_); - } - else + switch (static_cast(control_mode_)) { - RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Unsupported command mode"); + case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: + [[fallthrough]]; + case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: + { + const double * joint_positions_ = hw_position_commands_.data(); + robotCommand().setJointPosition(joint_positions_); + break; + } + case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: + { + const double * joint_torques_ = hw_torque_commands_.data(); + const double * joint_pos = robotState().getMeasuredJointPosition(); + std::array joint_pos_corr; + std::copy(joint_pos, joint_pos + DOF, joint_pos_corr.begin()); + activateFrictionCompensation(joint_pos_corr.data()); + robotCommand().setJointPosition(joint_pos_corr.data()); + robotCommand().setTorque(joint_torques_); + break; + } + default: + RCLCPP_ERROR( + rclcpp::get_logger("KukaFRIHardwareInterface"), + "Unsupported control mode, exiting updateCommand"); + return; } + for (auto & input : gpio_inputs_) { input.setValue(); @@ -302,14 +415,17 @@ std::vector KukaFRIHardwareInterface::export for (size_t i = 0; i < info_.joints.size(); i++) { state_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_states_[i]); state_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torques_[i]); + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_states_[i]); state_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_EXTERNAL_TORQUE, &hw_torques_ext_[i]); + info_.joints[i].name, hardware_interface::HW_IF_EXTERNAL_TORQUE, &hw_ext_torque_states_[i]); } + + state_interfaces.emplace_back( + hardware_interface::STATE_PREFIX, hardware_interface::SERVER_STATE, &server_state_); return state_interfaces; } @@ -318,9 +434,13 @@ KukaFRIHardwareInterface::export_command_interfaces() { std::vector command_interfaces; + command_interfaces.emplace_back( + hardware_interface::CONFIG_PREFIX, hardware_interface::CONTROL_MODE, &control_mode_); command_interfaces.emplace_back( hardware_interface::CONFIG_PREFIX, hardware_interface::RECEIVE_MULTIPLIER, &receive_multiplier_); + command_interfaces.emplace_back( + hardware_interface::CONFIG_PREFIX, hardware_interface::SEND_PERIOD, &send_period_ms_); // Register I/O inputs (write access) for (auto & input : gpio_inputs_) @@ -332,12 +452,50 @@ KukaFRIHardwareInterface::export_command_interfaces() for (size_t i = 0; i < info_.joints.size(); i++) { command_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_commands_[i]); + command_interfaces.emplace_back( + info_.joints[i].name, hardware_interface::HW_IF_STIFFNESS, &hw_stiffness_commands_[i]); command_interfaces.emplace_back( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_effort_command_[i]); + info_.joints[i].name, hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[i]); + command_interfaces.emplace_back( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_commands_[i]); } return command_interfaces; } + +// Friction compensation is activated only if the commanded and measured joint positions differ +void KukaFRIHardwareInterface::activateFrictionCompensation(double * values) const +{ + for (int i = 0; i < DOF; i++) + { + values[i] -= (values[i] / fabs(values[i]) * 0.1); + } +} + +void KukaFRIHardwareInterface::onError() +{ + std::lock_guard lk(event_mutex_); + last_event_ = kuka_drivers_core::HardwareEvent::ERROR; + RCLCPP_ERROR( + rclcpp::get_logger("KukaFRIHardwareInterface"), "External control stopped by an error"); +} + +bool KukaFRIHardwareInterface::FRIConfigChanged() +{ + // FRI config values are integers and only stored as doubles due to hwif constraints + if ( + prev_period_ == static_cast(send_period_ms_) && + prev_multiplier_ == static_cast(receive_multiplier_)) + { + return false; + } + else + { + prev_period_ = static_cast(send_period_ms_); + prev_multiplier_ = static_cast(receive_multiplier_); + return true; + } +} } // namespace kuka_sunrise_fri_driver PLUGINLIB_EXPORT_CLASS( diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 16b9e109..85466a23 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -17,6 +17,7 @@ #include "communication_helpers/ros2_control_tools.hpp" #include "communication_helpers/service_tools.hpp" #include "kuka_drivers_core/controller_names.hpp" +#include "kuka_drivers_core/hardware_event.hpp" #include "kuka_sunrise_fri_driver/robot_manager_node.hpp" @@ -36,36 +37,95 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_ // RT controllers are started after interface activation // non-RT controllers are started after interface configuration - fri_connection_ = std::make_shared( - [this] { this->handleControlEndedError(); }, [this] { this->handleFRIEndedError(); }); auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + event_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); change_hardware_state_client_ = this->create_client( "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_); change_controller_state_client_ = this->create_client( "controller_manager/switch_controller", qos.get_rmw_qos_profile(), cbg_); - command_state_changed_publisher_ = - this->create_publisher("robot_manager/commanding_state_changed", qos); - set_parameter_client_ = this->create_client( - "configuration_manager/set_params", ::rmw_qos_profile_default, cbg_); auto is_configured_qos = rclcpp::QoS(rclcpp::KeepLast(1)); is_configured_qos.best_effort(); - is_configured_pub_ = this->create_publisher("robot_manager/is_configured", is_configured_qos); - this->registerStaticParameter( - "robot_model", "lbr_iiwa14_r820", - kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, + fri_config_pub_ = this->create_publisher( + "fri_configuration_controller/set_fri_config", rclcpp::SystemDefaultsQoS()); + + control_mode_pub_ = this->create_publisher( + "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); + + joint_imp_pub_ = this->create_publisher( + "joint_group_impedance_controller/commands", rclcpp::SystemDefaultsQoS()); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = event_cbg_; + event_subscriber_ = this->create_subscription( + "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), + [this](const std_msgs::msg::UInt8::SharedPtr msg) { this->EventSubscriptionCallback(msg); }, + sub_options); + + registerStaticParameter( + "robot_model", "lbr_iiwa14_r820", kuka_drivers_core::ParameterSetAccessRights{false, false}, [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); + + registerStaticParameter( + "controller_ip", "", kuka_drivers_core::ParameterSetAccessRights{false, false}, + [this](const std::string & controller_ip) { return this->ValidateIPAdress(controller_ip); }); + + registerParameter( + "send_period_ms", 10, kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const int & send_period) { return this->onSendPeriodChangeRequest(send_period); }); + + registerParameter( + "receive_multiplier", 1, kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const int & receive_multiplier) + { return this->onReceiveMultiplierChangeRequest(receive_multiplier); }); + + registerParameter( + "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), + kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); + + registerParameter( + "position_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::string & controller_name) + { + return this->onControllerNameChangeRequest( + controller_name, kuka_drivers_core::ControllerType::JOINT_POSITION_CONTROLLER_TYPE); + }); + + registerParameter( + "torque_controller_name", "", kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::string & controller_name) + { + return this->onControllerNameChangeRequest( + controller_name, kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE); + }); + + registerParameter>( + "joint_stiffness", joint_stiffness_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::vector & joint_stiffness) + { return this->onJointStiffnessChangeRequest(joint_stiffness); }); + + registerParameter>( + "joint_damping", joint_damping_, kuka_drivers_core::ParameterSetAccessRights{true, false}, + [this](const std::vector & joint_damping) + { return this->onJointDampingChangeRequest(joint_damping); }); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { + // Publish control mode parameter to notify control_mode_handler of initial control mode + control_mode_pub_->publish(control_mode_msg_); + + // Publish FRI configuration to notify fri_configuration_controller of initial values + setFriConfiguration(send_period_ms_, receive_multiplier_); + // Configure hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, @@ -75,84 +135,33 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) return FAILURE; } - auto result = SUCCESS; - - // If this fails, the node should be restarted, with different parameter values - // Therefore exceptions are not caught - if (!configuration_manager_) - { - configuration_manager_ = std::make_unique( - std::dynamic_pointer_cast(this->shared_from_this()), - fri_connection_); - } - RCLCPP_INFO(get_logger(), "Successfully set 'controller_ip' parameter"); - // Start non-RT controllers if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, - {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::FRI_STATE_BROADCASTER}, + {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::CONTROL_MODE_HANDLER, + kuka_drivers_core::EVENT_BROADCASTER, kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate configuration controllers"); - result = FAILURE; - } - - const char * controller_ip = this->get_parameter("controller_ip").as_string().c_str(); - if (!fri_connection_->isConnected()) - { - if (!fri_connection_->connect(controller_ip, 30000)) - { - RCLCPP_ERROR(get_logger(), "could not connect"); - result = FAILURE; - } - } - else - { - RCLCPP_ERROR(get_logger(), "Robot manager is connected in inactive state"); - return ERROR; + return FAILURE; } - RCLCPP_INFO(get_logger(), "Successfully connected to FRI"); - if (result == SUCCESS) - { - auto trigger_request = std::make_shared(); - auto response = kuka_drivers_core::sendRequest( - set_parameter_client_, trigger_request, 0, 1000); + is_configured_pub_->on_activate(); + is_configured_msg_.data = true; + is_configured_pub_->publish(is_configured_msg_); - if (!response || !response->success) - { - RCLCPP_ERROR(get_logger(), "Could not set parameters"); - result = FAILURE; - } - } - if (result != SUCCESS) - { - this->on_cleanup(get_current_state()); - } - else - { - is_configured_pub_->on_activate(); - is_configured_msg_.data = true; - is_configured_pub_->publish(is_configured_msg_); - } - - return result; + return SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { - if (fri_connection_->isConnected() && !fri_connection_->disconnect()) - { - RCLCPP_ERROR(get_logger(), "could not disconnect"); - return ERROR; - } - // Stop non-RT controllers // With best effort strictness, cleanup succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {}, - {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::FRI_STATE_BROADCASTER}, + {kuka_drivers_core::FRI_CONFIGURATION_CONTROLLER, kuka_drivers_core::CONTROL_MODE_HANDLER, + kuka_drivers_core::EVENT_BROADCASTER, kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop controllers"); @@ -180,24 +189,17 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) } // TODO(Svastits): can we check if necessary 5s has passed after deactivation? -// TODO(Svastits): check if we have to send unconfigured msg to control node rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { - if (!fri_connection_->isConnected()) - { - RCLCPP_ERROR(get_logger(), "not connected"); - return ERROR; - } - - auto send_period_ms = static_cast(this->get_parameter("send_period_ms").as_int()); - auto receive_multiplier = static_cast(this->get_parameter("receive_multiplier").as_int()); - if (!fri_connection_->setFRIConfig(30200, send_period_ms, receive_multiplier)) + // Publish the values of the joint impedance parameters to the controller + std_msgs::msg::Float64MultiArray joint_imp_msg; + for (int i = 0; i < 7; i++) { - RCLCPP_ERROR(get_logger(), "could not set FRI config"); - return FAILURE; + joint_imp_msg.data.push_back(joint_stiffness_[i]); + joint_imp_msg.data.push_back(joint_damping_[i]); } - RCLCPP_INFO(get_logger(), "Successfully set FRI config"); + joint_imp_pub_->publish(joint_imp_msg); // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( @@ -205,76 +207,29 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) { RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); - // 'unset config' does not exist, safe to return return FAILURE; } - // Start FRI (in monitoring mode) - if (!fri_connection_->startFRI()) - { - RCLCPP_ERROR(get_logger(), "Could not start FRI"); - this->on_deactivate(get_current_state()); - return FAILURE; - } - RCLCPP_INFO(get_logger(), "Started FRI"); - - // Activate joint state broadcaster - // The other controller must be started later so that it can initialize internal state - // with broadcaster information -> TODO(Svastits): validate whether this is true - if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {kuka_drivers_core::JOINT_STATE_BROADCASTER}, {})) - { - RCLCPP_ERROR(get_logger(), "Could not activate joint state broadcaster"); - this->on_deactivate(get_current_state()); - return FAILURE; - } + // Workaround until controller_manager/jtc bug is fixed: + std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto position_controller_name = this->get_parameter("position_controller_name").as_string(); - auto torque_controller_name = this->get_parameter("torque_controller_name").as_string(); - controller_name_ = (this->get_parameter("command_mode").as_string() == "position") - ? position_controller_name - : torque_controller_name; - // Activate RT commander + // Activate joint state broadcaster and controller for given control mode if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {controller_name_}, {})) - { - RCLCPP_ERROR(get_logger(), "Could not activate RT controller"); - this->on_deactivate(get_current_state()); - return FAILURE; - } - command_state_changed_publisher_->on_activate(); - - // Start commanding mode - if (!activateControl()) + change_controller_state_client_, + {kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER, + GetControllerName()}, + {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER})) { + RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); this->on_deactivate(get_current_state()); return FAILURE; } - return SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) { - if (!fri_connection_->isConnected()) - { - RCLCPP_ERROR(get_logger(), "Not connected"); - return ERROR; - } - - if (!this->deactivateControl()) - { - RCLCPP_ERROR(get_logger(), "Could not deactivate control"); - return ERROR; - } - - if (!fri_connection_->endFRI()) - { - RCLCPP_ERROR(get_logger(), "Could not end FRI"); - return ERROR; - } - // Deactivate hardware interface // If it is inactive, deactivation will also succeed if (!kuka_drivers_core::changeHardwareState( @@ -288,90 +243,241 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers // With best effort strictness, deactivation succeeds if specific controller is not active if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, - {controller_name_, kuka_drivers_core::JOINT_STATE_BROADCASTER}, + change_controller_state_client_, {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}, + {GetControllerName(), kuka_drivers_core::JOINT_STATE_BROADCASTER, + kuka_drivers_core::FRI_STATE_BROADCASTER}, SwitchController::Request::BEST_EFFORT)) { - RCLCPP_ERROR(get_logger(), "Could not deactivate controllers"); + RCLCPP_ERROR(get_logger(), "Could not deactivate RT controllers"); return ERROR; } - if (command_state_changed_publisher_->is_activated()) - { - command_state_changed_publisher_->on_deactivate(); - } - RCLCPP_INFO( get_logger(), "Successfully deactivated driver, reactivation is possible after 5 seconds"); return SUCCESS; } -bool RobotManagerNode::activateControl() +bool RobotManagerNode::onRobotModelChangeRequest(const std::string & robot_model) +{ + auto ns = std::string(get_namespace()); + // Remove '/' from namespace (even empty namespace contains one '/') + ns.erase(ns.begin()); + + // Add '_' to prefix + if (ns.size() > 0) + { + ns += "_"; + } + robot_model_ = ns + robot_model; + return true; +} + +bool RobotManagerNode::onControlModeChangeRequest(int control_mode) { - if (!fri_connection_->isConnected()) + switch (static_cast(control_mode)) { - RCLCPP_ERROR(get_logger(), "Not connected"); + case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: + break; + case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: + // TODO(Svastits): check whether this is necessary for impedance mode too + [[fallthrough]]; + case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: + if (send_period_ms_ > 5) + { + RCLCPP_ERROR( + get_logger(), + "Unable to set non-position control mode, if send period is bigger than 5 [ms]"); + return false; + } + break; + default: + RCLCPP_ERROR(get_logger(), "Tried to change to a not implemented control mode"); + return false; + } + + // Publish the control mode to controller handler + control_mode_msg_.data = control_mode; + control_mode_pub_->publish(control_mode_msg_); + RCLCPP_INFO(get_logger(), "Control mode change successful"); + + return true; +} + +bool RobotManagerNode::onSendPeriodChangeRequest(int send_period) +{ + if (send_period < 1 || send_period > 100) + { + RCLCPP_ERROR(get_logger(), "Send period milliseconds must be >=1 && <=100"); return false; } - if (!fri_connection_->activateControl()) + if (send_period * receive_multiplier_ > 10) { - RCLCPP_ERROR(get_logger(), "Could not activate control"); + RCLCPP_ERROR(get_logger(), "Control signal send period must not be bigger than 10 ms"); return false; } - std_msgs::msg::Bool command_state; - command_state.data = true; - command_state_changed_publisher_->publish(command_state); + + send_period_ms_ = send_period; + setFriConfiguration(send_period_ms_, receive_multiplier_); return true; } -bool RobotManagerNode::deactivateControl() +bool RobotManagerNode::onReceiveMultiplierChangeRequest(const int & receive_multiplier) { - if (!fri_connection_->isConnected()) + if (receive_multiplier < 1) { - RCLCPP_ERROR(get_logger(), "Not connected"); + RCLCPP_ERROR(get_logger(), "Receive multiplier must be >=1"); return false; } - if (!fri_connection_->deactivateControl()) + if (receive_multiplier * send_period_ms_ > 10) { - RCLCPP_ERROR(get_logger(), "Could not deactivate control"); + RCLCPP_ERROR(get_logger(), "Control signal send period must be bigger than 10 ms"); return false; } - std_msgs::msg::Bool command_state; - command_state.data = false; - command_state_changed_publisher_->publish(command_state); + receive_multiplier_ = receive_multiplier; + setFriConfiguration(send_period_ms_, receive_multiplier_); return true; } -void RobotManagerNode::handleControlEndedError() +bool RobotManagerNode::ValidateIPAdress(std::string_view controller_ip) const { - RCLCPP_INFO(get_logger(), "Control ended"); - this->LifecycleNode::deactivate(); + // Check IP validity + size_t i = 0; + std::vector split_ip; + auto pos = controller_ip.find('.'); + while (pos != std::string_view::npos) + { + split_ip.emplace_back(controller_ip.substr(i, pos - i)); + i = ++pos; + pos = controller_ip.find('.', pos); + } + split_ip.emplace_back(controller_ip.substr(i, controller_ip.length())); + + if (split_ip.size() != 4) + { + RCLCPP_ERROR(get_logger(), "Valid IP must have 3 '.' delimiters"); + return false; + } + + for (const auto & ip : split_ip) + { + if ( + ip.empty() || (ip.find_first_not_of("[0123456789]") != std::string::npos) || stoi(ip) > 255 || + stoi(ip) < 0) + { + RCLCPP_ERROR(get_logger(), "Valid IP must contain only numbers between 0 and 255"); + return false; + } + } + return true; } -void RobotManagerNode::handleFRIEndedError() +bool RobotManagerNode::onControllerNameChangeRequest( + std::string_view controller_name, kuka_drivers_core::ControllerType controller_type) { - RCLCPP_INFO(get_logger(), "FRI ended"); - this->LifecycleNode::deactivate(); + switch (controller_type) + { + case kuka_drivers_core::ControllerType::JOINT_POSITION_CONTROLLER_TYPE: + joint_pos_controller_name_ = controller_name; + break; + case kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE: + joint_torque_controller_name_ = controller_name; + break; + default: + // This should actually never happen + RCLCPP_ERROR(get_logger(), "Invalid controller type"); + return false; + } + return true; } -bool RobotManagerNode::onRobotModelChangeRequest(const std::string & robot_model) +// the joint impedannce attributes cannot be modified in FRI after activation, therefore only one +// controller controls in each control mode +std::string RobotManagerNode::GetControllerName() const { - auto ns = std::string(this->get_namespace()); - // Remove '/' from namespace (even empty namespace contains one '/') - ns.erase(ns.begin()); + switch (static_cast(control_mode_msg_.data)) + { + case kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL: + return joint_pos_controller_name_; + case kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL: + return joint_pos_controller_name_; + case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL: + return joint_torque_controller_name_; + default: + throw std::runtime_error("Stored control mode is not allowed"); + } +} - // Add '_' to prefix - if (ns.size() > 0) +void RobotManagerNode::setFriConfiguration(int send_period_ms, int receive_multiplier) const +{ + kuka_driver_interfaces::msg::FriConfiguration msg; + msg.receive_multiplier = receive_multiplier; + msg.send_period_ms = send_period_ms; + fri_config_pub_->publish(msg); +} + +bool RobotManagerNode::onJointStiffnessChangeRequest(const std::vector & joint_stiffness) +{ + if (joint_stiffness.size() != 7) { - ns += "_"; + RCLCPP_ERROR(get_logger(), "Invalid parameter array length for parameter joint stiffness"); + return false; } - robot_model_ = ns + robot_model; + for (double js : joint_stiffness) + { + if (js < 0) + { + RCLCPP_ERROR(get_logger(), "Joint stiffness values must be >=0"); + return false; + } + } + joint_stiffness_ = joint_stiffness; return true; } +bool RobotManagerNode::onJointDampingChangeRequest(const std::vector & joint_damping) +{ + if (joint_damping.size() != 7) + { + RCLCPP_ERROR(get_logger(), "Invalid parameter array length for parameter joint damping"); + return false; + } + for (double jd : joint_damping) + { + if (jd < 0 || jd > 1) + { + RCLCPP_ERROR(get_logger(), "Joint damping values must be >=0 && <=1"); + return false; + } + } + joint_damping_ = joint_damping; + return true; +} + +void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) +{ + switch (static_cast(msg->data)) + { + case kuka_drivers_core::HardwareEvent::ERROR: + RCLCPP_INFO(get_logger(), "External control stopped"); + if (this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + this->deactivate(); + } + else if ( + this->get_current_state().id() == lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) + { + // Handle case, when error is received while still activating + this->on_deactivate(get_current_state()); + } + break; + default: + break; + } +} + } // namespace kuka_sunrise_fri_driver int main(int argc, char * argv[]) diff --git a/kuka_sunrise_fri_driver/test/test_driver_activation.py b/kuka_sunrise_fri_driver/test/test_driver_activation.py new file mode 100644 index 00000000..80705852 --- /dev/null +++ b/kuka_sunrise_fri_driver/test/test_driver_activation.py @@ -0,0 +1,88 @@ +# Copyright 2024 Aron Svastits +# +# 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. + +import unittest + +import launch +import launch.actions +import launch_testing.actions +import launch_testing.markers +import pytest + +from launch.launch_description_sources.python_launch_description_source import ( + PythonLaunchDescriptionSource, +) +from launch.actions.include_launch_description import IncludeLaunchDescription +from ament_index_python.packages import get_package_share_directory + + +# Launch driver startup +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + return launch.LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + get_package_share_directory("kuka_sunrise_fri_driver"), + "/launch/", + "startup.launch.py", + ] + ), + launch_arguments={ + "use_fake_hardware": "true", + }.items(), + ), + launch.actions.TimerAction( + period=10.0, + actions=[ + launch.actions.ExecuteProcess( + cmd=["ros2", "lifecycle", "set", "robot_manager", "configure"], + output="screen", + ), + ], + ), + launch.actions.TimerAction( + period=15.0, + actions=[ + launch.actions.ExecuteProcess( + cmd=["ros2", "lifecycle", "set", "robot_manager", "activate"], + output="screen", + ), + ], + ), + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestDriverActivation(unittest.TestCase): + def test_read_stdout(self, proc_output): + # Check for successful initialization + proc_output.assertWaitFor("got segment base", timeout=5) + proc_output.assertWaitFor( + "Successful initialization of hardware 'lbr_iiwa14_r820'", timeout=5 + ) + # Check whether disabling automatic activation was successful + proc_output.assertWaitFor( + "Setting component 'lbr_iiwa14_r820' to 'unconfigured' state.", timeout=5 + ) + # Check for successful configuration and activation + proc_output.assertWaitFor( + "Successful 'configure' of hardware 'lbr_iiwa14_r820'", timeout=15 + ) + proc_output.assertWaitFor( + "Successful 'activate' of hardware 'lbr_iiwa14_r820'", timeout=20 + )