From 29dc520e9e6d8caece60edacdf20a13aa7f3804c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Szitanics=20M=C3=A1rk?= Date: Mon, 29 Jan 2024 15:02:33 +0100 Subject: [PATCH 01/88] use client lib in hwif-without control mode switch --- kuka_iiqka_eac_driver/CMakeLists.txt | 44 +-- .../kuka_iiqka_eac_driver/event_observer.hpp | 48 +++ .../hardware_interface.hpp | 34 +- .../robot_manager_node.hpp | 2 - .../src/hardware_interface.cpp | 326 +++++------------- .../udp_replier_mock.cpp | 48 --- .../udp_socket_mock.cpp | 193 ----------- .../src/robot_manager_node.cpp | 1 + 8 files changed, 150 insertions(+), 546 deletions(-) create mode 100644 kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp delete mode 100644 kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_replier_mock.cpp delete mode 100644 kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_socket_mock.cpp diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index bf1ffec8..c21888ff 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -19,7 +19,6 @@ endif() # TODO: find better solution set(CMAKE_EXE_LINKER_FLAGS "-Wl,--copy-dt-needed-entries") -set(MOCK_KUKA_LIBS TRUE) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) @@ -33,43 +32,7 @@ find_package(yaml-cpp REQUIRED) include_directories(include) -if(NOT MOCK_KUKA_LIBS) - find_package(os-core-udp-communication REQUIRED) - find_package(motion-services-ecs-proto-api-cpp REQUIRED) - find_package(motion-external-proto-api-nanopb REQUIRED) - find_package(motion-services-ecs-proto-api-nanopb REQUIRED) - find_package(nanopb-helpers REQUIRED) - message("Using real kuka libs") - add_definitions(-DNON_MOCK_SETUP) -else() - find_package(Protobuf) - include_directories(include/mock) - add_library(os-core-udp-communication SHARED - src/mock/os-core-udp-communication/udp_replier_mock.cpp - src/mock/os-core-udp-communication/udp_socket_mock.cpp) - add_library(kuka::os-core-udp-communication ALIAS os-core-udp-communication) - install(TARGETS os-core-udp-communication - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ) - - add_library(nanopb-helpers INTERFACE) - target_include_directories(nanopb-helpers INTERFACE include/mock/nanopb-helpers) - target_link_libraries(nanopb-helpers INTERFACE protobuf::libprotobuf) - add_library(kuka::nanopb-helpers ALIAS nanopb-helpers) - - add_library(motion-services-ecs-proto-api-cpp INTERFACE) - target_include_directories(motion-services-ecs-proto-api-cpp INTERFACE include/mock/kuka) - - add_library(motion-external-proto-api-nanopb INTERFACE) - target_include_directories(motion-external-proto-api-nanopb INTERFACE include/mock/nanopb/kuka/motion/external) - target_link_libraries(motion-external-proto-api-nanopb INTERFACE protobuf::libprotobuf) - - add_library(motion-services-ecs-proto-api-nanopb INTERFACE) - target_include_directories(motion-services-ecs-proto-api-nanopb INTERFACE include/mock/nanopb/kuka/ecs/v1) - target_link_libraries(motion-external-proto-api-nanopb INTERFACE motion-external-proto-api-nanopb) - message("Using mock kuka libs") - remove_definitions(-DNON_MOCK_SETUP) -endif() +find_library(NAMES kuka-external-control-sdk PATHS "/usr/local/lib" REQUIRED) add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp @@ -80,14 +43,13 @@ add_library(${PROJECT_NAME} SHARED target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY") ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface kuka_drivers_core) -target_link_libraries(${PROJECT_NAME} motion-external-proto-api-nanopb motion-services-ecs-proto-api-cpp - motion-services-ecs-proto-api-nanopb yaml-cpp kuka::os-core-udp-communication kuka::nanopb-helpers) +target_link_libraries(${PROJECT_NAME} kuka-external-control-sdk yaml-cpp) add_executable(robot_manager_node src/robot_manager_node.cpp) ament_target_dependencies(robot_manager_node rclcpp kuka_drivers_core sensor_msgs controller_manager_msgs) -target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers motion-services-ecs-proto-api-cpp) +target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers kuka-external-control-sdk kuka-external-control-sdk-protobuf) pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp new file mode 100644 index 00000000..f5438419 --- /dev/null +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -0,0 +1,48 @@ +#ifndef KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ +#define KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ + + +#include "rclcpp/macros.hpp" + +#include "kuka/external-control-sdk/common/irobot.h" +#include "kuka_iiqka_eac_driver/hardware_interface.hpp" + +namespace kuka_eac +{ + +class KukaEACEventObserver : public kuka::external::control::EventHandler { +public: + + KukaEACEventObserver(KukaEACHardwareInterface* hw_interface) : hw_interface_(hw_interface) {} + void OnSampling() override + { + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active"); + } + void OnControlModeSwitch(const std::string& reason) override + { + RCLCPP_INFO( + rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); + } + void OnStopped(const std::string& reason) override + { + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); + hw_interface_->on_deactivate(hw_interface_->get_state()); + + } + void OnError(const std::string& reason) override + { + RCLCPP_ERROR( + rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); + hw_interface_->on_deactivate(hw_interface_->get_state()); + } + +private: + KukaEACHardwareInterface* hw_interface_; +}; + +} + +#endif // KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ \ No newline at end of file diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index ca5948a1..90777d89 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -27,13 +27,11 @@ #include "pluginlib/class_list_macros.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h" -#include "nanopb/kuka/core/motion/joint.pb.hh" -#include "nanopb/kuka/ecs/v1/control_signal_external.pb.hh" -#include "nanopb/kuka/ecs/v1/motion_state_external.pb.hh" +#include "rclcpp_lifecycle/state.hpp" #include "os-core-udp-communication/replier.h" +#include "kuka/external-control-sdk/iiqka/robot.h" +#include "kuka/external-control-sdk/iiqka/message_builder.h" #include "kuka_iiqka_eac_driver/visibility_control.h" @@ -73,38 +71,26 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface write(const rclcpp::Time & time, const rclcpp::Duration & period) override; private: + KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); + KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS(); KUKA_IIQKA_EAC_DRIVER_LOCAL void ObserveControl(); - bool is_active_ = false; - bool msg_received_ = false; + std::unique_ptr robot_ptr_; + kuka::external::control::BaseControlSignal* hw_control_signal_ = nullptr; std::vector hw_position_commands_; std::vector hw_torque_commands_; std::vector hw_stiffness_commands_; std::vector hw_damping_commands_; - std::vector hw_position_states_; std::vector hw_torque_states_; - double hw_control_mode_command_; - -#ifdef NON_MOCK_SETUP kuka::ecs::v1::CommandState command_state_; - std::unique_ptr stub_; - std::unique_ptr context_; -#endif - - std::thread observe_thread_; - - std::unique_ptr udp_replier_; - std::chrono::milliseconds receive_timeout_{100}; + bool msg_received_; + - uint8_t out_buff_arr_[1500]; + std::chrono::milliseconds receive_timeout_{1000}; - nanopb::kuka::ecs::v1::ControlSignalExternal control_signal_ext_{ - nanopb::kuka::ecs::v1::ControlSignalExternal_init_default}; - nanopb::kuka::ecs::v1::MotionStateExternal motion_state_external_{ - nanopb::kuka::ecs::v1::MotionStateExternal_init_default}; }; } // namespace kuka_eac diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index 9ccc2ddb..8e12d607 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -30,8 +30,6 @@ #include "kuka_drivers_core/controller_handler.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" -#include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h" - namespace kuka_eac { class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 461f8b88..799fcf39 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -16,15 +16,17 @@ #include #include #include +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "kuka_drivers_core/hardware_interface_types.hpp" -#include "nanopb-helpers/nanopb_serialization_helper.h" - #include "kuka_iiqka_eac_driver/hardware_interface.hpp" +#include "kuka/external-control-sdk/iiqka/configuration.h" +#include "kuka_iiqka_eac_driver/event_observer.hpp" using namespace kuka::ecs::v1; // NOLINT +using namespace std::chrono_literals; using os::core::udp::communication::Socket; @@ -37,46 +39,16 @@ CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::Hardw return CallbackReturn::ERROR; } - udp_replier_ = std::make_unique( - os::core::udp::communication::SocketAddress(info_.hardware_parameters.at("client_ip"), 44444)); - -#ifdef NON_MOCK_SETUP - - stub_ = ExternalControlService::NewStub(grpc::CreateChannel( - info_.hardware_parameters.at("controller_ip") + ":49335", grpc::InsecureChannelCredentials())); - -#endif // Initialize control mode with 'undefined', which should be changed by the appropriate controller - // during configuration - hw_control_mode_command_ = 0; + // during configuration + // TODO: rethink control mode changes hw_position_states_.resize(info_.joints.size(), 0.0); hw_torque_states_.resize(info_.joints.size(), 0.0); hw_position_commands_.resize(info_.joints.size(), 0.0); hw_torque_commands_.resize(info_.joints.size(), 0.0); hw_stiffness_commands_.resize(info_.joints.size(), 30); hw_damping_commands_.resize(info_.joints.size(), 0.7); - motion_state_external_.header.ipoc = 0; - control_signal_ext_.has_header = true; - control_signal_ext_.has_control_signal = true; - control_signal_ext_.control_signal.has_joint_command = true; - control_signal_ext_.control_signal.joint_command.values_count = info_.joints.size(); - control_signal_ext_.control_signal.has_joint_torque_command = true; - control_signal_ext_.control_signal.joint_torque_command.values_count = info_.joints.size(); - control_signal_ext_.control_signal.has_joint_attributes = true; - control_signal_ext_.control_signal.joint_attributes.stiffness_count = info_.joints.size(); - control_signal_ext_.control_signal.joint_attributes.damping_count = info_.joints.size(); -#ifdef NON_MOCK_SETUP - if (udp_replier_->Setup() != Socket::ErrorCode::kSuccess) - { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Could not setup udp replier"); - return CallbackReturn::FAILURE; - } -#else - // Start from home position in mock mode - hw_position_commands_[1] = -90 * (M_PI / 180); - hw_position_commands_[2] = 90 * (M_PI / 180); - hw_position_commands_[4] = 90 * (M_PI / 180); -#endif + for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -143,12 +115,6 @@ CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::Hardw } } - RCLCPP_INFO( - rclcpp::get_logger("KukaEACHardwareInterface"), - "Init successful with controller ip: %s and client ip: %s", - info_.hardware_parameters.at("controller_ip").c_str(), - info_.hardware_parameters.at("client_ip").c_str()); - return CallbackReturn::SUCCESS; } @@ -188,91 +154,52 @@ KukaEACHardwareInterface::export_command_interfaces() info_.joints[i].name, hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[i]); } - command_interfaces.emplace_back( - hardware_interface::CONFIG_PREFIX, hardware_interface::CONTROL_MODE, &hw_control_mode_command_); - return command_interfaces; } CallbackReturn KukaEACHardwareInterface::on_configure(const rclcpp_lifecycle::State &) { -#ifdef NON_MOCK_SETUP - SetQoSProfileRequest request; - SetQoSProfileResponse response; - grpc::ClientContext context; - - request.add_qos_profiles(); - - request.mutable_qos_profiles() - ->at(0) - .mutable_rt_packet_loss_profile() - ->set_consequent_lost_packets( - std::stoi(info_.hardware_parameters.at("consequent_lost_packets"))); - request.mutable_qos_profiles() - ->at(0) - .mutable_rt_packet_loss_profile() - ->set_lost_packets_in_timeframe( - std::stoi(info_.hardware_parameters.at("lost_packets_in_timeframe"))); - request.mutable_qos_profiles()->at(0).mutable_rt_packet_loss_profile()->set_timeframe_ms( - std::stoi(info_.hardware_parameters.at("timeframe_ms"))); - - if (stub_->SetQoSProfile(&context, request, &response).error_code() != grpc::StatusCode::OK) + if (!SetupRobot()) + { + return CallbackReturn::FAILURE; + } + + if (!SetupQoS()) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "SetQoSProfile failed"); return CallbackReturn::FAILURE; } + RCLCPP_INFO( + rclcpp::get_logger("KukaEACHardwareInterface"), + "Configure successful with controller ip: %s and client ip: %s", + info_.hardware_parameters.at("controller_ip").c_str(), + info_.hardware_parameters.at("client_ip").c_str()); + RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Set QoS profile with %s consequent and %s packet losses allowed in %s milliseconds", info_.hardware_parameters.at("consequent_lost_packets").c_str(), info_.hardware_parameters.at("lost_packets_in_timeframe").c_str(), info_.hardware_parameters.at("timeframe_ms").c_str()); -#endif + return CallbackReturn::SUCCESS; } CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Connecting to robot . . ."); - // Reset timeout to catch first tick message - receive_timeout_ = std::chrono::milliseconds(100); - control_signal_ext_.control_signal.stop_ipo = false; -#ifdef NON_MOCK_SETUP - if (context_ != nullptr) - { - context_->TryCancel(); - } -#endif - if (observe_thread_.joinable()) - { - observe_thread_.join(); + kuka::external::control::OperationStatus create_event_observer = robot_ptr_->RegisterEventHandler(std::make_unique(this)); + if (create_event_observer.return_code == kuka::external::control::ReturnCode::ERROR) { + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Creating event observer failed, error message: %s", create_event_observer.message); } -#ifdef NON_MOCK_SETUP - observe_thread_ = std::thread(&KukaEACHardwareInterface::ObserveControl, this); - - OpenControlChannelRequest request; - OpenControlChannelResponse response; - grpc::ClientContext context; - - request.set_ip_address(info_.hardware_parameters.at("client_ip")); - request.set_timeout(5000); - request.set_cycle_time(4); - request.set_external_control_mode( - kuka::motion::external::ExternalControlMode(hw_control_mode_command_)); - RCLCPP_INFO( - rclcpp::get_logger("KukaEACHardwareInterface"), "Starting control in %s", - kuka::motion::external::ExternalControlMode_Name(static_cast(hw_control_mode_command_)) - .c_str()); - - auto ret = stub_->OpenControlChannel(&context, request, &response); - if (ret.error_code() != grpc::StatusCode::OK) + kuka::external::control::OperationStatus start_control = robot_ptr_->StartControlling(); + if (start_control.return_code == kuka::external::control::ReturnCode::ERROR) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "%s", ret.error_message().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Starting external control failed, error message: %s", start_control.message); return CallbackReturn::FAILURE; } -#endif + + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control session started successfully"); return CallbackReturn::SUCCESS; } @@ -281,176 +208,99 @@ CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::S { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Deactivating"); - control_signal_ext_.control_signal.stop_ipo = true; - while (is_active_) - { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } + //set stop flag or something -#ifdef NON_MOCK_SETUP - if (context_ != nullptr) - { - context_->TryCancel(); - } -#endif - if (observe_thread_.joinable()) - { - observe_thread_.join(); - } return CallbackReturn::SUCCESS; } return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { -#ifndef NON_MOCK_SETUP - std::this_thread::sleep_for(std::chrono::microseconds(3900)); - for (size_t i = 0; i < info_.joints.size(); i++) - { - hw_position_states_[i] = hw_position_commands_[i]; - } - return return_type::OK; -#endif + kuka::external::control::OperationStatus receive_state = robot_ptr_->ReceiveMotionState(20ms); - if (!is_active_) + if ((msg_received_ = receive_state.return_code == kuka::external::control::ReturnCode::OK)) { - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - msg_received_ = false; + auto& req_message = robot_ptr_->GetLastMotionState(); - return return_type::OK; - } - - if (udp_replier_->ReceiveRequestOrTimeout(receive_timeout_) == Socket::ErrorCode::kSuccess) - { - auto req_message = udp_replier_->GetRequestMessage(); - - if (!nanopb::Decode( - req_message.first, req_message.second, motion_state_external_)) - { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Decoding request failed"); - throw std::runtime_error("Decoding request failed"); - } - control_signal_ext_.header.ipoc = motion_state_external_.header.ipoc; - - for (size_t i = 0; i < info_.joints.size(); i++) - { - hw_position_states_[i] = motion_state_external_.motion_state.measured_positions.values[i]; - hw_torque_states_[i] = motion_state_external_.motion_state.measured_torques.values[i]; - // This is necessary, as joint trajectory controller is initialized with 0 command values - if (!msg_received_ && motion_state_external_.header.ipoc == 0) - { - hw_position_commands_[i] = hw_position_states_[i]; - } - } - - if (motion_state_external_.motion_state.ipo_stopped) - { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Motion stopped"); - } - msg_received_ = true; - receive_timeout_ = std::chrono::milliseconds(6); - } - else - { - RCLCPP_WARN(rclcpp::get_logger("KukaEACHardwareInterface"), "Request was missed"); - RCLCPP_WARN( - rclcpp::get_logger("KukaEACHardwareInterface"), "Previous ipoc: %i", - motion_state_external_.header.ipoc); - msg_received_ = false; + std::copy(req_message.GetMeasuredPositions()->begin(), req_message.GetMeasuredPositions()->end(), hw_position_states_.begin()); + std::copy(req_message.GetMeasuredTorques()->begin(), req_message.GetMeasuredTorques()->end(), hw_torque_states_.begin()); + std::copy(hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); } + return return_type::OK; } return_type KukaEACHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) { // If control is not started or a request is missed, do not send back anything + if (!msg_received_) { return return_type::OK; } - + // TODO(Svastits): should we separate control modes somehow? - std::copy( - hw_position_commands_.begin(), hw_position_commands_.end(), - control_signal_ext_.control_signal.joint_command.values); - std::copy( - hw_torque_commands_.begin(), hw_torque_commands_.end(), - control_signal_ext_.control_signal.joint_torque_command.values); - std::copy( - hw_stiffness_commands_.begin(), hw_stiffness_commands_.end(), - control_signal_ext_.control_signal.joint_attributes.stiffness); - std::copy( - hw_damping_commands_.begin(), hw_damping_commands_.end(), - control_signal_ext_.control_signal.joint_attributes.damping); - - control_signal_ext_.control_signal.control_mode = - kuka_motion_external_ExternalControlMode(static_cast(hw_control_mode_command_)); - - auto encoded_bytes = nanopb::Encode( - control_signal_ext_, out_buff_arr_, sizeof(out_buff_arr_)); - if (encoded_bytes < 0) - { - RCLCPP_ERROR( - rclcpp::get_logger("KukaEACHardwareInterface"), - "Encoding of control signal to out_buffer failed."); - throw std::runtime_error("Encoding of control signal to out_buffer failed."); - } - if (udp_replier_->SendReply(out_buff_arr_, encoded_bytes) != Socket::ErrorCode::kSuccess) + hw_control_signal_->AddJointPositionValues(hw_position_commands_); + hw_control_signal_->AddTorqueValues(hw_torque_commands_); + hw_control_signal_->AddStiffnessAndDampingValues(hw_stiffness_commands_, hw_damping_commands_); + + kuka::external::control::OperationStatus send_reply = robot_ptr_->SendControlSignal(); + + if (send_reply.return_code != kuka::external::control::ReturnCode::OK) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Error sending reply"); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Send reply failed, error message: %s", send_reply.message); throw std::runtime_error("Error sending reply"); } return return_type::OK; } -void KukaEACHardwareInterface::ObserveControl() -{ -#ifdef NON_MOCK_SETUP - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Observe control"); - context_ = std::make_unique<::grpc::ClientContext>(); - ObserveControlStateRequest obs_control; - std::unique_ptr> reader( - stub_->ObserveControlState(context_.get(), obs_control)); +bool KukaEACHardwareInterface::SetupRobot() { + kuka::external::control::iiqka::Configuration config; + + config.client_ip_address = info_.hardware_parameters.at("client_ip"); + config.koni_ip_address = info_.hardware_parameters.at("controller_ip"); + + config.is_secure = false; + config.dof = info_.joints.size(); + config.initial_control_mode = kuka::external::control::ControlMode::JOINT_POSITION_CONTROL; + + robot_ptr_ = std::make_unique(config); - CommandState response; + hw_control_signal_ = &(robot_ptr_->GetControlSignal()); - while (reader->Read(&response)) + hw_control_signal_->AddJointPositionValues(hw_position_commands_); + hw_control_signal_->AddTorqueValues(hw_torque_commands_); + hw_control_signal_->AddStiffnessAndDampingValues(hw_stiffness_commands_, hw_damping_commands_); + + kuka::external::control::OperationStatus setup = robot_ptr_->Setup(); + + if (setup.return_code != kuka::external::control::ReturnCode::OK) { - command_state_ = response; - RCLCPP_INFO( - rclcpp::get_logger("KukaEACHardwareInterface"), - "Event streamed from external control service: %s", - CommandEvent_Name(response.event()).c_str()); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Setup failed, error message: %s", setup.message); + return false; + } - switch (static_cast(response.event())) - { - case CommandEvent::COMMAND_READY: - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Command accepted"); - break; - case CommandEvent::SAMPLING: - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active"); - is_active_ = true; - break; - case CommandEvent::CONTROL_MODE_SWITCH: - RCLCPP_INFO( - rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); - is_active_ = false; - break; - case CommandEvent::STOPPED: - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); - is_active_ = false; - break; - case CommandEvent::ERROR: - RCLCPP_ERROR( - rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), response.message().c_str()); - is_active_ = false; - break; - default: - break; - } + return true; + +} + +bool KukaEACHardwareInterface::SetupQoS() { + + kuka::external::control::iiqka::QoS_Configuration qos_config; + qos_config.packet_loss_in_timeframe_limit = std::stoi(info_.hardware_parameters.at("lost_packets_in_timeframe")); + qos_config.consecutive_packet_loss_limit = std::stoi(info_.hardware_parameters.at("consequent_lost_packets")); + qos_config.timeframe_ms = std::stoi(info_.hardware_parameters.at("timeframe_ms")); + + kuka::external::control::OperationStatus set_qos_status = robot_ptr_->SetQoSProfile(qos_config); + + if (set_qos_status.return_code != kuka::external::control::ReturnCode::OK) + { + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "QoS configuration failed, error message: %s", set_qos_status.message); + return false; } -#endif + + return true; + } } // namespace kuka_eac diff --git a/kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_replier_mock.cpp b/kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_replier_mock.cpp deleted file mode 100644 index 17fbf0d1..00000000 --- a/kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_replier_mock.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// This material is the exclusive property of KUKA Deutschland GmbH. -// Except as expressly permitted by separate agreement, this material may only -// be used by members of the development department of KUKA Deutschland GmbH for -// internal development purposes of KUKA Deutschland GmbH. -// -// Copyright (C) -// KUKA Deutschland GmbH, Germany. All Rights Reserved. - -#include "os-core-udp-communication/replier.h" - -namespace os::core::udp::communication -{ -Replier::Replier(const SocketAddress & local_address) -{ - // only a mock -} -Socket::ErrorCode Replier::Setup() -{ - // only a mock - return Socket::ErrorCode::kSuccess; -} -void Replier::Reset() -{ - // only a mock -} - -Socket::ErrorCode Replier::ReceiveRequest() -{ - // only a mock - return Socket::ErrorCode::kSuccess; -} -Socket::ErrorCode Replier::ReceiveRequestOrTimeout(std::chrono::microseconds recv_timeout) -{ - // only a mock - return Socket::ErrorCode::kSuccess; -} -Socket::ErrorCode Replier::SendReply(uint8_t * reply_msg_data, size_t reply_msg_size) -{ - // only a mock - return Socket::ErrorCode::kSuccess; -} - -std::pair Replier::GetRequestMessage() const -{ - // only a mock - return {nullptr, 0}; -} -} // namespace os::core::udp::communication diff --git a/kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_socket_mock.cpp b/kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_socket_mock.cpp deleted file mode 100644 index c411ffd1..00000000 --- a/kuka_iiqka_eac_driver/src/mock/os-core-udp-communication/udp_socket_mock.cpp +++ /dev/null @@ -1,193 +0,0 @@ -// This material is the exclusive property of KUKA Deutschland GmbH. -// Except as expressly permitted by separate agreement, this material may only -// be used by members of the development department of KUKA Deutschland GmbH for -// internal development purposes of KUKA Deutschland GmbH. -// -// Copyright (C) -// KUKA Deutschland GmbH, Germany. All Rights Reserved. - - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "os-core-udp-communication/socket.h" - -namespace os::core::udp::communication -{ - -SocketAddress::SocketAddress() -{ - // only a mock -} - -SocketAddress::SocketAddress(const std::string & ip, int port) -{ - // only a mock -} - -SocketAddress::SocketAddress(const std::string & ip) -{ - // only a mock -} - -SocketAddress::SocketAddress(int port) -{ - // only a mock -} - -SocketAddress::SocketAddress(const struct sockaddr_in * raw_address) -{ - // only a mock -} - -struct sockaddr * SocketAddress::RawAddr() -{ - return nullptr; -} - -const struct sockaddr * SocketAddress::RawAddr() const -{ - return nullptr; -} - -struct sockaddr_in * SocketAddress::RawInetAddr() -{ - return nullptr; -} - -const struct sockaddr_in * SocketAddress::RawInetAddr() const {return nullptr;} - -size_t SocketAddress::Size() const {return 0;} -const std::string SocketAddress::Ip() const {return "";} -uint16_t SocketAddress::Port() const {return 0;} - -Socket::~Socket() -{ - // only a mock -} - -int Socket::Map(int flags) -{ - return 0; -} - -int Socket::SetSocketOption(int level, int optname, const void * optval, socklen_t optlen) -{ - return 0; -} - -int Socket::SetReuseAddress(int flag) -{ - return 0; -} - -int Socket::SetReceiveBufferSize(int size) -{ - return 0; -} - -int Socket::SetSendTimeout(const std::chrono::microseconds & timeout) -{ - return 0; -} - -int Socket::SetReceiveTimeout(const std::chrono::microseconds & timeout) -{ - return 0; -} - -int Socket::JoinMulticastGroup(const SocketAddress & multicast_address) -{ - return 0; -} - -int Socket::LeaveMulticastGroup(const SocketAddress & multicast_address) -{ - return 0; -} - -int Socket::SetTTLForMulticast(int ttl) -{ - return 0; -} - -int Socket::SetTTLForUnicast(int ttl) -{ - return 0; -} - -int Socket::Bind(const SocketAddress & local_address) -{ - return 0; -} - -int Socket::Connect(const SocketAddress & remote_address) -{ - return 0; -} - -int Socket::Select(std::chrono::microseconds timeout, bool read) -{ - return 1; -} - -int Socket::Send(const unsigned char * raw_data, int raw_data_size, int flags) -{ - return 0; -} - -int Socket::SendTo( - const SocketAddress & remote_address, const unsigned char * raw_data, - int raw_data_size, int flags) -{ - return 0; -} - -int Socket::Receive(unsigned char * buffer, int buffer_size, int flags) -{ - return 0; -} - -int Socket::ReceiveOrTimeout( - const std::chrono::microseconds & timeout, unsigned char * buffer, - int buffer_size, int flags) -{ - return 0; -} - -int Socket::ReceiveFrom( - SocketAddress & incoming_remote_address, unsigned char * buffer, - int buffer_size, int flags) -{ - return 0; -} - -int Socket::ReceiveFromOrTimeout( - const std::chrono::microseconds & timeout, - SocketAddress & incoming_remote_address, unsigned char * buffer, - int buffer_size, int flags) -{ - return 0; -} - -int Socket::Close() -{ - return ErrorCode::kSuccess; -} - -int Socket::GetErrorCode() const {return errno;} -std::string Socket::GetErrorText() const {return "";} - -bool Socket::IsReadable() const -{ - return false; -} - -} // namespace os::core::udp::communication diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 807ffca5..575d68f4 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -18,6 +18,7 @@ #include "communication_helpers/service_tools.hpp" #include "kuka_iiqka_eac_driver/robot_manager_node.hpp" +#include "iiqka/proto-api/motion-external/external_control_mode.pb.h" using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT From da10b619513c62d125a79226fe55742155394246 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 11:17:16 +0100 Subject: [PATCH 02/88] add control mode change --- .../hardware_interface_types.hpp | 5 + kuka_iiqka_eac_driver/CMakeLists.txt | 3 +- .../config/ros2_controller_config.yaml | 2 + .../kuka_iiqka_eac_driver/event_observer.hpp | 75 +- .../hardware_interface.hpp | 16 +- .../kuka/ecs/v1/motion_services_ecs.grpc.pb.h | 951 ----------- .../mock/kuka/ecs/v1/motion_services_ecs.pb.h | 1508 ----------------- .../external/external_control_mode.pb.h | 162 -- .../nanopb_serialization_helper.h | 20 - .../mock/nanopb/kuka/core/motion/joint.pb.h | 31 - .../mock/nanopb/kuka/core/motion/joint.pb.hh | 16 - .../kuka/ecs/v1/control_signal_external.pb.h | 38 - .../kuka/ecs/v1/control_signal_external.pb.hh | 15 - .../nanopb/kuka/ecs/v1/external_header.pb.h | 30 - .../kuka/ecs/v1/motion_state_external.pb.h | 37 - .../kuka/ecs/v1/motion_state_external.pb.hh | 15 - .../motion/external/control_attributes.pb.h | 50 - .../external/control_signal_internal.pb.h | 105 -- .../external/external_control_mode.pb.h | 41 - .../external/motion_state_internal.pb.h | 44 - .../mock/os-core-udp-communication/replier.h | 51 - .../mock/os-core-udp-communication/socket.h | 122 -- .../launch/startup.launch.py | 1 + .../src/hardware_interface.cpp | 138 +- .../src/robot_manager_node.cpp | 11 +- 25 files changed, 159 insertions(+), 3328 deletions(-) delete mode 100644 kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.grpc.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/kuka/motion/external/external_control_mode.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.hh delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.hh delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/external_control_mode.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/replier.h delete mode 100644 kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/socket.h diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp index b1d236c4..381fb830 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp @@ -32,6 +32,8 @@ static constexpr char IO_PREFIX[] = "gpio"; static constexpr char CONFIG_PREFIX[] = "runtime_config"; // Constant defining prefix for fri state static constexpr char FRI_STATE_PREFIX[] = "fri_state"; +// Constant defining prefix for states +static constexpr char STATE_PREFIX[] = "state"; /* Configuration interfaces */ // Constant defining control_mode configuration interface @@ -49,6 +51,9 @@ static constexpr char DRIVE_STATE[] = "drive_state"; static constexpr char OVERLAY_TYPE[] = "overlay_type"; static constexpr char TRACKING_PERFORMANCE[] = "tracking_performance"; +// Constant defining server_state interface necessary for event broadcasting +static constexpr char SERVER_STATE[] = "server_state"; + } // namespace hardware_interface #endif // KUKA_DRIVERS_CORE__HARDWARE_INTERFACE_TYPES_HPP_ diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index b57bfafd..220e67e0 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -49,7 +49,8 @@ target_link_libraries(${PROJECT_NAME} kuka-external-control-sdk yaml-cpp) add_executable(robot_manager_node src/robot_manager_node.cpp) ament_target_dependencies(robot_manager_node rclcpp kuka_drivers_core sensor_msgs controller_manager_msgs) -target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers kuka-external-control-sdk kuka-external-control-sdk-protobuf) +target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers kuka-external-control-sdk + kuka-external-control-sdk-protobuf) pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) diff --git a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml index 0991ce42..782d91f1 100644 --- a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml @@ -12,5 +12,7 @@ controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler + event_broadcaster: + type: kuka_controllers/EventBroadcaster configure_components_on_start: [""] diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index f5438419..945650b2 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -1,7 +1,20 @@ +// Copyright 2024 Márk Szitanics +// +// 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_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ #define KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ - #include "rclcpp/macros.hpp" #include "kuka/external-control-sdk/common/irobot.h" @@ -9,40 +22,38 @@ namespace kuka_eac { - -class KukaEACEventObserver : public kuka::external::control::EventHandler { +class KukaEACEventObserver : public kuka::external::control::EventHandler +{ public: - - KukaEACEventObserver(KukaEACHardwareInterface* hw_interface) : hw_interface_(hw_interface) {} - void OnSampling() override - { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active"); - } - void OnControlModeSwitch(const std::string& reason) override - { - RCLCPP_INFO( - rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); - } - void OnStopped(const std::string& reason) override - { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); - hw_interface_->on_deactivate(hw_interface_->get_state()); - - } - void OnError(const std::string& reason) override - { - RCLCPP_ERROR( - rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); - hw_interface_->on_deactivate(hw_interface_->get_state()); - } + KukaEACEventObserver(KukaEACHardwareInterface * hw_interface) : hw_interface_(hw_interface) {} + void OnSampling() override + { + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active"); + } + void OnControlModeSwitch(const std::string & reason) override + { + RCLCPP_INFO( + rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); + } + void OnStopped(const std::string & reason) override + { + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); + hw_interface_->on_deactivate(hw_interface_->get_state()); + } + void OnError(const std::string & reason) override + { + RCLCPP_ERROR( + rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); + hw_interface_->on_deactivate(hw_interface_->get_state()); + } private: - KukaEACHardwareInterface* hw_interface_; + KukaEACHardwareInterface * hw_interface_; }; -} +} // namespace kuka_eac -#endif // KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ \ No newline at end of file +#endif // KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index 90777d89..a1db61be 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -28,10 +28,9 @@ #include "rclcpp/macros.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "os-core-udp-communication/replier.h" -#include "kuka/external-control-sdk/iiqka/robot.h" #include "kuka/external-control-sdk/iiqka/message_builder.h" +#include "kuka/external-control-sdk/iiqka/robot.h" +#include "rclcpp_lifecycle/state.hpp" #include "kuka_iiqka_eac_driver/visibility_control.h" @@ -76,7 +75,7 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_LOCAL void ObserveControl(); std::unique_ptr robot_ptr_; - kuka::external::control::BaseControlSignal* hw_control_signal_ = nullptr; + kuka::external::control::BaseControlSignal * hw_control_signal_ = nullptr; std::vector hw_position_commands_; std::vector hw_torque_commands_; @@ -85,12 +84,17 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface std::vector hw_position_states_; std::vector hw_torque_states_; + double hw_control_mode_command_; + double server_state_ = 0; + + int prev_control_mode_; + kuka::ecs::v1::CommandState command_state_; bool msg_received_; - - std::chrono::milliseconds receive_timeout_{1000}; + bool stop_requested_ = false; + std::chrono::milliseconds receive_timeout_{1000}; }; } // namespace kuka_eac diff --git a/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.grpc.pb.h b/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.grpc.pb.h deleted file mode 100644 index ace97623..00000000 --- a/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.grpc.pb.h +++ /dev/null @@ -1,951 +0,0 @@ -// Generated by the gRPC C++ plugin. -// If you make any local change, they will be lost. -// source: kuka/ecs/v1/motion_services_ecs.proto -// Original file comments: -// This material is the exclusive property of KUKA. -// Except as expressly permitted by separate agreement, this material -// may only be used by members of the development department of KUKA -// for internal development purposes of KUKA. -// -// Copyright (C) KUKA. All Rights Reserved. -// -#ifndef GRPC_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto__INCLUDED -#define GRPC_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto__INCLUDED - -#include "kuka/ecs/v1/motion_services_ecs.pb.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace kuka { -namespace ecs { -namespace v1 { - -// External control gRPC Service -class ExternalControlService final { - public: - static constexpr char const* service_full_name() { - return "kuka.ecs.v1.ExternalControlService"; - } - class StubInterface { - public: - virtual ~StubInterface() {} - // Start external control - virtual ::grpc::Status OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::kuka::ecs::v1::OpenControlChannelResponse* response) = 0; - std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::OpenControlChannelResponse>> AsyncOpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::OpenControlChannelResponse>>(AsyncOpenControlChannelRaw(context, request, cq)); - } - std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::OpenControlChannelResponse>> PrepareAsyncOpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::OpenControlChannelResponse>>(PrepareAsyncOpenControlChannelRaw(context, request, cq)); - } - // Observe external control state - std::unique_ptr< ::grpc::ClientReaderInterface< ::kuka::ecs::v1::CommandState>> ObserveControlState(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request) { - return std::unique_ptr< ::grpc::ClientReaderInterface< ::kuka::ecs::v1::CommandState>>(ObserveControlStateRaw(context, request)); - } - std::unique_ptr< ::grpc::ClientAsyncReaderInterface< ::kuka::ecs::v1::CommandState>> AsyncObserveControlState(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq, void* tag) { - return std::unique_ptr< ::grpc::ClientAsyncReaderInterface< ::kuka::ecs::v1::CommandState>>(AsyncObserveControlStateRaw(context, request, cq, tag)); - } - std::unique_ptr< ::grpc::ClientAsyncReaderInterface< ::kuka::ecs::v1::CommandState>> PrepareAsyncObserveControlState(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncReaderInterface< ::kuka::ecs::v1::CommandState>>(PrepareAsyncObserveControlStateRaw(context, request, cq)); - } - // Start External motinoring - virtual ::grpc::Status StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::kuka::ecs::v1::StartMonitoringResponse* response) = 0; - std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StartMonitoringResponse>> AsyncStartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StartMonitoringResponse>>(AsyncStartMonitoringRaw(context, request, cq)); - } - std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StartMonitoringResponse>> PrepareAsyncStartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StartMonitoringResponse>>(PrepareAsyncStartMonitoringRaw(context, request, cq)); - } - // Stop External monitoring - virtual ::grpc::Status StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::kuka::ecs::v1::StopMonitoringResponse* response) = 0; - std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StopMonitoringResponse>> AsyncStopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StopMonitoringResponse>>(AsyncStopMonitoringRaw(context, request, cq)); - } - std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StopMonitoringResponse>> PrepareAsyncStopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StopMonitoringResponse>>(PrepareAsyncStopMonitoringRaw(context, request, cq)); - } - class experimental_async_interface { - public: - virtual ~experimental_async_interface() {} - // Start external control - virtual void OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, std::function) = 0; - virtual void OpenControlChannel(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, std::function) = 0; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual void OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::ClientUnaryReactor* reactor) = 0; - #else - virtual void OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) = 0; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual void OpenControlChannel(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::ClientUnaryReactor* reactor) = 0; - #else - virtual void OpenControlChannel(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) = 0; - #endif - // Observe external control state - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual void ObserveControlState(::grpc::ClientContext* context, ::kuka::ecs::v1::ObserveControlStateRequest* request, ::grpc::ClientReadReactor< ::kuka::ecs::v1::CommandState>* reactor) = 0; - #else - virtual void ObserveControlState(::grpc::ClientContext* context, ::kuka::ecs::v1::ObserveControlStateRequest* request, ::grpc::experimental::ClientReadReactor< ::kuka::ecs::v1::CommandState>* reactor) = 0; - #endif - // Start External motinoring - virtual void StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response, std::function) = 0; - virtual void StartMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StartMonitoringResponse* response, std::function) = 0; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual void StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) = 0; - #else - virtual void StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) = 0; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual void StartMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) = 0; - #else - virtual void StartMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) = 0; - #endif - // Stop External monitoring - virtual void StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response, std::function) = 0; - virtual void StopMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StopMonitoringResponse* response, std::function) = 0; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual void StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) = 0; - #else - virtual void StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) = 0; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual void StopMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) = 0; - #else - virtual void StopMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) = 0; - #endif - }; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - typedef class experimental_async_interface async_interface; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - async_interface* async() { return experimental_async(); } - #endif - virtual class experimental_async_interface* experimental_async() { return nullptr; } - private: - virtual ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::OpenControlChannelResponse>* AsyncOpenControlChannelRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) = 0; - virtual ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::OpenControlChannelResponse>* PrepareAsyncOpenControlChannelRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) = 0; - virtual ::grpc::ClientReaderInterface< ::kuka::ecs::v1::CommandState>* ObserveControlStateRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request) = 0; - virtual ::grpc::ClientAsyncReaderInterface< ::kuka::ecs::v1::CommandState>* AsyncObserveControlStateRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq, void* tag) = 0; - virtual ::grpc::ClientAsyncReaderInterface< ::kuka::ecs::v1::CommandState>* PrepareAsyncObserveControlStateRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq) = 0; - virtual ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StartMonitoringResponse>* AsyncStartMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) = 0; - virtual ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StartMonitoringResponse>* PrepareAsyncStartMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) = 0; - virtual ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StopMonitoringResponse>* AsyncStopMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) = 0; - virtual ::grpc::ClientAsyncResponseReaderInterface< ::kuka::ecs::v1::StopMonitoringResponse>* PrepareAsyncStopMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) = 0; - }; - class Stub final : public StubInterface { - public: - Stub(const std::shared_ptr< ::grpc::ChannelInterface>& channel); - ::grpc::Status OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::kuka::ecs::v1::OpenControlChannelResponse* response) override; - std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::OpenControlChannelResponse>> AsyncOpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::OpenControlChannelResponse>>(AsyncOpenControlChannelRaw(context, request, cq)); - } - std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::OpenControlChannelResponse>> PrepareAsyncOpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::OpenControlChannelResponse>>(PrepareAsyncOpenControlChannelRaw(context, request, cq)); - } - std::unique_ptr< ::grpc::ClientReader< ::kuka::ecs::v1::CommandState>> ObserveControlState(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request) { - return std::unique_ptr< ::grpc::ClientReader< ::kuka::ecs::v1::CommandState>>(ObserveControlStateRaw(context, request)); - } - std::unique_ptr< ::grpc::ClientAsyncReader< ::kuka::ecs::v1::CommandState>> AsyncObserveControlState(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq, void* tag) { - return std::unique_ptr< ::grpc::ClientAsyncReader< ::kuka::ecs::v1::CommandState>>(AsyncObserveControlStateRaw(context, request, cq, tag)); - } - std::unique_ptr< ::grpc::ClientAsyncReader< ::kuka::ecs::v1::CommandState>> PrepareAsyncObserveControlState(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncReader< ::kuka::ecs::v1::CommandState>>(PrepareAsyncObserveControlStateRaw(context, request, cq)); - } - ::grpc::Status StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::kuka::ecs::v1::StartMonitoringResponse* response) override; - std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StartMonitoringResponse>> AsyncStartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StartMonitoringResponse>>(AsyncStartMonitoringRaw(context, request, cq)); - } - std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StartMonitoringResponse>> PrepareAsyncStartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StartMonitoringResponse>>(PrepareAsyncStartMonitoringRaw(context, request, cq)); - } - ::grpc::Status StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::kuka::ecs::v1::StopMonitoringResponse* response) override; - std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StopMonitoringResponse>> AsyncStopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StopMonitoringResponse>>(AsyncStopMonitoringRaw(context, request, cq)); - } - std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StopMonitoringResponse>> PrepareAsyncStopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) { - return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StopMonitoringResponse>>(PrepareAsyncStopMonitoringRaw(context, request, cq)); - } - class experimental_async final : - public StubInterface::experimental_async_interface { - public: - void OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, std::function) override; - void OpenControlChannel(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, std::function) override; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - void OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::ClientUnaryReactor* reactor) override; - #else - void OpenControlChannel(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) override; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - void OpenControlChannel(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::ClientUnaryReactor* reactor) override; - #else - void OpenControlChannel(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::OpenControlChannelResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) override; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - void ObserveControlState(::grpc::ClientContext* context, ::kuka::ecs::v1::ObserveControlStateRequest* request, ::grpc::ClientReadReactor< ::kuka::ecs::v1::CommandState>* reactor) override; - #else - void ObserveControlState(::grpc::ClientContext* context, ::kuka::ecs::v1::ObserveControlStateRequest* request, ::grpc::experimental::ClientReadReactor< ::kuka::ecs::v1::CommandState>* reactor) override; - #endif - void StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response, std::function) override; - void StartMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StartMonitoringResponse* response, std::function) override; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - void StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) override; - #else - void StartMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) override; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - void StartMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) override; - #else - void StartMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StartMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) override; - #endif - void StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response, std::function) override; - void StopMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StopMonitoringResponse* response, std::function) override; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - void StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) override; - #else - void StopMonitoring(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) override; - #endif - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - void StopMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::ClientUnaryReactor* reactor) override; - #else - void StopMonitoring(::grpc::ClientContext* context, const ::grpc::ByteBuffer* request, ::kuka::ecs::v1::StopMonitoringResponse* response, ::grpc::experimental::ClientUnaryReactor* reactor) override; - #endif - private: - friend class Stub; - explicit experimental_async(Stub* stub): stub_(stub) { } - Stub* stub() { return stub_; } - Stub* stub_; - }; - class experimental_async_interface* experimental_async() override { return &async_stub_; } - - private: - std::shared_ptr< ::grpc::ChannelInterface> channel_; - class experimental_async async_stub_{this}; - ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::OpenControlChannelResponse>* AsyncOpenControlChannelRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) override; - ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::OpenControlChannelResponse>* PrepareAsyncOpenControlChannelRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest& request, ::grpc::CompletionQueue* cq) override; - ::grpc::ClientReader< ::kuka::ecs::v1::CommandState>* ObserveControlStateRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request) override; - ::grpc::ClientAsyncReader< ::kuka::ecs::v1::CommandState>* AsyncObserveControlStateRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq, void* tag) override; - ::grpc::ClientAsyncReader< ::kuka::ecs::v1::CommandState>* PrepareAsyncObserveControlStateRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest& request, ::grpc::CompletionQueue* cq) override; - ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StartMonitoringResponse>* AsyncStartMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) override; - ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StartMonitoringResponse>* PrepareAsyncStartMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StartMonitoringRequest& request, ::grpc::CompletionQueue* cq) override; - ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StopMonitoringResponse>* AsyncStopMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) override; - ::grpc::ClientAsyncResponseReader< ::kuka::ecs::v1::StopMonitoringResponse>* PrepareAsyncStopMonitoringRaw(::grpc::ClientContext* context, const ::kuka::ecs::v1::StopMonitoringRequest& request, ::grpc::CompletionQueue* cq) override; - const ::grpc::internal::RpcMethod rpcmethod_OpenControlChannel_; - const ::grpc::internal::RpcMethod rpcmethod_ObserveControlState_; - const ::grpc::internal::RpcMethod rpcmethod_StartMonitoring_; - const ::grpc::internal::RpcMethod rpcmethod_StopMonitoring_; - }; - static std::unique_ptr NewStub(const std::shared_ptr< ::grpc::ChannelInterface>& channel, const ::grpc::StubOptions& options = ::grpc::StubOptions()); - - class Service : public ::grpc::Service { - public: - Service(); - virtual ~Service(); - // Start external control - virtual ::grpc::Status OpenControlChannel(::grpc::ServerContext* context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response); - // Observe external control state - virtual ::grpc::Status ObserveControlState(::grpc::ServerContext* context, const ::kuka::ecs::v1::ObserveControlStateRequest* request, ::grpc::ServerWriter< ::kuka::ecs::v1::CommandState>* writer); - // Start External motinoring - virtual ::grpc::Status StartMonitoring(::grpc::ServerContext* context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response); - // Stop External monitoring - virtual ::grpc::Status StopMonitoring(::grpc::ServerContext* context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response); - }; - template - class WithAsyncMethod_OpenControlChannel : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithAsyncMethod_OpenControlChannel() { - ::grpc::Service::MarkMethodAsync(0); - } - ~WithAsyncMethod_OpenControlChannel() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status OpenControlChannel(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestOpenControlChannel(::grpc::ServerContext* context, ::kuka::ecs::v1::OpenControlChannelRequest* request, ::grpc::ServerAsyncResponseWriter< ::kuka::ecs::v1::OpenControlChannelResponse>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncUnary(0, context, request, response, new_call_cq, notification_cq, tag); - } - }; - template - class WithAsyncMethod_ObserveControlState : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithAsyncMethod_ObserveControlState() { - ::grpc::Service::MarkMethodAsync(1); - } - ~WithAsyncMethod_ObserveControlState() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status ObserveControlState(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/, ::grpc::ServerWriter< ::kuka::ecs::v1::CommandState>* /*writer*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestObserveControlState(::grpc::ServerContext* context, ::kuka::ecs::v1::ObserveControlStateRequest* request, ::grpc::ServerAsyncWriter< ::kuka::ecs::v1::CommandState>* writer, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncServerStreaming(1, context, request, writer, new_call_cq, notification_cq, tag); - } - }; - template - class WithAsyncMethod_StartMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithAsyncMethod_StartMonitoring() { - ::grpc::Service::MarkMethodAsync(2); - } - ~WithAsyncMethod_StartMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StartMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestStartMonitoring(::grpc::ServerContext* context, ::kuka::ecs::v1::StartMonitoringRequest* request, ::grpc::ServerAsyncResponseWriter< ::kuka::ecs::v1::StartMonitoringResponse>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncUnary(2, context, request, response, new_call_cq, notification_cq, tag); - } - }; - template - class WithAsyncMethod_StopMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithAsyncMethod_StopMonitoring() { - ::grpc::Service::MarkMethodAsync(3); - } - ~WithAsyncMethod_StopMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StopMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestStopMonitoring(::grpc::ServerContext* context, ::kuka::ecs::v1::StopMonitoringRequest* request, ::grpc::ServerAsyncResponseWriter< ::kuka::ecs::v1::StopMonitoringResponse>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncUnary(3, context, request, response, new_call_cq, notification_cq, tag); - } - }; - typedef WithAsyncMethod_OpenControlChannel > > > AsyncService; - template - class ExperimentalWithCallbackMethod_OpenControlChannel : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithCallbackMethod_OpenControlChannel() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodCallback(0, - new ::grpc_impl::internal::CallbackUnaryHandler< ::kuka::ecs::v1::OpenControlChannelRequest, ::kuka::ecs::v1::OpenControlChannelResponse>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const ::kuka::ecs::v1::OpenControlChannelRequest* request, ::kuka::ecs::v1::OpenControlChannelResponse* response) { return this->OpenControlChannel(context, request, response); }));} - void SetMessageAllocatorFor_OpenControlChannel( - ::grpc::experimental::MessageAllocator< ::kuka::ecs::v1::OpenControlChannelRequest, ::kuka::ecs::v1::OpenControlChannelResponse>* allocator) { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::internal::MethodHandler* const handler = ::grpc::Service::GetHandler(0); - #else - ::grpc::internal::MethodHandler* const handler = ::grpc::Service::experimental().GetHandler(0); - #endif - static_cast<::grpc_impl::internal::CallbackUnaryHandler< ::kuka::ecs::v1::OpenControlChannelRequest, ::kuka::ecs::v1::OpenControlChannelResponse>*>(handler) - ->SetMessageAllocator(allocator); - } - ~ExperimentalWithCallbackMethod_OpenControlChannel() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status OpenControlChannel(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerUnaryReactor* OpenControlChannel( - ::grpc::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) - #else - virtual ::grpc::experimental::ServerUnaryReactor* OpenControlChannel( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) - #endif - { return nullptr; } - }; - template - class ExperimentalWithCallbackMethod_ObserveControlState : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithCallbackMethod_ObserveControlState() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodCallback(1, - new ::grpc_impl::internal::CallbackServerStreamingHandler< ::kuka::ecs::v1::ObserveControlStateRequest, ::kuka::ecs::v1::CommandState>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const ::kuka::ecs::v1::ObserveControlStateRequest* request) { return this->ObserveControlState(context, request); })); - } - ~ExperimentalWithCallbackMethod_ObserveControlState() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status ObserveControlState(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/, ::grpc::ServerWriter< ::kuka::ecs::v1::CommandState>* /*writer*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerWriteReactor< ::kuka::ecs::v1::CommandState>* ObserveControlState( - ::grpc::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/) - #else - virtual ::grpc::experimental::ServerWriteReactor< ::kuka::ecs::v1::CommandState>* ObserveControlState( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/) - #endif - { return nullptr; } - }; - template - class ExperimentalWithCallbackMethod_StartMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithCallbackMethod_StartMonitoring() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodCallback(2, - new ::grpc_impl::internal::CallbackUnaryHandler< ::kuka::ecs::v1::StartMonitoringRequest, ::kuka::ecs::v1::StartMonitoringResponse>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const ::kuka::ecs::v1::StartMonitoringRequest* request, ::kuka::ecs::v1::StartMonitoringResponse* response) { return this->StartMonitoring(context, request, response); }));} - void SetMessageAllocatorFor_StartMonitoring( - ::grpc::experimental::MessageAllocator< ::kuka::ecs::v1::StartMonitoringRequest, ::kuka::ecs::v1::StartMonitoringResponse>* allocator) { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::internal::MethodHandler* const handler = ::grpc::Service::GetHandler(2); - #else - ::grpc::internal::MethodHandler* const handler = ::grpc::Service::experimental().GetHandler(2); - #endif - static_cast<::grpc_impl::internal::CallbackUnaryHandler< ::kuka::ecs::v1::StartMonitoringRequest, ::kuka::ecs::v1::StartMonitoringResponse>*>(handler) - ->SetMessageAllocator(allocator); - } - ~ExperimentalWithCallbackMethod_StartMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StartMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerUnaryReactor* StartMonitoring( - ::grpc::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) - #else - virtual ::grpc::experimental::ServerUnaryReactor* StartMonitoring( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) - #endif - { return nullptr; } - }; - template - class ExperimentalWithCallbackMethod_StopMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithCallbackMethod_StopMonitoring() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodCallback(3, - new ::grpc_impl::internal::CallbackUnaryHandler< ::kuka::ecs::v1::StopMonitoringRequest, ::kuka::ecs::v1::StopMonitoringResponse>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const ::kuka::ecs::v1::StopMonitoringRequest* request, ::kuka::ecs::v1::StopMonitoringResponse* response) { return this->StopMonitoring(context, request, response); }));} - void SetMessageAllocatorFor_StopMonitoring( - ::grpc::experimental::MessageAllocator< ::kuka::ecs::v1::StopMonitoringRequest, ::kuka::ecs::v1::StopMonitoringResponse>* allocator) { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::internal::MethodHandler* const handler = ::grpc::Service::GetHandler(3); - #else - ::grpc::internal::MethodHandler* const handler = ::grpc::Service::experimental().GetHandler(3); - #endif - static_cast<::grpc_impl::internal::CallbackUnaryHandler< ::kuka::ecs::v1::StopMonitoringRequest, ::kuka::ecs::v1::StopMonitoringResponse>*>(handler) - ->SetMessageAllocator(allocator); - } - ~ExperimentalWithCallbackMethod_StopMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StopMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerUnaryReactor* StopMonitoring( - ::grpc::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) - #else - virtual ::grpc::experimental::ServerUnaryReactor* StopMonitoring( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) - #endif - { return nullptr; } - }; - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - typedef ExperimentalWithCallbackMethod_OpenControlChannel > > > CallbackService; - #endif - - typedef ExperimentalWithCallbackMethod_OpenControlChannel > > > ExperimentalCallbackService; - template - class WithGenericMethod_OpenControlChannel : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithGenericMethod_OpenControlChannel() { - ::grpc::Service::MarkMethodGeneric(0); - } - ~WithGenericMethod_OpenControlChannel() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status OpenControlChannel(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - }; - template - class WithGenericMethod_ObserveControlState : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithGenericMethod_ObserveControlState() { - ::grpc::Service::MarkMethodGeneric(1); - } - ~WithGenericMethod_ObserveControlState() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status ObserveControlState(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/, ::grpc::ServerWriter< ::kuka::ecs::v1::CommandState>* /*writer*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - }; - template - class WithGenericMethod_StartMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithGenericMethod_StartMonitoring() { - ::grpc::Service::MarkMethodGeneric(2); - } - ~WithGenericMethod_StartMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StartMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - }; - template - class WithGenericMethod_StopMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithGenericMethod_StopMonitoring() { - ::grpc::Service::MarkMethodGeneric(3); - } - ~WithGenericMethod_StopMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StopMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - }; - template - class WithRawMethod_OpenControlChannel : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithRawMethod_OpenControlChannel() { - ::grpc::Service::MarkMethodRaw(0); - } - ~WithRawMethod_OpenControlChannel() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status OpenControlChannel(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestOpenControlChannel(::grpc::ServerContext* context, ::grpc::ByteBuffer* request, ::grpc::ServerAsyncResponseWriter< ::grpc::ByteBuffer>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncUnary(0, context, request, response, new_call_cq, notification_cq, tag); - } - }; - template - class WithRawMethod_ObserveControlState : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithRawMethod_ObserveControlState() { - ::grpc::Service::MarkMethodRaw(1); - } - ~WithRawMethod_ObserveControlState() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status ObserveControlState(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/, ::grpc::ServerWriter< ::kuka::ecs::v1::CommandState>* /*writer*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestObserveControlState(::grpc::ServerContext* context, ::grpc::ByteBuffer* request, ::grpc::ServerAsyncWriter< ::grpc::ByteBuffer>* writer, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncServerStreaming(1, context, request, writer, new_call_cq, notification_cq, tag); - } - }; - template - class WithRawMethod_StartMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithRawMethod_StartMonitoring() { - ::grpc::Service::MarkMethodRaw(2); - } - ~WithRawMethod_StartMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StartMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestStartMonitoring(::grpc::ServerContext* context, ::grpc::ByteBuffer* request, ::grpc::ServerAsyncResponseWriter< ::grpc::ByteBuffer>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncUnary(2, context, request, response, new_call_cq, notification_cq, tag); - } - }; - template - class WithRawMethod_StopMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithRawMethod_StopMonitoring() { - ::grpc::Service::MarkMethodRaw(3); - } - ~WithRawMethod_StopMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StopMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - void RequestStopMonitoring(::grpc::ServerContext* context, ::grpc::ByteBuffer* request, ::grpc::ServerAsyncResponseWriter< ::grpc::ByteBuffer>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) { - ::grpc::Service::RequestAsyncUnary(3, context, request, response, new_call_cq, notification_cq, tag); - } - }; - template - class ExperimentalWithRawCallbackMethod_OpenControlChannel : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithRawCallbackMethod_OpenControlChannel() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodRawCallback(0, - new ::grpc_impl::internal::CallbackUnaryHandler< ::grpc::ByteBuffer, ::grpc::ByteBuffer>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const ::grpc::ByteBuffer* request, ::grpc::ByteBuffer* response) { return this->OpenControlChannel(context, request, response); })); - } - ~ExperimentalWithRawCallbackMethod_OpenControlChannel() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status OpenControlChannel(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerUnaryReactor* OpenControlChannel( - ::grpc::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/, ::grpc::ByteBuffer* /*response*/) - #else - virtual ::grpc::experimental::ServerUnaryReactor* OpenControlChannel( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/, ::grpc::ByteBuffer* /*response*/) - #endif - { return nullptr; } - }; - template - class ExperimentalWithRawCallbackMethod_ObserveControlState : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithRawCallbackMethod_ObserveControlState() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodRawCallback(1, - new ::grpc_impl::internal::CallbackServerStreamingHandler< ::grpc::ByteBuffer, ::grpc::ByteBuffer>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const::grpc::ByteBuffer* request) { return this->ObserveControlState(context, request); })); - } - ~ExperimentalWithRawCallbackMethod_ObserveControlState() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status ObserveControlState(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/, ::grpc::ServerWriter< ::kuka::ecs::v1::CommandState>* /*writer*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerWriteReactor< ::grpc::ByteBuffer>* ObserveControlState( - ::grpc::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/) - #else - virtual ::grpc::experimental::ServerWriteReactor< ::grpc::ByteBuffer>* ObserveControlState( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/) - #endif - { return nullptr; } - }; - template - class ExperimentalWithRawCallbackMethod_StartMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithRawCallbackMethod_StartMonitoring() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodRawCallback(2, - new ::grpc_impl::internal::CallbackUnaryHandler< ::grpc::ByteBuffer, ::grpc::ByteBuffer>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const ::grpc::ByteBuffer* request, ::grpc::ByteBuffer* response) { return this->StartMonitoring(context, request, response); })); - } - ~ExperimentalWithRawCallbackMethod_StartMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StartMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerUnaryReactor* StartMonitoring( - ::grpc::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/, ::grpc::ByteBuffer* /*response*/) - #else - virtual ::grpc::experimental::ServerUnaryReactor* StartMonitoring( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/, ::grpc::ByteBuffer* /*response*/) - #endif - { return nullptr; } - }; - template - class ExperimentalWithRawCallbackMethod_StopMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - ExperimentalWithRawCallbackMethod_StopMonitoring() { - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::Service:: - #else - ::grpc::Service::experimental(). - #endif - MarkMethodRawCallback(3, - new ::grpc_impl::internal::CallbackUnaryHandler< ::grpc::ByteBuffer, ::grpc::ByteBuffer>( - [this]( - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - ::grpc::CallbackServerContext* - #else - ::grpc::experimental::CallbackServerContext* - #endif - context, const ::grpc::ByteBuffer* request, ::grpc::ByteBuffer* response) { return this->StopMonitoring(context, request, response); })); - } - ~ExperimentalWithRawCallbackMethod_StopMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable synchronous version of this method - ::grpc::Status StopMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - #ifdef GRPC_CALLBACK_API_NONEXPERIMENTAL - virtual ::grpc::ServerUnaryReactor* StopMonitoring( - ::grpc::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/, ::grpc::ByteBuffer* /*response*/) - #else - virtual ::grpc::experimental::ServerUnaryReactor* StopMonitoring( - ::grpc::experimental::CallbackServerContext* /*context*/, const ::grpc::ByteBuffer* /*request*/, ::grpc::ByteBuffer* /*response*/) - #endif - { return nullptr; } - }; - template - class WithStreamedUnaryMethod_OpenControlChannel : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithStreamedUnaryMethod_OpenControlChannel() { - ::grpc::Service::MarkMethodStreamed(0, - new ::grpc::internal::StreamedUnaryHandler< - ::kuka::ecs::v1::OpenControlChannelRequest, ::kuka::ecs::v1::OpenControlChannelResponse>( - [this](::grpc_impl::ServerContext* context, - ::grpc_impl::ServerUnaryStreamer< - ::kuka::ecs::v1::OpenControlChannelRequest, ::kuka::ecs::v1::OpenControlChannelResponse>* streamer) { - return this->StreamedOpenControlChannel(context, - streamer); - })); - } - ~WithStreamedUnaryMethod_OpenControlChannel() override { - BaseClassMustBeDerivedFromService(this); - } - // disable regular version of this method - ::grpc::Status OpenControlChannel(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::OpenControlChannelRequest* /*request*/, ::kuka::ecs::v1::OpenControlChannelResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - // replace default version of method with streamed unary - virtual ::grpc::Status StreamedOpenControlChannel(::grpc::ServerContext* context, ::grpc::ServerUnaryStreamer< ::kuka::ecs::v1::OpenControlChannelRequest,::kuka::ecs::v1::OpenControlChannelResponse>* server_unary_streamer) = 0; - }; - template - class WithStreamedUnaryMethod_StartMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithStreamedUnaryMethod_StartMonitoring() { - ::grpc::Service::MarkMethodStreamed(2, - new ::grpc::internal::StreamedUnaryHandler< - ::kuka::ecs::v1::StartMonitoringRequest, ::kuka::ecs::v1::StartMonitoringResponse>( - [this](::grpc_impl::ServerContext* context, - ::grpc_impl::ServerUnaryStreamer< - ::kuka::ecs::v1::StartMonitoringRequest, ::kuka::ecs::v1::StartMonitoringResponse>* streamer) { - return this->StreamedStartMonitoring(context, - streamer); - })); - } - ~WithStreamedUnaryMethod_StartMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable regular version of this method - ::grpc::Status StartMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StartMonitoringRequest* /*request*/, ::kuka::ecs::v1::StartMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - // replace default version of method with streamed unary - virtual ::grpc::Status StreamedStartMonitoring(::grpc::ServerContext* context, ::grpc::ServerUnaryStreamer< ::kuka::ecs::v1::StartMonitoringRequest,::kuka::ecs::v1::StartMonitoringResponse>* server_unary_streamer) = 0; - }; - template - class WithStreamedUnaryMethod_StopMonitoring : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithStreamedUnaryMethod_StopMonitoring() { - ::grpc::Service::MarkMethodStreamed(3, - new ::grpc::internal::StreamedUnaryHandler< - ::kuka::ecs::v1::StopMonitoringRequest, ::kuka::ecs::v1::StopMonitoringResponse>( - [this](::grpc_impl::ServerContext* context, - ::grpc_impl::ServerUnaryStreamer< - ::kuka::ecs::v1::StopMonitoringRequest, ::kuka::ecs::v1::StopMonitoringResponse>* streamer) { - return this->StreamedStopMonitoring(context, - streamer); - })); - } - ~WithStreamedUnaryMethod_StopMonitoring() override { - BaseClassMustBeDerivedFromService(this); - } - // disable regular version of this method - ::grpc::Status StopMonitoring(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::StopMonitoringRequest* /*request*/, ::kuka::ecs::v1::StopMonitoringResponse* /*response*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - // replace default version of method with streamed unary - virtual ::grpc::Status StreamedStopMonitoring(::grpc::ServerContext* context, ::grpc::ServerUnaryStreamer< ::kuka::ecs::v1::StopMonitoringRequest,::kuka::ecs::v1::StopMonitoringResponse>* server_unary_streamer) = 0; - }; - typedef WithStreamedUnaryMethod_OpenControlChannel > > StreamedUnaryService; - template - class WithSplitStreamingMethod_ObserveControlState : public BaseClass { - private: - void BaseClassMustBeDerivedFromService(const Service* /*service*/) {} - public: - WithSplitStreamingMethod_ObserveControlState() { - ::grpc::Service::MarkMethodStreamed(1, - new ::grpc::internal::SplitServerStreamingHandler< - ::kuka::ecs::v1::ObserveControlStateRequest, ::kuka::ecs::v1::CommandState>( - [this](::grpc_impl::ServerContext* context, - ::grpc_impl::ServerSplitStreamer< - ::kuka::ecs::v1::ObserveControlStateRequest, ::kuka::ecs::v1::CommandState>* streamer) { - return this->StreamedObserveControlState(context, - streamer); - })); - } - ~WithSplitStreamingMethod_ObserveControlState() override { - BaseClassMustBeDerivedFromService(this); - } - // disable regular version of this method - ::grpc::Status ObserveControlState(::grpc::ServerContext* /*context*/, const ::kuka::ecs::v1::ObserveControlStateRequest* /*request*/, ::grpc::ServerWriter< ::kuka::ecs::v1::CommandState>* /*writer*/) override { - abort(); - return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, ""); - } - // replace default version of method with split streamed - virtual ::grpc::Status StreamedObserveControlState(::grpc::ServerContext* context, ::grpc::ServerSplitStreamer< ::kuka::ecs::v1::ObserveControlStateRequest,::kuka::ecs::v1::CommandState>* server_split_streamer) = 0; - }; - typedef WithSplitStreamingMethod_ObserveControlState SplitStreamedService; - typedef WithStreamedUnaryMethod_OpenControlChannel > > > StreamedService; -}; - -} // namespace v1 -} // namespace ecs -} // namespace kuka - - -#endif // GRPC_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto__INCLUDED diff --git a/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h b/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h deleted file mode 100644 index 81fac0a8..00000000 --- a/kuka_iiqka_eac_driver/include/mock/kuka/ecs/v1/motion_services_ecs.pb.h +++ /dev/null @@ -1,1508 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! -// source: kuka/ecs/v1/motion_services_ecs.proto - -#ifndef GOOGLE_PROTOBUF_INCLUDED_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto -#define GOOGLE_PROTOBUF_INCLUDED_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto - -#include -#include - -#include -#if PROTOBUF_VERSION < 3012000 -#error This file was generated by a newer version of protoc which is -#error incompatible with your Protocol Buffer headers. Please update -#error your headers. -#endif -#if 3012004 < PROTOBUF_MIN_PROTOC_VERSION -#error This file was generated by an older version of protoc which is -#error incompatible with your Protocol Buffer headers. Please -#error regenerate this file with a newer version of protoc. -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // IWYU pragma: export -#include // IWYU pragma: export -#include -#include -#include "kuka/motion/external/external_control_mode.pb.h" -// @@protoc_insertion_point(includes) -#include -#define PROTOBUF_INTERNAL_EXPORT_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto -PROTOBUF_NAMESPACE_OPEN -namespace internal { -class AnyMetadata; -} // namespace internal -PROTOBUF_NAMESPACE_CLOSE - -// Internal implementation detail -- do not use these members. -struct TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto { - static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTableField entries[] - PROTOBUF_SECTION_VARIABLE(protodesc_cold); - static const ::PROTOBUF_NAMESPACE_ID::internal::AuxillaryParseTableField aux[] - PROTOBUF_SECTION_VARIABLE(protodesc_cold); - static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[8] - PROTOBUF_SECTION_VARIABLE(protodesc_cold); - static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[]; - static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[]; - static const ::PROTOBUF_NAMESPACE_ID::uint32 offsets[]; -}; -extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -namespace kuka { -namespace ecs { -namespace v1 { -class CommandState; -class CommandStateDefaultTypeInternal; -extern CommandStateDefaultTypeInternal _CommandState_default_instance_; -class ObserveControlStateRequest; -class ObserveControlStateRequestDefaultTypeInternal; -extern ObserveControlStateRequestDefaultTypeInternal _ObserveControlStateRequest_default_instance_; -class OpenControlChannelRequest; -class OpenControlChannelRequestDefaultTypeInternal; -extern OpenControlChannelRequestDefaultTypeInternal _OpenControlChannelRequest_default_instance_; -class OpenControlChannelResponse; -class OpenControlChannelResponseDefaultTypeInternal; -extern OpenControlChannelResponseDefaultTypeInternal _OpenControlChannelResponse_default_instance_; -class StartMonitoringRequest; -class StartMonitoringRequestDefaultTypeInternal; -extern StartMonitoringRequestDefaultTypeInternal _StartMonitoringRequest_default_instance_; -class StartMonitoringResponse; -class StartMonitoringResponseDefaultTypeInternal; -extern StartMonitoringResponseDefaultTypeInternal _StartMonitoringResponse_default_instance_; -class StopMonitoringRequest; -class StopMonitoringRequestDefaultTypeInternal; -extern StopMonitoringRequestDefaultTypeInternal _StopMonitoringRequest_default_instance_; -class StopMonitoringResponse; -class StopMonitoringResponseDefaultTypeInternal; -extern StopMonitoringResponseDefaultTypeInternal _StopMonitoringResponse_default_instance_; -} // namespace v1 -} // namespace ecs -} // namespace kuka -PROTOBUF_NAMESPACE_OPEN -template<> ::kuka::ecs::v1::CommandState* Arena::CreateMaybeMessage<::kuka::ecs::v1::CommandState>(Arena*); -template<> ::kuka::ecs::v1::ObserveControlStateRequest* Arena::CreateMaybeMessage<::kuka::ecs::v1::ObserveControlStateRequest>(Arena*); -template<> ::kuka::ecs::v1::OpenControlChannelRequest* Arena::CreateMaybeMessage<::kuka::ecs::v1::OpenControlChannelRequest>(Arena*); -template<> ::kuka::ecs::v1::OpenControlChannelResponse* Arena::CreateMaybeMessage<::kuka::ecs::v1::OpenControlChannelResponse>(Arena*); -template<> ::kuka::ecs::v1::StartMonitoringRequest* Arena::CreateMaybeMessage<::kuka::ecs::v1::StartMonitoringRequest>(Arena*); -template<> ::kuka::ecs::v1::StartMonitoringResponse* Arena::CreateMaybeMessage<::kuka::ecs::v1::StartMonitoringResponse>(Arena*); -template<> ::kuka::ecs::v1::StopMonitoringRequest* Arena::CreateMaybeMessage<::kuka::ecs::v1::StopMonitoringRequest>(Arena*); -template<> ::kuka::ecs::v1::StopMonitoringResponse* Arena::CreateMaybeMessage<::kuka::ecs::v1::StopMonitoringResponse>(Arena*); -PROTOBUF_NAMESPACE_CLOSE -namespace kuka { -namespace ecs { -namespace v1 { - -enum CommandEvent : int { - CONTROL_EVENT_UNSPECIFIED = 0, - IDLE = 1, - COMMAND_READY = 2, - SAMPLING = 3, - STOPPED = 4, - ERROR = 6, - CommandEvent_INT_MIN_SENTINEL_DO_NOT_USE_ = std::numeric_limits<::PROTOBUF_NAMESPACE_ID::int32>::min(), - CommandEvent_INT_MAX_SENTINEL_DO_NOT_USE_ = std::numeric_limits<::PROTOBUF_NAMESPACE_ID::int32>::max() -}; -bool CommandEvent_IsValid(int value); -constexpr CommandEvent CommandEvent_MIN = CONTROL_EVENT_UNSPECIFIED; -constexpr CommandEvent CommandEvent_MAX = ERROR; -constexpr int CommandEvent_ARRAYSIZE = CommandEvent_MAX + 1; - -const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* CommandEvent_descriptor(); -template -inline const std::string& CommandEvent_Name(T enum_t_value) { - static_assert(::std::is_same::value || - ::std::is_integral::value, - "Incorrect type passed to function CommandEvent_Name."); - return ::PROTOBUF_NAMESPACE_ID::internal::NameOfEnum( - CommandEvent_descriptor(), enum_t_value); -} -inline bool CommandEvent_Parse( - const std::string& name, CommandEvent* value) { - return ::PROTOBUF_NAMESPACE_ID::internal::ParseNamedEnum( - CommandEvent_descriptor(), name, value); -} -// =================================================================== - -class OpenControlChannelRequest PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.OpenControlChannelRequest) */ { - public: - inline OpenControlChannelRequest() : OpenControlChannelRequest(nullptr) {}; - virtual ~OpenControlChannelRequest(); - - OpenControlChannelRequest(const OpenControlChannelRequest& from); - OpenControlChannelRequest(OpenControlChannelRequest&& from) noexcept - : OpenControlChannelRequest() { - *this = ::std::move(from); - } - - inline OpenControlChannelRequest& operator=(const OpenControlChannelRequest& from) { - CopyFrom(from); - return *this; - } - inline OpenControlChannelRequest& operator=(OpenControlChannelRequest&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const OpenControlChannelRequest& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const OpenControlChannelRequest* internal_default_instance() { - return reinterpret_cast( - &_OpenControlChannelRequest_default_instance_); - } - static constexpr int kIndexInFileMessages = - 0; - - friend void swap(OpenControlChannelRequest& a, OpenControlChannelRequest& b) { - a.Swap(&b); - } - inline void Swap(OpenControlChannelRequest* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(OpenControlChannelRequest* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline OpenControlChannelRequest* New() const final { - return CreateMaybeMessage(nullptr); - } - - OpenControlChannelRequest* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const OpenControlChannelRequest& from); - void MergeFrom(const OpenControlChannelRequest& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(OpenControlChannelRequest* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.OpenControlChannelRequest"; - } - protected: - explicit OpenControlChannelRequest(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - enum : int { - kVelocityThresholdFieldNumber = 4, - kTimeoutFieldNumber = 1, - kCycleTimeFieldNumber = 2, - kExternalControlModeFieldNumber = 3, - }; - // repeated double velocity_threshold = 4; - int velocity_threshold_size() const; - private: - int _internal_velocity_threshold_size() const; - public: - void clear_velocity_threshold(); - private: - double _internal_velocity_threshold(int index) const; - const ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >& - _internal_velocity_threshold() const; - void _internal_add_velocity_threshold(double value); - ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >* - _internal_mutable_velocity_threshold(); - public: - double velocity_threshold(int index) const; - void set_velocity_threshold(int index, double value); - void add_velocity_threshold(double value); - const ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >& - velocity_threshold() const; - ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >* - mutable_velocity_threshold(); - - // uint32 timeout = 1; - void clear_timeout(); - ::PROTOBUF_NAMESPACE_ID::uint32 timeout() const; - void set_timeout(::PROTOBUF_NAMESPACE_ID::uint32 value); - private: - ::PROTOBUF_NAMESPACE_ID::uint32 _internal_timeout() const; - void _internal_set_timeout(::PROTOBUF_NAMESPACE_ID::uint32 value); - public: - - // uint32 cycle_time = 2; - void clear_cycle_time(); - ::PROTOBUF_NAMESPACE_ID::uint32 cycle_time() const; - void set_cycle_time(::PROTOBUF_NAMESPACE_ID::uint32 value); - private: - ::PROTOBUF_NAMESPACE_ID::uint32 _internal_cycle_time() const; - void _internal_set_cycle_time(::PROTOBUF_NAMESPACE_ID::uint32 value); - public: - - // .kuka.motion.external.ExternalControlMode external_control_mode = 3; - void clear_external_control_mode(); - ::kuka::motion::external::ExternalControlMode external_control_mode() const; - void set_external_control_mode(::kuka::motion::external::ExternalControlMode value); - private: - ::kuka::motion::external::ExternalControlMode _internal_external_control_mode() const; - void _internal_set_external_control_mode(::kuka::motion::external::ExternalControlMode value); - public: - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.OpenControlChannelRequest) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - ::PROTOBUF_NAMESPACE_ID::RepeatedField< double > velocity_threshold_; - mutable std::atomic _velocity_threshold_cached_byte_size_; - ::PROTOBUF_NAMESPACE_ID::uint32 timeout_; - ::PROTOBUF_NAMESPACE_ID::uint32 cycle_time_; - int external_control_mode_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// ------------------------------------------------------------------- - -class OpenControlChannelResponse PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.OpenControlChannelResponse) */ { - public: - inline OpenControlChannelResponse() : OpenControlChannelResponse(nullptr) {}; - virtual ~OpenControlChannelResponse(); - - OpenControlChannelResponse(const OpenControlChannelResponse& from); - OpenControlChannelResponse(OpenControlChannelResponse&& from) noexcept - : OpenControlChannelResponse() { - *this = ::std::move(from); - } - - inline OpenControlChannelResponse& operator=(const OpenControlChannelResponse& from) { - CopyFrom(from); - return *this; - } - inline OpenControlChannelResponse& operator=(OpenControlChannelResponse&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const OpenControlChannelResponse& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const OpenControlChannelResponse* internal_default_instance() { - return reinterpret_cast( - &_OpenControlChannelResponse_default_instance_); - } - static constexpr int kIndexInFileMessages = - 1; - - friend void swap(OpenControlChannelResponse& a, OpenControlChannelResponse& b) { - a.Swap(&b); - } - inline void Swap(OpenControlChannelResponse* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(OpenControlChannelResponse* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline OpenControlChannelResponse* New() const final { - return CreateMaybeMessage(nullptr); - } - - OpenControlChannelResponse* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const OpenControlChannelResponse& from); - void MergeFrom(const OpenControlChannelResponse& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(OpenControlChannelResponse* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.OpenControlChannelResponse"; - } - protected: - explicit OpenControlChannelResponse(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.OpenControlChannelResponse) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// ------------------------------------------------------------------- - -class StartMonitoringRequest PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.StartMonitoringRequest) */ { - public: - inline StartMonitoringRequest() : StartMonitoringRequest(nullptr) {}; - virtual ~StartMonitoringRequest(); - - StartMonitoringRequest(const StartMonitoringRequest& from); - StartMonitoringRequest(StartMonitoringRequest&& from) noexcept - : StartMonitoringRequest() { - *this = ::std::move(from); - } - - inline StartMonitoringRequest& operator=(const StartMonitoringRequest& from) { - CopyFrom(from); - return *this; - } - inline StartMonitoringRequest& operator=(StartMonitoringRequest&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const StartMonitoringRequest& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const StartMonitoringRequest* internal_default_instance() { - return reinterpret_cast( - &_StartMonitoringRequest_default_instance_); - } - static constexpr int kIndexInFileMessages = - 2; - - friend void swap(StartMonitoringRequest& a, StartMonitoringRequest& b) { - a.Swap(&b); - } - inline void Swap(StartMonitoringRequest* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(StartMonitoringRequest* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline StartMonitoringRequest* New() const final { - return CreateMaybeMessage(nullptr); - } - - StartMonitoringRequest* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const StartMonitoringRequest& from); - void MergeFrom(const StartMonitoringRequest& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(StartMonitoringRequest* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.StartMonitoringRequest"; - } - protected: - explicit StartMonitoringRequest(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.StartMonitoringRequest) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// ------------------------------------------------------------------- - -class StartMonitoringResponse PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.StartMonitoringResponse) */ { - public: - inline StartMonitoringResponse() : StartMonitoringResponse(nullptr) {}; - virtual ~StartMonitoringResponse(); - - StartMonitoringResponse(const StartMonitoringResponse& from); - StartMonitoringResponse(StartMonitoringResponse&& from) noexcept - : StartMonitoringResponse() { - *this = ::std::move(from); - } - - inline StartMonitoringResponse& operator=(const StartMonitoringResponse& from) { - CopyFrom(from); - return *this; - } - inline StartMonitoringResponse& operator=(StartMonitoringResponse&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const StartMonitoringResponse& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const StartMonitoringResponse* internal_default_instance() { - return reinterpret_cast( - &_StartMonitoringResponse_default_instance_); - } - static constexpr int kIndexInFileMessages = - 3; - - friend void swap(StartMonitoringResponse& a, StartMonitoringResponse& b) { - a.Swap(&b); - } - inline void Swap(StartMonitoringResponse* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(StartMonitoringResponse* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline StartMonitoringResponse* New() const final { - return CreateMaybeMessage(nullptr); - } - - StartMonitoringResponse* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const StartMonitoringResponse& from); - void MergeFrom(const StartMonitoringResponse& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(StartMonitoringResponse* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.StartMonitoringResponse"; - } - protected: - explicit StartMonitoringResponse(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.StartMonitoringResponse) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// ------------------------------------------------------------------- - -class StopMonitoringRequest PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.StopMonitoringRequest) */ { - public: - inline StopMonitoringRequest() : StopMonitoringRequest(nullptr) {}; - virtual ~StopMonitoringRequest(); - - StopMonitoringRequest(const StopMonitoringRequest& from); - StopMonitoringRequest(StopMonitoringRequest&& from) noexcept - : StopMonitoringRequest() { - *this = ::std::move(from); - } - - inline StopMonitoringRequest& operator=(const StopMonitoringRequest& from) { - CopyFrom(from); - return *this; - } - inline StopMonitoringRequest& operator=(StopMonitoringRequest&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const StopMonitoringRequest& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const StopMonitoringRequest* internal_default_instance() { - return reinterpret_cast( - &_StopMonitoringRequest_default_instance_); - } - static constexpr int kIndexInFileMessages = - 4; - - friend void swap(StopMonitoringRequest& a, StopMonitoringRequest& b) { - a.Swap(&b); - } - inline void Swap(StopMonitoringRequest* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(StopMonitoringRequest* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline StopMonitoringRequest* New() const final { - return CreateMaybeMessage(nullptr); - } - - StopMonitoringRequest* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const StopMonitoringRequest& from); - void MergeFrom(const StopMonitoringRequest& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(StopMonitoringRequest* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.StopMonitoringRequest"; - } - protected: - explicit StopMonitoringRequest(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.StopMonitoringRequest) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// ------------------------------------------------------------------- - -class StopMonitoringResponse PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.StopMonitoringResponse) */ { - public: - inline StopMonitoringResponse() : StopMonitoringResponse(nullptr) {}; - virtual ~StopMonitoringResponse(); - - StopMonitoringResponse(const StopMonitoringResponse& from); - StopMonitoringResponse(StopMonitoringResponse&& from) noexcept - : StopMonitoringResponse() { - *this = ::std::move(from); - } - - inline StopMonitoringResponse& operator=(const StopMonitoringResponse& from) { - CopyFrom(from); - return *this; - } - inline StopMonitoringResponse& operator=(StopMonitoringResponse&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const StopMonitoringResponse& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const StopMonitoringResponse* internal_default_instance() { - return reinterpret_cast( - &_StopMonitoringResponse_default_instance_); - } - static constexpr int kIndexInFileMessages = - 5; - - friend void swap(StopMonitoringResponse& a, StopMonitoringResponse& b) { - a.Swap(&b); - } - inline void Swap(StopMonitoringResponse* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(StopMonitoringResponse* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline StopMonitoringResponse* New() const final { - return CreateMaybeMessage(nullptr); - } - - StopMonitoringResponse* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const StopMonitoringResponse& from); - void MergeFrom(const StopMonitoringResponse& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(StopMonitoringResponse* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.StopMonitoringResponse"; - } - protected: - explicit StopMonitoringResponse(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.StopMonitoringResponse) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// ------------------------------------------------------------------- - -class ObserveControlStateRequest PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.ObserveControlStateRequest) */ { - public: - inline ObserveControlStateRequest() : ObserveControlStateRequest(nullptr) {}; - virtual ~ObserveControlStateRequest(); - - ObserveControlStateRequest(const ObserveControlStateRequest& from); - ObserveControlStateRequest(ObserveControlStateRequest&& from) noexcept - : ObserveControlStateRequest() { - *this = ::std::move(from); - } - - inline ObserveControlStateRequest& operator=(const ObserveControlStateRequest& from) { - CopyFrom(from); - return *this; - } - inline ObserveControlStateRequest& operator=(ObserveControlStateRequest&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const ObserveControlStateRequest& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const ObserveControlStateRequest* internal_default_instance() { - return reinterpret_cast( - &_ObserveControlStateRequest_default_instance_); - } - static constexpr int kIndexInFileMessages = - 6; - - friend void swap(ObserveControlStateRequest& a, ObserveControlStateRequest& b) { - a.Swap(&b); - } - inline void Swap(ObserveControlStateRequest* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(ObserveControlStateRequest* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline ObserveControlStateRequest* New() const final { - return CreateMaybeMessage(nullptr); - } - - ObserveControlStateRequest* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const ObserveControlStateRequest& from); - void MergeFrom(const ObserveControlStateRequest& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(ObserveControlStateRequest* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.ObserveControlStateRequest"; - } - protected: - explicit ObserveControlStateRequest(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.ObserveControlStateRequest) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// ------------------------------------------------------------------- - -class CommandState PROTOBUF_FINAL : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:kuka.ecs.v1.CommandState) */ { - public: - inline CommandState() : CommandState(nullptr) {}; - virtual ~CommandState(); - - CommandState(const CommandState& from); - CommandState(CommandState&& from) noexcept - : CommandState() { - *this = ::std::move(from); - } - - inline CommandState& operator=(const CommandState& from) { - CopyFrom(from); - return *this; - } - inline CommandState& operator=(CommandState&& from) noexcept { - if (GetArena() == from.GetArena()) { - if (this != &from) InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return GetMetadataStatic().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return GetMetadataStatic().reflection; - } - static const CommandState& default_instance(); - - static void InitAsDefaultInstance(); // FOR INTERNAL USE ONLY - static inline const CommandState* internal_default_instance() { - return reinterpret_cast( - &_CommandState_default_instance_); - } - static constexpr int kIndexInFileMessages = - 7; - - friend void swap(CommandState& a, CommandState& b) { - a.Swap(&b); - } - inline void Swap(CommandState* other) { - if (other == this) return; - if (GetArena() == other->GetArena()) { - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(CommandState* other) { - if (other == this) return; - GOOGLE_DCHECK(GetArena() == other->GetArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - inline CommandState* New() const final { - return CreateMaybeMessage(nullptr); - } - - CommandState* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final { - return CreateMaybeMessage(arena); - } - void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final; - void CopyFrom(const CommandState& from); - void MergeFrom(const CommandState& from); - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize( - ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _cached_size_.Get(); } - - private: - inline void SharedCtor(); - inline void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(CommandState* other); - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "kuka.ecs.v1.CommandState"; - } - protected: - explicit CommandState(::PROTOBUF_NAMESPACE_ID::Arena* arena); - private: - static void ArenaDtor(void* object); - inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena); - public: - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - private: - static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() { - ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto); - return ::descriptor_table_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto.file_level_metadata[kIndexInFileMessages]; - } - - public: - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - enum : int { - kMessageFieldNumber = 2, - kEventFieldNumber = 1, - }; - // string message = 2; - void clear_message(); - const std::string& message() const; - void set_message(const std::string& value); - void set_message(std::string&& value); - void set_message(const char* value); - void set_message(const char* value, size_t size); - std::string* mutable_message(); - std::string* release_message(); - void set_allocated_message(std::string* message); - GOOGLE_PROTOBUF_RUNTIME_DEPRECATED("The unsafe_arena_ accessors for" - " string fields are deprecated and will be removed in a" - " future release.") - std::string* unsafe_arena_release_message(); - GOOGLE_PROTOBUF_RUNTIME_DEPRECATED("The unsafe_arena_ accessors for" - " string fields are deprecated and will be removed in a" - " future release.") - void unsafe_arena_set_allocated_message( - std::string* message); - private: - const std::string& _internal_message() const; - void _internal_set_message(const std::string& value); - std::string* _internal_mutable_message(); - public: - - // .kuka.ecs.v1.CommandEvent event = 1; - void clear_event(); - ::kuka::ecs::v1::CommandEvent event() const; - void set_event(::kuka::ecs::v1::CommandEvent value); - private: - ::kuka::ecs::v1::CommandEvent _internal_event() const; - void _internal_set_event(::kuka::ecs::v1::CommandEvent value); - public: - - // @@protoc_insertion_point(class_scope:kuka.ecs.v1.CommandState) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr message_; - int event_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - friend struct ::TableStruct_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto; -}; -// =================================================================== - - -// =================================================================== - -#ifdef __GNUC__ - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wstrict-aliasing" -#endif // __GNUC__ -// OpenControlChannelRequest - -// uint32 timeout = 1; -inline void OpenControlChannelRequest::clear_timeout() { - timeout_ = 0u; -} -inline ::PROTOBUF_NAMESPACE_ID::uint32 OpenControlChannelRequest::_internal_timeout() const { - return timeout_; -} -inline ::PROTOBUF_NAMESPACE_ID::uint32 OpenControlChannelRequest::timeout() const { - // @@protoc_insertion_point(field_get:kuka.ecs.v1.OpenControlChannelRequest.timeout) - return _internal_timeout(); -} -inline void OpenControlChannelRequest::_internal_set_timeout(::PROTOBUF_NAMESPACE_ID::uint32 value) { - - timeout_ = value; -} -inline void OpenControlChannelRequest::set_timeout(::PROTOBUF_NAMESPACE_ID::uint32 value) { - _internal_set_timeout(value); - // @@protoc_insertion_point(field_set:kuka.ecs.v1.OpenControlChannelRequest.timeout) -} - -// uint32 cycle_time = 2; -inline void OpenControlChannelRequest::clear_cycle_time() { - cycle_time_ = 0u; -} -inline ::PROTOBUF_NAMESPACE_ID::uint32 OpenControlChannelRequest::_internal_cycle_time() const { - return cycle_time_; -} -inline ::PROTOBUF_NAMESPACE_ID::uint32 OpenControlChannelRequest::cycle_time() const { - // @@protoc_insertion_point(field_get:kuka.ecs.v1.OpenControlChannelRequest.cycle_time) - return _internal_cycle_time(); -} -inline void OpenControlChannelRequest::_internal_set_cycle_time(::PROTOBUF_NAMESPACE_ID::uint32 value) { - - cycle_time_ = value; -} -inline void OpenControlChannelRequest::set_cycle_time(::PROTOBUF_NAMESPACE_ID::uint32 value) { - _internal_set_cycle_time(value); - // @@protoc_insertion_point(field_set:kuka.ecs.v1.OpenControlChannelRequest.cycle_time) -} - -// .kuka.motion.external.ExternalControlMode external_control_mode = 3; -inline void OpenControlChannelRequest::clear_external_control_mode() { - external_control_mode_ = 0; -} -inline ::kuka::motion::external::ExternalControlMode OpenControlChannelRequest::_internal_external_control_mode() const { - return static_cast< ::kuka::motion::external::ExternalControlMode >(external_control_mode_); -} -inline ::kuka::motion::external::ExternalControlMode OpenControlChannelRequest::external_control_mode() const { - // @@protoc_insertion_point(field_get:kuka.ecs.v1.OpenControlChannelRequest.external_control_mode) - return _internal_external_control_mode(); -} -inline void OpenControlChannelRequest::_internal_set_external_control_mode(::kuka::motion::external::ExternalControlMode value) { - - external_control_mode_ = value; -} -inline void OpenControlChannelRequest::set_external_control_mode(::kuka::motion::external::ExternalControlMode value) { - _internal_set_external_control_mode(value); - // @@protoc_insertion_point(field_set:kuka.ecs.v1.OpenControlChannelRequest.external_control_mode) -} - -// repeated double velocity_threshold = 4; -inline int OpenControlChannelRequest::_internal_velocity_threshold_size() const { - return velocity_threshold_.size(); -} -inline int OpenControlChannelRequest::velocity_threshold_size() const { - return _internal_velocity_threshold_size(); -} -inline void OpenControlChannelRequest::clear_velocity_threshold() { - velocity_threshold_.Clear(); -} -inline double OpenControlChannelRequest::_internal_velocity_threshold(int index) const { - return velocity_threshold_.Get(index); -} -inline double OpenControlChannelRequest::velocity_threshold(int index) const { - // @@protoc_insertion_point(field_get:kuka.ecs.v1.OpenControlChannelRequest.velocity_threshold) - return _internal_velocity_threshold(index); -} -inline void OpenControlChannelRequest::set_velocity_threshold(int index, double value) { - velocity_threshold_.Set(index, value); - // @@protoc_insertion_point(field_set:kuka.ecs.v1.OpenControlChannelRequest.velocity_threshold) -} -inline void OpenControlChannelRequest::_internal_add_velocity_threshold(double value) { - velocity_threshold_.Add(value); -} -inline void OpenControlChannelRequest::add_velocity_threshold(double value) { - _internal_add_velocity_threshold(value); - // @@protoc_insertion_point(field_add:kuka.ecs.v1.OpenControlChannelRequest.velocity_threshold) -} -inline const ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >& -OpenControlChannelRequest::_internal_velocity_threshold() const { - return velocity_threshold_; -} -inline const ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >& -OpenControlChannelRequest::velocity_threshold() const { - // @@protoc_insertion_point(field_list:kuka.ecs.v1.OpenControlChannelRequest.velocity_threshold) - return _internal_velocity_threshold(); -} -inline ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >* -OpenControlChannelRequest::_internal_mutable_velocity_threshold() { - return &velocity_threshold_; -} -inline ::PROTOBUF_NAMESPACE_ID::RepeatedField< double >* -OpenControlChannelRequest::mutable_velocity_threshold() { - // @@protoc_insertion_point(field_mutable_list:kuka.ecs.v1.OpenControlChannelRequest.velocity_threshold) - return _internal_mutable_velocity_threshold(); -} - -// ------------------------------------------------------------------- - -// OpenControlChannelResponse - -// ------------------------------------------------------------------- - -// StartMonitoringRequest - -// ------------------------------------------------------------------- - -// StartMonitoringResponse - -// ------------------------------------------------------------------- - -// StopMonitoringRequest - -// ------------------------------------------------------------------- - -// StopMonitoringResponse - -// ------------------------------------------------------------------- - -// ObserveControlStateRequest - -// ------------------------------------------------------------------- - -// CommandState - -// .kuka.ecs.v1.CommandEvent event = 1; -inline void CommandState::clear_event() { - event_ = 0; -} -inline ::kuka::ecs::v1::CommandEvent CommandState::_internal_event() const { - return static_cast< ::kuka::ecs::v1::CommandEvent >(event_); -} -inline ::kuka::ecs::v1::CommandEvent CommandState::event() const { - // @@protoc_insertion_point(field_get:kuka.ecs.v1.CommandState.event) - return _internal_event(); -} -inline void CommandState::_internal_set_event(::kuka::ecs::v1::CommandEvent value) { - - event_ = value; -} -inline void CommandState::set_event(::kuka::ecs::v1::CommandEvent value) { - _internal_set_event(value); - // @@protoc_insertion_point(field_set:kuka.ecs.v1.CommandState.event) -} - -// string message = 2; -inline void CommandState::clear_message() { - message_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena()); -} -inline const std::string& CommandState::message() const { - // @@protoc_insertion_point(field_get:kuka.ecs.v1.CommandState.message) - return _internal_message(); -} -inline void CommandState::set_message(const std::string& value) { - _internal_set_message(value); - // @@protoc_insertion_point(field_set:kuka.ecs.v1.CommandState.message) -} -inline std::string* CommandState::mutable_message() { - // @@protoc_insertion_point(field_mutable:kuka.ecs.v1.CommandState.message) - return _internal_mutable_message(); -} -inline const std::string& CommandState::_internal_message() const { - return message_.Get(); -} -inline void CommandState::_internal_set_message(const std::string& value) { - - message_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), value, GetArena()); -} -inline void CommandState::set_message(std::string&& value) { - - message_.Set( - &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::move(value), GetArena()); - // @@protoc_insertion_point(field_set_rvalue:kuka.ecs.v1.CommandState.message) -} -inline void CommandState::set_message(const char* value) { - GOOGLE_DCHECK(value != nullptr); - - message_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(value), - GetArena()); - // @@protoc_insertion_point(field_set_char:kuka.ecs.v1.CommandState.message) -} -inline void CommandState::set_message(const char* value, - size_t size) { - - message_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string( - reinterpret_cast(value), size), GetArena()); - // @@protoc_insertion_point(field_set_pointer:kuka.ecs.v1.CommandState.message) -} -inline std::string* CommandState::_internal_mutable_message() { - - return message_.Mutable(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena()); -} -inline std::string* CommandState::release_message() { - // @@protoc_insertion_point(field_release:kuka.ecs.v1.CommandState.message) - return message_.Release(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena()); -} -inline void CommandState::set_allocated_message(std::string* message) { - if (message != nullptr) { - - } else { - - } - message_.SetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), message, - GetArena()); - // @@protoc_insertion_point(field_set_allocated:kuka.ecs.v1.CommandState.message) -} -inline std::string* CommandState::unsafe_arena_release_message() { - // @@protoc_insertion_point(field_unsafe_arena_release:kuka.ecs.v1.CommandState.message) - GOOGLE_DCHECK(GetArena() != nullptr); - - return message_.UnsafeArenaRelease(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), - GetArena()); -} -inline void CommandState::unsafe_arena_set_allocated_message( - std::string* message) { - GOOGLE_DCHECK(GetArena() != nullptr); - if (message != nullptr) { - - } else { - - } - message_.UnsafeArenaSetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), - message, GetArena()); - // @@protoc_insertion_point(field_unsafe_arena_set_allocated:kuka.ecs.v1.CommandState.message) -} - -#ifdef __GNUC__ - #pragma GCC diagnostic pop -#endif // __GNUC__ -// ------------------------------------------------------------------- - -// ------------------------------------------------------------------- - -// ------------------------------------------------------------------- - -// ------------------------------------------------------------------- - -// ------------------------------------------------------------------- - -// ------------------------------------------------------------------- - -// ------------------------------------------------------------------- - - -// @@protoc_insertion_point(namespace_scope) - -} // namespace v1 -} // namespace ecs -} // namespace kuka - -PROTOBUF_NAMESPACE_OPEN - -template <> struct is_proto_enum< ::kuka::ecs::v1::CommandEvent> : ::std::true_type {}; -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::kuka::ecs::v1::CommandEvent>() { - return ::kuka::ecs::v1::CommandEvent_descriptor(); -} - -PROTOBUF_NAMESPACE_CLOSE - -// @@protoc_insertion_point(global_scope) - -#include -#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_kuka_2fecs_2fv1_2fmotion_5fservices_5fecs_2eproto diff --git a/kuka_iiqka_eac_driver/include/mock/kuka/motion/external/external_control_mode.pb.h b/kuka_iiqka_eac_driver/include/mock/kuka/motion/external/external_control_mode.pb.h deleted file mode 100644 index 5bdf3b51..00000000 --- a/kuka_iiqka_eac_driver/include/mock/kuka/motion/external/external_control_mode.pb.h +++ /dev/null @@ -1,162 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! -// source: kuka/motion/external/external_control_mode.proto - -#ifndef GOOGLE_PROTOBUF_INCLUDED_kuka_2fmotion_2fexternal_2fexternal_5fcontrol_5fmode_2eproto -#define GOOGLE_PROTOBUF_INCLUDED_kuka_2fmotion_2fexternal_2fexternal_5fcontrol_5fmode_2eproto - -#include -#include - -#include -#if PROTOBUF_VERSION < 3012000 -#error This file was generated by a newer version of protoc which is -#error incompatible with your Protocol Buffer headers. Please update -#error your headers. -#endif -#if 3012004 < PROTOBUF_MIN_PROTOC_VERSION -#error This file was generated by an older version of protoc which is -#error incompatible with your Protocol Buffer headers. Please -#error regenerate this file with a newer version of protoc. -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // IWYU pragma: export -#include // IWYU pragma: export -#include -#include -#include -// @@protoc_insertion_point(includes) -#include -#define PROTOBUF_INTERNAL_EXPORT_kuka_2fmotion_2fexternal_2fexternal_5fcontrol_5fmode_2eproto -PROTOBUF_NAMESPACE_OPEN -namespace internal { -class AnyMetadata; -} // namespace internal -PROTOBUF_NAMESPACE_CLOSE - -// Internal implementation detail -- do not use these members. -struct TableStruct_kuka_2fmotion_2fexternal_2fexternal_5fcontrol_5fmode_2eproto { - static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTableField entries[] - PROTOBUF_SECTION_VARIABLE(protodesc_cold); - static const ::PROTOBUF_NAMESPACE_ID::internal::AuxillaryParseTableField aux[] - PROTOBUF_SECTION_VARIABLE(protodesc_cold); - static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[1] - PROTOBUF_SECTION_VARIABLE(protodesc_cold); - static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[]; - static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[]; - static const ::PROTOBUF_NAMESPACE_ID::uint32 offsets[]; -}; -extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_kuka_2fmotion_2fexternal_2fexternal_5fcontrol_5fmode_2eproto; -PROTOBUF_NAMESPACE_OPEN -PROTOBUF_NAMESPACE_CLOSE -namespace kuka { -namespace motion { -namespace external { - -std::string control_mode_string; - -enum ExternalControlMode : int { - EXTERNAL_CONTROL_MODE_UNSPECIFIED = 0, - JOINT_POSITION_CONTROL = 1, - JOINT_IMPEDANCE_CONTROL = 2, - JOINT_VELOCITY_CONTROL = 3, - JOINT_TORQUE_CONTROL = 4, - CARTESIAN_POSITION_CONTROL = 5, - CARTESIAN_IMPEDANCE_CONTROL = 6, - CARTESIAN_VELOCITY_CONTROL = 7, - WRENCH_CONTROL = 8, - ExternalControlMode_INT_MIN_SENTINEL_DO_NOT_USE_ = std::numeric_limits<::PROTOBUF_NAMESPACE_ID::int32>::min(), - ExternalControlMode_INT_MAX_SENTINEL_DO_NOT_USE_ = std::numeric_limits<::PROTOBUF_NAMESPACE_ID::int32>::max() -}; -bool ExternalControlMode_IsValid(int value); -constexpr ExternalControlMode ExternalControlMode_MIN = EXTERNAL_CONTROL_MODE_UNSPECIFIED; -constexpr ExternalControlMode ExternalControlMode_MAX = WRENCH_CONTROL; -constexpr int ExternalControlMode_ARRAYSIZE = ExternalControlMode_MAX + 1; - -const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* ExternalControlMode_descriptor(); -template -inline const std::string& ExternalControlMode_Name(T enum_t_value) { - static_assert(::std::is_same::value || - ::std::is_integral::value, - "Incorrect type passed to function ExternalControlMode_Name."); - switch(enum_t_value) - { - case ExternalControlMode::JOINT_POSITION_CONTROL: - control_mode_string = "JOINT_POSITION_CONTROL"; - break; - case ExternalControlMode::JOINT_IMPEDANCE_CONTROL: - control_mode_string = "JOINT_IMPEDANCE_CONTROL"; - break; - case ExternalControlMode::JOINT_VELOCITY_CONTROL: - control_mode_string = "JOINT_VELOCITY_CONTROL"; - break; - case ExternalControlMode::JOINT_TORQUE_CONTROL: - control_mode_string = "JOINT_TORQUE_CONTROL"; - break; - case ExternalControlMode::CARTESIAN_POSITION_CONTROL: - control_mode_string = "CARTESIAN_POSITION_CONTROL"; - break; - case ExternalControlMode::CARTESIAN_IMPEDANCE_CONTROL: - control_mode_string = "CARTESIAN_IMPEDANCE_CONTROL"; - break; - case ExternalControlMode::CARTESIAN_VELOCITY_CONTROL: - control_mode_string = "CARTESIAN_VELOCITY_CONTROL"; - break; - case ExternalControlMode::WRENCH_CONTROL: - control_mode_string = "WRENCH_CONTROL"; - break; - default: - control_mode_string = "EXTERNAL_CONTROL_MODE_UNSPECIFIED"; - break; - } - return control_mode_string; -} -inline bool ExternalControlMode_Parse( - const std::string& name, ExternalControlMode* value) { - return ::PROTOBUF_NAMESPACE_ID::internal::ParseNamedEnum( - ExternalControlMode_descriptor(), name, value); -} -// =================================================================== - - -// =================================================================== - - -// =================================================================== - -#ifdef __GNUC__ - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wstrict-aliasing" -#endif // __GNUC__ -#ifdef __GNUC__ - #pragma GCC diagnostic pop -#endif // __GNUC__ - -// @@protoc_insertion_point(namespace_scope) - -} // namespace external -} // namespace motion -} // namespace kuka - -PROTOBUF_NAMESPACE_OPEN - -template <> struct is_proto_enum< ::kuka::motion::external::ExternalControlMode> : ::std::true_type {}; -template <> -inline const EnumDescriptor* GetEnumDescriptor< ::kuka::motion::external::ExternalControlMode>() { - return ::kuka::motion::external::ExternalControlMode_descriptor(); -} - -PROTOBUF_NAMESPACE_CLOSE - -// @@protoc_insertion_point(global_scope) - -#include -#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_kuka_2fmotion_2fexternal_2fexternal_5fcontrol_5fmode_2eproto diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h b/kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h deleted file mode 100644 index d887e822..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb-helpers/nanopb_serialization_helper.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace nanopb{ - -template -inline int Encode(const T &, uint8_t *, size_t ) { - // only a mock - return -1; -} - -template -inline bool Decode(const uint8_t *, size_t , T &) { - // only a mock - return false; -} -} // namespace nanopb diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h deleted file mode 100644 index a5805e1e..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef PB_KUKA_CORE_MOTION_KUKA_CORE_MOTION_JOINT_PB_H_INCLUDED -#define PB_KUKA_CORE_MOTION_KUKA_CORE_MOTION_JOINT_PB_H_INCLUDED -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -/* An array of joint positions for a given kinematic structure. */ -typedef struct _kuka_core_motion_JointPositions { - pb_size_t values_count; - double values[24]; -} kuka_core_motion_JointPositions; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Initializer values for message structs */ -#define kuka_core_motion_JointPositions_init_default {0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}} -#define kuka_core_motion_JointPositions_init_zero {0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}} - -/* Maximum encoded size of messages (where known) */ -#define kuka_core_motion_JointPositions_size 216 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh deleted file mode 100644 index 95b38f29..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/core/motion/joint.pb.hh +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef PB_KUKA_CORE_MOTION_JOINT_PB_HH_INCLUDED -#define PB_KUKA_CORE_MOTION_JOINT_PB_HH_INCLUDED - -#include -#include "nanopb/kuka/core/motion/joint.pb.h" - -namespace nanopb::kuka::core::motion { - -using JointPositions = kuka_core_motion_JointPositions; - - -constexpr JointPositions JointPositions_init_default kuka_core_motion_JointPositions_init_default; - -} // namespace nanopb::kuka::core::motion - -#endif // PB_KUKA_CORE_MOTION_JOINT_PB_HH_INCLUDED diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h deleted file mode 100644 index 72bf2ba9..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef PB_KUKA_ECS_V1_KUKA_ECS_V1_CONTROL_SIGNAL_EXTERNAL_PB_H_INCLUDED -#define PB_KUKA_ECS_V1_KUKA_ECS_V1_CONTROL_SIGNAL_EXTERNAL_PB_H_INCLUDED -#include -#include -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -typedef struct _kuka_ecs_v1_ControlSignalExternal { - bool has_header; - kuka_ecs_v1_ExternalHeader header; - - bool has_control_signal; - kuka_motion_external_ControlSignalInternal control_signal; -} kuka_ecs_v1_ControlSignalExternal; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Initializer values for message structs */ -#define kuka_ecs_v1_ControlSignalExternal_init_default {false, kuka_ecs_v1_ExternalHeader_init_default, false, kuka_motion_external_ControlSignalInternal_init_default} -#define kuka_ecs_v1_ControlSignalExternal_init_zero {false, kuka_ecs_v1_ExternalHeader_init_zero, false, kuka_motion_external_ControlSignalInternal_init_zero} - - -/* Maximum encoded size of messages (where known) */ -#if defined(kuka_motion_external_ControlSignalInternal_size) -#define kuka_ecs_v1_ControlSignalExternal_size (20 + kuka_motion_external_ControlSignalInternal_size) -#endif - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.hh b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.hh deleted file mode 100644 index 7f425571..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/control_signal_external.pb.hh +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef PB_KUKA_ECS_V1_CONTROL_SIGNAL_EXTERNAL_PB_HH_INCLUDED -#define PB_KUKA_ECS_V1_CONTROL_SIGNAL_EXTERNAL_PB_HH_INCLUDED - -#include -#include "nanopb/kuka/ecs/v1/control_signal_external.pb.h" - -namespace nanopb::kuka::ecs::v1 { - -using ControlSignalExternal = kuka_ecs_v1_ControlSignalExternal; - -constexpr ControlSignalExternal ControlSignalExternal_init_default kuka_ecs_v1_ControlSignalExternal_init_default; - -} // namespace nanopb::kuka::ecs::v1 - -#endif // PB_KUKA_ECS_V1_CONTROL_SIGNAL_EXTERNAL_PB_HH_INCLUDED diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h deleted file mode 100644 index f8ba8acc..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/external_header.pb.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef PB_KUKA_ECS_V1_KUKA_ECS_V1_EXTERNAL_HEADER_PB_H_INCLUDED -#define PB_KUKA_ECS_V1_KUKA_ECS_V1_EXTERNAL_HEADER_PB_H_INCLUDED -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -typedef struct _kuka_ecs_v1_ExternalHeader { - uint32_t message_id; - uint32_t ipoc; -} kuka_ecs_v1_ExternalHeader; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Initializer values for message structs */ -#define kuka_ecs_v1_ExternalHeader_init_default {0, 0} -#define kuka_ecs_v1_ExternalHeader_init_zero {0, 0} - -/* Maximum encoded size of messages (where known) */ -#define kuka_ecs_v1_ExternalHeader_size 12 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.h deleted file mode 100644 index a4c6d2a7..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef PB_KUKA_ECS_V1_KUKA_ECS_V1_MOTION_STATE_EXTERNAL_PB_H_INCLUDED -#define PB_KUKA_ECS_V1_KUKA_ECS_V1_MOTION_STATE_EXTERNAL_PB_H_INCLUDED -#include -#include -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -typedef struct _kuka_ecs_v1_MotionStateExternal { - bool has_header; - kuka_ecs_v1_ExternalHeader header; - - bool has_motion_state; - kuka_motion_external_MotionStateInternal motion_state; -} kuka_ecs_v1_MotionStateExternal; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Initializer values for message structs */ -#define kuka_ecs_v1_MotionStateExternal_init_default {false, kuka_ecs_v1_ExternalHeader_init_default, false, kuka_motion_external_MotionStateInternal_init_default} -#define kuka_ecs_v1_MotionStateExternal_init_zero {false, kuka_ecs_v1_ExternalHeader_init_zero, false, kuka_motion_external_MotionStateInternal_init_zero} - -/* Maximum encoded size of messages (where known) */ -#if defined(kuka_motion_external_MotionStateInternal_size) -#define kuka_ecs_v1_MotionStateExternal_size (20 + kuka_motion_external_MotionStateInternal_size) -#endif - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.hh b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.hh deleted file mode 100644 index 2bc48b32..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/ecs/v1/motion_state_external.pb.hh +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef PB_KUKA_ECS_V1_MOTION_STATE_EXTERNAL_PB_HH_INCLUDED -#define PB_KUKA_ECS_V1_MOTION_STATE_EXTERNAL_PB_HH_INCLUDED - -#include -#include "nanopb/kuka/ecs/v1/motion_state_external.pb.h" - -namespace nanopb::kuka::ecs::v1 { - -using MotionStateExternal = kuka_ecs_v1_MotionStateExternal; - -constexpr MotionStateExternal MotionStateExternal_init_default kuka_ecs_v1_MotionStateExternal_init_default; - -} // namespace nanopb::kuka::ecs::v1 - -#endif // PB_KUKA_ECS_V1_MOTION_STATE_EXTERNAL_PB_HH_INCLUDED diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h deleted file mode 100644 index f8ee2087..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_attributes.pb.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_CONTROL_ATTRIBUTES_PB_H_INCLUDED -#define PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_CONTROL_ATTRIBUTES_PB_H_INCLUDED -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -typedef struct _kuka_motion_external_CartesianImpedanceControlAttributes { - pb_size_t stiffness_count; - double stiffness[24]; - - pb_size_t damping_count; - double damping[24]; - - pb_size_t nullspace_stiffness_count; - double nullspace_stiffness[24]; - - pb_size_t nullspace_damping_count; - double nullspace_damping[24]; -} kuka_motion_external_CartesianImpedanceControlAttributes; - -typedef struct _kuka_motion_external_JointImpedanceControlAttributes { - pb_size_t stiffness_count; - double stiffness[24]; - - pb_size_t damping_count; - double damping[24]; -} kuka_motion_external_JointImpedanceControlAttributes; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Initializer values for message structs */ -#define kuka_motion_external_JointImpedanceControlAttributes_init_default {0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}} -#define kuka_motion_external_CartesianImpedanceControlAttributes_init_default {0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}} -#define kuka_motion_external_JointImpedanceControlAttributes_init_zero {0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}} -#define kuka_motion_external_CartesianImpedanceControlAttributes_init_zero {0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}} - -/* Maximum encoded size of messages (where known) */ -#define kuka_motion_external_CartesianImpedanceControlAttributes_size 864 -#define kuka_motion_external_JointImpedanceControlAttributes_size 432 - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h deleted file mode 100644 index 9982aae1..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/control_signal_internal.pb.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_CONTROL_SIGNAL_INTERNAL_PB_H_INCLUDED -#define PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_CONTROL_SIGNAL_INTERNAL_PB_H_INCLUDED -#include -#include -#include -#include - - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -typedef struct _kuka_core_geometry_Vector { - double x; - double y; - double z; -} kuka_core_geometry_Vector; - -#define kuka_core_geometry_Vector_init_default {0, 0, 0} -#define kuka_core_geometry_Vector_init_zero {0, 0, 0} -#define kuka_core_geometry_Vector_size 27 - -typedef struct _kuka_core_geometry_Quaternion { - double qx; - double qy; - double qz; - double qw; -} kuka_core_geometry_Quaternion; - -#define kuka_core_geometry_Quaternion_init_default {0, 0, 0, 0} -#define kuka_core_geometry_Quaternion_init_zero {0, 0, 0, 0} -#define kuka_core_geometry_Quaternion_size 36 - -typedef struct _kuka_core_geometry_Transform { - bool has_translation; - kuka_core_geometry_Vector translation; - bool has_rotation; - kuka_core_geometry_Quaternion rotation; -} kuka_core_geometry_Transform; - -#define kuka_core_geometry_Transform_init_default {false, kuka_core_geometry_Vector_init_default, false, kuka_core_geometry_Quaternion_init_default} -#define kuka_core_geometry_Transform_init_zero {false, kuka_core_geometry_Vector_init_zero, false, kuka_core_geometry_Quaternion_init_zero} - -typedef struct _kuka_core_motion_Twist { - bool has_linear; - kuka_core_geometry_Vector linear; - bool has_angular; - kuka_core_geometry_Vector angular; -} kuka_core_motion_Twist; - -#define kuka_core_motion_Twist_init_default {false, kuka_core_geometry_Vector_init_default, false, kuka_core_geometry_Vector_init_default} -#define kuka_core_motion_Twist_init_zero {false, kuka_core_geometry_Vector_init_zero, false, kuka_core_geometry_Vector_init_zero} -#define kuka_core_motion_Twist_size 58 - - -typedef struct _kuka_motion_external_ControlSignalInternal { - bool stop_ipo; - - bool has_joint_command; - kuka_core_motion_JointPositions joint_command; - - bool has_cartesian_command; - kuka_core_geometry_Transform cartesian_command; - - bool has_joint_velocity_command; - kuka_core_motion_JointPositions joint_velocity_command; - - bool has_twist_command; - kuka_core_motion_Twist twist_command; - - bool has_joint_torque_command; - kuka_core_motion_JointPositions joint_torque_command; - - bool has_wrench_command; - kuka_core_motion_JointPositions wrench_command; - - bool has_joint_attributes; - kuka_motion_external_JointImpedanceControlAttributes joint_attributes; - - bool has_cartesian_attributes; - kuka_motion_external_CartesianImpedanceControlAttributes cartesian_attributes; - - kuka_motion_external_ExternalControlMode control_mode; -} kuka_motion_external_ControlSignalInternal; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Initializer values for message structs */ -#define kuka_motion_external_ControlSignalInternal_init_default {0, false, kuka_core_motion_JointPositions_init_default, false, kuka_core_geometry_Transform_init_default, false, kuka_core_motion_JointPositions_init_default, false, kuka_core_motion_Twist_init_default, false, kuka_core_motion_JointPositions_init_default, false, kuka_core_motion_JointPositions_init_default, false, kuka_motion_external_JointImpedanceControlAttributes_init_default, false, kuka_motion_external_CartesianImpedanceControlAttributes_init_default, _kuka_motion_external_ExternalControlMode_MIN} -#define kuka_motion_external_ControlSignalInternal_init_zero {0, false, kuka_core_motion_JointPositions_init_zero, false, kuka_core_geometry_Transform_init_zero, false, kuka_core_motion_JointPositions_init_zero, false, kuka_core_motion_Twist_init_zero, false, kuka_core_motion_JointPositions_init_zero, false, kuka_core_motion_JointPositions_init_zero, false, kuka_motion_external_JointImpedanceControlAttributes_init_zero, false, kuka_motion_external_CartesianImpedanceControlAttributes_init_zero, _kuka_motion_external_ExternalControlMode_MIN} - - -/* Maximum encoded size of messages (where known) */ -#if defined(kuka_core_motion_JointPositions_size) && defined(kuka_core_motion_JointVelocities_size) && defined(kuka_core_motion_JointTorques_size) -#define kuka_motion_external_ControlSignalInternal_size (1513 + kuka_core_motion_JointPositions_size + kuka_core_motion_JointVelocities_size + kuka_core_motion_JointTorques_size) -#endif - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/external_control_mode.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/external_control_mode.pb.h deleted file mode 100644 index 8b6b92fc..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/external_control_mode.pb.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_EXTERNAL_CONTROL_MODE_PB_H_INCLUDED -#define PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_EXTERNAL_CONTROL_MODE_PB_H_INCLUDED -#include - - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -typedef enum _kuka_motion_external_ExternalControlMode -{ - kuka_motion_external_ExternalControlMode_EXTERNAL_CONTROL_MODE_UNSPECIFIED = 0, - kuka_motion_external_ExternalControlMode_JOINT_POSITION_CONTROL = 1, - kuka_motion_external_ExternalControlMode_JOINT_IMPEDANCE_CONTROL = 2, - kuka_motion_external_ExternalControlMode_JOINT_VELOCITY_CONTROL = 3, - kuka_motion_external_ExternalControlMode_JOINT_TORQUE_CONTROL = 4, - kuka_motion_external_ExternalControlMode_CARTESIAN_POSITION_CONTROL = 5, - kuka_motion_external_ExternalControlMode_CARTESIAN_IMPEDANCE_CONTROL = 6, - kuka_motion_external_ExternalControlMode_CARTESIAN_VELOCITY_CONTROL = 7, - kuka_motion_external_ExternalControlMode_WRENCH_CONTROL = 8 -} kuka_motion_external_ExternalControlMode; - -/* Helper constants for enums */ -#define _kuka_motion_external_ExternalControlMode_MIN \ - kuka_motion_external_ExternalControlMode_EXTERNAL_CONTROL_MODE_UNSPECIFIED -#define _kuka_motion_external_ExternalControlMode_MAX \ - kuka_motion_external_ExternalControlMode_WRENCH_CONTROL -#define _kuka_motion_external_ExternalControlMode_ARRAYSIZE (( \ - kuka_motion_external_ExternalControlMode)( \ - kuka_motion_external_ExternalControlMode_WRENCH_CONTROL + 1)) - - -#ifdef __cplusplus -extern "C" { -#endif - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h b/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h deleted file mode 100644 index 843437e0..00000000 --- a/kuka_iiqka_eac_driver/include/mock/nanopb/kuka/motion/external/motion_state_internal.pb.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_MOTION_STATE_INTERNAL_PB_H_INCLUDED -#define PB_KUKA_MOTION_EXTERNAL_KUKA_MOTION_EXTERNAL_MOTION_STATE_INTERNAL_PB_H_INCLUDED -#include -#include -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -typedef struct _kuka_motion_external_MotionStateInternal { - bool ipo_stopped; - - kuka_motion_external_ExternalControlMode control_mode; - - bool has_measured_positions; - kuka_core_motion_JointPositions measured_positions; - - bool has_measured_velocities; - kuka_core_motion_JointPositions measured_velocities; - - bool has_measured_torques; - kuka_core_motion_JointPositions measured_torques; -} kuka_motion_external_MotionStateInternal; - - -#ifdef __cplusplus -extern "C" { -#endif - -/* Initializer values for message structs */ -#define kuka_motion_external_MotionStateInternal_init_default {0, _kuka_motion_external_ExternalControlMode_MIN, false, kuka_core_motion_JointPositions_init_default, false, kuka_core_motion_JointPositions_init_default, false, kuka_core_motion_JointPositions_init_default} -#define kuka_motion_external_MotionStateInternal_init_zero {0, _kuka_motion_external_ExternalControlMode_MIN, false, kuka_core_motion_JointPositions_init_zero, false, kuka_core_motion_JointPositions_init_zero, false, kuka_core_motion_JointPositions_init_zero} - -/* Maximum encoded size of messages (where known) */ -#if defined(kuka_core_motion_JointPositions_size) && defined(kuka_core_motion_JointVelocities_size) && defined(kuka_core_motion_JointTorques_size) -#define kuka_motion_external_MotionStateInternal_size (22 + kuka_core_motion_JointPositions_size + kuka_core_motion_JointVelocities_size + kuka_core_motion_JointTorques_size) -#endif - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif diff --git a/kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/replier.h b/kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/replier.h deleted file mode 100644 index 5ee83941..00000000 --- a/kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/replier.h +++ /dev/null @@ -1,51 +0,0 @@ -// This material is the exclusive property of KUKA Deutschland GmbH. -// Except as expressly permitted by separate agreement, this material may only -// be used by members of the development department of KUKA Deutschland GmbH for -// internal development purposes of KUKA Deutschland GmbH. -// -// Copyright (C) -// KUKA Deutschland GmbH, Germany. All Rights Reserved. - -#ifndef REPLIER_H -#define REPLIER_H - -#include - -#include "os-core-udp-communication/socket.h" - -namespace os::core::udp::communication -{ - -class Replier -{ -public: - // - Replier(const SocketAddress & local_address); - virtual ~Replier() = default; - Socket::ErrorCode Setup(); - void Reset(); - -public: - // - Socket::ErrorCode ReceiveRequest(); - Socket::ErrorCode ReceiveRequestOrTimeout(std::chrono::microseconds recv_timeout); - Socket::ErrorCode SendReply(uint8_t * reply_msg_data, size_t reply_msg_size); - -public: - // - std::pair GetRequestMessage() const; - -protected: - static constexpr uint16_t kMaxBufferSize = 1500; - uint8_t server_buffer_[kMaxBufferSize]; - - Socket socket_; - SocketAddress local_address_; - - bool active_request_ = false; - SocketAddress last_remote_address_; - int last_request_size_ = 0; -}; -} // namespace os::core::udp::communication - -#endif // REPLIER_H diff --git a/kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/socket.h b/kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/socket.h deleted file mode 100644 index b683b8fc..00000000 --- a/kuka_iiqka_eac_driver/include/mock/os-core-udp-communication/socket.h +++ /dev/null @@ -1,122 +0,0 @@ -// This material is the exclusive property of KUKA Deutschland GmbH. -// Except as expressly permitted by separate agreement, this material may only -// be used by members of the development department of KUKA Deutschland GmbH for -// internal development purposes of KUKA Deutschland GmbH. -// -// Copyright (C) -// KUKA Deutschland GmbH, Germany. All Rights Reserved. - -#ifndef SOCKET_H -#define SOCKET_H - -#include - -#include -#include -#include - -namespace os::core::udp::communication -{ - -class SocketAddress -{ -public: - SocketAddress(); - SocketAddress(const std::string & ip, int port); - SocketAddress(const std::string & ip); - SocketAddress(int port); - SocketAddress(const struct sockaddr_in * raw_address); - -public: - struct sockaddr * RawAddr(); - const struct sockaddr * RawAddr() const; - struct sockaddr_in * RawInetAddr(); - const struct sockaddr_in * RawInetAddr() const; - size_t Size() const; - const std::string Ip() const; - uint16_t Port() const; - -public: - static const std::string kAnyAddress; -}; - -class Socket -{ -public: - enum ErrorCode - { - kSuccess = 0, - kSocketError = -1, - kNotActive = -2, - kAlreadyActive = -2, - kNotBound = -3, - kNotConnected = -4, - kTimeout = -5, - kError = -6 - }; - -public: - virtual ~Socket(); - -public: - int Map(int flags = 0); - - int SetSocketOption(int level, int optname, const void * optval, socklen_t optlen); - - int SetSendTimeout(const std::chrono::microseconds & timeout); - - int SetReceiveTimeout(const std::chrono::microseconds & timeout); - - int JoinMulticastGroup(const SocketAddress & multicast_address); - - int LeaveMulticastGroup(const SocketAddress & multicast_address); - - int SetTTLForMulticast(int ttl = 1); - - int SetTTLForUnicast(int ttl = 1); - - int SetReuseAddress(int flag = 1); - - int SetReceiveBufferSize(int size); - - int Bind(const SocketAddress & local_address); - - int Connect(const SocketAddress & remote_address); - - int Select(std::chrono::microseconds timeout, bool read = true); - - int Send(const unsigned char * raw_data, int raw_data_size, int flags = 0); - - int SendTo( - const SocketAddress & remote_address, const unsigned char * raw_data, int raw_data_size, - int flags = 0); - - int Receive(unsigned char * buffer, int buffer_size, int flags = 0); - - int ReceiveOrTimeout( - const std::chrono::microseconds & timeout, unsigned char * buffer, - int buffer_size, int flags = 0); - - int ReceiveFrom( - SocketAddress & incoming_remote_address, unsigned char * buffer, int buffer_size, - int flags = 0); - - int ReceiveFromOrTimeout( - const std::chrono::microseconds & timeout, - SocketAddress & incoming_remote_address, unsigned char * buffer, - int buffer_size, int flags = 0); - - int Close(); - -public: - int GetSocketFd() const {return 0;} - bool IsActive() const {return false;} - - int GetErrorCode() const; - std::string GetErrorText() const; - bool IsReadable() const; -}; - -} // namespace os::core::udp::communication - -#endif // SOCKET_H diff --git a/kuka_iiqka_eac_driver/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py index f11ebd93..c53af839 100644 --- a/kuka_iiqka_eac_driver/launch/startup.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup.launch.py @@ -148,6 +148,7 @@ def controller_spawner(controller_names, activate=False): "joint_group_impedance_controller", "effort_controller", "control_mode_handler", + "event_broadcaster", ] controller_spawners = [controller_spawner(name) for name in controller_names] diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 799fcf39..412cd762 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -13,17 +13,17 @@ // limitations under the License. #include +#include #include #include #include -#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "kuka_drivers_core/hardware_interface_types.hpp" -#include "kuka_iiqka_eac_driver/hardware_interface.hpp" #include "kuka/external-control-sdk/iiqka/configuration.h" #include "kuka_iiqka_eac_driver/event_observer.hpp" +#include "kuka_iiqka_eac_driver/hardware_interface.hpp" using namespace kuka::ecs::v1; // NOLINT using namespace std::chrono_literals; @@ -40,7 +40,7 @@ CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::Hardw } // Initialize control mode with 'undefined', which should be changed by the appropriate controller - // during configuration + // during configuration // TODO: rethink control mode changes hw_position_states_.resize(info_.joints.size(), 0.0); hw_torque_states_.resize(info_.joints.size(), 0.0); @@ -49,7 +49,6 @@ CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::Hardw hw_stiffness_commands_.resize(info_.joints.size(), 30); hw_damping_commands_.resize(info_.joints.size(), 0.7); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { if (joint.command_interfaces.size() != 4) @@ -115,6 +114,12 @@ CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::Hardw } } + RCLCPP_INFO( + rclcpp::get_logger("KukaEACHardwareInterface"), + "Init successful with controller ip: %s and client ip: %s", + info_.hardware_parameters.at("controller_ip").c_str(), + info_.hardware_parameters.at("client_ip").c_str()); + return CallbackReturn::SUCCESS; } @@ -130,6 +135,10 @@ std::vector KukaEACHardwareInterface::export state_interfaces.emplace_back( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_torque_states_[i]); } + + state_interfaces.emplace_back( + hardware_interface::STATE_PREFIX, hardware_interface::SERVER_STATE, &server_state_); + return state_interfaces; } @@ -154,53 +163,61 @@ KukaEACHardwareInterface::export_command_interfaces() info_.joints[i].name, hardware_interface::HW_IF_DAMPING, &hw_damping_commands_[i]); } + command_interfaces.emplace_back( + hardware_interface::CONFIG_PREFIX, hardware_interface::CONTROL_MODE, &hw_control_mode_command_); + return command_interfaces; } CallbackReturn KukaEACHardwareInterface::on_configure(const rclcpp_lifecycle::State &) { - if (!SetupRobot()) + if (!SetupRobot()) { - return CallbackReturn::FAILURE; + return CallbackReturn::FAILURE; } - + // Initialize previous control mode with first + prev_control_mode_ = static_cast(hw_control_mode_command_); + if (!SetupQoS()) { return CallbackReturn::FAILURE; } - RCLCPP_INFO( - rclcpp::get_logger("KukaEACHardwareInterface"), - "Configure successful with controller ip: %s and client ip: %s", - info_.hardware_parameters.at("controller_ip").c_str(), - info_.hardware_parameters.at("client_ip").c_str()); - RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Set QoS profile with %s consequent and %s packet losses allowed in %s milliseconds", info_.hardware_parameters.at("consequent_lost_packets").c_str(), info_.hardware_parameters.at("lost_packets_in_timeframe").c_str(), info_.hardware_parameters.at("timeframe_ms").c_str()); - + return CallbackReturn::SUCCESS; } CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - kuka::external::control::OperationStatus create_event_observer = robot_ptr_->RegisterEventHandler(std::make_unique(this)); - if (create_event_observer.return_code == kuka::external::control::ReturnCode::ERROR) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Creating event observer failed, error message: %s", create_event_observer.message); + kuka::external::control::OperationStatus create_event_observer = + robot_ptr_->RegisterEventHandler(std::make_unique(this)); + if (create_event_observer.return_code == kuka::external::control::ReturnCode::ERROR) + { + RCLCPP_ERROR( + rclcpp::get_logger("KukaEACHardwareInterface"), + "Creating event observer failed, error message: %s", create_event_observer.message); } kuka::external::control::OperationStatus start_control = robot_ptr_->StartControlling(); if (start_control.return_code == kuka::external::control::ReturnCode::ERROR) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Starting external control failed, error message: %s", start_control.message); + RCLCPP_ERROR( + rclcpp::get_logger("KukaEACHardwareInterface"), + "Starting external control failed, error message: %s", start_control.message); return CallbackReturn::FAILURE; } - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control session started successfully"); + RCLCPP_INFO( + rclcpp::get_logger("KukaEACHardwareInterface"), + "External control session started successfully"); + stop_requested_ = false; return CallbackReturn::SUCCESS; } @@ -208,7 +225,7 @@ CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::S { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Deactivating"); - //set stop flag or something + stop_requested_ = true; return CallbackReturn::SUCCESS; } @@ -219,50 +236,70 @@ return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::D if ((msg_received_ = receive_state.return_code == kuka::external::control::ReturnCode::OK)) { - auto& req_message = robot_ptr_->GetLastMotionState(); - - std::copy(req_message.GetMeasuredPositions()->begin(), req_message.GetMeasuredPositions()->end(), hw_position_states_.begin()); - std::copy(req_message.GetMeasuredTorques()->begin(), req_message.GetMeasuredTorques()->end(), hw_torque_states_.begin()); - std::copy(hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); + auto & req_message = robot_ptr_->GetLastMotionState(); + + std::copy( + req_message.GetMeasuredPositions()->begin(), req_message.GetMeasuredPositions()->end(), + hw_position_states_.begin()); + std::copy( + req_message.GetMeasuredTorques()->begin(), req_message.GetMeasuredTorques()->end(), + hw_torque_states_.begin()); + std::copy( + hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); } - + return return_type::OK; } return_type KukaEACHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) { // If control is not started or a request is missed, do not send back anything - if (!msg_received_) { return return_type::OK; } - - // TODO(Svastits): should we separate control modes somehow? hw_control_signal_->AddJointPositionValues(hw_position_commands_); hw_control_signal_->AddTorqueValues(hw_torque_commands_); hw_control_signal_->AddStiffnessAndDampingValues(hw_stiffness_commands_, hw_damping_commands_); - kuka::external::control::OperationStatus send_reply = robot_ptr_->SendControlSignal(); - + kuka::external::control::OperationStatus send_reply; + if (stop_requested_) + { + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Sending stop signal"); + send_reply = robot_ptr_->StopControlling(); + } + else if (static_cast(hw_control_mode_command_) != prev_control_mode_) + { + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Requesting control mode switch"); + send_reply = robot_ptr_->SwitchControlMode( + static_cast(hw_control_mode_command_)); + } + else + { + send_reply = robot_ptr_->SendControlSignal(); + } if (send_reply.return_code != kuka::external::control::ReturnCode::OK) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Send reply failed, error message: %s", send_reply.message); + RCLCPP_ERROR( + rclcpp::get_logger("KukaEACHardwareInterface"), "Send reply failed, error message: %s", + send_reply.message); throw std::runtime_error("Error sending reply"); } return return_type::OK; } -bool KukaEACHardwareInterface::SetupRobot() { +bool KukaEACHardwareInterface::SetupRobot() +{ kuka::external::control::iiqka::Configuration config; - + config.client_ip_address = info_.hardware_parameters.at("client_ip"); - config.koni_ip_address = info_.hardware_parameters.at("controller_ip"); - + config.koni_ip_address = info_.hardware_parameters.at("controller_ip"); + config.is_secure = false; config.dof = info_.joints.size(); - config.initial_control_mode = kuka::external::control::ControlMode::JOINT_POSITION_CONTROL; + config.initial_control_mode = + static_cast(hw_control_mode_command_); robot_ptr_ = std::make_unique(config); @@ -276,33 +313,36 @@ bool KukaEACHardwareInterface::SetupRobot() { if (setup.return_code != kuka::external::control::ReturnCode::OK) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Setup failed, error message: %s", setup.message); + RCLCPP_ERROR( + rclcpp::get_logger("KukaEACHardwareInterface"), "Setup failed, error message: %s", + setup.message); return false; } return true; - } -bool KukaEACHardwareInterface::SetupQoS() { - +bool KukaEACHardwareInterface::SetupQoS() +{ kuka::external::control::iiqka::QoS_Configuration qos_config; - qos_config.packet_loss_in_timeframe_limit = std::stoi(info_.hardware_parameters.at("lost_packets_in_timeframe")); - qos_config.consecutive_packet_loss_limit = std::stoi(info_.hardware_parameters.at("consequent_lost_packets")); - qos_config.timeframe_ms = std::stoi(info_.hardware_parameters.at("timeframe_ms")); + qos_config.packet_loss_in_timeframe_limit = + std::stoi(info_.hardware_parameters.at("lost_packets_in_timeframe")); + qos_config.consecutive_packet_loss_limit = + std::stoi(info_.hardware_parameters.at("consequent_lost_packets")); + qos_config.timeframe_ms = std::stoi(info_.hardware_parameters.at("timeframe_ms")); kuka::external::control::OperationStatus set_qos_status = robot_ptr_->SetQoSProfile(qos_config); - + if (set_qos_status.return_code != kuka::external::control::ReturnCode::OK) { - RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "QoS configuration failed, error message: %s", set_qos_status.message); - return false; + RCLCPP_ERROR( + rclcpp::get_logger("KukaEACHardwareInterface"), "QoS configuration failed, error message: %s", + set_qos_status.message); + return false; } return true; - } - } // namespace kuka_eac PLUGINLIB_EXPORT_CLASS(kuka_eac::KukaEACHardwareInterface, hardware_interface::SystemInterface) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 575d68f4..5cf27c9e 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -17,8 +17,8 @@ #include "communication_helpers/ros2_control_tools.hpp" #include "communication_helpers/service_tools.hpp" -#include "kuka_iiqka_eac_driver/robot_manager_node.hpp" #include "iiqka/proto-api/motion-external/external_control_mode.pb.h" +#include "kuka_iiqka_eac_driver/robot_manager_node.hpp" using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT @@ -82,9 +82,12 @@ RobotManagerNode::RobotManagerNode() return this->controller_handler_.UpdateControllerName( kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE, controller_name); }); + + // TODO(Svastits): enable control mode change in inactive, after client lib was fixed + // Currently initial control mode must be specified at setup (configuring transition) this->registerParameter( "control_mode", static_cast(ExternalControlMode::JOINT_POSITION_CONTROL), - kuka_drivers_core::ParameterSetAccessRights{true, true, true, false, false}, + kuka_drivers_core::ParameterSetAccessRights{true, false, true, false, false}, [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); this->registerStaticParameter( "controller_ip", "", @@ -142,7 +145,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Activate control mode handler if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {"control_mode_handler"}, {})) + change_controller_state_client_, {"control_mode_handler", "event_broadcaster"}, {})) { RCLCPP_ERROR(get_logger(), "Could not activate control mode handler"); // TODO(Svastits): this can be removed if rollback is implemented properly @@ -160,7 +163,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { // Deactivate control mode handler if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, {"control_mode_handler"})) + change_controller_state_client_, {}, {"control_mode_handler", "event_broadcaster"})) { RCLCPP_ERROR(get_logger(), "Could not deactivate control mode handler"); } From 2057b2c9f6e130a6b301fef7f6cf532d35d46d34 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 11:41:55 +0100 Subject: [PATCH 03/88] add event subscriber to manager --- .../hardware_interface.hpp | 1 - .../robot_manager_node.hpp | 15 +-- .../src/robot_manager_node.cpp | 123 +++++------------- 3 files changed, 41 insertions(+), 98 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index a1db61be..e7203179 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -39,7 +39,6 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface namespace kuka_eac { - class KukaEACHardwareInterface : public hardware_interface::SystemInterface { public: diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index 8e12d607..e05f1977 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -26,6 +26,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/u_int32.hpp" +#include "std_msgs/msg/int64.hpp" #include "kuka_drivers_core/controller_handler.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" @@ -36,7 +37,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode { public: RobotManagerNode(); - ~RobotManagerNode(); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State &) override; @@ -51,7 +51,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode const rclcpp_lifecycle::State &) override; private: - void ObserveControl(); + void EventSubscriptionCallback(const std_msgs::msg::Int64::SharedPtr msg); bool onControlModeChangeRequest(int control_mode); bool onRobotModelChangeRequest(const std::string & robot_model); @@ -64,23 +64,20 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode kuka_drivers_core::ControllerHandler controller_handler_; - std::thread observe_thread_; std::atomic terminate_{false}; bool param_declared_ = false; -#ifdef NON_MOCK_SETUP - std::unique_ptr stub_; - std::unique_ptr context_; std::condition_variable control_mode_cv_; std::mutex control_mode_cv_m_; bool control_mode_change_finished_; -#endif - rclcpp::Publisher::SharedPtr control_mode_pub_; - std::shared_ptr> is_configured_pub_; + rclcpp::Publisher::SharedPtr is_configured_pub_; std_msgs::msg::Bool is_configured_msg_; + rclcpp::Subscription::SharedPtr event_subscriber_; + + // There are two kinds of control modes with different number of necessary interfaces to be set: // - in standard modes (position, torque), only the control signal to the used interface (1) // - in impedance modes, the setpoint and the parameters describing the behaviour (2) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 5cf27c9e..1c2217f3 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -32,11 +32,9 @@ RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_manager"), controller_handler_({ "joint_state_broadcaster", - }) -#ifdef NON_MOCK_SETUP - , + }), control_mode_change_finished_(false) -#endif + { RCLCPP_DEBUG(get_logger(), "Starting Robot Manager Node init"); @@ -57,6 +55,10 @@ RobotManagerNode::RobotManagerNode() control_mode_pub_ = this->create_publisher( "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); + event_subscriber_ = this->create_subscription( + "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Int64::SharedPtr msg) + {this->EventSubscriptionCallback(msg);}); + // Register parameters this->registerParameter( "position_controller_name", "joint_trajectory_controller", @@ -99,28 +101,6 @@ RobotManagerNode::RobotManagerNode() [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); -#ifdef NON_MOCK_SETUP - RCLCPP_INFO( - get_logger(), "IP address of controller: %s", - this->get_parameter("controller_ip").as_string().c_str()); - - stub_ = kuka::ecs::v1::ExternalControlService::NewStub(grpc::CreateChannel( - this->get_parameter("controller_ip").as_string() + ":49335", - grpc::InsecureChannelCredentials())); -#endif -} -RobotManagerNode::~RobotManagerNode() -{ -#ifdef NON_MOCK_SETUP - if (context_ != nullptr) - { - context_->TryCancel(); - } -#endif - if (observe_thread_.joinable()) - { - observe_thread_.join(); - } } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -139,7 +119,6 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) return FAILURE; } - is_configured_pub_->on_activate(); is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); @@ -176,78 +155,49 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) 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(?) + is_configured_msg_.data = false; + is_configured_pub_->publish(is_configured_msg_); return SUCCESS; } -void RobotManagerNode::ObserveControl() -{ -#ifdef NON_MOCK_SETUP - context_ = std::make_unique<::grpc::ClientContext>(); - kuka::ecs::v1::ObserveControlStateRequest observe_request; - std::unique_ptr> reader( - stub_->ObserveControlState(context_.get(), observe_request)); - - kuka::ecs::v1::CommandState response; - - while (reader->Read(&response)) +void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::Int64::SharedPtr msg) +{ + switch (msg->data) { - switch (static_cast(response.event())) + case 5: { - case kuka::ecs::v1::CommandEvent::CONTROL_MODE_SWITCH: + std::lock_guard lk(control_mode_cv_m_); + control_mode_change_finished_ = true; + } + RCLCPP_INFO(get_logger(), "Control mode change has finished, restarting with new mode"); + control_mode_cv_.notify_all(); + break; + case 4: + case 6: + RCLCPP_INFO(get_logger(), "External control stopped"); + terminate_ = true; + if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) { - std::lock_guard lk(control_mode_cv_m_); - control_mode_change_finished_ = true; + this->deactivate(); } - RCLCPP_INFO(get_logger(), "Command mode switched in the robot controller"); - control_mode_cv_.notify_all(); - break; - case kuka::ecs::v1::CommandEvent::STOPPED: - case kuka::ecs::v1::CommandEvent::ERROR: - RCLCPP_INFO(get_logger(), "External control stopped"); - terminate_ = true; - if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) - { - this->deactivate(); - } - else if (this->get_current_state().id() == State::TRANSITION_STATE_ACTIVATING) - { - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - } - break; - default: - break; - } + else if (this->get_current_state().id() == State::TRANSITION_STATE_ACTIVATING) + { + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + } + break; + default: + break; } -#endif + } // TODO(Svastits): rollback in case of failures rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) { -#ifdef NON_MOCK_SETUP - if (context_ != nullptr) - { - context_->TryCancel(); - } -#endif // Join observe thread, necessary if previous activation failed - if (observe_thread_.joinable()) - { - observe_thread_.join(); - } terminate_ = false; - // Subscribe to stream of state changes - observe_thread_ = std::thread(&RobotManagerNode::ObserveControl, this); - // Activate hardware interface if (!kuka_drivers_core::changeHardwareState( change_hardware_state_client_, robot_model_, State::PRIMARY_STATE_ACTIVE, 5000)) @@ -405,15 +355,13 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) { // The driver is in active state -#ifdef NON_MOCK_SETUP - // Wait for ObserveControl to approve that the robot succefully changed control mode + // Wait for approval that the robot succefully changed control mode std::unique_lock control_mode_lk(this->control_mode_cv_m_); if (!this->control_mode_cv_.wait_for( - control_mode_lk, std::chrono::milliseconds(2000), + control_mode_lk, std::chrono::milliseconds(3000), [this]() { return this->control_mode_change_finished_; })) { - // Control Mode change timeout reached RCLCPP_ERROR(get_logger(), "Timeout reached while waiting for robot to change control mode."); this->on_deactivate(get_current_state()); return false; @@ -421,7 +369,6 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) control_mode_change_finished_ = false; control_mode_lk.unlock(); RCLCPP_INFO(get_logger(), "Robot Controller finished control mode change"); -#endif // Deactivate unnecessary controllers if ( From 07cb9d14f09886d5db7e3a6692d289882111979c Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 11:47:23 +0100 Subject: [PATCH 04/88] change type --- .../include/kuka_iiqka_eac_driver/robot_manager_node.hpp | 6 +++--- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index e05f1977..2ba6ec5f 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -26,7 +26,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/u_int32.hpp" -#include "std_msgs/msg/int64.hpp" +#include "std_msgs/msg/u_int8.hpp" #include "kuka_drivers_core/controller_handler.hpp" #include "kuka_drivers_core/ros2_base_lc_node.hpp" @@ -51,7 +51,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode const rclcpp_lifecycle::State &) override; private: - void EventSubscriptionCallback(const std_msgs::msg::Int64::SharedPtr msg); + void EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg); bool onControlModeChangeRequest(int control_mode); bool onRobotModelChangeRequest(const std::string & robot_model); @@ -75,7 +75,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode rclcpp::Publisher::SharedPtr is_configured_pub_; std_msgs::msg::Bool is_configured_msg_; - rclcpp::Subscription::SharedPtr event_subscriber_; + rclcpp::Subscription::SharedPtr event_subscriber_; // There are two kinds of control modes with different number of necessary interfaces to be set: diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 1c2217f3..34b1fc25 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -55,8 +55,8 @@ RobotManagerNode::RobotManagerNode() control_mode_pub_ = this->create_publisher( "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); - event_subscriber_ = this->create_subscription( - "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::Int64::SharedPtr msg) + event_subscriber_ = this->create_subscription( + "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::UInt8::SharedPtr msg) {this->EventSubscriptionCallback(msg);}); // Register parameters @@ -160,7 +160,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) return SUCCESS; } -void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::Int64::SharedPtr msg) +void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) { switch (msg->data) { From 4c62501080ecd5c30b42b468af399a51677e745b Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 11:48:01 +0100 Subject: [PATCH 05/88] format --- .../include/kuka_iiqka_eac_driver/robot_manager_node.hpp | 1 - kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 8 +++----- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index 2ba6ec5f..c52a9f93 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -77,7 +77,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode rclcpp::Subscription::SharedPtr event_subscriber_; - // There are two kinds of control modes with different number of necessary interfaces to be set: // - in standard modes (position, torque), only the control signal to the used interface (1) // - in impedance modes, the setpoint and the parameters describing the behaviour (2) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 34b1fc25..5af8a079 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -56,8 +56,8 @@ RobotManagerNode::RobotManagerNode() "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); event_subscriber_ = this->create_subscription( - "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), [this](const std_msgs::msg::UInt8::SharedPtr msg) - {this->EventSubscriptionCallback(msg);}); + "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), + [this](const std_msgs::msg::UInt8::SharedPtr msg) { this->EventSubscriptionCallback(msg); }); // Register parameters this->registerParameter( @@ -100,7 +100,6 @@ RobotManagerNode::RobotManagerNode() kuka_drivers_core::ParameterSetAccessRights{true, false, false, false, false}, [this](const std::string & robot_model) { return this->onRobotModelChangeRequest(robot_model); }); - } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -161,7 +160,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) } void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) -{ +{ switch (msg->data) { case 5: @@ -189,7 +188,6 @@ void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::Sha default: break; } - } // TODO(Svastits): rollback in case of failures From cd9acb7240e09602a47c27c6932c040e239dc439 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 12:34:09 +0100 Subject: [PATCH 06/88] add enums --- .../kuka_drivers_core/control_mode.hpp | 2 +- .../kuka_drivers_core/hardware_event.hpp | 35 +++++++++++++++++++ kuka_drivers_core/src/controller_handler.cpp | 4 +-- .../src/robot_manager_node.cpp | 10 +++--- 4 files changed, 44 insertions(+), 7 deletions(-) create mode 100644 kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp diff --git a/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp b/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp index 01111653..eb6333e7 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/control_mode.hpp @@ -22,7 +22,7 @@ namespace kuka_drivers_core */ enum class ControlMode : std::uint8_t { - UNSPECIFIED_CONTROL_MODE = 0, + CONTROL_MODE_UNSPECIFIED = 0, JOINT_POSITION_CONTROL = 1, JOINT_IMPEDANCE_CONTROL = 2, JOINT_VELOCITY_CONTROL = 3, diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp new file mode 100644 index 00000000..7d1bdd50 --- /dev/null +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp @@ -0,0 +1,35 @@ +// Copyright 2024 Á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. + +#ifndef KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_ +#define KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_ + +namespace kuka_drivers_core +{ +/** + * @brief Enum for controller-side events + */ +enum class HardwareEvent : std::uint8_t +{ + HARDWARE_EVENT_UNSPECIFIED = 0, + COMMAND_ACCEPTED = 2, + CONTROL_STARTED = 3, + CONTROL_STOPPED = 4, + CONTROL_MODE_SWITCH = 5, + ERROR = 6 +}; + +} // namespace kuka_drivers_core + +#endif // KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_ \ No newline at end of file diff --git a/kuka_drivers_core/src/controller_handler.cpp b/kuka_drivers_core/src/controller_handler.cpp index 1cf13999..00a6d27a 100644 --- a/kuka_drivers_core/src/controller_handler.cpp +++ b/kuka_drivers_core/src/controller_handler.cpp @@ -76,9 +76,9 @@ ControllerHandler::GetControllersForSwitch(ControlMode new_control_mode) throw std::out_of_range("Attribute new_control_mode is out of range"); } - if (new_control_mode == ControlMode::UNSPECIFIED_CONTROL_MODE) + if (new_control_mode == ControlMode::CONTROL_MODE_UNSPECIFIED) { - throw std::logic_error("UNSPECIFIED_CONTROL_MODE is not valid control mode"); + throw std::logic_error("CONTROL_MODE_UNSPECIFIED is not valid control mode"); } // Set controllers which should be activated and deactivated diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 5af8a079..ea7f2659 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -20,6 +20,8 @@ #include "iiqka/proto-api/motion-external/external_control_mode.pb.h" #include "kuka_iiqka_eac_driver/robot_manager_node.hpp" +#include "kuka_drivers_core/hardware_event.hpp" + using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT using namespace kuka::motion::external; // NOLINT @@ -161,9 +163,9 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) { - switch (msg->data) + switch (static_cast(msg->data)) { - case 5: + case kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH: { std::lock_guard lk(control_mode_cv_m_); control_mode_change_finished_ = true; @@ -171,8 +173,8 @@ void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::Sha RCLCPP_INFO(get_logger(), "Control mode change has finished, restarting with new mode"); control_mode_cv_.notify_all(); break; - case 4: - case 6: + case kuka_drivers_core::HardwareEvent::CONTROL_STOPPED: + case kuka_drivers_core::HardwareEvent::ERROR: RCLCPP_INFO(get_logger(), "External control stopped"); terminate_ = true; if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) From e33e3661de2c24f5e7ad648f9869b5b2b2f311bb Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 14:48:06 +0100 Subject: [PATCH 07/88] hack for LD_LIBRARY_PATH --- kuka_iiqka_eac_driver/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 220e67e0..d44880cc 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -71,4 +71,7 @@ ament_export_libraries( ${PROJECT_NAME} ) +# Hack to extend $LD_LIBRARY_PATH with /usr/local/lib when sourcing +file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv "prepend-non-duplicate;LD_LIBRARY_PATH;/usr/local/lib") + ament_package() From 068136160b9c9c27bcca1ac82613e3ae341f2b9a Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 14:51:55 +0100 Subject: [PATCH 08/88] fake hw in activation test --- kuka_iiqka_eac_driver/test/test_driver_activation.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/test/test_driver_activation.py b/kuka_iiqka_eac_driver/test/test_driver_activation.py index 91ffbc59..63c44aeb 100644 --- a/kuka_iiqka_eac_driver/test/test_driver_activation.py +++ b/kuka_iiqka_eac_driver/test/test_driver_activation.py @@ -40,7 +40,10 @@ def generate_test_description(): "/launch/", "startup.launch.py", ] - ) + ), + launch_arguments={ + "use_fake_hardware": "true", + }.items(), ), launch.actions.TimerAction( period=2.0, From 95e6891b899c77cb68509e922ed05fb6d2689439 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 15:34:42 +0100 Subject: [PATCH 09/88] set server event --- .../include/kuka_drivers_core/hardware_event.hpp | 2 +- kuka_iiqka_eac_driver/CMakeLists.txt | 3 +-- .../include/kuka_iiqka_eac_driver/event_observer.hpp | 5 +++++ .../include/kuka_iiqka_eac_driver/hardware_interface.hpp | 5 +++++ kuka_iiqka_eac_driver/package.xml | 1 - kuka_iiqka_eac_driver/src/hardware_interface.cpp | 6 ++++++ 6 files changed, 18 insertions(+), 4 deletions(-) diff --git a/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp b/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp index 7d1bdd50..90fac1c2 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/hardware_event.hpp @@ -32,4 +32,4 @@ enum class HardwareEvent : std::uint8_t } // namespace kuka_drivers_core -#endif // KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_ \ No newline at end of file +#endif // KUKA_DRIVERS_CORE__HARDWARE_EVENT_HPP_ diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index d44880cc..93da6adc 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(kuka_drivers_core REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(controller_manager_msgs REQUIRED) -find_package(yaml-cpp REQUIRED) include_directories(include) @@ -43,7 +42,7 @@ add_library(${PROJECT_NAME} SHARED target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY") ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface kuka_drivers_core) -target_link_libraries(${PROJECT_NAME} kuka-external-control-sdk yaml-cpp) +target_link_libraries(${PROJECT_NAME} kuka-external-control-sdk) add_executable(robot_manager_node diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index 945650b2..a6ff232e 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -18,6 +18,7 @@ #include "rclcpp/macros.hpp" #include "kuka/external-control-sdk/common/irobot.h" +#include "kuka_drivers_core/hardware_event.hpp" #include "kuka_iiqka_eac_driver/hardware_interface.hpp" namespace kuka_eac @@ -28,22 +29,26 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler KukaEACEventObserver(KukaEACHardwareInterface * hw_interface) : hw_interface_(hw_interface) {} void OnSampling() override { + hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STARTED); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active"); } void OnControlModeSwitch(const std::string & reason) override { + hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH); RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); } void OnStopped(const std::string & reason) override { + hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STOPPED); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); hw_interface_->on_deactivate(hw_interface_->get_state()); } void OnError(const std::string & reason) override { + hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::ERROR); RCLCPP_ERROR( rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index e7203179..d913755e 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -32,6 +32,8 @@ #include "kuka/external-control-sdk/iiqka/robot.h" #include "rclcpp_lifecycle/state.hpp" +#include "kuka_drivers_core/hardware_event.hpp" + #include "kuka_iiqka_eac_driver/visibility_control.h" using hardware_interface::return_type; @@ -68,6 +70,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_server_event(kuka_drivers_core::HardwareEvent event); + private: KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS(); @@ -87,6 +91,7 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface double server_state_ = 0; int prev_control_mode_; + kuka_drivers_core::HardwareEvent last_event_ = kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED; kuka::ecs::v1::CommandState command_state_; bool msg_received_; diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index a57c9f6b..9af0cd03 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -20,7 +20,6 @@ hardware_interface pluginlib - yaml-cpp protobuf-dev libnanopb-dev libgrpc diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 412cd762..44c7e4d2 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -248,6 +248,10 @@ return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::D hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); } + // Modify state interface only in read + // TODO(Svasits): guard last_event_ with mutex + server_state_ = static_cast(last_event_); + return return_type::OK; } @@ -343,6 +347,8 @@ bool KukaEACHardwareInterface::SetupQoS() return true; } + +void KukaEACHardwareInterface::set_server_event(kuka_drivers_core::HardwareEvent event) { last_event_ = event; } } // namespace kuka_eac PLUGINLIB_EXPORT_CLASS(kuka_eac::KukaEACHardwareInterface, hardware_interface::SystemInterface) From d421a63fe056dc87680ca802f97ed5f13776ae79 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 16:01:43 +0100 Subject: [PATCH 10/88] recv timeout --- .../kuka_iiqka_eac_driver/event_observer.hpp | 2 ++ .../kuka_iiqka_eac_driver/hardware_interface.hpp | 9 ++++++--- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 13 ++----------- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 3 +-- 4 files changed, 11 insertions(+), 16 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index a6ff232e..2dce2e44 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -35,6 +35,8 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler void OnControlModeSwitch(const std::string & reason) override { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH); + // Increase receive timeout after control mode switch + hw_interface_->set_timeout(3000); RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index d913755e..a5de93cb 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -72,6 +72,11 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_server_event(kuka_drivers_core::HardwareEvent event); + KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_timeout(int timeout_ms) + { + receive_timeout_ = std::chrono::milliseconds(timeout_ms); + } + private: KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS(); @@ -93,12 +98,10 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface int prev_control_mode_; kuka_drivers_core::HardwareEvent last_event_ = kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED; - kuka::ecs::v1::CommandState command_state_; bool msg_received_; - bool stop_requested_ = false; - std::chrono::milliseconds receive_timeout_{1000}; + std::chrono::milliseconds receive_timeout_{6}; }; } // namespace kuka_eac diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 44c7e4d2..90624775 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -25,11 +25,6 @@ #include "kuka_iiqka_eac_driver/event_observer.hpp" #include "kuka_iiqka_eac_driver/hardware_interface.hpp" -using namespace kuka::ecs::v1; // NOLINT -using namespace std::chrono_literals; - -using os::core::udp::communication::Socket; - namespace kuka_eac { CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) @@ -41,7 +36,6 @@ CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::Hardw // Initialize control mode with 'undefined', which should be changed by the appropriate controller // during configuration - // TODO: rethink control mode changes hw_position_states_.resize(info_.joints.size(), 0.0); hw_torque_states_.resize(info_.joints.size(), 0.0); hw_position_commands_.resize(info_.joints.size(), 0.0); @@ -232,10 +226,11 @@ CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::S return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { - kuka::external::control::OperationStatus receive_state = robot_ptr_->ReceiveMotionState(20ms); + kuka::external::control::OperationStatus receive_state = robot_ptr_->ReceiveMotionState(receive_timeout_); if ((msg_received_ = receive_state.return_code == kuka::external::control::ReturnCode::OK)) { + receive_timeout_ = std::chrono::milliseconds(6); auto & req_message = robot_ptr_->GetLastMotionState(); std::copy( @@ -309,10 +304,6 @@ bool KukaEACHardwareInterface::SetupRobot() hw_control_signal_ = &(robot_ptr_->GetControlSignal()); - hw_control_signal_->AddJointPositionValues(hw_position_commands_); - hw_control_signal_->AddTorqueValues(hw_torque_commands_); - hw_control_signal_->AddStiffnessAndDampingValues(hw_stiffness_commands_, hw_damping_commands_); - kuka::external::control::OperationStatus setup = robot_ptr_->Setup(); if (setup.return_code != kuka::external::control::ReturnCode::OK) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index ea7f2659..07b712af 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -24,7 +24,6 @@ using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT -using namespace kuka::motion::external; // NOLINT namespace kuka_eac { @@ -87,7 +86,7 @@ RobotManagerNode::RobotManagerNode() kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE, controller_name); }); - // TODO(Svastits): enable control mode change in inactive, after client lib was fixed + // TODO(Svastits): enable control mode change in inactive state, after client lib was fixed // Currently initial control mode must be specified at setup (configuring transition) this->registerParameter( "control_mode", static_cast(ExternalControlMode::JOINT_POSITION_CONTROL), From 82ca89b5279a39002431eac9c67c937c10c0b167 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 14 Feb 2024 16:14:00 +0100 Subject: [PATCH 11/88] clean up --- kuka_iiqka_eac_driver/CMakeLists.txt | 6 +++--- .../include/kuka_iiqka_eac_driver/hardware_interface.hpp | 3 ++- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 8 ++++++-- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 8 +++----- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 93da6adc..a57ac37c 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -48,8 +48,7 @@ target_link_libraries(${PROJECT_NAME} kuka-external-control-sdk) add_executable(robot_manager_node src/robot_manager_node.cpp) ament_target_dependencies(robot_manager_node rclcpp kuka_drivers_core sensor_msgs controller_manager_msgs) -target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers kuka-external-control-sdk - kuka-external-control-sdk-protobuf) +target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers kuka-external-control-sdk) pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) @@ -71,6 +70,7 @@ ament_export_libraries( ) # Hack to extend $LD_LIBRARY_PATH with /usr/local/lib when sourcing -file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv "prepend-non-duplicate;LD_LIBRARY_PATH;/usr/local/lib") +file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv + "prepend-non-duplicate;LD_LIBRARY_PATH;/usr/local/lib") ament_package() diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index a5de93cb..c6498749 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -96,7 +96,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface double server_state_ = 0; int prev_control_mode_; - kuka_drivers_core::HardwareEvent last_event_ = kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED; + kuka_drivers_core::HardwareEvent last_event_ = + kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED; bool msg_received_; bool stop_requested_ = false; diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 90624775..a0e6a5af 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -226,7 +226,8 @@ CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::S return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { - kuka::external::control::OperationStatus receive_state = robot_ptr_->ReceiveMotionState(receive_timeout_); + kuka::external::control::OperationStatus receive_state = + robot_ptr_->ReceiveMotionState(receive_timeout_); if ((msg_received_ = receive_state.return_code == kuka::external::control::ReturnCode::OK)) { @@ -339,7 +340,10 @@ bool KukaEACHardwareInterface::SetupQoS() return true; } -void KukaEACHardwareInterface::set_server_event(kuka_drivers_core::HardwareEvent event) { last_event_ = event; } +void KukaEACHardwareInterface::set_server_event(kuka_drivers_core::HardwareEvent event) +{ + last_event_ = event; +} } // namespace kuka_eac PLUGINLIB_EXPORT_CLASS(kuka_eac::KukaEACHardwareInterface, hardware_interface::SystemInterface) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 07b712af..c2c99dcb 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -17,9 +17,9 @@ #include "communication_helpers/ros2_control_tools.hpp" #include "communication_helpers/service_tools.hpp" -#include "iiqka/proto-api/motion-external/external_control_mode.pb.h" #include "kuka_iiqka_eac_driver/robot_manager_node.hpp" +#include "kuka_drivers_core/control_mode.hpp" #include "kuka_drivers_core/hardware_event.hpp" using namespace controller_manager_msgs::srv; // NOLINT @@ -89,7 +89,7 @@ RobotManagerNode::RobotManagerNode() // TODO(Svastits): enable control mode change in inactive state, after client lib was fixed // Currently initial control mode must be specified at setup (configuring transition) this->registerParameter( - "control_mode", static_cast(ExternalControlMode::JOINT_POSITION_CONTROL), + "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), kuka_drivers_core::ParameterSetAccessRights{true, false, true, false, false}, [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); this->registerStaticParameter( @@ -389,9 +389,7 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) } } - RCLCPP_INFO( - get_logger(), "Successfully changed control mode to %s", - ExternalControlMode_Name(control_mode).c_str()); + RCLCPP_INFO(get_logger(), "Successfully changed control mode"); param_declared_ = true; return true; } From 0a9a31a99108b3ab327e5534bc39a80859f5f75f Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Thu, 15 Feb 2024 11:38:45 +0100 Subject: [PATCH 12/88] fix control mode changes --- .../config/driver_config.yaml | 1 - .../hardware_interface.hpp | 2 +- .../robot_manager_node.hpp | 2 ++ .../launch/startup.launch.py | 7 +++++ .../src/hardware_interface.cpp | 10 +++--- .../src/robot_manager_node.cpp | 31 ++++++++++++++----- 6 files changed, 40 insertions(+), 13 deletions(-) diff --git a/kuka_iiqka_eac_driver/config/driver_config.yaml b/kuka_iiqka_eac_driver/config/driver_config.yaml index de5843b9..eaffd5b7 100644 --- a/kuka_iiqka_eac_driver/config/driver_config.yaml +++ b/kuka_iiqka_eac_driver/config/driver_config.yaml @@ -1,6 +1,5 @@ robot_manager: ros__parameters: - control_mode: 1 position_controller_name: "joint_trajectory_controller" impedance_controller_name: "joint_group_impedance_controller" torque_controller_name: "effort_controller" diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index c6498749..d312506e 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -92,7 +92,7 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface std::vector hw_position_states_; std::vector hw_torque_states_; - double hw_control_mode_command_; + double hw_control_mode_command_ = 0; double server_state_ = 0; int prev_control_mode_; diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index c52a9f93..5f4eebd9 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -60,6 +60,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode rclcpp::Client::SharedPtr change_controller_state_client_; rclcpp::CallbackGroup::SharedPtr cbg_; + rclcpp::CallbackGroup::SharedPtr event_cbg_; std::string robot_model_; kuka_drivers_core::ControllerHandler controller_handler_; @@ -70,6 +71,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::condition_variable control_mode_cv_; std::mutex control_mode_cv_m_; bool control_mode_change_finished_; + bool control_mode_init_ = false; rclcpp::Publisher::SharedPtr control_mode_pub_; rclcpp::Publisher::SharedPtr is_configured_pub_; diff --git a/kuka_iiqka_eac_driver/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py index c53af839..d788cde8 100644 --- a/kuka_iiqka_eac_driver/launch/startup.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup.launch.py @@ -27,6 +27,7 @@ def launch_setup(context, *args, **kwargs): client_ip = LaunchConfiguration("client_ip") use_fake_hardware = LaunchConfiguration("use_fake_hardware") ns = LaunchConfiguration("namespace") + control_mode = LaunchConfiguration("control_mode") x = LaunchConfiguration("x") y = LaunchConfiguration("y") z = LaunchConfiguration("z") @@ -68,6 +69,9 @@ def launch_setup(context, *args, **kwargs): "prefix:=", tf_prefix, " ", + "initial_control_mode:=", + control_mode, + " ", "x:=", x, " ", @@ -118,6 +122,7 @@ def launch_setup(context, *args, **kwargs): { "robot_model": robot_model, "controller_ip": controller_ip, + "control_mode": control_mode }, ], ) @@ -169,6 +174,8 @@ def generate_launch_description(): launch_arguments.append(DeclareLaunchArgument("client_ip", default_value="0.0.0.0")) launch_arguments.append(DeclareLaunchArgument("use_fake_hardware", default_value="false")) launch_arguments.append(DeclareLaunchArgument("namespace", default_value="")) + launch_arguments.append(DeclareLaunchArgument( + "control_mode", default_value="1")) launch_arguments.append(DeclareLaunchArgument("x", default_value="0")) launch_arguments.append(DeclareLaunchArgument("y", default_value="0")) launch_arguments.append(DeclareLaunchArgument("z", default_value="0")) diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index a0e6a5af..1bf3d125 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -169,8 +169,6 @@ CallbackReturn KukaEACHardwareInterface::on_configure(const rclcpp_lifecycle::St { return CallbackReturn::FAILURE; } - // Initialize previous control mode with first - prev_control_mode_ = static_cast(hw_control_mode_command_); if (!SetupQoS()) { @@ -274,6 +272,7 @@ return_type KukaEACHardwareInterface::write(const rclcpp::Time &, const rclcpp:: RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Requesting control mode switch"); send_reply = robot_ptr_->SwitchControlMode( static_cast(hw_control_mode_command_)); + prev_control_mode_ = static_cast(hw_control_mode_command_); } else { @@ -298,8 +297,11 @@ bool KukaEACHardwareInterface::SetupRobot() config.is_secure = false; config.dof = info_.joints.size(); - config.initial_control_mode = - static_cast(hw_control_mode_command_); + config.initial_control_mode = static_cast( + std::stoi(info_.hardware_parameters.at("initial_control_mode"))); + + // Initialize previous control mode + prev_control_mode_ = static_cast(config.initial_control_mode); robot_ptr_ = std::make_unique(config); diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index c2c99dcb..a913f378 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -42,6 +42,7 @@ RobotManagerNode::RobotManagerNode() auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + event_cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); change_hardware_state_client_ = this->create_client( "controller_manager/set_hardware_component_state", qos.get_rmw_qos_profile(), cbg_); change_controller_state_client_ = this->create_client( @@ -56,9 +57,12 @@ RobotManagerNode::RobotManagerNode() control_mode_pub_ = this->create_publisher( "control_mode_handler/control_mode", rclcpp::SystemDefaultsQoS()); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = event_cbg_; + event_subscriber_ = this->create_subscription( "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::UInt8::SharedPtr msg) { this->EventSubscriptionCallback(msg); }); + [this](const std_msgs::msg::UInt8::SharedPtr msg) { this->EventSubscriptionCallback(msg); }, sub_options); // Register parameters this->registerParameter( @@ -86,8 +90,9 @@ RobotManagerNode::RobotManagerNode() kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE, controller_name); }); - // TODO(Svastits): enable control mode change in inactive state, after client lib was fixed - // Currently initial control mode must be specified at setup (configuring transition) + // Currently initial control mode must be specified at the setup of the sdk, which is in the + // configuration transition. Therefore it is not possible to change the control mode after + // startup, only in active state, when the control_mode_handler is active. this->registerParameter( "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), kuka_drivers_core::ParameterSetAccessRights{true, false, true, false, false}, @@ -164,14 +169,16 @@ void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::Sha { switch (static_cast(msg->data)) { - case kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH: + case kuka_drivers_core::HardwareEvent::CONTROL_STARTED: { - std::lock_guard lk(control_mode_cv_m_); - control_mode_change_finished_ = true; - } + { + std::lock_guard lk(control_mode_cv_m_); + control_mode_change_finished_ = true; + } RCLCPP_INFO(get_logger(), "Control mode change has finished, restarting with new mode"); control_mode_cv_.notify_all(); break; + } case kuka_drivers_core::HardwareEvent::CONTROL_STOPPED: case kuka_drivers_core::HardwareEvent::ERROR: RCLCPP_INFO(get_logger(), "External control stopped"); @@ -229,6 +236,9 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) return FAILURE; } + // Workaround until controller_manager/jtc bug is fixed: + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + // Activate RT controller(s) if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, new_controllers.first, new_controllers.second)) @@ -316,6 +326,12 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) bool is_active_state = get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + if (!is_active_state && control_mode_init_) + { + RCLCPP_ERROR(get_logger(), "Control mode parameter can be only changed in active state"); + return false; + } + // Determine which controllers to activate and deactivate try { @@ -391,6 +407,7 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) RCLCPP_INFO(get_logger(), "Successfully changed control mode"); param_declared_ = true; + control_mode_init_ = true; return true; } From 5da166593e693b3e6352be0f39d476ab6ab88c54 Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Thu, 15 Feb 2024 11:44:03 +0100 Subject: [PATCH 13/88] log fix --- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index a913f378..b1e86c38 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -167,15 +167,16 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) { + switch (static_cast(msg->data)) { case kuka_drivers_core::HardwareEvent::CONTROL_STARTED: { + // Nofify lock after control mode change { std::lock_guard lk(control_mode_cv_m_); control_mode_change_finished_ = true; } - RCLCPP_INFO(get_logger(), "Control mode change has finished, restarting with new mode"); control_mode_cv_.notify_all(); break; } From ada4329697cf071978fb7cf42ca0458bd8983240 Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Thu, 15 Feb 2024 11:44:48 +0100 Subject: [PATCH 14/88] log --- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 1bf3d125..9c0c5d73 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -215,7 +215,7 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Deactivating"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Deactivating hardware interface"); stop_requested_ = true; From 60d2b67ac230c63072b5d8621abed2d09fdff0bf Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Thu, 15 Feb 2024 14:47:10 +0100 Subject: [PATCH 15/88] rollback launch --- kuka_iiqka_eac_driver/config/driver_config.yaml | 1 + kuka_iiqka_eac_driver/launch/startup.launch.py | 7 ------- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/kuka_iiqka_eac_driver/config/driver_config.yaml b/kuka_iiqka_eac_driver/config/driver_config.yaml index eaffd5b7..de5843b9 100644 --- a/kuka_iiqka_eac_driver/config/driver_config.yaml +++ b/kuka_iiqka_eac_driver/config/driver_config.yaml @@ -1,5 +1,6 @@ robot_manager: ros__parameters: + control_mode: 1 position_controller_name: "joint_trajectory_controller" impedance_controller_name: "joint_group_impedance_controller" torque_controller_name: "effort_controller" diff --git a/kuka_iiqka_eac_driver/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py index d788cde8..c53af839 100644 --- a/kuka_iiqka_eac_driver/launch/startup.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup.launch.py @@ -27,7 +27,6 @@ def launch_setup(context, *args, **kwargs): client_ip = LaunchConfiguration("client_ip") use_fake_hardware = LaunchConfiguration("use_fake_hardware") ns = LaunchConfiguration("namespace") - control_mode = LaunchConfiguration("control_mode") x = LaunchConfiguration("x") y = LaunchConfiguration("y") z = LaunchConfiguration("z") @@ -69,9 +68,6 @@ def launch_setup(context, *args, **kwargs): "prefix:=", tf_prefix, " ", - "initial_control_mode:=", - control_mode, - " ", "x:=", x, " ", @@ -122,7 +118,6 @@ def launch_setup(context, *args, **kwargs): { "robot_model": robot_model, "controller_ip": controller_ip, - "control_mode": control_mode }, ], ) @@ -174,8 +169,6 @@ def generate_launch_description(): launch_arguments.append(DeclareLaunchArgument("client_ip", default_value="0.0.0.0")) launch_arguments.append(DeclareLaunchArgument("use_fake_hardware", default_value="false")) launch_arguments.append(DeclareLaunchArgument("namespace", default_value="")) - launch_arguments.append(DeclareLaunchArgument( - "control_mode", default_value="1")) launch_arguments.append(DeclareLaunchArgument("x", default_value="0")) launch_arguments.append(DeclareLaunchArgument("y", default_value="0")) launch_arguments.append(DeclareLaunchArgument("z", default_value="0")) From 26675f553e9c299777dd4e737635edaa0e821945 Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Thu, 15 Feb 2024 16:23:43 +0100 Subject: [PATCH 16/88] modify access rights --- .../kuka_iiqka_eac_driver/robot_manager_node.hpp | 1 - kuka_iiqka_eac_driver/src/hardware_interface.cpp | 9 +++------ kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 12 +----------- 3 files changed, 4 insertions(+), 18 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index 5f4eebd9..be924153 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -71,7 +71,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::condition_variable control_mode_cv_; std::mutex control_mode_cv_m_; bool control_mode_change_finished_; - bool control_mode_init_ = false; rclcpp::Publisher::SharedPtr control_mode_pub_; rclcpp::Publisher::SharedPtr is_configured_pub_; diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 9c0c5d73..3d845698 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -196,7 +196,7 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta "Creating event observer failed, error message: %s", create_event_observer.message); } - kuka::external::control::OperationStatus start_control = robot_ptr_->StartControlling(); + kuka::external::control::OperationStatus start_control = robot_ptr_->StartControlling(static_cast(hw_control_mode_command_)); if (start_control.return_code == kuka::external::control::ReturnCode::ERROR) { RCLCPP_ERROR( @@ -205,6 +205,8 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta return CallbackReturn::FAILURE; } + prev_control_mode_ = static_cast(hw_control_mode_command_); + RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "External control session started successfully"); @@ -297,11 +299,6 @@ bool KukaEACHardwareInterface::SetupRobot() config.is_secure = false; config.dof = info_.joints.size(); - config.initial_control_mode = static_cast( - std::stoi(info_.hardware_parameters.at("initial_control_mode"))); - - // Initialize previous control mode - prev_control_mode_ = static_cast(config.initial_control_mode); robot_ptr_ = std::make_unique(config); diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index b1e86c38..bb1b3f59 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -90,12 +90,9 @@ RobotManagerNode::RobotManagerNode() kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE, controller_name); }); - // Currently initial control mode must be specified at the setup of the sdk, which is in the - // configuration transition. Therefore it is not possible to change the control mode after - // startup, only in active state, when the control_mode_handler is active. this->registerParameter( "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), - kuka_drivers_core::ParameterSetAccessRights{true, false, true, false, false}, + kuka_drivers_core::ParameterSetAccessRights{true, true, true, false, false}, [this](int control_mode) { return this->onControlModeChangeRequest(control_mode); }); this->registerStaticParameter( "controller_ip", "", @@ -327,12 +324,6 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) bool is_active_state = get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; - if (!is_active_state && control_mode_init_) - { - RCLCPP_ERROR(get_logger(), "Control mode parameter can be only changed in active state"); - return false; - } - // Determine which controllers to activate and deactivate try { @@ -408,7 +399,6 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) RCLCPP_INFO(get_logger(), "Successfully changed control mode"); param_declared_ = true; - control_mode_init_ = true; return true; } From 55200576a69bcd699b48b12709c55ed75777837e Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Thu, 15 Feb 2024 16:35:03 +0100 Subject: [PATCH 17/88] format --- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 3 ++- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 3d845698..c0b18a74 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -196,7 +196,8 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta "Creating event observer failed, error message: %s", create_event_observer.message); } - kuka::external::control::OperationStatus start_control = robot_ptr_->StartControlling(static_cast(hw_control_mode_command_)); + kuka::external::control::OperationStatus start_control = robot_ptr_->StartControlling( + static_cast(hw_control_mode_command_)); if (start_control.return_code == kuka::external::control::ReturnCode::ERROR) { RCLCPP_ERROR( diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index bb1b3f59..205e42a3 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -62,7 +62,8 @@ RobotManagerNode::RobotManagerNode() event_subscriber_ = this->create_subscription( "event_broadcaster/hardware_event", rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::UInt8::SharedPtr msg) { this->EventSubscriptionCallback(msg); }, sub_options); + [this](const std_msgs::msg::UInt8::SharedPtr msg) { this->EventSubscriptionCallback(msg); }, + sub_options); // Register parameters this->registerParameter( @@ -164,7 +165,6 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::SharedPtr msg) { - switch (static_cast(msg->data)) { case kuka_drivers_core::HardwareEvent::CONTROL_STARTED: From a2d1baa2f61539165086d23fe22537f21f9b07c9 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 08:00:13 +0100 Subject: [PATCH 18/88] recv timeout --- .../include/kuka_iiqka_eac_driver/hardware_interface.hpp | 7 ------- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 5 +++-- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index d312506e..7f242d74 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -72,11 +72,6 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_server_event(kuka_drivers_core::HardwareEvent event); - KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_timeout(int timeout_ms) - { - receive_timeout_ = std::chrono::milliseconds(timeout_ms); - } - private: KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS(); @@ -101,8 +96,6 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface bool msg_received_; bool stop_requested_ = false; - - std::chrono::milliseconds receive_timeout_{6}; }; } // namespace kuka_eac diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index c0b18a74..6b7cb703 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -227,12 +227,13 @@ CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::S return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { + // If motion is stopped by user/error, an event is received, therefore it does not make sense to + // reduce the receive timeout to 4 ms kuka::external::control::OperationStatus receive_state = - robot_ptr_->ReceiveMotionState(receive_timeout_); + robot_ptr_->ReceiveMotionState(std::chrono::milliseconds(1000)); if ((msg_received_ = receive_state.return_code == kuka::external::control::ReturnCode::OK)) { - receive_timeout_ = std::chrono::milliseconds(6); auto & req_message = robot_ptr_->GetLastMotionState(); std::copy( From 4102a5bcd546a93b92a725bc31e14ec26e7718f4 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 08:03:38 +0100 Subject: [PATCH 19/88] prev_control_mode type --- .../include/kuka_iiqka_eac_driver/hardware_interface.hpp | 3 ++- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index 7f242d74..e976818e 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -90,7 +90,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface double hw_control_mode_command_ = 0; double server_state_ = 0; - int prev_control_mode_; + kuka_drivers_core::ControlMode prev_control_mode_ = + kuka_drivers_core::ControlMode::CONTROL_MODE_UNSPECIFIED; kuka_drivers_core::HardwareEvent last_event_ = kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED; diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 6b7cb703..7293d57c 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -206,7 +206,7 @@ CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::Sta return CallbackReturn::FAILURE; } - prev_control_mode_ = static_cast(hw_control_mode_command_); + prev_control_mode_ = static_cast(hw_control_mode_command_); RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), @@ -271,12 +271,13 @@ return_type KukaEACHardwareInterface::write(const rclcpp::Time &, const rclcpp:: RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Sending stop signal"); send_reply = robot_ptr_->StopControlling(); } - else if (static_cast(hw_control_mode_command_) != prev_control_mode_) + else if ( + static_cast(hw_control_mode_command_) != prev_control_mode_) { RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Requesting control mode switch"); send_reply = robot_ptr_->SwitchControlMode( static_cast(hw_control_mode_command_)); - prev_control_mode_ = static_cast(hw_control_mode_command_); + prev_control_mode_ = static_cast(hw_control_mode_command_); } else { From 4b1011c8d605c2fb8aafd34de9092fdbf9347e01 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 08:05:27 +0100 Subject: [PATCH 20/88] fix build --- .../include/kuka_iiqka_eac_driver/event_observer.hpp | 1 - kuka_iiqka_eac_driver/src/hardware_interface.cpp | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index 2dce2e44..851339fb 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -36,7 +36,6 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH); // Increase receive timeout after control mode switch - hw_interface_->set_timeout(3000); RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 7293d57c..08b0846d 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -20,6 +20,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "kuka_drivers_core/hardware_interface_types.hpp" +#include "kuka_drivers_core/control_mode.hpp" #include "kuka/external-control-sdk/iiqka/configuration.h" #include "kuka_iiqka_eac_driver/event_observer.hpp" From 88f47e838bb682bb65581c0cefb9510aaa627fbb Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 08:11:34 +0100 Subject: [PATCH 21/88] mutex --- .../include/kuka_iiqka_eac_driver/hardware_interface.hpp | 2 ++ kuka_iiqka_eac_driver/src/hardware_interface.cpp | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index e976818e..c8e1511a 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -90,6 +90,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface double hw_control_mode_command_ = 0; double server_state_ = 0; + std::mutex event_mutex_; + kuka_drivers_core::ControlMode prev_control_mode_ = kuka_drivers_core::ControlMode::CONTROL_MODE_UNSPECIFIED; kuka_drivers_core::HardwareEvent last_event_ = diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 08b0846d..3fa317dd 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -19,8 +19,8 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "kuka_drivers_core/hardware_interface_types.hpp" #include "kuka_drivers_core/control_mode.hpp" +#include "kuka_drivers_core/hardware_interface_types.hpp" #include "kuka/external-control-sdk/iiqka/configuration.h" #include "kuka_iiqka_eac_driver/event_observer.hpp" @@ -248,7 +248,7 @@ return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::D } // Modify state interface only in read - // TODO(Svasits): guard last_event_ with mutex + std::lock_guard lk(event_mutex_); server_state_ = static_cast(last_event_); return return_type::OK; @@ -345,6 +345,7 @@ bool KukaEACHardwareInterface::SetupQoS() void KukaEACHardwareInterface::set_server_event(kuka_drivers_core::HardwareEvent event) { + std::lock_guard lk(event_mutex_); last_event_ = event; } } // namespace kuka_eac From 52bb6a3c1d271c0f7c157af9cd48c8dda4d96288 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 08:34:48 +0100 Subject: [PATCH 22/88] cleanup --- .../include/kuka_iiqka_eac_driver/event_observer.hpp | 7 ++----- .../include/kuka_iiqka_eac_driver/robot_manager_node.hpp | 6 ------ kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 2 -- 3 files changed, 2 insertions(+), 13 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index 851339fb..d0d34be1 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -32,19 +32,16 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STARTED); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active"); } - void OnControlModeSwitch(const std::string & reason) override + void OnControlModeSwitch(const std::string &) override { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH); - // Increase receive timeout after control mode switch RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); } - void OnStopped(const std::string & reason) override + void OnStopped(const std::string &) override { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STOPPED); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); - RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); hw_interface_->on_deactivate(hw_interface_->get_state()); } void OnError(const std::string & reason) override diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index be924153..2d809332 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -77,12 +77,6 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std_msgs::msg::Bool is_configured_msg_; rclcpp::Subscription::SharedPtr event_subscriber_; - - // There are two kinds of control modes with different number of necessary interfaces to be set: - // - in standard modes (position, torque), only the control signal to the used interface (1) - // - in impedance modes, the setpoint and the parameters describing the behaviour (2) - static constexpr int STANDARD_MODE_IF_SIZE = 1; - static constexpr int IMPEDANCE_MODE_IF_SIZE = 2; }; } // namespace kuka_eac diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 205e42a3..80b878e9 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -37,8 +37,6 @@ RobotManagerNode::RobotManagerNode() control_mode_change_finished_(false) { - RCLCPP_DEBUG(get_logger(), "Starting Robot Manager Node init"); - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); cbg_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); From 38f7d5b92af8674cdf91bacd945fa20bcf16dec5 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 08:41:47 +0100 Subject: [PATCH 23/88] package xml update + ci --- .github/workflows/industrial_ci.yml | 1 + kuka_iiqka_eac_driver/package.xml | 5 ----- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 57b883af..a5ade69d 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,6 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} + BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && mkdir -p kuka-external-control-sdk/build && cd kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && sudo make install' BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} PR_BASE: ${{ github.event.pull_request.base.ref }} diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index 9af0cd03..5a7d9c4f 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -16,14 +16,9 @@ std_srvs sensor_msgs kuka_drivers_core - tinyxml_vendor hardware_interface pluginlib - protobuf-dev - libnanopb-dev - libgrpc - kuka_lbr_iisy_support ros2_control ros2_controllers From 013c7d72201af4ac66dcf65af2feed6debe3c6f2 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 08:45:08 +0100 Subject: [PATCH 24/88] sudo mkdir --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index a5ade69d..88ca883c 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} - BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && mkdir -p kuka-external-control-sdk/build && cd kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && sudo make install' + BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && sudo mkdir -p kuka-external-control-sdk/build && cd kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && sudo make install' BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} PR_BASE: ${{ github.event.pull_request.base.ref }} From 3ee052658e2ff177259eb90c093c9fda7e1bff01 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 09:02:05 +0100 Subject: [PATCH 25/88] fix --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 88ca883c..534065d8 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} - BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && sudo mkdir -p kuka-external-control-sdk/build && cd kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && sudo make install' + BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && mkdir -p ~/kuka-external-control-sdk/build && cd ~/kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && make install' BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} PR_BASE: ${{ github.event.pull_request.base.ref }} From c482233f03d6a778ba38641c9df9e078daa33d93 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 09:05:01 +0100 Subject: [PATCH 26/88] git --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 534065d8..43e3ef8b 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} - BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && mkdir -p ~/kuka-external-control-sdk/build && cd ~/kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && make install' + BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev git && mkdir -p ~/kuka-external-control-sdk/build && cd ~/kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && make install' BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} PR_BASE: ${{ github.event.pull_request.base.ref }} From 58914f405510d77ee86dd0e86eb375b83acdeaeb Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 09:27:20 +0100 Subject: [PATCH 27/88] pipe fix --- .github/workflows/industrial_ci.yml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 43e3ef8b..cf0657ac 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,6 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) EVENT_NAME: ${{ github.event_name }} - BEFORE_INIT: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev git && mkdir -p ~/kuka-external-control-sdk/build && cd ~/kuka-external-control-sdk && git clone https://github.com/kroshu/eci_client_sdk.git && cd build && cmake .. && make install' BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} PR_BASE: ${{ github.event.pull_request.base.ref }} @@ -60,6 +59,19 @@ jobs: with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + + # Install and build kuka-external-control-sdk + - name: Install dependencies + run: apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev + - name: Create work dir for lib + run: mkdir -p ~/kuka-external-control-sdk/build + - name: Clone sdk lib + run: git clone https://github.com/kroshu/eci_client_sdk.git + working-directory: ~/kuka-external-control-sdk + - name: Build and install sdk + run: cmake .. && make install + working-directory: ~/kuka-external-control-sdk/build + # Run industrial_ci - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} From 1b50a328b353033abbedf8bcf0842ed36e8f7ab4 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 09:52:38 +0100 Subject: [PATCH 28/88] sudo --- .github/workflows/industrial_ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index cf0657ac..2e012a90 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -62,14 +62,14 @@ jobs: # Install and build kuka-external-control-sdk - name: Install dependencies - run: apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev + run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev - name: Create work dir for lib run: mkdir -p ~/kuka-external-control-sdk/build - name: Clone sdk lib run: git clone https://github.com/kroshu/eci_client_sdk.git working-directory: ~/kuka-external-control-sdk - name: Build and install sdk - run: cmake .. && make install + run: cmake .. && sudo make install working-directory: ~/kuka-external-control-sdk/build # Run industrial_ci From c140debff2e6cf5cc9b5b45552dd0f1fe8fab091 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 10:06:29 +0100 Subject: [PATCH 29/88] home --- .github/workflows/industrial_ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 2e012a90..2b5bed96 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -67,10 +67,10 @@ jobs: run: mkdir -p ~/kuka-external-control-sdk/build - name: Clone sdk lib run: git clone https://github.com/kroshu/eci_client_sdk.git - working-directory: ~/kuka-external-control-sdk + working-directory: $HOME/kuka-external-control-sdk - name: Build and install sdk run: cmake .. && sudo make install - working-directory: ~/kuka-external-control-sdk/build + working-directory: $HOME/kuka-external-control-sdk/build # Run industrial_ci - uses: 'kroshu/industrial_ci@master' From 123f14408768d1c64a7e7890d43aebdd86a47a6e Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 10:14:40 +0100 Subject: [PATCH 30/88] fix dir --- .github/workflows/industrial_ci.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 2b5bed96..8ae8b163 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -64,13 +64,13 @@ jobs: - name: Install dependencies run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev - name: Create work dir for lib - run: mkdir -p ~/kuka-external-control-sdk/build + run: mkdir -p /home/runner/work/kuka-external-control-sdk/build - name: Clone sdk lib run: git clone https://github.com/kroshu/eci_client_sdk.git - working-directory: $HOME/kuka-external-control-sdk + working-directory: /home/runner/work/kuka-external-control-sdk - name: Build and install sdk run: cmake .. && sudo make install - working-directory: $HOME/kuka-external-control-sdk/build + working-directory: /home/runner/work/kuka-external-control-sdk/build # Run industrial_ci - uses: 'kroshu/industrial_ci@master' From b154233271a54b107a806074fa11ed6359c1847f Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 10:49:28 +0100 Subject: [PATCH 31/88] update name --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 8ae8b163..53e588d8 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -66,7 +66,7 @@ jobs: - name: Create work dir for lib run: mkdir -p /home/runner/work/kuka-external-control-sdk/build - name: Clone sdk lib - run: git clone https://github.com/kroshu/eci_client_sdk.git + run: git clone https://github.com/kroshu/kuka-external-control-sdk.git working-directory: /home/runner/work/kuka-external-control-sdk - name: Build and install sdk run: cmake .. && sudo make install From 1a88dc0e7dd120dfaa9d5cb4c78ec05653f908fd Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 10:54:56 +0100 Subject: [PATCH 32/88] fix dir structure --- .github/workflows/industrial_ci.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 53e588d8..d81588e6 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -68,9 +68,11 @@ jobs: - name: Clone sdk lib run: git clone https://github.com/kroshu/kuka-external-control-sdk.git working-directory: /home/runner/work/kuka-external-control-sdk + - name: Create build dir + run: mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build - name: Build and install sdk run: cmake .. && sudo make install - working-directory: /home/runner/work/kuka-external-control-sdk/build + working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build # Run industrial_ci - uses: 'kroshu/industrial_ci@master' From 0169a81043499a5ff0e6482dd8206689f7e2b519 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 11:03:14 +0100 Subject: [PATCH 33/88] fix dir --- .github/workflows/industrial_ci.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index d81588e6..ec8cf0bd 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -63,12 +63,10 @@ jobs: # Install and build kuka-external-control-sdk - name: Install dependencies run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev - - name: Create work dir for lib - run: mkdir -p /home/runner/work/kuka-external-control-sdk/build - name: Clone sdk lib run: git clone https://github.com/kroshu/kuka-external-control-sdk.git - working-directory: /home/runner/work/kuka-external-control-sdk - - name: Create build dir + working-directory: /home/runner/work + - name: Create build directory run: mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build - name: Build and install sdk run: cmake .. && sudo make install From 031d366021ad9f8c36ef5aaf1e5d28ea2a777213 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 11:21:23 +0100 Subject: [PATCH 34/88] update env --- .github/workflows/industrial_ci.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index ec8cf0bd..329d90e3 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -73,5 +73,8 @@ jobs: working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build # Run industrial_ci + - name: Extend CMAKE_PREFIX_PATH + run: echo "CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}:/usr/local/lib" >> $GITHUB_ENV + - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} From 1688283244f297d572da56a7b5df4de4426e1a63 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 11:21:58 +0100 Subject: [PATCH 35/88] fix deactivate --- .../include/kuka_iiqka_eac_driver/event_observer.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index d0d34be1..be4e5ba1 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -42,7 +42,7 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STOPPED); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); - hw_interface_->on_deactivate(hw_interface_->get_state()); + hw_interface_->deactivate(hw_interface_->get_state()); } void OnError(const std::string & reason) override { @@ -50,7 +50,7 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler RCLCPP_ERROR( rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); - hw_interface_->on_deactivate(hw_interface_->get_state()); + hw_interface_->deactivate(hw_interface_->get_state()); } private: From 16e558aae4d44c250cf45b6d36b10f23f6f92d35 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 11:37:45 +0100 Subject: [PATCH 36/88] find_lib --- kuka_iiqka_eac_driver/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index a57ac37c..2ec2175c 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(controller_manager_msgs REQUIRED) include_directories(include) -find_library(NAMES kuka-external-control-sdk PATHS "/usr/local/lib" REQUIRED) +find_library(NAMES libkuka-external-control-sdk.so PATHS "/usr/local/lib" REQUIRED) add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp From ce584d3111ce3c63eb103c35e048ff010b83d235 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 11:42:56 +0100 Subject: [PATCH 37/88] fix build --- .../include/kuka_iiqka_eac_driver/event_observer.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index be4e5ba1..d0d34be1 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -42,7 +42,7 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STOPPED); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); - hw_interface_->deactivate(hw_interface_->get_state()); + hw_interface_->on_deactivate(hw_interface_->get_state()); } void OnError(const std::string & reason) override { @@ -50,7 +50,7 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler RCLCPP_ERROR( rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); - hw_interface_->deactivate(hw_interface_->get_state()); + hw_interface_->on_deactivate(hw_interface_->get_state()); } private: From 344d634329278433384468787367d47b718ed383 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 11:54:13 +0100 Subject: [PATCH 38/88] debug --- .github/workflows/industrial_ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 329d90e3..d304a978 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -73,8 +73,8 @@ jobs: working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build # Run industrial_ci - - name: Extend CMAKE_PREFIX_PATH - run: echo "CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}:/usr/local/lib" >> $GITHUB_ENV + - name: Debug + run: ls -l /usr/local/lib && ls -l /usr/include/kuka/external-control-sdk/iiqka/ - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} From a80ab26784b61a1033d77a4c9179f03df2d56919 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 12:54:45 +0100 Subject: [PATCH 39/88] try without path --- kuka_iiqka_eac_driver/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 2ec2175c..91fa8e9c 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(controller_manager_msgs REQUIRED) include_directories(include) -find_library(NAMES libkuka-external-control-sdk.so PATHS "/usr/local/lib" REQUIRED) +find_library(NAMES kuka-external-control-sdk REQUIRED) add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp From 0fad5eaa21e5c9cf63b2059dc526f3dacb1eb937 Mon Sep 17 00:00:00 2001 From: Svastits Date: Fri, 16 Feb 2024 12:54:45 +0100 Subject: [PATCH 40/88] Revert "try without path" This reverts commit a80ab26784b61a1033d77a4c9179f03df2d56919. --- kuka_iiqka_eac_driver/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 91fa8e9c..2ec2175c 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(controller_manager_msgs REQUIRED) include_directories(include) -find_library(NAMES kuka-external-control-sdk REQUIRED) +find_library(NAMES libkuka-external-control-sdk.so PATHS "/usr/local/lib" REQUIRED) add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp From 5c98cad8018536b3db2ff3b8704fc4131f037c7b Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 13:25:03 +0100 Subject: [PATCH 41/88] fix multiple deact --- .github/workflows/industrial_ci.yml | 4 +++- kuka_iiqka_eac_driver/CMakeLists.txt | 2 +- .../include/kuka_iiqka_eac_driver/event_observer.hpp | 4 ++-- .../include/kuka_iiqka_eac_driver/hardware_interface.hpp | 2 ++ 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index d304a978..77f5f007 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -73,8 +73,10 @@ jobs: working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build # Run industrial_ci + - name: Extend LD_LIBRARY_PATH + run: echo "LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib" >> $GITHUB_ENV - name: Debug - run: ls -l /usr/local/lib && ls -l /usr/include/kuka/external-control-sdk/iiqka/ + run: ls -l /usr/local/lib && ls -l /usr/include/kuka/external-control-sdk/iiqka/ && echo $LD_LIBRARY_PATH - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 2ec2175c..a57ac37c 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(controller_manager_msgs REQUIRED) include_directories(include) -find_library(NAMES libkuka-external-control-sdk.so PATHS "/usr/local/lib" REQUIRED) +find_library(NAMES kuka-external-control-sdk PATHS "/usr/local/lib" REQUIRED) add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index d0d34be1..659a5fa8 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -42,7 +42,7 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STOPPED); RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); - hw_interface_->on_deactivate(hw_interface_->get_state()); + hw_interface_->set_stop_flag(); } void OnError(const std::string & reason) override { @@ -50,7 +50,7 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler RCLCPP_ERROR( rclcpp::get_logger("KukaEACHardwareInterface"), "External control stopped by an error"); RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), reason.c_str()); - hw_interface_->on_deactivate(hw_interface_->get_state()); + hw_interface_->set_stop_flag(); } private: diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index c8e1511a..f674a3c9 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -72,6 +72,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_server_event(kuka_drivers_core::HardwareEvent event); + KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_stop_flag() { stop_requested_ = true; }; + private: KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS(); From 21a3d7176b6536cda82d098a3daa6db48486d5b4 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 16 Feb 2024 14:30:39 +0100 Subject: [PATCH 42/88] cleanup --- .../include/kuka_iiqka_eac_driver/hardware_interface.hpp | 4 +--- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 9 ++++----- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index f674a3c9..c57305f2 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -77,10 +77,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface private: KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS(); - KUKA_IIQKA_EAC_DRIVER_LOCAL void ObserveControl(); std::unique_ptr robot_ptr_; - kuka::external::control::BaseControlSignal * hw_control_signal_ = nullptr; std::vector hw_position_commands_; std::vector hw_torque_commands_; @@ -100,7 +98,7 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface kuka_drivers_core::HardwareEvent::HARDWARE_EVENT_UNSPECIFIED; bool msg_received_; - bool stop_requested_ = false; + std::atomic stop_requested_{false}; }; } // namespace kuka_eac diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 3fa317dd..86c80f3e 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -262,9 +262,10 @@ return_type KukaEACHardwareInterface::write(const rclcpp::Time &, const rclcpp:: return return_type::OK; } - hw_control_signal_->AddJointPositionValues(hw_position_commands_); - hw_control_signal_->AddTorqueValues(hw_torque_commands_); - hw_control_signal_->AddStiffnessAndDampingValues(hw_stiffness_commands_, hw_damping_commands_); + robot_ptr_->GetControlSignal().AddJointPositionValues(hw_position_commands_); + robot_ptr_->GetControlSignal().AddTorqueValues(hw_torque_commands_); + robot_ptr_->GetControlSignal().AddStiffnessAndDampingValues( + hw_stiffness_commands_, hw_damping_commands_); kuka::external::control::OperationStatus send_reply; if (stop_requested_) @@ -306,8 +307,6 @@ bool KukaEACHardwareInterface::SetupRobot() robot_ptr_ = std::make_unique(config); - hw_control_signal_ = &(robot_ptr_->GetControlSignal()); - kuka::external::control::OperationStatus setup = robot_ptr_->Setup(); if (setup.return_code != kuka::external::control::ReturnCode::OK) From dc1202427475f20ca8917196ff4592c6b970051f Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Fri, 16 Feb 2024 15:26:49 +0100 Subject: [PATCH 43/88] fix controller configuration --- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 86c80f3e..76fb4e73 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -228,10 +228,9 @@ CallbackReturn KukaEACHardwareInterface::on_deactivate(const rclcpp_lifecycle::S return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { - // If motion is stopped by user/error, an event is received, therefore it does not make sense to - // reduce the receive timeout to 4 ms + // Bigger timeout blocks controller configuration kuka::external::control::OperationStatus receive_state = - robot_ptr_->ReceiveMotionState(std::chrono::milliseconds(1000)); + robot_ptr_->ReceiveMotionState(std::chrono::milliseconds(10)); if ((msg_received_ = receive_state.return_code == kuka::external::control::ReturnCode::OK)) { From ddaf6a209f5806709f841cbed9bc968c31fbe1fb Mon Sep 17 00:00:00 2001 From: Svastits Date: Mon, 19 Feb 2024 09:30:51 +0100 Subject: [PATCH 44/88] update build instructions --- README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/README.md b/README.md index d9fadb8f..f07baa44 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,18 @@ sudo apt upgrade rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y ``` +Clone and build kuka-external-control-sdk in a different workspace. + - This library is necessary for the iiQKA ExternalAPI.Control driver + - The library is not a ROS2 package, therefore a different workspace is necessary, otherwise colcon will fail to build it +```bash +mkdir -p ~/sdk_ws/src +git clone https://github.com/kroshu/kuka-external-control-sdk.git +mkdir -p ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build +cd ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build +cmake .. +sudo make install +``` + Build KUKA packages. ```bash cd ~/ros2_ws From 972ccc45f98be2bf5062f4b8c92caf5afa96b48d Mon Sep 17 00:00:00 2001 From: Svastits Date: Mon, 19 Feb 2024 09:33:42 +0100 Subject: [PATCH 45/88] fix --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index f07baa44..a0c54f17 100644 --- a/README.md +++ b/README.md @@ -45,6 +45,7 @@ Clone and build kuka-external-control-sdk in a different workspace. - The library is not a ROS2 package, therefore a different workspace is necessary, otherwise colcon will fail to build it ```bash mkdir -p ~/sdk_ws/src +cd ~/sdk_ws/src git clone https://github.com/kroshu/kuka-external-control-sdk.git mkdir -p ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build cd ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build From 1c1d664ecf664e8251b2bc888b1efd42163ef41a Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 19 Feb 2024 14:05:46 +0100 Subject: [PATCH 46/88] remove unnecessary copy --- kuka_iiqka_eac_driver/src/hardware_interface.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 76fb4e73..130afcb7 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -242,8 +242,6 @@ return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::D std::copy( req_message.GetMeasuredTorques()->begin(), req_message.GetMeasuredTorques()->end(), hw_torque_states_.begin()); - std::copy( - hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); } // Modify state interface only in read From d094985b4b2f256339b64a7fd7fa3d08b80a3f26 Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Wed, 21 Feb 2024 15:06:01 +0100 Subject: [PATCH 47/88] fix switch --- .../kuka_iiqka_eac_driver/event_observer.hpp | 2 + .../hardware_interface.hpp | 4 + .../src/hardware_interface.cpp | 10 ++- .../src/robot_manager_node.cpp | 74 +++++++++++-------- 4 files changed, 57 insertions(+), 33 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index 659a5fa8..50d42532 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -37,6 +37,8 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH); RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); + hw_interface_->reset_cycle_count(); + } void OnStopped(const std::string &) override { diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index c57305f2..52e04621 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -74,6 +74,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_stop_flag() { stop_requested_ = true; }; + KUKA_IIQKA_EAC_DRIVER_PUBLIC void reset_cycle_count() { cycle_count_ = 0; }; + private: KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupQoS(); @@ -89,6 +91,8 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface double hw_control_mode_command_ = 0; double server_state_ = 0; + int cycle_count_ = 0; + std::mutex event_mutex_; diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 76fb4e73..89c8e2d6 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -242,14 +242,18 @@ return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::D std::copy( req_message.GetMeasuredTorques()->begin(), req_message.GetMeasuredTorques()->end(), hw_torque_states_.begin()); - std::copy( - hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); + + if (cycle_count_ == 0) { + std::copy( + hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); + } + + cycle_count_++; } // Modify state interface only in read std::lock_guard lk(event_mutex_); server_state_ = static_cast(last_event_); - return return_type::OK; } diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 80b878e9..f8982fca 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -167,6 +167,7 @@ void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::Sha { case kuka_drivers_core::HardwareEvent::CONTROL_STARTED: { + RCLCPP_INFO(get_logger(), "External control started"); // Nofify lock after control mode change { std::lock_guard lk(control_mode_cv_m_); @@ -189,6 +190,9 @@ void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::Sha this->on_deactivate(get_current_state()); } break; + case kuka_drivers_core::HardwareEvent::CONTROL_MODE_SWITCH: + control_mode_change_finished_ = false; + break; default: break; } @@ -327,6 +331,11 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) { switch_controllers = controller_handler_.GetControllersForSwitch(kuka_drivers_core::ControlMode(control_mode)); + + if (param_declared_ && control_mode == 1) { + switch_controllers.first.push_back("joint_trajectory_controller"); + switch_controllers.second.push_back("joint_trajectory_controller"); + } } catch (const std::exception & e) { @@ -334,27 +343,31 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) return false; } - // Activate controllers needed for the new control mode - if (is_active_state) - { + + // Publish the control mode to controller handler + auto message = std_msgs::msg::UInt32(); + message.data = control_mode; + control_mode_pub_->publish(message); + RCLCPP_INFO(get_logger(), "Control mode change process has started"); + + // Deactivate unnecessary controllers if ( - !switch_controllers.first.empty() && + !switch_controllers.second.empty() && !kuka_drivers_core::changeControllerState( - change_controller_state_client_, switch_controllers.first, {})) + change_controller_state_client_, {}, switch_controllers.second)) { - RCLCPP_ERROR(get_logger(), "Could not activate controllers for new control mode"); + RCLCPP_ERROR(get_logger(), "Could not deactivate controllers for new control mode"); // TODO(Svastits): this can be removed if rollback is implemented properly this->on_deactivate(get_current_state()); return false; } - controller_handler_.ApproveControllerActivation(); - } - - // Publish the control mode to controller handler - auto message = std_msgs::msg::UInt32(); - message.data = control_mode; - control_mode_pub_->publish(message); - RCLCPP_INFO(get_logger(), "Control mode change process has started"); + if (!controller_handler_.ApproveControllerDeactivation()) + { + RCLCPP_ERROR( + get_logger(), + "Controller handler state is improper, active controller list was modified" + "before approval"); + } if (is_active_state) { @@ -375,23 +388,24 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) control_mode_lk.unlock(); RCLCPP_INFO(get_logger(), "Robot Controller finished control mode change"); - // Deactivate unnecessary controllers - if ( - !switch_controllers.second.empty() && - !kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, switch_controllers.second)) - { - RCLCPP_ERROR(get_logger(), "Could not deactivate controllers for new control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - return false; - } - if (!controller_handler_.ApproveControllerDeactivation()) + + + // Activate controllers needed for the new control mode + if (is_active_state) { - RCLCPP_ERROR( - get_logger(), - "Controller handler state is improper, active controller list was modified" - "before approval"); + // Workaround until controller_manager/jtc bug is fixed: + std::this_thread::sleep_for(std::chrono::milliseconds(550)); + if ( + !switch_controllers.first.empty() && + !kuka_drivers_core::changeControllerState( + change_controller_state_client_, switch_controllers.first, {})) + { + RCLCPP_ERROR(get_logger(), "Could not activate controllers for new control mode"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return false; + } + controller_handler_.ApproveControllerActivation(); } } From 19c06c56ace1a0f34c437d1a185082b096d46842 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Thu, 22 Feb 2024 17:25:06 +0100 Subject: [PATCH 48/88] package xml --- .github/workflows/industrial_ci.yml | 32 ++++++++++++++--------------- kuka_iiqka_eac_driver/package.xml | 1 + 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 77f5f007..12596c2f 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -38,7 +38,7 @@ jobs: TEST_COVERAGE: true # Hacky solution needed to be able to build upstream WS: # kuka_driver_interfaces and kuka_drivers_core must be also added to upstream - UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' + UPSTREAM_WORKSPACE: 'github:kroshu/kuka-external-control-sdk#master github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) @@ -61,22 +61,22 @@ jobs: key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} # Install and build kuka-external-control-sdk - - name: Install dependencies - run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev - - name: Clone sdk lib - run: git clone https://github.com/kroshu/kuka-external-control-sdk.git - working-directory: /home/runner/work - - name: Create build directory - run: mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build - - name: Build and install sdk - run: cmake .. && sudo make install - working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build + # - name: Install dependencies + # run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev + # - name: Clone sdk lib + # run: git clone https://github.com/kroshu/kuka-external-control-sdk.git + # working-directory: /home/runner/work + # - name: Create build directory + # run: mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build + # - name: Build and install sdk + # run: cmake .. && sudo make install + # working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build - # Run industrial_ci - - name: Extend LD_LIBRARY_PATH - run: echo "LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib" >> $GITHUB_ENV - - name: Debug - run: ls -l /usr/local/lib && ls -l /usr/include/kuka/external-control-sdk/iiqka/ && echo $LD_LIBRARY_PATH + # # Run industrial_ci + # - name: Extend LD_LIBRARY_PATH + # run: echo "LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib" >> $GITHUB_ENV + # - name: Debug + # run: ls -l /usr/local/lib && ls -l /usr/include/kuka/external-control-sdk/iiqka/ && echo $LD_LIBRARY_PATH - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index 5a7d9c4f..b96d4a88 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -18,6 +18,7 @@ kuka_drivers_core hardware_interface pluginlib + kuka_external_control_sdk kuka_lbr_iisy_support ros2_control From fb688accd8a7fae8ffc22010a36630807b5c632c Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 07:42:55 +0100 Subject: [PATCH 49/88] install deps --- .github/workflows/industrial_ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 12596c2f..a6da3422 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -61,8 +61,8 @@ jobs: key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} # Install and build kuka-external-control-sdk - # - name: Install dependencies - # run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev + - name: Install dependencies + run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev # - name: Clone sdk lib # run: git clone https://github.com/kroshu/kuka-external-control-sdk.git # working-directory: /home/runner/work From a41cd8526aed18388d6685212f0e702263a2d16a Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 07:53:17 +0100 Subject: [PATCH 50/88] format --- .github/workflows/industrial_ci.yml | 2 +- .../kuka_iiqka_eac_driver/event_observer.hpp | 7 ++- .../hardware_interface.hpp | 5 +- .../src/hardware_interface.cpp | 3 +- .../src/robot_manager_node.cpp | 48 +++++++++---------- 5 files changed, 33 insertions(+), 32 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index a6da3422..f1f2e53d 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -38,7 +38,7 @@ jobs: TEST_COVERAGE: true # Hacky solution needed to be able to build upstream WS: # kuka_driver_interfaces and kuka_drivers_core must be also added to upstream - UPSTREAM_WORKSPACE: 'github:kroshu/kuka-external-control-sdk#master github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' + UPSTREAM_WORKSPACE: 'github:kroshu/kuka-external-control-sdk#master -kuka-external-control-sdk/kuka-external-control-sdk-examples github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp index 50d42532..3f1947c2 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/event_observer.hpp @@ -15,6 +15,7 @@ #ifndef KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ #define KUKA_IIQKA_EAC_DRIVER__EVENT_OBSERVER_HPP_ +#include #include "rclcpp/macros.hpp" #include "kuka/external-control-sdk/common/irobot.h" @@ -26,7 +27,10 @@ namespace kuka_eac class KukaEACEventObserver : public kuka::external::control::EventHandler { public: - KukaEACEventObserver(KukaEACHardwareInterface * hw_interface) : hw_interface_(hw_interface) {} + explicit KukaEACEventObserver(KukaEACHardwareInterface * hw_interface) + : hw_interface_(hw_interface) + { + } void OnSampling() override { hw_interface_->set_server_event(kuka_drivers_core::HardwareEvent::CONTROL_STARTED); @@ -38,7 +42,6 @@ class KukaEACEventObserver : public kuka::external::control::EventHandler RCLCPP_INFO( rclcpp::get_logger("KukaEACHardwareInterface"), "Control mode switch is in progress"); hw_interface_->reset_cycle_count(); - } void OnStopped(const std::string &) override { diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp index 52e04621..f59f178b 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/hardware_interface.hpp @@ -72,9 +72,9 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_server_event(kuka_drivers_core::HardwareEvent event); - KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_stop_flag() { stop_requested_ = true; }; + KUKA_IIQKA_EAC_DRIVER_PUBLIC void set_stop_flag() { stop_requested_ = true; } - KUKA_IIQKA_EAC_DRIVER_PUBLIC void reset_cycle_count() { cycle_count_ = 0; }; + KUKA_IIQKA_EAC_DRIVER_PUBLIC void reset_cycle_count() { cycle_count_ = 0; } private: KUKA_IIQKA_EAC_DRIVER_LOCAL bool SetupRobot(); @@ -93,7 +93,6 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface double server_state_ = 0; int cycle_count_ = 0; - std::mutex event_mutex_; kuka_drivers_core::ControlMode prev_control_mode_ = diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index 89c8e2d6..bb6c6b02 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -243,7 +243,8 @@ return_type KukaEACHardwareInterface::read(const rclcpp::Time &, const rclcpp::D req_message.GetMeasuredTorques()->begin(), req_message.GetMeasuredTorques()->end(), hw_torque_states_.begin()); - if (cycle_count_ == 0) { + if (cycle_count_ == 0) + { std::copy( hw_position_states_.begin(), hw_position_states_.end(), hw_position_commands_.begin()); } diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index f8982fca..8e116a75 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -331,8 +331,9 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) { switch_controllers = controller_handler_.GetControllersForSwitch(kuka_drivers_core::ControlMode(control_mode)); - - if (param_declared_ && control_mode == 1) { + + if (param_declared_ && control_mode == 1) + { switch_controllers.first.push_back("joint_trajectory_controller"); switch_controllers.second.push_back("joint_trajectory_controller"); } @@ -343,31 +344,30 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) return false; } - // Publish the control mode to controller handler auto message = std_msgs::msg::UInt32(); message.data = control_mode; control_mode_pub_->publish(message); RCLCPP_INFO(get_logger(), "Control mode change process has started"); - // Deactivate unnecessary controllers - if ( - !switch_controllers.second.empty() && - !kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, switch_controllers.second)) - { - RCLCPP_ERROR(get_logger(), "Could not deactivate controllers for new control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - return false; - } - if (!controller_handler_.ApproveControllerDeactivation()) - { - RCLCPP_ERROR( - get_logger(), - "Controller handler state is improper, active controller list was modified" - "before approval"); - } + // Deactivate unnecessary controllers + if ( + !switch_controllers.second.empty() && + !kuka_drivers_core::changeControllerState( + change_controller_state_client_, {}, switch_controllers.second)) + { + RCLCPP_ERROR(get_logger(), "Could not deactivate controllers for new control mode"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return false; + } + if (!controller_handler_.ApproveControllerDeactivation()) + { + RCLCPP_ERROR( + get_logger(), + "Controller handler state is improper, active controller list was modified" + "before approval"); + } if (is_active_state) { @@ -388,13 +388,11 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) control_mode_lk.unlock(); RCLCPP_INFO(get_logger(), "Robot Controller finished control mode change"); - - // Activate controllers needed for the new control mode if (is_active_state) { - // Workaround until controller_manager/jtc bug is fixed: - std::this_thread::sleep_for(std::chrono::milliseconds(550)); + // Workaround until controller_manager/jtc bug is fixed: + std::this_thread::sleep_for(std::chrono::milliseconds(550)); if ( !switch_controllers.first.empty() && !kuka_drivers_core::changeControllerState( From ac6861a85e8bff522384a4de19a06776a8eeaafd Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 08:51:10 +0100 Subject: [PATCH 51/88] try pre-build script --- .github/workflows/industrial_ci.yml | 3 ++- .github/workflows/install_sdk.sh | 6 ++++++ 2 files changed, 8 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/install_sdk.sh diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index f1f2e53d..ee30b2bb 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -38,10 +38,11 @@ jobs: TEST_COVERAGE: true # Hacky solution needed to be able to build upstream WS: # kuka_driver_interfaces and kuka_drivers_core must be also added to upstream - UPSTREAM_WORKSPACE: 'github:kroshu/kuka-external-control-sdk#master -kuka-external-control-sdk/kuka-external-control-sdk-examples github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' + UPSTREAM_WORKSPACE: github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) + BEFORE_BUILD_TARGET_WORKSPACE: .github/workflows/install_sdk.sh EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} diff --git a/.github/workflows/install_sdk.sh b/.github/workflows/install_sdk.sh new file mode 100644 index 00000000..bd3c493b --- /dev/null +++ b/.github/workflows/install_sdk.sh @@ -0,0 +1,6 @@ +cd /home/runner/work +git clone https://github.com/kroshu/kuka-external-control-sdk.git +mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build +cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build +cmake .. +make install From b731c6ba3a977bbcf0ed3fe5b27e1f0297ee5acf Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 08:53:08 +0100 Subject: [PATCH 52/88] remove sdk dep --- kuka_iiqka_eac_driver/package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index b96d4a88..4a8c7eab 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -18,8 +18,7 @@ kuka_drivers_core hardware_interface pluginlib - kuka_external_control_sdk - + kuka_lbr_iisy_support ros2_control ros2_controllers From ff2beb9f2fde2aaa09249166c1b27da85f411b5a Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 09:26:22 +0100 Subject: [PATCH 53/88] fix upstream --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index ee30b2bb..a63708e6 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -38,7 +38,7 @@ jobs: TEST_COVERAGE: true # Hacky solution needed to be able to build upstream WS: # kuka_driver_interfaces and kuka_drivers_core must be also added to upstream - UPSTREAM_WORKSPACE: github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' + UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka_controllers#master github:kroshu/kuka_drivers#master -kuka_drivers/examples -kuka_drivers/kuka_drivers -kuka_drivers/kuka_iiqka_eac_driver -kuka_drivers/kuka_kss_rsi_driver -kuka_drivers/kuka_sunrise_fri_driver' ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) From f456876ba25d77e41a93d069046fcff2e2567336 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 09:43:27 +0100 Subject: [PATCH 54/88] sudo --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index a63708e6..0ef0d7a6 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -42,7 +42,7 @@ jobs: ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) - BEFORE_BUILD_TARGET_WORKSPACE: .github/workflows/install_sdk.sh + BEFORE_BUILD_TARGET_WORKSPACE: sudo .github/workflows/install_sdk.sh EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From baa677aa34dc81d813eec2c503045fd8de31a886 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 09:52:43 +0100 Subject: [PATCH 55/88] clean up control mode handling --- .../kuka_drivers_core/controller_handler.hpp | 4 +- kuka_drivers_core/src/controller_handler.cpp | 15 +- .../robot_manager_node.hpp | 3 +- kuka_iiqka_eac_driver/package.xml | 2 +- .../src/robot_manager_node.cpp | 141 ++++-------------- 5 files changed, 53 insertions(+), 112 deletions(-) diff --git a/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp b/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp index 29feecd9..0f618468 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/controller_handler.hpp @@ -76,7 +76,7 @@ class ControllerHandler * * @param fixed_controllers: Controllers that have to be active in all control modes */ - explicit ControllerHandler(std::vector fixed_controllers); + explicit ControllerHandler(std::vector fixed_controllers = {}); /** * @brief Destroy the control mode handler object @@ -126,6 +126,8 @@ class ControllerHandler * */ bool ApproveControllerDeactivation(); + + std::vector GetControllersForMode(ControlMode control_mode); }; } // namespace kuka_drivers_core diff --git a/kuka_drivers_core/src/controller_handler.cpp b/kuka_drivers_core/src/controller_handler.cpp index 00a6d27a..018c0e69 100644 --- a/kuka_drivers_core/src/controller_handler.cpp +++ b/kuka_drivers_core/src/controller_handler.cpp @@ -72,7 +72,7 @@ ControllerHandler::GetControllersForSwitch(ControlMode new_control_mode) { if (control_mode_map_.find(new_control_mode) == control_mode_map_.end()) { - // Not valid control mode, through error + // Not valid control mode, throw exception throw std::out_of_range("Attribute new_control_mode is out of range"); } @@ -148,4 +148,17 @@ bool ControllerHandler::ApproveControllerDeactivation() return true; } + +std::vector ControllerHandler::GetControllersForMode(ControlMode control_mode) +{ + std::vector controllers; + + auto controller_types = control_mode_map_.at(control_mode); + controllers.push_back(controller_types.standard_controller); + if (!controller_types.impedance_controller.empty()) + { + controllers.push_back(controller_types.impedance_controller); + } +} + } // namespace kuka_drivers_core diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index 2d809332..c2f3abab 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -64,9 +64,10 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::string robot_model_; kuka_drivers_core::ControllerHandler controller_handler_; + kuka_drivers_core::ControlMode control_mode_ = + kuka_drivers_core::ControlMode::CONTROL_MODE_UNSPECIFIED; std::atomic terminate_{false}; - bool param_declared_ = false; std::condition_variable control_mode_cv_; std::mutex control_mode_cv_m_; diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index 4a8c7eab..5a7d9c4f 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -18,7 +18,7 @@ kuka_drivers_core hardware_interface pluginlib - + kuka_lbr_iisy_support ros2_control ros2_controllers diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 8e116a75..cbfd7651 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -31,9 +31,6 @@ namespace kuka_eac // after controller handler properly implemented with working initial control mode change RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_manager"), - controller_handler_({ - "joint_state_broadcaster", - }), control_mode_change_finished_(false) { @@ -109,7 +106,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) { // Publish control mode parameter to notify control_mode_handler of initial control mode auto message = std_msgs::msg::UInt32(); - message.data = static_cast(this->get_parameter("control_mode").as_int()); + message.data = static_cast(control_mode_); control_mode_pub_->publish(message); // Configure hardware interface @@ -212,50 +209,19 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) return FAILURE; } - // Select controllers - auto control_mode = this->get_parameter("control_mode").as_int(); - std::pair, std::vector> new_controllers; - - try - { - new_controllers = - controller_handler_.GetControllersForSwitch(kuka_drivers_core::ControlMode(control_mode)); - } - catch (const std::exception & e) - { - RCLCPP_ERROR(get_logger(), "Error while activating controllers: %s", e.what()); - return ERROR; - } - - // Deactivate list for activation should always be empty, safety check - if (!new_controllers.second.empty()) - { - RCLCPP_ERROR( - get_logger(), - "Controller handler state is improper, active controller list not empty before activation"); - return FAILURE; - } - // Workaround until controller_manager/jtc bug is fixed: std::this_thread::sleep_for(std::chrono::milliseconds(20)); // Activate RT controller(s) if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, new_controllers.first, new_controllers.second)) + change_controller_state_client_, controller_handler_.GetControllersForMode(control_mode_), + {})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); this->on_deactivate(get_current_state()); return FAILURE; } - controller_handler_.ApproveControllerActivation(); - if (!controller_handler_.ApproveControllerDeactivation()) - { - RCLCPP_ERROR( - get_logger(), - "Controller handler state is improper, active controller list was modified before approval"); - } - RCLCPP_INFO(get_logger(), "Successfully activated controllers"); // Return failure if control is stopped while in state activating @@ -282,98 +248,59 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // Stop RT controllers if (!kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, controller_handler_.GetControllersForDeactivation(), + change_controller_state_client_, {}, + controller_handler_.GetControllersForMode(control_mode_), SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not stop RT controllers"); return ERROR; } - if (!controller_handler_.ApproveControllerDeactivation()) - { - RCLCPP_ERROR( - get_logger(), - "Controller handler state is improper, active controller list was modified before approval"); - } - RCLCPP_INFO(get_logger(), "Successfully stopped controllers"); return SUCCESS; } bool RobotManagerNode::onControlModeChangeRequest(int control_mode) { - if (param_declared_ && this->get_parameter("control_mode").as_int() == control_mode) + if (control_mode_ == static_cast(control_mode)) { RCLCPP_WARN(get_logger(), "Tried to change control mode to the one currently used"); return true; } RCLCPP_INFO(get_logger(), "Control mode change requested"); - // TODO(komaromi): Remove this if a new control mode is supported if ( - control_mode == static_cast(kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL) || - control_mode == static_cast(kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL) || - control_mode == static_cast(kuka_drivers_core::ControlMode::WRENCH_CONTROL) || - control_mode == static_cast(kuka_drivers_core::ControlMode::JOINT_VELOCITY_CONTROL) || - control_mode == static_cast(kuka_drivers_core::ControlMode::CARTESIAN_VELOCITY_CONTROL)) + control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL) || + control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL) || + control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL)) { RCLCPP_ERROR(get_logger(), "Tried to change to a not implemented control mode"); return false; } - std::pair, std::vector> switch_controllers; - bool is_active_state = get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; - // Determine which controllers to activate and deactivate - try - { - switch_controllers = - controller_handler_.GetControllersForSwitch(kuka_drivers_core::ControlMode(control_mode)); - - if (param_declared_ && control_mode == 1) - { - switch_controllers.first.push_back("joint_trajectory_controller"); - switch_controllers.second.push_back("joint_trajectory_controller"); - } - } - catch (const std::exception & e) - { - RCLCPP_ERROR(get_logger(), "Error while control mode change: %s", e.what()); - return false; - } - // Publish the control mode to controller handler auto message = std_msgs::msg::UInt32(); message.data = control_mode; control_mode_pub_->publish(message); RCLCPP_INFO(get_logger(), "Control mode change process has started"); - // Deactivate unnecessary controllers - if ( - !switch_controllers.second.empty() && - !kuka_drivers_core::changeControllerState( - change_controller_state_client_, {}, switch_controllers.second)) - { - RCLCPP_ERROR(get_logger(), "Could not deactivate controllers for new control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - return false; - } - if (!controller_handler_.ApproveControllerDeactivation()) - { - RCLCPP_ERROR( - get_logger(), - "Controller handler state is improper, active controller list was modified" - "before approval"); - } - if (is_active_state) { - // The driver is in active state + // Deactivate controllers of previous control mode + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, {}, + controller_handler_.GetControllersForMode(control_mode_))) + { + RCLCPP_ERROR(get_logger(), "Could not deactivate controllers of previous control mode"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return false; + } - // Wait for approval that the robot succefully changed control mode + // Wait for event of successful restart std::unique_lock control_mode_lk(this->control_mode_cv_m_); if (!this->control_mode_cv_.wait_for( @@ -388,27 +315,25 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) control_mode_lk.unlock(); RCLCPP_INFO(get_logger(), "Robot Controller finished control mode change"); + // Workaround until controller_manager/jtc bug is fixed: + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + // Activate controllers needed for the new control mode - if (is_active_state) + if (!kuka_drivers_core::changeControllerState( + change_controller_state_client_, + controller_handler_.GetControllersForMode( + static_cast(control_mode)), + {})) { - // Workaround until controller_manager/jtc bug is fixed: - std::this_thread::sleep_for(std::chrono::milliseconds(550)); - if ( - !switch_controllers.first.empty() && - !kuka_drivers_core::changeControllerState( - change_controller_state_client_, switch_controllers.first, {})) - { - RCLCPP_ERROR(get_logger(), "Could not activate controllers for new control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly - this->on_deactivate(get_current_state()); - return false; - } - controller_handler_.ApproveControllerActivation(); + RCLCPP_ERROR(get_logger(), "Could not activate controllers for new control mode"); + // TODO(Svastits): this can be removed if rollback is implemented properly + this->on_deactivate(get_current_state()); + return false; } } RCLCPP_INFO(get_logger(), "Successfully changed control mode"); - param_declared_ = true; + control_mode_ = static_cast(control_mode); return true; } From 9af1556cca32a2ad8de6044cce4032bd1c13b4ad Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 09:55:27 +0100 Subject: [PATCH 56/88] fix before_build --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 0ef0d7a6..7ed5aa88 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -42,7 +42,7 @@ jobs: ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) - BEFORE_BUILD_TARGET_WORKSPACE: sudo .github/workflows/install_sdk.sh + BEFORE_BUILD_TARGET_WORKSPACE: '.github/workflows/install_sdk.sh' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From ca5b320d3cc1e9327a9c4bf25286da4f3b66a87d Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 10:04:59 +0100 Subject: [PATCH 57/88] try path fix --- .github/workflows/industrial_ci.yml | 2 +- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 7ed5aa88..9d71645b 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -42,7 +42,7 @@ jobs: ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) - BEFORE_BUILD_TARGET_WORKSPACE: '.github/workflows/install_sdk.sh' + BEFORE_BUILD_TARGET_WORKSPACE: './.github/workflows/install_sdk.sh' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index cbfd7651..81486ea8 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -30,8 +30,7 @@ namespace kuka_eac // TODO(Komaromi): Re-add "control_mode_handler" controller to controller_handlers constructor // after controller handler properly implemented with working initial control mode change RobotManagerNode::RobotManagerNode() -: kuka_drivers_core::ROS2BaseLCNode("robot_manager"), - control_mode_change_finished_(false) +: kuka_drivers_core::ROS2BaseLCNode("robot_manager"), control_mode_change_finished_(false) { auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); From b4d05923ba167e6233750ac3dab51754b9ce4d5d Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 10:13:23 +0100 Subject: [PATCH 58/88] before init --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 9d71645b..7d70cc10 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -42,7 +42,7 @@ jobs: ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) - BEFORE_BUILD_TARGET_WORKSPACE: './.github/workflows/install_sdk.sh' + BEFORE_INIT: 'cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From c95dd231f09ffb5038ae1aa8fa9188690efc03fd Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 10:17:04 +0100 Subject: [PATCH 59/88] install deps --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 7d70cc10..ed5ec047 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -42,7 +42,7 @@ jobs: ROS_DISTRO: humble env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) - BEFORE_INIT: 'cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' + BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From f0cdacd8f2b626dcd21d0138cbf6a581eb325be2 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 10:31:14 +0100 Subject: [PATCH 60/88] fix install path --- kuka_iiqka_eac_driver/CMakeLists.txt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index a57ac37c..7567f05b 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -31,7 +31,8 @@ find_package(controller_manager_msgs REQUIRED) include_directories(include) -find_library(NAMES kuka-external-control-sdk PATHS "/usr/local/lib" REQUIRED) +find_library(EXTERNAL_CONTROL_SDK kuka-external-control-sdk PATHS "~/.local/lib" REQUIRED) +find_library(EXTERNAL_CONTROL_SDK_PROTOBUF kuka-external-control-sdk-protobuf PATHS "~/.local/lib" REQUIRED) add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp @@ -42,13 +43,13 @@ add_library(${PROJECT_NAME} SHARED target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY") ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface kuka_drivers_core) -target_link_libraries(${PROJECT_NAME} kuka-external-control-sdk) +target_link_libraries(${PROJECT_NAME} ${EXTERNAL_CONTROL_SDK}) add_executable(robot_manager_node src/robot_manager_node.cpp) ament_target_dependencies(robot_manager_node rclcpp kuka_drivers_core sensor_msgs controller_manager_msgs) -target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers kuka-external-control-sdk) +target_link_libraries(robot_manager_node kuka_drivers_core::communication_helpers ${EXTERNAL_CONTROL_SDK}) pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) From 6a102eb40d91a4ca32a5ade411ab08d70030e9c3 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 11:13:10 +0100 Subject: [PATCH 61/88] protobuf dep --- kuka_drivers_core/src/controller_handler.cpp | 1 + kuka_sunrise_fri_driver/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/kuka_drivers_core/src/controller_handler.cpp b/kuka_drivers_core/src/controller_handler.cpp index 018c0e69..fc3d372b 100644 --- a/kuka_drivers_core/src/controller_handler.cpp +++ b/kuka_drivers_core/src/controller_handler.cpp @@ -159,6 +159,7 @@ std::vector ControllerHandler::GetControllersForMode(ControlMode co { controllers.push_back(controller_types.impedance_controller); } + return controllers; } } // namespace kuka_drivers_core diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index eb74f19d..90a225b9 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -21,6 +21,7 @@ kuka_drivers_core hardware_interface controller_manager_msgs + protobuf-dev ros2_control ros2_controllers From 74e297c0b2245a3cd49997106fcdb3154beed581 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 11:31:23 +0100 Subject: [PATCH 62/88] fix dep --- kuka_sunrise_fri_driver/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index 90a225b9..855faac0 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -21,7 +21,7 @@ kuka_drivers_core hardware_interface controller_manager_msgs - protobuf-dev + libnanopb-dev ros2_control ros2_controllers From b9d8d4b802c377795247e1df3beab18750215a73 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 11:36:21 +0100 Subject: [PATCH 63/88] fix condition --- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 81486ea8..78238a77 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -269,8 +269,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) RCLCPP_INFO(get_logger(), "Control mode change requested"); if ( - control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL) || - control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL) || + control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL) && + control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_IMPEDANCE_CONTROL) && control_mode != static_cast(kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL)) { RCLCPP_ERROR(get_logger(), "Tried to change to a not implemented control mode"); From fd8ec016ba8bec65837c91a5664075f5b3c5ff89 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 11:51:52 +0100 Subject: [PATCH 64/88] target include --- kuka_iiqka_eac_driver/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 7567f05b..912f6a33 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -38,6 +38,11 @@ add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp ) +target_include_directories(${PROJECT_NAME} + PUBLIC + "~/.local/include" +) + # 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 "KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY") From d76dcc800ac701b6178561b073a5d4364812d24b Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 12:21:31 +0100 Subject: [PATCH 65/88] fix hack --- kuka_iiqka_eac_driver/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 912f6a33..627c33c5 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -75,8 +75,8 @@ ament_export_libraries( ${PROJECT_NAME} ) -# Hack to extend $LD_LIBRARY_PATH with /usr/local/lib when sourcing +# Hack to extend $LD_LIBRARY_PATH with ~/.local/include when sourcing file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv - "prepend-non-duplicate;LD_LIBRARY_PATH;/usr/local/lib") + "prepend-non-duplicate;LD_LIBRARY_PATH;~/.local/include") ament_package() From a974c294afd15133bf43350e881d28d8746753f4 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 13:19:08 +0100 Subject: [PATCH 66/88] remove activation test --- kuka_iiqka_eac_driver/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 627c33c5..a9b077b2 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -67,7 +67,8 @@ install(DIRECTORY config launch test if(BUILD_TESTING) find_package(launch_testing_ament_cmake) add_launch_test(test/test_driver_startup.py) - add_launch_test(test/test_driver_activation.py) + # TODO: readd activation test, if KUKA mock hardware is finished + # add_launch_test(test/test_driver_activation.py) add_launch_test(test/test_multi_robot_startup.py) endif() From 4df484ab702584a1d0aa420c8c800ec8cb09903f Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 13:35:06 +0100 Subject: [PATCH 67/88] spell --- kuka_iiqka_eac_driver/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index a9b077b2..a40eb7ac 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -67,8 +67,8 @@ install(DIRECTORY config launch test if(BUILD_TESTING) find_package(launch_testing_ament_cmake) add_launch_test(test/test_driver_startup.py) - # TODO: readd activation test, if KUKA mock hardware is finished - # add_launch_test(test/test_driver_activation.py) + # TODO: re-add activation test, if KUKA mock hardware is finished + # add_launch_test(test/test_driver_activation.py) add_launch_test(test/test_multi_robot_startup.py) endif() From a924ae2c90e70368fb9192a393c3e7d17d1bf54d Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 14:20:56 +0100 Subject: [PATCH 68/88] fix tests with new config --- .../config/ros2_controller_config.yaml | 6 +++++- .../test/config/test1_ros2_controller_config.yaml | 4 +++- .../test/config/test2_ros2_controller_config.yaml | 4 +++- kuka_iiqka_eac_driver/test/test_driver_activation.py | 4 +++- kuka_iiqka_eac_driver/test/test_driver_startup.py | 4 +++- kuka_iiqka_eac_driver/test/test_multi_robot_startup.py | 7 ++++++- kuka_kss_rsi_driver/config/ros2_controller_config.yaml | 10 +++++++++- .../test/config/test1_ros2_controller_config.yaml | 4 +++- .../test/config/test2_ros2_controller_config.yaml | 4 +++- kuka_kss_rsi_driver/test/test_driver_activation.py | 4 +++- kuka_kss_rsi_driver/test/test_driver_startup.py | 4 +++- kuka_kss_rsi_driver/test/test_multi_robot_startup.py | 7 ++++++- .../config/ros2_controller_config.yaml | 4 +++- .../test/config/test1_ros2_controller_config.yaml | 4 +++- .../test/config/test2_ros2_controller_config.yaml | 4 +++- kuka_sunrise_fri_driver/test/test_driver_startup.py | 4 +++- .../test/test_multi_robot_startup.py | 7 ++++++- 17 files changed, 68 insertions(+), 17 deletions(-) diff --git a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml index 782d91f1..34492df5 100644 --- a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml @@ -15,4 +15,8 @@ controller_manager: event_broadcaster: type: kuka_controllers/EventBroadcaster - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "lbr_iisy3_r760" + - "lbr_iisy11_r1300" + - "lbr_iisy15_r930" diff --git a/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml index dd0d84d2..c7acdb84 100644 --- a/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml @@ -13,4 +13,6 @@ test1/controller_manager: control_mode_handler: type: kuka_controllers/ControlModeHandler - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "test1_lbr_iisy3_r760" diff --git a/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml index 81fe5cb8..6c3eceac 100644 --- a/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml @@ -13,4 +13,6 @@ test2/controller_manager: control_mode_handler: type: kuka_controllers/ControlModeHandler - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "test2_lbr_iisy3_r760" diff --git a/kuka_iiqka_eac_driver/test/test_driver_activation.py b/kuka_iiqka_eac_driver/test/test_driver_activation.py index 63c44aeb..35da23bd 100644 --- a/kuka_iiqka_eac_driver/test/test_driver_activation.py +++ b/kuka_iiqka_eac_driver/test/test_driver_activation.py @@ -76,7 +76,9 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'lbr_iisy3_r760'", timeout=5 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) + proc_output.assertWaitFor( + "Setting component 'lbr_iisy3_r760' to 'unconfigured' state.", timeout=5 + ) # Check for successful configuration and activation proc_output.assertWaitFor( "Successful 'configure' of hardware 'lbr_iisy3_r760'", timeout=10 diff --git a/kuka_iiqka_eac_driver/test/test_driver_startup.py b/kuka_iiqka_eac_driver/test/test_driver_startup.py index c93dea6c..22c5ee3c 100644 --- a/kuka_iiqka_eac_driver/test/test_driver_startup.py +++ b/kuka_iiqka_eac_driver/test/test_driver_startup.py @@ -55,4 +55,6 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'lbr_iisy3_r760'", timeout=5 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) + proc_output.assertWaitFor( + "Setting component 'lbr_iisy3_r760' to 'unconfigured' state.", timeout=5 + ) diff --git a/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py b/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py index 66857010..c8220fdf 100644 --- a/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py +++ b/kuka_iiqka_eac_driver/test/test_multi_robot_startup.py @@ -84,4 +84,9 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'test2_lbr_iisy3_r760'", timeout=20 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=20) + proc_output.assertWaitFor( + "Setting component 'test1_lbr_iisy3_r760' to 'unconfigured' state.", timeout=20 + ) + proc_output.assertWaitFor( + "Setting component 'test2_lbr_iisy3_r760' to 'unconfigured' state.", timeout=20 + ) diff --git a/kuka_kss_rsi_driver/config/ros2_controller_config.yaml b/kuka_kss_rsi_driver/config/ros2_controller_config.yaml index ee137419..455f9123 100644 --- a/kuka_kss_rsi_driver/config/ros2_controller_config.yaml +++ b/kuka_kss_rsi_driver/config/ros2_controller_config.yaml @@ -8,4 +8,12 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "kr6_r700_sixx" + - "kr6_r900_sixx" + - "kr10_r1100_2" + - "kr16_r2010_2" + - "kr210-r2700_2" + - "kr210-r3100_2" + - "kr560_r3100_2" diff --git a/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml b/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml index dd0d84d2..673f31c9 100644 --- a/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml +++ b/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml @@ -13,4 +13,6 @@ test1/controller_manager: control_mode_handler: type: kuka_controllers/ControlModeHandler - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "test1_kr6_r700_sixx" diff --git a/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml b/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml index 81fe5cb8..72bfdded 100644 --- a/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml +++ b/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml @@ -13,4 +13,6 @@ test2/controller_manager: control_mode_handler: type: kuka_controllers/ControlModeHandler - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "test2_kr6_r700_sixx" diff --git a/kuka_kss_rsi_driver/test/test_driver_activation.py b/kuka_kss_rsi_driver/test/test_driver_activation.py index 4b780eed..78ffd9c7 100644 --- a/kuka_kss_rsi_driver/test/test_driver_activation.py +++ b/kuka_kss_rsi_driver/test/test_driver_activation.py @@ -82,7 +82,9 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'kr6_r700_sixx'", timeout=5 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) + proc_output.assertWaitFor( + "Setting component 'kr6_r700_sixx' to 'unconfigured' state.", timeout=5 + ) # Check for successful configuration and activation proc_output.assertWaitFor("Successful 'configure' of hardware 'kr6_r700_sixx'", timeout=10) proc_output.assertWaitFor("Successful 'activate' of hardware 'kr6_r700_sixx'", timeout=15) diff --git a/kuka_kss_rsi_driver/test/test_driver_startup.py b/kuka_kss_rsi_driver/test/test_driver_startup.py index e8e11752..045ef6da 100644 --- a/kuka_kss_rsi_driver/test/test_driver_startup.py +++ b/kuka_kss_rsi_driver/test/test_driver_startup.py @@ -55,4 +55,6 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'kr6_r700_sixx'", timeout=5 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) + proc_output.assertWaitFor( + "Setting component 'kr6_r700_sixx' to 'unconfigured' state.", timeout=5 + ) diff --git a/kuka_kss_rsi_driver/test/test_multi_robot_startup.py b/kuka_kss_rsi_driver/test/test_multi_robot_startup.py index 589bc843..da9e3ff1 100644 --- a/kuka_kss_rsi_driver/test/test_multi_robot_startup.py +++ b/kuka_kss_rsi_driver/test/test_multi_robot_startup.py @@ -80,4 +80,9 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'test2_kr6_r700_sixx'", timeout=20 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=20) + proc_output.assertWaitFor( + "Setting component 'test1_kr6_r700_sixx' to 'unconfigured' state.", timeout=20 + ) + proc_output.assertWaitFor( + "Setting component 'test2_kr6_r700_sixx' to 'unconfigured' state.", timeout=20 + ) diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index 89ae9fd9..a3ec1bb9 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -14,4 +14,6 @@ controller_manager: fri_state_broadcaster: type: kuka_controllers/FRIStateBroadcaster - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "lbr_iiwa14_r820" diff --git a/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml b/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml index dd0d84d2..b3e6d35e 100644 --- a/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml @@ -13,4 +13,6 @@ test1/controller_manager: control_mode_handler: type: kuka_controllers/ControlModeHandler - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "test1_lbr_iiwa14_r820" diff --git a/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml b/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml index 81fe5cb8..2a0104a4 100644 --- a/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml @@ -13,4 +13,6 @@ test2/controller_manager: control_mode_handler: type: kuka_controllers/ControlModeHandler - configure_components_on_start: [""] + hardware_components_initial_state: + unconfigured: + - "test2_lbr_iiwa14_r820" diff --git a/kuka_sunrise_fri_driver/test/test_driver_startup.py b/kuka_sunrise_fri_driver/test/test_driver_startup.py index 583faa20..02ee48bc 100644 --- a/kuka_sunrise_fri_driver/test/test_driver_startup.py +++ b/kuka_sunrise_fri_driver/test/test_driver_startup.py @@ -55,4 +55,6 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'lbr_iiwa14_r820'", timeout=5 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=5) + proc_output.assertWaitFor( + "Setting component 'lbr_iiwa14_r820' to 'unconfigured' state.", timeout=5 + ) diff --git a/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py b/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py index 494ac910..c9c5f84c 100644 --- a/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py +++ b/kuka_sunrise_fri_driver/test/test_multi_robot_startup.py @@ -80,4 +80,9 @@ def test_read_stdout(self, proc_output): "Successful initialization of hardware 'test2_lbr_iiwa14_r820'", timeout=20 ) # Check whether disabling automatic activation was successful - proc_output.assertWaitFor("Hardware Component with name '' does not exists", timeout=20) + proc_output.assertWaitFor( + "Setting component 'test1_lbr_iiwa14_r820' to 'unconfigured' state.", timeout=20 + ) + proc_output.assertWaitFor( + "Setting component 'test2_lbr_iiwa14_r820' to 'unconfigured' state.", timeout=20 + ) From cc8b29f3ead59f85e98fc1dabdae7bcb769a1a66 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 15:22:22 +0100 Subject: [PATCH 69/88] extend ld_lib_path --- .github/workflows/industrial_ci.yml | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index ed5ec047..74543296 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,6 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' + AFTER_BUILD_TARGET_WORKSPACE: "export LD_LIBRARY_PATH=~/.local/lib:$LD_LIBRARY_PATH" EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} @@ -61,23 +62,5 @@ jobs: path: ${{ env.CCACHE_DIR }} key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} - # Install and build kuka-external-control-sdk - - name: Install dependencies - run: sudo apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev - # - name: Clone sdk lib - # run: git clone https://github.com/kroshu/kuka-external-control-sdk.git - # working-directory: /home/runner/work - # - name: Create build directory - # run: mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build - # - name: Build and install sdk - # run: cmake .. && sudo make install - # working-directory: /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build - - # # Run industrial_ci - # - name: Extend LD_LIBRARY_PATH - # run: echo "LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib" >> $GITHUB_ENV - # - name: Debug - # run: ls -l /usr/local/lib && ls -l /usr/include/kuka/external-control-sdk/iiqka/ && echo $LD_LIBRARY_PATH - - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} From 6ef38d0dd6c78812e39b55da11d05d85d5960bad Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 15:40:40 +0100 Subject: [PATCH 70/88] fix --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 74543296..99bf9143 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' - AFTER_BUILD_TARGET_WORKSPACE: "export LD_LIBRARY_PATH=~/.local/lib:$LD_LIBRARY_PATH" + AFTER_BUILD_TARGET_WORKSPACE: "export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:~/.local/lib}"" EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From d7db184afccf9d402bb93f6c28908b2cc96aacf3 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 15:47:13 +0100 Subject: [PATCH 71/88] syntax --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 99bf9143..1090433b 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' - AFTER_BUILD_TARGET_WORKSPACE: "export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:~/.local/lib}"" + AFTER_BUILD_TARGET_WORKSPACE: 'export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:~/.local/lib}"' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From 9a00b07a9d86ef2e797cd8172df0118ddee33c2b Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 15:48:45 +0100 Subject: [PATCH 72/88] try extend again --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 1090433b..3851c7a6 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' - AFTER_BUILD_TARGET_WORKSPACE: 'export LD_LIBRARY_PATH="${LD_LIBRARY_PATH:~/.local/lib}"' + AFTER_BUILD_TARGET_WORKSPACE: 'export LD_LIBRARY_PATH="~/.local/lib:$LD_LIBRARY_PATH"' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From 35d83a856323d30219c3e74c30ecbab19f2f38e5 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 15:50:34 +0100 Subject: [PATCH 73/88] fix hack --- .github/workflows/industrial_ci.yml | 2 +- kuka_iiqka_eac_driver/CMakeLists.txt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 3851c7a6..9f59265d 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' - AFTER_BUILD_TARGET_WORKSPACE: 'export LD_LIBRARY_PATH="~/.local/lib:$LD_LIBRARY_PATH"' + #AFTER_BUILD_TARGET_WORKSPACE: 'export LD_LIBRARY_PATH="~/.local/lib:$LD_LIBRARY_PATH"' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index a40eb7ac..ad7e5c88 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -76,8 +76,8 @@ ament_export_libraries( ${PROJECT_NAME} ) -# Hack to extend $LD_LIBRARY_PATH with ~/.local/include when sourcing +# Hack to extend $LD_LIBRARY_PATH with ~/.local/lib when sourcing file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv - "prepend-non-duplicate;LD_LIBRARY_PATH;~/.local/include") + "prepend-non-duplicate;LD_LIBRARY_PATH;~/.local/lib") ament_package() From 3e59d66d8cd566fe253aa0a3566581af65ebe387 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 16:13:05 +0100 Subject: [PATCH 74/88] fix hack --- kuka_iiqka_eac_driver/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index ad7e5c88..2c7b5c7a 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -78,6 +78,6 @@ ament_export_libraries( # Hack to extend $LD_LIBRARY_PATH with ~/.local/lib when sourcing file(APPEND ${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_environment_hooks/library_path.dsv - "prepend-non-duplicate;LD_LIBRARY_PATH;~/.local/lib") + "prepend-non-duplicate;LD_LIBRARY_PATH;$ENV{HOME}/.local/lib") ament_package() From b78026a04d06b43d04659cc3c2a921aaae9b837d Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 16:21:29 +0100 Subject: [PATCH 75/88] debug --- .github/workflows/industrial_ci.yml | 2 +- .github/workflows/install_sdk.sh | 6 ------ 2 files changed, 1 insertion(+), 7 deletions(-) delete mode 100644 .github/workflows/install_sdk.sh diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 9f59265d..0f94836a 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' - #AFTER_BUILD_TARGET_WORKSPACE: 'export LD_LIBRARY_PATH="~/.local/lib:$LD_LIBRARY_PATH"' + AFTER_INIT: 'echo $LD_LIBRARY_PATH' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} diff --git a/.github/workflows/install_sdk.sh b/.github/workflows/install_sdk.sh deleted file mode 100644 index bd3c493b..00000000 --- a/.github/workflows/install_sdk.sh +++ /dev/null @@ -1,6 +0,0 @@ -cd /home/runner/work -git clone https://github.com/kroshu/kuka-external-control-sdk.git -mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build -cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build -cmake .. -make install From 47584471a7ab8189986f2689617fca5f03d0fe52 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Fri, 23 Feb 2024 16:31:12 +0100 Subject: [PATCH 76/88] debug2 --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 0f94836a..60ef51a2 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,7 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' - AFTER_INIT: 'echo $LD_LIBRARY_PATH' + AFTER_INIT: 'rosenv && echo $LD_LIBRARY_PATH' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From 7cddbe38302f673a70af7dabe4685f0caac100fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=81ron=20Svastits?= <49677296+Svastits@users.noreply.github.com> Date: Fri, 23 Feb 2024 22:04:28 +0100 Subject: [PATCH 77/88] Update industrial_ci.yml --- .github/workflows/industrial_ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 60ef51a2..667d9345 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -51,6 +51,7 @@ jobs: PR_NUMBER: ${{ github.event.number }} ANALYZER_TOKEN: ${{ secrets.ANALYZER_TOKEN }} DEBUG_BASH: true + DOCKER_RUN_OPTS: '-e LD_LIBRARY_PATH=~/.local/lib' runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 From ad7f7dbc560b0015ffb16f974ba34c0623459ea3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=81ron=20Svastits?= <49677296+Svastits@users.noreply.github.com> Date: Fri, 23 Feb 2024 22:13:18 +0100 Subject: [PATCH 78/88] Remove after init --- .github/workflows/industrial_ci.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 667d9345..db4650d7 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -43,7 +43,6 @@ jobs: env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) BEFORE_BUILD_TARGET_WORKSPACE: 'apt update && apt install -y cmake build-essential pkg-config libssl-dev protobuf-compiler-grpc libgrpc++-dev && cd /home/runner/work && git clone https://github.com/kroshu/kuka-external-control-sdk.git && mkdir -p /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cd /home/runner/work/kuka-external-control-sdk/kuka-external-control-sdk/build && cmake .. && make install' - AFTER_INIT: 'rosenv && echo $LD_LIBRARY_PATH' EVENT_NAME: ${{ github.event_name }} BRANCH: ${{ github.event.ref }} PR_BRANCH: ${{ github.event.pull_request.head.ref }} From b1c7d61004e88a1865c2460e1d4c0e15c89700ac Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 26 Feb 2024 07:16:26 +0100 Subject: [PATCH 79/88] debug --- kuka_iiqka_eac_driver/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 2c7b5c7a..225f5678 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -64,6 +64,8 @@ install(TARGETS ${PROJECT_NAME} robot_manager_node install(DIRECTORY config launch test DESTINATION share/${PROJECT_NAME}) +message("The value of the environment variable LD_LIBRARY_PATH: $ENV{LD_LIBRARY_PATH}") + if(BUILD_TESTING) find_package(launch_testing_ament_cmake) add_launch_test(test/test_driver_startup.py) From c151645b286dbabe73169629b3c9edf5ceb09921 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 26 Feb 2024 07:35:59 +0100 Subject: [PATCH 80/88] fix env --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index db4650d7..d0549a07 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -50,7 +50,7 @@ jobs: PR_NUMBER: ${{ github.event.number }} ANALYZER_TOKEN: ${{ secrets.ANALYZER_TOKEN }} DEBUG_BASH: true - DOCKER_RUN_OPTS: '-e LD_LIBRARY_PATH=~/.local/lib' + DOCKER_RUN_OPTS: '-e LD_LIBRARY_PATH=/root/.local/lib' runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 From 9f698f7e3a13d2412997ecbe8973b3fcf3603035 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 26 Feb 2024 10:26:01 +0100 Subject: [PATCH 81/88] cleanup --- .github/workflows/industrial_ci.yml | 1 + README.md | 2 +- kuka_iiqka_eac_driver/CMakeLists.txt | 2 -- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index d0549a07..1f6f3193 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -62,5 +62,6 @@ jobs: path: ${{ env.CCACHE_DIR }} key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} + # Run industrial_ci - uses: 'kroshu/industrial_ci@master' env: ${{ matrix.env }} diff --git a/README.md b/README.md index a0c54f17..979d59d6 100644 --- a/README.md +++ b/README.md @@ -50,7 +50,7 @@ git clone https://github.com/kroshu/kuka-external-control-sdk.git mkdir -p ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build cd ~/sdk_ws/src/kuka-external-control-sdk/kuka-external-control-sdk/build cmake .. -sudo make install +make install ``` Build KUKA packages. diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 225f5678..2c7b5c7a 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -64,8 +64,6 @@ install(TARGETS ${PROJECT_NAME} robot_manager_node install(DIRECTORY config launch test DESTINATION share/${PROJECT_NAME}) -message("The value of the environment variable LD_LIBRARY_PATH: $ENV{LD_LIBRARY_PATH}") - if(BUILD_TESTING) find_package(launch_testing_ament_cmake) add_launch_test(test/test_driver_startup.py) From 159934a7e635d975ebbbe657acd39962d95193c9 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 26 Feb 2024 10:57:54 +0100 Subject: [PATCH 82/88] better driver init --- doc/wiki/KSS_RSI.md | 2 +- doc/wiki/Sunrise_FRI.md | 2 +- doc/wiki/iiQKA_EAC.md | 2 +- .../config/ros2_controller_config.yaml | 6 ------ kuka_iiqka_eac_driver/launch/startup.launch.py | 13 ++++++++++++- .../test/config/test1_ros2_controller_config.yaml | 4 ---- .../test/config/test2_ros2_controller_config.yaml | 4 ---- .../config/ros2_controller_config.yaml | 10 ---------- kuka_kss_rsi_driver/launch/startup.launch.py | 11 ++++++++++- .../test/config/test1_ros2_controller_config.yaml | 4 ---- .../test/config/test2_ros2_controller_config.yaml | 4 ---- .../config/ros2_controller_config.yaml | 4 ---- kuka_sunrise_fri_driver/launch/startup.launch.py | 11 ++++++++++- .../test/config/test1_ros2_controller_config.yaml | 4 ---- .../test/config/test2_ros2_controller_config.yaml | 4 ---- 15 files changed, 35 insertions(+), 50 deletions(-) diff --git a/doc/wiki/KSS_RSI.md b/doc/wiki/KSS_RSI.md index 4c50e346..edfb29eb 100644 --- a/doc/wiki/KSS_RSI.md +++ b/doc/wiki/KSS_RSI.md @@ -65,7 +65,7 @@ Method 2: The following configuration files are available in the `config` directory of the package: - `driver_config.yaml`: contains the IP address of the client machine -- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup. +- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. - configuration files for specific controllers, for further information, see the documentation of the given controller ##### IP configuration diff --git a/doc/wiki/Sunrise_FRI.md b/doc/wiki/Sunrise_FRI.md index 6acc73bc..b830916a 100644 --- a/doc/wiki/Sunrise_FRI.md +++ b/doc/wiki/Sunrise_FRI.md @@ -17,7 +17,7 @@ The following configuration files are available in the `config` directory of the package: - `driver_config.yaml`: : contains runtime parameters of the `robot_manager` node -- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup. +- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. - configuration files for specific controllers, for further information, see the documentation of the given controller - `gpio_config.xacro`: contains the I/O setup of the system, but this was not tested yet diff --git a/doc/wiki/iiQKA_EAC.md b/doc/wiki/iiQKA_EAC.md index efb03bc0..b227fe5f 100644 --- a/doc/wiki/iiQKA_EAC.md +++ b/doc/wiki/iiQKA_EAC.md @@ -22,7 +22,7 @@ The following configuration files are available in the `config` directory of the package: - `driver_config.yaml`: contains runtime parameters of the `robot_manager` node - `qos_config.yaml`: contains the configuration options for the QoS profile defining the connection quality (description later) -- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. The `configure_components_on_start` parameter should never be modified, which ensures that the hardware interface is not activated at startup. +- `ros2_controller_config.yaml`: contains the controller types for every controller name. Should be only modified if a different controller is to be used. - configuration files for specific controllers (for further information, see the documentation of the given controller) ##### QoS profile configuration diff --git a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml index 34492df5..b4f8f503 100644 --- a/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/ros2_controller_config.yaml @@ -14,9 +14,3 @@ controller_manager: type: kuka_controllers/ControlModeHandler event_broadcaster: type: kuka_controllers/EventBroadcaster - - hardware_components_initial_state: - unconfigured: - - "lbr_iisy3_r760" - - "lbr_iisy11_r1300" - - "lbr_iisy15_r930" diff --git a/kuka_iiqka_eac_driver/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py index c53af839..d87934c9 100644 --- a/kuka_iiqka_eac_driver/launch/startup.launch.py +++ b/kuka_iiqka_eac_driver/launch/startup.launch.py @@ -106,7 +106,18 @@ def launch_setup(context, *args, **kwargs): namespace=ns, package="kuka_drivers_core", executable="control_node", - parameters=[robot_description, controller_config, jtc_config, jic_config, ec_config], + parameters=[ + robot_description, + controller_config, + jtc_config, + jic_config, + ec_config, + { + "hardware_components_initial_state": { + "unconfigured": [tf_prefix + robot_model.perform(context)] + }, + }, + ], ) robot_manager_node = LifecycleNode( name=["robot_manager"], diff --git a/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml index c7acdb84..7383c386 100644 --- a/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test1_ros2_controller_config.yaml @@ -12,7 +12,3 @@ test1/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler - - hardware_components_initial_state: - unconfigured: - - "test1_lbr_iisy3_r760" diff --git a/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml b/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml index 6c3eceac..0a8547e6 100644 --- a/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml +++ b/kuka_iiqka_eac_driver/test/config/test2_ros2_controller_config.yaml @@ -12,7 +12,3 @@ test2/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler - - hardware_components_initial_state: - unconfigured: - - "test2_lbr_iisy3_r760" diff --git a/kuka_kss_rsi_driver/config/ros2_controller_config.yaml b/kuka_kss_rsi_driver/config/ros2_controller_config.yaml index 455f9123..a8cbf9c7 100644 --- a/kuka_kss_rsi_driver/config/ros2_controller_config.yaml +++ b/kuka_kss_rsi_driver/config/ros2_controller_config.yaml @@ -7,13 +7,3 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - - hardware_components_initial_state: - unconfigured: - - "kr6_r700_sixx" - - "kr6_r900_sixx" - - "kr10_r1100_2" - - "kr16_r2010_2" - - "kr210-r2700_2" - - "kr210-r3100_2" - - "kr560_r3100_2" diff --git a/kuka_kss_rsi_driver/launch/startup.launch.py b/kuka_kss_rsi_driver/launch/startup.launch.py index fd1e93e1..30be4b86 100644 --- a/kuka_kss_rsi_driver/launch/startup.launch.py +++ b/kuka_kss_rsi_driver/launch/startup.launch.py @@ -97,7 +97,16 @@ def launch_setup(context, *args, **kwargs): namespace=ns, package="kuka_drivers_core", executable="control_node", - parameters=[robot_description, controller_config, jtc_config], + parameters=[ + robot_description, + controller_config, + jtc_config, + { + "hardware_components_initial_state": { + "unconfigured": [tf_prefix + robot_model.perform(context)] + }, + }, + ], ) robot_manager_node = LifecycleNode( name=["robot_manager"], diff --git a/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml b/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml index 673f31c9..7383c386 100644 --- a/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml +++ b/kuka_kss_rsi_driver/test/config/test1_ros2_controller_config.yaml @@ -12,7 +12,3 @@ test1/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler - - hardware_components_initial_state: - unconfigured: - - "test1_kr6_r700_sixx" diff --git a/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml b/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml index 72bfdded..0a8547e6 100644 --- a/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml +++ b/kuka_kss_rsi_driver/test/config/test2_ros2_controller_config.yaml @@ -12,7 +12,3 @@ test2/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler - - hardware_components_initial_state: - unconfigured: - - "test2_kr6_r700_sixx" diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index a3ec1bb9..9948d169 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -13,7 +13,3 @@ controller_manager: fri_state_broadcaster: type: kuka_controllers/FRIStateBroadcaster - - hardware_components_initial_state: - unconfigured: - - "lbr_iiwa14_r820" diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index e62994d1..e515ff35 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -93,7 +93,16 @@ def launch_setup(context, *args, **kwargs): namespace=ns, package="kuka_drivers_core", executable="control_node", - parameters=[robot_description, controller_config, jtc_config], + parameters=[ + robot_description, + controller_config, + jtc_config, + { + "hardware_components_initial_state": { + "unconfigured": [tf_prefix + robot_model.perform(context)] + }, + }, + ], ) robot_manager_node = LifecycleNode( name=["robot_manager"], diff --git a/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml b/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml index b3e6d35e..7383c386 100644 --- a/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/test/config/test1_ros2_controller_config.yaml @@ -12,7 +12,3 @@ test1/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler - - hardware_components_initial_state: - unconfigured: - - "test1_lbr_iiwa14_r820" diff --git a/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml b/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml index 2a0104a4..0a8547e6 100644 --- a/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/test/config/test2_ros2_controller_config.yaml @@ -12,7 +12,3 @@ test2/controller_manager: type: effort_controllers/JointGroupPositionController control_mode_handler: type: kuka_controllers/ControlModeHandler - - hardware_components_initial_state: - unconfigured: - - "test2_lbr_iiwa14_r820" From 0c2aac30cb11348ac00a5c9081b27fdf2fc8236e Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 26 Feb 2024 11:01:42 +0100 Subject: [PATCH 83/88] extend doc with torque difference --- doc/wiki/iiQKA_EAC.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/wiki/iiQKA_EAC.md b/doc/wiki/iiQKA_EAC.md index b227fe5f..e346246a 100644 --- a/doc/wiki/iiQKA_EAC.md +++ b/doc/wiki/iiQKA_EAC.md @@ -65,6 +65,8 @@ After successful startup, the `robot_manager` node has to be activated to start On successful activation the brakes of the robot will be released and external control is started using the requested control mode. To test moving the robot, the `rqt_joint_trajectory_controller` is not recommended, use the launch file in the `iiqka_moveit_example` package instead (usage is described in the *Additional packages* section of the [project overview](Project%20overview.md)). +It is important to note, that the commanded and measures torques have a different meaning: the `effort` command interface accepts values that should be superimposed on internal gravity compensation, while the state interface provides the actually measured torques (sum on internal and external effects). + ##### Launch arguments From 9a3b0a86dcd192063603b85d86f2556855cd9dcd Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 26 Feb 2024 11:08:37 +0100 Subject: [PATCH 84/88] remove mock from doc --- doc/wiki/iiQKA_EAC.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/doc/wiki/iiQKA_EAC.md b/doc/wiki/iiQKA_EAC.md index e346246a..b5a7dc9a 100644 --- a/doc/wiki/iiQKA_EAC.md +++ b/doc/wiki/iiQKA_EAC.md @@ -4,8 +4,6 @@ #### Client side - It is recommended to use the driver on a real-time capable client machine (further information about setting up the PREEMPT_RT patch can be found [here](https://github.com/kroshu/kuka_drivers/wiki/Realtime)). -- The driver depends on some KUKA-specific packages, which are only available with the real robot, therefore a mock mode is provided to enable trying out solutions with the same components running. - - By default, the mock libraries are used, this can be changed in the `CmakeLists.txt` file by setting `MOCK_KUKA_LIBS` to `FALSE` before building. - Set a fixed IP in the subnet of the KONI interface for the real-time machine. #### Controller side From a53fdfea3291ab88e8848dc02b46e2b9b4a22327 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 26 Feb 2024 11:29:23 +0100 Subject: [PATCH 85/88] cleanup --- .../robot_manager_node.hpp | 2 +- .../src/robot_manager_node.cpp | 37 ++++++++----------- 2 files changed, 17 insertions(+), 22 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp index c2f3abab..0f7bccd4 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/robot_manager_node.hpp @@ -71,7 +71,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::condition_variable control_mode_cv_; std::mutex control_mode_cv_m_; - bool control_mode_change_finished_; + bool control_mode_change_finished_ = false; rclcpp::Publisher::SharedPtr control_mode_pub_; rclcpp::Publisher::SharedPtr is_configured_pub_; diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 78238a77..88d92af1 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -27,11 +27,7 @@ using namespace lifecycle_msgs::msg; // NOLINT namespace kuka_eac { -// TODO(Komaromi): Re-add "control_mode_handler" controller to controller_handlers constructor -// after controller handler properly implemented with working initial control mode change -RobotManagerNode::RobotManagerNode() -: kuka_drivers_core::ROS2BaseLCNode("robot_manager"), control_mode_change_finished_(false) - +RobotManagerNode::RobotManagerNode() : kuka_drivers_core::ROS2BaseLCNode("robot_manager") { auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); qos.reliable(); @@ -84,7 +80,6 @@ RobotManagerNode::RobotManagerNode() return this->controller_handler_.UpdateControllerName( kuka_drivers_core::ControllerType::TORQUE_CONTROLLER_TYPE, controller_name); }); - this->registerParameter( "control_mode", static_cast(kuka_drivers_core::ControlMode::JOINT_POSITION_CONTROL), kuka_drivers_core::ParameterSetAccessRights{true, true, true, false, false}, @@ -116,20 +111,23 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) return FAILURE; } + // Publish message about HWIF configuration + // TODO(Svastits): this can be removed in the future, if all drivers do simple blocking waits in + // read with msg_received_ flag is_configured_msg_.data = true; is_configured_pub_->publish(is_configured_msg_); - // Activate control mode handler + // Activate control mode handler and event broadcaster if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {"control_mode_handler", "event_broadcaster"}, {})) { - RCLCPP_ERROR(get_logger(), "Could not activate control mode handler"); - // TODO(Svastits): this can be removed if rollback is implemented properly + RCLCPP_ERROR(get_logger(), "Could not activate control mode handler or event broadcaster"); + // Rollback this->on_cleanup(get_current_state()); return FAILURE; } - RCLCPP_INFO(get_logger(), "Activated control mode handler"); + RCLCPP_INFO(get_logger(), "Activated control mode handler and event broadcaster"); return SUCCESS; } @@ -137,11 +135,11 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) { - // Deactivate control mode handler + // Deactivate control mode handler and event broadcaster if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {}, {"control_mode_handler", "event_broadcaster"})) { - RCLCPP_ERROR(get_logger(), "Could not deactivate control mode handler"); + RCLCPP_ERROR(get_logger(), "Could not deactivate control mode handler and event broadcaster"); } // Clean up hardware interface @@ -182,7 +180,7 @@ void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::Sha } else if (this->get_current_state().id() == State::TRANSITION_STATE_ACTIVATING) { - // TODO(Svastits): this can be removed if rollback is implemented properly + // Handle case, when error is received while still activating this->on_deactivate(get_current_state()); } break; @@ -277,16 +275,12 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) return false; } - bool is_active_state = - get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; - // Publish the control mode to controller handler auto message = std_msgs::msg::UInt32(); message.data = control_mode; control_mode_pub_->publish(message); - RCLCPP_INFO(get_logger(), "Control mode change process has started"); - if (is_active_state) + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { // Deactivate controllers of previous control mode if (!kuka_drivers_core::changeControllerState( @@ -294,7 +288,7 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) controller_handler_.GetControllersForMode(control_mode_))) { RCLCPP_ERROR(get_logger(), "Could not deactivate controllers of previous control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly + // Deactivate in case of an error this->on_deactivate(get_current_state()); return false; } @@ -325,14 +319,15 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode) {})) { RCLCPP_ERROR(get_logger(), "Could not activate controllers for new control mode"); - // TODO(Svastits): this can be removed if rollback is implemented properly + // Deactivate in case of an error this->on_deactivate(get_current_state()); return false; } } - RCLCPP_INFO(get_logger(), "Successfully changed control mode"); control_mode_ = static_cast(control_mode); + RCLCPP_INFO(get_logger(), "Successfully changed control mode to %i", control_mode); + return true; } From 978bd6648386631ad33dbe2d2f82dea9784fe96f Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 28 Feb 2024 08:52:57 +0100 Subject: [PATCH 86/88] fix log --- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index 88d92af1..cb84c29d 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -224,7 +224,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Return failure if control is stopped while in state activating if (terminate_) { - RCLCPP_ERROR(get_logger(), "UDP communication could not be set up"); + RCLCPP_ERROR(get_logger(), "Error occured during driver activation"); return FAILURE; } From 4c9dc07956e6011d0af62a355c0278ac5cd8ccef Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 28 Feb 2024 08:54:25 +0100 Subject: [PATCH 87/88] spell --- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index cb84c29d..d3210210 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -224,7 +224,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Return failure if control is stopped while in state activating if (terminate_) { - RCLCPP_ERROR(get_logger(), "Error occured during driver activation"); + RCLCPP_ERROR(get_logger(), "Error occurred during driver activation"); return FAILURE; } From 223744e23362396eac77f8dd129a92ae3288c06c Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 28 Feb 2024 11:06:01 +0100 Subject: [PATCH 88/88] typo --- kuka_iiqka_eac_driver/src/robot_manager_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp index d3210210..dbe16e5b 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -162,7 +162,7 @@ void RobotManagerNode::EventSubscriptionCallback(const std_msgs::msg::UInt8::Sha case kuka_drivers_core::HardwareEvent::CONTROL_STARTED: { RCLCPP_INFO(get_logger(), "External control started"); - // Nofify lock after control mode change + // Notify lock after control mode change { std::lock_guard lk(control_mode_cv_m_); control_mode_change_finished_ = true;