Fix freeze in hdl_grabber.cpp (#5826)

* Fix freeze in hdl_grabber.cpp

Close the udp socket before stopping HDLGrabber. Tested only on Windows.

* Fix freeze in hdl_grabber.cpp
Address PR comments to fix HDLGrabber freeze on Windows and Linux.

---------

Co-authored-by: SunlayGGX <[email protected]>

io/src/hdl_grabber.cpp

@@ -536,6 +536,20 @@ pcl::HDLGrabber::stop ()
   terminate_read_packet_thread_ = true;
   hdl_data_.stopQueue ();
 
+  if (hdl_read_socket_ != nullptr)
+  {
+    try
+    {
+      hdl_read_socket_->shutdown (boost::asio::ip::tcp::socket::shutdown_both);
+    }
+    catch (const boost::system::system_error& e)
+    {
+      PCL_ERROR("[pcl::HDLGrabber::stop] Failed to shutdown the socket. %s\n", e.what ());
+    }
+
+    hdl_read_socket_->close ();
+  }
+
   if (hdl_read_packet_thread_ != nullptr)
   {
     hdl_read_packet_thread_->join ();
@@ -645,12 +659,21 @@ pcl::HDLGrabber::readPacketsFromSocket ()
 
   while (!terminate_read_packet_thread_ && hdl_read_socket_->is_open ())
   {
-    std::size_t length = hdl_read_socket_->receive_from (boost::asio::buffer (data, 1500), sender_endpoint);
+    try
+    {
+      std::size_t length = hdl_read_socket_->receive_from (boost::asio::buffer (data, 1500), sender_endpoint);
 
-    if (isAddressUnspecified (source_address_filter_)
-        || (source_address_filter_ == sender_endpoint.address () && source_port_filter_ == sender_endpoint.port ()))
+      if (isAddressUnspecified (source_address_filter_)
+          || (source_address_filter_ == sender_endpoint.address () && source_port_filter_ == sender_endpoint.port ()))
+      {
+        enqueueHDLPacket (data, length);
+      }
+    }
+    catch (const boost::system::system_error& e)
     {
-      enqueueHDLPacket (data, length);
+      // We must ignore those errors triggered on socket close/shutdown request.
+      if (!(e.code () == boost::asio::error::interrupted || e.code () == boost::asio::error::operation_aborted))
+        throw;
     }
   }
 }