salmanoff/smocore/body/body.cpp

#include <iostream>
#include <opts.h>
#include <asynchronousContinuation.h>
#include <asynchronousLoop.h>
#include <body/body.h>
#include <componentThread.h>
#include <mind.h>
#include <senseApis/senseApiManager.h>

namespace smo {
namespace body {

Body::Body(Mind &parent, const std::shared_ptr<ComponentThread> &thread)
    : MindComponent(parent, thread)
{
}

class Body::InitializeReq
:	public AsynchronousContinuation<bodyLifetimeMgmtOpCbFn>,
	public ContinuationTarget
{
public:
	InitializeReq(
		Mind &parent, const std::shared_ptr<ComponentThread> &caller,
		bodyLifetimeMgmtOpCbFn callback)
	:	AsynchronousContinuation<bodyLifetimeMgmtOpCbFn>(callback),
		ContinuationTarget(caller),
	parent(parent)
	{}

	void callOriginalCbFn(bool success)
	{
		if (originalCbFn)
		{
			caller->getIoService().post(
				std::bind(originalCbFn, success));
		}
	}

private:
	Mind &parent;

public:
	void initializeReq1(
		[[maybe_unused]] std::shared_ptr<InitializeReq> 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(
					&InitializeReq::initializeReq2,
					context.get(), context,
					std::placeholders::_1));
	}

	void initializeReq2(
		[[maybe_unused]] std::shared_ptr<InitializeReq> context,
		smo::AsynchronousLoop &results
		)
	{
		parent.bodyComponentInitialized = true;
		std::cout << "Mrntt: attached "
			<< results.nSucceeded << " of " << results.nTotal
			<< " sense devices." << "\n";

		callOriginalCbFn(results.nSucceeded == results.nTotal);
	}
};

class Body::FinalizeReq
:	public InitializeReq
{
public:
	using InitializeReq::InitializeReq;

public:
	void finalizeReq1(
		[[maybe_unused]] std::shared_ptr<FinalizeReq> 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(
				&FinalizeReq::finalizeReq2,
				context.get(), context,
				std::placeholders::_1));
	}

	void finalizeReq2(
		[[maybe_unused]] std::shared_ptr<FinalizeReq> 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 Body::initializeReq(bodyLifetimeMgmtOpCbFn 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<InitializeReq>(
		parent, mrntt, callback);

	thread->getIoService().post(
		std::bind(
			&InitializeReq::initializeReq1,
			request.get(), request));
}

void Body::finalizeReq(bodyLifetimeMgmtOpCbFn callback)
{
	auto mrntt = ComponentThread::getSelf();

	if (mrntt->id != ComponentThread::MRNTT)
	{
		throw std::runtime_error(std::string(__func__)
			+ ": Must be invoked by Mrntt thread");
	}

	if (!parent.bodyComponentInitialized)
	{
		std::cout << "Mrntt: Body component not initialized. "
			<< "Skipping finalization." << "\n";
		callback(true);
		return;
	}

	auto request = std::make_shared<FinalizeReq>(
		parent, mrntt, callback);

	thread->getIoService().post(
		std::bind(
			&FinalizeReq::finalizeReq1,
			request.get(), request));
}

} // namespace body
} // namespace smo