From ac36e53902f6520b2d70d06a9e24ecac19014993 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 15:07:50 +0100 Subject: [PATCH 1/2] added a worker class --- lbr_fri_ros2/CMakeLists.txt | 1 + lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 14 +++-- lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp | 32 ++++++++++++ lbr_fri_ros2/src/app.cpp | 55 ++++++-------------- lbr_fri_ros2/src/worker.cpp | 53 +++++++++++++++++++ 5 files changed, 112 insertions(+), 43 deletions(-) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp create mode 100644 lbr_fri_ros2/src/worker.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index e28e2132..04dff3fd 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -36,6 +36,7 @@ add_library(lbr_fri_ros2 src/filters.cpp src/ft_estimator.cpp src/kinematics.cpp + src/worker.cpp ) target_include_directories(lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index f66a8af0..e1fa36a9 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -14,9 +14,17 @@ #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/worker.hpp" namespace lbr_fri_ros2 { -class App { +class App : public Worker { + /** + * @brief This clas provides utilities to run the KUKA::FRI::ClientApplication asynchronously. + * Note that the rate at which the application runs is determined by the robot. This is because + * the run_thread_ uses blocking function calls from the FRI client SDK, i.e. + * KUKA::FRI::ClientApplication::step() (this is by KUKA's design). + * + */ protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; @@ -26,10 +34,10 @@ class App { bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); bool close_udp_socket(); - void run_async(int rt_prio = 80); - void request_stop(); + void run_async(int rt_prio = 80) override; protected: + void perform_work_() override; bool valid_port_(const int &port_id); std::atomic_bool should_stop_, running_; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp new file mode 100644 index 00000000..d1ff65ac --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp @@ -0,0 +1,32 @@ +#ifndef LBR_FRI_ROS2__WORKER_HPP_ +#define LBR_FRI_ROS2__WORKER_HPP_ + +#include +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "realtime_tools/thread_priority.hpp" + +#include "lbr_fri_ros2/formatting.hpp" + +namespace lbr_fri_ros2 { +class Worker { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Worker"; + +public: + Worker(); + ~Worker(); + + virtual void run_async(int rt_prio = 80); + void request_stop(); + +protected: + virtual void perform_work_() = 0; + + std::atomic_bool should_stop_, running_; + std::thread run_thread_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__WORKER_HPP_ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index d8860834..45559466 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -2,8 +2,7 @@ namespace lbr_fri_ros2 { App::App(const std::shared_ptr async_client_ptr) - : should_stop_(true), running_(false), async_client_ptr_(nullptr), connection_ptr_(nullptr), - app_ptr_(nullptr) { + : async_client_ptr_(nullptr), connection_ptr_(nullptr), app_ptr_(nullptr) { async_client_ptr_ = async_client_ptr; connection_ptr_ = std::make_unique(); app_ptr_ = std::make_unique(*connection_ptr_, *async_client_ptr_); @@ -83,48 +82,24 @@ void App::run_async(int rt_prio) { ColorScheme::ERROR << "App not configured" << ColorScheme::ENDC); return; } - if (running_) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::WARNING << "App already running" << ColorScheme::ENDC); - return; - } - run_thread_ = std::thread([this, rt_prio]() { - if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::WARNING - << "Failed to set FIFO realtime scheduling policy. Refer to " - "[https://control.ros.org/master/doc/ros2_control/" - "controller_manager/doc/userdoc.html]." - << ColorScheme::ENDC); - } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::OKGREEN - << "Realtime scheduling policy set to FIFO with priority '" << rt_prio - << "'" << ColorScheme::ENDC); - } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); - should_stop_ = false; - bool success = true; - while (rclcpp::ok() && success && !should_stop_) { - success = app_ptr_->step(); // stuck if never connected, we thus detach the thread as join may - // never return - running_ = true; - if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); - break; - } - } - async_client_ptr_->get_state_interface()->uninitialize(); - running_ = false; - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); - }); + // call base class run_async post checks + Worker::run_async(rt_prio); run_thread_.detach(); } -void App::request_stop() { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); - should_stop_ = true; +void App::perform_work_() { + bool success = true; + while (rclcpp::ok() && success && !should_stop_) { + success = app_ptr_->step(); // stuck if never connected, we thus detach the run_thread_ as join + // may never return + running_ = true; + if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); + break; + } + } + async_client_ptr_->get_state_interface()->uninitialize(); } bool App::valid_port_(const int &port_id) { diff --git a/lbr_fri_ros2/src/worker.cpp b/lbr_fri_ros2/src/worker.cpp new file mode 100644 index 00000000..ed8d0495 --- /dev/null +++ b/lbr_fri_ros2/src/worker.cpp @@ -0,0 +1,53 @@ +#include "lbr_fri_ros2/worker.hpp" + +namespace lbr_fri_ros2 { +Worker::Worker() : should_stop_(true), running_(false) {} + +Worker::~Worker() { + this->request_stop(); + if (run_thread_.joinable()) { + run_thread_.join(); + } +} + +void Worker::run_async(int rt_prio) { + if (running_) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::WARNING << "Worker already running" << ColorScheme::ENDC); + return; + } + run_thread_ = std::thread([this, rt_prio]() { + if (!realtime_tools::configure_sched_fifo(rt_prio)) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::WARNING + << "Failed to set FIFO realtime scheduling policy. Refer to " + "[https://control.ros.org/master/doc/ros2_control/" + "controller_manager/doc/userdoc.html]." + << ColorScheme::ENDC); + } else { + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKGREEN + << "Realtime scheduling policy set to FIFO with priority '" << rt_prio + << "'" << ColorScheme::ENDC); + } + + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); + should_stop_ = false; + + // perform work in child-class + this->perform_work_(); + // perform work end + + running_ = false; + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); + }); +} + +void Worker::request_stop() { + if (!running_) { + return; + } + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); + should_stop_ = true; +} +} // namespace lbr_fri_ros2 From 74afbb89877c57392d79302dcc2cb3fc3ef68a4c Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 15:20:41 +0100 Subject: [PATCH 2/2] removed redundant members --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index e1fa36a9..6e2f3f52 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -1,13 +1,10 @@ #ifndef LBR_FRI_ROS2__APP_HPP_ #define LBR_FRI_ROS2__APP_HPP_ -#include #include -#include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" -#include "realtime_tools/thread_priority.hpp" #include "friClientApplication.h" #include "friUdpConnection.h" @@ -40,9 +37,6 @@ class App : public Worker { void perform_work_() override; bool valid_port_(const int &port_id); - std::atomic_bool should_stop_, running_; - std::thread run_thread_; - std::shared_ptr async_client_ptr_; std::unique_ptr connection_ptr_; std::unique_ptr app_ptr_;