i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 02:27:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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)
This commit is contained in:
matlabbe
2026-07-04 18:16:37 -07:00
committed by GitHub
parent cb34c4bd37
commit d069becc72
27 changed files with 982 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/lidar/LidarVLP16.cpp
+167 -2
View File
@@ -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.");