Mind: Implement initialize/finalizeBodyReq()

We've done a lot of general work on the init sequencing.
2025-09-12 16:09:26 -04:00
parent b99b147959
commit 25a9721f92
10 changed files with 294 additions and 163 deletions
@@ -4,6 +4,7 @@
 #include <asynchronousLoop.h>
 #include <mind.h>
 #include <componentThread.h>
+#include <senseApis/senseApiManager.h>

 namespace smo {

@@ -65,10 +66,10 @@ public:
 		parent(parent)
 	{}

-	void callOriginalCbFn(void)
+	void callOriginalCbFn(bool success)
 	{
 		if (originalCbFn) {
-			originalCbFn(true);
+			originalCbFn(success);
 		}
 	}

@@ -93,27 +94,71 @@ public:
 		)
 	{
 		std::cout << "Mrntt: All mind threads started." << "\n";
-		callOriginalCbFn();
+
+		parent.initializeBodyReq(
+			std::bind(
+				&MindLifetimeMgmtOp::initializeReq3,
+				context.get(), context, std::placeholders::_1));
+	}
+
+	void initializeReq3(
+		[[maybe_unused]] std::shared_ptr<MindLifetimeMgmtOp> context,
+		bool success
+		)
+	{
+		std::cout << "Mrntt: Body component initialized." << "\n";
+		callOriginalCbFn(success);
 	}

 	void finalizeReq1(
-		[[maybe_unused]] std::shared_ptr<MindLifetimeMgmtOp> context
+		[[maybe_unused]] std::shared_ptr<MindLifetimeMgmtOp> context,
+		bool success
 		)
 	{
-		std::cout << "Mrntt: All mind threads JOLTed." << "\n";
+		if (!success) {
+			std::cerr << "Mrntt: Body component failed to finalize." << "\n";
+		} else {
+			std::cout << "Mrntt: Body component finalized." << "\n";
+		}

-		parent.exitAllMindThreadsReq(
-			std::bind(
-				&MindLifetimeMgmtOp::finalizeReq2,
-				context.get(), context));
+		/* If the threads haven't been jolted, we need to do that first, because
+		* otherwise they'll just enter their main loops and wait for control
+		* messages from mrntt after processing the exit request.
+		*/
+		if (!parent.threadsHaveBeenJolted)
+		{
+			parent.joltAllMindThreadsReq(
+				std::bind(
+					&MindLifetimeMgmtOp::finalizeReq2,
+					context.get(), context));
+		}
+		else
+		{
+			parent.exitAllMindThreadsReq(
+				std::bind(
+					&MindLifetimeMgmtOp::finalizeReq3,
+					context.get(), context));
+		}
 	}

 	void finalizeReq2(
 		[[maybe_unused]] std::shared_ptr<MindLifetimeMgmtOp> context
 		)
+	{
+		std::cout << "Mrntt: All mind threads JOLTed for finalization." << "\n";
+
+		parent.exitAllMindThreadsReq(
+			std::bind(
+				&MindLifetimeMgmtOp::finalizeReq3,
+				context.get(), context));
+	}
+
+	void finalizeReq3(
+		[[maybe_unused]] std::shared_ptr<MindLifetimeMgmtOp> context
+		)
 	{
 		std::cout << "Mrntt: All mind threads exited." << "\n";
-		callOriginalCbFn();
+		callOriginalCbFn(true);
 	}
 };

@@ -146,24 +191,169 @@ void Mind::finalizeReq(mindLifetimeMgmtOpCbFn callback)
 	auto request = std::make_shared<MindLifetimeMgmtOp>(
 		*this, callback);

-    /* If the threads haven't been jolted, we need to do that first, because
-     * otherwise they'll just enter their main loops and wait for control
-     * messages from mrntt after processing the exit request.
-     */
-    if (!threadsHaveBeenJolted)
-    {
-        joltAllMindThreadsReq(
+	finalizeBodyReq(
+		std::bind(
+			&MindLifetimeMgmtOp::finalizeReq1,
+			request.get(), request, std::placeholders::_1));
+}
+
+class Mind::InitializeBodyReq
+:	public MindLifetimeMgmtOp, public ContinuationTarget
+{
+public:
+	InitializeBodyReq(
+		Mind &parent, const std::shared_ptr<ComponentThread> &caller,
+		mindLifetimeMgmtOpCbFn callback)
+	:	MindLifetimeMgmtOp(parent, callback), ContinuationTarget(caller)
+	{}
+
+	void callOriginalCbFn(bool success)
+	{
+		if (originalCbFn)
+		{
+			caller->getIoService().post(
+				std::bind(originalCbFn, success));
+		}
+	}
+
+public:
+	void initializeBodyReq1(
+		[[maybe_unused]] std::shared_ptr<InitializeBodyReq> context
+		)
+	{
+		auto self = ComponentThread::getSelf();
+		if (self->id != ComponentThread::BODY)
+		{
+			throw std::runtime_error(std::string(__func__)
+				+ ": Must be executed on Body thread");
+		}
+
+		/**	EXPLANATION:
+		 * The ComponentThread instance we pass in here is the one that will be
+		 * used by Senseapi libs to perform device-independent background
+		 * operations.
+		 * For example, liblivoxProto1's BroadcastListener will use this thread
+		 * to listen for UDP broadcast dgrams from Livox devices.
+		 *
+		 * Right now we use Marionette, but there's a strong argument for using
+		 * Body instead since it's meant to handle device-management operations.
+		 */
+		sense_api::SenseApiManager::getInstance()
+			.loadAllSenseApiLibsFromOptions(caller);
+
+		std::cout << sense_api::SenseApiManager::getInstance().stringifyLibs()
+			<< std::endl;
+
+		if (OptionParser::getOptions().verbose)
+		{
+			std::cout << __func__ << ": About to initializeAllSenseApiLibs"
+				<< '\n';
+		}
+		sense_api::SenseApiManager::getInstance().initializeAllSenseApiLibs();
+
+		if (OptionParser::getOptions().verbose)
+		{
+			std::cout << __func__ << ": About to attachAllSenseDevicesFromSpecs"
+				<< '\n';
+		}
+		sense_api::SenseApiManager::getInstance()
+			.attachAllSenseDevicesFromSpecsReq(
+				std::bind(
+					&InitializeBodyReq::initializeBodyReq2,
+					context.get(), context,
+					std::placeholders::_1));
+	}
+
+	void initializeBodyReq2(
+		[[maybe_unused]] std::shared_ptr<InitializeBodyReq> context,
+		smo::AsynchronousLoop &results
+		)
+	{
+		std::cout << "Mrntt: attached "
+			<< results.nSucceeded << " of " << results.nTotal
+			<< " sense devices." << "\n";
+
+		callOriginalCbFn(results.nSucceeded == results.nTotal);
+	}
+};
+
+class Mind::FinalizeBodyReq
+:	public InitializeBodyReq
+{
+public:
+	using InitializeBodyReq::InitializeBodyReq;
+
+public:
+	void finalizeBodyReq1(
+		[[maybe_unused]] std::shared_ptr<FinalizeBodyReq> context
+		)
+	{
+		auto self = ComponentThread::getSelf();
+		if (self->id != ComponentThread::BODY)
+		{
+			throw std::runtime_error(std::string(__func__)
+				+ ": Must be executed on Body thread");
+		}
+
+		std::cout << "Mrntt: About to detach all sense devices." << "\n";
+		sense_api::SenseApiManager::getInstance().detachAllSenseDevicesReq(
 			std::bind(
-				&MindLifetimeMgmtOp::finalizeReq1,
-				request.get(), request));
-    }
-    else
-    {
-        exitAllMindThreadsReq(
-			std::bind(
-				&MindLifetimeMgmtOp::finalizeReq1,
-				request.get(), request));
-    }
+				&FinalizeBodyReq::finalizeBodyReq2,
+				context.get(), context,
+				std::placeholders::_1));
+	}
+
+	void finalizeBodyReq2(
+		[[maybe_unused]] std::shared_ptr<FinalizeBodyReq> context,
+		smo::AsynchronousLoop &results
+	)
+	{
+		std::cout << "Mrntt: Successfully detached "
+			<< results.nSucceeded << " of " << results.nTotal
+			<< " sense devices." << "\n";
+
+		std::cout << "Mrntt: About to unload all sense api libs." << "\n";
+		sense_api::SenseApiManager::getInstance().unloadAllSenseApiLibs();
+		callOriginalCbFn(results.nSucceeded == results.nTotal);
+	}
+};
+
+void Mind::initializeBodyReq(mindLifetimeMgmtOpCbFn callback)
+{
+	auto mrntt = ComponentThread::getSelf();
+
+	if (mrntt->id != ComponentThread::MRNTT)
+	{
+		throw std::runtime_error(std::string(__func__)
+			+ ": Must be invoked by Mrntt thread");
+	}
+
+	auto request = std::make_shared<InitializeBodyReq>(
+		*this, mrntt, callback);
+
+	this->getComponentThread(ComponentThread::BODY)->getIoService().post(
+		std::bind(
+			&InitializeBodyReq::initializeBodyReq1,
+			request.get(), request));
+}
+
+void Mind::finalizeBodyReq(mindLifetimeMgmtOpCbFn callback)
+{
+	auto mrntt = ComponentThread::getSelf();
+
+	if (mrntt->id != ComponentThread::MRNTT)
+	{
+		throw std::runtime_error(std::string(__func__)
+			+ ": Must be invoked by Mrntt thread");
+	}
+
+	auto request = std::make_shared<FinalizeBodyReq>(
+		*this, mrntt, callback);
+
+	this->getComponentThread(ComponentThread::BODY)->getIoService().post(
+		std::bind(
+			&FinalizeBodyReq::finalizeBodyReq1,
+			request.get(), request));
 }

 void Mind::distributeAndPinThreadsAcrossCpus()