Feature/handle reconnection

config/sick_safety_scanner.yaml

@@ -0,0 +1,20 @@
+sensor_ip: "192.168.1.11"
+host_ip: "192.168.1.9"
+interface_ip: "0.0.0.0"
+host_udp_port: 0
+frame_id: scan
+skip: 0
+angle_start: 0
+angle_end: 0
+time_offset: 0.0
+min_intensities: 0.0
+channel: 0 
+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/SickSafetyscanners.h

@@ -114,57 +114,58 @@ class SickSafetyscanners
    * \brief Changes the internal settings of the sensor.
    * \param settings New set of settings to pass to the sensor.
    */
-  void changeSensorSettings(const sick::datastructure::CommSettings& settings);
+  bool changeSensorSettings(const sick::datastructure::CommSettings& settings);
 
   /*!
    * \brief Requests the typecode of the sensor.
    * \param settings Settings containing information to establish a connection to the sensor.
    * \param type_code Returned typecode.
    */
-  void requestTypeCode(const sick::datastructure::CommSettings& settings,
+  bool requestTypeCode(const sick::datastructure::CommSettings& settings,
                        sick::datastructure::TypeCode& type_code);
 
+  bool requestApplicationName(const sick::datastructure::CommSettings& settings,
+                                sick::datastructure::ApplicationName& application_name);
+
   /*!
    * \brief Requests the config meta data of the sensor.
    * \param settings Settings containing information to establish a connection to the sensor.
    * \param config_metadata Returned config meta data.
    */
-  void requestConfigMetadata(const datastructure::CommSettings& settings,
+  bool requestConfigMetadata(const datastructure::CommSettings& settings,
                              datastructure::ConfigMetadata& config_metadata);
 
   /*!
    * \brief Requests the firmware version of the sensor.
    * \param settings Settings containing information to establish a connection to the sensor.
    * \param firmware_version Returned firmware version.
    */
-  void requestFirmwareVersion(const sick::datastructure::CommSettings& settings,
+  bool requestFirmwareVersion(const sick::datastructure::CommSettings& settings,
                               sick::datastructure::FirmwareVersion& firmware_version);
-
-
-  void requestApplicationName(const sick::datastructure::CommSettings& settings,
-                              sick::datastructure::ApplicationName& application_name);
-  void requestSerialNumber(const sick::datastructure::CommSettings& settings,
+  bool requestSerialNumber(const sick::datastructure::CommSettings& settings,
                            sick::datastructure::SerialNumber& serial_number);
-  void requestOrderNumber(const datastructure::CommSettings& settings,
+
+  bool requestOrderNumber(const datastructure::CommSettings& settings,
                           datastructure::OrderNumber& order_number);
-  void requestProjectName(const datastructure::CommSettings& settings,
+  bool requestProjectName(const datastructure::CommSettings& settings,
                           datastructure::ProjectName& project_name);
-  void requestUserName(const datastructure::CommSettings& settings,
+  bool requestUserName(const datastructure::CommSettings& settings,
                        datastructure::UserName& user_name);
-  void requestStatusOverview(const datastructure::CommSettings& settings,
+
+  bool requestStatusOverview(const datastructure::CommSettings& settings,
                              datastructure::StatusOverview& status_overview);
-  void requestDeviceStatus(const datastructure::CommSettings& settings,
+  bool requestDeviceStatus(const datastructure::CommSettings& settings,
                            datastructure::DeviceStatus& device_status);
-  void requestRequiredUserAction(const datastructure::CommSettings& settings,
+  bool requestRequiredUserAction(const datastructure::CommSettings& settings,
                                  datastructure::RequiredUserAction& required_user_action);
-  void FindSensor(const datastructure::CommSettings& settings, uint16_t blink_time);
+  bool FindSensor(const datastructure::CommSettings& settings, uint16_t blink_time);
   /*!
    * \brief Requests data of the protective and warning fields from the sensor.
    *
    * \param settings Settings containing information to establish a connection to the sensor.
    * \param field_data Returned field data.
    */
-  void requestFieldData(const sick::datastructure::CommSettings& settings,
+  bool requestFieldData(const sick::datastructure::CommSettings& settings,
                         std::vector<sick::datastructure::FieldData>& field_data);
 
   /*!
@@ -173,7 +174,7 @@ class SickSafetyscanners
    * \param settings Settings containing information to establish a connection to the sensor.
    * \param device_name Returned device name.
    */
-  void requestDeviceName(const sick::datastructure::CommSettings& settings,
+  bool requestDeviceName(const sick::datastructure::CommSettings& settings,
                          datastructure::DeviceName& device_name);
 
   /*!
@@ -182,15 +183,15 @@ class SickSafetyscanners
    * \param settings Settings containing information to establish a connection to the sensor.
    * \param config_data Returned persistent configuration data.
    */
-  void requestPersistentConfig(const datastructure::CommSettings& settings,
+  bool requestPersistentConfig(const datastructure::CommSettings& settings,
                                sick::datastructure::ConfigData& config_data);
   /*!
    * \brief Requests the monitoring cases from the sensor.
    *
    * \param settings Settings containing information to establish a connection to the sensor.
    * \param monitoring_cases Returned monitoring cases.
    */
-  void
+  bool
   requestMonitoringCases(const sick::datastructure::CommSettings& settings,
                          std::vector<sick::datastructure::MonitoringCaseData>& monitoring_cases);
 
@@ -211,27 +212,27 @@ class SickSafetyscanners
   void processUDPPacket(const sick::datastructure::PacketBuffer& buffer);
   bool udpClientThread();
   void processTCPPacket(const sick::datastructure::PacketBuffer& buffer);
-  void startTCPConnection(const sick::datastructure::CommSettings& settings);
-  void changeCommSettingsInColaSession(const datastructure::CommSettings& settings);
-  void stopTCPConnection();
-  void requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code);
-  void requestFieldDataInColaSession(std::vector<sick::datastructure::FieldData>& fields);
-  void requestDeviceNameInColaSession(datastructure::DeviceName& device_name);
-  void requestApplicationNameInColaSession(sick::datastructure::ApplicationName& application_name);
-  void requestSerialNumberInColaSession(sick::datastructure::SerialNumber& serial_number);
-  void requestOrderNumberInColaSession(sick::datastructure::OrderNumber& order_number);
-  void requestProjectNameInColaSession(sick::datastructure::ProjectName& project_name);
-  void requestUserNameInColaSession(sick::datastructure::UserName& user_name);
-  void requestFirmwareVersionInColaSession(sick::datastructure::FirmwareVersion& firmware_version);
-  void requestPersistentConfigInColaSession(sick::datastructure::ConfigData& config_data);
-  void requestConfigMetadataInColaSession(sick::datastructure::ConfigMetadata& config_metadata);
-  void requestStatusOverviewInColaSession(sick::datastructure::StatusOverview& status_overview);
-  void requestDeviceStatusInColaSession(sick::datastructure::DeviceStatus& device_status);
-  void requestRequiredUserActionInColaSession(
+  bool startTCPConnection(const sick::datastructure::CommSettings& settings);
+  bool changeCommSettingsInColaSession(const datastructure::CommSettings& settings);
+  bool stopTCPConnection();
+  bool requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code);
+  bool requestFieldDataInColaSession(std::vector<sick::datastructure::FieldData>& fields);
+  bool requestDeviceNameInColaSession(datastructure::DeviceName& device_name);
+  bool requestApplicationNameInColaSession(sick::datastructure::ApplicationName& application_name);
+  bool requestSerialNumberInColaSession(sick::datastructure::SerialNumber& serial_number);
+  bool requestOrderNumberInColaSession(sick::datastructure::OrderNumber& order_number);
+  bool requestProjectNameInColaSession(sick::datastructure::ProjectName& project_name);
+  bool requestUserNameInColaSession(sick::datastructure::UserName& user_name);
+  bool requestFirmwareVersionInColaSession(sick::datastructure::FirmwareVersion& firmware_version);
+  bool requestPersistentConfigInColaSession(sick::datastructure::ConfigData& config_data);
+  bool requestConfigMetadataInColaSession(sick::datastructure::ConfigMetadata& config_metadata);
+  bool requestStatusOverviewInColaSession(sick::datastructure::StatusOverview& status_overview);
+  bool requestDeviceStatusInColaSession(sick::datastructure::DeviceStatus& device_status);
+  bool requestRequiredUserActionInColaSession(
     sick::datastructure::RequiredUserAction& required_user_action);
-  void requestMonitoringCaseDataInColaSession(
+  bool requestMonitoringCaseDataInColaSession(
     std::vector<sick::datastructure::MonitoringCaseData>& monitoring_cases);
-  void FindSensorInColaSession(uint16_t blink_time);
+  bool FindSensorInColaSession(uint16_t blink_time);
 };
 
 } // namespace sick

include/sick_safetyscanners/SickSafetyscannersRos.h

@@ -42,6 +42,7 @@
 #include <ros/ros.h>
 #include <sensor_msgs/JointState.h>
 #include <sensor_msgs/LaserScan.h>
+#include <std_msgs/Bool.h>
 
 // STD
 #include <string>
@@ -134,13 +135,17 @@ class SickSafetyscannersRos
   ros::Publisher m_extended_laser_scan_publisher;
   ros::Publisher m_raw_data_publisher;
   ros::Publisher m_output_path_publisher;
+  ros::Publisher m_connection_status_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;
   sick_safetyscanners::RawMicroScanDataMsg m_last_raw_data;
-  sick::datastructure::ConfigMetadata config_meta_data;
-  sick::datastructure::FirmwareVersion firmware_version;
+  sick::datastructure::ConfigMetadata m_config_meta_data;
+  sick::datastructure::FirmwareVersion m_firmware_version;
   void sensorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status);
 
   ros::ServiceServer m_field_service_server;
@@ -170,6 +175,11 @@ class SickSafetyscannersRos
   bool m_use_sick_angles;
   float m_angle_offset;
   bool m_use_pers_conf;
+  bool m_connected;
+
+  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.
@@ -221,11 +231,14 @@ class SickSafetyscannersRos
   createApplicationInputsMessage(const sick::datastructure::Data& data);
   sick_safetyscanners::ApplicationOutputsMsg
   createApplicationOutputsMessage(const sick::datastructure::Data& data);
-  void readTypeCodeSettings();
-  void readPersistentConfig();
+  bool readTypeCodeSettings();
+  bool readPersistentConfig();
 
   bool getFieldData(sick_safetyscanners::FieldData::Request& req,
                     sick_safetyscanners::FieldData::Response& res);
+
+  void udpConnectionMonitorHandler();
+  bool setCommunicationSettingScanner();
 
   bool getConfigMetadata(sick_safetyscanners::ConfigMetadata::Request& req,
                          sick_safetyscanners::ConfigMetadata::Response& res);

include/sick_safetyscanners/cola2/Cola2Session.h

@@ -89,7 +89,7 @@ class Cola2Session
   /*!
    * \brief Triggers the disconnection of the tcp socket.
    */
-  void doDisconnect();
+  bool doDisconnect();
 
   /*!
    * \brief Executes the command passed to the function.

include/sick_safetyscanners/communication/AsyncTCPClient.h

@@ -89,12 +89,12 @@ class AsyncTCPClient
   /*!
    * \brief Establishes a connection from the host to the sensor.
    */
-  void doConnect();
+  bool doConnect();
 
   /*!
    * \brief Disconnects the host from the sensor
    */
-  void doDisconnect();
+  bool doDisconnect();
 
   /*!
    * \brief Start a cycle of sensing a command and waiting got the return.

launch/sick_safetyscanners.launch

@@ -1,46 +1,5 @@
 <launch>
-  <arg name="sensor_ip"             default="192.168.1.11" />
-  <arg name="host_ip"               default="192.168.1.9" />
-  <arg name="interface_ip"          default="0.0.0.0" />
-  <arg name="host_udp_port"         default="0" />
-  <arg name="frame_id"              default="scan" />
-  <arg name="skip"                  default="0" />
-  <arg name="angle_start"           default="0" />
-  <arg name="angle_end"             default="0" />
-  <arg name="time_offset"           default="0.0" />
-  <arg name="min_intensities"       default="0.0" doc="minimal intensity for a laserscan point" />
-  <arg name="channel"               default="0" />
-  <arg name="channel_enabled"       default="True" />
-  <arg name="general_system_state"  default="True" />
-  <arg name="derived_settings"      default="True" />
-  <arg name="measurement_data"      default="True" />
-  <arg name="intrusion_data"        default="True" />
-  <arg name="application_io_data"   default="True" />
-  <arg name="use_persistent_config"   default="False" />
-
-  <!-- Launch Sick SickSafetyscanners Ros Driver Node -->
-  <node pkg="sick_safetyscanners" type="sick_safetyscanners_node" name="sick_safetyscanners" output="screen" ns="sick_safetyscanners">
-     <param name="sensor_ip"              type="string" value="$(arg sensor_ip)" />
-     <param name="host_ip"                type="string" value="$(arg host_ip)" />
-     <param name="interface_ip"           type="string" value="$(arg interface_ip)" />
-     <param name="host_udp_port"          type="int"    value="$(arg host_udp_port)" />
-     <param name="frame_id"               type="string" value="$(arg frame_id)" />
-     <param name="skip"                   type="int"    value="$(arg skip)" />
-     <param name="angle_start"            type="double" value="$(arg angle_start)" />
-     <param name="angle_end"              type="double" value="$(arg angle_end)" />
-     <param name="time_offset"            type="double" value="$(arg time_offset)" />
-     <param name="min_intensities"        type="double" value="$(arg min_intensities)" />
-     <param name="channel"                type="int"    value="$(arg channel)" />
-     <param name="channel_enabled"        type="bool"   value="$(arg channel_enabled)" />
-     <param name="general_system_state"   type="bool"   value="$(arg general_system_state)" />
-     <param name="derived_settings"       type="bool"   value="$(arg derived_settings)" />
-     <param name="measurement_data"       type="bool"   value="$(arg measurement_data)" />
-     <param name="intrusion_data"         type="bool"   value="$(arg intrusion_data)" />
-     <param name="application_io_data"    type="bool"   value="$(arg application_io_data)" />
-     <param name="use_persistent_config"  type="bool"   value="$(arg use_persistent_config)" />
+  <node pkg="sick_safetyscanners" type="sick_safetyscanners_node" name="sick_safetyscanners" output="screen" ns="sick_safetyscanners" respawn="true">
+    <rosparam file="$(find sick_safetyscanners)config/sick_safety_scanner.yaml" command="load"/>
   </node>
-
-
-  <!-- node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" /-->
 </launch>
-

msg/GeneralSystemStateMsg.msg

@@ -14,5 +14,6 @@ uint8 current_monitoring_case_no_table_2
 uint8 current_monitoring_case_no_table_3
 uint8 current_monitoring_case_no_table_4
 
+bool connection_status
 bool application_error
 bool device_error