From 29b0d396e3eaa4a38ab70ee902432ff53164f296 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 27 Nov 2024 09:50:24 +0100 Subject: [PATCH] Move the header files to `.hpp` extension (#206) --- include/realtime_tools/realtime_box.h | 83 +------ include/realtime_tools/realtime_box.hpp | 77 ++++++ include/realtime_tools/realtime_buffer.h | 181 +------------- include/realtime_tools/realtime_buffer.hpp | 179 ++++++++++++++ include/realtime_tools/realtime_clock.h | 103 +------- include/realtime_tools/realtime_clock.hpp | 101 ++++++++ include/realtime_tools/realtime_publisher.h | 223 +----------------- include/realtime_tools/realtime_publisher.hpp | 221 +++++++++++++++++ .../realtime_server_goal_handle.h | 176 +------------- .../realtime_server_goal_handle.hpp | 174 ++++++++++++++ test/realtime_box_tests.cpp | 2 +- test/realtime_buffer_tests.cpp | 2 +- test/realtime_clock_tests.cpp | 2 +- test/realtime_publisher_tests.cpp | 2 +- test/realtime_publisher_tests_non_polling.cpp | 2 +- test/realtime_server_goal_handle_tests.cpp | 2 +- 16 files changed, 815 insertions(+), 715 deletions(-) create mode 100644 include/realtime_tools/realtime_box.hpp create mode 100644 include/realtime_tools/realtime_buffer.hpp create mode 100644 include/realtime_tools/realtime_clock.hpp create mode 100644 include/realtime_tools/realtime_publisher.hpp create mode 100644 include/realtime_tools/realtime_server_goal_handle.hpp diff --git a/include/realtime_tools/realtime_box.h b/include/realtime_tools/realtime_box.h index c5fc3002..aa45280e 100644 --- a/include/realtime_tools/realtime_box.h +++ b/include/realtime_tools/realtime_box.h @@ -1,77 +1,20 @@ -// Copyright (c) 2009, Willow Garage, Inc. +// Copyright 2024 ros2_control Development Team // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// 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 // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// Author: Stuart Glaser - -#ifndef REALTIME_TOOLS__REALTIME_BOX_H__ -#define REALTIME_TOOLS__REALTIME_BOX_H__ - -#include -#include - -namespace realtime_tools -{ -/*! - - Strongly suggested that you use an std::shared_ptr in this box to - guarantee realtime safety. - - */ -template -class RealtimeBox -{ -public: - explicit RealtimeBox(const T & initial = T()) : thing_(initial) {} - - void set(const T & value) - { - std::lock_guard guard(thing_lock_RT_); - thing_ = value; - } - - void get(T & ref) - { - std::lock_guard guard(thing_lock_RT_); - ref = thing_; - } - -private: - // The thing that's in the box. - T thing_; +// 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. - // Protects access to the thing in the box. This mutex is - // guaranteed to be locked for no longer than the duration of the - // copy, so as long as the copy is realtime safe and the OS has - // priority inheritance for mutexes, this lock can be safely locked - // from within realtime. - std::mutex thing_lock_RT_; -}; +#ifndef REALTIME_TOOLS__REALTIME_BOX_H_ +#define REALTIME_TOOLS__REALTIME_BOX_H_ -} // namespace realtime_tools +#include "realtime_tools/realtime_box.hpp" #endif // REALTIME_TOOLS__REALTIME_BOX_H_ diff --git a/include/realtime_tools/realtime_box.hpp b/include/realtime_tools/realtime_box.hpp new file mode 100644 index 00000000..c5fc3002 --- /dev/null +++ b/include/realtime_tools/realtime_box.hpp @@ -0,0 +1,77 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: Stuart Glaser + +#ifndef REALTIME_TOOLS__REALTIME_BOX_H__ +#define REALTIME_TOOLS__REALTIME_BOX_H__ + +#include +#include + +namespace realtime_tools +{ +/*! + + Strongly suggested that you use an std::shared_ptr in this box to + guarantee realtime safety. + + */ +template +class RealtimeBox +{ +public: + explicit RealtimeBox(const T & initial = T()) : thing_(initial) {} + + void set(const T & value) + { + std::lock_guard guard(thing_lock_RT_); + thing_ = value; + } + + void get(T & ref) + { + std::lock_guard guard(thing_lock_RT_); + ref = thing_; + } + +private: + // The thing that's in the box. + T thing_; + + // Protects access to the thing in the box. This mutex is + // guaranteed to be locked for no longer than the duration of the + // copy, so as long as the copy is realtime safe and the OS has + // priority inheritance for mutexes, this lock can be safely locked + // from within realtime. + std::mutex thing_lock_RT_; +}; + +} // namespace realtime_tools + +#endif // REALTIME_TOOLS__REALTIME_BOX_H_ diff --git a/include/realtime_tools/realtime_buffer.h b/include/realtime_tools/realtime_buffer.h index f624970a..63973f74 100644 --- a/include/realtime_tools/realtime_buffer.h +++ b/include/realtime_tools/realtime_buffer.h @@ -1,179 +1,20 @@ -// Copyright (c) 2013, hiDOF, Inc. +// Copyright 2024 ros2_control Development Team // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// 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 // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the hiDOF, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -/* - * Publishing ROS messages is difficult, as the publish function is - * not realtime safe. This class provides the proper locking so that - * you can call publish in realtime and a separate (non-realtime) - * thread will ensure that the message gets published over ROS. - * - * Author: Wim Meeussen - */ +// 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 REALTIME_TOOLS__REALTIME_BUFFER_H_ #define REALTIME_TOOLS__REALTIME_BUFFER_H_ -#include -#include -#include - -namespace realtime_tools -{ -template -class RealtimeBuffer -{ -public: - RealtimeBuffer() : new_data_available_(false) - { - // allocate memory - non_realtime_data_ = new T(); - realtime_data_ = new T(); - } - - /** - * @brief Constructor for objects that don't have - * a default constructor - * @param data The object to use as default value - */ - explicit RealtimeBuffer(const T & data) : new_data_available_(false) - { - // allocate memory - non_realtime_data_ = new T(data); - realtime_data_ = new T(data); - } - - ~RealtimeBuffer() - { - if (non_realtime_data_) { - delete non_realtime_data_; - } - if (realtime_data_) { - delete realtime_data_; - } - } - - RealtimeBuffer(const RealtimeBuffer & source) - { - // allocate memory - non_realtime_data_ = new T(); - realtime_data_ = new T(); - - // Copy the data from old RTB to new RTB - writeFromNonRT(*source.readFromNonRT()); - } - - /*! - * @brief Custom assignment operator - */ - RealtimeBuffer & operator=(const RealtimeBuffer & source) - { - if (this == &source) { - return *this; - } - - // Copy the data from old RTB to new RTB - writeFromNonRT(*source.readFromNonRT()); - - return *this; - } - - T * readFromRT() - { - // Check if the data is currently being written to (is locked) - std::unique_lock guard(mutex_, std::try_to_lock); - if (guard.owns_lock()) { - // swap pointers - if (new_data_available_) { - T * tmp = realtime_data_; - realtime_data_ = non_realtime_data_; - non_realtime_data_ = tmp; - new_data_available_ = false; - } - } - return realtime_data_; - } - - T * readFromNonRT() const - { - std::lock_guard guard(mutex_); - - if (new_data_available_) { - return non_realtime_data_; - } else { - return realtime_data_; - } - } - - void writeFromNonRT(const T & data) - { -#ifdef NON_POLLING - std::lock_guard guard(mutex_); -#else - std::unique_lock guard(mutex_, std::defer_lock); - while (!guard.try_lock()) { - std::this_thread::sleep_for(std::chrono::microseconds(500)); - } -#endif - - // copy data into non-realtime buffer - *non_realtime_data_ = data; - new_data_available_ = true; - } - - void initRT(const T & data) - { - *non_realtime_data_ = data; - *realtime_data_ = data; - } - - void reset() - { - // delete the old memory - if (non_realtime_data_) { - delete non_realtime_data_; - } - if (realtime_data_) { - delete realtime_data_; - } - - // allocate memory - non_realtime_data_ = new T(); - realtime_data_ = new T(); - } - -private: - T * realtime_data_; - T * non_realtime_data_; - bool new_data_available_; - - // Set as mutable so that readFromNonRT() can be performed on a const buffer - mutable std::mutex mutex_; -}; // class +#include "realtime_tools/realtime_buffer.hpp" -} // namespace realtime_tools #endif // REALTIME_TOOLS__REALTIME_BUFFER_H_ diff --git a/include/realtime_tools/realtime_buffer.hpp b/include/realtime_tools/realtime_buffer.hpp new file mode 100644 index 00000000..78c320fa --- /dev/null +++ b/include/realtime_tools/realtime_buffer.hpp @@ -0,0 +1,179 @@ +// Copyright (c) 2013, hiDOF, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/* + * Publishing ROS messages is difficult, as the publish function is + * not realtime safe. This class provides the proper locking so that + * you can call publish in realtime and a separate (non-realtime) + * thread will ensure that the message gets published over ROS. + * + * Author: Wim Meeussen + */ + +#ifndef REALTIME_TOOLS__REALTIME_BUFFER_HPP_ +#define REALTIME_TOOLS__REALTIME_BUFFER_HPP_ + +#include +#include +#include + +namespace realtime_tools +{ +template +class RealtimeBuffer +{ +public: + RealtimeBuffer() : new_data_available_(false) + { + // allocate memory + non_realtime_data_ = new T(); + realtime_data_ = new T(); + } + + /** + * @brief Constructor for objects that don't have + * a default constructor + * @param data The object to use as default value + */ + explicit RealtimeBuffer(const T & data) : new_data_available_(false) + { + // allocate memory + non_realtime_data_ = new T(data); + realtime_data_ = new T(data); + } + + ~RealtimeBuffer() + { + if (non_realtime_data_) { + delete non_realtime_data_; + } + if (realtime_data_) { + delete realtime_data_; + } + } + + RealtimeBuffer(const RealtimeBuffer & source) + { + // allocate memory + non_realtime_data_ = new T(); + realtime_data_ = new T(); + + // Copy the data from old RTB to new RTB + writeFromNonRT(*source.readFromNonRT()); + } + + /*! + * @brief Custom assignment operator + */ + RealtimeBuffer & operator=(const RealtimeBuffer & source) + { + if (this == &source) { + return *this; + } + + // Copy the data from old RTB to new RTB + writeFromNonRT(*source.readFromNonRT()); + + return *this; + } + + T * readFromRT() + { + // Check if the data is currently being written to (is locked) + std::unique_lock guard(mutex_, std::try_to_lock); + if (guard.owns_lock()) { + // swap pointers + if (new_data_available_) { + T * tmp = realtime_data_; + realtime_data_ = non_realtime_data_; + non_realtime_data_ = tmp; + new_data_available_ = false; + } + } + return realtime_data_; + } + + T * readFromNonRT() const + { + std::lock_guard guard(mutex_); + + if (new_data_available_) { + return non_realtime_data_; + } else { + return realtime_data_; + } + } + + void writeFromNonRT(const T & data) + { +#ifdef NON_POLLING + std::lock_guard guard(mutex_); +#else + std::unique_lock guard(mutex_, std::defer_lock); + while (!guard.try_lock()) { + std::this_thread::sleep_for(std::chrono::microseconds(500)); + } +#endif + + // copy data into non-realtime buffer + *non_realtime_data_ = data; + new_data_available_ = true; + } + + void initRT(const T & data) + { + *non_realtime_data_ = data; + *realtime_data_ = data; + } + + void reset() + { + // delete the old memory + if (non_realtime_data_) { + delete non_realtime_data_; + } + if (realtime_data_) { + delete realtime_data_; + } + + // allocate memory + non_realtime_data_ = new T(); + realtime_data_ = new T(); + } + +private: + T * realtime_data_; + T * non_realtime_data_; + bool new_data_available_; + + // Set as mutable so that readFromNonRT() can be performed on a const buffer + mutable std::mutex mutex_; +}; // class + +} // namespace realtime_tools +#endif // REALTIME_TOOLS__REALTIME_BUFFER_HPP_ diff --git a/include/realtime_tools/realtime_clock.h b/include/realtime_tools/realtime_clock.h index 4de04dd7..7633ff59 100644 --- a/include/realtime_tools/realtime_clock.h +++ b/include/realtime_tools/realtime_clock.h @@ -1,101 +1,20 @@ -// Copyright (c) 2013, hiDOF, Inc. +// Copyright 2024 ros2_control Development Team // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// 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 // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the hiDOF, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -/* - * Publishing ROS messages is difficult, as the publish function is - * not realtime safe. This class provides the proper locking so that - * you can call publish in realtime and a separate (non-realtime) - * thread will ensure that the message gets published over ROS. - * - * Author: Wim Meeussen - */ +// 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 REALTIME_TOOLS__REALTIME_CLOCK_H_ #define REALTIME_TOOLS__REALTIME_CLOCK_H_ -#include -#include - -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/time.hpp" - -namespace realtime_tools -{ -class RealtimeClock -{ -public: - /** - * Default constructor creates an instance that always returns zero time. - */ - RealtimeClock(); - - /** - * Create a realtime-safe wrapper around a clock object. - */ - explicit RealtimeClock(rclcpp::Clock::SharedPtr clock); - - /** - * Create a realtime-safe wrapper around a clock object with a specified logger. - */ - RealtimeClock(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger); - - ~RealtimeClock(); - - /** - * Get the current time from the clock. - * \deprecated use now() instead. - */ - [[deprecated]] rclcpp::Time getSystemTime(const rclcpp::Time & realtime_time = rclcpp::Time()); - - /** - * Get the current time from the clock. - * \return current time, or - * \return zero if RealtimeClock was not given a valid clock object or time is uninitialized. - */ - rclcpp::Time now(const rclcpp::Time & realtime_time = rclcpp::Time()); - -private: - void loop(); - - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_; - unsigned int lock_misses_ = 0; - rclcpp::Time system_time_; - rclcpp::Duration clock_offset_{0, 0u}; - - rclcpp::Time last_realtime_time_; - bool running_ = false; - bool initialized_ = false; - std::mutex mutex_; - std::thread thread_; -}; // class +#include "realtime_tools/realtime_clock.hpp" -} // namespace realtime_tools #endif // REALTIME_TOOLS__REALTIME_CLOCK_H_ diff --git a/include/realtime_tools/realtime_clock.hpp b/include/realtime_tools/realtime_clock.hpp new file mode 100644 index 00000000..2ff9bf4c --- /dev/null +++ b/include/realtime_tools/realtime_clock.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2013, hiDOF, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/* + * Publishing ROS messages is difficult, as the publish function is + * not realtime safe. This class provides the proper locking so that + * you can call publish in realtime and a separate (non-realtime) + * thread will ensure that the message gets published over ROS. + * + * Author: Wim Meeussen + */ + +#ifndef REALTIME_TOOLS__REALTIME_CLOCK_HPP_ +#define REALTIME_TOOLS__REALTIME_CLOCK_HPP_ + +#include +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/time.hpp" + +namespace realtime_tools +{ +class RealtimeClock +{ +public: + /** + * Default constructor creates an instance that always returns zero time. + */ + RealtimeClock(); + + /** + * Create a realtime-safe wrapper around a clock object. + */ + explicit RealtimeClock(rclcpp::Clock::SharedPtr clock); + + /** + * Create a realtime-safe wrapper around a clock object with a specified logger. + */ + RealtimeClock(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger); + + ~RealtimeClock(); + + /** + * Get the current time from the clock. + * \deprecated use now() instead. + */ + [[deprecated]] rclcpp::Time getSystemTime(const rclcpp::Time & realtime_time = rclcpp::Time()); + + /** + * Get the current time from the clock. + * \return current time, or + * \return zero if RealtimeClock was not given a valid clock object or time is uninitialized. + */ + rclcpp::Time now(const rclcpp::Time & realtime_time = rclcpp::Time()); + +private: + void loop(); + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + unsigned int lock_misses_ = 0; + rclcpp::Time system_time_; + rclcpp::Duration clock_offset_{0, 0u}; + + rclcpp::Time last_realtime_time_; + bool running_ = false; + bool initialized_ = false; + std::mutex mutex_; + std::thread thread_; +}; // class + +} // namespace realtime_tools +#endif // REALTIME_TOOLS__REALTIME_CLOCK_HPP_ diff --git a/include/realtime_tools/realtime_publisher.h b/include/realtime_tools/realtime_publisher.h index c720c403..bc1f1994 100644 --- a/include/realtime_tools/realtime_publisher.h +++ b/include/realtime_tools/realtime_publisher.h @@ -1,221 +1,20 @@ -// Copyright (c) 2008, Willow Garage, Inc. +// Copyright 2024 ros2_control Development Team // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// 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 // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// 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. -/* - * Publishing ROS messages is difficult, as the publish function is - * not realtime safe. This class provides the proper locking so that - * you can call publish in realtime and a separate (non-realtime) - * thread will ensure that the message gets published over ROS. - * - * Author: Stuart Glaser - */ #ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_H_ #define REALTIME_TOOLS__REALTIME_PUBLISHER_H_ -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/publisher.hpp" - -namespace realtime_tools -{ -template -class RealtimePublisher -{ -private: - using PublisherSharedPtr = typename rclcpp::Publisher::SharedPtr; - -public: - /// The msg_ variable contains the data that will get published on the ROS topic. - Msg msg_; - - /** \brief Constructor for the realtime publisher - * - * \param publisher the publisher to wrap - */ - explicit RealtimePublisher(PublisherSharedPtr publisher) - : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED) - { - thread_ = std::thread(&RealtimePublisher::publishingLoop, this); - } - - RealtimePublisher() : is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED) {} - - /// Destructor - ~RealtimePublisher() - { - stop(); - while (is_running()) { - std::this_thread::sleep_for(std::chrono::microseconds(100)); - } - if (thread_.joinable()) { - thread_.join(); - } - } - - /// Stop the realtime publisher from sending out more ROS messages - void stop() - { - keep_running_ = false; -#ifdef NON_POLLING - updated_cond_.notify_one(); // So the publishing loop can exit -#endif - } - - /** \brief Try to get the data lock from realtime - * - * To publish data from the realtime loop, you need to run trylock to - * attempt to get unique access to the msg_ variable. Trylock returns - * true if the lock was acquired, and false if it failed to get the lock. - */ - bool trylock() - { - if (msg_mutex_.try_lock()) { - if (turn_ == REALTIME) { - return true; - } else { - msg_mutex_.unlock(); - return false; - } - } else { - return false; - } - } - - /** \brief Unlock the msg_ variable - * - * After a successful trylock and after the data is written to the mgs_ - * variable, the lock has to be released for the message to get - * published on the specified topic. - */ - void unlockAndPublish() - { - turn_ = NON_REALTIME; - unlock(); - } - - /** \brief Get the data lock form non-realtime - * - * To publish data from the realtime loop, you need to run trylock to - * attempt to get unique access to the msg_ variable. Trylock returns - * true if the lock was acquired, and false if it failed to get the lock. - */ - void lock() - { -#ifdef NON_POLLING - msg_mutex_.lock(); -#else - // never actually block on the lock - while (!msg_mutex_.try_lock()) { - std::this_thread::sleep_for(std::chrono::microseconds(200)); - } -#endif - } - - /** \brief Unlocks the data without publishing anything - * - */ - void unlock() - { - msg_mutex_.unlock(); -#ifdef NON_POLLING - updated_cond_.notify_one(); -#endif - } - -private: - // non-copyable - RealtimePublisher(const RealtimePublisher &) = delete; - RealtimePublisher & operator=(const RealtimePublisher &) = delete; - - bool is_running() const { return is_running_; } - - void publishingLoop() - { - is_running_ = true; - turn_ = REALTIME; - - while (keep_running_) { - Msg outgoing; - - // Locks msg_ and copies it - -#ifdef NON_POLLING - std::unique_lock lock_(msg_mutex_); -#else - lock(); -#endif - - while (turn_ != NON_REALTIME && keep_running_) { -#ifdef NON_POLLING - updated_cond_.wait(lock_); -#else - unlock(); - std::this_thread::sleep_for(std::chrono::microseconds(500)); - lock(); -#endif - } - outgoing = msg_; - turn_ = REALTIME; - - unlock(); - - // Sends the outgoing message - if (keep_running_) { - publisher_->publish(outgoing); - } - } - is_running_ = false; - } - - PublisherSharedPtr publisher_; - std::atomic is_running_; - std::atomic keep_running_; - - std::thread thread_; - - std::mutex msg_mutex_; // Protects msg_ - -#ifdef NON_POLLING - std::condition_variable updated_cond_; -#endif - - enum { REALTIME, NON_REALTIME, LOOP_NOT_STARTED }; - std::atomic turn_; // Who's turn is it to use msg_? -}; - -template -using RealtimePublisherSharedPtr = std::shared_ptr>; +#include "realtime_tools/realtime_publisher.hpp" -} // namespace realtime_tools #endif // REALTIME_TOOLS__REALTIME_PUBLISHER_H_ diff --git a/include/realtime_tools/realtime_publisher.hpp b/include/realtime_tools/realtime_publisher.hpp new file mode 100644 index 00000000..ca004342 --- /dev/null +++ b/include/realtime_tools/realtime_publisher.hpp @@ -0,0 +1,221 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/* + * Publishing ROS messages is difficult, as the publish function is + * not realtime safe. This class provides the proper locking so that + * you can call publish in realtime and a separate (non-realtime) + * thread will ensure that the message gets published over ROS. + * + * Author: Stuart Glaser + */ +#ifndef REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_ +#define REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/publisher.hpp" + +namespace realtime_tools +{ +template +class RealtimePublisher +{ +private: + using PublisherSharedPtr = typename rclcpp::Publisher::SharedPtr; + +public: + /// The msg_ variable contains the data that will get published on the ROS topic. + Msg msg_; + + /** \brief Constructor for the realtime publisher + * + * \param publisher the publisher to wrap + */ + explicit RealtimePublisher(PublisherSharedPtr publisher) + : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED) + { + thread_ = std::thread(&RealtimePublisher::publishingLoop, this); + } + + RealtimePublisher() : is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED) {} + + /// Destructor + ~RealtimePublisher() + { + stop(); + while (is_running()) { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + if (thread_.joinable()) { + thread_.join(); + } + } + + /// Stop the realtime publisher from sending out more ROS messages + void stop() + { + keep_running_ = false; +#ifdef NON_POLLING + updated_cond_.notify_one(); // So the publishing loop can exit +#endif + } + + /** \brief Try to get the data lock from realtime + * + * To publish data from the realtime loop, you need to run trylock to + * attempt to get unique access to the msg_ variable. Trylock returns + * true if the lock was acquired, and false if it failed to get the lock. + */ + bool trylock() + { + if (msg_mutex_.try_lock()) { + if (turn_ == REALTIME) { + return true; + } else { + msg_mutex_.unlock(); + return false; + } + } else { + return false; + } + } + + /** \brief Unlock the msg_ variable + * + * After a successful trylock and after the data is written to the mgs_ + * variable, the lock has to be released for the message to get + * published on the specified topic. + */ + void unlockAndPublish() + { + turn_ = NON_REALTIME; + unlock(); + } + + /** \brief Get the data lock form non-realtime + * + * To publish data from the realtime loop, you need to run trylock to + * attempt to get unique access to the msg_ variable. Trylock returns + * true if the lock was acquired, and false if it failed to get the lock. + */ + void lock() + { +#ifdef NON_POLLING + msg_mutex_.lock(); +#else + // never actually block on the lock + while (!msg_mutex_.try_lock()) { + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } +#endif + } + + /** \brief Unlocks the data without publishing anything + * + */ + void unlock() + { + msg_mutex_.unlock(); +#ifdef NON_POLLING + updated_cond_.notify_one(); +#endif + } + +private: + // non-copyable + RealtimePublisher(const RealtimePublisher &) = delete; + RealtimePublisher & operator=(const RealtimePublisher &) = delete; + + bool is_running() const { return is_running_; } + + void publishingLoop() + { + is_running_ = true; + turn_ = REALTIME; + + while (keep_running_) { + Msg outgoing; + + // Locks msg_ and copies it + +#ifdef NON_POLLING + std::unique_lock lock_(msg_mutex_); +#else + lock(); +#endif + + while (turn_ != NON_REALTIME && keep_running_) { +#ifdef NON_POLLING + updated_cond_.wait(lock_); +#else + unlock(); + std::this_thread::sleep_for(std::chrono::microseconds(500)); + lock(); +#endif + } + outgoing = msg_; + turn_ = REALTIME; + + unlock(); + + // Sends the outgoing message + if (keep_running_) { + publisher_->publish(outgoing); + } + } + is_running_ = false; + } + + PublisherSharedPtr publisher_; + std::atomic is_running_; + std::atomic keep_running_; + + std::thread thread_; + + std::mutex msg_mutex_; // Protects msg_ + +#ifdef NON_POLLING + std::condition_variable updated_cond_; +#endif + + enum { REALTIME, NON_REALTIME, LOOP_NOT_STARTED }; + std::atomic turn_; // Who's turn is it to use msg_? +}; + +template +using RealtimePublisherSharedPtr = std::shared_ptr>; + +} // namespace realtime_tools +#endif // REALTIME_TOOLS__REALTIME_PUBLISHER_HPP_ diff --git a/include/realtime_tools/realtime_server_goal_handle.h b/include/realtime_tools/realtime_server_goal_handle.h index 1b035c3c..331ec4d9 100644 --- a/include/realtime_tools/realtime_server_goal_handle.h +++ b/include/realtime_tools/realtime_server_goal_handle.h @@ -1,174 +1,20 @@ -// Copyright (c) 2008, Willow Garage, Inc. +// Copyright 2024 ros2_control Development Team // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// 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 // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -/// \author Stuart Glaser +// 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 REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_ #define REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_ -#include - -#include "rclcpp/exceptions.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp_action/server_goal_handle.hpp" - -namespace realtime_tools -{ -template -class RealtimeServerGoalHandle -{ -private: - using GoalHandle = rclcpp_action::ServerGoalHandle; - using ResultSharedPtr = typename Action::Result::SharedPtr; - using FeedbackSharedPtr = typename Action::Feedback::SharedPtr; - - bool req_abort_; - bool req_cancel_; - bool req_succeed_; - bool req_execute_; - - std::mutex mutex_; - ResultSharedPtr req_result_; - FeedbackSharedPtr req_feedback_; - rclcpp::Logger logger_; - -public: - std::shared_ptr gh_; - ResultSharedPtr preallocated_result_; // Preallocated so it can be used in realtime - FeedbackSharedPtr preallocated_feedback_; // Preallocated so it can be used in realtime - - explicit RealtimeServerGoalHandle( - std::shared_ptr & gh, const ResultSharedPtr & preallocated_result = nullptr, - const FeedbackSharedPtr & preallocated_feedback = nullptr) - : RealtimeServerGoalHandle( - gh, preallocated_result, preallocated_feedback, rclcpp::get_logger("realtime_tools")) - { - } - - RealtimeServerGoalHandle( - std::shared_ptr & gh, const ResultSharedPtr & preallocated_result, - const FeedbackSharedPtr & preallocated_feedback, rclcpp::Logger logger) - : req_abort_(false), - req_cancel_(false), - req_succeed_(false), - req_execute_(false), - logger_(logger), - gh_(gh), - preallocated_result_(preallocated_result), - preallocated_feedback_(preallocated_feedback) - { - if (!preallocated_result_) { - preallocated_result_.reset(new typename Action::Result); - } - if (!preallocated_feedback_) { - preallocated_feedback_.reset(new typename Action::Feedback); - } - } - - void setAborted(ResultSharedPtr result = nullptr) - { - if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) { - std::lock_guard guard(mutex_); - - req_result_ = result; - req_abort_ = true; - } - } - - void setCanceled(ResultSharedPtr result = nullptr) - { - if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) { - std::lock_guard guard(mutex_); - - req_result_ = result; - req_cancel_ = true; - } - } - - void setSucceeded(ResultSharedPtr result = nullptr) - { - if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) { - std::lock_guard guard(mutex_); - - req_result_ = result; - req_succeed_ = true; - } - } - - void setFeedback(FeedbackSharedPtr feedback = nullptr) - { - std::lock_guard guard(mutex_); - req_feedback_ = feedback; - } - - void execute() - { - if (!req_succeed_ && !req_abort_ && !req_cancel_) { - std::lock_guard guard(mutex_); - req_execute_ = true; - } - } - - bool valid() { return nullptr != gh_.get(); } - - void runNonRealtime() - { - if (!valid()) { - return; - } - - std::lock_guard guard(mutex_); - - try { - if (req_execute_ && !gh_->is_executing() && gh_->is_active() && !gh_->is_canceling()) { - gh_->execute(); - } - if (req_abort_ && gh_->is_executing()) { - gh_->abort(req_result_); - req_abort_ = false; - } - if (req_cancel_ && gh_->is_active()) { - gh_->canceled(req_result_); - req_cancel_ = false; - } - if (req_succeed_ && !gh_->is_canceling()) { - gh_->succeed(req_result_); - req_succeed_ = false; - } - if (req_feedback_ && gh_->is_executing()) { - gh_->publish_feedback(req_feedback_); - } - } catch (const rclcpp::exceptions::RCLErrorBase & e) { - // Likely invalid state transition - RCLCPP_WARN(logger_, "%s", e.formatted_message.c_str()); - } - } -}; +#include "realtime_tools/realtime_server_goal_handle.hpp" -} // namespace realtime_tools #endif // REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_ diff --git a/include/realtime_tools/realtime_server_goal_handle.hpp b/include/realtime_tools/realtime_server_goal_handle.hpp new file mode 100644 index 00000000..1b035c3c --- /dev/null +++ b/include/realtime_tools/realtime_server_goal_handle.hpp @@ -0,0 +1,174 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/// \author Stuart Glaser + +#ifndef REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_ +#define REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_ + +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp_action/server_goal_handle.hpp" + +namespace realtime_tools +{ +template +class RealtimeServerGoalHandle +{ +private: + using GoalHandle = rclcpp_action::ServerGoalHandle; + using ResultSharedPtr = typename Action::Result::SharedPtr; + using FeedbackSharedPtr = typename Action::Feedback::SharedPtr; + + bool req_abort_; + bool req_cancel_; + bool req_succeed_; + bool req_execute_; + + std::mutex mutex_; + ResultSharedPtr req_result_; + FeedbackSharedPtr req_feedback_; + rclcpp::Logger logger_; + +public: + std::shared_ptr gh_; + ResultSharedPtr preallocated_result_; // Preallocated so it can be used in realtime + FeedbackSharedPtr preallocated_feedback_; // Preallocated so it can be used in realtime + + explicit RealtimeServerGoalHandle( + std::shared_ptr & gh, const ResultSharedPtr & preallocated_result = nullptr, + const FeedbackSharedPtr & preallocated_feedback = nullptr) + : RealtimeServerGoalHandle( + gh, preallocated_result, preallocated_feedback, rclcpp::get_logger("realtime_tools")) + { + } + + RealtimeServerGoalHandle( + std::shared_ptr & gh, const ResultSharedPtr & preallocated_result, + const FeedbackSharedPtr & preallocated_feedback, rclcpp::Logger logger) + : req_abort_(false), + req_cancel_(false), + req_succeed_(false), + req_execute_(false), + logger_(logger), + gh_(gh), + preallocated_result_(preallocated_result), + preallocated_feedback_(preallocated_feedback) + { + if (!preallocated_result_) { + preallocated_result_.reset(new typename Action::Result); + } + if (!preallocated_feedback_) { + preallocated_feedback_.reset(new typename Action::Feedback); + } + } + + void setAborted(ResultSharedPtr result = nullptr) + { + if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) { + std::lock_guard guard(mutex_); + + req_result_ = result; + req_abort_ = true; + } + } + + void setCanceled(ResultSharedPtr result = nullptr) + { + if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) { + std::lock_guard guard(mutex_); + + req_result_ = result; + req_cancel_ = true; + } + } + + void setSucceeded(ResultSharedPtr result = nullptr) + { + if (req_execute_ && !req_succeed_ && !req_abort_ && !req_cancel_) { + std::lock_guard guard(mutex_); + + req_result_ = result; + req_succeed_ = true; + } + } + + void setFeedback(FeedbackSharedPtr feedback = nullptr) + { + std::lock_guard guard(mutex_); + req_feedback_ = feedback; + } + + void execute() + { + if (!req_succeed_ && !req_abort_ && !req_cancel_) { + std::lock_guard guard(mutex_); + req_execute_ = true; + } + } + + bool valid() { return nullptr != gh_.get(); } + + void runNonRealtime() + { + if (!valid()) { + return; + } + + std::lock_guard guard(mutex_); + + try { + if (req_execute_ && !gh_->is_executing() && gh_->is_active() && !gh_->is_canceling()) { + gh_->execute(); + } + if (req_abort_ && gh_->is_executing()) { + gh_->abort(req_result_); + req_abort_ = false; + } + if (req_cancel_ && gh_->is_active()) { + gh_->canceled(req_result_); + req_cancel_ = false; + } + if (req_succeed_ && !gh_->is_canceling()) { + gh_->succeed(req_result_); + req_succeed_ = false; + } + if (req_feedback_ && gh_->is_executing()) { + gh_->publish_feedback(req_feedback_); + } + } catch (const rclcpp::exceptions::RCLErrorBase & e) { + // Likely invalid state transition + RCLCPP_WARN(logger_, "%s", e.formatted_message.c_str()); + } + } +}; + +} // namespace realtime_tools +#endif // REALTIME_TOOLS__REALTIME_SERVER_GOAL_HANDLE_H_ diff --git a/test/realtime_box_tests.cpp b/test/realtime_box_tests.cpp index 73067664..edd58d94 100644 --- a/test/realtime_box_tests.cpp +++ b/test/realtime_box_tests.cpp @@ -30,7 +30,7 @@ #include -#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_box.hpp" using realtime_tools::RealtimeBox; diff --git a/test/realtime_buffer_tests.cpp b/test/realtime_buffer_tests.cpp index 90b7a79c..5ba3839b 100644 --- a/test/realtime_buffer_tests.cpp +++ b/test/realtime_buffer_tests.cpp @@ -27,7 +27,7 @@ // POSSIBILITY OF SUCH DAMAGE. #include -#include +#include using realtime_tools::RealtimeBuffer; diff --git a/test/realtime_clock_tests.cpp b/test/realtime_clock_tests.cpp index 43be3370..c7f3e572 100644 --- a/test/realtime_clock_tests.cpp +++ b/test/realtime_clock_tests.cpp @@ -32,7 +32,7 @@ #include #include "rclcpp/utilities.hpp" -#include "realtime_tools/realtime_clock.h" +#include "realtime_tools/realtime_clock.hpp" using realtime_tools::RealtimeClock; diff --git a/test/realtime_publisher_tests.cpp b/test/realtime_publisher_tests.cpp index 2a2658d7..d560b93c 100644 --- a/test/realtime_publisher_tests.cpp +++ b/test/realtime_publisher_tests.cpp @@ -36,7 +36,7 @@ #include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "test_msgs/msg/strings.hpp" using StringMsg = test_msgs::msg::Strings; diff --git a/test/realtime_publisher_tests_non_polling.cpp b/test/realtime_publisher_tests_non_polling.cpp index 92194f69..001e796b 100644 --- a/test/realtime_publisher_tests_non_polling.cpp +++ b/test/realtime_publisher_tests_non_polling.cpp @@ -38,7 +38,7 @@ #include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "test_msgs/msg/strings.hpp" using StringMsg = test_msgs::msg::Strings; diff --git a/test/realtime_server_goal_handle_tests.cpp b/test/realtime_server_goal_handle_tests.cpp index 69631d35..2cd7e537 100644 --- a/test/realtime_server_goal_handle_tests.cpp +++ b/test/realtime_server_goal_handle_tests.cpp @@ -39,7 +39,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_action/create_server.hpp" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_server_goal_handle.hpp" #include "test_msgs/action/fibonacci.hpp" using test_msgs::action::Fibonacci;