Merge pull request #59 from ros-realtime/feature/exercise4-2-update

update exercise4-2

exercise4-2/solutions/application_nodes.cc

@@ -0,0 +1,126 @@
+#include "application_nodes.h"
+#include <cstdlib>
+#include <chrono>
+#include <list>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "utils.h"
+
+using cactus_rt::tracing::ThreadTracer;
+using camera_demo_interfaces::msg::FakeImage;
+
+CameraProcessingNode::CameraProcessingNode(
+  std::shared_ptr<ThreadTracer> tracer_object_detector,
+  std::shared_ptr<ThreadTracer> tracer_data_logger
+) : Node("obj_detect"), tracer_object_detector_(tracer_object_detector), tracer_data_logger_(tracer_data_logger) {
+  realtime_group_ = this->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive
+  );
+  besteffort_group_ = this->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive
+  );
+
+  rclcpp::SubscriptionOptions data_logging_options;
+  data_logging_options.callback_group = besteffort_group_;
+  subscription_data_logger_ = this->create_subscription<FakeImage>(
+    "/image",
+    10,
+    std::bind(&CameraProcessingNode::DataLoggerCallback, this, std::placeholders::_1),
+    data_logging_options
+  );
+
+  rclcpp::SubscriptionOptions obj_det_options;
+  obj_det_options.callback_group = realtime_group_;
+  subscription_object_detector_ = this->create_subscription<FakeImage>(
+    "/image",
+    10,
+    std::bind(&CameraProcessingNode::ObjectDetectorCallback, this, std::placeholders::_1),
+    obj_det_options
+  );
+
+  publisher_ = this->create_publisher<std_msgs::msg::Int64>("/actuation", 10);
+
+  // initialization of random variable for DataLogger latency
+  srand((unsigned)time(NULL));
+}
+
+void CameraProcessingNode::ObjectDetectorCallback(const FakeImage::SharedPtr image) {
+  // A hack to trace the message passing delay between publisher and this node
+  auto now = cactus_rt::NowNs();
+  tracer_object_detector_->StartSpan("MessageDelay", nullptr, image->published_at_monotonic_nanos);
+  tracer_object_detector_->EndSpan(now);
+
+  {
+    auto span = tracer_object_detector_->WithSpan("ObjectDetect");
+
+    WasteTime(std::chrono::microseconds(3000));
+
+    // Send a signal to the downstream actuation node
+    std_msgs::msg::Int64 msg;
+    msg.data = image->published_at_monotonic_nanos;
+    publisher_->publish(msg);
+  }
+}
+
+void CameraProcessingNode::DataLoggerCallback(const FakeImage::SharedPtr image) {
+  // A hack to trace the message passing delay between publisher and this node
+  auto now = cactus_rt::NowNs();
+  tracer_data_logger_->StartSpan("MessageDelay", nullptr, image->published_at_monotonic_nanos);
+  tracer_data_logger_->EndSpan(now);
+
+  {
+    auto span = tracer_data_logger_->WithSpan("DataLogger");
+
+    // variable duration to serialize the data between [5ms,10ms]  
+    unsigned int data_logger_latency = 5000 + (rand() % 5001);
+    WasteTime(std::chrono::microseconds(data_logger_latency));
+
+    // Assume it takes about 1ms to write the data where it is blocking but yielded to the CPU.
+    std::this_thread::sleep_for(std::chrono::microseconds(1000));
+  }
+}
+
+rclcpp::CallbackGroup::SharedPtr CameraProcessingNode::get_realtime_cbg() {
+  if (!realtime_group_) {
+    realtime_group_ = this->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive
+    );
+  }
+  return realtime_group_;
+}
+
+rclcpp::CallbackGroup::SharedPtr CameraProcessingNode::get_besteffort_cbg() {
+  if (!besteffort_group_) {
+    besteffort_group_ = this->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive
+    );
+  }
+  return besteffort_group_;
+}
+
+ActuationNode::ActuationNode(
+  std::shared_ptr<cactus_rt::tracing::ThreadTracer> tracer
+) : Node("actuation"), tracer_(tracer) {
+  subscription_ = this->create_subscription<std_msgs::msg::Int64>(
+    "/actuation",
+    10,
+    std::bind(&ActuationNode::MessageCallback, this, std::placeholders::_1)
+  );
+}
+
+void ActuationNode::MessageCallback(const std_msgs::msg::Int64::SharedPtr published_at_timestamp) {
+  auto now = cactus_rt::NowNs();
+
+  // A hack to show the end to end latency, from the moment it was published to
+  // the moment it is received by this node.
+  tracer_->StartSpan("EndToEndDelay", nullptr, published_at_timestamp->data);
+  tracer_->EndSpan(now);
+
+  {
+    auto span = tracer_->WithSpan("Actuation");
+    WasteTime(std::chrono::microseconds(150));
+  }
+}

exercise4-2/solutions/main.cc

@@ -0,0 +1,53 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include "application_nodes.h"
+#include "system_nodes.h"
+#include "tracing.h"
+
+int main(int argc, char** argv) {
+  // initialization of ROS and DDS middleware
+  rclcpp::init(argc, argv);
+
+  StartImagePublisherNode();
+
+  auto actuation_tracer = std::make_shared<cactus_rt::tracing::ThreadTracer>("actuation");
+  auto actuation_node = std::make_shared<ActuationNode>(actuation_tracer);
+
+  auto object_detector_tracer = std::make_shared<cactus_rt::tracing::ThreadTracer>("object_detector_callback");
+  auto data_logger_tracer = std::make_shared<cactus_rt::tracing::ThreadTracer>("data_logger_callback");
+  auto camera_processing_node = std::make_shared<CameraProcessingNode>(object_detector_tracer, data_logger_tracer);
+  auto data_logger_group = camera_processing_node->get_besteffort_cbg();
+  auto object_detector_group = camera_processing_node->get_realtime_cbg();
+
+  StartTracing("camera_demo_4_2", "exercise4-2.perfetto");
+  RegisterThreadTracer(actuation_tracer);
+  RegisterThreadTracer(object_detector_tracer);
+  RegisterThreadTracer(data_logger_tracer);
+
+  rclcpp::executors::SingleThreadedExecutor real_time_executor;
+  rclcpp::executors::SingleThreadedExecutor best_effort_executor;
+
+  best_effort_executor.add_callback_group(data_logger_group, camera_processing_node->get_node_base_interface());
+  real_time_executor.add_callback_group(object_detector_group, camera_processing_node->get_node_base_interface());
+  real_time_executor.add_node(actuation_node);
+
+  // Launch real-time Executor in a thread
+  std::thread real_time_thread([&real_time_executor]() {
+    sched_param sch;
+    sch.sched_priority = 60;
+    if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) {
+      perror("sched_setscheduler failed");
+      exit(-1);
+    }
+    real_time_executor.spin();
+  });
+
+  best_effort_executor.spin();
+
+  rclcpp::shutdown();
+  StopTracing();
+
+  JoinImagePublisherNode();
+  real_time_thread.join();
+  return 0;
+}

exercise4-2/src/camera_demo/src/application_nodes.cc

@@ -1,5 +1,5 @@
 #include "application_nodes.h"
-
+#include <cstdlib>
 #include <chrono>
 #include <list>
 #include <memory>
@@ -16,32 +16,23 @@ CameraProcessingNode::CameraProcessingNode(
   std::shared_ptr<ThreadTracer> tracer_object_detector,
   std::shared_ptr<ThreadTracer> tracer_data_logger
 ) : Node("obj_detect"), tracer_object_detector_(tracer_object_detector), tracer_data_logger_(tracer_data_logger) {
-  realtime_group_ = this->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive
-  );
-  besteffort_group_ = this->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive
-  );
-
-  rclcpp::SubscriptionOptions data_logging_options;
-  data_logging_options.callback_group = besteffort_group_;
+
   subscription_data_logger_ = this->create_subscription<FakeImage>(
     "/image",
     10,
-    std::bind(&CameraProcessingNode::DataLoggerCallback, this, std::placeholders::_1),
-    data_logging_options
+    std::bind(&CameraProcessingNode::DataLoggerCallback, this, std::placeholders::_1)
   );
 
-  rclcpp::SubscriptionOptions obj_det_options;
-  obj_det_options.callback_group = realtime_group_;
   subscription_object_detector_ = this->create_subscription<FakeImage>(
     "/image",
     10,
-    std::bind(&CameraProcessingNode::ObjectDetectorCallback, this, std::placeholders::_1),
-    obj_det_options
+    std::bind(&CameraProcessingNode::ObjectDetectorCallback, this, std::placeholders::_1)
   );
 
   publisher_ = this->create_publisher<std_msgs::msg::Int64>("/actuation", 10);
+
+  // initialization of random variable for DataLogger latency
+  srand((unsigned)time(NULL));
 }
 
 void CameraProcessingNode::ObjectDetectorCallback(const FakeImage::SharedPtr image) {
@@ -52,8 +43,6 @@ void CameraProcessingNode::ObjectDetectorCallback(const FakeImage::SharedPtr ima
 
   {
     auto span = tracer_object_detector_->WithSpan("ObjectDetect");
-
-    // Pretend it takes 3ms to do object detection.
     WasteTime(std::chrono::microseconds(3000));
 
     // Send a signal to the downstream actuation node
@@ -72,8 +61,9 @@ void CameraProcessingNode::DataLoggerCallback(const FakeImage::SharedPtr image)
   {
     auto span = tracer_data_logger_->WithSpan("DataLogger");
 
-    // Assume it takes 6ms to serialize the data which is all on the CPU
-    WasteTime(std::chrono::microseconds(6000));
+    // variable duration to serialize the data between [5ms,10ms]
+    unsigned int data_logger_latency = 5000 + (rand() % 5001);
+    WasteTime(std::chrono::microseconds(data_logger_latency));
 
     // Assume it takes about 1ms to write the data where it is blocking but yielded to the CPU.
     std::this_thread::sleep_for(std::chrono::microseconds(1000));

exercise4-2/src/camera_demo/src/main.cc

@@ -24,31 +24,15 @@ int main(int argc, char** argv) {
   RegisterThreadTracer(object_detector_tracer);
   RegisterThreadTracer(data_logger_tracer);
 
-  rclcpp::executors::SingleThreadedExecutor real_time_executor;
-  rclcpp::executors::SingleThreadedExecutor best_effort_executor;
-
-  best_effort_executor.add_callback_group(data_logger_group, camera_processing_node->get_node_base_interface());
-  real_time_executor.add_callback_group(object_detector_group, camera_processing_node->get_node_base_interface());
-  real_time_executor.add_node(actuation_node);
-
-  // Launch best effort thread
-  std::thread best_effort_thread([&best_effort_executor]() {
-    best_effort_executor.spin();
-  });
-
-  // Set the priority of this realtime thread
-  sched_param sch;
-  sch.sched_priority = 50;
-  if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) {
-    perror("sched_setscheduler failed");
-    exit(-1);
-  }
-  real_time_executor.spin();
+  rclcpp::executors::SingleThreadedExecutor executor;
+
+  executor.add_node(camera_processing_node);
+  executor.add_node(actuation_node);
+  executor.spin();
 
   rclcpp::shutdown();
   StopTracing();
 
   JoinImagePublisherNode();
-  best_effort_thread.join();
   return 0;
 }