LivoxGen/Proto1: Move en/disablePcloudData call to Gen1

We no longer try to enable pcloud data as part of the connectReq()
sequence. Instead we separate them so that a device can be connected
but not be issuing pcloud data.

commonLibs/livoxProto1/device.cpp

@@ -142,17 +142,6 @@ public:
 	 * WE need to assign the ipAddr to the Device being connected up.
 	 */
 
-	// Helper method to delay and then call enablePcloudDataReq
-	void delayedEnablePcloudData(std::shared_ptr<ConnectReq> context)
-	{
-		delayTimer.expires_from_now(boost::posix_time::milliseconds(5));
-		delayTimer.async_wait(
-			std::bind(
-				&ConnectReq::connectReq3,
-				context.get(), context,
-				std::placeholders::_1));
-	}
-
 	// Callback methods for the connection sequence
 	void connectReq1(
 		std::shared_ptr<ConnectReq> context,
@@ -181,8 +170,7 @@ public:
 		context->device.discoveredDevice.ipAddr = ipAddr;
 		context->device.startHeartbeat();
 
-		// Use async delay before enabling point cloud data
+		context->connectReq3(context, success);
-		delayedEnablePcloudData(context);
 	}
 
 	void connectReq2(
@@ -201,39 +189,18 @@ public:
 		context->device.discoveredDevice.ipAddr = ipAddr;
 		context->device.startHeartbeat();
 
-		// Use async delay before enabling point cloud data
+		context->connectReq3(context, success);
-		delayedEnablePcloudData(context);
 	}
 
 	void connectReq3(
-		std::shared_ptr<ConnectReq> context,
+		std::shared_ptr<ConnectReq> context, bool success
-		const boost::system::error_code& error
-		)
-	{
-		if (error)
-		{
-			// Timer was cancelled or error occurred
-			context->callOriginalCallbackWithFailure();
-			return;
-		}
-
-		context->device.enablePcloudDataReq(
-			{context, std::bind(&ConnectReq::connectReq4,
-			context.get(), context,
-			std::placeholders::_1)});
-		}
-
-	void connectReq4(
-		std::shared_ptr<ConnectReq> context,
-		bool success
 		)
 	{
 		if (!success)
 		{
-			std::cerr << __func__ << ": Failed to enable point cloud data for "
+			std::cerr << __func__ << ": Failed to connect to device "
-				"device (" << context->device.discoveredDevice.deviceIdentifier
+				"(" << context->device.discoveredDevice.deviceIdentifier
 				<< ") @(" << context->device.discoveredDevice.ipAddr << ").\n";
-
 			context->callOriginalCallbackWithFailure();
 			return;
 		}
@@ -1550,13 +1517,16 @@ protected:
 		      response->command.cmd_id == 0x04 &&
 		      response->ret_code == 0x00))
 		{
-			std::cerr << __func__ << ": Failed to enable pcloud data for dev "
+			if (OptionParser::getOptions().verbose)
+			{
+				std::cout << __func__ << ": Failed to en/disable pcloud data "
+					"for device "
 					"(" << context->device.discoveredDevice.deviceIdentifier
 					<< ") @(" << context->device.discoveredDevice.ipAddr << "). "
 					<< "cmd_set: " << (int)response->command.cmd_set
 					<< ", cmd_id: " << (int)response->command.cmd_id
 					<< ", ret_code: " << (int)response->ret_code << "\n";
-
+			}
 			context->callOriginalCallbackWithFailure();
 			return;
 		}

commonLibs/livoxProto1/livoxProto1.cpp

@@ -62,4 +62,32 @@ void livoxProto1_exit(void)
 	livoxProto1::exit();
 }
 
+void livoxProto1_device_enablePcloudDataReq(
+	std::shared_ptr<livoxProto1::Device> device,
+	smo::Callback<livoxProto1_device_enablePcloudDataReqCbFn> callback
+)
+{
+	if (!device)
+	{
+		throw std::runtime_error(std::string(__func__)
+			+ ": Device pointer is null");
+	}
+
+	device->enablePcloudDataReq(callback);
+}
+
+void livoxProto1_device_disablePcloudDataReq(
+	std::shared_ptr<livoxProto1::Device> device,
+	smo::Callback<livoxProto1_device_disablePcloudDataReqCbFn> callback
+)
+{
+	if (!device)
+	{
+		throw std::runtime_error(std::string(__func__)
+			+ ": Device pointer is null");
+	}
+
+	device->disablePcloudDataReq(callback);
+}
+
 } // extern "C"

commonLibs/livoxProto1/livoxProto1.h

@@ -67,11 +67,26 @@ typedef void livoxProto1_destroyDeviceReqFn(
 	std::shared_ptr<livoxProto1::Device> device,
 	smo::Callback<livoxProto1_destroyDeviceReqCbFn> callback);
 
+typedef std::function<void(bool success)>
+	livoxProto1_device_enablePcloudDataReqCbFn;
+typedef void livoxProto1_device_enablePcloudDataReqFn(
+	std::shared_ptr<livoxProto1::Device> device,
+	smo::Callback<livoxProto1_device_enablePcloudDataReqCbFn> callback);
+
+typedef std::function<void(bool success)>
+	livoxProto1_device_disablePcloudDataReqCbFn;
+typedef void livoxProto1_device_disablePcloudDataReqFn(
+	std::shared_ptr<livoxProto1::Device> device,
+	smo::Callback<livoxProto1_device_disablePcloudDataReqCbFn> callback);
+
 
 livoxProto1_mainFn livoxProto1_main;
 livoxProto1_exitFn livoxProto1_exit;
 livoxProto1_getOrCreateDeviceReqFn livoxProto1_getOrCreateDeviceReq;
 livoxProto1_destroyDeviceReqFn livoxProto1_destroyDeviceReq;
+livoxProto1_device_enablePcloudDataReqFn livoxProto1_device_enablePcloudDataReq;
+livoxProto1_device_disablePcloudDataReqFn
+	livoxProto1_device_disablePcloudDataReq;
 
 #ifdef __cplusplus
 }

stimBuffApis/livoxGen1/livoxGen1.cpp

@@ -12,7 +12,9 @@
 #include <callback.h>
 #include <livoxProto1/livoxProto1.h>
 #include <livoxProto1/device.h>
+#include <livoxProto1/protocol.h>
 #include <asynchronousContinuation.h>
+#include <boost/asio/deadline_timer.hpp>
 
 
 namespace smo {
@@ -30,7 +32,9 @@ struct LivoxProto1DllState
 	  livoxProto1_main(nullptr),
 	  livoxProto1_exit(nullptr),
 	  livoxProto1_getOrCreateDeviceReq(nullptr),
-	  livoxProto1_destroyDeviceReq(nullptr)
+	  livoxProto1_destroyDeviceReq(nullptr),
+	  livoxProto1_device_enablePcloudDataReq(nullptr),
+	  livoxProto1_device_disablePcloudDataReq(nullptr)
 	{}
 
 	static void DlCloser(void* handle)
@@ -45,13 +49,30 @@ struct LivoxProto1DllState
 	livoxProto1_exitFn *livoxProto1_exit;
 	livoxProto1_getOrCreateDeviceReqFn *livoxProto1_getOrCreateDeviceReq;
 	livoxProto1_destroyDeviceReqFn *livoxProto1_destroyDeviceReq;
+	livoxProto1_device_enablePcloudDataReqFn
+		*livoxProto1_device_enablePcloudDataReq;
+	livoxProto1_device_disablePcloudDataReqFn
+		*livoxProto1_device_disablePcloudDataReq;
 };
 
 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
 :	public smo::NonPostedAsynchronousContinuation<sal_mlo_attachDeviceReqCbFn>
@@ -67,13 +88,16 @@ public:
 
 public:
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
+	std::shared_ptr<livoxProto1::Device> device;
+private:
+	std::unique_ptr<boost::asio::deadline_timer> delayTimer;
 
 public:
 	void attachDeviceReq1(
 		std::shared_ptr<AttachDeviceReq> context,
 		bool success, std::shared_ptr<livoxProto1::Device> dev)
 	{
-		if (!dev)
+		if (!success || !dev)
 		{
 			std::cerr << __func__ << ": Failed to create Livox device: "
 				<< context->spec->deviceSelector << std::endl;
@@ -89,8 +113,68 @@ public:
 				<< context->spec->deviceIdentifier << ")\n";
 		}
 
+		// Set the device in the context and enable point cloud data after 5ms delay
+		context->device = dev;
+		context->delayedEnablePcloudData(context);
+	}
+
+	void attachDeviceReq2(
+		std::shared_ptr<AttachDeviceReq> context,
+		bool success)
+	{
+		if (!success)
+		{
+			std::cerr << __func__ << ": Failed to enable pcloud data for dev "
+				<< context->spec->deviceSelector << std::endl;
+			context->callOriginalCb(false, context->spec);
+			return;
+		}
+
+		if (1 || OptionParser::getOptions().verbose)
+		{
+			std::cout << __func__ << ": Enabled pcloud data for device: "
+				<< context->spec->deviceSelector << std::endl;
+		}
+
 		context->callOriginalCb(success, context->spec);
 	}
+
+	// Helper method to delay and then call enablePcloudDataReq
+	void delayedEnablePcloudData(
+		std::shared_ptr<AttachDeviceReq> context)
+	{
+		// Initialize timer with device's component thread
+		delayTimer = std::make_unique<boost::asio::deadline_timer>(
+			device->componentThread->getIoService());
+
+		delayTimer->expires_from_now(boost::posix_time::milliseconds(5));
+		delayTimer->async_wait(
+			std::bind(
+				&AttachDeviceReq::delayedEnablePcloudDataCallback,
+				context.get(), context,
+				std::placeholders::_1));
+	}
+
+	// Callback for the delayed enablePcloudDataReq
+	void delayedEnablePcloudDataCallback(
+		std::shared_ptr<AttachDeviceReq> context,
+		const boost::system::error_code& error)
+	{
+		if (error)
+		{
+			std::cerr << __func__ << ": Timer error: " << error.message()
+				<< std::endl;
+			context->callOriginalCb(false, context->spec);
+			return;
+		}
+
+		(*livoxProto1.livoxProto1_device_enablePcloudDataReq)(
+			device,
+			{context, std::bind(
+			&AttachDeviceReq::attachDeviceReq2,
+			context.get(), context,
+			std::placeholders::_1)});
+	}
 };
 
 class DetachDeviceReq
@@ -99,42 +183,55 @@ class DetachDeviceReq
 public:
 	DetachDeviceReq(
 		const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec,
+		const std::shared_ptr<livoxProto1::Device>& device,
 		smo::Callback<sal_mlo_detachDeviceReqCbFn> cb)
 	:	smo::NonPostedAsynchronousContinuation<sal_mlo_detachDeviceReqCbFn>(
 		std::move(cb)),
-	spec(spec)
+	spec(spec), device(device)
 	{}
 
 public:
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
+	std::shared_ptr<livoxProto1::Device> device;
+private:
+	std::unique_ptr<boost::asio::deadline_timer> delayTimer;
 
 public:
 	void detachDeviceReq1(
 		std::shared_ptr<DetachDeviceReq> context,
 		bool success)
+	{
+		if (!success)
+		{
+			std::cerr << __func__ << ": Failed to disable pcloud data for dev "
+				<< context->spec->deviceSelector << std::endl;
+			// Fallthrough.
+		}
+
+		// Add 5ms delay before destroying device
+		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 eraseIt = std::find_if(
+		auto deviceToRemove = getDevice(context->spec->deviceSelector);
-			g_attachedDevices.begin(), g_attachedDevices.end(),
+		if (!deviceToRemove)
-			[context](const std::shared_ptr<livoxProto1::Device>& dev)
-			{
-				const std::string& devId = dev->discoveredDevice.deviceIdentifier;
-				std::string devIdPrefix = devId.substr(
-					0, std::min<size_t>(14, devId.size()));
-				return devIdPrefix == context->spec->deviceSelector.substr(
-					0, std::min<size_t>(14, context->spec->deviceSelector.size()));
-			}
-		);
-
-		if (eraseIt == g_attachedDevices.end())
 		{
 			std::cerr << __func__ << ": Race condition: device not found "
 				"in g_attachedDevices for detachment: "
@@ -143,12 +240,57 @@ public:
 			return;
 		}
 
-		g_attachedDevices.erase(eraseIt);
+		// 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());
+
+		delayTimer->expires_from_now(boost::posix_time::milliseconds(5));
+		delayTimer->async_wait(
+			std::bind(
+				&DetachDeviceReq::delayedDestroyDeviceCallback,
+				context.get(), context,
+				std::placeholders::_1));
+	}
+
+	// Callback for the delayed destroyDeviceReq
+	void delayedDestroyDeviceCallback(
+		std::shared_ptr<DetachDeviceReq> context,
+		const boost::system::error_code& error)
+	{
+		if (error)
+		{
+			std::cerr << __func__ << ": Timer error: " << error.message()
+				<< std::endl;
+			// Fallthrough.
+		}
+
+		(*livoxProto1.livoxProto1_destroyDeviceReq)(
+			context->device,
+			{context, std::bind(
+				&DetachDeviceReq::detachDeviceReq2,
+				context.get(), context,
+				std::placeholders::_1)});
+	}
 };
 
 // Callback function declarations
@@ -213,10 +355,22 @@ extern "C" int livoxGen1_initializeInd(void)
 			dlsym(
 				livoxProto1.dlopenHandle.get(),
 				"livoxProto1_destroyDeviceReq"));
+	livoxProto1.livoxProto1_device_enablePcloudDataReq = reinterpret_cast<
+		livoxProto1_device_enablePcloudDataReqFn *>(
+			dlsym(
+				livoxProto1.dlopenHandle.get(),
+				"livoxProto1_device_enablePcloudDataReq"));
+	livoxProto1.livoxProto1_device_disablePcloudDataReq = reinterpret_cast<
+		livoxProto1_device_disablePcloudDataReqFn *>(
+			dlsym(
+				livoxProto1.dlopenHandle.get(),
+				"livoxProto1_device_disablePcloudDataReq"));
 
 	if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit
 		|| !livoxProto1.livoxProto1_getOrCreateDeviceReq
-		|| !livoxProto1.livoxProto1_destroyDeviceReq)
+		|| !livoxProto1.livoxProto1_destroyDeviceReq
+		|| !livoxProto1.livoxProto1_device_enablePcloudDataReq
+		|| !livoxProto1.livoxProto1_device_disablePcloudDataReq)
 	{
 		throw std::runtime_error(
 			std::string(__func__) +
@@ -263,14 +417,21 @@ extern "C" void livoxGen1_attachDeviceReq(
 			"not available");
 	}
 
-	for (const auto& dev : g_attachedDevices)
+	auto request = std::make_shared<AttachDeviceReq>(desc, cb);
+
+	// Check if device already exists
+	auto existingDevice = getDevice(desc->deviceSelector);
+	if (existingDevice)
 	{
-		if (dev->discoveredDevice.deviceIdentifier == desc->deviceIdentifier)
+		// Device already exists, set device and enable point cloud data
-		{
+		std::cout << __func__ << ": Dev "
-			cb.callbackFn(true, desc);
+			<< existingDevice->discoveredDevice.deviceIdentifier
+			<< " already attached for DASpec: " << desc->deviceSelector
+			<< ".\n";
+		request->device = existingDevice;
+		request->delayedEnablePcloudData(request);
 		return;
 	}
-	}
 
 	// Parse integer parameters from provider params with defaults
 	/* The Livox Avia will generally respond to a handshake request within
@@ -344,8 +505,6 @@ extern "C" void livoxGen1_attachDeviceReq(
 		}
 	}
 
-	auto request = std::make_shared<AttachDeviceReq>(desc, cb);
-
 	(*livoxProto1.livoxProto1_getOrCreateDeviceReq)(
 		desc->deviceSelector,  // deviceIdentifier (broadcast code)
 		componentThread,
@@ -363,23 +522,9 @@ extern "C" void livoxGen1_detachDeviceReq(
 	Callback<smo::stim_buff::sal_mlo_detachDeviceReqCbFn> cb
 	)
 {
-	// Find and remove the device from our collection
+	// Find the device in our collection
-	auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(),
+	auto device = getDevice(desc->deviceSelector);
-		[&desc](const std::shared_ptr<livoxProto1::Device>& dev) {
+	if (!device)
-			/**		EXPLANATION:
-			 * Compare the first 14 characters of the deviceIdentifier with
-			 * the first 14 characters of the deviceSelector
-			 */
-			const std::string& devId = dev->discoveredDevice.deviceIdentifier;
-			std::string devIdPrefix = devId.substr(
-				0, std::min<size_t>(14, devId.size()));
-
-			return devIdPrefix == desc->deviceSelector.substr(
-				0, std::min<size_t>(14, desc->deviceSelector.size()));
-		}
-	);
-
-	if (it == g_attachedDevices.end())
 	{
 		std::cerr << std::string(__func__)
 		          << ": Device not found for detachment: "
@@ -388,10 +533,11 @@ extern "C" void livoxGen1_detachDeviceReq(
 		return;
 	}
 
-	auto request = std::make_shared<DetachDeviceReq>(desc, cb);
+	auto request = std::make_shared<DetachDeviceReq>(desc, device, cb);
 
-	(*livoxProto1.livoxProto1_destroyDeviceReq)(
+	// Disable point cloud data first
-		*it,
+	(*livoxProto1.livoxProto1_device_disablePcloudDataReq)(
+		device,
 		{request, std::bind(
 			&DetachDeviceReq::detachDeviceReq1,
 			request.get(), request,