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 <mutex>
-#include <string>
-
-namespace realtime_tools
-{
-/*!
-
-  Strongly suggested that you use an std::shared_ptr in this box to
-  guarantee realtime safety.
-
- */
-template <class T>
-class RealtimeBox
-{
-public:
-  explicit RealtimeBox(const T & initial = T()) : thing_(initial) {}
-
-  void set(const T & value)
-  {
-    std::lock_guard<std::mutex> guard(thing_lock_RT_);
-    thing_ = value;
-  }
-
-  void get(T & ref)
-  {
-    std::lock_guard<std::mutex> 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 <mutex>
+#include <string>
+
+namespace realtime_tools
+{
+/*!
+
+  Strongly suggested that you use an std::shared_ptr in this box to
+  guarantee realtime safety.
+
+ */
+template <class T>
+class RealtimeBox
+{
+public:
+  explicit RealtimeBox(const T & initial = T()) : thing_(initial) {}
+
+  void set(const T & value)
+  {
+    std::lock_guard<std::mutex> guard(thing_lock_RT_);
+    thing_ = value;
+  }
+
+  void get(T & ref)
+  {
+    std::lock_guard<std::mutex> 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 <chrono>
-#include <mutex>
-#include <thread>
-
-namespace realtime_tools
-{
-template <class T>
-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<std::mutex> 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<std::mutex> 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<std::mutex> guard(mutex_);
-#else
-    std::unique_lock<std::mutex> 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 <chrono>
+#include <mutex>
+#include <thread>
+
+namespace realtime_tools
+{
+template <class T>
+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<std::mutex> 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<std::mutex> 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<std::mutex> guard(mutex_);
+#else
+    std::unique_lock<std::mutex> 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 <mutex>
-#include <thread>
-
-#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 <mutex>
+#include <thread>
+
+#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 <atomic>
-#include <chrono>
-#include <condition_variable>
-#include <memory>
-#include <mutex>
-#include <string>
-#include <thread>
-
-#include "rclcpp/publisher.hpp"
-
-namespace realtime_tools
-{
-template <class Msg>
-class RealtimePublisher
-{
-private:
-  using PublisherSharedPtr = typename rclcpp::Publisher<Msg>::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<std::mutex> 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<bool> is_running_;
-  std::atomic<bool> 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<int> turn_;  // Who's turn is it to use msg_?
-};
-
-template <class Msg>
-using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg>>;
+#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 <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include "rclcpp/publisher.hpp"
+
+namespace realtime_tools
+{
+template <class Msg>
+class RealtimePublisher
+{
+private:
+  using PublisherSharedPtr = typename rclcpp::Publisher<Msg>::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<std::mutex> 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<bool> is_running_;
+  std::atomic<bool> 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<int> turn_;  // Who's turn is it to use msg_?
+};
+
+template <class Msg>
+using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg>>;
+
+}  // 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 <memory>
-
-#include "rclcpp/exceptions.hpp"
-#include "rclcpp/logging.hpp"
-#include "rclcpp_action/server_goal_handle.hpp"
-
-namespace realtime_tools
-{
-template <class Action>
-class RealtimeServerGoalHandle
-{
-private:
-  using GoalHandle = rclcpp_action::ServerGoalHandle<Action>;
-  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<GoalHandle> 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<GoalHandle> & 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<GoalHandle> & 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<std::mutex> 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<std::mutex> 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<std::mutex> guard(mutex_);
-
-      req_result_ = result;
-      req_succeed_ = true;
-    }
-  }
-
-  void setFeedback(FeedbackSharedPtr feedback = nullptr)
-  {
-    std::lock_guard<std::mutex> guard(mutex_);
-    req_feedback_ = feedback;
-  }
-
-  void execute()
-  {
-    if (!req_succeed_ && !req_abort_ && !req_cancel_) {
-      std::lock_guard<std::mutex> guard(mutex_);
-      req_execute_ = true;
-    }
-  }
-
-  bool valid() { return nullptr != gh_.get(); }
-
-  void runNonRealtime()
-  {
-    if (!valid()) {
-      return;
-    }
-
-    std::lock_guard<std::mutex> 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 <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp_action/server_goal_handle.hpp"
+
+namespace realtime_tools
+{
+template <class Action>
+class RealtimeServerGoalHandle
+{
+private:
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Action>;
+  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<GoalHandle> 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<GoalHandle> & 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<GoalHandle> & 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<std::mutex> 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<std::mutex> 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<std::mutex> guard(mutex_);
+
+      req_result_ = result;
+      req_succeed_ = true;
+    }
+  }
+
+  void setFeedback(FeedbackSharedPtr feedback = nullptr)
+  {
+    std::lock_guard<std::mutex> guard(mutex_);
+    req_feedback_ = feedback;
+  }
+
+  void execute()
+  {
+    if (!req_succeed_ && !req_abort_ && !req_cancel_) {
+      std::lock_guard<std::mutex> guard(mutex_);
+      req_execute_ = true;
+    }
+  }
+
+  bool valid() { return nullptr != gh_.get(); }
+
+  void runNonRealtime()
+  {
+    if (!valid()) {
+      return;
+    }
+
+    std::lock_guard<std::mutex> 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 <array>
 
-#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 <gmock/gmock.h>
-#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_buffer.hpp>
 
 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 <thread>
 
 #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;