Demo for callback-group-level executor concept. (#302)

* Demo for callback-group-level executor concept.

Signed-off-by: Ralph Lange <[email protected]>

* Apply suggestions from code review

Co-authored-by: Karsten Knese <[email protected]>
Signed-off-by: Ralph Lange <[email protected]>

* Fixed CMake line length warning.

Signed-off-by: Ralph Lange <[email protected]>

* First attempt for support of macOS.

Signed-off-by: Ralph Lange <[email protected]>

* Fixed linter warnings.

Signed-off-by: Ralph Lange <[email protected]>

* Simplified #ifdefs for logger.

Signed-off-by: Ralph Lange <[email protected]>

* make compile on osx

Signed-off-by: Karsten Knese <[email protected]>

* Fixed includes so that CPPLINT.cfg can be removed and formatting of int64 in logging.

Signed-off-by: Ralph Lange <[email protected]>

* fix osx cpu affinity

Signed-off-by: Karsten Knese <[email protected]>

* duplicate main methods for simplicity

Signed-off-by: Karsten Knese <[email protected]>

* uncrustify

Signed-off-by: Karsten Knese <[email protected]>

* Ensure that both executor instance continue after the scheduling configuration.

Signed-off-by: Ralph Lange <[email protected]>

* wip, thread suspend

Signed-off-by: Karsten Knese <[email protected]>

* Improved documentation and cleaned up main functions.

Signed-off-by: Ralph Lange <[email protected]>

* Added note on failed core pinning under macOS.

Signed-off-by: Ralph Lange <[email protected]>

* Removed unused timestamp variables.

Signed-off-by: Ralph Lange <[email protected]>

* Added explicit cast from size_t to int32.

Signed-off-by: Ralph Lange <[email protected]>

Co-authored-by: Karsten Knese <[email protected]>

rclcpp/executors/cbg_executor/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5)
+project(examples_rclcpp_cbg_executor)
+
+# 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)
+
+add_executable(
+  ping
+  src/ping.cpp
+  src/examples_rclcpp_cbg_executor/ping_node.cpp
+)
+target_include_directories(ping PUBLIC include)
+ament_target_dependencies(ping rclcpp std_msgs)
+
+add_executable(
+  pong
+  src/pong.cpp
+  src/examples_rclcpp_cbg_executor/pong_node.cpp
+)
+target_include_directories(pong PUBLIC include)
+ament_target_dependencies(pong rclcpp std_msgs)
+
+add_executable(
+  ping_pong
+  src/ping_pong.cpp
+  src/examples_rclcpp_cbg_executor/ping_node.cpp
+  src/examples_rclcpp_cbg_executor/pong_node.cpp
+)
+target_include_directories(ping_pong PUBLIC include)
+ament_target_dependencies(ping_pong rclcpp std_msgs)
+
+install(TARGETS ping pong ping_pong
+  DESTINATION lib/${PROJECT_NAME}
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_dependencies(rclcpp std_msgs)
+ament_package()

rclcpp/executors/cbg_executor/README.md

@@ -0,0 +1,95 @@
+# examples_rclcpp_cbg_executor
+
+The *examples_rclcpp_cbg_executor* package provides a demo and test bench for the *Callback-group-level Executor* concept. This concept was developed in 2018 and has been integrated in ROS 2 mainline in 2020, i.e., is available from ROS 2 Galactic on. It does not add a new Executor but leverages callback groups for refining the Executor API to callback-group-level granularity.
+
+This allows a single node to have callbacks with different real-time requirements assigned to different Executor instances – within one process. Thus, an Executor instance can be dedicated to one or few specific callback groups and the Executor’s thread (or threads) can be prioritized according to the real-time requirements of these groups. For example, all critical callbacks may be handled by an Executor instance based on an thread running at the highest scheduler priority.
+
+## Introduction to demo
+
+The demo comprises a *Ping Node* and a *Pong Node* which exchange messages on two communication paths simultaneously. There is a high priority path formed by the topics *high_ping* and *high_pong* and a low priority path formed by *low_ping* and *low_pong*, respectively.
+
+![](doc/ping_pong_diagram.png)
+
+The Ping Node sends ping messages on both paths simultaneously at a configurable rate. The Pong Node takes these ping messages and replies each of them. Before sending a reply, it burns a configurable number of CPU cycles (thereby varying the processor load) to simulate some message processing.
+
+All callbacks of the Ping Node (i.e., for the timer for sending ping messages and for the two subscription on high_pong and low_pong) are handled in one callback group and thus Executor instance. However, the two callbacks of the Pong Node that process the incoming ping messages and answer with a pong message are assigned to two different callback groups. In the main function, these two groups are distributed to two Executor instances and threads. Both threads are pinned to the same CPU (No. 0) and thus share its processing power, but with different scheduler priorities following the names *high* and *low*.
+
+## Running the demo
+
+The Ping Node and Pong Node may be either started in one process or in two processes. Please note that on Linux the demo requires sudo privileges to be able to change the thread priorities using `pthread_setschedparam(..)`.
+
+Running the two nodes in one process:
+
+```bash
+sudo bash
+source /opt/ros/[ROS_DISTRO]/setup.bash
+ros2 run examples_rclcpp_cbg_executor ping_pong
+```
+
+Example of a typical output - note the zero pongs received on the low prio path:
+
+```
+[INFO] [..] [pong_node]: Running experiment from now on for 10s ...
+[INFO] [..] [ping_node]: Both paths: Sent out 982 of configured 1000 pings, i.e. 98%.
+[INFO] [..] [ping_node]: High prio path: Received 980 pongs, i.e. for 99% of the pings.
+[INFO] [..] [ping_node]: High prio path: Average RTT is 17.2ms.
+[INFO] [..] [ping_node]: Low prio path: Received 0 pongs, i.e. for 0% of the pings.
+[INFO] [..] [pong_node]: High priority executor thread ran for 9995ms.
+[INFO] [..] [pong_node]: Low priority executor thread ran for 0ms.
+```
+
+Running the two nodes in separate processes:
+
+```bash
+sudo bash
+source /opt/ros/[ROS_DISTRO]/setup.bash
+ros2 run examples_rclcpp_cbg_executor ping
+```
+
+```bash
+sudo bash
+source /opt/ros/[ROS_DISTRO]/setup.bash
+ros2 run examples_rclcpp_cbg_executor pong
+```
+
+The two processes should be started simultaneously as the experiment runtime is just 10 seconds.
+
+## Parameters
+
+There are three parameters to configure the experiment:
+
+* `ping_period` - period (double value in seconds) for sending out pings on the topics high_ping and low_ping simultaneously in the Ping Node.
+* `high_busyloop` - duration (double value in seconds) for burning CPU cycles on receiving a message from high_ping in the Pong Node.
+* `low_busyloop` - duration (double value in seconds) for burning CPU cycles on receiving a message from low_ping in the Pong Node.
+
+The default values are 0.01 seconds for all three parameters.
+
+Example for changing the values on the command line:
+
+```bash
+ros2 run examples_rclcpp_cbg_executor ping_pong --ros-args -p ping_period:=0.033 -p high_busyloop:=0.025
+```
+
+With these values, about (0.033s - 0.025s) / 0.010s = 80% of the ping messages on the low prio path should be processed and answered by a pong message:
+
+```
+...
+[INFO] [..] [ping_node]: Both paths: Sent out 303 of configured 303 pings, i.e. 100%.
+[INFO] [..] [ping_node]: High prio path: Received 302 pongs, i.e. for 99% of the pings.
+[INFO] [..] [ping_node]: High prio path: Average RTT is 25.2ms.
+[INFO] [..] [ping_node]: Low prio path: Received 231 pongs, i.e. for 76% of the pings.
+[INFO] [..] [ping_node]: Low prio path: Average RTT is 196.1ms.
+...
+```
+
+## Implementation details
+
+The Ping Node and the Pong Node are implemented in two classes `PingNode` (see [ping_node.hpp](include/examples_rclcpp_cbg_executor/ping_node.hpp)) and `PongNode` (see [pong_node.hpp](include/examples_rclcpp_cbg_executor/pong_node.hpp)), respectively. In addition to the mentioned timer and subscriptions, the PingNode class provides a function `print_statistics()` to print statistics on the number of sent and received messages on each path and the average round trip times. To burn the specified number of CPU cycles, the PongNode class contains a function `burn_cpu_cycles(duration)` to simulate a given processing time before replying with a pong.
+
+The Ping and Pong nodes, the two executors, etc. are composed and configured in the `main(..)` function of [main.cpp](src/main.cpp). This function also starts and ends the experiment for a duration of 10 seconds and prints out the throughput and round trip time (RTT) statistics.
+
+The demo also runs on Windows, where the two threads are prioritized as *above normal* and *below normal*, respectively, which does not require elevated privileges. When running the demo on Linux without sudo privileges, a warning is shown but the execution is not stopped.
+
+## Known issues
+
+On macOS the core pinning failed silently in our experiments. Please see the function `configure_native_thread(..)` in [utilities.hpp](src/examples_rclcpp_cbg_executor/utilities.hpp) for details.

rclcpp/executors/cbg_executor/include/examples_rclcpp_cbg_executor/ping_node.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2020 Robert Bosch GmbH
+//
+// 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.
+
+#ifndef EXAMPLES_RCLCPP_CBG_EXECUTOR__PING_NODE_HPP_
+#define EXAMPLES_RCLCPP_CBG_EXECUTOR__PING_NODE_HPP_
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+
+namespace examples_rclcpp_cbg_executor
+{
+
+struct RTTData
+{
+  explicit RTTData(const rclcpp::Time & sent)
+  : sent_(sent) {}
+  rclcpp::Time sent_{0, 0};
+  rclcpp::Time high_received_{0, 0};
+  rclcpp::Time low_received_{0, 0};
+};
+
+class PingNode : public rclcpp::Node
+{
+public:
+  PingNode();
+
+  virtual ~PingNode() = default;
+
+  void print_statistics(std::chrono::seconds experiment_duration) const;
+
+private:
+  rclcpp::TimerBase::SharedPtr ping_timer_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr high_ping_publisher_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr low_ping_publisher_;
+  void send_ping();
+
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr high_pong_subscription_;
+  void high_pong_received(const std_msgs::msg::Int32::SharedPtr msg);
+
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr low_pong_subscription_;
+  void low_pong_received(const std_msgs::msg::Int32::SharedPtr msg);
+
+  std::vector<RTTData> rtt_data_;
+};
+
+}  // namespace examples_rclcpp_cbg_executor
+
+#endif  // EXAMPLES_RCLCPP_CBG_EXECUTOR__PING_NODE_HPP_

rclcpp/executors/cbg_executor/include/examples_rclcpp_cbg_executor/pong_node.hpp

@@ -0,0 +1,53 @@
+// Copyright (c) 2020 Robert Bosch GmbH
+//
+// 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.
+
+#ifndef EXAMPLES_RCLCPP_CBG_EXECUTOR__PONG_NODE_HPP_
+#define EXAMPLES_RCLCPP_CBG_EXECUTOR__PONG_NODE_HPP_
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+
+namespace examples_rclcpp_cbg_executor
+{
+
+class PongNode : public rclcpp::Node
+{
+public:
+  PongNode();
+
+  virtual ~PongNode() = default;
+
+  rclcpp::CallbackGroup::SharedPtr get_high_prio_callback_group();
+
+  rclcpp::CallbackGroup::SharedPtr get_low_prio_callback_group();
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr high_ping_subscription_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr high_pong_publisher_;
+  void high_ping_received(const std_msgs::msg::Int32::SharedPtr msg);
+
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr low_ping_subscription_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr low_pong_publisher_;
+  void low_ping_received(const std_msgs::msg::Int32::SharedPtr msg);
+
+  static void burn_cpu_cycles(std::chrono::nanoseconds duration);
+};
+
+}  // namespace examples_rclcpp_cbg_executor
+
+#endif  // EXAMPLES_RCLCPP_CBG_EXECUTOR__PONG_NODE_HPP_

rclcpp/executors/cbg_executor/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>examples_rclcpp_cbg_executor</name>
+  <version>0.10.2</version>
+  <description>Example for multiple Executor instances in one process, using the callback-group-level interface of the Executor class.</description>
+  <maintainer email="[email protected]">Ralph Lange</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>