Merge pull request #8 from microROS/feature/cbg-executor_ping-pong

cbg-executor ping-pong demo

Author: ralph-lange

Cpp/cbg-executor_ping-pong/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.5)
+project(cbg-executor_ping-pong_cpp)
+
+# 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)
+
+include_directories(include)
+
+add_executable(ping-pong main.cpp src/PingSubNode.cpp src/PongSubNode.cpp)
+ament_target_dependencies(ping-pong rclcpp std_msgs)
+
+install(TARGETS ping-pong
+  DESTINATION bin/${PROJECT_NAME}
+)
+
+ament_package()

Cpp/cbg-executor_ping-pong/README.md

@@ -0,0 +1,58 @@
+# cbg-executor_ping-pong_cpp
+
+This package provides a small example for the use of the Callback-group-level Executor concept.
+
+The Callback-group-level Executor leverages the callback-group concept in rclcpp by introducing real-time profiles such as RT-CRITICAL and BEST-EFFORT in the callback-group API (i.e. [rclcpp/callback_group.hpp](https://github.com/microROS/rclcpp/blob/master/rclcpp/include/rclcpp/callback_group.hpp)). Each callback requiring specific real-time guarantees, when created, may therefore be associated with a dedicated callback group. With this in place, the Executor class and depending classes (e.g., for memory allocation) were enhanced to operate at a finer, callback-group-level granularity.
+
+This allows a single node to have callbacks with different real-time profiles 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 time-critical callbacks may be handled by an "RT-CRITICAL" Executor instance running at the highest scheduler priority.
+
+As a proof of concept, we implemented a small test bench in the present package *cbg-executor_ping-pong_cpp*. The test bench comprises a Ping node and a Pong node which exchange real-time and best-effort messages simultaneously with each other. Each class of messages is handled with a dedicated Executor, as illustrated in the following figure.
+
+![](doc/ping_pong_diagram.png)
+
+The Ping node can be configured to send messages at a configured rate. The Pong node takes these ping messages and replies each of them. Before sending the reply, it can be configured to burn cycles (thereby varying the processor load) to simulate some message processing. We provide bash scripts to test intra-process and inter-process communication scenarios, wherein the nodes are co-located either in one process or two processes, respectively. These scripts also vary the message rates and processor loads. After each run, the Ping node outputs the measured throughput of real-time and best effort messages.
+
+
+## Running the test bench
+
+After building the test bench with [colcon](https://github.com/ros2/ros2/wiki/Colcon-Tutorial), the Ping and Pong nodes may be either started in one process or in two processes. Please note that the test bench requires sudo privileges to be able to change the thread priorities using `pthread_setschedparam(..)`. Start the executable by
+
+```
+ros2 run cbg-executor_ping-pong_cpp ping-pong args...
+```
+
+where `args...` are five or six arguments as follows:
+
+```
+[type] [rt_ping_period_us] [be_ping_period_us] [rt_busyloop_us] [be_busyloop_us] [cpu_id]
+
+type: determines the nodes included in this process:
+  i: ping node only
+  o: pong node only
+  io: ping node and pong node
+
+rt_ping_period_us: microseconds between publishing of ping messages by real-time thread
+                   in ping node
+be_ping_period_us: microseconds between publishing of ping messages by best-effort
+                   thread in ping node
+rt_busyloop_us:    microseconds of computation by real-time thread in pong node before
+                   answering with pong
+be_busyloop_us:    microseconds of computation by best-effort thread in pong node before
+                   answering with pong
+cpu_id (optional): pins both, real-time thread and best-effort thread, to the given cpu
+```
+
+When using type `i`, a second process with type `o` has to be started simultaneously.
+
+The shell run\* scripts in this folder run various experiments in sequence. For pertinent results, we propose to use the [PREEMPT_RT patch](https://wiki.linuxfoundation.org/realtime/start) for the Linux kernel.
+
+
+## Implementation Details
+
+The algorithms of the Ping node and of the Pong node are factored out into classes [_PingSubNode_](include/PingSubNode.hpp) and [_PongSubNode_](include/PongSubNode.hpp) - configurable with regard to the real-time profile and the topic prefix. Thus, the Ping node contains two instances of the _PingSubNode_ and the Pong node contains two instances of _PongSubNode_. (And the test bench could be easily extended to more than two ping-pong paths.)
+
+The PingSubNode contains a timer for sending the ping messages and a subscription for the corresponding pong messages. Also, it records the number of messages being sent and received and measures the roundtrip time.
+
+The PongSubNode contains a subscription for the ping messages and a publisher for the corresponding pong messages. On receiving a ping message, it calls the `PongSubNode::burn_cpu_cycles()` functions 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](main.cpp). This function also starts and ends the experiment for a predefined duration and prints out the throughput and latency statistics.

Cpp/cbg-executor_ping-pong/doc/ping_pong_diagram.png

@@ -0,0 +1,69 @@
+// Copyright
+//
+// 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 CBG_EXECUTOR_PING_PONG__PING_SUB_NODE_HPP_
+#define CBG_EXECUTOR_PING_PONG__PING_SUB_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+
+
+/// This class implements the Ping side of one ping-pong path of the test-bench. See README.md
+/// for a simple architecture diagram and a description of the whole test bench.
+class PingSubNode
+{
+public:
+  typedef std::shared_ptr<PingSubNode> SharedPtr;
+
+  PingSubNode(
+    rclcpp::Node::SharedPtr node, rclcpp::callback_group::RealTimeClass cbg_class,
+    const std::string & topics_prefix, const std::chrono::microseconds send_period);
+
+  virtual ~PingSubNode() = default;
+
+  rclcpp::callback_group::CallbackGroup::SharedPtr get_callback_group() {return callback_group_;}
+
+  /// Prints out the measured message throughput and latency.
+  void print_statistics();
+
+private:
+  /// The callback group for the timer and the pong subscription.
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_{};
+
+  /// Prefix for the ping and pong topics - here RT or BE.
+  const std::string topics_prefix_;
+
+  /// Timer for sending the pings in the given send period (cf. ctor).
+  rclcpp::TimerBase::SharedPtr ping_timer_{};
+
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr ping_publisher_{};
+
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr pong_subscription_{};
+
+  size_t ping_sent_count_ = 0;
+  size_t pong_received_count_ = 0;
+  std::vector<std::chrono::system_clock::time_point> ping_sent_timestamps_{};
+  std::vector<std::chrono::system_clock::time_point> pong_received_timestamps_{};
+
+  void ping_timer_callback();
+
+  void pong_subscription_callback(const std_msgs::msg::Int32::SharedPtr msg);
+};
+
+#endif  // CBG_EXECUTOR_PING_PONG__PING_SUB_NODE_HPP_

Cpp/cbg-executor_ping-pong/include/PingSubNode.hpp

@@ -0,0 +1,60 @@
+// Copyright
+//
+// 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 CBG_EXECUTOR_PING_PONG__PONG_SUB_NODE_HPP_
+#define CBG_EXECUTOR_PING_PONG__PONG_SUB_NODE_HPP_
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+
+/// This class implements the Pong side of one ping-pong path of the test-bench. See README.md
+/// for a simple architecture diagram and a description of the whole test bench.
+class PongSubNode
+{
+public:
+  typedef std::shared_ptr<PongSubNode> SharedPtr;
+
+  PongSubNode(
+    rclcpp::Node::SharedPtr node, rclcpp::callback_group::RealTimeClass cbg_class,
+    const std::string & topics_prefix, std::chrono::microseconds cpu_load);
+
+  virtual ~PongSubNode() = default;
+
+  rclcpp::callback_group::CallbackGroup::SharedPtr get_callback_group() {return callback_group_;}
+
+private:
+  /// The callback group for the ping subscription.
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_;
+
+  /// Prefix for the ping and pong topics - here RT or BE.
+  const std::string topics_prefix_;
+
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr ping_subscription_{};
+
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pong_publisher_{};
+
+  /// The given duration (cf. ctor) to simulate some processing on receiving a ping message.
+  const std::chrono::microseconds cpu_load_;
+
+  void ping_subscription_callback(const std_msgs::msg::Int32::SharedPtr msg);
+
+  /// Burns CPU cycles for the cpu_load_ duration.
+  void burn_cpu_cycles();
+};
+
+#endif  // CBG_EXECUTOR_PING_PONG__PONG_SUB_NODE_HPP_