added configurable option for monitoring udp connection

config/rear_lidar.yaml

@@ -0,0 +1,17 @@
+sensor_ip: "192.168.12.12"
+host_ip: "192.168.12.8"
+host_udp_port: 0
+frame_id: rear_lidar_link
+skip: 0
+angle_start: -2.3561944901923448
+angle_end: 2.3561944901923448
+time_offset: -0.065
+channel_enabled: True
+general_system_state: True
+derived_settings: True
+measurement_data: True
+intrusion_data: True
+application_io_data: True
+use_persistent_config: False
+udp_connection_monitor : True
+udp_connection_monitor_watchdog_timeout_ms : 5000

include/sick_safetyscanners/SickSafetyscannersRos.h

@@ -133,6 +133,9 @@ class SickSafetyscannersRos
   ros::Publisher m_raw_data_publisher;
   ros::Publisher m_output_path_publisher;
 
+  // ROS Timer
+  ros::Timer m_udp_connection_monitor_timer;
+
   // Diagnostics
   diagnostic_updater::Updater m_diagnostic_updater;
   std::shared_ptr<DiagnosedLaserScanPublisher> m_diagnosed_laser_scan_publisher;
@@ -165,6 +168,10 @@ class SickSafetyscannersRos
   float m_angle_offset;
   bool m_use_pers_conf;
 
+  bool m_udp_connection_monitor;
+  uint32_t m_connection_monitor_watchdog_timeout_ms;
+  double m_last_udp_pkt_received;
+
   /*!
    * @brief Reads and verifies the ROS parameters.
    * @return True if successful.
@@ -220,6 +227,8 @@ class SickSafetyscannersRos
 
   bool getFieldData(sick_safetyscanners::FieldData::Request& req,
                     sick_safetyscanners::FieldData::Response& res);
+
+  void udpConnectionMonitorHandler();
 };
 
 } // namespace sick

launch/sick_safetyscanner_rear.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="sick_safetyscanners" type="sick_safetyscanners_node" name="sick_safetyscanners" output="screen" ns="rear_lidar" respawn="false">
+    <rosparam file="$(find sick_safetyscanners)config/rear_lidar.yaml" command="load"/>
+  </node>
+</launch>

src/SickSafetyscannersRos.cpp

@@ -91,6 +91,16 @@ SickSafetyscannersRos::SickSafetyscannersRos()
   }
 
   m_device->changeSensorSettings(m_communication_settings);
+
+  if (m_udp_connection_monitor)
+  {
+    m_udp_connection_monitor_timer = m_nh.createTimer(ros::Duration((m_connection_monitor_watchdog_timeout_ms/1000)),
+                                          std::bind(&SickSafetyscannersRos::udpConnectionMonitorHandler, this));
+
+    /* Initialization */
+    m_last_udp_pkt_received = ros::Time::now().toSec();
+  }
+
   m_initialised = true;
   ROS_INFO("Successfully launched node.");
 }
@@ -246,6 +256,32 @@ bool SickSafetyscannersRos::readParameters()
 
   m_communication_settings.setFeatures(
     general_system_state, derived_settings, measurement_data, intrusion_data, application_io_data);
+
+
+  m_udp_connection_monitor = false;
+  if (!m_private_nh.getParam("udp_connection_monitor", m_udp_connection_monitor))
+  {
+    ROS_WARN("Using default setting for udp connection monitoring : %s", m_udp_connection_monitor ? "true" : "false");
+  }
+  else
+  {
+    ROS_INFO("Udp connection monitoring : %s", m_udp_connection_monitor ? "true" : "false");
+  }
+
+  if (m_udp_connection_monitor)
+  {
+    int udp_connection_timeout = 5000;
+    if (!m_private_nh.getParam("udp_connection_monitor_watchdog_timeout_ms", udp_connection_timeout))
+    {
+      ROS_WARN("Using default udp monitor watchdog time: %d ms", udp_connection_timeout);
+    }
+    else
+    {
+      ROS_INFO("udp connection monitor watchdog timeour : %d ms", udp_connection_timeout);
+    }
+
+    m_connection_monitor_watchdog_timeout_ms = udp_connection_timeout;
+  }
 
   m_private_nh.getParam("frame_id", m_frame_id);
 
@@ -280,6 +316,7 @@ void SickSafetyscannersRos::receivedUDPPacket(const sick::datastructure::Data& d
   m_raw_data_publisher.publish(m_last_raw_data);
 
   m_diagnostic_updater.update();
+  m_last_udp_pkt_received = ros::Time::now().toSec();
 }
 
 std::string boolToString(bool b)
@@ -829,5 +866,13 @@ bool SickSafetyscannersRos::getFieldData(sick_safetyscanners::FieldData::Request
   return true;
 }
 
+void SickSafetyscannersRos::udpConnectionMonitorHandler()
+{
+  double time_now = ros::Time::now().toSec();
+  if ((time_now - m_last_udp_pkt_received) > (m_connection_monitor_watchdog_timeout_ms/1000))
+  {
+    ROS_WARN("No udp packet received for %f , Shutting down the node", time_now - m_last_udp_pkt_received);
+  }
+}
 
 } // namespace sick