Add minimal_data_sharing package

Author: carlos-apex

README.md

@@ -3,6 +3,8 @@
 Collection of ROS 2 minimal examples showing how to leverage different POSIX and ROS 2 features to
 create real-time applications.
 
+* [minimal_data_sharing](minimal_data_sharing/README.md): shows how to use different data sharing approached to avoid
+ blocking calls when sharing data between real-time and non real-time threads 
 
 TODO: Add other packages after review (currently in rolling-experimental branch): 
 * minimal_memory_lock: shows how to lock the process memory and
@@ -15,7 +17,6 @@ does not allocate dynamic memory
 * minimal_memory_allocator: shows how to use different memory strategies to avoid dynamic
  memory allocations during the runtime phase
 * minimal_loaned_messages: shows how to use loaned messages APIs
-* minimal_data_sharing: shows how to use different data sharing approached to avoid
  blocking calls when sharing data between real-time and non real-time threads 
 * minimal_dds_tuning: shows how to use DDS specific configurations to tune real-time
  related settings (TODO: add XML profiles for more DDS implementations)

minimal_data_sharing/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.5)
+project(minimal_data_sharing)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(iceoryx_utils REQUIRED)
+find_package(apex_containers REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(pendulum_msgs REQUIRED)
+
+add_executable(minimal_atomic_message minimal_atomic_message.cpp)
+ament_target_dependencies(minimal_atomic_message rclcpp std_msgs)
+
+add_executable(minimal_atomic_parameter minimal_atomic_parameter.cpp)
+ament_target_dependencies(minimal_atomic_parameter rclcpp std_msgs)
+
+add_executable(minimal_lock_free_queue_logging minimal_lock_free_queue_logging.cpp)
+ament_target_dependencies(minimal_lock_free_queue_logging
+    rclcpp std_msgs iceoryx_utils apex_containers)
+
+add_executable(minimal_lock_free_queue minimal_lock_free_queue.cpp)
+ament_target_dependencies(minimal_lock_free_queue rclcpp iceoryx_utils trajectory_msgs)
+
+add_executable(minimal_pendulum_controller minimal_pendulum_controller.cpp)
+ament_target_dependencies(minimal_pendulum_controller rclcpp iceoryx_utils trajectory_msgs pendulum_msgs)
+
+install(TARGETS
+  minimal_atomic_message
+  minimal_atomic_parameter
+  minimal_lock_free_queue
+  minimal_lock_free_queue_logging
+  minimal_pendulum_controller
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

minimal_data_sharing/README.md

@@ -0,0 +1,41 @@
+# minimal_data_sharing
+
+## How to run
+
+### minimal_atomic_message
+
+```bash
+$ ros2 run minimal_data_sharing minimal_atomic_message
+```
+
+### minimal_atomic_parameter
+
+```bash
+$ ros2 run minimal_data_sharing minimal_atomic_parameter
+```
+
+Reset the count parameter:
+
+```bash
+ros2 param set /minimal_publisher count 0
+```
+
+### minimal_lock_free_queue
+
+```bash
+$ ros2 run minimal_data_sharing minimal_lock_free_queue
+```
+
+### minimal_lock_free_queue_logging
+
+```bash
+$ ros2 run minimal_data_sharing minimal_lock_free_queue_logging
+```
+
+## Resources
+
+- Matthias Killat - Lock-free programming for real-time systems - Meeting C++ 2021
+([video](https://www.youtube.com/watch?v=j2AgjFSFgRc))
+- [Eclipse iceoryx hoofs overview](https://github.com/eclipse-iceoryx/iceoryx/tree/master/iceoryx_hoofs#concurrent)
+- Real time 101 - David Rowland & Fabian Renn Giles - Meeting C++ 2019
+([video](https://www.youtube.com/watch?v=ndeN983j_GQ))

minimal_data_sharing/minimal_atomic_message.cpp

@@ -0,0 +1,81 @@
+// Copyright 2022 Carlos San Vicente
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/u_int32.hpp"
+
+using namespace std::chrono_literals;
+
+static_assert(std::atomic<std_msgs::msg::UInt32>::is_always_lock_free);
+
+// Bigger data types are not lock free, this asserts would fail
+// static_assert(std::atomic<std_msgs::msg::String>::is_always_lock_free);
+
+class MinimalPublisher : public rclcpp::Node
+{
+public:
+  MinimalPublisher()
+  : Node("minimal_publisher"), msg_(std_msgs::msg::UInt32())
+  {
+    // Make the callback groups reentrant so they can run concurrently
+    auto reentrant_callback_group =
+      this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+    rclcpp::PublisherOptions options;
+    options.callback_group = reentrant_callback_group;
+    for (auto pub_index = 0; pub_index < 10; pub_index++) {
+      auto publisher = this->create_publisher<std_msgs::msg::UInt32>("topic", 10, options);
+      auto timer_callback =
+        [this, pub_index, publisher]() -> void {
+          auto msg = increment_message_data();
+          RCLCPP_INFO(this->get_logger(), "Publisher %d: '%d'", pub_index, msg.data);
+          publisher->publish(msg);
+        };
+      timers_.push_back(this->create_wall_timer(100ms, timer_callback, reentrant_callback_group));
+    }
+  }
+
+  std_msgs::msg::UInt32 increment_message_data()
+  {
+    auto old_value = msg_.load();
+    auto new_value = old_value;
+    new_value.data++;
+    do {
+      if (msg_.compare_exchange_strong(old_value, new_value)) {
+        break;
+      }
+    } while(true);
+    return new_value;
+  }
+
+private:
+  std::vector<rclcpp::TimerBase::SharedPtr> timers_;
+  std::atomic<std_msgs::msg::UInt32> msg_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<MinimalPublisher>();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+  rclcpp::shutdown();
+  return 0;
+}

minimal_data_sharing/minimal_atomic_parameter.cpp

@@ -0,0 +1,94 @@
+// Copyright 2022 Carlos San Vicente
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+static_assert(std::atomic<std::uint32_t>::is_always_lock_free);
+
+class MinimalPublisher : public rclcpp::Node
+{
+public:
+  MinimalPublisher()
+  : Node("minimal_publisher"), count_(0)
+  {
+    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
+    auto timer_callback =
+      [this]() -> void {
+        auto message = std_msgs::msg::String();
+        message.data = "Hello, world! " + std::to_string(this->count_++);
+        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
+        this->publisher_->publish(message);
+      };
+
+    realtime_callback_group_ = this->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    timer_ = this->create_wall_timer(500ms, timer_callback, realtime_callback_group_);
+
+    count_ = declare_parameter("count", 0);
+    parameter_event_handler_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
+    auto param_callback = [this](const rclcpp::Parameter & p) {
+        RCLCPP_INFO(this->get_logger(), "Count changed to: '%ld'", p.as_int());
+        count_.store(static_cast<std::uint32_t>(p.as_int()));
+      };
+    parameter_callback_handle_ =
+      parameter_event_handler_->add_parameter_callback("count", param_callback);
+  }
+
+  rclcpp::CallbackGroup::SharedPtr get_realtime_callback_group()
+  {
+    return realtime_callback_group_;
+  }
+
+private:
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  std::atomic<std::uint32_t> count_;
+  rclcpp::CallbackGroup::SharedPtr realtime_callback_group_;
+  std::shared_ptr<rclcpp::ParameterEventHandler> parameter_event_handler_;
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> parameter_callback_handle_;
+};
+
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<MinimalPublisher>();
+  rclcpp::executors::StaticSingleThreadedExecutor default_callback_group_executor;
+  rclcpp::executors::StaticSingleThreadedExecutor realtime_executor;
+
+  default_callback_group_executor.add_node(node);
+  realtime_executor.add_callback_group(
+    node->get_realtime_callback_group(), node->get_node_base_interface());
+
+  // spin real-time tasks in a separate thread
+  auto realtime_thread = std::thread(
+    [&]() {
+      realtime_executor.spin();
+    });
+
+  default_callback_group_executor.spin();
+  realtime_thread.join();
+
+  rclcpp::shutdown();
+  return 0;
+}