LivoxGen1: Add basic stimbuff creation & destruction

stimBuffApis/livoxGen1/livoxGen1.cpp

@@ -15,6 +15,7 @@
 #include <livoxProto1/protocol.h>
 #include <asynchronousContinuation.h>
 #include <boost/asio/deadline_timer.hpp>
+#include "pcloudStimulusBuffer.h"
 
 
 namespace smo {
@@ -24,6 +25,26 @@ namespace stim_buff {
 static const SmoCallbacks* smoHooksPtr = nullptr;
 static SmoThreadingModelDesc smoThreadingModelDesc;
 
+// Local collection of stimulus buffers
+static std::vector<std::shared_ptr<StimulusBuffer>> attachedStimBuffs;
+
+// Get stimulus buffer by device attachment spec
+static std::shared_ptr<StimulusBuffer>
+getStimBuff(const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec)
+{
+	for (const auto& stimBuff : attachedStimBuffs)
+	{
+		// Compare device selectors to find matching buffer
+		if (stimBuff->deviceAttachmentSpec.deviceSelector
+			== spec->deviceSelector)
+		{
+			return stimBuff;
+		}
+	}
+
+	return nullptr;
+}
+
 // LivoxProto1 library state
 struct LivoxProto1DllState
 {
@@ -56,22 +77,6 @@ struct LivoxProto1DllState
 };
 
 static LivoxProto1DllState livoxProto1;
-// Attached Livox devices
-static std::vector<std::shared_ptr<livoxProto1::Device>> g_attachedDevices;
-
-// Utility function to find a device in g_attachedDevices by identifier
-static std::shared_ptr<livoxProto1::Device> getDevice(
-	const std::string& deviceIdentifier
-	)
-{
-	auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(),
-		[&deviceIdentifier](const std::shared_ptr<livoxProto1::Device>& dev) {
-			return livoxProto1::comms::deviceIdentifiersEqual(
-				dev->discoveredDevice.deviceIdentifier, deviceIdentifier);
-		});
-
-	return (it != g_attachedDevices.end()) ? *it : nullptr;
-}
 
 // Continuation classes for async operations
 class AttachDeviceReq
@@ -88,7 +93,8 @@ public:
 
 public:
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
-	std::shared_ptr<livoxProto1::Device> device;
+	std::shared_ptr<PcloudStimulusBuffer> stimBuff;
+
 private:
 	std::unique_ptr<boost::asio::deadline_timer> delayTimer;
 
@@ -99,22 +105,32 @@ public:
 	{
 		if (!success || !dev)
 		{
-			std::cerr << __func__ << ": Failed to create Livox device: "
+			std::cerr << __func__ << ": Failed to create/find Livox device: "
 				<< context->spec->deviceSelector << std::endl;
 			context->callOriginalCb(false, context->spec);
 			return;
 		}
 
-		g_attachedDevices.push_back(dev);
+		// Create and add PcloudStimulusBuffer to collection
+		StimulusBuffer::PcloudFormatDesc formatDesc;
+		formatDesc.format = StimulusBuffer::PcloudFormatDesc::Format::XYZI;
+		auto pcloudStimBuff = std::make_shared<PcloudStimulusBuffer>(
+			*context->spec, dev, formatDesc);
+
+		context->stimBuff = pcloudStimBuff;
+		dev->nAttachedStimBuffs++;
+		attachedStimBuffs.push_back(pcloudStimBuff);
+
 		if (1 || OptionParser::getOptions().verbose)
 		{
-			std::cout << __func__ << ": Successfully attached Livox "
+			std::cout << __func__ << ": Successfully attached/found Livox "
 				"device: " << context->spec->deviceSelector << " (ID: "
 				<< context->spec->deviceIdentifier << ")\n";
 		}
 
-		// Set the device in the context and enable point cloud data after 5ms delay
+		/* Delay here because getOrCreate just sent HandshakeReq, so device
-		context->device = dev;
+		 * may not yet be ready for another command.
+		 */
 		context->delayedEnablePcloudData(context);
 	}
 
@@ -126,8 +142,8 @@ public:
 		{
 			std::cerr << __func__ << ": Failed to enable pcloud data for dev "
 				<< context->spec->deviceSelector << std::endl;
-			context->callOriginalCb(false, context->spec);
+
-			return;
+				// Fallthrough.
 		}
 
 		if (1 || OptionParser::getOptions().verbose)
@@ -145,7 +161,7 @@ public:
 	{
 		// Initialize timer with device's component thread
 		delayTimer = std::make_unique<boost::asio::deadline_timer>(
-			device->componentThread->getIoService());
+			context->stimBuff->device->componentThread->getIoService());
 
 		delayTimer->expires_from_now(boost::posix_time::milliseconds(5));
 		delayTimer->async_wait(
@@ -169,7 +185,7 @@ public:
 		}
 
 		(*livoxProto1.livoxProto1_device_enablePcloudDataReq)(
-			device,
+			context->stimBuff->device,
 			{context, std::bind(
 			&AttachDeviceReq::attachDeviceReq2,
 			context.get(), context,
@@ -183,16 +199,16 @@ class DetachDeviceReq
 public:
 	DetachDeviceReq(
 		const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec,
-		const std::shared_ptr<livoxProto1::Device>& device,
+		const std::shared_ptr<PcloudStimulusBuffer>& stimBuff,
 		smo::Callback<sal_mlo_detachDeviceReqCbFn> cb)
 	:	smo::NonPostedAsynchronousContinuation<sal_mlo_detachDeviceReqCbFn>(
 		std::move(cb)),
-	spec(spec), device(device)
+	spec(spec), stimBuff(stimBuff)
 	{}
 
 public:
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
-	std::shared_ptr<livoxProto1::Device> device;
+	std::shared_ptr<PcloudStimulusBuffer> stimBuff;
 private:
 	std::unique_ptr<boost::asio::deadline_timer> delayTimer;
 
@@ -203,8 +219,8 @@ public:
 	{
 		if (!success)
 		{
-			std::cerr << __func__ << ": Failed to disable pcloud data for dev "
+			std::cerr << __func__ << ": Failed to disable pcloud data for "
-				<< context->spec->deviceSelector << std::endl;
+				"stimbuff " << context->spec->deviceSelector << std::endl;
 			// Fallthrough.
 		}
 
@@ -212,68 +228,24 @@ public:
 		context->delayedDestroyDevice(context);
 	}
 
-	void detachDeviceReq2(
-		std::shared_ptr<DetachDeviceReq> context,
-		bool success)
-	{
-		if (!success)
-		{
-			std::cerr << __func__ << ": Failed to destroy Livox device: "
-				<< context->spec->deviceIdentifier << "\n";
-
-			/**	NOTE:
-			 * There's a decent argument for falling through here and still
-			 * removing the device from g_attachedDevices.
-			 */
-			context->callOriginalCb(false, context->spec);
-			return;
-		}
-
-		// Find the device in g_attachedDevices and remove it.
-		auto deviceToRemove = getDevice(context->spec->deviceSelector);
-		if (!deviceToRemove)
-		{
-			std::cerr << __func__ << ": Race condition: device not found "
-				"in g_attachedDevices for detachment: "
-				<< context->spec->deviceIdentifier << "\n";
-			context->callOriginalCb(false, context->spec);
-			return;
-		}
-
-		// Remove the device from the collection
-		g_attachedDevices.erase(
-			std::remove(
-				g_attachedDevices.begin(), g_attachedDevices.end(),
-				deviceToRemove),
-			g_attachedDevices.end());
-
-		if (1 || OptionParser::getOptions().verbose)
-		{
-			std::cout << __func__ << ": Successfully detached Livox device: "
-				<< context->spec->deviceIdentifier << "\n";
-		}
-
-		context->callOriginalCb(success, context->spec);
-	}
-
 	// Helper method to delay and then call destroyDeviceReq
 	void delayedDestroyDevice(
 		std::shared_ptr<DetachDeviceReq> context)
 	{
 		// Initialize timer with device's component thread
 		delayTimer = std::make_unique<boost::asio::deadline_timer>(
-			device->componentThread->getIoService());
+			context->stimBuff->device->componentThread->getIoService());
 
 		delayTimer->expires_from_now(boost::posix_time::milliseconds(5));
 		delayTimer->async_wait(
 			std::bind(
-				&DetachDeviceReq::delayedDestroyDeviceCallback,
+				&DetachDeviceReq::detachDeviceReq1_delayed,
 				context.get(), context,
 				std::placeholders::_1));
 	}
 
 	// Callback for the delayed destroyDeviceReq
-	void delayedDestroyDeviceCallback(
+	void detachDeviceReq1_delayed(
 		std::shared_ptr<DetachDeviceReq> context,
 		const boost::system::error_code& error)
 	{
@@ -284,13 +256,49 @@ public:
 			// Fallthrough.
 		}
 
+		// Remove stimulus buffer from collection before destroying device
+		context->stimBuff->device->nAttachedStimBuffs--;
+		auto it = std::find(
+			attachedStimBuffs.begin(), attachedStimBuffs.end(),
+			context->stimBuff);
+		if (it != attachedStimBuffs.end())
+			{ attachedStimBuffs.erase(it); }
+
 		(*livoxProto1.livoxProto1_destroyDeviceReq)(
-			context->device,
+			context->stimBuff->device,
 			{context, std::bind(
 				&DetachDeviceReq::detachDeviceReq2,
 				context.get(), context,
 				std::placeholders::_1)});
 	}
+
+	void detachDeviceReq2(
+		std::shared_ptr<DetachDeviceReq> context,
+		bool success)
+	{
+		if (!success)
+		{
+			std::cerr << __func__ << ": Failed to destroy dev "
+				"device " << context->spec->deviceSelector << " for stimbuff."
+				"\n";
+
+			/**	NOTE:
+			 * There's a decent argument for falling through here and still
+			 * removing the stimulus buffer from attachedStimBuffs.
+			 */
+			context->callOriginalCb(false, context->spec);
+			return;
+		}
+
+		if (1 || OptionParser::getOptions().verbose)
+		{
+			std::cout << __func__ << ": Successfully detached pcloud stimbuff "
+				"for device " << context->spec->deviceSelector
+				<< " and possibly also destroyed device.\n";
+		}
+
+		context->callOriginalCb(success, context->spec);
+	}
 };
 
 // Callback function declarations
@@ -386,8 +394,8 @@ extern "C" int livoxGen1_initializeInd(void)
 
 extern "C" int livoxGen1_finalizeInd(void)
 {
-	// Clear all attached devices
+
-	g_attachedDevices.clear();
+	attachedStimBuffs.clear();
 
 	// Call LivoxProto1 library exit function
 	if (livoxProto1.livoxProto1_exit) {
@@ -419,17 +427,32 @@ extern "C" void livoxGen1_attachDeviceReq(
 
 	auto request = std::make_shared<AttachDeviceReq>(desc, cb);
 
-	// Check if device already exists
+	// Check if stimulus buffer already exists in the collection
-	auto existingDevice = getDevice(desc->deviceSelector);
+	auto pcloudStimBuff = std::static_pointer_cast<PcloudStimulusBuffer>(
-	if (existingDevice)
+		getStimBuff(desc));
+
+	if (pcloudStimBuff)
 	{
-		// Device already exists, set device and enable point cloud data
+		request->stimBuff = pcloudStimBuff;
-		std::cout << __func__ << ": Dev "
+
-			<< existingDevice->discoveredDevice.deviceIdentifier
+		// Check if device's point cloud data is already active
-			<< " already attached for DASpec: " << desc->deviceSelector
+		if (pcloudStimBuff->device && pcloudStimBuff->device->pcloudDataActive)
-			<< ".\n";
+		{
-		request->device = existingDevice;
+			// Point cloud data is already active, call success callback
-		request->delayedEnablePcloudData(request);
+			request->callOriginalCb(true, request->spec);
+			return;
+		}
+
+		/* Enable pcloud data. Don't need delay since this no commands were
+		 * sent to device prior to us reaching here.
+		 */
+		(*livoxProto1.livoxProto1_device_enablePcloudDataReq)(
+			pcloudStimBuff->device,
+			{request, std::bind(
+				&AttachDeviceReq::attachDeviceReq2,
+				request.get(), request,
+				std::placeholders::_1)});
+
 		return;
 	}
 
@@ -522,22 +545,22 @@ extern "C" void livoxGen1_detachDeviceReq(
 	Callback<smo::stim_buff::sal_mlo_detachDeviceReqCbFn> cb
 	)
 {
-	// Find the device in our collection
+	// Check if stimulus buffer exists in the collection
-	auto device = getDevice(desc->deviceSelector);
+	auto stimBuff = std::static_pointer_cast<PcloudStimulusBuffer>(
-	if (!device)
+		getStimBuff(desc));
+
+	if (!stimBuff)
 	{
-		std::cerr << std::string(__func__)
-		          << ": Device not found for detachment: "
-		          << desc->deviceIdentifier << std::endl;
 		cb.callbackFn(false, desc);
 		return;
 	}
 
-	auto request = std::make_shared<DetachDeviceReq>(desc, device, cb);
+	auto request = std::make_shared<DetachDeviceReq>(
+		desc, stimBuff, cb);
 
 	// Disable point cloud data first
 	(*livoxProto1.livoxProto1_device_disablePcloudDataReq)(
-		device,
+		stimBuff->device,
 		{request, std::bind(
 			&DetachDeviceReq::detachDeviceReq1,
 			request.get(), request,

stimBuffApis/livoxGen1/pcloudStimulusBuffer.h

@@ -3,6 +3,7 @@
 
 #include <user/stimulusBuffer.h>
 #include <user/stimFrame.h>
+#include <livoxProto1/device.h>
 
 namespace smo {
 namespace stim_buff {
@@ -21,12 +22,14 @@ class PcloudStimulusBuffer
 public:
 	explicit PcloudStimulusBuffer(
 		const device::DeviceAttachmentSpec& deviceAttachmentSpec,
+		std::shared_ptr<livoxProto1::Device> &device,
 		const PcloudFormatDesc& formatDesc)
 	:	StimulusBuffer(deviceAttachmentSpec),
+	deviceAttachmentSpec(deviceAttachmentSpec), device(device),
 	formatDesc(formatDesc)
 	{}
 
-	~PcloudStimulusBuffer();
+	~PcloudStimulusBuffer() = default;
 
 	// Non-copyable, movable
 	PcloudStimulusBuffer(const PcloudStimulusBuffer&) = delete;
@@ -34,7 +37,9 @@ public:
 	PcloudStimulusBuffer(PcloudStimulusBuffer&&) = default;
 	PcloudStimulusBuffer& operator=(PcloudStimulusBuffer&&) = default;
 
-private:
+public:
+	device::DeviceAttachmentSpec deviceAttachmentSpec;
+	std::shared_ptr<livoxProto1::Device> device;
 	PcloudFormatDesc formatDesc;
 };