Windows CI: make vcpkg download compatible with draft release (#1727)

* Windows CI: make vcpkg download compatible with draft release * forcing nvdia driver dll to be ignored in fixup_bundle * disabling opencv cudec and python * disabling rtabmap-console --version on cuda build * Added assert when bad imu data is provided. Added checks when zed sdk is returning nan imu data. * Updated zed sdk5 resolution and quality * Updated zed sdk5 resolution and quality * Fixing parameters reloaded when testing camera. Also fixed app freezing when closing Preferences dialog after using test camera dialog * cleanup and debug logs * Added dialogs when starting/stopping sensors/detection. Fixed nullptr bug on MainWindow::handleEvents * renamed * found a way to expose ZED model downloads * Updated default config path on Windows * Fixed extra endline in file logging (windows) * Fixed camera local transform when used with lidar and odomSensor * CI(macos): build g2o from source with OpenMP (libomp include/link fix) * Mac: added ceres dep, fixed omp not found by cmake * Mac: bundle orbbec extensions * stripping rpath of orbbec's extensions * cleanup * Realsense2 error as error (not warning) * Fixed config ownership changed to root when sarting in sudo. LidarVLP16: fixed crash on Mac by reimplementing our own socket threading * LidarVLP16: own socket implementation only for Mac (linux and windows use original PCL's reader)
2026-09-02 17:40:23 +08:00 · 2026-07-04 18:16:37 -07:00
parent cb34c4bd37
commit d069becc72
27 changed files with 982 additions and 141 deletions
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -1247,7 +1247,12 @@ void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, Pa
 				{
 					if(RTABMAP_VERSION_COMPARE(<, std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
 					{
-						if(configFilePath.find(".rtabmap") != std::string::npos)
+						// Detect the user's private config - matches both the legacy
+						// ~/.rtabmap/rtabmap.ini and the Windows %LOCALAPPDATA%/rtabmap/rtabmap.ini
+						// (".rtabmap/rtabmap.ini" also contains "rtabmap/rtabmap.ini"): downgrade-on-save is fine there.
+						// Accept both forward and backward slashes (native Windows paths).
+						if(configFilePath.find("rtabmap/rtabmap.ini") != std::string::npos ||
+						   configFilePath.find("rtabmap\\rtabmap.ini") != std::string::npos)
 						{
 							UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
 								   "current RTAB-Map version used (\"%s\"). The config file will be downgraded "
--- a/corelib/src/SensorCaptureThread.cpp
+++ b/corelib/src/SensorCaptureThread.cpp
@@ -114,7 +114,6 @@ SensorCaptureThread::SensorCaptureThread(
 				_camera(camera),
 				_odomSensor(odomSensor),
 				_lidar(lidar),
-				_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
 				_odomAsGt(false),
 				_poseTimeOffset(poseTimeOffset),
 				_poseScaleFactor(poseScaleFactor),
@@ -153,10 +152,14 @@ SensorCaptureThread::SensorCaptureThread(
 	{
 		if(_camera)
 		{
-			if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull())
+			if(_odomSensor == _camera && extrinsics.isNull())
 			{
 				_extrinsicsOdomToCamera.setIdentity();
 			}
+			else
+			{
+				_extrinsicsOdomToCamera = extrinsics * CameraModel::opticalRotation();
+			}
 			UASSERT(!_extrinsicsOdomToCamera.isNull());
 			UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
 		}
@@ -492,20 +495,26 @@ void SensorCaptureThread::mainLoop()
 					}
 				}

-				// Adjust local transform of the camera based on the pose frame
+				// Adjust local transform of the camera(s) based on the pose frame. The correction
+				// and odom->camera extrinsics are frame-level, so apply the same prefix to each
+				// camera while keeping its own local transform (multi-camera supported).
 				if(!data.cameraModels().empty())
 				{
-					UASSERT(data.cameraModels().size()==1);
-					CameraModel model = data.cameraModels()[0];
-					model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
-					data.setCameraModel(model);
+					std::vector<CameraModel> models = data.cameraModels();
+					for(size_t i=0; i<models.size(); ++i)
+					{
+						models[i].setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera*models[i].localTransform());
+					}
+					data.setCameraModels(models);
 				}
 				else if(!data.stereoCameraModels().empty())
 				{
-					UASSERT(data.stereoCameraModels().size()==1);
-					StereoCameraModel model = data.stereoCameraModels()[0];
-					model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
-					data.setStereoCameraModel(model);
+					std::vector<StereoCameraModel> models = data.stereoCameraModels();
+					for(size_t i=0; i<models.size(); ++i)
+					{
+						models[i].setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera*models[i].localTransform());
+					}
+					data.setStereoCameraModels(models);
 				}
 			}

@@ -530,15 +539,21 @@ void SensorCaptureThread::mainLoop()
 		info.odomPose.setNull();
 	}

-	if(!data.imageCompressed().empty() || !data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
+	if(this->isKilled())
+	{
+		// A kill was requested (e.g. while we were blocked capturing this frame): don't
+		// publish anything so we never deliver events to handlers that are being torn down.
+	}
+	else if(!data.imageCompressed().empty() || !data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
 		postUpdate(&data, &info);
 		info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();
 		info.timeTotal = totalTime.ticks();
 		this->post(new SensorEvent(data, info));
 	}
-	else if(!this->isKilled())
+	else
 	{
+		// Not killed but no data: end of stream. Signal consumers once, then stop.
 		UWARN("no more data...");
 		this->kill();
 		this->post(new SensorEvent());
--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -952,6 +952,22 @@ void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const
 	_descriptors = descriptors;
 }

+void SensorData::setIMU(const IMU & imu)
+{
+	UASSERT(
+		uIsFinite(imu.orientation()[0]) && 
+		uIsFinite(imu.orientation()[1]) && 
+		uIsFinite(imu.orientation()[2]) && 
+		uIsFinite(imu.orientation()[3]) &&
+		uIsFinite(imu.angularVelocity()[0]) && 
+		uIsFinite(imu.angularVelocity()[1]) && 
+		uIsFinite(imu.angularVelocity()[2]) && 
+		uIsFinite(imu.linearAcceleration()[0]) &&
+		uIsFinite(imu.linearAcceleration()[1]) &&
+		uIsFinite(imu.linearAcceleration()[2]));
+	imu_ = imu;
+}
+
 unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 {
 	return sizeof(SensorData) +
--- a/corelib/src/camera/CameraRealSense2.cpp
+++ b/corelib/src/camera/CameraRealSense2.cpp
@@ -107,10 +107,14 @@ void CameraRealSense2::close()
 			{
 				if(!_sensor.get_active_streams().empty())
 				{
+					std::string sensorName = _sensor.supports(RS2_CAMERA_INFO_NAME) ? _sensor.get_info(RS2_CAMERA_INFO_NAME) : "?";
 					try
 					{
+						UDEBUG("stop() sensor \"%s\"...", sensorName.c_str());
 						_sensor.stop();
+						UDEBUG("stop() sensor \"%s\" done; close()...", sensorName.c_str());
 						_sensor.close();
+						UDEBUG("close() sensor \"%s\" done", sensorName.c_str());
 					}
 					catch(const rs2::error & error)
 					{
@@ -119,12 +123,15 @@ void CameraRealSense2::close()
 				}
 			}
 #ifdef WIN32
+			UDEBUG("Windows-only: Hardware reset (to avoid freezing when clearing devices)...");
 			dev_[i].hardware_reset(); // To avoid freezing on some Windows computers in the following destructor
 			// Don't do this on linux (tested on Ubuntu 18.04, realsense v2.41.0): T265 cannot be restarted
+			UDEBUG("Windows-only: Hardware reset... done");
 #endif
 		}
 		UDEBUG("Clearing devices...");
 		dev_.clear();
+		UDEBUG("Clearing devices... done!");
 	}
 	catch(const rs2::error & error)
 	{
@@ -554,10 +561,10 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		catch(const rs2::error & error)
 		{
 #ifdef __APPLE__
-			UWARN("%s. Is the camera already used with another app? On macOS, accessing a "
+			UERROR("%s. Is the camera already used with another app? On macOS, accessing a "
 			      "RealSense camera requires root privileges, so try running with sudo.", error.what());
 #else
-			UWARN("%s. Is the camera already used with another app?", error.what());
+			UERROR("%s. Is the camera already used with another app?", error.what());
 #endif
 		}

--- a/corelib/src/camera/CameraStereoZed.cpp
+++ b/corelib/src/camera/CameraStereoZed.cpp
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>

 #ifdef RTABMAP_ZED
 #include <sl/Camera.hpp>
@@ -77,7 +78,7 @@ static cv::Mat slMat2cvMat(sl::Mat& input) {
 #endif
 }

-Transform zedPoseToTransform(const sl::Pose & pose)
+static Transform zedPoseToTransform(const sl::Pose & pose)
 {
 	return Transform(
 			pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
@@ -86,7 +87,7 @@ Transform zedPoseToTransform(const sl::Pose & pose)
 }

 #if ZED_SDK_MAJOR_VERSION < 3
-IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform)
+static IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform)
 {
 	sl::Orientation orientation = imuData.pose_data.getOrientation();

@@ -124,7 +125,7 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform
 			imuLocalTransform);
 }
 #else
-IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTransform)
+static IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTransform)
 {
    sl::Orientation orientation = sensorData.imu.pose.getOrientation();

@@ -161,6 +162,23 @@ IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTr
            accCov,
            imuLocalTransform);
 }
+
+// sl::SensorsData::imu.is_available only means the camera has an IMU; a returned
+// sample can still contain NaN pose/accel/gyro (e.g. before the IMU fusion has
+// initialized, or in SVO/STREAM mode). Validate the actual measurements.
+static bool isImuValid(const sl::SensorsData & sensorData)
+{
+	if(!sensorData.imu.is_available)
+	{
+		return false;
+	}
+	const sl::float3 & acc = sensorData.imu.linear_acceleration;
+	const sl::float3 & gyr = sensorData.imu.angular_velocity;
+	const sl::Orientation ori = sensorData.imu.pose.getOrientation();
+	return uIsFinite(acc.v[0]) && uIsFinite(acc.v[1]) && uIsFinite(acc.v[2]) &&
+	       uIsFinite(gyr.v[0]) && uIsFinite(gyr.v[1]) && uIsFinite(gyr.v[2]) &&
+	       uIsFinite(ori.ox) && uIsFinite(ori.oy) && uIsFinite(ori.oz) && uIsFinite(ori.ow);
+}
 #endif

 class ZedIMUThread: public UThread
@@ -217,7 +235,7 @@ private:
 #else
        sl::SensorsData sensordata;
        sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::CURRENT);
-        if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available)
+        if(res == sl::ERROR_CODE::SUCCESS && isImuValid(sensordata))
        {
        	camera_->postInterIMUPublic(zedIMUtoIMU(sensordata, imuLocalTransform_), double(sensordata.imu.timestamp.getNanoseconds())/10e8);
        }
@@ -251,6 +269,56 @@ int CameraStereoZed::sdkVersion()
 #endif
 }

+#ifdef RTABMAP_ZED
+static void backwardCompatibility(int & resolution, int & quality)
+{
+	// -1 = AUTO, 0=HD4K 1=QHDPLUS 2=HD2K 3=HD1536 4=HD1080 5=HD1200 6=HD720 7=SVGA 8=VGA 9=XVGA 10=TXVGA
+#if ZED_SDK_MAJOR_VERSION < 4
+	// Zed3 Supported: 2=HD2K 4=HD1080 6=HD720 8=VGA
+	if(resolution == 0 || resolution == 1) // 0=HD4K 1=QHDPLUS
+	{
+		UWARN("Zed SDK v3 doesn't support HD4K and QHDPLUS, setting HD2K.");
+		resolution = 2; // 2=HD2K
+	}
+	if(resolution == 3 || resolution == 5) // 3=HD1536 5=HD1200
+	{
+		UWARN("Zed SDK v3 doesn't support HD1536 and HD1200, setting HD1080.");
+		resolution = 4; // 4=HD1080
+	}
+	if(resolution == 7 || resolution >=9) // 7=SVGA 9=XVGA 10=TXVGA
+	{
+		UWARN("Zed SDK v3 doesn't support SVGA, XVGA and TXVGA, setting VGA.");
+		resolution = 8; // 8=VGA
+	}
+	if(quality == 4) { // 4=NEURAL_LIGHT 6=NEURAL_PLUS
+		UWARN("Zed SDK v3 doesn't support NEURAL_LIGHT and NEURAL PLUS, setting NEURAL.");
+		quality = 5; // NEURAL
+	}
+#else 
+	if(resolution == -1)
+	{
+		resolution = int(sl::RESOLUTION::AUTO); // AUTO
+	}
+#if ZED_SDK_MAJOR_VERSION < 5
+	// Zed4 Supported: 0=HD4K 1=QHDPLUS 2=HD2K 4=HD1080 5=HD1200 6=HD720 7=SVGA 8=VGA
+	if(resolution == 3) // 3=HD1536
+	{
+		UWARN("Zed SDK v4 doesn't support HD1536, setting HD1200.");
+		resolution_ = 5; // 5=HD1200
+	}
+	if(resolution >=9) // 9=XVGA 10=TXVGA
+	{
+		UWARN("Zed SDK v4 doesn't support XVGA and TXVGA, setting VGA.");
+		resolution = 8; // 8=VGA
+	}
+	if(quality > 5) { // NEURAL_PLUS
+		UWARN("Zed SDK v4 doesn't support NEURAL_LIGHT and NEURAL PLUS, setting NEURAL.");
+		quality = 5; // NEURAL
+	}
+#endif
+#endif
+}
+#endif

 CameraStereoZed::CameraStereoZed(
 		int deviceId,
@@ -285,29 +353,8 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
-#if ZED_SDK_MAJOR_VERSION < 4
-	if(resolution_ == 1 || resolution_ == 2) // HD2K, HD1080
-	{
-		resolution_ -= 1; // HD2K=0, HD1080=1
-	}
-	if(resolution_ == 3) // HD1200
-	{
-		resolution_ = 1; // HD1080=1
-	}
-	if(resolution_ == 4 || resolution_ == -1)
-	{
-		resolution_ = 2; // HD720=2
-	}
-	else if(resolution_ == 5 || resolution_ == 6) // SVGA, VGA
-	{
-		resolution_ = 3; // VGA=3
-	}
-#else // ZED=4
-	if(resolution_ == -1)
-	{
-		resolution_ = int(sl::RESOLUTION::AUTO); // AUTO
-	}
-#endif
+	
+	backwardCompatibility(resolution_, quality_);

 #if ZED_SDK_MAJOR_VERSION < 3
 	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
@@ -371,6 +418,7 @@ CameraStereoZed::CameraStereoZed(
 {
 	UDEBUG("");
 #ifdef RTABMAP_ZED
+	backwardCompatibility(resolution_, quality_);
 #if ZED_SDK_MAJOR_VERSION < 3
 	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
 	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
@@ -406,6 +454,33 @@ CameraStereoZed::~CameraStereoZed()
 #endif
 }

+std::string CameraStereoZed::getNeuralModelWarning(int quality)
+{
+	(void)quality;
+#if defined(RTABMAP_ZED) && ZED_SDK_MAJOR_VERSION >= 5
+	sl::DEPTH_MODE depthMode = (sl::DEPTH_MODE)quality;
+	if(depthMode == sl::DEPTH_MODE::NEURAL_LIGHT ||
+	   depthMode == sl::DEPTH_MODE::NEURAL ||
+	   depthMode == sl::DEPTH_MODE::NEURAL_PLUS)
+	{
+		sl::AI_MODELS aiModel =
+			depthMode == sl::DEPTH_MODE::NEURAL_LIGHT ? sl::AI_MODELS::NEURAL_LIGHT_DEPTH :
+			depthMode == sl::DEPTH_MODE::NEURAL_PLUS  ? sl::AI_MODELS::NEURAL_PLUS_DEPTH :
+			                                            sl::AI_MODELS::NEURAL_DEPTH;
+		sl::AI_Model_status status = sl::checkAIModelStatus(aiModel);
+		if(!status.downloaded || !status.optimized)
+		{
+			return uFormat("The selected ZED NEURAL depth model is not ready yet (downloaded=%s, "
+			               "optimized=%s): the first start may take significantly longer while the "
+			               "ZED SDK downloads and/or optimizes the model for your GPU. Subsequent "
+			               "starts will be faster.",
+			               status.downloaded?"true":"false", status.optimized?"true":"false");
+		}
+	}
+#endif
+	return std::string();
+}
+
 bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -463,9 +538,35 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		r = zed_->open(param);
 	}

+#if ZED_SDK_MAJOR_VERSION >= 3
+	// CORRUPTED_SDK_INSTALLATION on open() is typically a NEURAL depth mode whose optional
+	// neural/TensorRT runtime files aren't installed. Give a clear, actionable error.
+	if(r == sl::ERROR_CODE::CORRUPTED_SDK_INSTALLATION &&
+#if ZED_SDK_MAJOR_VERSION >= 5
+		param.depth_mode >= sl::DEPTH_MODE::NEURAL_LIGHT)
+#else
+		param.depth_mode >= sl::DEPTH_MODE::NEURAL)
+#endif
+	{
+		UERROR("ZED open() returned \"%s\": the optional NEURAL/TensorRT runtime files are "
+		       "likely missing. Install ZED SDK %d.%d, or select a non-NEURAL depth mode "
+		       "(e.g. PERFORMANCE).", toString(r).c_str(), ZED_SDK_MAJOR_VERSION, ZED_SDK_MINOR_VERSION);
+		// Do NOT delete zed_ here: after a CORRUPTED_SDK_INSTALLATION open failure (NEURAL depth
+		// mode selected but the TensorRT/neural runtime isn't installed) the ZED SDK is left in a
+		// bad state and ~sl::Camera() crashes inside sl_zed64.dll. Leak the object (one-time,
+		// terminal error path) so we fail gracefully with the message above instead of crashing.
+		zed_ = 0;
+		return false;
+	}
+#endif
+
 	if(r!=sl::ERROR_CODE::SUCCESS)
 	{
+#if ZED_SDK_MAJOR_VERSION >= 4
+		UERROR("Camera initialization failed: \"%s\": %s", toString(r).c_str(), toVerbose(r).c_str());
+#else
 		UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
+#endif
 		delete zed_;
 		zed_ = 0;
 		return false;
@@ -502,7 +603,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 #endif
        if(r!=sl::ERROR_CODE::SUCCESS)
 		{
+#if ZED_SDK_MAJOR_VERSION >= 4
+			UERROR("Camera tracking initialization failed: \"%s\": %s", toString(r).c_str(), toVerbose(r).c_str());
+#else
 			UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
+#endif
 		}
 	}

@@ -731,16 +836,24 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 			res = zed_->grab(rparam);
 			timestamp = zed_->getTimestamp(sl::TIME_REFERENCE::IMAGE);

-			// If the sensor supports IMU, wait IMU to be available before sending data.
+			// If the sensor supports IMU, wait for IMU to be available before sending data.
 			if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull())
 			{
 				 sl::SensorsData imudatatmp;
 				res = zed_->getSensorsData(imudatatmp, sl::TIME_REFERENCE::IMAGE);
-				imuReceived = res == sl::ERROR_CODE::SUCCESS && imudatatmp.imu.is_available && imudatatmp.imu.timestamp.getNanoseconds() != 0;
+				imuReceived = res == sl::ERROR_CODE::SUCCESS && isImuValid(imudatatmp) && imudatatmp.imu.timestamp.getNanoseconds() != 0;
 			}
 		}
 		while(src_ == CameraVideo::kUsbDevice && (res!=sl::ERROR_CODE::SUCCESS || !imuReceived) && timer.elapsed() < 2.0);

+		// If no valid IMU arrived within the 2 sec startup window, null the IMU transform so
+		// we don't re-wait 2 sec on every subsequent frame (camera likely has no working IMU).
+		if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull() && !imuReceived)
+		{
+			UWARN("No valid IMU received within 2 sec; ignoring IMU for the rest of this session.");
+			imuLocalTransform_.setNull();
+		}
+
        if(res==sl::ERROR_CODE::SUCCESS)
 #endif
 		{
@@ -794,7 +907,7 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 #endif
 			}

-			if(imuPublishingThread_ == 0)
+			if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull())
 			{
 #if ZED_SDK_MAJOR_VERSION < 3
 				sl::IMUData imudata;
@@ -803,7 +916,7 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 #else
                sl::SensorsData imudata;
                res = zed_->getSensorsData(imudata, sl::TIME_REFERENCE::IMAGE);
-                if(res == sl::ERROR_CODE::SUCCESS && imudata.imu.is_available)
+                if(res == sl::ERROR_CODE::SUCCESS && isImuValid(imudata))
 #endif
 				{
 					//ZED-Mini
@@ -876,7 +989,11 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 		}
 		else if(src_ == CameraVideo::kUsbDevice)
 		{
+#if ZED_SDK_MAJOR_VERSION >= 4
+			UERROR("CameraStereoZed: Failed to grab images after 2 seconds! (%s: %s)", toString(res).c_str(), toVerbose(res).c_str());
+#else
 			UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
+#endif
 		}
 		else
 		{
--- a/corelib/src/lidar/LidarVLP16.cpp
+++ b/corelib/src/lidar/LidarVLP16.cpp
@@ -34,6 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define VLP_DUAL_MODE 0x39
 #endif

+// RTABMAP_VLP16_USE_CUSTOM_SOCKET is defined (Apple only, and toggleable) in
+// LidarVLP16.h. When set, network mode uses our own single-threaded UDP receive
+// loop; otherwise start()/stop()/isRunning() delegate to pcl::VLPGrabber.
+
 namespace rtabmap {

 /** @brief Function used to check that hour assigned to timestamp in conversion is
@@ -92,8 +96,17 @@ LidarVLP16::LidarVLP16(
 	startSweepTimeHost_(0),
 	organized_(organized),
 	useHostTime_(false),
-	stampLast_(stampLast)
+	stampLast_(stampLast),
+	networkMode_(false),
+	port_(0),
+	receivedScan_(false)
+#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET
+	,socket_(0)
+	,readThread_(0)
+	,terminate_(false)
+#endif
 {
+	// ipAddress_ unused in PCAP mode (default-constructed / unspecified).
 	UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str());
 }
 LidarVLP16::LidarVLP16(
@@ -111,7 +124,19 @@ LidarVLP16::LidarVLP16(
 	startSweepTimeHost_(0),
 	organized_(organized),
 	useHostTime_(useHostTime),
-	stampLast_(stampLast)
+	stampLast_(stampLast),
+	// networkMode_ = live network capture (vs PCAP playback), on all platforms;
+	// it drives the "no data received" warning below. The custom socket path it
+	// also selects in start()/stop() is Apple-only (RTABMAP_VLP16_USE_CUSTOM_SOCKET).
+	networkMode_(true),
+	ipAddress_(ipAddress),
+	port_(port),
+	receivedScan_(false)
+#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET
+	,socket_(0)
+	,readThread_(0)
+	,terminate_(false)
+#endif
 {
 	UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port);
 }
@@ -129,6 +154,132 @@ void LidarVLP16::setOrganized(bool enable)
 	organized_ = true;
 }

+#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET
+void LidarVLP16::start()
+{
+	if(networkMode_)
+	{
+		if(readThread_ != 0)
+		{
+			// already running
+			return;
+		}
+
+		terminate_ = false;
+		// ipAddress_ is the LOCAL interface to listen on (as in pcl::HDLGrabber),
+		// not the sensor's address. Binding to a specific local IP scopes
+		// reception to that NIC on a multi-homed host. If it is unspecified
+		// (0.0.0.0) or not a local address (e.g. the sensor's IP was entered by
+		// mistake), fall back to listening on all interfaces, like pcl::HDLGrabber.
+		try
+		{
+			boost::asio::ip::udp::endpoint endpoint(
+					ipAddress_.is_unspecified() ? boost::asio::ip::address(boost::asio::ip::address_v4::any()) : ipAddress_,
+					port_);
+			// Unlike pcl::HDLGrabber, we read and process packets in a single
+			// thread instead of a producer/consumer queue. A large receive buffer
+			// lets the kernel hold a backlog (~800 VLP16 packets here) so a brief
+			// stall in toPointClouds() doesn't drop packets.
+			const int receiveBufferSize = 1024 * 1024; // 1 MB
+			try
+			{
+				socket_ = new boost::asio::ip::udp::socket(ioContext_);
+				socket_->open(boost::asio::ip::udp::v4());
+				socket_->set_option(boost::asio::socket_base::reuse_address(true));
+				socket_->set_option(boost::asio::socket_base::receive_buffer_size(receiveBufferSize));
+				socket_->bind(endpoint);
+			}
+			catch(const std::exception & e)
+			{
+				UWARN("Could not bind VLP16 socket to local address %s (%s); "
+					"falling back to listening on all interfaces (0.0.0.0).",
+					endpoint.address().to_string().c_str(), e.what());
+				delete socket_;
+				socket_ = new boost::asio::ip::udp::socket(ioContext_);
+				socket_->open(boost::asio::ip::udp::v4());
+				socket_->set_option(boost::asio::socket_base::reuse_address(true));
+				socket_->set_option(boost::asio::socket_base::receive_buffer_size(receiveBufferSize));
+				socket_->bind(boost::asio::ip::udp::endpoint(boost::asio::ip::address_v4::any(), port_));
+			}
+		}
+		catch(const std::exception & e)
+		{
+			UERROR("Failed to bind to VLP16 UDP port %d: %s", (int)port_, e.what());
+			delete socket_;
+			socket_ = 0;
+			return;
+		}
+		receivedScan_ = false;
+		readThread_ = new std::thread(&LidarVLP16::readPackets, this);
+		return;
+	}
+	// PCAP mode: use the base grabber.
+	pcl::VLPGrabber::start();
+}
+
+void LidarVLP16::stop()
+{
+	if(networkMode_)
+	{
+		terminate_ = true;
+		if(socket_ != 0)
+		{
+			// Closing the socket unblocks receive_from in the read thread. Because
+			// that call uses the non-throwing (error_code) overload, the EBADF that
+			// BSD/macOS returns here surfaces as an error code instead of an
+			// exception, so the thread exits cleanly rather than aborting.
+			boost::system::error_code ec;
+			socket_->close(ec);
+		}
+		if(readThread_ != 0)
+		{
+			readThread_->join();
+			delete readThread_;
+			readThread_ = 0;
+		}
+		delete socket_;
+		socket_ = 0;
+		return;
+	}
+	pcl::VLPGrabber::stop();
+}
+
+bool LidarVLP16::isRunning() const
+{
+	if(networkMode_)
+	{
+		return readThread_ != 0;
+	}
+	return pcl::VLPGrabber::isRunning();
+}
+
+void LidarVLP16::readPackets()
+{
+	std::uint8_t data[1500];
+	boost::asio::ip::udp::endpoint sender;
+	while(!terminate_)
+	{
+		boost::system::error_code ec;
+		std::size_t length = socket_->receive_from(boost::asio::buffer(data, sizeof(data)), sender, 0, ec);
+		if(ec)
+		{
+			// On stop() the socket is closed under us (bad_descriptor /
+			// operation_aborted); only warn if it wasn't an expected shutdown.
+			if(!terminate_)
+			{
+				UWARN("VLP16 socket receive error: %s", ec.message().c_str());
+			}
+			break;
+		}
+		// VLP-16 data packets are 1206 bytes; ignore anything shorter.
+		if(length >= 1206)
+		{
+			toPointClouds(reinterpret_cast<HDLDataPacket*>(data));
+		}
+	}
+}
+#endif // RTABMAP_VLP16_USE_CUSTOM_SOCKET
+
 bool LidarVLP16::init(const std::string &, const std::string &)
 {
 	UDEBUG("Init lidar");
@@ -365,8 +516,22 @@ SensorData LidarVLP16::captureData(SensorCaptureInfo * info)
 		{
 			data = lastScan_;
 			lastScan_ = SensorData();
+			receivedScan_ = true;
 		}
 	}
+	else if(networkMode_ && !receivedScan_)
+	{
+		// No packet has ever arrived: most likely the sensor isn't sending data
+		// to this computer. Point the user at the configured listening endpoint.
+		UWARN("Did not receive any VLP16 data packets for the past 5 seconds. "
+			"This computer is listening on %s:%d (%s). Make sure the LiDAR is "
+			"powered on and configured to send its data to this computer's IP "
+			"address on UDP port %d (set the data destination host in the "
+			"LiDAR's own web/configuration interface).",
+			ipAddress_.to_string().c_str(), (int)port_,
+			ipAddress_.is_unspecified() ? "all local interfaces" : "this interface only",
+			(int)port_);
+	}
 	else
 	{
 		UWARN("Did not receive any scans for the past 5 seconds.");