Perplexing problem

node_z.cpp

#include <chrono>
#include <memory>

extern "C" {
	#include <sched.h>
	#include <string.h>
	#include <unistd.h>
	#include <errno.h>
	#include <stdio.h>
	#include <sys/fcntl.h>
	#include <math.h>
	#include <time.h>
	#include <sys/time.h>
	#include <sys/wait.h>
}


#include "rclcpp/rclcpp.hpp"
#include "tutorial_interfaces/msg/num.hpp"

using namespace std::chrono_literals;
using std::placeholders::_1;


/*
 *******************************************************************************
 *                             Symbolic Constants                              *
 *******************************************************************************
*/


#define TRACE_FILE_PATH        "/sys/kernel/debug/tracing/trace_marker"


/*
 *******************************************************************************
 *                              Global Variables                               *
 *******************************************************************************
*/

int g_fd   = -1;
int g_core = -1;


/*
 *******************************************************************************
 *                        Utility Function Definitions                         *
 *******************************************************************************
*/


bool trace_write (int fd, const char *fmt, ...)
{
	va_list ap;
	char buf[256];
	int n;

	if (fd < 0) {
		return false;
	}

	va_start(ap, fmt);
	n = vsnprintf(buf, 256, fmt, ap);
	va_end(ap);

	return (write(fd, buf, n) == n);
}


/*
 *******************************************************************************
 *                             Class A Definition                              *
 *******************************************************************************
*/


class A : public rclcpp::Node {
public:

	A() : Node("A")
	{
		// Initialize publisher
		pub_x = this->create_publisher<tutorial_interfaces::msg::Num>("topic_x", 10);

		// Initialize publisher
		pub_y = this->create_publisher<tutorial_interfaces::msg::Num>("topic_y", 10);

		// Create timer
		timer = this->create_wall_timer(100ms, std::bind(&A::dispatch_all, this));
	}

	void dispatch_all () 
	{
		auto msg = tutorial_interfaces::msg::Num();
		msg.num = 1;

		// Mark start of dispatch
		trace_write(g_fd, "Task-%d-start", g_core);

		// Publish 5 messages on topic x
		for (int i = 0; i < 5; ++i) {
			pub_x->publish(msg);
		}

		// Send a single high QoS message
		pub_y->publish(msg);

		// Print
		RCLCPP_INFO(this->get_logger(), "Done");

		trace_write(g_fd, "Task-%d-end", g_core);
	}

private:

	// Publisher
	rclcpp::Publisher<tutorial_interfaces::msg::Num>::SharedPtr pub_x, pub_y;

	// Timer
	rclcpp::TimerBase::SharedPtr timer;
};


class X : public rclcpp::Node {
public:
	X(): Node("X") {
		sub = this->create_subscription<tutorial_interfaces::msg::Num>("topic_x", 10, std::bind(&X::cb, this, _1));
		RCLCPP_INFO(this->get_logger(), "X ready");
	}

private:

	// Subscription
	rclcpp::Subscription<tutorial_interfaces::msg::Num>::SharedPtr sub;

	// Callback
	void cb (const tutorial_interfaces::msg::Num::SharedPtr msg) const {
		trace_write(g_fd, "Task-%d-start", g_core);
		RCLCPP_INFO(this->get_logger(), "X: Receivied %d", msg->num);
		trace_write(g_fd, "Task-%d-end", g_core);
	}
};


class Y : public rclcpp::Node {
public:
	Y(): Node("Y") {
		sub = this->create_subscription<tutorial_interfaces::msg::Num>("topic_y", 10, std::bind(&Y::cb, this, _1));
		RCLCPP_INFO(this->get_logger(), "Y ready");
	}

private:

	// Subscription
	rclcpp::Subscription<tutorial_interfaces::msg::Num>::SharedPtr sub;

	// Callback
	void cb (const tutorial_interfaces::msg::Num::SharedPtr msg) const {
		trace_write(g_fd, "Task-%d-start", g_core);
		RCLCPP_INFO(this->get_logger(), "Y: Receivied %d", msg->num);
		trace_write(g_fd, "Task-%d-end", g_core);
	}
};


/*
 *******************************************************************************
 *                                    Main                                     *
 *******************************************************************************
*/

void random_trace (int fd, int core)
{

	// Seed random generator
	time_t t;
	srand((unsigned) time(&t));


	// Create random sleep (under 10ms)
	struct timespec ts;
    ts.tv_sec = 0;
    ts.tv_nsec = 1000 * (rand() % 100);

	if (trace_write (fd, "task-%d-start", core) == false) {
		fprintf(stderr, "Err: Couldn't write to trace file!\n");
		exit(-1);
	}

	nanosleep(&ts, NULL);

	if (trace_write (fd, "task-%d-end", core) == false) {
		fprintf(stderr, "Err: Couldn't write to trace file!\n");
		exit(-1);
	}
}

int main (int argc, char *argv[])
{
	cpu_set_t cpuset;
	int set_result;
	int child_pids[2] = {0}, pid;
	int core = 1;
	int trace_fd = -1;

	// Open the trace file (should persist between forks)
	if ((trace_fd = open(TRACE_FILE_PATH, O_WRONLY)) == -1) {
		fprintf(stderr, "Err: Couldn't open trace file: %s\n", strerror(errno));
		return -1;
	}


	// Zero out the core
	CPU_ZERO(&cpuset);

	printf("- About to fork from parent process %d: Open kernelshark now!\n", getpid());
	int x; scanf("%d", &x);

	// Fork here
	for (int i = 0; i < 2; ++i) {
		if ((pid = fork()) == -1) {
			fprintf(stderr, "Err: Bad fork: %s\n", strerror(errno));
			return -1;
		}

		// Parent
		if (pid > 0) {
			child_pids[i] = pid;
		} else {
		// Child
			core = g_core = (core + i + 1);
			g_fd = trace_fd;
			break;
		}
	}

	// Configure the core
	fprintf(stdout, "Fork #%d\n", core);
	CPU_SET(core, &cpuset);

	// Apply process-level CPU affinity (first param is zero -> this process)
	if ((set_result = sched_setaffinity(0, sizeof(cpu_set_t), &cpuset)) == -1) {
		fprintf(stderr, "Err: Bad affinity: %s\n", strerror(errno));
		return -1;
	}


	// Init and launch ROS
	rclcpp::init(argc, argv);
	switch (core) {

		case 1: { // Process 1
			printf("Launching %d\n!", core);
			rclcpp::spin(std::make_shared<A>());
		}
		break;

		case 2: { // Process 2
			printf("Launching %d\n!", core);
			rclcpp::spin(std::make_shared<X>());
		}
		break;

		case 3: { // Process 3
			printf("Launching %d\n!", core);
			rclcpp::spin(std::make_shared<Y>());
		}
		break;

		default: {
			fprintf(stderr, "Err: Unexpected case (%d)!\n", core);
			return -1;			
		}

	}
	rclcpp::shutdown();

	// If parent - collect children
	if (core == 1) {
		for (int i = 0; i < 2; ++i) {
			wait(NULL);
		}
	}

	return 0;
}