diff --git a/doc/wiki/Home.md b/doc/wiki/Home.md index c5391f48..a3aebc21 100644 --- a/doc/wiki/Home.md +++ b/doc/wiki/Home.md @@ -64,7 +64,21 @@ The following features of the FRI are exposed to ROS2: ## KUKA KSS driver (RSI) -Another project in the repo centers on the development of a ROS2 driver for KSS robots through Robot Sensor Interface (RSI). It is in an experimental state, with only joint angle states and commands available. The structure of the driver is similiar to that of the Sunrise driver and the same interfaces are opened, so the same joint controller can be used to move robots. Howewer this controller should be improved, as it allows big torques that stop the robot for machine protection reasons. +Another project in the repo centers on the development of a ROS2 driver for KSS robots through Robot Sensor Interface (RSI). It is in an experimental state, with only joint angle states and commands available. The guide to set up this driver on a real robot can be found in kuka_rsi_hw_interface\krl for both KCR4 and KRC5 controllers. + +### Simulation + +To try out the driver with an open-loop simulation the driver and the kuka_rsi_simulator must be started, (at first only a "collapsed" robot will be visible in rviz): + +**`ros2 launch kuka_rsi_hw_interface startup_with_rviz.launch.py`** + +**`ros2 launch kuka_rsi_simulator kuka_rsi_simulator_launch.py`** + +After all components have started successfully, the system needs to be configured and activated to start the simulation, the robot will be visible in rviz after activation: + +**`ros2 lifecycle set robot_manager configure`** + +**`ros2 lifecycle set robot_manager activate`** ## iiQKA driver (ECI) diff --git a/kuka_rox_hardware_interface/CMakeLists.txt b/kuka_rox_hardware_interface/CMakeLists.txt index 3d08f78d..91c079f2 100644 --- a/kuka_rox_hardware_interface/CMakeLists.txt +++ b/kuka_rox_hardware_interface/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(sensor_msgs REQUIRED) find_package(kroshu_ros2_core REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) -find_package(controller_manager REQUIRED) find_package(controller_manager_msgs REQUIRED) find_package(yaml-cpp REQUIRED) @@ -76,19 +75,15 @@ ament_target_dependencies(kuka_rox_hw_interface rclcpp sensor_msgs hardware_inte target_link_libraries(kuka_rox_hw_interface motion-external-proto-api-nanopb motion-services-ecs-proto-api-cpp motion-services-ecs-proto-api-nanopb yaml-cpp os-core-udp-communication nanopb-helpers) -add_executable(rox_control_node - src/rox_control_node.cpp) -ament_target_dependencies(rox_control_node rclcpp kroshu_ros2_core sensor_msgs controller_manager controller_manager_msgs) -target_link_libraries(rox_control_node kuka_rox_hw_interface) add_executable(robot_manager_node src/robot_manager_node.cpp) -ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager controller_manager_msgs) +ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager_msgs) target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers motion-services-ecs-proto-api-cpp) pluginlib_export_plugin_description_file(hardware_interface kuka_rox_hw_interface.xml) -install(TARGETS kuka_rox_hw_interface rox_control_node robot_manager_node +install(TARGETS kuka_rox_hw_interface robot_manager_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch diff --git a/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp b/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp index 7a53b656..93d04afb 100644 --- a/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp +++ b/kuka_rox_hardware_interface/include/kuka_rox_hw_interface/robot_manager_node.hpp @@ -56,12 +56,10 @@ class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; - bool onControlModeChangeRequest(int control_mode); - bool onRobotModelChangeRequest(const std::string & robot_model); - private: void ObserveControl(); - + bool onControlModeChangeRequest(int control_mode); + bool onRobotModelChangeRequest(const std::string & robot_model); rclcpp::Client::SharedPtr change_hardware_state_client_; diff --git a/kuka_rox_hardware_interface/launch/startup.launch.py b/kuka_rox_hardware_interface/launch/startup.launch.py index 2bfd1280..005869a8 100644 --- a/kuka_rox_hardware_interface/launch/startup.launch.py +++ b/kuka_rox_hardware_interface/launch/startup.launch.py @@ -15,12 +15,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch_ros.actions import Node -from launch_ros.actions import LifecycleNode -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from launch.substitutions import LaunchConfiguration from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node, LifecycleNode +from launch_ros.substitutions import FindPackageShare def launch_setup(context, *args, **kwargs): @@ -70,8 +68,8 @@ def launch_setup(context, *args, **kwargs): controller_manager_node = '/controller_manager' control_node = Node( - package='kuka_rox_hw_interface', - executable='rox_control_node', + package='kroshu_ros2_core', + executable='control_node', parameters=[robot_description, controller_config] ) robot_manager_node = LifecycleNode( diff --git a/kuka_rox_hardware_interface/package.xml b/kuka_rox_hardware_interface/package.xml index 0481e50b..1a7c0d25 100644 --- a/kuka_rox_hardware_interface/package.xml +++ b/kuka_rox_hardware_interface/package.xml @@ -20,7 +20,6 @@ tinyxml_vendor hardware_interface pluginlib - controller_manager yaml-cpp protobuf-dev diff --git a/kuka_rox_hardware_interface/src/rox_control_node.cpp b/kuka_rox_hardware_interface/src/rox_control_node.cpp deleted file mode 100644 index 2b958412..00000000 --- a/kuka_rox_hardware_interface/src/rox_control_node.cpp +++ /dev/null @@ -1,82 +0,0 @@ -// Copyright 2022 Áron 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. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto executor = std::make_shared(); - auto controller_manager = std::make_shared( - executor, - "controller_manager"); - - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); - qos.best_effort(); - - std::atomic_bool is_configured = false; - auto is_configured_sub = controller_manager->create_subscription( - "robot_manager/is_configured", qos, - [&is_configured](std_msgs::msg::Bool::SharedPtr msg) { - is_configured = msg->data; - }); - - - std::thread control_loop([controller_manager, &is_configured]() { - struct sched_param param; - param.sched_priority = 95; - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - RCLCPP_ERROR(controller_manager->get_logger(), "setscheduler error"); - RCLCPP_ERROR(controller_manager->get_logger(), strerror(errno)); - RCLCPP_WARN( - controller_manager->get_logger(), - "You can use the driver but scheduler priority was not set"); - } - - const rclcpp::Duration dt = - rclcpp::Duration::from_seconds(1.0 / controller_manager->get_update_rate()); - std::chrono::milliseconds dt_ms {1000 / controller_manager->get_update_rate()}; - - try { - while (rclcpp::ok()) { - if (is_configured) { - controller_manager->read(controller_manager->now(), dt); - controller_manager->update(controller_manager->now(), dt); - controller_manager->write(controller_manager->now(), dt); - } else { - std::this_thread::sleep_for(dt_ms); - } - } - } catch (std::exception & e) { - RCLCPP_ERROR( - controller_manager->get_logger(), "Quitting control loop due to: %s", - e.what()); - } - }); - - executor->add_node(controller_manager); - - executor->spin(); - control_loop.join(); - - // shutdown - rclcpp::shutdown(); - - return 0; -} diff --git a/kuka_rsi_hw_interface/CMakeLists.txt b/kuka_rsi_hw_interface/CMakeLists.txt index 8b934422..71edd5bf 100644 --- a/kuka_rsi_hw_interface/CMakeLists.txt +++ b/kuka_rsi_hw_interface/CMakeLists.txt @@ -20,25 +20,38 @@ find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(kroshu_ros2_core REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(controller_manager_msgs REQUIRED) +find_package(pluginlib REQUIRED) find_package(tinyxml_vendor REQUIRED) find_package(TinyXML REQUIRED) include_directories(include ${TinyXML2_INCLUDE_DIRS}) -add_library(kuka_hardware_interface +add_library(${PROJECT_NAME} SHARED src/kuka_hardware_interface.cpp ) -ament_target_dependencies(kuka_hardware_interface rclcpp sensor_msgs) -target_link_libraries(kuka_hardware_interface tinyxml) -add_executable(robot_control_node - src/robot_control_node.cpp) -ament_target_dependencies(robot_control_node rclcpp kroshu_ros2_core sensor_msgs) -target_link_libraries(robot_control_node kuka_hardware_interface) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface) +target_link_libraries(${PROJECT_NAME} tinyxml) -install(TARGETS kuka_hardware_interface robot_control_node + + +add_executable(robot_manager_node + src/robot_manager_node.cpp) +ament_target_dependencies(robot_manager_node rclcpp kroshu_ros2_core sensor_msgs controller_manager_msgs) +target_link_libraries(robot_manager_node kroshu_ros2_core::communication_helpers) + +pluginlib_export_plugin_description_file(hardware_interface kuka_rsi_hw_interface.xml) + +install(TARGETS ${PROJECT_NAME} robot_manager_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) @@ -60,4 +73,21 @@ if(BUILD_TESTING) ament_xmllint(--exclude ros_rsi.rsi.xml) endif() +## EXPORTS +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME}) + ament_package() diff --git a/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml b/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml new file mode 100644 index 00000000..3de96e7b --- /dev/null +++ b/kuka_rsi_hw_interface/config/joint_trajectory_controller_config.yaml @@ -0,0 +1,16 @@ +joint_trajectory_controller: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + + command_interfaces: + - position + state_interfaces: + - position + state_publish_rate: 50.0 + action_monitor_rate: 20.0 diff --git a/kuka_rsi_hw_interface/config/ros2_controller_config.yaml b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml new file mode 100644 index 00000000..2eabba85 --- /dev/null +++ b/kuka_rsi_hw_interface/config/ros2_controller_config.yaml @@ -0,0 +1,11 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + configure_components_on_start: [""] diff --git a/kuka_rsi_hw_interface/config/rsi_config.yaml b/kuka_rsi_hw_interface/config/rsi_config.yaml new file mode 100644 index 00000000..f037e35e --- /dev/null +++ b/kuka_rsi_hw_interface/config/rsi_config.yaml @@ -0,0 +1,2 @@ +client_ip: "0.0.0.0" +client_port: 59152 \ No newline at end of file diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp index ab3f0fbd..7b409e86 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.hpp @@ -1,37 +1,37 @@ /********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Norwegian University of Science and - * Technology, nor the names of its contributors may be used to - * endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ +* Software License Agreement (BSD License) +* +* Copyright (c) 2014 Norwegian University of Science and Technology +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Norwegian University of Science and +* Technology, nor the names of its contributors may be used to +* endorse or promote products derived from this software without +* specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ /* * Author: Lars Tingelstad @@ -52,28 +52,64 @@ #include #include +#include "rclcpp/macros.hpp" + +#include "kuka_rsi_hw_interface/visibility_control.h" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "pluginlib/class_list_macros.hpp" + + +using hardware_interface::return_type; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + namespace kuka_rsi_hw_interface { -class KukaHardwareInterface + +class KukaRSIHardwareInterface : public hardware_interface::SystemInterface { public: - explicit KukaHardwareInterface(const std::string & rsi_ip_address, int rsi_port, uint8_t n_dof); + RCLCPP_SHARED_PTR_DEFINITIONS(KukaRSIHardwareInterface) - void start(std::vector & joint_state_msg_position); - void stop(); - bool read(std::vector & joint_state_msg_position); - bool write(const std::vector & joint_pos_correction_deg_); + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - bool isActive() const; + KUKA_RSI_HW_INTERFACE_PUBLIC + std::vector export_state_interfaces() override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + std::vector export_command_interfaces() override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + KUKA_RSI_HW_INTERFACE_PUBLIC + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; private: + bool stop_flag_ = false; bool is_active_ = false; - const uint8_t n_dof_ = 6; std::string rsi_ip_address_ = ""; int rsi_port_ = 0; - std::vector initial_joint_pos_ = std::vector(n_dof_, 0.0); - std::vector joint_pos_correction_deg_ = std::vector(n_dof_, 0.0); + std::vector hw_commands_; + std::vector hw_states_; + + // RSI related joint positions + std::vector initial_joint_pos_; + std::vector joint_pos_correction_deg_; uint64_t ipoc_ = 0; RSIState rsi_state_; @@ -81,7 +117,6 @@ class KukaHardwareInterface std::unique_ptr server_; std::string in_buffer_; std::string out_buffer_; - std::mutex m_; static constexpr double R2D = 180 / M_PI; static constexpr double D2R = M_PI / 180; diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp deleted file mode 100644 index 545ab0e5..00000000 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_control_node.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2022 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. - -#ifndef KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_NODE_HPP_ -#define KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_NODE_HPP_ - -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "kuka_rsi_hw_interface/kuka_hardware_interface.hpp" -#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" -#include "sensor_msgs/msg/joint_state.hpp" - -#define DEFAULT_N_DOF 6 - -namespace kuka_rsi_hw_interface -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class RobotControlNode : public kroshu_ros2_core::ROS2BaseLCNode -{ -public: - RobotControlNode(const std::string & node_name, const rclcpp::NodeOptions & options); - -private: - CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; - CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; - - std::unique_ptr kuka_rsi_hw_interface_; - - void commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg); - bool onRSIIPAddressChange(const std::string & rsi_ip_address); - bool onRSIPortAddressChange(int rsi_port); - bool onNDOFChange(uint8_t n_dof); - - void ControlLoop(); - - std::thread control_thread_; - - std::string rsi_ip_address_ = ""; - int rsi_port_ = 0; - uint8_t n_dof_ = DEFAULT_N_DOF; - std::vector controller_joint_names_; - - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - joint_state_publisher_; - rclcpp::Subscription::SharedPtr joint_command_subscription_; - rclcpp::CallbackGroup::SharedPtr cbg_; - sensor_msgs::msg::JointState::SharedPtr joint_command_msg_; - sensor_msgs::msg::JointState joint_state_msg_; - std::mutex m_; - std::condition_variable cv_; -}; - -} // namespace kuka_rsi_hw_interface -#endif // KUKA_RSI_HW_INTERFACE__ROBOT_CONTROL_NODE_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_manager_node.hpp b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_manager_node.hpp new file mode 100644 index 00000000..ac48280a --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/robot_manager_node.hpp @@ -0,0 +1,69 @@ +// Copyright 2023 Svastits Áron +// +// 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_RSI_HW_INTERFACE__ROBOT_MANAGER_NODE_HPP_ +#define KUKA_RSI_HW_INTERFACE__ROBOT_MANAGER_NODE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/client.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "communication_helpers/service_tools.hpp" +#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace kuka_rsi +{ +class RobotManagerNode : public kroshu_ros2_core::ROS2BaseLCNode +{ +public: + RobotManagerNode(); + ~RobotManagerNode() = default; + + CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; + +private: + bool onRobotModelChangeRequest(const std::string & robot_model); + + rclcpp::Client::SharedPtr + change_hardware_state_client_; + rclcpp::Client::SharedPtr + change_controller_state_client_; + rclcpp::CallbackGroup::SharedPtr cbg_; + + std::string robot_model_; + + std::shared_ptr> is_configured_pub_; + std_msgs::msg::Bool is_configured_msg_; +}; +} // namespace kuka_rsi + + +#endif // KUKA_RSI_HW_INTERFACE__ROBOT_MANAGER_NODE_HPP_ diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h index b4d19e3d..4be14a34 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h @@ -55,7 +55,7 @@ class RSICommand { TiXmlDocument doc; TiXmlElement * root = new TiXmlElement("Sen"); - root->SetAttribute("Type", "ImFree"); + root->SetAttribute("Type", "KROSHU"); TiXmlElement * el = new TiXmlElement("AK"); // Add string attribute el->SetAttribute("A1", std::to_string(joint_position_correction[0])); @@ -64,7 +64,7 @@ class RSICommand el->SetAttribute("A4", std::to_string(joint_position_correction[3])); el->SetAttribute("A5", std::to_string(joint_position_correction[4])); el->SetAttribute("A6", std::to_string(joint_position_correction[5])); - if (!stop) {root->LinkEndChild(el);} + root->LinkEndChild(el); el = new TiXmlElement("Stop"); el->LinkEndChild(new TiXmlText(std::to_string(static_cast(stop)))); diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h index 37947bd5..6ebdb4e7 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/udp_server.h @@ -57,6 +57,8 @@ #include #include +#include "rclcpp/rclcpp.hpp" + #define BUFSIZE 1024 class UDPServer @@ -66,7 +68,7 @@ class UDPServer : local_host_(host), local_port_(port), timeout_( false) { - printf("%s: %i\n", local_host_.c_str(), local_port_); + RCLCPP_INFO(rclcpp::get_logger("UDPServer"), "%s: %i", local_host_.c_str(), local_port_); sockfd_ = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd_ < 0) { throw std::runtime_error("Error opening socket: " + std::string(strerror(errno))); @@ -107,7 +109,7 @@ class UDPServer sockfd_, buffer.c_str(), buffer.size(), 0, (struct sockaddr *) &clientaddr_, clientlen_); if (bytes < 0) { - std::cout << "ERROR in sendto" << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in send"); } return bytes; @@ -135,7 +137,7 @@ class UDPServer bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); if (bytes < 0) { - std::cout << "ERROR in recvfrom" << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } } else { return 0; @@ -145,7 +147,7 @@ class UDPServer memset(buffer_, 0, BUFSIZE); bytes = recvfrom(sockfd_, buffer_, BUFSIZE, 0, (struct sockaddr *) &clientaddr_, &clientlen_); if (bytes < 0) { - std::cout << "ERROR in recvfrom" << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("UDPServer"), "Error in receive"); } } diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.h new file mode 100644 index 00000000..e008e7cc --- /dev/null +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_H_ +#define KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define KUKA_RSI_HW_INTERFACE_EXPORT __attribute__((dllexport)) +#define KUKA_RSI_HW_INTERFACE_IMPORT __attribute__((dllimport)) +#else +#define KUKA_RSI_HW_INTERFACE_EXPORT __declspec(dllexport) +#define KUKA_RSI_HW_INTERFACE_IMPORT __declspec(dllimport) +#endif +#ifdef KUKA_RSI_HW_INTERFACE_BUILDING_DLL +#define KUKA_RSI_HW_INTERFACE_PUBLIC KUKA_RSI_HW_INTERFACE_EXPORT +#else +#define KUKA_RSI_HW_INTERFACE_PUBLIC KUKA_RSI_HW_INTERFACE_IMPORT +#endif +#define KUKA_RSI_HW_INTERFACE_PUBLIC_TYPE KUKA_RSI_HW_INTERFACE_PUBLIC +#define KUKA_RSI_HW_INTERFACE_LOCAL +#else +#define KUKA_RSI_HW_INTERFACE_EXPORT __attribute__((visibility("default"))) +#define KUKA_RSI_HW_INTERFACE_IMPORT +#if __GNUC__ >= 4 +#define KUKA_RSI_HW_INTERFACE_PUBLIC __attribute__((visibility("default"))) +#define KUKA_RSI_HW_INTERFACE_LOCAL __attribute__((visibility("hidden"))) +#else +#define KUKA_RSI_HW_INTERFACE_PUBLIC +#define KUKA_RSI_HW_INTERFACE_LOCAL +#endif +#define KUKA_RSI_HW_INTERFACE_PUBLIC_TYPE +#endif + +#endif // KUKA_RSI_HW_INTERFACE__VISIBILITY_CONTROL_H_ diff --git a/kuka_rsi_hw_interface/krl/KR_C4/README_KRC4.md b/kuka_rsi_hw_interface/krl/KR_C4/README_KRC4.md deleted file mode 100644 index 0aba235f..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/README_KRC4.md +++ /dev/null @@ -1,115 +0,0 @@ -# Configuring RSI on the controller - -This guide highlights the steps needed in order to successfully configure the **RSI interface** on the controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS. - -## 1. Controller network configuration - -Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. - -1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). -2. There should already be an interface checked out as the **Windows interface**. For example: - * **IP**: 192.168.250.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 - * **Windows interface checkbox** should be checked. -3. Minimize the SmartHMI (**Start-up > Service > Minimize HMI**). -4. Run **RSI-Network** from the Windows Start menu (**All Programs > RSI-Network**). -5. Check that the **Network - Kuka User Interface** show the Windows interface with the specified IP address. -6. Add a new IP address on another subnet (e.g. 192.168.1.20) for the **RSI interface**. - * Select the entry **New** under **RSI Ethernet** in the tree structure and press **Edit**. - * Enter the IP address and confirm with **OK**. - * Close **RSI-Network** and maximize the SmartHMI. -7. Reboot the controller with a cold restart (**Shutdown > Check *Force cold start* and *Reload files* > Reboot control PC**). -8. After reboot, minimize the SmartHMI (**Start-up > Service > Minimize HMI**). -9. Run **cmd.exe** and ping the PC you want to communicate with on the same subnet (e.g. 192.168.250.xx). - -If your **PC** has an IP address on the same subnet as the **Windows interface** on the controller, the controller should receive answers from the PC: -* If this is the case, add another IP address to the current PC connection (e.g. 192.168.1.xx) on the same subnet as the **RSI** interface. - -## 2. KRL Files - -The files included in this folder specifies the data transferred via RSI. Some of the files needs to be modified to work for your specific configuration. - -##### ros_rsi_ethernet.xml -1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. -2. Keep the `PORT` tag as it is (49152) or change it if you want to use another port. - -Note that the `rsi/listen_address` and `rsi/listen_port` parameters of the `kuka_rsi_hw_interface` must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. - -##### ros_rsi.rsi.xml -This file may be edited with application specific joint limits in degrees. -* Edit the parameters within the RSIObject `AXISCORR` to specify joint limits such as **LowerLimA1**, **UpperLimA1** etc. Note that these limits are in reference to the start position of the robot. -* Edit the parameters within `AXISCORRMON` to specify the overall correction limitation. If this limit is exceeded in either of the joint directions, RSI is stopped. The values of **MaxA1**, **MaxA2** etc. may be large to allow free movement within the specified joint limits in `AXISCORR`. - -Notice the RSIObject of type `ETHERNET`. Within this object is a parameter called **Timeout**. This parameter is set to **100** by default. The RSI interface operates at `250 Hz` (4ms cycle). The **kuka_rsi_hardware_interface** does not have a period configured and is completely driven by the controller's output. Every controller RSI output has a IPOC timestamp which increments for every cycle. The **kuka_rsi_hardware_interface** will answer as quickly as possible. The answer includes the last IPOC received. If the connected **PC with ROS** did not manage to answer within the RSI cycle of **4ms**, the IPOC timestamp of RSI has incremented. The IPOC included in the answer is not matched and the controller will increment a counter. When this counter hits the **Timeout** parameter (**100**), the RSI connection will shut down. If this parameter is lowered, the demand for real-time computation will increase. - -If you have problems with the connection to RSI shutting down now and then while moving the robot it is suggested to: -* Compile and install a [RT-Preempt](https://rt.wiki.kernel.org/index.php/RT_PREEMPT_HOWTO) kernel for your PC. -* Give **kuka_rsi_hardware_interface** on your PC real-time priority when the RSI connection is established. - -##### ros_rsi.src -This should only be edited if the start position specified within the file is not desirable for your application. - -##### Copy files to controller -The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: - -1. Copy the files to a USB-stick. -2. Plug it into the teach pad or controller. -3. Log in as **Expert** or **Administrator**. -4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. -5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. - -## 3. Configure the kuka_rsi_hw_interface -The **kuka_rsi_hardware_interface** needs to be configured in order to successfully communicate with RSI. Inside `/kuka_rsi_hw_interface/test` and `/kuka_rsi_hw_interface/config` in this repository is a set of `*.yaml` files. These configuration files may be loaded into a launch-file used to start the **kuka_rsi_hardware_interface** with correct parameters, such as: - -* **Joint names** corresponding to the robot description (URDF or .xacro). -* **IP address** and **port** corresponding to the RSI setup specified in **ros_rsi_ethernet.xml**. - -We recommend that you copy the configuration files, edit the copies for your needs and use these files to create your own launch file. A template will be provided at a later time. However, at this time, have a look at `test_hardware_interface.launch`, `test_params.yaml`, `controller_joint_names.yaml` and `hardware_controllers.yaml` to achieve a working test-launch. - -In order to successfully launch the **kuka_rsi_hardware_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and .xacro to match your application): - -``` - -``` - -Make sure that the line is added before the `kuka_hardware_interface` itself is loaded. - -## 4. Testing -At this point you are ready to test the RSI interface. Before the test, make sure that: - -* You have specified the `rsi/listen_address` and `rsi/listen_port` of the **kuka_rsi_hardware_interface** to correspond with the KRL files on the controller. -* You have a launch-file loading the network parameters, robot description, kuka_hardware_interface, hardware controller and controller joint names. - -The next steps describe how to launch the test file: - -* In a new terminal: - -``` -$ roscore -``` - -* Open a new terminal and roslaunch the previously configured launch-file: - -``` -$ roslaunch kuka_rsi_hw_interface test_hardware_interface.launch sim:=false -``` - -The **kuka_rsi_hardware_interface** is now waiting for the robot controller to connect. Follow the next steps to connect RSI: - -1. On the teach pad, enter mode **T1** for testing purposes. -2. Navigate to `KRC:\R1\Program` and select `ros_rsi.src`. -3. Press and hold an enabling switch and the run/play-button. The robot will first move to the start position. - * A message like **Programmed path reached (BCO)** will be shown at the teach pad. -4. Press and hold again. The teach pad will post a warning **!!! Attention - Sensor correction goes active !!!**. -5. Confirm the warning and press and hold the buttons again. This time the terminal where **kuka_rsi_hardware_interface** is running should output **Got connection from robot**. The RSI connection is now up and running. -6. In a new terminal: - -``` -$ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller -``` - -Choose **controller manager ns** and **controller** and you should be able to move each robot joint. - -* Note that T1-mode limits the robot movement velocity and is intended for testing purposes. - diff --git a/kuka_rsi_hw_interface/krl/KR_C4/README_KRC5.md b/kuka_rsi_hw_interface/krl/KR_C4/README_KRC5.md deleted file mode 100644 index d92b78f0..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/README_KRC5.md +++ /dev/null @@ -1,121 +0,0 @@ -# Configuring RSI on the controller - -This guide highlights the steps needed in order to successfully configure the **RSI interface** on the KRC5 controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS. - -## 1. Controller network configuration - -Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. - -1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). -2. There should already be an interface checked out as the **Windows interface**. For example: - * **IP**: 192.168.250.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 or leave it empty - * **Windows interface checkbox** should be checked. -3. Add a new interface bz pressign the **Advanced** button and **New interface**. -4. Select **Mixed IP address** and keep the default **Receiving task: Target subnet** and **Real-time receiving Task: UDP** -5. Set the IP address to a different subnet then the **KLI interface**. For example: - * **IP**: 192.168.10.20 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: leave it empty - * **Windows interface checkbox** should NOT be checked -6. Save the changes and reboot the robot controller using the settings **Cold start** and **Reload files** - -## 2. Set fix IP in your PC -1. Set one IP in the subnet of the KLI interface, it is required to connect the WorkVisual and transfer project. For example: - * **IP**: 192.168.250.10 - * **Subnet mask**: 255.255.255.0 - * **Default gateway**: 192.168.250.20 or leave it empty -2. Add another IP in the subnet of the RSI interface, it is required to send commands via the RSI interface. For example: - * In windows **Network and Sharing center**, select the internet adapter settings *e.g. Ethernet Properties. - * Select the IPV4 properties and their the **Advanced** settings. - * In the new window, you can **Add** new IP addresses. - -## 2. KRL Files - -The files included in this folder specifies the data transferred via RSI. Some of the files needs to be modified to work for your specific configuration. - -##### ros_rsi_ethernet.xml -1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. -2. Keep the `PORT` tag as it is (59152) or change it if you want to use another port. - -Note that the `rsi/listen_address` and `rsi/listen_port` parameters of the `kuka_rsi_hw_interface` must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. - -##### ros_rsi.rsi.xml -This file may be edited with application specific joint limits in degrees. -* Edit the parameters within the RSIObject `AXISCORR` to specify joint limits such as **LowerLimA1**, **UpperLimA1** etc. Note that these limits are in reference to the start position of the robot. -* Edit the parameters within `AXISCORRMON` to specify the overall correction limitation. If this limit is exceeded in either of the joint directions, RSI is stopped. The values of **MaxA1**, **MaxA2** etc. may be large to allow free movement within the specified joint limits in `AXISCORR`. - -Notice the RSIObject of type `ETHERNET`. Within this object is a parameter called **Timeout**. This parameter is set to **100** by default. The RSI interface operates at `250 Hz` (4ms cycle). The **kuka_rsi_hardware_interface** does not have a period configured and is completely driven by the controller's output. Every controller RSI output has a IPOC timestamp which increments for every cycle. The **kuka_rsi_hardware_interface** will answer as quickly as possible. The answer includes the last IPOC received. If the connected **PC with ROS** did not manage to answer within the RSI cycle of **4ms**, the IPOC timestamp of RSI has incremented. The IPOC included in the answer is not matched and the controller will increment a counter. When this counter hits the **Timeout** parameter (**100**), the RSI connection will shut down. If this parameter is lowered, the demand for real-time computation will increase. - -If you have problems with the connection to RSI shutting down now and then while moving the robot it is suggested to: -* Compile and install a [RT-Preempt](https://rt.wiki.kernel.org/index.php/RT_PREEMPT_HOWTO) kernel for your PC. -* Give **kuka_rsi_hardware_interface** on your PC real-time priority when the RSI connection is established. - -##### ros_rsi.src -This should only be edited if the start position specified within the file is not desirable for your application. - -##### Copy files to controller -The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: - -Method 1: -1. Copy the files to a USB-stick. -2. Plug it into the teach pad or controller. -3. Log in as **Expert** or **Administrator**. -4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. -5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. - -Method 2: -1. Use the WorkVisual, connect to the KRC -2. Log in as **Expert** or **Administrator**. -4. Copy the `ros_rsi.src` file to `KRC:\R1\Program` in the WorkVisual -5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface` in the WorkVisual -6. Deploy the project, and follow the orders - -## 3. Configure the kuka_rsi_hw_interface -The **kuka_rsi_hardware_interface** needs to be configured in order to successfully communicate with RSI. Inside `/kuka_rsi_hw_interface/test` and `/kuka_rsi_hw_interface/config` in this repository is a set of `*.yaml` files. These configuration files may be loaded into a launch-file used to start the **kuka_rsi_hardware_interface** with correct parameters, such as: - -* **Joint names** corresponding to the robot description (URDF or .xacro). -* **IP address** and **port** corresponding to the RSI setup specified in **ros_rsi_ethernet.xml**. - -We recommend that you copy the configuration files, edit the copies for your needs and use these files to create your own launch file. A template will be provided at a later time. However, at this time, have a look at `test_hardware_interface.launch`, `test_params.yaml`, `controller_joint_names.yaml` and `hardware_controllers.yaml` to achieve a working test-launch. - -In order to successfully launch the **kuka_rsi_hardware_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and .xacro to match your application): - -``` - -``` - -Make sure that the line is added before the `kuka_hardware_interface` itself is loaded. - -## 4. Testing -At this point you are ready to test the RSI interface. Before the test, make sure that: - -* You have specified the `rsi/listen_address` and `rsi/listen_port` of the **kuka_rsi_hardware_interface** to correspond with the KRL files on the controller. -* You have a launch-file loading the network parameters, robot description, kuka_hardware_interface, hardware controller and controller joint names. -The next steps describe how to launch the test file: -* Open a new terminal and roslaunch the previously configured launch-file: - -``` - -$ roslaunch kuka_rsi_hw_interface test_hardware_interface.launch sim:=false - -``` - -The **kuka_rsi_hardware_interface** is now waiting for the robot controller to connect. Follow the next steps to connect RSI: -1. On the teach pad, enter mode **T1** for testing purposes. -2. Navigate to `KRC:\R1\Program` and select `ros_rsi.src`. -3. Press and hold an enabling switch and the run/play-button. The robot will first move to the start position. - * A message like **Programmed path reached (BCO)** will be shown at the teach pad. -4. Press and hold again. The teach pad will post a warning **!!! Attention - Sensor correction goes active !!!**. -5. Confirm the warning and press and hold the buttons again. This time the terminal where **kuka_rsi_hardware_interface** is running should output **Got connection from robot**. The RSI connection is now up and running. -6. In a new terminal: - -``` - -$ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller - -``` - -Choose **controller manager ns** and **controller** and you should be able to move each robot joint. -* Note that T1-mode limits the robot movement velocity and is intended for testing purposes. \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi deleted file mode 100644 index 25bcf984..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi +++ /dev/null @@ -1,112 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram deleted file mode 100644 index 6386e420..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.diagram +++ /dev/null @@ -1,198 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml b/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml deleted file mode 100644 index 30f5715c..00000000 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.rsi.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/README_KRC4.md b/kuka_rsi_hw_interface/krl/README_KRC4.md new file mode 100644 index 00000000..d4d73191 --- /dev/null +++ b/kuka_rsi_hw_interface/krl/README_KRC4.md @@ -0,0 +1,74 @@ +# Configuring RSI on the controller + +This tutorial was tested with RSI 4.1.3 (on KSS8.6) + +This guide highlights the steps needed in order to successfully configure the **RSI interface** on the controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS2. + +## 1. Controller network configuration + +Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. + +1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). +2. There should already be an interface checked out as the **Windows interface**. For example: + * **IP**: 192.168.250.20 + * **Subnet mask**: 255.255.255.0 + * **Default gateway**: 192.168.250.20 + * **Windows interface checkbox** should be checked. +3. Minimize the SmartHMI (**Start-up > Service > Minimize HMI**). +4. Run **RSI-Network** from the Windows Start menu (**All Programs > RSI-Network**). +5. Check that the **Network - Kuka User Interface** show the Windows interface with the specified IP address. +6. Add a new IP address on another subnet (e.g. 192.168.1.20) for the **RSI interface**. + * Select the entry **New** under **RSI Ethernet** in the tree structure and press **Edit**. + * Enter the IP address and confirm with **OK**. + * Close **RSI-Network** and maximize the SmartHMI. +7. Reboot the controller with a cold restart (**Shutdown > Check *Force cold start* and *Reload files* > Reboot control PC**). +8. After reboot, minimize the SmartHMI (**Start-up > Service > Minimize HMI**). +9. Run **cmd.exe** and ping the PC you want to communicate with on the same subnet (e.g. 192.168.250.xx). + +If your **PC** has an IP address on the same subnet as the **Windows interface** on the controller, the controller should receive answers from the PC: +* If this is the case, add another IP address to the current PC connection (e.g. 192.168.1.xx) on the same subnet as the **RSI** interface. + +## 2. KRL Files + +The files included in this folder specifies the data transferred via RSI. Some of the files needs to be modified to work for your specific configuration. + +##### ros_rsi_ethernet.xml +1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. +2. Keep the `PORT` tag as it is (59152) or change it if you want to use another port. + +Note that the `client_ip` and `client_port` parameters in config/rsi_config.yaml must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. + +##### ros_rsi.src +This should only be edited if the start position specified within the file is not desirable for your application. + +##### Copy files to controller +The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: + +1. Copy the files to a USB-stick. +2. Plug it into the teach pad or controller. +3. Log in as **Expert** or **Administrator**. +4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. +5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. + +## 3. Testing +At this point you are ready to test the RSI interface. Before the test, make sure that: + +* You have specified the `client_ip` and `client_port` tags in the configuration file (config/rsi_config.yaml) to correspond with the KRL files on the controller. +* This IP address is available on the client machine (see Network configuration) + +The next steps describe how to start external control using RSI: + +* Start the driver: ```ros2 launch kuka_rsi_hw_interface startup.launch.py``` + +* In a new terminal: ```ros2 lifecycle set robot_manager configure``` + +* Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and move to the step before RSI_MOVECORR() is run + * in T1, a warning (!!! Attention - Sensor correction goes active !!!) should be visible after reaching RSI_MOVECORR(), which is also okay +* Activate driver and controllers: ```ros2 lifecycle set robot_manager activate``` + * The hardware interface is now waiting for the robot controller to connect, the timeout for this is currently 2 seconds +* Start step RSI_MOVECORR() withing the given timeout + * in T1 this can be done with confirming the previously described warning + * This time the terminal where the driver is running should output **Got connection from robot**. The RSI connection is now up and running. + +After this, starting an rqt joint trajectory controller (```ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller```) should enable moving the robot with sliders +- This sends the trajectory in batches, which can result in a little jerky movement, so that is not a bug of the driver diff --git a/kuka_rsi_hw_interface/krl/README_KRC5.md b/kuka_rsi_hw_interface/krl/README_KRC5.md new file mode 100644 index 00000000..b3ff5066 --- /dev/null +++ b/kuka_rsi_hw_interface/krl/README_KRC5.md @@ -0,0 +1,87 @@ +# Configuring RSI on the controller + +This tutorial was tested with RSI 5.0.2 (on KSS8.7) + +This guide highlights the steps needed in order to successfully configure the **RSI interface** on the KRC5 controller to work with the **kuka_rsi_hardware_interface** on your PC with ROS. + +## 1. Controller network configuration + +Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet. + +1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**). +2. There should already be an interface checked out as the **Windows interface**. For example: + * **IP**: 192.168.250.20 + * **Subnet mask**: 255.255.255.0 + * **Default gateway**: 192.168.250.20 or leave it empty + * **Windows interface checkbox** should be checked. +3. Add a new interface bz pressign the **Advanced** button and **New interface**. +4. Select **Mixed IP address** and keep the default **Receiving task: Target subnet** and **Real-time receiving Task: UDP** +5. Set the IP address to a different subnet then the **KLI interface**. For example: + * **IP**: 192.168.10.20 + * **Subnet mask**: 255.255.255.0 + * **Default gateway**: leave it empty + * **Windows interface checkbox** should NOT be checked +6. Save the changes and reboot the robot controller using the settings **Cold start** and **Reload files** + +## 2. Set fix IP in your PC +1. Set one IP in the subnet of the KLI interface, it is required to connect the WorkVisual and transfer project. For example: + * **IP**: 192.168.250.10 + * **Subnet mask**: 255.255.255.0 + * **Default gateway**: 192.168.250.20 or leave it empty +2. Add another IP in the subnet of the RSI interface, it is required to send commands via the RSI interface. For example: + * In windows **Network and Sharing center**, select the internet adapter settings *e.g. Ethernet Properties. + * Select the IPV4 properties and their the **Advanced** settings. + * In the new window, you can **Add** new IP addresses. + +## 3. KRL Files + +The files included in this folder specifies the data transferred via RSI. Some of the files needs to be modified to work for your specific configuration. + +##### ros_rsi_ethernet.xml +1. Edit the `IP_NUMBER` tag so that it corresponds to the IP address (192.168.1.xx) previously added for your PC. +2. Keep the `PORT` tag as it is (59152) or change it if you want to use another port. + +Note that the `client_ip` and `client_port` parameters in config/rsi_config.yaml must correspond to the `IP_NUMBER`and `PORT` set in these KRL files. + +##### ros_rsi.src +This should only be edited if the start position specified within the file is not desirable for your application. + +##### Copy files to controller +The files **ros_rsi.rsi** and **ros_rsi.rsi.diagram** should not be edited. All files are now ready to be copied to the Kuka controller: + +Method 1: +1. Copy the files to a USB-stick. +2. Plug it into the teach pad or controller. +3. Log in as **Expert** or **Administrator**. +4. Copy the `ros_rsi.src` file to `KRC:\R1\Program`. +5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface`. + +Method 2: +1. Use the WorkVisual, connect to the KRC +2. Log in as **Expert** or **Administrator**. +4. Copy the `ros_rsi.src` file to `KRC:\R1\Program` in the WorkVisual +5. Copy the rest of the files to `C:\KRC\ROBOTER\Config\User\Common\SensorInterface` in the WorkVisual +6. Deploy the project, and follow the orders + +## 4. Testing +At this point you are ready to test the RSI interface. Before the test, make sure that: + +* You have specified the `client_ip` and `client_port` tags in the configuration file (config/rsi_config.yaml) to correspond with the KRL files on the controller. +* This IP address is available on the client machine (see Network configuration) + +The next steps describe how to start external control using RSI: + +* Start the driver: ```ros2 launch kuka_rsi_hw_interface startup.launch.py``` + +* In a new terminal: ```ros2 lifecycle set robot_manager configure``` + +* Start the `KRC:\R1\Program\ros_rsi.src` program on the controller and move to the step before RSI_MOVECORR() is run + * in T1, a warning (!!! Attention - Sensor correction goes active !!!) should be visible after reaching RSI_MOVECORR(), which is also okay +* Activate driver and controllers: ```ros2 lifecycle set robot_manager activate``` + * The hardware interface is now waiting for the robot controller to connect, the timeout for this is currently 2 seconds +* Start step RSI_MOVECORR() withing the given timeout + * in T1 this can be done with confirming the previously described warning + * This time the terminal where the driver is running should output **Got connection from robot**. The RSI connection is now up and running. + +After this, starting an rqt joint trajectory controller (```ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller```) should enable moving the robot with sliders +- This sends the trajectory in batches, which can result in a little jerky movement, so that is not a bug of the driver diff --git a/kuka_rsi_hw_interface/krl/ros_rsi.rsix b/kuka_rsi_hw_interface/krl/ros_rsi.rsix new file mode 100755 index 00000000..4e95c079 --- /dev/null +++ b/kuka_rsi_hw_interface/krl/ros_rsi.rsix @@ -0,0 +1,2437 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Tooltip_COMMENT + + + + + + Tooltip_FunctionBlockInput + + + + + + Tooltip_FunctionBlockInOrder + + + + + Tooltip_FunctionBlockOutput + + + + + + Tooltip_FunctionBlockOutOrder + + + + + + + Tooltip_ANIN + + + Tooltip_ANIN_Out1 + + + + + Tooltip_ANIN_Index + + + + + Tooltip_DIGIN + + + + + + + Remarks_DIGIN_Index + Tooltip_DIGIN_Index + + + + Tooltip_DIGIN_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + Tooltip_ANOUT + + + Tooltip_ANOUT_Out1 + + + + + Tooltip_ANOUT_Index + + + + + Tooltip_DIGOUT + + + + + + + Remarks_DIGOUT_Index + Tooltip_DIGOUT_Index + + + + Tooltip_DIGOUT_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + + Tooltip_ETHERNET + Summary_ETHERNET + + + + + + + + + + Tooltip_ETHERNET_ConfigFile + + + Tooltip_ETHERNET_Timeout + + + Tooltip_ETHERNET_Flag + + + + Remarks_ETHERNET_Precision + Tooltip_ETHERNET_Precision + + + + + + Tooltip_INPUT + + + Tooltip_INPUT_Port + + + + + Tooltip_INPUT_Index + + + Tooltip_INPUT_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + Tooltip_MAP2ANOUT + + + Tooltip_MAP2ANOUT_In1 + + + + + Tooltip_MAP2ANOUT_Index + + + + + Tooltip_MAP2DIGOUT + + + + + + + Remarks_MAP2DIGOUT_Index + Tooltip_MAP2DIGOUT_Index + + + + Tooltip_MAP2DIGOUT_DataType + + Bit + Byte + Int16 + + + + + + Tooltip_MAP2SEN_PINT + + + Tooltip_MAP2SEN_PINT_In1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_MAP2SEN_PINT_Index + + + + + + Tooltip_MAP2SEN_PREA + + + Tooltip_MAP2SEN_PREA_In1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_MAP2SEN_PREA_Index + + + + + + Tooltip_MONITOR + + + + + + + Tooltip_MONITOR_Refresh + + + Tooltip_MONITOR_Timeout + + + Tooltip_MONITOR_ReqTimeZero + + + Tooltip_MONITOR_IP + + + + Remarks_MONITOR_Channel + Tooltip_MONITOR_Channel + + + + + + Tooltip_OUTPUT + + + Tooltip_OUTPUT_Port + + + + + Tooltip_OUTPUT_Index + + + Tooltip_OUTPUT_DataType + + Bit + Byte + UByte + Int16 + UInt16 + Int32 + UInt32 + Float + + + + + + Tooltip_RESETDIGOUT + + + + + + Tooltip_RESETDIGOUT_Index + + + + + Tooltip_SEN_PINT + + + Tooltip_SEN_PINT_Out1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_SEN_PINT_Index + + + + + + Tooltip_SEN_PREA + + + Tooltip_SEN_PREA_Out1 + + + + + + Remark_SenPint_SenPrea_Index + Tooltip_SEN_PREA_Index + + + + + + Tooltip_SETDIGOUT + + + + + + Tooltip_SETDIGOUT_Index + + + + + + + + Tooltip_AND + Summary_AND + + + + + + + + + Tooltip_AND_Out1 + + + + + Tooltip_BAND + + + + + + + + + + Tooltip_BAND_DefVal + + + + + Tooltip_BCOMPL + + + + + + + + + Tooltip_BOR + + + + + + + + + + Tooltip_BOR_DefVal + + + + + Tooltip_NOT + + + + + + + + + Tooltip_OR + + + + + + + + + + + Tooltip_SIGNALSWITCH + + + Tooltip_SIGNALSWITCH_Ctrl + + + + + + + + + + + + Remarks_XOR + Tooltip_XOR + + + + + + + + + + + + + Remarks_RS + Tooltip_RS + + + + Tooltip_RS_Set + + + Tooltip_RS_Reset + + + + + + Remarks_RS_Out1 + Tooltip_RS_Out1 + rs_remarks.png + + + + + + + Remarks_SR + Tooltip_SR + + + + Tooltip_SR_Set + + + Tooltip_SR_Reset + + + + + + Remarks_SR_Out1 + Tooltip_SR_Out1 + sr_remarks.png + + + + + + + + Tooltip_ABS + + + Tooltip_ABS_In1 + + + + + Tooltip_ABS_Out1 + + + + + Tooltip_ACOS + + + Tooltip_ACOS_In1 + + + + + Tooltip_ACOS_Out1 + + + + + Tooltip_ASIN + + + Tooltip_ASIN_In1 + + + + + Tooltip_ASIN_Out1 + + + + + Tooltip_ATAN + + + + + + Tooltip_ATAN_Out1 + + + + + Tooltip_ATAN2 + + + + + + + Tooltip_ATAN2_Out1 + + + + + Tooltip_CEIL + + + + + + + + + Tooltip_COS + + + Tooltip_COS_In1 + + + + + Tooltip_COS_Out1 + + + + + Tooltip_Division + + + Tooltip_Division_Dividend + + + Tooltip_Division_Divisor + + + + + Tooltip_Division_Quotient + + + + + Tooltip_EQUAL + + + + + + + + + + Tooltip_EQUAL_CompVal + + + Tooltip_EQUAL_Tolerance + + + + + Tooltip_EXP + + + Tooltip_EXP_In1 + + + + + Tooltip_EXP_Out1 + + + + + Tooltip_FLOOR + + + + + + + + + Tooltip_GREATER + + + + + + + Tooltip_GREATER_Out1 + + + + + Tooltip_GREATER_CompVal + + + + Remarks_GREATER_Hysteresis + Tooltip_GREATER_Hysteresis + + + + + + Tooltip_LESS + + + + + + + + + + Tooltip_LESS_CompVal + + + + Remarks_LESS_Hysteresis + Tooltip_LESS_Hysteresis + + + + + + Tooltip_LIMIT + + + + + + + + + Tooltip_LIMIT_LowerLimit + + + Tooltip_LIMIT_UpperLimit + + + + + Tooltip_LOG + + + + + + Tooltip_LOG_Out1 + + + + + Tooltip_MINMAX + + + + + + + + Tooltip_MINMAX_Min + + + Tooltip_MINMAX_Max + + + + + Tooltip_MULTI + + + + + + + Tooltip_MULTI_Out1 + + + + + Tooltip_NORM + + + + + + + + + Remarks_NORM_Out1 + Tooltip_NORM_Out1 + + + + + + Tooltip_POW + + + Tooltip_POW_In1 + + + + + Tooltip_POW_Out1 + + + + + Tooltip_POW_Exp + + + + + Tooltip_ROUND + + + + + + Tooltip_ROUND_Out1 + + + + + Tooltip_ROUND_nDig + + + + + Tooltip_SIN + + + Tooltip_SIN_In1 + + + + + Tooltip_SIN_Out1 + + + + + Tooltip_SUB + + + Tooltip_SUB_Minu + + + Tooltip_SUB_Sub1 + + + + + Tooltip_SUB_Diff + + + + + Tooltip_SUB_Value + + + + + Tooltip_SUM + + + + + + + Tooltip_SUM_Sum + + + + + Tooltip_SUM_Value + + + + + Tooltip_TAN + + + Tooltip_TAN_In1 + + + + + Tooltip_TAN_Out1 + + + + + + Remarks_TRAFO_ROBFRAME + Tooltip_TRAFO_ROBFRAME + + + + Tooltip_TRAFO_ROBFRAME_In1 + + + Tooltip_TRAFO_ROBFRAME_In2 + + + Tooltip_TRAFO_ROBFRAME_In3 + + + + + Tooltip_TRAFO_ROBFRAME_Out1 + + + Tooltip_TRAFO_ROBFRAME_Out2 + + + Tooltip_TRAFO_ROBFRAME_Out3 + + + + + Tooltip_TRAFO_ROBFRAME_Type + + RotTrans + OnlyRot + EulerAngles + + + + Tooltip_TRAFO_ROBFRAME_Source + + World + Base + RobRoot + Tool + TTS + + + + Tooltip_TRAFO_ROBFRAME_Target + + World + Base + RobRoot + Tool + TTS + + + + + + Tooltip_TRAFO_USERFRAME + + + + + + + + + + + + + Tooltip_TRAFO_USERFRAME_Type + + RotTrans + OnlyRot + EulerAngles + + + + Tooltip_TRAFO_USERFRAME_FraX + + + Tooltip_TRAFO_USERFRAME_FraY + + + Tooltip_TRAFO_USERFRAME_FraZ + + + Tooltip_TRAFO_USERFRAME_FraA + + + Tooltip_TRAFO_USERFRAME_FraB + + + Tooltip_TRAFO_USERFRAME_FraC + + + + + + + + Remarks_AXISCORR + Tooltip_AXISCORR + + + + Tooltip_AXISCORR_CorrA1 + + + Tooltip_AXISCORR_CorrA2 + + + Tooltip_AXISCORR_CorrA3 + + + Tooltip_AXISCORR_CorrA4 + + + Tooltip_AXISCORR_CorrA5 + + + Tooltip_AXISCORR_CorrA6 + + + + + + Remarks_AXISCORR_Stat + Tooltip_AXISCORR_Stat + + + + Tooltip_AXISCORR_A1 + + + Tooltip_AXISCORR_A2 + + + Tooltip_AXISCORR_A3 + + + Tooltip_AXISCORR_A4 + + + Tooltip_AXISCORR_A5 + + + Tooltip_AXISCORR_A6 + + + + + Tooltip_AXISCORR_LowerLimA1 + + + Tooltip_AXISCORR_LowerLimA2 + + + Tooltip_AXISCORR_LowerLimA3 + + + Tooltip_AXISCORR_LowerLimA4 + + + Tooltip_AXISCORR_LowerLimA5 + + + Tooltip_AXISCORR_LowerLimA6 + + + Tooltip_AXISCORR_UpperLimA1 + + + Tooltip_AXISCORR_UpperLimA2 + + + Tooltip_AXISCORR_UpperLimA3 + + + Tooltip_AXISCORR_UpperLimA4 + + + Tooltip_AXISCORR_UpperLimA5 + + + Tooltip_AXISCORR_UpperLimA6 + + + + + + Remarks_AXISCORREXT + Tooltip_AXISCORREXT + + + + Tooltip_AXISCORREXT_CorrE1 + + + Tooltip_AXISCORREXT_CorrE2 + + + Tooltip_AXISCORREXT_CorrE3 + + + Tooltip_AXISCORREXT_CorrE4 + + + Tooltip_AXISCORREXT_CorrE5 + + + Tooltip_AXISCORREXT_CorrE6 + + + + + + Remarks_AXISCORREXT_Stat + Tooltip_AXISCORREXT_Stat + + + + Tooltip_AXISCORREXT_E1 + + + Tooltip_AXISCORREXT_E2 + + + Tooltip_AXISCORREXT_E3 + + + Tooltip_AXISCORREXT_E4 + + + Tooltip_AXISCORREXT_E5 + + + Tooltip_AXISCORREXT_E6 + + + + + Tooltip_AXISCORREXT_LowerLimE1 + + + Tooltip_AXISCORREXT_LowerLimE2 + + + Tooltip_AXISCORREXT_LowerLimE3 + + + Tooltip_AXISCORREXT_LowerLimE4 + + + Tooltip_AXISCORREXT_LowerLimE5 + + + Tooltip_AXISCORREXT_LowerLimE6 + + + Tooltip_AXISCORREXT_UpperLimE1 + + + Tooltip_AXISCORREXT_UpperLimE2 + + + Tooltip_AXISCORREXT_UpperLimE3 + + + Tooltip_AXISCORREXT_UpperLimE4 + + + Tooltip_AXISCORREXT_UpperLimE5 + + + Tooltip_AXISCORREXT_UpperLimE6 + + + + + Tooltip_AXISCORRMON + + + Tooltip_AXISCORRMON_A1 + + + Tooltip_AXISCORRMON_A2 + + + Tooltip_AXISCORRMON_A3 + + + Tooltip_AXISCORRMON_A4 + + + Tooltip_AXISCORRMON_A5 + + + Tooltip_AXISCORRMON_A6 + + + Tooltip_AXISCORRMON_E1 + + + Tooltip_AXISCORRMON_E2 + + + Tooltip_AXISCORRMON_E3 + + + Tooltip_AXISCORRMON_E4 + + + Tooltip_AXISCORRMON_E5 + + + Tooltip_AXISCORRMON_E6 + + + + + Tooltip_AXISCORRMON_MaxA1 + + + Tooltip_AXISCORRMON_MaxA2 + + + Tooltip_AXISCORRMON_MaxA3 + + + Tooltip_AXISCORRMON_MaxA4 + + + Tooltip_AXISCORRMON_MaxA5 + + + Tooltip_AXISCORRMON_MaxA6 + + + Tooltip_AXISCORRMON_MaxE1 + + + Tooltip_AXISCORRMON_MaxE2 + + + Tooltip_AXISCORRMON_MaxE3 + + + Tooltip_AXISCORRMON_MaxE4 + + + Tooltip_AXISCORRMON_MaxE5 + + + Tooltip_AXISCORRMON_MaxE6 + + + + + Tooltip_MAP2OV_PRO + + + Tooltip_MAP2OV_PRO_In1 + + + + + + Remarks_POSCORR + Tooltip_POSCORR + + + + Tooltip_POSCORR_CorrX + + + Tooltip_POSCORR_CorrY + + + Tooltip_POSCORR_CorrZ + + + Tooltip_POSCORR_CorrA + + + Tooltip_POSCORR_CorrB + + + Tooltip_POSCORR_CorrC + + + + + + Remarks_POSCORR_Stat + Tooltip_POSCORR_Stat + + + + Tooltip_POSCORR_X + + + Tooltip_POSCORR_Y + + + Tooltip_POSCORR_Z + + + Tooltip_POSCORR_A + + + Tooltip_POSCORR_B + + + Tooltip_POSCORR_C + + + + + Tooltip_POSCORR_LowerLimX + + + Tooltip_POSCORR_LowerLimY + + + Tooltip_POSCORR_LowerLimZ + + + Tooltip_POSCORR_UpperLimX + + + Tooltip_POSCORR_UpperLimY + + + Tooltip_POSCORR_UpperLimZ + + + Tooltip_POSCORR_MaxRotAngle + + + Tooltip_POSCORR_LastCorrStat + + + Tooltip_POSCORR_LastCorrX + + + Tooltip_POSCORR_LastCorrY + + + Tooltip_POSCORR_LastCorrZ + + + Tooltip_POSCORR_LastCorrA + + + Tooltip_POSCORR_LastCorrB + + + Tooltip_POSCORR_LastCorrC + + + Tooltip_POSCORR_RefCorrSys + + World + Base + RobRoot + Tool + TTS + + + + + + Tooltip_POSCORRMON + + + Tooltip_POSCORRMON_X + + + Tooltip_POSCORRMON_Y + + + Tooltip_POSCORRMON_Z + + + Tooltip_POSCORRMON_A + + + Tooltip_POSCORRMON_B + + + Tooltip_POSCORRMON_C + + + + + + Remark_ParamNoChangeDuringRuntime + Tooltip_POSCORRMON_MaxTrans + + + + + Remark_ParamNoChangeDuringRuntime + Tooltip_POSCORRMON_MaxRotAngle + + + + + + + Remarks_STOP + Tooltip_STOP + + + + + + + Tooltip_STOP_Mode + + InfoMessage + PathNormal + Velocity + PathFast + ExitMoveCorr + + + + + Remarks_STOP_Channel + Tooltip_STOP_Channel + + + + + + + + Tooltip_AXISACT + + + Tooltip_AXISACT_A1 + + + Tooltip_AXISACT_A2 + + + Tooltip_AXISACT_A3 + + + Tooltip_AXISACT_A4 + + + Tooltip_AXISACT_A5 + + + Tooltip_AXISACT_A6 + + + + + + Remarks_AXISACT_Type + Tooltip_POSACT_AXISACT_Type + axisact_remarks.png + + + Measured + IPO + IPO_FLT + CF + + + + + + Tooltip_AXISACTEXT + + + Tooltip_AXISACTEXT_E1 + + + Tooltip_AXISACTEXT_E2 + + + Tooltip_AXISACTEXT_E3 + + + Tooltip_AXISACTEXT_E4 + + + Tooltip_AXISACTEXT_E5 + + + Tooltip_AXISACTEXT_E6 + + + + + + Remarks_AXISACT_Type + Tooltip_POSACT_AXISACT_Type + axisact_remarks.png + + + Measured + IPO + IPO_FLT + CF + + + + + + Tooltip_GEARTORQUE + + + Tooltip_GEARTORQUE_A1 + + + Tooltip_GEARTORQUE_A2 + + + Tooltip_GEARTORQUE_A3 + + + Tooltip_GEARTORQUE_A4 + + + Tooltip_GEARTORQUE_A5 + + + Tooltip_GEARTORQUE_A6 + + + + + Tooltip_GEARTORQUE_TorqueSource + + Measured + Setpoint + + + + Tooltip_GEARTORQUE_LocationOnJoint + + Gear + Drive + + + + + + Tooltip_GEARTORQUEEXT + + + Tooltip_GEARTORQUEEXT_E1 + + + Tooltip_GEARTORQUEEXT_E2 + + + Tooltip_GEARTORQUEEXT_E3 + + + Tooltip_GEARTORQUEEXT_E4 + + + Tooltip_GEARTORQUEEXT_E5 + + + Tooltip_GEARTORQUEEXT_E6 + + + + + Tooltip_GEARTORQUE_TorqueSource + + Measured + Setpoint + + + + Tooltip_GEARTORQUE_LocationOnJoint + + Gear + Drive + + + + + + Tooltip_MOTORCURRENT + + + Tooltip_MOTORCURRENT_A1 + + + Tooltip_MOTORCURRENT_A2 + + + Tooltip_MOTORCURRENT_A3 + + + Tooltip_MOTORCURRENT_A4 + + + Tooltip_MOTORCURRENT_A5 + + + Tooltip_MOTORCURRENT_A6 + + + + + Tooltip_MOTORCURRENTEXT + + + Tooltip_MOTORCURRENTEXT_E1 + + + Tooltip_MOTORCURRENTEXT_E2 + + + Tooltip_MOTORCURRENTEXT_E3 + + + Tooltip_MOTORCURRENTEXT_E4 + + + Tooltip_MOTORCURRENTEXT_E5 + + + Tooltip_MOTORCURRENTEXT_E6 + + + + + Tooltip_OV_PRO + + + Tooltip_OV_PRO_Out1 + + + + + Tooltip_POSACT + + + Tooltip_POSACT_X + + + Tooltip_POSACT_Y + + + Tooltip_POSACT_Z + + + Tooltip_POSACT_A + + + Tooltip_POSACT_B + + + Tooltip_POSACT_C + + + Tooltip_POSACT_S + + + Tooltip_POSACT_T + + + + + + Remarks_POSACT_Type + Tooltip_POSACT_AXISACT_Type + posact_remarks.png + + + Measured + IPO + IPO_FLT + CF + + + + + + Tooltip_STATUS + + + Tooltip_STATUS_Stat + + + + + + Remarks_STATUS_Type + Tooltip_STATUS_Type + + + IPO_State + ProState_S + ProState_R + Pro_Mode_S + Pro_Mode_R + Mode_Op + IPO_Mode + IPO_Mode_C + Sensor + + + + + + + + Tooltip_D + + + + + + + + + Tooltip_D_KD + + + + + Tooltip_DELAY + + + + + + Tooltip_DELAY_Out1 + + + + + Tooltip_DELAY_DelayTime + + + + + Tooltip_GENCTRL + + + + + + + + + Tooltip_GENCTRL_Reset + + + Tooltip_GENCTRL_B0 + + + Tooltip_GENCTRL_A1 + + + Tooltip_GENCTRL_B1 + + + Tooltip_GENCTRL_A2 + + + Tooltip_GENCTRL_B2 + + + Tooltip_GENCTRL_A3 + + + Tooltip_GENCTRL_B3 + + + Tooltip_GENCTRL_A4 + + + Tooltip_GENCTRL_B4 + + + Tooltip_GENCTRL_A5 + + + Tooltip_GENCTRL_B5 + + + Tooltip_GENCTRL_A6 + + + Tooltip_GENCTRL_B6 + + + Tooltip_GENCTRL_A7 + + + Tooltip_GENCTRL_B7 + + + Tooltip_GENCTRL_A8 + + + Tooltip_GENCTRL_B8 + + + + + Tooltip_I + + + Tooltip_I_In1 + + + + + Tooltip_I_Out1 + + + + + Tooltip_I_Reset + + + Tooltip_I_TI + + + Tooltip_I_Type + + Always + OnCorrActive + + + + Tooltip_I_LowerLimit + + + Tooltip_I_UpperLimit + + + + + Tooltip_IIRFILTER + + + Tooltip_IIRFILTER_In1 + + + + + Tooltip_IIRFILTER_Out1 + + + + + Tooltip_IIRFILTER_Reset + + + Tooltip_IIRFILTER_Type + + Lowpass + Highpass + + + + Tooltip_IIRFILTER_Name + + Bessel + Butterworth + Tschebyscheff_0_5db + Tschebyscheff_1db + Tschebyscheff_2db + Tschebyscheff_3db + + + + Tooltip_IIRFILTER_Order + + O2 + O4 + O6 + O8 + O10 + + + + Tooltip_IIRFILTER_Cuttoff + + + + + Tooltip_P + + + + + + + + + Tooltip_P_KR + + + + + Tooltip_PD + + + + + + + + + Tooltip_PD_KR + + + Tooltip_PD_TV + + + + + Tooltip_PID + + + + + + + + + Tooltip_PID_Reset + + + Tooltip_PID_KR + + + + Remarks_PID_TN + Tooltip_PID_TN + + + + Tooltip_PID_TV + + + + Remarks_PID_LimLow + Tooltip_PID_LimLow + + + + + Remarks_PID_LimUpp + Tooltip_PID_LimUpp + + + + + + Tooltip_PT1 + + + + + + + + + Tooltip_PT1_KR + + + Tooltip_PT1_T1 + + + + + Tooltip_PT2 + + + + + + + + + Tooltip_PT2_KR + + + Tooltip_PT2_T1 + + + Tooltip_PT2_T2 + + + + + + Remarks_SOURCE + Tooltip_SOURCE_2 + Tooltip_SOURCE + Source.png + + + + + + + + Remarks_SOURCE_Type + Tooltip_SOURCE_Type + + + Const + Sin + Cos + Square + Sawtooth + + + + Tooltip_SOURCE_Offset + + + + Remarks_SOURCE_Amplitude + Tooltip_SOURCE_Amplitude + + + + + Remarks_SOURCE_Period + Tooltip_SOURCE_Period + + + + + + Tooltip_TIMER + + + Tooltip_TIMER_Ctrl + + + Tooltip_TIMER_InReset + + + + + Tooltip_TIMER_Out1 + + + + + Tooltip_TIMER_Reset + + + Tooltip_TIMER_Time + + + + + Tooltip_Constant + + + + + + Tooltip_Constant_Value + + + + + + Remarks_SINE + Summary_SINE + Sine.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_SINE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_COSINE + Summary_COSINE + Cosine.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_COSINE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_SAWTOOTH + Summary_SAWTOOTH + Sawtooth.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_SAWTOOTH_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_TRIANGLE + Summary_TRIANGLE + Triangle.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_TRIANGLE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + Remarks_RECTANGLE + Summary_RECTANGLE + Rectangle.png + + + + + Remarks_PeriodicSignalBase_Active + Summary_PeriodicSignalBase_Active + + + + + Remarks_PeriodicSignalBase_Reset + Summary_PeriodicSignalBase_Reset + + + + + Remarks_PeriodicSignalBase_InAmp + Summary_PeriodicSignalBase_Amp + + + + + Remarks_PeriodicSignalBase_InFreq + Summary_PeriodicSignalBase_Freq + + + + + + Summary_RECTANGLE_Signal + + + + + Summary_PeriodicSignalBase_Amp + + + + Remarks_PeriodicSignalBase_ParamFreq + Summary_PeriodicSignalBase_Freq + + + + + Remarks_PeriodicSignalBase_Phase + Summary_PeriodicSignalBase_Phase + + + + Summary_PeriodicSignalBase_Offset + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src b/kuka_rsi_hw_interface/krl/ros_rsi.src similarity index 94% rename from kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src rename to kuka_rsi_hw_interface/krl/ros_rsi.src index 9df9f6d6..45885d34 100644 --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi.src +++ b/kuka_rsi_hw_interface/krl/ros_rsi.src @@ -65,11 +65,8 @@ DECL INT CONTID ; ContainerID ;ENDFOLD (USER INI) ;ENDFOLD (INI) -; Move to start position -PTP {A1 0, A2 -90, A3 90, A4 0, A5 90, A6 0} - ; Create RSI Context -ret = RSI_CREATE("ros_rsi.rsi",CONTID,TRUE) +ret = RSI_CREATE("ros_rsi",CONTID,TRUE) IF (ret <> RSIOK) THEN HALT ENDIF @@ -88,7 +85,4 @@ ret = RSI_OFF() IF (ret <> RSIOK) THEN HALT ENDIF - -PTP {A1 0, A2 -90, A3 90, A4 0, A5 90, A6 0} - END diff --git a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml b/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml old mode 100644 new mode 100755 similarity index 87% rename from kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml rename to kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml index 806e89a9..aadb5a8a --- a/kuka_rsi_hw_interface/krl/KR_C4/ros_rsi_ethernet.xml +++ b/kuka_rsi_hw_interface/krl/ros_rsi_ethernet.xml @@ -1,8 +1,8 @@ ip.address.of.rospc - 49152 - ImFree + port.of.rospc + KROSHU FALSE @@ -29,6 +29,7 @@ + - + \ No newline at end of file diff --git a/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml new file mode 100644 index 00000000..faaebf5e --- /dev/null +++ b/kuka_rsi_hw_interface/kuka_rsi_hw_interface.xml @@ -0,0 +1,7 @@ + + + + ros2_control hardware interface for KSS-powered robots + + diff --git a/kuka_rsi_hw_interface/launch/startup.launch.py b/kuka_rsi_hw_interface/launch/startup.launch.py new file mode 100644 index 00000000..e37cb9ac --- /dev/null +++ b/kuka_rsi_hw_interface/launch/startup.launch.py @@ -0,0 +1,111 @@ +# Copyright 2023 Áron 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. + + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node, LifecycleNode +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + robot_model = LaunchConfiguration('robot_model') + + # TODO(Svastits):better way to handle supported robot models and families + if robot_model.perform(context) in ["kr6_r700_sixx", "kr6_r900_sixx"]: + robot_family = "agilus" + elif robot_model.perform(context) in ["kr16_r2010-2"]: + robot_family = "cybertech" + else: + print("[ERROR] [launch]: robot model not recognized") + raise Exception + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare('kuka_{}_support'.format(robot_family)), + "urdf", robot_model.perform(context) + ".urdf.xacro"] + ), + " ", + ] + ) + + robot_description = {'robot_description': robot_description_content} + + controller_config = (get_package_share_directory('kuka_rsi_hw_interface') + + "/config/ros2_controller_config.yaml") + + joint_traj_controller_config = (get_package_share_directory('kuka_rsi_hw_interface') + + "/config/joint_trajectory_controller_config.yaml") + + controller_manager_node = '/controller_manager' + + control_node = Node( + package='kroshu_ros2_core', + executable='control_node', + parameters=[robot_description, controller_config] + ) + robot_manager_node = LifecycleNode( + name=['robot_manager'], + namespace='', + package="kuka_rsi_hw_interface", + executable="robot_manager_node", + parameters=[{'robot_model': robot_model}] + ) + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[robot_description] + ) + + # Spawn controllers + def controller_spawner(controller_with_config): + return Node( + package="controller_manager", + executable="spawner", + arguments=[controller_with_config[0], "-c", controller_manager_node, "-p", + controller_with_config[1], "--inactive"] + ) + + controller_names_and_config = [ + ("joint_state_broadcaster", []), + ("joint_trajectory_controller", joint_traj_controller_config), + ] + + controller_spawners = [controller_spawner(controllers) + for controllers in controller_names_and_config] + + nodes_to_start = [ + control_node, + robot_manager_node, + robot_state_publisher + ] + controller_spawners + + return nodes_to_start + + +def generate_launch_description(): + launch_arguments = [] + launch_arguments.append(DeclareLaunchArgument( + 'robot_model', + default_value='kr6_r700_sixx' + )) + return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/kuka_rsi_hw_interface/launch/startup_with_rviz.launch.py b/kuka_rsi_hw_interface/launch/startup_with_rviz.launch.py new file mode 100644 index 00000000..62c7ec44 --- /dev/null +++ b/kuka_rsi_hw_interface/launch/startup_with_rviz.launch.py @@ -0,0 +1,42 @@ +# Copyright 2022 Áron 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 os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions.include_launch_description import IncludeLaunchDescription +from launch.launch_description_sources.python_launch_description_source import PythonLaunchDescriptionSource # noqa: E501 + + +def generate_launch_description(): + rviz_config_file = os.path.join( + get_package_share_directory('kuka_agilus_support'), 'config', 'agilus_urdf.rviz') + + startup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource( + [get_package_share_directory('kuka_rsi_hw_interface'), '/launch/startup.launch.py'])) + + return LaunchDescription([ + startup_launch, + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file, + "--ros-args", "--log-level", "error"], + ) + ]) diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml index 671d03b6..74a30497 100644 --- a/kuka_rsi_hw_interface/package.xml +++ b/kuka_rsi_hw_interface/package.xml @@ -17,6 +17,9 @@ sensor_msgs kroshu_ros2_core tinyxml_vendor + hardware_interface + pluginlib + controller_manager_msgs ament_cmake_copyright ament_cmake_cppcheck diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 0288b2a1..1d2eabfc 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -1,37 +1,37 @@ /********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2014 Norwegian University of Science and Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Norwegian University of Science and - * Technology, nor the names of its contributors may be used to - * endorse or promote products derived from this software without - * specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ +* Software License Agreement (BSD License) +* +* Copyright (c) 2014 Norwegian University of Science and Technology +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Norwegian University of Science and +* Technology, nor the names of its contributors may be used to +* endorse or promote products derived from this software without +* specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ /* * Author: Lars Tingelstad @@ -47,25 +47,105 @@ namespace kuka_rsi_hw_interface { - -KukaHardwareInterface::KukaHardwareInterface( - const std::string & rsi_ip_address, int rsi_port, uint8_t n_dof) -: rsi_ip_address_(rsi_ip_address), - rsi_port_(rsi_port), - n_dof_(n_dof) +CallbackReturn KukaRSIHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { - in_buffer_.resize(1024); + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + hw_states_.resize(info_.joints.size(), 0.0); + hw_commands_.resize(info_.joints.size(), 0.0); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + if (joint.command_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting exactly 1 command interface"); + return CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting only POSITION command interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting exactly 1 state interface"); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL( + rclcpp::get_logger( + "KukaRSIHardwareInterface"), "expecting only POSITION state interface"); + return CallbackReturn::ERROR; + } + } + + // RSI + in_buffer_.resize(1024); // udp_server.h --> #define BUFSIZE 1024 out_buffer_.resize(1024); + + initial_joint_pos_.resize(info_.joints.size(), 0.0); + joint_pos_correction_deg_.resize(info_.joints.size(), 0.0); + ipoc_ = 0; + + rsi_ip_address_ = info_.hardware_parameters["client_ip"]; + rsi_port_ = std::stoi(info_.hardware_parameters["client_port"]); + + RCLCPP_INFO( + rclcpp::get_logger("KukaRSIHardwareInterface"), + "IP of client machine: %s:%d", rsi_ip_address_.c_str(), rsi_port_); + + return CallbackReturn::SUCCESS; } -void KukaHardwareInterface::start(std::vector & joint_state_msg_position) +std::vector KukaRSIHardwareInterface::export_state_interfaces() { - std::lock_guard lock(m_); + std::vector state_interfaces; + 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]); + } + return state_interfaces; +} + +std::vector KukaRSIHardwareInterface:: +export_command_interfaces() +{ + std::vector 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]); + } + return command_interfaces; +} + +CallbackReturn KukaRSIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) +{ + stop_flag_ = false; // Wait for connection from robot server_.reset(new UDPServer(rsi_ip_address_, rsi_port_)); - // TODO(Marton): use any logger - std::cout << "Waiting for robot connection\n"; + server_->set_timeout(10000); // Set receive timeout to 10 seconds for activation + + + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Connecting to robot . . ."); + int bytes = server_->recv(in_buffer_); + if (bytes == 0) { + RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "Connection timeout"); + return CallbackReturn::FAILURE; + } + + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Got data from robot"); // Drop empty frame with RSI <= 2.3 if (bytes < 100) { @@ -73,69 +153,81 @@ void KukaHardwareInterface::start(std::vector & joint_state_msg_position } rsi_state_ = RSIState(in_buffer_); - for (std::size_t i = 0; i < n_dof_; ++i) { - joint_state_msg_position[i] = rsi_state_.positions[i] * KukaHardwareInterface::D2R; - initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaHardwareInterface::D2R; + + for (size_t i = 0; i < info_.joints.size(); ++i) { + hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; + hw_commands_[i] = hw_states_[i]; + initial_joint_pos_[i] = rsi_state_.initial_positions[i] * KukaRSIHardwareInterface::D2R; } ipoc_ = rsi_state_.ipoc; - out_buffer_ = RSICommand(std::vector(n_dof_, 0), ipoc_).xml_doc; + + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); - // Set receive timeout to 1 second - server_->set_timeout(1000); - // TODO(Marton): use any logger - std::cout << "Got connection from robot\n"; + server_->set_timeout(1000); // Set receive timeout to 1 second + + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "System Successfully started!"); is_active_ = true; + + return CallbackReturn::SUCCESS; } -bool KukaHardwareInterface::read(std::vector & joint_state_msg_position) +CallbackReturn KukaRSIHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State &) +{ + stop_flag_ = true; + RCLCPP_INFO(rclcpp::get_logger("KukaRSIHardwareInterface"), "Stop flag was set!"); + return CallbackReturn::SUCCESS; +} + +return_type KukaRSIHardwareInterface::read( + const rclcpp::Time &, + const rclcpp::Duration &) { - std::lock_guard lock(m_); if (!is_active_) { - return false; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + return return_type::OK; } if (server_->recv(in_buffer_) == 0) { - return false; + RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "No data received from robot"); + this->on_deactivate(this->get_state()); + return return_type::ERROR; } rsi_state_ = RSIState(in_buffer_); - for (std::size_t i = 0; i < n_dof_; ++i) { - joint_state_msg_position[i] = rsi_state_.positions[i] * KukaHardwareInterface::D2R; + + for (std::size_t i = 0; i < info_.joints.size(); ++i) { + hw_states_[i] = rsi_state_.positions[i] * KukaRSIHardwareInterface::D2R; } ipoc_ = rsi_state_.ipoc; - return true; + return return_type::OK; } -bool KukaHardwareInterface::write(const std::vector & joint_command_position_msg_) +return_type KukaRSIHardwareInterface::write( + const rclcpp::Time &, + const rclcpp::Duration &) { - std::lock_guard lock(m_); - if (!is_active_) { - std::cout << "Controller deactivated\n"; - return false; + // It is possible, that write is called immediately after activation + // In this case write in that tick should be skipped to be able to read state at first + // First cycle (with 0 ipoc) is handled in the on_activate method, so 0 ipoc means + // read was not called yet + if (!is_active_ || ipoc_ == 0) { + return return_type::OK; } - for (size_t i = 0; i < n_dof_; i++) { - joint_pos_correction_deg_[i] = (joint_command_position_msg_[i] - initial_joint_pos_[i]) * - KukaHardwareInterface::R2D; - } + if (stop_flag_) {is_active_ = false;} - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_).xml_doc; - server_->send(out_buffer_); - return true; -} + for (size_t i = 0; i < info_.joints.size(); i++) { + joint_pos_correction_deg_[i] = (hw_commands_[i] - initial_joint_pos_[i]) * + KukaRSIHardwareInterface::R2D; + } -void KukaHardwareInterface::stop() -{ - std::lock_guard lock(m_); - out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, true).xml_doc; + out_buffer_ = RSICommand(joint_pos_correction_deg_, ipoc_, stop_flag_).xml_doc; server_->send(out_buffer_); - server_.reset(); - is_active_ = false; - std::cout << "Connection to robot terminated\n"; + return return_type::OK; } - -bool KukaHardwareInterface::isActive() const -{ - return is_active_; -} - } // namespace namespace kuka_rsi_hw_interface + +PLUGINLIB_EXPORT_CLASS( + kuka_rsi_hw_interface::KukaRSIHardwareInterface, + hardware_interface::SystemInterface +) diff --git a/kuka_rsi_hw_interface/src/robot_control_node.cpp b/kuka_rsi_hw_interface/src/robot_control_node.cpp deleted file mode 100644 index f096b75c..00000000 --- a/kuka_rsi_hw_interface/src/robot_control_node.cpp +++ /dev/null @@ -1,180 +0,0 @@ -// Copyright 2022 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. - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "kuka_rsi_hw_interface/robot_control_node.hpp" -#include "kroshu_ros2_core/ROS2BaseLCNode.hpp" - -namespace kuka_rsi_hw_interface -{ -RobotControlNode::RobotControlNode( - const std::string & node_name, - const rclcpp::NodeOptions & options) -: kroshu_ros2_core::ROS2BaseLCNode(node_name, options) -{ - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); - qos.best_effort(); - auto callback = - [this](sensor_msgs::msg::JointState::SharedPtr msg) - { - this->commandReceivedCallback(msg); - }; - - joint_command_msg_ = std::make_shared(); - - registerParameter( - "rsi_ip_address_", "127.0.0.1", - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](const std::string & rsi_ip_address) - { - return this->onRSIIPAddressChange(rsi_ip_address); - }); - - registerParameter( - "rsi_port_", 59152, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](int rsi_port) - { - return this->onRSIPortAddressChange(rsi_port); - }); - - registerParameter( - "n_dof_", 6, - kroshu_ros2_core::ParameterSetAccessRights{true, false, false, false}, - [this](uint8_t n_dof) - { - return this->onNDOFChange(n_dof); - }); - - joint_command_subscription_ = this->create_subscription( - "rsi_joint_command", qos, callback); - - joint_state_publisher_ = - this->create_publisher("rsi_joint_state", 1); - joint_command_msg_ = std::make_shared(); - controller_joint_names_ = std::vector( - {"joint_a1", "joint_a2", "joint_a3", - "joint_a4", "joint_a5", "joint_a6"}); -} - -CallbackReturn RobotControlNode::on_configure(const rclcpp_lifecycle::State &) -{ - kuka_rsi_hw_interface_ = std::make_unique( - rsi_ip_address_, rsi_port_, n_dof_); - return SUCCESS; -} - -CallbackReturn RobotControlNode::on_activate(const rclcpp_lifecycle::State &) -{ - kuka_rsi_hw_interface_->start(joint_state_msg_.position); - joint_command_msg_->position = joint_state_msg_.position; - joint_state_msg_.name = controller_joint_names_; - joint_state_msg_.header.frame_id = "base"; - - joint_state_publisher_->on_activate(); - joint_state_publisher_->publish(joint_state_msg_); - - control_thread_ = std::thread(&RobotControlNode::ControlLoop, this); - return SUCCESS; -} - -CallbackReturn RobotControlNode::on_deactivate(const rclcpp_lifecycle::State &) -{ - kuka_rsi_hw_interface_->stop(); - joint_state_publisher_->on_deactivate(); - if (control_thread_.joinable()) { - control_thread_.join(); - } - - return SUCCESS; -} - -CallbackReturn RobotControlNode::on_cleanup(const rclcpp_lifecycle::State &) -{ - kuka_rsi_hw_interface_ = nullptr; - return SUCCESS; -} - -void RobotControlNode::ControlLoop() -{ - while (kuka_rsi_hw_interface_->isActive()) { - std::unique_lock lock(m_); - if (!kuka_rsi_hw_interface_->read(joint_state_msg_.position)) { - RCLCPP_ERROR(get_logger(), "Failed to read state from robot. Shutting down!"); - rclcpp::shutdown(); - return; - } - joint_state_msg_.header.stamp = this->now(); - joint_state_publisher_->publish(joint_state_msg_); - - cv_.wait(lock); - kuka_rsi_hw_interface_->write(joint_command_msg_->position); - } -} - -void RobotControlNode::commandReceivedCallback(sensor_msgs::msg::JointState::SharedPtr msg) -{ - std::lock_guard lock(m_); - joint_command_msg_ = msg; - - cv_.notify_one(); -} - -bool RobotControlNode::onRSIIPAddressChange(const std::string & rsi_ip_address) -{ - rsi_ip_address_ = rsi_ip_address; - return true; -} - -bool RobotControlNode::onRSIPortAddressChange(int rsi_port) -{ - rsi_port_ = rsi_port; - return true; -} - -bool RobotControlNode::onNDOFChange(uint8_t n_dof) -{ - n_dof_ = n_dof; - joint_command_msg_->position.resize(n_dof_); - joint_command_msg_->effort.resize(n_dof_); - joint_command_msg_->velocity.resize(n_dof_); - joint_state_msg_.position.resize(n_dof_); - joint_state_msg_.effort.resize(n_dof_); - joint_state_msg_.velocity.resize(n_dof_); - return true; -} - -} // namespace kuka_rsi_hw_interface - -int main(int argc, char * argv[]) -{ - setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared( - "robot_control_node", - rclcpp::NodeOptions()); - executor.add_node(node->get_node_base_interface()); - executor.spin(); - rclcpp::shutdown(); - return 0; -} diff --git a/kuka_rsi_hw_interface/src/robot_manager_node.cpp b/kuka_rsi_hw_interface/src/robot_manager_node.cpp new file mode 100644 index 00000000..670f229c --- /dev/null +++ b/kuka_rsi_hw_interface/src/robot_manager_node.cpp @@ -0,0 +1,200 @@ +// Copyright 2023 Svastits Áron +// +// 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 "kuka_rsi_hw_interface/robot_manager_node.hpp" +#include "kroshu_ros2_core/ControlMode.hpp" + + +using namespace controller_manager_msgs::srv; // NOLINT +using namespace lifecycle_msgs::msg; // NOLINT + +namespace kuka_rsi +{ +RobotManagerNode::RobotManagerNode() +: kroshu_ros2_core::ROS2BaseLCNode("robot_manager") +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + qos.reliable(); + 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_ + ); + + 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", "kr6_r700_sixx", + kroshu_ros2_core::ParameterSetAccessRights{true, false, + false, false, false}, [this](const std::string & robot_model) { + return this->onRobotModelChangeRequest(robot_model); + }); +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) +{ + // Configure hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = robot_model_; + hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not configure hardware interface"); + return FAILURE; + } + RCLCPP_INFO(get_logger(), "Successfully configured hardware interface"); + + is_configured_pub_->on_activate(); + is_configured_msg_.data = true; + is_configured_pub_->publish(is_configured_msg_); + return SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) +{ + // Clean up hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = robot_model_; + hw_request->target_state.id = State::PRIMARY_STATE_UNCONFIGURED; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 2000); + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not clean up hardware interface"); + return FAILURE; + } + + if (is_configured_pub_->is_activated()) { + is_configured_msg_.data = false; + is_configured_pub_->publish(is_configured_msg_); + is_configured_pub_->on_deactivate(); + } + // TODO(Svastits): add else branch, and throw exception(?) + return SUCCESS; +} + +// TODO(Svastits): rollback in case of failures +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) +{ + // Activate hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = robot_model_; + hw_request->target_state.id = State::PRIMARY_STATE_ACTIVE; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 10000); + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not activate hardware interface"); + return FAILURE; + } + + + // Activate RT controller(s) + auto controller_request = + std::make_shared(); + controller_request->strictness = SwitchController::Request::STRICT; + controller_request->activate_controllers = + {"joint_state_broadcaster", "joint_trajectory_controller"}; + + auto controller_response = + kroshu_ros2_core::sendRequest( + change_controller_state_client_, controller_request, 0, 2000 + ); + if (!controller_response || !controller_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not activate controller"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return FAILURE; + } + + RCLCPP_INFO(get_logger(), "Successfully activated controllers"); + return SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) +{ + // Deactivate hardware interface + auto hw_request = + std::make_shared(); + hw_request->name = robot_model_; + hw_request->target_state.id = State::PRIMARY_STATE_INACTIVE; + auto hw_response = + kroshu_ros2_core::sendRequest( + change_hardware_state_client_, hw_request, 0, 3000); // was not stable with 2000 ms timeout + if (!hw_response || !hw_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not deactivate hardware interface"); + return ERROR; + } + + RCLCPP_INFO(get_logger(), "Deactivated hardware interface"); + + + // Stop RT controllers + auto controller_request = + std::make_shared(); + // With best effort strictness, deactivation succeeds if specific controller is not active + controller_request->strictness = + SwitchController::Request::BEST_EFFORT; + controller_request->deactivate_controllers = + {"joint_state_broadcaster", "joint_trajectory_controller"}; + auto controller_response = + kroshu_ros2_core::sendRequest( + change_controller_state_client_, controller_request, 0, 2000 + ); + if (!controller_response || !controller_response->ok) { + RCLCPP_ERROR(get_logger(), "Could not stop controllers"); + return ERROR; + } + + RCLCPP_INFO(get_logger(), "Successfully stopped controllers"); + return SUCCESS; +} + +bool RobotManagerNode::onRobotModelChangeRequest(const std::string & robot_model) +{ + robot_model_ = robot_model; + return true; +} +} // namespace kuka_rsi + +int main(int argc, char * argv[]) +{ + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/kuka_sunrise_driver/CMakeLists.txt b/kuka_sunrise_driver/CMakeLists.txt index d64369a2..00b2e8dc 100644 --- a/kuka_sunrise_driver/CMakeLists.txt +++ b/kuka_sunrise_driver/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(kroshu_ros2_core REQUIRED) find_package(fri REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) -find_package(controller_manager) find_package(controller_manager_msgs) include_directories(include) @@ -57,13 +56,9 @@ target_link_libraries(robot_manager_node fri_connection configuration_manager) -add_executable(sunrise_control_node - src/sunrise_control_node.cpp) -ament_target_dependencies(sunrise_control_node kuka_driver_interfaces rclcpp rclcpp_lifecycle controller_manager) - pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) -install(TARGETS fri_connection kuka_fri_hw_interface robot_manager_node sunrise_control_node +install(TARGETS fri_connection kuka_fri_hw_interface robot_manager_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config diff --git a/kuka_sunrise_driver/launch/lbr_iiwa7_control.launch.py b/kuka_sunrise_driver/launch/lbr_iiwa7_control.launch.py index 36be4c2e..7efb7dcb 100644 --- a/kuka_sunrise_driver/launch/lbr_iiwa7_control.launch.py +++ b/kuka_sunrise_driver/launch/lbr_iiwa7_control.launch.py @@ -47,8 +47,8 @@ def generate_launch_description(): return LaunchDescription([ Node( - package='kuka_sunrise', - executable='sunrise_control_node', + package='kroshu_ros2_core', + executable='control_node', output='both', parameters=[robot_description, controller_config] ), diff --git a/kuka_sunrise_driver/launch/lbr_iiwa7_control_rviz.launch.py b/kuka_sunrise_driver/launch/lbr_iiwa7_control_rviz.launch.py index 97de7c7a..80f46ed0 100644 --- a/kuka_sunrise_driver/launch/lbr_iiwa7_control_rviz.launch.py +++ b/kuka_sunrise_driver/launch/lbr_iiwa7_control_rviz.launch.py @@ -46,8 +46,8 @@ def generate_launch_description(): return LaunchDescription([ Node( - package='kuka_sunrise', - executable='sunrise_control_node', + package='kroshu_ros2_core', + executable='control_node', parameters=[robot_description, controller_config] ), Node( diff --git a/kuka_sunrise_driver/package.xml b/kuka_sunrise_driver/package.xml index ea736d45..4d351380 100644 --- a/kuka_sunrise_driver/package.xml +++ b/kuka_sunrise_driver/package.xml @@ -21,7 +21,7 @@ kuka_driver_interfaces kroshu_ros2_core hardware_interface - controller_manager + controller_manager_msgs rosidl_default_runtime diff --git a/kuka_sunrise_driver/src/sunrise_control_node.cpp b/kuka_sunrise_driver/src/sunrise_control_node.cpp deleted file mode 100644 index 9574c388..00000000 --- a/kuka_sunrise_driver/src/sunrise_control_node.cpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2022 Áron 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. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - auto executor = std::make_shared(); - auto controller_manager = std::make_shared( - executor, - "controller_manager"); - - auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); - qos.best_effort(); - - std::atomic_bool is_configured = false; - auto is_configured_sub = controller_manager->create_subscription( - "robot_manager/is_configured", qos, - [&is_configured](std_msgs::msg::Bool::SharedPtr msg) { - is_configured = msg->data; - }); - - - std::thread control_loop([controller_manager, &is_configured]() { - const rclcpp::Duration dt = - rclcpp::Duration::from_seconds(1.0 / controller_manager->get_update_rate()); - std::chrono::milliseconds dt_ms {1000 / controller_manager->get_update_rate()}; - - while (rclcpp::ok()) { - if (is_configured) { - controller_manager->read(controller_manager->now(), dt); - controller_manager->update(controller_manager->now(), dt); - controller_manager->write(controller_manager->now(), dt); - } else { - controller_manager->update(controller_manager->now(), dt); - std::this_thread::sleep_for(dt_ms); - } - } - }); - - executor->add_node(controller_manager); - - executor->spin(); - control_loop.join(); - - // shutdown - rclcpp::shutdown(); - - return 0; -}