diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ebf3264..04db7c88 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,6 +46,7 @@ add_library(urcl SHARED src/rtde/rtde_writer.cpp src/default_log_handler.cpp src/log.cpp + src/helpers.cpp ) add_library(ur_client_library::urcl ALIAS urcl) target_compile_options(urcl PRIVATE -Wall -Wextra -Wno-unused-parameter) diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 5120be36..03e2e3a0 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -22,6 +22,7 @@ #include "ur_client_library/comm/package.h" #include "ur_client_library/log.h" +#include "ur_client_library/helpers.h" #include "ur_client_library/queue/readerwriterqueue.h" #include #include @@ -245,8 +246,15 @@ class Pipeline * \param name The pipeline's name * \param notifier The notifier to use */ - Pipeline(IProducer& producer, IConsumer* consumer, std::string name, INotifier& notifier) - : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false } + Pipeline(IProducer& producer, IConsumer* consumer, std::string name, INotifier& notifier, + const bool producer_fifo_scheduling = false) + : producer_(producer) + , consumer_(consumer) + , name_(name) + , notifier_(notifier) + , queue_{ 32 } + , running_{ false } + , producer_fifo_scheduling_(producer_fifo_scheduling) { } /*! @@ -257,8 +265,14 @@ class Pipeline * \param name The pipeline's name * \param notifier The notifier to use */ - Pipeline(IProducer& producer, std::string name, INotifier& notifier) - : producer_(producer), consumer_(nullptr), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false } + Pipeline(IProducer& producer, std::string name, INotifier& notifier, const bool producer_fifo_scheduling = false) + : producer_(producer) + , consumer_(nullptr) + , name_(name) + , notifier_(notifier) + , queue_{ 32 } + , running_{ false } + , producer_fifo_scheduling_(producer_fifo_scheduling) { } @@ -349,62 +363,18 @@ class Pipeline moodycamel::BlockingReaderWriterQueue> queue_; std::atomic running_; std::thread pThread_, cThread_; + bool producer_fifo_scheduling_; void runProducer() { URCL_LOG_DEBUG("Starting up producer"); - std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); - bool has_realtime; - realtime_file >> has_realtime; - if (has_realtime) + if (producer_fifo_scheduling_) { + pthread_t this_thread = pthread_self(); const int max_thread_priority = sched_get_priority_max(SCHED_FIFO); - if (max_thread_priority != -1) - { - // We'll operate on the currently running thread. - pthread_t this_thread = pthread_self(); - - // struct sched_param is used to store the scheduling priority - struct sched_param params; - - // We'll set the priority to the maximum. - params.sched_priority = max_thread_priority; - - int ret = pthread_setschedparam(this_thread, SCHED_FIFO, ¶ms); - if (ret != 0) - { - URCL_LOG_ERROR("Unsuccessful in setting producer thread realtime priority. Error code: %d", ret); - } - // Now verify the change in thread priority - int policy = 0; - ret = pthread_getschedparam(this_thread, &policy, ¶ms); - if (ret != 0) - { - std::cout << "Couldn't retrieve real-time scheduling paramers" << std::endl; - } - - // Check the correct policy was applied - if (policy != SCHED_FIFO) - { - URCL_LOG_ERROR("Producer thread: Scheduling is NOT SCHED_FIFO!"); - } - else - { - URCL_LOG_INFO("Producer thread: SCHED_FIFO OK"); - } - - // Print thread scheduling priority - URCL_LOG_INFO("Thread priority is %d", params.sched_priority); - } - else - { - URCL_LOG_ERROR("Could not get maximum thread priority for producer thread"); - } - } - else - { - URCL_LOG_WARN("No realtime capabilities found. Consider using a realtime system for better performance"); + setFiFoScheduling(this_thread, max_thread_priority); } + std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); std::vector> products; while (running_) { diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h new file mode 100644 index 00000000..06b9f6bc --- /dev/null +++ b/include/ur_client_library/helpers.h @@ -0,0 +1,38 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 FZI Forschungszentrum Informatik +// Created on behalf of Universal Robots A/S +// +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2022-12-15 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED +#define UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED + +#include + +namespace urcl{ + bool setFiFoScheduling(pthread_t& thread, const int priority); +} +#endif // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED diff --git a/src/helpers.cpp b/src/helpers.cpp new file mode 100644 index 00000000..a5a6c7f2 --- /dev/null +++ b/src/helpers.cpp @@ -0,0 +1,88 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 FZI Forschungszentrum Informatik +// Created on behalf of Universal Robots A/S +// +// 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. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2022-12-15 + * + */ +//---------------------------------------------------------------------- + +#include +#include + +#include +#include +#include + +namespace urcl +{ +bool setFiFoScheduling(pthread_t& thread, const int priority) +{ + struct sched_param params; + params.sched_priority = priority; + int ret = pthread_setschedparam(thread, SCHED_FIFO, ¶ms); + if (ret != 0) + { + switch (ret) + { + case EPERM: + { + URCL_LOG_ERROR("Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency " + "kernel with FIFO scheduling. See " + "https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/" + "doc/real_time.md for details."); + break; + } + default: + + { + URCL_LOG_ERROR("Unsuccessful in setting thread to FIFO scheduling with priority %i. %s", priority, + strerror(ret)); + } + } + } + // Now verify the change in thread priority + int policy = 0; + ret = pthread_getschedparam(thread, &policy, ¶ms); + if (ret != 0) + { + URCL_LOG_ERROR("Couldn't retrieve scheduling parameters"); + return false; + } + + // Check the correct policy was applied + if (policy != SCHED_FIFO) + { + URCL_LOG_ERROR("Scheduling is NOT SCHED_FIFO!"); + return false; + } + else + { + URCL_LOG_INFO("SCHED_FIFO OK, priority %i", params.sched_priority); + if (params.sched_priority != priority) + { + return false; + } + } + return true; +} +} // namespace urcl diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 52934cd5..e58ecbca 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -41,7 +41,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , input_recipe_(readRecipe(input_recipe_file)) , parser_(output_recipe_) , prod_(stream_, parser_) - , pipeline_(prod_, PIPELINE_NAME, notifier) + , pipeline_(prod_, PIPELINE_NAME, notifier, true) , writer_(&stream_, input_recipe_) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency)