From 4795907b06708b13200ecb0dd98a5b0c95576e21 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 13 Nov 2023 13:54:46 +0100 Subject: [PATCH 1/9] rox -> eac --- kuka_iiqka_eac_driver/hardware_interface.xml | 2 +- .../hardware_interface.hpp | 8 +- .../robot_manager_node.hpp | 4 +- .../src/hardware_interface.cpp | 74 +++++++++---------- .../src/robot_manager_node.cpp | 6 +- .../hardware_interface.xml | 2 +- .../hardware_interface.hpp | 4 +- .../src/hardware_interface.cpp | 58 +++++++-------- 8 files changed, 79 insertions(+), 79 deletions(-) diff --git a/kuka_iiqka_eac_driver/hardware_interface.xml b/kuka_iiqka_eac_driver/hardware_interface.xml index e94d3c0d..20d191da 100644 --- a/kuka_iiqka_eac_driver/hardware_interface.xml +++ b/kuka_iiqka_eac_driver/hardware_interface.xml @@ -1,6 +1,6 @@ - ROS2 control Kuka driver for RoX 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 12af060f..6a131aa7 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 @@ -44,13 +44,13 @@ using hardware_interface::return_type; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -namespace kuka_rox +namespace kuka_eac { -class KukaRoXHardwareInterface : public hardware_interface::SystemInterface +class KukaEACHardwareInterface : public hardware_interface::SystemInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(KukaRoXHardwareInterface) + RCLCPP_SHARED_PTR_DEFINITIONS(KukaEACHardwareInterface) CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; @@ -108,6 +108,6 @@ class KukaRoXHardwareInterface : public hardware_interface::SystemInterface static constexpr char HW_IF_DAMPING[] = "damping"; static constexpr char CONTROL_MODE[] = "control_mode"; }; -} // namespace kuka_rox +} // namespace kuka_eac #endif // KUKA_IIQKA_EAC_DRIVER__HARDWARE_INTERFACE_HPP_ 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 702098fa..75c5b2d2 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 @@ -35,7 +35,7 @@ #include "kuka/ecs/v1/motion_services_ecs.grpc.pb.h" -namespace kuka_rox +namespace kuka_eac { class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode { @@ -94,7 +94,7 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode static constexpr int IMPEDANCE_MODE_IF_SIZE = 2; }; -} // namespace kuka_rox +} // namespace kuka_eac #endif // KUKA_IIQKA_EAC_DRIVER__ROBOT_MANAGER_NODE_HPP_ diff --git a/kuka_iiqka_eac_driver/src/hardware_interface.cpp b/kuka_iiqka_eac_driver/src/hardware_interface.cpp index ff5288b4..023baad8 100644 --- a/kuka_iiqka_eac_driver/src/hardware_interface.cpp +++ b/kuka_iiqka_eac_driver/src/hardware_interface.cpp @@ -26,9 +26,9 @@ using namespace kuka::ecs::v1; // NOLINT using os::core::udp::communication::Socket; -namespace kuka_rox +namespace kuka_eac { -CallbackReturn KukaRoXHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) +CallbackReturn KukaEACHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -66,7 +66,7 @@ CallbackReturn KukaRoXHardwareInterface::on_init(const hardware_interface::Hardw 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("KukaRoXHardwareInterface"), "Could not setup udp replier"); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Could not setup udp replier"); return CallbackReturn::FAILURE; } #else @@ -76,15 +76,15 @@ CallbackReturn KukaRoXHardwareInterface::on_init(const hardware_interface::Hardw hw_position_commands_[4] = 90 * (M_PI / 180); #endif - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Init successful"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Init successful"); return CallbackReturn::SUCCESS; } std::vector -KukaRoXHardwareInterface::export_state_interfaces() +KukaEACHardwareInterface::export_state_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Export state interfaces"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Export state interfaces"); std::vector state_interfaces; for (size_t i = 0; i < info_.joints.size(); i++) { state_interfaces.emplace_back( @@ -100,10 +100,10 @@ KukaRoXHardwareInterface::export_state_interfaces() return state_interfaces; } -std::vector KukaRoXHardwareInterface:: +std::vector KukaEACHardwareInterface:: export_command_interfaces() { - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Export command interfaces"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Export command interfaces"); std::vector command_interfaces; for (size_t i = 0; i < info_.joints.size(); i++) { @@ -136,7 +136,7 @@ export_command_interfaces() return command_interfaces; } -CallbackReturn KukaRoXHardwareInterface::on_configure(const rclcpp_lifecycle::State &) +CallbackReturn KukaEACHardwareInterface::on_configure(const rclcpp_lifecycle::State &) { #ifdef NON_MOCK_SETUP SetQoSProfileRequest request; @@ -158,12 +158,12 @@ CallbackReturn KukaRoXHardwareInterface::on_configure(const rclcpp_lifecycle::St "timeframe_ms"))); if (stub_->SetQoSProfile(&context, request, &response).error_code() != grpc::StatusCode::OK) { - RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), "SetQoSProfile failed"); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "SetQoSProfile failed"); return CallbackReturn::FAILURE; } RCLCPP_INFO( - rclcpp::get_logger("KukaRoXHardwareInterface"), + 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(), @@ -173,9 +173,9 @@ CallbackReturn KukaRoXHardwareInterface::on_configure(const rclcpp_lifecycle::St } -CallbackReturn KukaRoXHardwareInterface::on_activate(const rclcpp_lifecycle::State &) +CallbackReturn KukaEACHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Connecting to robot . . ."); + 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; @@ -189,7 +189,7 @@ CallbackReturn KukaRoXHardwareInterface::on_activate(const rclcpp_lifecycle::Sta } #ifdef NON_MOCK_SETUP - observe_thread_ = std::thread(&KukaRoXHardwareInterface::ObserveControl, this); + observe_thread_ = std::thread(&KukaEACHardwareInterface::ObserveControl, this); OpenControlChannelRequest request; OpenControlChannelResponse response; @@ -201,13 +201,13 @@ CallbackReturn KukaRoXHardwareInterface::on_activate(const rclcpp_lifecycle::Sta request.set_external_control_mode( kuka::motion::external::ExternalControlMode(hw_control_mode_command_)); RCLCPP_INFO( - rclcpp::get_logger("KukaRoXHardwareInterface"), "Starting control in %s", + 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) { - RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), "%s", ret.error_message().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "%s", ret.error_message().c_str()); return CallbackReturn::FAILURE; } #endif @@ -215,10 +215,10 @@ CallbackReturn KukaRoXHardwareInterface::on_activate(const rclcpp_lifecycle::Sta return CallbackReturn::SUCCESS; } -CallbackReturn KukaRoXHardwareInterface::on_deactivate( +CallbackReturn KukaEACHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Deactivating"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Deactivating"); control_signal_ext_.control_signal.stop_ipo = true; while (is_active_) { @@ -236,7 +236,7 @@ CallbackReturn KukaRoXHardwareInterface::on_deactivate( return CallbackReturn::SUCCESS; } -return_type KukaRoXHardwareInterface::read( +return_type KukaEACHardwareInterface::read( const rclcpp::Time &, const rclcpp::Duration &) { @@ -263,7 +263,7 @@ return_type KukaRoXHardwareInterface::read( if (!nanopb::Decode( req_message.first, req_message.second, motion_state_external_)) { - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Decoding request failed"); + 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; @@ -278,21 +278,21 @@ return_type KukaRoXHardwareInterface::read( } if (motion_state_external_.motion_state.ipo_stopped) { - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Motion stopped"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Motion stopped"); } msg_received_ = true; receive_timeout_ = std::chrono::milliseconds(6); } else { - RCLCPP_WARN(rclcpp::get_logger("KukaRoXHardwareInterface"), "Request was missed"); + RCLCPP_WARN(rclcpp::get_logger("KukaEACHardwareInterface"), "Request was missed"); RCLCPP_WARN( - rclcpp::get_logger("KukaRoXHardwareInterface"), + rclcpp::get_logger("KukaEACHardwareInterface"), "Previous ipoc: %i", motion_state_external_.header.ipoc); msg_received_ = false; } return return_type::OK; } -return_type KukaRoXHardwareInterface::write( +return_type KukaEACHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration &) { @@ -323,7 +323,7 @@ return_type KukaRoXHardwareInterface::write( if (encoded_bytes < 0) { RCLCPP_ERROR( rclcpp::get_logger( - "KukaRoXHardwareInterface"), + "KukaEACHardwareInterface"), "Encoding of control signal to out_buffer failed."); throw std::runtime_error("Encoding of control signal to out_buffer failed."); } @@ -331,16 +331,16 @@ return_type KukaRoXHardwareInterface::write( if (udp_replier_->SendReply(out_buff_arr_, encoded_bytes) != Socket::ErrorCode::kSuccess) { - RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), "Error sending reply"); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "Error sending reply"); throw std::runtime_error("Error sending reply"); } return return_type::OK; } -void KukaRoXHardwareInterface::ObserveControl() +void KukaEACHardwareInterface::ObserveControl() { #ifdef NON_MOCK_SETUP - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Observe control"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Observe control"); context_ = std::make_unique<::grpc::ClientContext>(); ObserveControlStateRequest obs_control; std::unique_ptr> reader( @@ -352,34 +352,34 @@ void KukaRoXHardwareInterface::ObserveControl() command_state_ = response; RCLCPP_INFO( rclcpp::get_logger( - "KukaRoXHardwareInterface"), + "KukaEACHardwareInterface"), "Event streamed from external control service: %s", CommandEvent_Name( response.event()).c_str()); switch (static_cast(response.event())) { case CommandEvent::COMMAND_READY: - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Command accepted"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "Command accepted"); break; case CommandEvent::SAMPLING: - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "External control is active"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control is active"); is_active_ = true; break; case CommandEvent::CONTROL_MODE_SWITCH: RCLCPP_INFO( rclcpp::get_logger( - "KukaRoXHardwareInterface"), "Control mode switch is in progress"); + "KukaEACHardwareInterface"), "Control mode switch is in progress"); is_active_ = false; break; case CommandEvent::STOPPED: - RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "External control finished"); + RCLCPP_INFO(rclcpp::get_logger("KukaEACHardwareInterface"), "External control finished"); is_active_ = false; break; case CommandEvent::ERROR: RCLCPP_ERROR( rclcpp::get_logger( - "KukaRoXHardwareInterface"), + "KukaEACHardwareInterface"), "External control stopped by an error"); - RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), response.message().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), response.message().c_str()); is_active_ = false; break; default: @@ -389,8 +389,8 @@ void KukaRoXHardwareInterface::ObserveControl() #endif } -} // namespace namespace kuka_rox +} // namespace namespace kuka_eac PLUGINLIB_EXPORT_CLASS( - kuka_rox::KukaRoXHardwareInterface, + 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 69992f99..5683b304 100644 --- a/kuka_iiqka_eac_driver/src/robot_manager_node.cpp +++ b/kuka_iiqka_eac_driver/src/robot_manager_node.cpp @@ -21,7 +21,7 @@ using namespace controller_manager_msgs::srv; // NOLINT using namespace lifecycle_msgs::msg; // NOLINT using namespace kuka::motion::external; // NOLINT -namespace kuka_rox +namespace kuka_eac { // TODO(Komaromi): Readd "control_mode_handler" controller to controller_handlers constrctor // after controller handler poperly implemented with working initial control mode change @@ -501,7 +501,7 @@ bool RobotManagerNode::onRobotModelChangeRequest(const std::string & robot_model robot_model_ = robot_model; return true; } -} // namespace kuka_rox +} // namespace kuka_eac int main(int argc, char * argv[]) { @@ -509,7 +509,7 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); + auto node = std::make_shared(); executor.add_node(node->get_node_base_interface()); executor.spin(); rclcpp::shutdown(); diff --git a/kuka_sunrise_fri_driver/hardware_interface.xml b/kuka_sunrise_fri_driver/hardware_interface.xml index 4cd6afa3..23ecbb29 100644 --- a/kuka_sunrise_fri_driver/hardware_interface.xml +++ b/kuka_sunrise_fri_driver/hardware_interface.xml @@ -1,6 +1,6 @@ - ROS2 control hardware interface for KUKA LBR robots using the Fast Robot Interface (FRI). diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index daae9508..0fd7985a 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -51,11 +51,11 @@ static std::unordered_map const types = {{"analog", IOTypes::ANALOG}, {"digital", IOTypes::DIGITAL}, {"boolean", IOTypes::BOOLEAN}}; -class KUKAFRIHardwareInterface : public hardware_interface::SystemInterface, +class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, public KUKA::FRI::LBRClient { public: - KUKAFRIHardwareInterface() + KukaFRIHardwareInterface() : client_application_(udp_connection_, *this) {} CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 3de66934..fab1f84e 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -18,7 +18,7 @@ namespace kuka_sunrise_fri_driver { -CallbackReturn KUKAFRIHardwareInterface::on_init( +CallbackReturn KukaFRIHardwareInterface::on_init( const hardware_interface::HardwareInfo & system_info) { if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { @@ -32,7 +32,7 @@ CallbackReturn KUKAFRIHardwareInterface::on_init( if (info_.gpios.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("KUKAFRIHardwareInterface"), + rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 1 GPIO"); return CallbackReturn::ERROR; } @@ -41,7 +41,7 @@ CallbackReturn KUKAFRIHardwareInterface::on_init( info_.gpios[0].state_interfaces.size() > 10) { RCLCPP_FATAL( - rclcpp::get_logger("KUKAFRIHardwareInterface"), + rclcpp::get_logger("KukaFRIHardwareInterface"), "A maximum of 10 inputs and outputs can be registered to FRI"); return CallbackReturn::ERROR; } @@ -60,7 +60,7 @@ CallbackReturn KUKAFRIHardwareInterface::on_init( for (const hardware_interface::ComponentInfo & joint : info_.joints) { if (joint.command_interfaces.size() != 2) { RCLCPP_FATAL( - rclcpp::get_logger("KUKAFRIHardwareInterface"), + rclcpp::get_logger("KukaFRIHardwareInterface"), "expecting exactly 2 command interface"); return CallbackReturn::ERROR; } @@ -68,42 +68,42 @@ CallbackReturn KUKAFRIHardwareInterface::on_init( if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "expecting POSITION command interface as first"); + "KukaFRIHardwareInterface"), "expecting POSITION command interface as first"); return CallbackReturn::ERROR; } if (joint.command_interfaces[1].name != hardware_interface::HW_IF_EFFORT) { RCLCPP_FATAL( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "expecting EFFORT command interface as second"); + "KukaFRIHardwareInterface"), "expecting EFFORT command interface as second"); return CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 3) { RCLCPP_FATAL( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "expecting exactly 3 state interface"); + "KukaFRIHardwareInterface"), "expecting exactly 3 state interface"); return CallbackReturn::ERROR; } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "expecting POSITION state interface as first"); + "KukaFRIHardwareInterface"), "expecting POSITION state interface as first"); return CallbackReturn::ERROR; } if (joint.state_interfaces[1].name != hardware_interface::HW_IF_EFFORT) { RCLCPP_FATAL( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "expecting EFFORT state interface as second"); + "KukaFRIHardwareInterface"), "expecting EFFORT state interface as second"); return CallbackReturn::ERROR; } if (joint.state_interfaces[2].name != "external_torque") { RCLCPP_FATAL( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "expecting 'external torque' state interface as third"); + "KukaFRIHardwareInterface"), "expecting 'external torque' state interface as third"); return CallbackReturn::ERROR; } } @@ -111,32 +111,32 @@ CallbackReturn KUKAFRIHardwareInterface::on_init( struct sched_param param; param.sched_priority = 95; if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), "setscheduler error"); - RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), strerror(errno)); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), "setscheduler error"); + RCLCPP_ERROR(rclcpp::get_logger("KukaEACHardwareInterface"), strerror(errno)); return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } -CallbackReturn KUKAFRIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) +CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { if (!client_application_.connect(30200, nullptr)) { - RCLCPP_ERROR(rclcpp::get_logger("KUKAFRIHardwareInterface"), "Could not connect"); + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Could not connect"); return CallbackReturn::FAILURE; } is_active_ = true; return CallbackReturn::SUCCESS; } -CallbackReturn KUKAFRIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) +CallbackReturn KukaFRIHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { client_application_.disconnect(); is_active_ = false; return CallbackReturn::SUCCESS; } -void KUKAFRIHardwareInterface::waitForCommand() +void KukaFRIHardwareInterface::waitForCommand() { hw_commands_ = hw_states_; hw_effort_command_ = hw_torques_; @@ -148,7 +148,7 @@ void KUKAFRIHardwareInterface::waitForCommand() } } -void KUKAFRIHardwareInterface::command() +void KukaFRIHardwareInterface::command() { rclcpp::Time stamp = ros_clock_.now(); if (++receive_counter_ == receive_multiplier_) { @@ -158,14 +158,14 @@ void KUKAFRIHardwareInterface::command() } -hardware_interface::return_type KUKAFRIHardwareInterface::read( +hardware_interface::return_type KukaFRIHardwareInterface::read( const rclcpp::Time &, const rclcpp::Duration &) { // Read is called in inactive state, check is necessary if (!is_active_) { active_read_ = false; - RCLCPP_DEBUG(rclcpp::get_logger("KUKAFRIHardwareInterface"), "Hardware interface not active"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware interface not active"); std::this_thread::sleep_for(std::chrono::milliseconds(1)); return hardware_interface::return_type::OK; } @@ -174,7 +174,7 @@ hardware_interface::return_type KUKAFRIHardwareInterface::read( if (!client_application_.client_app_read()) { RCLCPP_ERROR( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "Failed to read data from controller"); + "KukaFRIHardwareInterface"), "Failed to read data from controller"); return hardware_interface::return_type::ERROR; } @@ -203,13 +203,13 @@ hardware_interface::return_type KUKAFRIHardwareInterface::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type KUKAFRIHardwareInterface::write( +hardware_interface::return_type KukaFRIHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration &) { // Client app update and read must be called if read has been called in current cycle if (!active_read_) { - RCLCPP_DEBUG(rclcpp::get_logger("KUKAFRIHardwareInterface"), "Hardware interface not active"); + RCLCPP_DEBUG(rclcpp::get_logger("KukaFRIHardwareInterface"), "Hardware interface not active"); return hardware_interface::return_type::OK; } @@ -220,19 +220,19 @@ hardware_interface::return_type KUKAFRIHardwareInterface::write( if (!client_application_.client_app_write() && is_active_) { RCLCPP_ERROR( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "Could not send command to controller"); + "KukaFRIHardwareInterface"), "Could not send command to controller"); return hardware_interface::return_type::ERROR; } return hardware_interface::return_type::OK; } -void KUKAFRIHardwareInterface::updateCommand(const rclcpp::Time &) +void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &) { if (!is_active_) { RCLCPP_ERROR( rclcpp::get_logger( - "KUKAFRIHardwareInterface"), "Hardware inactive, exiting updateCommand"); + "KukaFRIHardwareInterface"), "Hardware inactive, exiting updateCommand"); return; } if (robot_state_.command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) { @@ -243,14 +243,14 @@ void KUKAFRIHardwareInterface::updateCommand(const rclcpp::Time &) const double * joint_positions_ = hw_commands_.data(); robotCommand().setJointPosition(joint_positions_); } else { - RCLCPP_ERROR(rclcpp::get_logger("KUKAFRIHardwareInterface"), "Unsupported command mode"); + RCLCPP_ERROR(rclcpp::get_logger("KukaFRIHardwareInterface"), "Unsupported command mode"); } for (auto & input : gpio_inputs_) { input.setValue(); } } -std::vector KUKAFRIHardwareInterface::export_state_interfaces() +std::vector KukaFRIHardwareInterface::export_state_interfaces() { std::vector state_interfaces; @@ -287,7 +287,7 @@ std::vector KUKAFRIHardwareInterface::export return state_interfaces; } -std::vector KUKAFRIHardwareInterface:: +std::vector KukaFRIHardwareInterface:: export_command_interfaces() { std::vector command_interfaces; @@ -314,6 +314,6 @@ export_command_interfaces() } // namespace kuka_sunrise_fri_driver PLUGINLIB_EXPORT_CLASS( - kuka_sunrise_fri_driver::KUKAFRIHardwareInterface, + kuka_sunrise_fri_driver::KukaFRIHardwareInterface, hardware_interface::SystemInterface ) From 4f93aab0ae1aa2d59e3b849a2607370a085a22cd Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 13 Nov 2023 14:13:43 +0100 Subject: [PATCH 2/9] critical code smells --- .../include/kuka_kss_rsi_driver/rsi_command.h | 4 +--- .../include/kuka_kss_rsi_driver/udp_server.h | 8 +++++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.h index f22842b4..e2b2f6bc 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.h +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/rsi_command.h @@ -48,9 +48,7 @@ namespace kuka_kss_rsi_driver class RSICommand { public: - RSICommand() - { - } + RSICommand() = default; RSICommand(std::vector joint_position_correction, uint64_t ipoc, bool stop = false) { TiXmlDocument doc; diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.h index 9f0babae..fd455d00 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.h +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/udp_server.h @@ -59,8 +59,6 @@ #include "rclcpp/rclcpp.hpp" -#define BUFSIZE 1024 - class UDPServer { public: @@ -90,6 +88,9 @@ class UDPServer close(sockfd_); } + UDPServer(UDPServer & other) = delete; + UDPServer & operator=(const UDPServer & other) = delete; + bool set_timeout(int millisecs) { if (millisecs != 0) { @@ -128,7 +129,7 @@ class UDPServer tv.tv_sec = tv_.tv_sec; tv.tv_usec = tv_.tv_usec; - if (select(sockfd_ + 1, &read_fds, NULL, NULL, &tv) < 0) { + if (select(sockfd_ + 1, &read_fds, nullptr, nullptr, &tv) < 0) { return 0; } @@ -157,6 +158,7 @@ class UDPServer } private: + static const int BUFSIZE = 1024; std::string local_host_; uint16_t local_port_; bool timeout_; From a53e2be61168965cc2d37d304137635ad4082aff Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 13 Nov 2023 16:52:15 +0100 Subject: [PATCH 3/9] sunrise robot_model --- .../launch/moveit_planning_example.launch.py | 2 +- .../robot_manager_node.hpp | 2 + .../launch/lbr_iiwa7_control.launch.py | 93 ------------------- .../launch/lbr_iiwa7_control_rviz.launch.py | 78 ---------------- .../src/robot_manager_node.cpp | 21 ++++- 5 files changed, 20 insertions(+), 176 deletions(-) delete mode 100644 kuka_sunrise_fri_driver/launch/lbr_iiwa7_control.launch.py delete mode 100644 kuka_sunrise_fri_driver/launch/lbr_iiwa7_control_rviz.launch.py diff --git a/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py b/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py index 6a667271..a8d3c660 100644 --- a/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py +++ b/examples/iiqka_moveit_example/launch/moveit_planning_example.launch.py @@ -41,7 +41,7 @@ def launch_setup(context, *args, **kwargs): ) rviz_config_file = get_package_share_directory( - 'kuka_lbr_iisy_moveit_config') + "/config/urdf_planning_scene.rviz" + 'kuka_resources') + "/config/view_6_axis_urdf.rviz" startup_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource( [get_package_share_directory('kuka_iiqka_eac_driver'), '/launch/startup.launch.py']), diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp index 467d92a3..88902ea2 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/robot_manager_node.hpp @@ -74,9 +74,11 @@ class RobotManagerNode : public kuka_drivers_core::ROS2BaseLCNode std::shared_ptr> is_configured_pub_; std_msgs::msg::Bool is_configured_msg_; std::string controller_name_; + std::string robot_model_; void handleControlEndedError(); void handleFRIEndedError(); + bool onRobotModelChangeRequest(const std::string & robot_model); }; } // namespace kuka_sunrise_fri_driver diff --git a/kuka_sunrise_fri_driver/launch/lbr_iiwa7_control.launch.py b/kuka_sunrise_fri_driver/launch/lbr_iiwa7_control.launch.py deleted file mode 100644 index 2c3e34dd..00000000 --- a/kuka_sunrise_fri_driver/launch/lbr_iiwa7_control.launch.py +++ /dev/null @@ -1,93 +0,0 @@ -# Copyright 2022 Áron Svastits -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_ros.actions import LifecycleNode -from launch_ros.descriptions import ParameterValue -from launch.substitutions import Command - -fri_config_file = get_package_share_directory( - 'kuka_sunrise_fri_driver') + "/config/driver_config.yaml" - - -def load_file(absolute_file_path): - try: - with open(absolute_file_path, 'r') as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def generate_launch_description(): - controller_config = (get_package_share_directory('kuka_sunrise_fri_driver') + - "/config/iiwa_ros2_controller_config.yaml") - joint_traj_controller_config = (get_package_share_directory('kuka_sunrise_fri_driver') + - "/config/joint_trajectory_controller_config.yaml") - robot_description_path = (get_package_share_directory('kuka_lbr_iiwa7_support') + - "/urdf/lbriiwa7.xacro") - robot_description = {'robot_description': ParameterValue( - Command(['xacro ', str(robot_description_path)]), value_type=str - )} - - conntroller_manager_node = '/controller_manager' - - return LaunchDescription([ - Node( - package='kuka_drivers_core', - executable='control_node', - output='both', - parameters=[robot_description, controller_config] - ), - LifecycleNode( - namespace='', package='kuka_sunrise_fri_driver', executable='robot_manager_node', - output='screen', name=['robot_manager'], - parameters=[fri_config_file, - {'position_controller_name': 'joint_trajectory_controller'}, - {'torque_controller_name': ''}] - ), - # robot_description topic is needed for rqt_joint_trajectory_controller - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description] - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "-c", - conntroller_manager_node, "--inactive"] - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["joint_trajectory_controller", "-c", conntroller_manager_node, "-p", - joint_traj_controller_config, "--inactive"] - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["fri_configuration_controller", "-c", - conntroller_manager_node, "--inactive"] - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["fri_state_broadcaster", "-c", - conntroller_manager_node, "--inactive"] - ) - ]) diff --git a/kuka_sunrise_fri_driver/launch/lbr_iiwa7_control_rviz.launch.py b/kuka_sunrise_fri_driver/launch/lbr_iiwa7_control_rviz.launch.py deleted file mode 100644 index 86d687c9..00000000 --- a/kuka_sunrise_fri_driver/launch/lbr_iiwa7_control_rviz.launch.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright 2022 Áron Svastits -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_ros.descriptions import ParameterValue -from launch.substitutions import Command - - -def load_file(absolute_file_path): - try: - with open(absolute_file_path, 'r') as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def generate_launch_description(): - controller_config = (get_package_share_directory('kuka_sunrise_fri_driver') + - "/config/iiwa_ros2_controller_config.yaml") - forward_controller_config = (get_package_share_directory('kuka_sunrise_fri_driver') + - "/config/forward_controller_config.yaml") - robot_description_path = (get_package_share_directory('kuka_lbr_iiwa7_support') + - "/urdf/lbriiwa7.xacro") - robot_description = {'robot_description': ParameterValue( - Command(['xacro ', str(robot_description_path)]), value_type=str - )} - - rviz_config_file = os.path.join(get_package_share_directory('kuka_lbr_iiwa7_support'), - 'launch', 'urdf.rviz') - - return LaunchDescription([ - Node( - package='kuka_drivers_core', - executable='control_node', - parameters=[robot_description, controller_config] - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "-c", - "/controller_manager", "--inactive"] - ), - Node( - package="controller_manager", - executable="spawner", - arguments=["forward_command_controller", "-c", "/controller_manager", "-p", - forward_controller_config, "--inactive"] - ), - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description] - ), - ]) diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 9e5dfcba..f5ff5b34 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -58,6 +58,13 @@ RobotManagerNode::RobotManagerNode() is_configured_pub_ = this->create_publisher( "robot_manager/is_configured", is_configured_qos); + + this->registerStaticParameter( + "robot_model", "lbr_iiwa14_r820", + kuka_drivers_core::ParameterSetAccessRights{true, false, + false, false, false}, [this](const std::string & robot_model) { + return this->onRobotModelChangeRequest(robot_model); + }); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -66,7 +73,7 @@ RobotManagerNode::on_configure(const rclcpp_lifecycle::State &) // Configure hardware interface auto hw_request = std::make_shared(); - hw_request->name = "iiwa_hardware"; + hw_request->name = robot_model_; hw_request->target_state.label = "inactive"; auto hw_response = kuka_drivers_core::sendRequest( @@ -163,7 +170,7 @@ RobotManagerNode::on_cleanup(const rclcpp_lifecycle::State &) // If it is inactive, cleanup will also succeed auto hw_request = std::make_shared(); - hw_request->name = "iiwa_hardware"; + hw_request->name = robot_model_; hw_request->target_state.label = "unconfigured"; auto hw_response = kuka_drivers_core::sendRequest( @@ -204,7 +211,7 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate hardware interface auto hw_request = std::make_shared(); - hw_request->name = "iiwa_hardware"; + hw_request->name = robot_model_; hw_request->target_state.label = "active"; auto hw_response = kuka_drivers_core::sendRequest( @@ -288,7 +295,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) // If it is inactive, deactivation will also succeed auto hw_request = std::make_shared(); - hw_request->name = "iiwa_hardware"; + hw_request->name = robot_model_; hw_request->target_state.label = "inactive"; auto hw_response = kuka_drivers_core::sendRequest( @@ -371,6 +378,12 @@ void RobotManagerNode::handleFRIEndedError() this->LifecycleNode::deactivate(); } +bool RobotManagerNode::onRobotModelChangeRequest(const std::string & robot_model) +{ + robot_model_ = robot_model; + return true; +} + } // namespace kuka_sunrise_fri_driver int main(int argc, char * argv[]) From e9acadbd9e81ffbe0064a839da0b31add1de450a Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 20 Nov 2023 08:53:31 +0100 Subject: [PATCH 4/9] add rsi_simulator package --- .../kuka_rsi_simulator/rsi_simulator.py | 139 ++++++++++++++++++ .../launch/kuka_rsi_simulator_launch.py | 43 ++++++ kuka_rsi_simulator/package.xml | 19 +++ .../resource/kuka_rsi_simulator | 0 kuka_rsi_simulator/setup.cfg | 4 + kuka_rsi_simulator/setup.py | 33 +++++ kuka_rsi_simulator/test/test_copyright.py | 23 +++ kuka_rsi_simulator/test/test_flake8.py | 25 ++++ kuka_rsi_simulator/test/test_pep257.py | 23 +++ 9 files changed, 309 insertions(+) create mode 100755 kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py create mode 100644 kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py create mode 100644 kuka_rsi_simulator/package.xml create mode 100644 kuka_rsi_simulator/resource/kuka_rsi_simulator create mode 100644 kuka_rsi_simulator/setup.cfg create mode 100644 kuka_rsi_simulator/setup.py create mode 100644 kuka_rsi_simulator/test/test_copyright.py create mode 100644 kuka_rsi_simulator/test/test_flake8.py create mode 100644 kuka_rsi_simulator/test/test_pep257.py diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py b/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py new file mode 100755 index 00000000..5335d2aa --- /dev/null +++ b/kuka_rsi_simulator/kuka_rsi_simulator/rsi_simulator.py @@ -0,0 +1,139 @@ +# Copyright 2022 Márton Antal +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.from launch import LaunchDescription + +import sys +import socket +import xml.etree.ElementTree as ET +import numpy as np + + +import errno +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + + +def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc): + q = act_joint_pos + qd = setpoint_joint_pos + root = ET.Element('Rob', {'TYPE': 'KUKA'}) + ET.SubElement(root, 'RIst', {'X': '0.0', 'Y': '0.0', 'Z': '0.0', + 'A': '0.0', 'B': '0.0', 'C': '0.0'}) + ET.SubElement(root, 'RSol', {'X': '0.0', 'Y': '0.0', 'Z': '0.0', + 'A': '0.0', 'B': '0.0', 'C': '0.0'}) + ET.SubElement(root, 'AIPos', {'A1': str(q[0]), 'A2': str(q[1]), 'A3': str(q[2]), + 'A4': str(q[3]), 'A5': str(q[4]), 'A6': str(q[5])}) + ET.SubElement(root, 'ASPos', {'A1': str(qd[0]), 'A2': str(qd[1]), 'A3': str(qd[2]), + 'A4': str(qd[3]), 'A5': str(qd[4]), 'A6': str(qd[5])}) + ET.SubElement(root, 'Delay', {'D': str(timeout_count)}) + ET.SubElement(root, 'IPOC').text = str(ipoc) + return ET.tostring(root) + + +def parse_rsi_xml_sen(data): + root = ET.fromstring(data) + AK = root.find('AK').attrib + desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'], + AK['A4'], AK['A5'], AK['A6']]).astype(np.float64) + IPOC = root.find('IPOC').text + stop_flag = root.find('Stop').text + + return desired_joint_correction, int(IPOC), bool(int(stop_flag)) + + +class RSISimulator(Node): + cycle_time = 0.04 + act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64) + initial_joint_pos = act_joint_pos.copy() + des_joint_correction_absolute = np.zeros(6) + timeout_count = 0 + ipoc = 0 + rsi_ip_address_ = '127.0.0.1' + rsi_port_address_ = 59152 + rsi_send_name_ = 'IamFree' + rsi_act_pub_ = None + rsi_cmd_pub_ = None + node_name_ = 'rsi_simulator_node' + socket_ = None + + def __init__(self, node_name): + super().__init__(node_name) + self.node_name_ = node_name + self.timer = self.create_timer(self.cycle_time, self.timer_callback) + self.declare_parameter('rsi_ip_address', '127.0.0.1') + self.declare_parameter('rsi_port', 59152) + self.declare_parameter('rsi_send_name', "IamFree") + self.rsi_ip_address_ = (self.get_parameter('rsi_ip_address').get_parameter_value() + .string_value) + self.rsi_port_address_ = self.get_parameter('rsi_port').get_parameter_value().integer_value + self.rsi_send_name_ = (self.get_parameter('rsi_send_name').get_parameter_value() + .string_value) + self.rsi_act_pub_ = self.create_publisher(String, self.node_name_ + '/rsi/state', 1) + self.rsi_cmd_pub_ = self.create_publisher(String, self.node_name_ + '/rsi/command', 1) + self.get_logger().info('rsi_ip_address: {}'.format(self.rsi_ip_address_)) + self.get_logger().info('rsi_port: {}'.format(self.rsi_port_address_)) + + self.socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.get_logger().info('{}, Successfully created socket'.format(self.node_name_)) + self.socket_.settimeout(self.cycle_time) + + def timer_callback(self): + if self.timeout_count == 100: + self.get_logger().fatal('{} Timeout count of 100 exceeded'.format(self.node_name_)) + sys.exit() + try: + msg = create_rsi_xml_rob(self.act_joint_pos, self.initial_joint_pos, + self.timeout_count, self.ipoc) + self.rsi_act_pub_.publish(msg) + self.socket_.sendto(msg, (self.rsi_ip_address_, self.rsi_port_address_)) + recv_msg, addr = self.socket_.recvfrom(1024) + self.rsi_cmd_pub_.publish(recv_msg) + self.get_logger().warn('msg: {}'.format(recv_msg)) + des_joint_correction_absolute, ipoc_recv, stop_flag = parse_rsi_xml_sen(recv_msg) + if ipoc_recv == self.ipoc: + self.act_joint_pos = self.initial_joint_pos + des_joint_correction_absolute + else: + self.get_logger().warn('{}: Packet is late'.format(self.node_name_)) + self.get_logger().warn('{}: sent ipoc: {}, received: {}'. + format(self.node_name_, self.ipoc, ipoc_recv)) + if self.ipoc != 0: + self.timeout_count += 1 + self.ipoc += 1 + if stop_flag: + self.on_shutdown() + sys.exit() + except OSError: + if self.ipoc != 0: + self.timeout_count += 1 + self.get_logger().warn('{}: Socket timed out'.format(self.node_name_)) + + def on_shutdown(self): + self.socket_.close() + self.get_logger().info('Socket closed.') + + +def main(): + node_name = 'rsi_simulator_node' + + rclpy.init() + node = RSISimulator(node_name) + node.get_logger().info('{}: Started'.format(node_name)) + + rclpy.spin(node) + node.on_shutdown() + node.get_logger().info('{}: Shutting down'.format(node_name)) + + +if __name__ == '__main__': + main() diff --git a/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py b/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py new file mode 100644 index 00000000..a683f4ce --- /dev/null +++ b/kuka_rsi_simulator/launch/kuka_rsi_simulator_launch.py @@ -0,0 +1,43 @@ +# Copyright 2022 Márton Antal +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.from launch import LaunchDescription + + +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch import LaunchDescription + +from launch_ros.actions import Node + + +def generate_launch_description(): + rsi_ip_address = DeclareLaunchArgument( + 'rsi_ip_address', default_value=TextSubstitution(text='127.0.0.1') + ) + rsi_port = DeclareLaunchArgument( + 'rsi_port', default_value=TextSubstitution(text='59152') + ) + + return LaunchDescription([ + rsi_ip_address, + rsi_port, + Node( + package='kuka_rsi_simulator', + executable='rsi_simulator', + name='kuka_rsi_simulator', + parameters=[{ + 'rsi_ip_address': LaunchConfiguration('rsi_ip_address'), + 'rsi_port': LaunchConfiguration('rsi_port'), + }] + ) + ]) diff --git a/kuka_rsi_simulator/package.xml b/kuka_rsi_simulator/package.xml new file mode 100644 index 00000000..d3b944b0 --- /dev/null +++ b/kuka_rsi_simulator/package.xml @@ -0,0 +1,19 @@ + + + + kuka_rsi_simulator + 0.0.0 + Simple package for simulating the KUKA RSI interface + Marton Antal + BSD + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + ros2launch + + + ament_python + + diff --git a/kuka_rsi_simulator/resource/kuka_rsi_simulator b/kuka_rsi_simulator/resource/kuka_rsi_simulator new file mode 100644 index 00000000..e69de29b diff --git a/kuka_rsi_simulator/setup.cfg b/kuka_rsi_simulator/setup.cfg new file mode 100644 index 00000000..2922234f --- /dev/null +++ b/kuka_rsi_simulator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/kuka_rsi_simulator +[install] +install_scripts=$base/lib/kuka_rsi_simulator diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py new file mode 100644 index 00000000..21a11608 --- /dev/null +++ b/kuka_rsi_simulator/setup.py @@ -0,0 +1,33 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'kuka_rsi_simulator' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*.launch.py'))), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*_launch.py'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*.yaml'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Marton Antal', + maintainer_email='antal.marci@gmail.com', + description='Simple package for simulating the KUKA RSI interface.', + license='BSD', + entry_points={ + 'console_scripts': [ + 'rsi_simulator = kuka_rsi_simulator.rsi_simulator:main' + ], + }, +) diff --git a/kuka_rsi_simulator/test/test_copyright.py b/kuka_rsi_simulator/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/kuka_rsi_simulator/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/kuka_rsi_simulator/test/test_flake8.py b/kuka_rsi_simulator/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/kuka_rsi_simulator/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/kuka_rsi_simulator/test/test_pep257.py b/kuka_rsi_simulator/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/kuka_rsi_simulator/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From d148841b38ec68ff2de27432b47d4a44952828ba Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 20 Nov 2023 13:41:56 +0100 Subject: [PATCH 5/9] add rsi sim to meta package --- kuka_drivers/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/kuka_drivers/package.xml b/kuka_drivers/package.xml index e540e7a8..ec9af207 100644 --- a/kuka_drivers/package.xml +++ b/kuka_drivers/package.xml @@ -18,6 +18,7 @@ kuka_kss_rsi_driver kuka_sunrise_fri_driver iiqka_moveit_example + kuka_rsi_simulator ament_cmake From dfe91744d113514c770a78a4849f31396f49bfae Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Mon, 20 Nov 2023 19:10:20 +0100 Subject: [PATCH 6/9] join_ai -> joint_i --- .../config/dummy_impedance_publisher.yaml | 12 +++++----- .../config/dummy_publisher.yaml | 12 +++++----- .../config/moveit_controllers.yaml | 12 +++++----- .../config/effort_controller_config.yaml | 24 +++++++++---------- .../joint_impedance_controller_config.yaml | 12 +++++----- .../joint_trajectory_controller_config.yaml | 12 +++++----- .../joint_trajectory_controller_config.yaml | 12 +++++----- .../joint_trajectory_controller_config.yaml | 13 +++++----- 8 files changed, 55 insertions(+), 54 deletions(-) diff --git a/examples/iiqka_moveit_example/config/dummy_impedance_publisher.yaml b/examples/iiqka_moveit_example/config/dummy_impedance_publisher.yaml index 2eec55ca..aae601cb 100644 --- a/examples/iiqka_moveit_example/config/dummy_impedance_publisher.yaml +++ b/examples/iiqka_moveit_example/config/dummy_impedance_publisher.yaml @@ -7,9 +7,9 @@ damping_goals: [0.7, 0.0, 0.7, 0.7, 0.7, 0.7] joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 diff --git a/examples/iiqka_moveit_example/config/dummy_publisher.yaml b/examples/iiqka_moveit_example/config/dummy_publisher.yaml index 8c2bce81..6b2a013f 100644 --- a/examples/iiqka_moveit_example/config/dummy_publisher.yaml +++ b/examples/iiqka_moveit_example/config/dummy_publisher.yaml @@ -15,11 +15,11 @@ positions: [-0.2, -1.57, -0.2, -0.2, -0.2, -0.2] joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 check_starting_point: false diff --git a/examples/iiqka_moveit_example/config/moveit_controllers.yaml b/examples/iiqka_moveit_example/config/moveit_controllers.yaml index 2539cb08..3dd73c3f 100644 --- a/examples/iiqka_moveit_example/config/moveit_controllers.yaml +++ b/examples/iiqka_moveit_example/config/moveit_controllers.yaml @@ -15,10 +15,10 @@ moveit_simple_controller_manager: type: FollowJointTrajectory default: true joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 diff --git a/kuka_iiqka_eac_driver/config/effort_controller_config.yaml b/kuka_iiqka_eac_driver/config/effort_controller_config.yaml index 1a87f1ab..838d627b 100644 --- a/kuka_iiqka_eac_driver/config/effort_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/effort_controller_config.yaml @@ -1,34 +1,34 @@ effort_controller: ros__parameters: joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 gains: - joint_a1: + joint_1: p: 5.0 i: 0.0 d: 0.1 - joint_a2: + joint_2: p: 5.0 i: 0.0 d: 0.1 - joint_a3: + joint_3: p: 5.0 i: 0.0 d: 0.1 - joint_a4: + joint_4: p: 5.0 i: 0.0 d: 0.1 - joint_a5: + joint_5: p: 5.0 i: 0.0 d: 0.1 - joint_a6: + joint_6: p: 5.0 i: 0.0 d: 0.1 diff --git a/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml b/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml index abe8b217..4bf5cb9c 100644 --- a/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/joint_impedance_controller_config.yaml @@ -1,9 +1,9 @@ joint_impedance_controller: ros__parameters: joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 \ No newline at end of file + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 \ No newline at end of file diff --git a/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml b/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml index 724670e2..0fb9c446 100644 --- a/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_iiqka_eac_driver/config/joint_trajectory_controller_config.yaml @@ -1,12 +1,12 @@ joint_trajectory_controller: ros__parameters: joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 command_interfaces: - position diff --git a/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml b/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml index 3de96e7b..85b8feaa 100644 --- a/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_kss_rsi_driver/config/joint_trajectory_controller_config.yaml @@ -1,12 +1,12 @@ joint_trajectory_controller: ros__parameters: joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 command_interfaces: - position diff --git a/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml b/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml index 6d0ce6b3..62619923 100644 --- a/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml @@ -1,12 +1,13 @@ joint_trajectory_controller: ros__parameters: joints: - - joint_a1 - - joint_a2 - - joint_a3 - - joint_a4 - - joint_a5 - - joint_a6 + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - joint_7 command_interfaces: - position From 6ff90aff6b7d2241ec1280a86e30131953487928 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Thu, 23 Nov 2023 09:26:06 +0100 Subject: [PATCH 7/9] visibility control --- kuka_iiqka_eac_driver/CMakeLists.txt | 13 +++++-- .../hardware_interface.hpp | 30 +++++++++----- .../visibility_control.h | 33 +++++++--------- kuka_kss_rsi_driver/CMakeLists.txt | 2 +- .../kuka_kss_rsi_driver/visibility_control.h | 31 ++++++--------- kuka_sunrise_fri_driver/CMakeLists.txt | 13 +++++-- .../hardware_interface.hpp | 39 ++++++++++++------- .../visibility_control.h | 33 +++++++--------- 8 files changed, 102 insertions(+), 92 deletions(-) diff --git a/kuka_iiqka_eac_driver/CMakeLists.txt b/kuka_iiqka_eac_driver/CMakeLists.txt index 35cc1ae5..c9895842 100644 --- a/kuka_iiqka_eac_driver/CMakeLists.txt +++ b/kuka_iiqka_eac_driver/CMakeLists.txt @@ -71,11 +71,16 @@ else() remove_definitions(-DNON_MOCK_SETUP) endif() -add_library(kuka_iiqka_eac_driver SHARED +add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp ) -ament_target_dependencies(kuka_iiqka_eac_driver rclcpp sensor_msgs hardware_interface) -target_link_libraries(kuka_iiqka_eac_driver motion-external-proto-api-nanopb motion-services-ecs-proto-api-cpp + +# 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") + +ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs hardware_interface) +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) @@ -86,7 +91,7 @@ target_link_libraries(robot_manager_node kuka_drivers_core::communication_helper pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) -install(TARGETS kuka_iiqka_eac_driver robot_manager_node +install(TARGETS ${PROJECT_NAME} robot_manager_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config launch 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 6a131aa7..6c863cd5 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 @@ -41,6 +41,8 @@ #include "nanopb/kuka/ecs/v1/motion_state_external.pb.hh" #include "os-core-udp-communication/replier.h" +#include "kuka_iiqka_eac_driver/visibility_control.h" + using hardware_interface::return_type; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -52,24 +54,34 @@ class KukaEACHardwareInterface : public hardware_interface::SystemInterface public: RCLCPP_SHARED_PTR_DEFINITIONS(KukaEACHardwareInterface) - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo & info) + override; - std::vector export_state_interfaces() override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC std::vector + export_state_interfaces() override; - std::vector export_command_interfaces() override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC std::vector + export_command_interfaces() override; - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type read( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; - return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + KUKA_IIQKA_EAC_DRIVER_PUBLIC return_type write( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; private: - void ObserveControl(); + KUKA_IIQKA_EAC_DRIVER_LOCAL void ObserveControl(); bool is_active_ = false; bool msg_received_ = false; diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h index ef70c09c..cc10a4d8 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h @@ -1,23 +1,16 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2023 Áron Svastits // -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at +// 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 +// http://www.apache.org/licenses/LICENSE-2.0 // -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ +// 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__VISIBILITY_CONTROL_H_ #define KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_ @@ -33,7 +26,7 @@ #define KUKA_IIQKA_EAC_DRIVER_EXPORT __declspec(dllexport) #define KUKA_IIQKA_EAC_DRIVER_IMPORT __declspec(dllimport) #endif -#ifdef KUKA_IIQKA_EAC_DRIVER_BUILDING_DLL +#ifdef KUKA_IIQKA_EAC_DRIVER_BUILDING_LIBRARY #define KUKA_IIQKA_EAC_DRIVER_PUBLIC KUKA_IIQKA_EAC_DRIVER_EXPORT #else #define KUKA_IIQKA_EAC_DRIVER_PUBLIC KUKA_IIQKA_EAC_DRIVER_IMPORT @@ -50,7 +43,7 @@ #define KUKA_IIQKA_EAC_DRIVER_PUBLIC #define KUKA_IIQKA_EAC_DRIVER_LOCAL #endif -#define KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_ +#define KUKA_IIQKA_EAC_DRIVER_PUBLIC_TYPE #endif -#endif // KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_ +#endif // KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/kuka_kss_rsi_driver/CMakeLists.txt b/kuka_kss_rsi_driver/CMakeLists.txt index 207c9db3..fe7010ee 100644 --- a/kuka_kss_rsi_driver/CMakeLists.txt +++ b/kuka_kss_rsi_driver/CMakeLists.txt @@ -35,7 +35,7 @@ add_library(${PROJECT_NAME} SHARED # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "KUKA_KSS_RSI_DRIVER_BUILDING_LIBRARY") # prevent pluginlib from using boost target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h index b9cb1946..2f606555 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h @@ -1,23 +1,16 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2023 Áron Svastits // -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at +// 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 +// http://www.apache.org/licenses/LICENSE-2.0 // -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ +// 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_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_ #define KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_ @@ -33,7 +26,7 @@ #define KUKA_KSS_RSI_DRIVER_EXPORT __declspec(dllexport) #define KUKA_KSS_RSI_DRIVER_IMPORT __declspec(dllimport) #endif -#ifdef KUKA_KSS_RSI_DRIVER_BUILDING_DLL +#ifdef KUKA_KSS_RSI_DRIVER_BUILDING_LIBRARY #define KUKA_KSS_RSI_DRIVER_PUBLIC KUKA_KSS_RSI_DRIVER_EXPORT #else #define KUKA_KSS_RSI_DRIVER_PUBLIC KUKA_KSS_RSI_DRIVER_IMPORT @@ -53,4 +46,4 @@ #define KUKA_KSS_RSI_DRIVER_PUBLIC_TYPE #endif -#endif // KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_ +#endif // KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/kuka_sunrise_fri_driver/CMakeLists.txt b/kuka_sunrise_fri_driver/CMakeLists.txt index c5def3c7..94f1fe99 100644 --- a/kuka_sunrise_fri_driver/CMakeLists.txt +++ b/kuka_sunrise_fri_driver/CMakeLists.txt @@ -72,11 +72,16 @@ target_link_libraries(fri_client_sdk PRIVATE protobuf-nanopb) install(DIRECTORY include/fri_client_sdk DESTINATION include) install(FILES ${private_headers} DESTINATION include) -add_library(kuka_fri_hw_interface SHARED +add_library(${PROJECT_NAME} SHARED src/hardware_interface.cpp ) -ament_target_dependencies(kuka_fri_hw_interface kuka_driver_interfaces rclcpp rclcpp_lifecycle hardware_interface) -target_link_libraries(kuka_fri_hw_interface fri_client_sdk) + +# 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_SUNRISE_FRI_DRIVER_BUILDING_LIBRARY") + +ament_target_dependencies(${PROJECT_NAME} kuka_driver_interfaces rclcpp rclcpp_lifecycle hardware_interface) +target_link_libraries(${PROJECT_NAME} fri_client_sdk) add_library(configuration_manager SHARED src/connection_helpers/configuration_manager.cpp) @@ -92,7 +97,7 @@ target_link_libraries(robot_manager_node pluginlib_export_plugin_description_file(hardware_interface hardware_interface.xml) -install(TARGETS fri_connection fri_client_sdk kuka_fri_hw_interface robot_manager_node +install(TARGETS ${PROJECT_NAME} fri_connection fri_client_sdk robot_manager_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp index 0fd7985a..8b34f545 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/hardware_interface.hpp @@ -32,6 +32,8 @@ #include "fri_client_sdk/friUdpConnection.h" #include "fri_client_sdk/friClientIf.h" +#include "kuka_sunrise_fri_driver/visibility_control.h" + #include "pluginlib/class_list_macros.hpp" @@ -55,26 +57,33 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, public KUKA::FRI::LBRClient { public: - KukaFRIHardwareInterface() - : client_application_(udp_connection_, *this) {} - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + RCLCPP_SHARED_PTR_DEFINITIONS(KukaFRIHardwareInterface) - std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - - hardware_interface::return_type read( + KUKA_SUNRISE_FRI_DRIVER_PUBLIC KukaFRIHardwareInterface() + : client_application_(udp_connection_, *this) {} + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + KUKA_SUNRISE_FRI_DRIVER_PUBLIC std::vector + export_state_interfaces() override; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC std::vector + export_command_interfaces() override; + + KUKA_SUNRISE_FRI_DRIVER_PUBLIC hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; - hardware_interface::return_type write( + KUKA_SUNRISE_FRI_DRIVER_PUBLIC hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; - void updateCommand(const rclcpp::Time & stamp); - bool setReceiveMultiplier(int receive_multiplier); + KUKA_SUNRISE_FRI_DRIVER_PUBLIC void updateCommand(const rclcpp::Time & stamp); + KUKA_SUNRISE_FRI_DRIVER_PUBLIC bool setReceiveMultiplier(int receive_multiplier); - void waitForCommand() final; - void command() final; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC void waitForCommand() final; + KUKA_SUNRISE_FRI_DRIVER_PUBLIC void command() final; class InvalidGPIOTypeException : public std::runtime_error { @@ -124,7 +133,7 @@ class KukaFRIHardwareInterface : public hardware_interface::SystemInterface, RobotState robot_state_; - IOTypes getType(const std::string & type_string) const + KUKA_SUNRISE_FRI_DRIVER_LOCAL IOTypes getType(const std::string & type_string) const { auto it = types.find(type_string); if (it != types.end()) { diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h index c948a88d..02a28954 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h @@ -1,23 +1,16 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2023 Áron Svastits // -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at +// 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 +// http://www.apache.org/licenses/LICENSE-2.0 // -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* This header must be included by all rclcpp headers which declare symbols - * which are defined in the rclcpp library. When not building the rclcpp - * library, i.e. when using the headers in other package's code, the contents - * of this header change the visibility of certain symbols which the rclcpp - * library cannot have, but the consuming code must have inorder to link. - */ +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_ #define KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_ @@ -33,7 +26,7 @@ #define KUKA_SUNRISE_FRI_DRIVER_EXPORT __declspec(dllexport) #define KUKA_SUNRISE_FRI_DRIVER_IMPORT __declspec(dllimport) #endif -#ifdef KUKA_SUNRISE_FRI_DRIVER_BUILDING_DLL +#ifdef KUKA_SUNRISE_FRI_DRIVER_BUILDING_LIBRARY #define KUKA_SUNRISE_FRI_DRIVER_PUBLIC KUKA_SUNRISE_FRI_DRIVER_EXPORT #else #define KUKA_SUNRISE_FRI_DRIVER_PUBLIC KUKA_SUNRISE_FRI_DRIVER_IMPORT @@ -50,7 +43,7 @@ #define KUKA_SUNRISE_FRI_DRIVER_PUBLIC #define KUKA_SUNRISE_FRI_DRIVER_LOCAL #endif -#define KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_ +#define KUKA_SUNRISE_FRI_DRIVER_PUBLIC_TYPE #endif -#endif // KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_ +#endif // KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_ \ No newline at end of file From 698d4cc385aebc2750bc2d4e97bce09eb2c1c1c1 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Tue, 12 Dec 2023 13:26:25 +0100 Subject: [PATCH 8/9] uncrustify --- .../include/kuka_iiqka_eac_driver/visibility_control.h | 2 +- .../include/kuka_kss_rsi_driver/visibility_control.h | 2 +- .../include/kuka_sunrise_fri_driver/visibility_control.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h index cc10a4d8..5640c7f4 100644 --- a/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h +++ b/kuka_iiqka_eac_driver/include/kuka_iiqka_eac_driver/visibility_control.h @@ -46,4 +46,4 @@ #define KUKA_IIQKA_EAC_DRIVER_PUBLIC_TYPE #endif -#endif // KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_ \ No newline at end of file +#endif // KUKA_IIQKA_EAC_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h index 2f606555..a91aaa35 100644 --- a/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h +++ b/kuka_kss_rsi_driver/include/kuka_kss_rsi_driver/visibility_control.h @@ -46,4 +46,4 @@ #define KUKA_KSS_RSI_DRIVER_PUBLIC_TYPE #endif -#endif // KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_ \ No newline at end of file +#endif // KUKA_KSS_RSI_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h index 02a28954..2d986a30 100644 --- a/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h +++ b/kuka_sunrise_fri_driver/include/kuka_sunrise_fri_driver/visibility_control.h @@ -46,4 +46,4 @@ #define KUKA_SUNRISE_FRI_DRIVER_PUBLIC_TYPE #endif -#endif // KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_ \ No newline at end of file +#endif // KUKA_SUNRISE_FRI_DRIVER__VISIBILITY_CONTROL_H_ From 788e56ac8873dfd9aa17fefdb8bf3180bc803d12 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Tue, 12 Dec 2023 13:56:57 +0100 Subject: [PATCH 9/9] extend package xml --- kuka_iiqka_eac_driver/package.xml | 2 + kuka_kss_rsi_driver/package.xml | 5 ++- kuka_sunrise_fri_driver/package.xml | 60 +++++++++++++++-------------- 3 files changed, 37 insertions(+), 30 deletions(-) diff --git a/kuka_iiqka_eac_driver/package.xml b/kuka_iiqka_eac_driver/package.xml index 3c47d6a0..b0fbdc80 100644 --- a/kuka_iiqka_eac_driver/package.xml +++ b/kuka_iiqka_eac_driver/package.xml @@ -27,6 +27,8 @@ libgrpc kuka_lbr_iisy_support + ros2_control + ros2_controllers ament_cmake_copyright ament_cmake_cppcheck diff --git a/kuka_kss_rsi_driver/package.xml b/kuka_kss_rsi_driver/package.xml index 07c38e95..83b5a5cd 100644 --- a/kuka_kss_rsi_driver/package.xml +++ b/kuka_kss_rsi_driver/package.xml @@ -19,7 +19,10 @@ tinyxml_vendor hardware_interface pluginlib - controller_manager_msgs + controller_manager_msgs + + ros2_control + ros2_controllers ament_cmake_copyright ament_cmake_cppcheck diff --git a/kuka_sunrise_fri_driver/package.xml b/kuka_sunrise_fri_driver/package.xml index 26bd21e8..d9b6a9ed 100644 --- a/kuka_sunrise_fri_driver/package.xml +++ b/kuka_sunrise_fri_driver/package.xml @@ -1,38 +1,40 @@ - kuka_sunrise_fri_driver - 0.0.0 - ROS2 KUKA sunrise interface - Zoltan Resi - Gergely Kovacs - Apache 2.0 + kuka_sunrise_fri_driver + 0.0.0 + ROS2 KUKA sunrise interface + Zoltan Resi + Gergely Kovacs + Apache 2.0 - ament_cmake - ament_cmake_python - rosidl_default_generators + ament_cmake + ament_cmake_python + rosidl_default_generators - rclcpp - rclcpp_lifecycle - std_msgs - std_srvs - sensor_msgs - kuka_driver_interfaces - kuka_drivers_core - hardware_interface - controller_manager_msgs + rclcpp + rclcpp_lifecycle + std_msgs + std_srvs + sensor_msgs + kuka_driver_interfaces + kuka_drivers_core + hardware_interface + controller_manager_msgs - rosidl_default_runtime + rosidl_default_runtime + ros2_control + ros2_controllers - ament_cmake_copyright - ament_cmake_cppcheck - ament_cmake_pep257 - ament_cmake_flake8 - ament_cmake_cpplint - ament_cmake_lint_cmake - ament_cmake_xmllint + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_pep257 + ament_cmake_flake8 + ament_cmake_cpplint + ament_cmake_lint_cmake + ament_cmake_xmllint - - ament_cmake - + + ament_cmake +