From 430eb3df65835bcc244692ac7e5cc076dee918fd Mon Sep 17 00:00:00 2001 From: smalik007 Date: Tue, 23 May 2023 15:35:58 +0200 Subject: [PATCH 01/11] added configurable option for monitoring udp connection --- config/rear_lidar.yaml | 17 +++++++ .../SickSafetyscannersRos.h | 9 ++++ launch/sick_safetyscanner_rear.launch | 5 +++ src/SickSafetyscannersRos.cpp | 45 +++++++++++++++++++ 4 files changed, 76 insertions(+) create mode 100644 config/rear_lidar.yaml create mode 100644 launch/sick_safetyscanner_rear.launch diff --git a/config/rear_lidar.yaml b/config/rear_lidar.yaml new file mode 100644 index 0000000..d3ce437 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/include/sick_safetyscanners/SickSafetyscannersRos.h b/include/sick_safetyscanners/SickSafetyscannersRos.h index 7a999bd..895da68 100644 --- a/include/sick_safetyscanners/SickSafetyscannersRos.h +++ b/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 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 diff --git a/launch/sick_safetyscanner_rear.launch b/launch/sick_safetyscanner_rear.launch new file mode 100644 index 0000000..84719f7 --- /dev/null +++ b/launch/sick_safetyscanner_rear.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index cb8ea23..7c806b8 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/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 From 9d3300c80ba06774ed1b4770982596ed38dbd6b5 Mon Sep 17 00:00:00 2001 From: smalik007 Date: Tue, 23 May 2023 15:53:21 +0200 Subject: [PATCH 02/11] Added shutdown cmd --- launch/sick_safetyscanner_rear.launch | 2 +- src/SickSafetyscannersRos.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/sick_safetyscanner_rear.launch b/launch/sick_safetyscanner_rear.launch index 84719f7..91ac0bf 100644 --- a/launch/sick_safetyscanner_rear.launch +++ b/launch/sick_safetyscanner_rear.launch @@ -1,5 +1,5 @@ - + diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 7c806b8..0097705 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -872,6 +872,7 @@ void SickSafetyscannersRos::udpConnectionMonitorHandler() 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); + ros::requestShutdown(); } } From 931bb8f8b3e896559f53aed433a0fd45d7e8702c Mon Sep 17 00:00:00 2001 From: smalik007 Date: Wed, 24 May 2023 10:50:59 +0200 Subject: [PATCH 03/11] simplified launch file to use yaml config for ros params --- config/sick_safety_scanner.yaml | 20 ++++++++++++++ launch/sick_safetyscanners.launch | 45 ++----------------------------- 2 files changed, 22 insertions(+), 43 deletions(-) create mode 100644 config/sick_safety_scanner.yaml diff --git a/config/sick_safety_scanner.yaml b/config/sick_safety_scanner.yaml new file mode 100644 index 0000000..ebe43a4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/launch/sick_safetyscanners.launch b/launch/sick_safetyscanners.launch index cff7cbd..bbafdbc 100644 --- a/launch/sick_safetyscanners.launch +++ b/launch/sick_safetyscanners.launch @@ -1,46 +1,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - From 6dbf14be188e29858bb740637a55f47f463f5598 Mon Sep 17 00:00:00 2001 From: smalik007 Date: Thu, 25 May 2023 14:04:40 +0200 Subject: [PATCH 04/11] converted tcp function return to bool --- .../sick_safetyscanners/SickSafetyscanners.h | 70 ++-- .../SickSafetyscannersRos.h | 4 +- .../sick_safetyscanners/cola2/Cola2Session.h | 2 +- .../communication/AsyncTCPClient.h | 4 +- src/SickSafetyscanners.cpp | 376 +++++++++++++----- src/SickSafetyscannersRos.cpp | 32 +- src/cola2/Cola2Session.cpp | 16 +- src/communication/AsyncTCPClient.cpp | 18 +- 8 files changed, 356 insertions(+), 166 deletions(-) diff --git a/include/sick_safetyscanners/SickSafetyscanners.h b/include/sick_safetyscanners/SickSafetyscanners.h index 33c5d3a..b0f8f0d 100644 --- a/include/sick_safetyscanners/SickSafetyscanners.h +++ b/include/sick_safetyscanners/SickSafetyscanners.h @@ -114,44 +114,44 @@ 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); - void requestApplicationName(const sick::datastructure::CommSettings& settings, + bool 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 requestFirmwareVersion(const sick::datastructure::CommSettings& settings, + bool requestFirmwareVersion(const sick::datastructure::CommSettings& settings, sick::datastructure::FirmwareVersion& firmware_version); - 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 requestConfigMetadata(const datastructure::CommSettings& settings, + bool requestConfigMetadata(const datastructure::CommSettings& settings, datastructure::ConfigMetadata& config_metadata); - 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& field_data); /*! @@ -160,7 +160,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); /*! @@ -169,7 +169,7 @@ 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. @@ -177,7 +177,7 @@ class SickSafetyscanners * \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& monitoring_cases); @@ -198,27 +198,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); + bool startTCPConnection(const sick::datastructure::CommSettings& settings); + bool changeCommSettingsInColaSession(const datastructure::CommSettings& settings); void stopTCPConnection(); - void requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code); - void requestFieldDataInColaSession(std::vector& 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 requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code); + bool requestFieldDataInColaSession(std::vector& 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& monitoring_cases); - void FindSensorInColaSession(uint16_t blink_time); + bool FindSensorInColaSession(uint16_t blink_time); }; } // namespace sick diff --git a/include/sick_safetyscanners/SickSafetyscannersRos.h b/include/sick_safetyscanners/SickSafetyscannersRos.h index 895da68..d172722 100644 --- a/include/sick_safetyscanners/SickSafetyscannersRos.h +++ b/include/sick_safetyscanners/SickSafetyscannersRos.h @@ -222,8 +222,8 @@ 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); diff --git a/include/sick_safetyscanners/cola2/Cola2Session.h b/include/sick_safetyscanners/cola2/Cola2Session.h index cc8d12e..b3056a7 100644 --- a/include/sick_safetyscanners/cola2/Cola2Session.h +++ b/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. diff --git a/include/sick_safetyscanners/communication/AsyncTCPClient.h b/include/sick_safetyscanners/communication/AsyncTCPClient.h index a87f041..a5903a6 100644 --- a/include/sick_safetyscanners/communication/AsyncTCPClient.h +++ b/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. diff --git a/src/SickSafetyscanners.cpp b/src/SickSafetyscanners.cpp index 534b443..38083df 100644 --- a/src/SickSafetyscanners.cpp +++ b/src/SickSafetyscanners.cpp @@ -101,169 +101,300 @@ void SickSafetyscanners::processTCPPacket(const sick::datastructure::PacketBuffe // Not needed for current functionality, inplace for possible future developments } -void SickSafetyscanners::changeSensorSettings(const datastructure::CommSettings& settings) +bool SickSafetyscanners::changeSensorSettings(const datastructure::CommSettings& settings) { - startTCPConnection(settings); - changeCommSettingsInColaSession(settings); - stopTCPConnection(); + bool status = false; + + status = startTCPConnection(settings); + if (true == status) + { + changeCommSettingsInColaSession(settings); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::FindSensor(const datastructure::CommSettings& settings, +bool SickSafetyscanners::FindSensor(const datastructure::CommSettings& settings, uint16_t blink_time) { - startTCPConnection(settings); - FindSensorInColaSession(blink_time); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + FindSensorInColaSession(blink_time); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestTypeCode(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestTypeCode(const datastructure::CommSettings& settings, sick::datastructure::TypeCode& type_code) { - startTCPConnection(settings); - requestTypeCodeInColaSession(type_code); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestTypeCodeInColaSession(type_code); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestApplicationName( +bool SickSafetyscanners::requestApplicationName( const datastructure::CommSettings& settings, sick::datastructure::ApplicationName& application_name) { - startTCPConnection(settings); - requestApplicationNameInColaSession(application_name); - stopTCPConnection(); + + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestApplicationNameInColaSession(application_name); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestFieldData(const datastructure::CommSettings& settings, + +bool SickSafetyscanners::requestFieldData(const datastructure::CommSettings& settings, std::vector& field_data) { - startTCPConnection(settings); - requestFieldDataInColaSession(field_data); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestFieldDataInColaSession(field_data); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestMonitoringCases( +bool SickSafetyscanners::requestMonitoringCases( const datastructure::CommSettings& settings, std::vector& monitoring_cases) { - startTCPConnection(settings); - requestMonitoringCaseDataInColaSession(monitoring_cases); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestMonitoringCaseDataInColaSession(monitoring_cases); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestDeviceName(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestDeviceName(const datastructure::CommSettings& settings, datastructure::DeviceName& device_name) { - startTCPConnection(settings); - requestDeviceNameInColaSession(device_name); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestDeviceNameInColaSession(device_name); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestSerialNumber(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestSerialNumber(const datastructure::CommSettings& settings, datastructure::SerialNumber& serial_number) { - startTCPConnection(settings); - requestSerialNumberInColaSession(serial_number); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestSerialNumberInColaSession(serial_number); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestOrderNumber(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestOrderNumber(const datastructure::CommSettings& settings, datastructure::OrderNumber& order_number) { - startTCPConnection(settings); - requestOrderNumberInColaSession(order_number); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestOrderNumberInColaSession(order_number); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestProjectName(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestProjectName(const datastructure::CommSettings& settings, datastructure::ProjectName& project_name) { - startTCPConnection(settings); - requestProjectNameInColaSession(project_name); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestProjectNameInColaSession(project_name); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestUserName(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestUserName(const datastructure::CommSettings& settings, datastructure::UserName& user_name) { - startTCPConnection(settings); - requestUserNameInColaSession(user_name); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestUserNameInColaSession(user_name); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestFirmwareVersion(const datastructure::CommSettings& settings, + +bool SickSafetyscanners::requestFirmwareVersion(const datastructure::CommSettings& settings, datastructure::FirmwareVersion& firmware_version) { - startTCPConnection(settings); - requestFirmwareVersionInColaSession(firmware_version); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestFirmwareVersionInColaSession(firmware_version); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestPersistentConfig(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestPersistentConfig(const datastructure::CommSettings& settings, sick::datastructure::ConfigData& config_data) { - startTCPConnection(settings); - requestPersistentConfigInColaSession(config_data); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestPersistentConfigInColaSession(config_data); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestConfigMetadata(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestConfigMetadata(const datastructure::CommSettings& settings, sick::datastructure::ConfigMetadata& config_metadata) { - startTCPConnection(settings); - requestConfigMetadataInColaSession(config_metadata); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestConfigMetadataInColaSession(config_metadata); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestStatusOverview(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestStatusOverview(const datastructure::CommSettings& settings, sick::datastructure::StatusOverview& status_overview) { - startTCPConnection(settings); - requestStatusOverviewInColaSession(status_overview); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestStatusOverviewInColaSession(status_overview); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestDeviceStatus(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestDeviceStatus(const datastructure::CommSettings& settings, sick::datastructure::DeviceStatus& device_status) { - startTCPConnection(settings); - requestDeviceStatusInColaSession(device_status); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestDeviceStatusInColaSession(device_status); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::requestRequiredUserAction( +bool SickSafetyscanners::requestRequiredUserAction( const datastructure::CommSettings& settings, sick::datastructure::RequiredUserAction& required_user_action) { - startTCPConnection(settings); - requestRequiredUserActionInColaSession(required_user_action); - stopTCPConnection(); + bool status = false; + status = startTCPConnection(settings); + + if (true == status) + { + requestRequiredUserActionInColaSession(required_user_action); + stopTCPConnection(); + } + + return status; } -void SickSafetyscanners::startTCPConnection(const sick::datastructure::CommSettings& settings) +bool SickSafetyscanners::startTCPConnection(const sick::datastructure::CommSettings& settings) { + bool status = false; std::shared_ptr async_tcp_client = std::make_shared( boost::bind(&SickSafetyscanners::processTCPPacket, this, _1), boost::ref(*m_io_service_ptr), settings.getSensorIp(), settings.getSensorTcpPort()); - async_tcp_client->doConnect(); + + status = async_tcp_client->doConnect(); - m_session_ptr.reset(); - m_session_ptr = std::make_shared(async_tcp_client); + if (true == status) + { + m_session_ptr.reset(); + m_session_ptr = std::make_shared(async_tcp_client); + + status = m_session_ptr->open(); + } - m_session_ptr->open(); + return status; } -void SickSafetyscanners::changeCommSettingsInColaSession( +bool SickSafetyscanners::changeCommSettingsInColaSession( const datastructure::CommSettings& settings) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), settings); - m_session_ptr->executeCommand(command_ptr); + return m_session_ptr->executeCommand(command_ptr); } -void SickSafetyscanners::requestFieldDataInColaSession( +bool SickSafetyscanners::requestFieldDataInColaSession( std::vector& fields) { + bool status = false; + sick::cola2::Cola2Session::CommandPtr command_ptr; for (int i = 0; i < 128; i++) @@ -272,13 +403,14 @@ void SickSafetyscanners::requestFieldDataInColaSession( command_ptr = std::make_shared( boost::ref(*m_session_ptr), field_data, i); - m_session_ptr->executeCommand(command_ptr); + + status = m_session_ptr->executeCommand(command_ptr); if (field_data.getIsValid()) { command_ptr = std::make_shared( boost::ref(*m_session_ptr), field_data, i); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); fields.push_back(field_data); } @@ -287,11 +419,15 @@ void SickSafetyscanners::requestFieldDataInColaSession( break; // skip other requests after first invalid } } + + return status; } -void SickSafetyscanners::requestMonitoringCaseDataInColaSession( +bool SickSafetyscanners::requestMonitoringCaseDataInColaSession( std::vector& monitoring_cases) { + bool status = false; + sick::cola2::Cola2Session::CommandPtr command_ptr; for (int i = 0; i < 254; i++) { @@ -299,7 +435,7 @@ void SickSafetyscanners::requestMonitoringCaseDataInColaSession( command_ptr = std::make_shared( boost::ref(*m_session_ptr), monitoring_case_data, i); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); if (monitoring_case_data.getIsValid()) { monitoring_cases.push_back(monitoring_case_data); @@ -309,134 +445,158 @@ void SickSafetyscanners::requestMonitoringCaseDataInColaSession( break; // skip other requests after first invalid } } + + return status; } -void SickSafetyscanners::FindSensorInColaSession(uint16_t blink_time) +bool SickSafetyscanners::FindSensorInColaSession(uint16_t blink_time) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), blink_time); - m_session_ptr->executeCommand(command_ptr); + return m_session_ptr->executeCommand(command_ptr); } -void SickSafetyscanners::requestDeviceNameInColaSession(datastructure::DeviceName& device_name) +bool SickSafetyscanners::requestDeviceNameInColaSession(datastructure::DeviceName& device_name) { + bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), device_name); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("Device name: %s", device_name.getDeviceName().c_str()); + + return status; } -void SickSafetyscanners::requestApplicationNameInColaSession( +bool SickSafetyscanners::requestApplicationNameInColaSession( datastructure::ApplicationName& application_name) { + bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), application_name); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("Application name: %s", application_name.getApplicationName().c_str()); + + return status; } -void SickSafetyscanners::requestSerialNumberInColaSession( +bool SickSafetyscanners::requestSerialNumberInColaSession( datastructure::SerialNumber& serial_number) { + bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), serial_number); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("Serial Number: %s", serial_number.getSerialNumber().c_str()); + + return status; } -void SickSafetyscanners::requestFirmwareVersionInColaSession( +bool SickSafetyscanners::requestFirmwareVersionInColaSession( datastructure::FirmwareVersion& firmware_version) { + bool status = false; + sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), firmware_version); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("Firmware Version: %s", firmware_version.getFirmwareVersion().c_str()); + + return status; } -void SickSafetyscanners::requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code) +bool SickSafetyscanners::requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code) { + bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), type_code); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("Type Code: %s", type_code.getTypeCode().c_str()); + + return status; } -void SickSafetyscanners::requestOrderNumberInColaSession( +bool SickSafetyscanners::requestOrderNumberInColaSession( sick::datastructure::OrderNumber& order_number) { + bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), order_number); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("Order Number: %s", order_number.getOrderNumber().c_str()); + return status; } -void SickSafetyscanners::requestProjectNameInColaSession( +bool SickSafetyscanners::requestProjectNameInColaSession( sick::datastructure::ProjectName& project_name) { + bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), project_name); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("Project Name: %s", project_name.getProjectName().c_str()); + return status; } -void SickSafetyscanners::requestUserNameInColaSession(sick::datastructure::UserName& user_name) +bool SickSafetyscanners::requestUserNameInColaSession(sick::datastructure::UserName& user_name) { + bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), user_name); - m_session_ptr->executeCommand(command_ptr); + status = m_session_ptr->executeCommand(command_ptr); ROS_INFO("User Name: %s", user_name.getUserName().c_str()); + return status; } -void SickSafetyscanners::requestConfigMetadataInColaSession( +bool SickSafetyscanners::requestConfigMetadataInColaSession( sick::datastructure::ConfigMetadata& config_metadata) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), config_metadata); - m_session_ptr->executeCommand(command_ptr); + return m_session_ptr->executeCommand(command_ptr); } -void SickSafetyscanners::requestStatusOverviewInColaSession( +bool SickSafetyscanners::requestStatusOverviewInColaSession( sick::datastructure::StatusOverview& status_overview) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), status_overview); - m_session_ptr->executeCommand(command_ptr); + return m_session_ptr->executeCommand(command_ptr); } -void SickSafetyscanners::requestDeviceStatusInColaSession( +bool SickSafetyscanners::requestDeviceStatusInColaSession( sick::datastructure::DeviceStatus& device_status) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), device_status); - m_session_ptr->executeCommand(command_ptr); + return m_session_ptr->executeCommand(command_ptr); } -void SickSafetyscanners::requestRequiredUserActionInColaSession( +bool SickSafetyscanners::requestRequiredUserActionInColaSession( sick::datastructure::RequiredUserAction& required_user_action) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), required_user_action); - m_session_ptr->executeCommand(command_ptr); + return m_session_ptr->executeCommand(command_ptr); } -void SickSafetyscanners::requestPersistentConfigInColaSession( +bool SickSafetyscanners::requestPersistentConfigInColaSession( sick::datastructure::ConfigData& config_data) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared( boost::ref(*m_session_ptr), config_data); - m_session_ptr->executeCommand(command_ptr); + return m_session_ptr->executeCommand(command_ptr); } void SickSafetyscanners::stopTCPConnection() diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 0097705..1d98c42 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -105,23 +105,35 @@ SickSafetyscannersRos::SickSafetyscannersRos() ROS_INFO("Successfully launched node."); } -void SickSafetyscannersRos::readTypeCodeSettings() +bool SickSafetyscannersRos::readTypeCodeSettings() { + bool status = false; ROS_INFO("Reading Type code settings"); sick::datastructure::TypeCode type_code; - m_device->requestTypeCode(m_communication_settings, type_code); - m_communication_settings.setEInterfaceType(type_code.getInterfaceType()); - m_range_min = 0.1; - m_range_max = type_code.getMaxRange(); -} + status = m_device->requestTypeCode(m_communication_settings, type_code); + if (true == status) + { + m_communication_settings.setEInterfaceType(type_code.getInterfaceType()); + m_range_min = 0.1; + m_range_max = type_code.getMaxRange(); + } + + return status; +} -void SickSafetyscannersRos::readPersistentConfig() +bool SickSafetyscannersRos::readPersistentConfig() { + bool status = false; ROS_INFO("Reading Persistent Configuration"); sick::datastructure::ConfigData config_data; - m_device->requestPersistentConfig(m_communication_settings, config_data); - m_communication_settings.setStartAngle(config_data.getStartAngle()); - m_communication_settings.setEndAngle(config_data.getEndAngle()); + status = m_device->requestPersistentConfig(m_communication_settings, config_data); + if (true == status) + { + m_communication_settings.setStartAngle(config_data.getStartAngle()); + m_communication_settings.setEndAngle(config_data.getEndAngle()); + } + + return status; } void SickSafetyscannersRos::reconfigureCallback( diff --git a/src/cola2/Cola2Session.cpp b/src/cola2/Cola2Session.cpp index 6fcc056..0529fcb 100644 --- a/src/cola2/Cola2Session.cpp +++ b/src/cola2/Cola2Session.cpp @@ -59,16 +59,22 @@ bool Cola2Session::close() return executeCommand(command_ptr); } -void Cola2Session::doDisconnect() +bool Cola2Session::doDisconnect() { - m_async_tcp_client_ptr->doDisconnect(); + return m_async_tcp_client_ptr->doDisconnect(); } bool Cola2Session::executeCommand(const CommandPtr& command) { - addCommand(command->getRequestID(), command); - sendTelegramAndListenForAnswer(command); - return true; + bool status = false; + status = addCommand(command->getRequestID(), command); + + if (true == status) + { + status = sendTelegramAndListenForAnswer(command); + } + + return status; } bool Cola2Session::sendTelegramAndListenForAnswer(const CommandPtr& command) diff --git a/src/communication/AsyncTCPClient.cpp b/src/communication/AsyncTCPClient.cpp index d37c6fb..d68605d 100644 --- a/src/communication/AsyncTCPClient.cpp +++ b/src/communication/AsyncTCPClient.cpp @@ -60,48 +60,60 @@ AsyncTCPClient::AsyncTCPClient(const PacketHandler& packet_handler, AsyncTCPClient::~AsyncTCPClient() {} -void AsyncTCPClient::doDisconnect() +bool AsyncTCPClient::doDisconnect() { + bool status = false; boost::mutex::scoped_lock lock(m_socket_mutex); boost::system::error_code ec; m_socket_ptr->shutdown(boost::asio::ip::tcp::socket::shutdown_both, ec); if (ec != boost::system::errc::success) { ROS_ERROR("Error shutting socket down: %i", ec.value()); + status = false; } else { ROS_INFO("TCP Connection successfully shutdown"); + status = true; } m_socket_ptr->close(ec); if (ec != boost::system::errc::success) { ROS_ERROR("Error closing Socket: %i", ec.value()); + status = false; } else { ROS_INFO("TCP Socket successfully closed."); + status = true; } + + return status; } -void AsyncTCPClient::doConnect() +bool AsyncTCPClient::doConnect() { + bool status = false; boost::mutex::scoped_lock lock(m_socket_mutex); boost::mutex::scoped_lock lock_connect(m_connect_mutex); - m_socket_ptr->async_connect(m_remote_endpoint, [this](boost::system::error_code ec) { + m_socket_ptr->async_connect(m_remote_endpoint, [this, &status](boost::system::error_code ec) { if (ec != boost::system::errc::success) { ROS_ERROR("TCP error code: %i", ec.value()); + status = false; } else { ROS_INFO("TCP connection successfully established."); + status = true; } m_connect_condition.notify_all(); }); m_connect_condition.wait(lock_connect); + + return status; } From b129298c7175be61924e0abe766867f3e405812c Mon Sep 17 00:00:00 2001 From: smalik007 Date: Thu, 25 May 2023 14:33:53 +0200 Subject: [PATCH 05/11] added infinite retries for tcp request --- src/SickSafetyscannersRos.cpp | 33 ++++++++++++++++++++++++--------- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 1d98c42..82c9700 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -34,7 +34,8 @@ #include "sick_safetyscanners/SickSafetyscannersRos.h" - +#include +#include namespace sick { @@ -78,20 +79,34 @@ SickSafetyscannersRos::SickSafetyscannersRos() m_laser_scan_publisher, m_diagnostic_updater, frequency_param, timestamp_param)); m_diagnostic_updater.add("State", this, &SickSafetyscannersRos::sensorDiagnostics); + bool status = false; + while (!status) + { + status = readTypeCodeSettings(); + if (m_use_pers_conf && status) + { + status = readPersistentConfig(); + } + + if (status) + { + status = m_device->changeSensorSettings(m_communication_settings); + } + + if (!status) + { + int retry_after = 1000; + ROS_WARN("Retry tcp request after %d ms", retry_after); + std::this_thread::sleep_for (std::chrono::milliseconds(retry_after)); + } + } + m_device = std::make_shared( boost::bind(&SickSafetyscannersRos::receivedUDPPacket, this, _1), &m_communication_settings, m_interface_ip); m_device->run(); - readTypeCodeSettings(); - if (m_use_pers_conf) - { - readPersistentConfig(); - } - - 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)), From 6e06c1ad2dc530769a7f33b074aa3f5caee2100b Mon Sep 17 00:00:00 2001 From: smalik007 Date: Wed, 31 May 2023 13:15:03 +0200 Subject: [PATCH 06/11] resend cola msg on reconnection --- launch/sick_safetyscanner_rear.launch | 2 +- src/SickSafetyscannersRos.cpp | 37 +++++++++++++++++++++------ 2 files changed, 30 insertions(+), 9 deletions(-) diff --git a/launch/sick_safetyscanner_rear.launch b/launch/sick_safetyscanner_rear.launch index 91ac0bf..84719f7 100644 --- a/launch/sick_safetyscanner_rear.launch +++ b/launch/sick_safetyscanner_rear.launch @@ -1,5 +1,5 @@ - + diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 82c9700..3513293 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -79,6 +79,12 @@ SickSafetyscannersRos::SickSafetyscannersRos() m_laser_scan_publisher, m_diagnostic_updater, frequency_param, timestamp_param)); m_diagnostic_updater.add("State", this, &SickSafetyscannersRos::sensorDiagnostics); + m_device = std::make_shared( + boost::bind(&SickSafetyscannersRos::receivedUDPPacket, this, _1), + &m_communication_settings, + m_interface_ip); + m_device->run(); + bool status = false; while (!status) { @@ -100,12 +106,6 @@ SickSafetyscannersRos::SickSafetyscannersRos() std::this_thread::sleep_for (std::chrono::milliseconds(retry_after)); } } - - m_device = std::make_shared( - boost::bind(&SickSafetyscannersRos::receivedUDPPacket, this, _1), - &m_communication_settings, - m_interface_ip); - m_device->run(); if (m_udp_connection_monitor) { @@ -898,8 +898,29 @@ 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); - ros::requestShutdown(); + ROS_WARN("No udp packet received for %f , Trying to re-establish connection", time_now - m_last_udp_pkt_received); + + bool status = false; + while (!status) + { + status = readTypeCodeSettings(); + if (m_use_pers_conf && status) + { + status = readPersistentConfig(); + } + + if (status) + { + status = m_device->changeSensorSettings(m_communication_settings); + } + + if (!status) + { + int retry_after = 1000; + ROS_WARN("Retry tcp request after %d ms", retry_after); + std::this_thread::sleep_for (std::chrono::milliseconds(retry_after)); + } + } } } From 1b50198b474204fda47e1976aab6fdcc82cd8200 Mon Sep 17 00:00:00 2001 From: smalik007 Date: Wed, 31 May 2023 14:13:08 +0200 Subject: [PATCH 07/11] Added configurable options --- config/rear_lidar.yaml | 2 + config/sick_safety_scanner.yaml | 2 + .../SickSafetyscannersRos.h | 4 + src/SickSafetyscannersRos.cpp | 93 +++++++++++-------- 4 files changed, 61 insertions(+), 40 deletions(-) diff --git a/config/rear_lidar.yaml b/config/rear_lidar.yaml index d3ce437..728a71c 100644 --- a/config/rear_lidar.yaml +++ b/config/rear_lidar.yaml @@ -13,5 +13,7 @@ measurement_data: True intrusion_data: True application_io_data: True use_persistent_config: False +tcp_request_retry_ms : 1000 +tcp_max_request_retries : 65535 udp_connection_monitor : True udp_connection_monitor_watchdog_timeout_ms : 5000 \ No newline at end of file diff --git a/config/sick_safety_scanner.yaml b/config/sick_safety_scanner.yaml index ebe43a4..7697c14 100644 --- a/config/sick_safety_scanner.yaml +++ b/config/sick_safety_scanner.yaml @@ -16,5 +16,7 @@ measurement_data: True intrusion_data: True application_io_data: True use_persistent_config: False +tcp_request_retry_ms : 1000 +tcp_max_request_retries : 10 udp_connection_monitor : True udp_connection_monitor_watchdog_timeout_ms : 5000 \ No newline at end of file diff --git a/include/sick_safetyscanners/SickSafetyscannersRos.h b/include/sick_safetyscanners/SickSafetyscannersRos.h index d172722..d595998 100644 --- a/include/sick_safetyscanners/SickSafetyscannersRos.h +++ b/include/sick_safetyscanners/SickSafetyscannersRos.h @@ -168,6 +168,9 @@ class SickSafetyscannersRos float m_angle_offset; bool m_use_pers_conf; + uint32_t m_tcp_request_retry_ms; + int m_tcp_max_request_retries; + bool m_udp_connection_monitor; uint32_t m_connection_monitor_watchdog_timeout_ms; double m_last_udp_pkt_received; @@ -229,6 +232,7 @@ class SickSafetyscannersRos sick_safetyscanners::FieldData::Response& res); void udpConnectionMonitorHandler(); + void setCommunicationSettingScanner(); }; } // namespace sick diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 3513293..8aa9635 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -85,27 +85,7 @@ SickSafetyscannersRos::SickSafetyscannersRos() m_interface_ip); m_device->run(); - bool status = false; - while (!status) - { - status = readTypeCodeSettings(); - if (m_use_pers_conf && status) - { - status = readPersistentConfig(); - } - - if (status) - { - status = m_device->changeSensorSettings(m_communication_settings); - } - - if (!status) - { - int retry_after = 1000; - ROS_WARN("Retry tcp request after %d ms", retry_after); - std::this_thread::sleep_for (std::chrono::milliseconds(retry_after)); - } - } + setCommunicationSettingScanner(); if (m_udp_connection_monitor) { @@ -284,6 +264,27 @@ bool SickSafetyscannersRos::readParameters() m_communication_settings.setFeatures( general_system_state, derived_settings, measurement_data, intrusion_data, application_io_data); + int tcp_request_retry_ms = 1000; + if (!m_private_nh.getParam("tcp_request_retry_ms", tcp_request_retry_ms)) + { + ROS_WARN("Using default tcp request retry time: %d ms", tcp_request_retry_ms); + } + else + { + ROS_INFO("tcp request retry time : %d ms", tcp_request_retry_ms); + } + m_tcp_request_retry_ms = tcp_request_retry_ms; + + int tcp_max_request_retries = 10; + if (!m_private_nh.getParam("tcp_max_request_retries", tcp_max_request_retries)) + { + ROS_WARN("Using default tcp max request retries: %d", tcp_max_request_retries); + } + else + { + ROS_INFO("max tcp request retries : %d", tcp_max_request_retries); + } + m_tcp_max_request_retries = tcp_max_request_retries; m_udp_connection_monitor = false; if (!m_private_nh.getParam("udp_connection_monitor", m_udp_connection_monitor)) @@ -304,7 +305,7 @@ bool SickSafetyscannersRos::readParameters() } else { - ROS_INFO("udp connection monitor watchdog timeour : %d ms", udp_connection_timeout); + ROS_INFO("udp connection monitor watchdog timeout : %d ms", udp_connection_timeout); } m_connection_monitor_watchdog_timeout_ms = udp_connection_timeout; @@ -899,28 +900,40 @@ void SickSafetyscannersRos::udpConnectionMonitorHandler() if ((time_now - m_last_udp_pkt_received) > (m_connection_monitor_watchdog_timeout_ms/1000)) { ROS_WARN("No udp packet received for %f , Trying to re-establish connection", time_now - m_last_udp_pkt_received); - - bool status = false; - while (!status) + setCommunicationSettingScanner(); + } +} + +void SickSafetyscannersRos::setCommunicationSettingScanner() +{ + bool status = false; + int retries = 0; + + while (!status && retries < m_tcp_max_request_retries) + { + status = readTypeCodeSettings(); + if (m_use_pers_conf && status) { - status = readTypeCodeSettings(); - if (m_use_pers_conf && status) - { - status = readPersistentConfig(); - } + status = readPersistentConfig(); + } - if (status) - { - status = m_device->changeSensorSettings(m_communication_settings); - } + if (status) + { + status = m_device->changeSensorSettings(m_communication_settings); + } - if (!status) - { - int retry_after = 1000; - ROS_WARN("Retry tcp request after %d ms", retry_after); - std::this_thread::sleep_for (std::chrono::milliseconds(retry_after)); - } + if (!status) + { + ROS_WARN("Retry tcp request after %d ms", m_tcp_request_retry_ms); + std::this_thread::sleep_for (std::chrono::milliseconds(m_tcp_request_retry_ms)); } + retries++; + } + + if (false == status && retries >= m_tcp_max_request_retries) + { + ROS_WARN("Could not establish connection, max retries exhausted : %d, Shutting down the node", m_tcp_max_request_retries); + ros::requestShutdown(); } } From 2b948d05ac8bac65cf5adbdf8423c8e9e2ec1edd Mon Sep 17 00:00:00 2001 From: smalik007 Date: Wed, 31 May 2023 15:59:43 +0200 Subject: [PATCH 08/11] Added connection status in driver diagnostics --- include/sick_safetyscanners/SickSafetyscannersRos.h | 3 ++- msg/GeneralSystemStateMsg.msg | 1 + src/SickSafetyscannersRos.cpp | 9 +++++++++ 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/sick_safetyscanners/SickSafetyscannersRos.h b/include/sick_safetyscanners/SickSafetyscannersRos.h index d595998..043419a 100644 --- a/include/sick_safetyscanners/SickSafetyscannersRos.h +++ b/include/sick_safetyscanners/SickSafetyscannersRos.h @@ -167,7 +167,8 @@ class SickSafetyscannersRos bool m_use_sick_angles; float m_angle_offset; bool m_use_pers_conf; - + bool m_connection_status; + uint32_t m_tcp_request_retry_ms; int m_tcp_max_request_retries; diff --git a/msg/GeneralSystemStateMsg.msg b/msg/GeneralSystemStateMsg.msg index 3b3b705..32a2bd4 100644 --- a/msg/GeneralSystemStateMsg.msg +++ b/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 diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 8aa9635..99f8847 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -47,6 +47,7 @@ SickSafetyscannersRos::SickSafetyscannersRos() , m_range_max(0.0) , m_angle_offset(-90.0) , m_use_pers_conf(false) + , m_connection_status(false) { dynamic_reconfigure::Server< sick_safetyscanners::SickSafetyscannersConfigurationConfig>::CallbackType reconf_callback = @@ -97,6 +98,7 @@ SickSafetyscannersRos::SickSafetyscannersRos() } m_initialised = true; + m_connection_status = true; ROS_INFO("Successfully launched node."); } @@ -391,6 +393,7 @@ void SickSafetyscannersRos::sensorDiagnostics( "Current monitoring case no table 3", "%u", state.current_monitoring_case_no_table_3); diagnostic_status.addf( "Current monitoring case no table 4", "%u", state.current_monitoring_case_no_table_4); + diagnostic_status.add("Connection Status", boolToString(state.connection_status)); diagnostic_status.add("Application error", boolToString(state.application_error)); diagnostic_status.add("Device error", boolToString(state.device_error)); @@ -654,6 +657,7 @@ SickSafetyscannersRos::createGeneralSystemStateMessage(const sick::datastructure msg.current_monitoring_case_no_table_4 = general_system_state->getCurrentMonitoringCaseNoTable4(); + msg.connection_status = m_connection_status; msg.application_error = general_system_state->getApplicationError(); msg.device_error = general_system_state->getDeviceError(); } @@ -900,6 +904,7 @@ void SickSafetyscannersRos::udpConnectionMonitorHandler() if ((time_now - m_last_udp_pkt_received) > (m_connection_monitor_watchdog_timeout_ms/1000)) { ROS_WARN("No udp packet received for %f , Trying to re-establish connection", time_now - m_last_udp_pkt_received); + m_connection_status = false; setCommunicationSettingScanner(); } } @@ -935,6 +940,10 @@ void SickSafetyscannersRos::setCommunicationSettingScanner() ROS_WARN("Could not establish connection, max retries exhausted : %d, Shutting down the node", m_tcp_max_request_retries); ros::requestShutdown(); } + else + { + m_connection_status = true; + } } } // namespace sick From ecc65f19895f2bbb8a8d97f96f3e5dff05bf8194 Mon Sep 17 00:00:00 2001 From: smalik007 Date: Thu, 1 Jun 2023 14:27:39 +0200 Subject: [PATCH 09/11] publish last raw data to have the connection status reflected on diagnostics --- src/SickSafetyscannersRos.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 99f8847..b34a5a2 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -904,7 +904,10 @@ void SickSafetyscannersRos::udpConnectionMonitorHandler() if ((time_now - m_last_udp_pkt_received) > (m_connection_monitor_watchdog_timeout_ms/1000)) { ROS_WARN("No udp packet received for %f , Trying to re-establish connection", time_now - m_last_udp_pkt_received); - m_connection_status = false; + m_connection_status = false; + m_last_raw_data.general_system_state.connection_status = m_connection_status; + m_raw_data_publisher.publish(m_last_raw_data); + m_diagnostic_updater.update(); setCommunicationSettingScanner(); } } From b2855a2b9a4431b8d41c875e721ceed7528c8ac0 Mon Sep 17 00:00:00 2001 From: smalik007 Date: Thu, 1 Jun 2023 14:29:45 +0200 Subject: [PATCH 10/11] removed duplicated files --- config/rear_lidar.yaml | 19 ------------------- launch/sick_safetyscanner_rear.launch | 5 ----- 2 files changed, 24 deletions(-) delete mode 100644 config/rear_lidar.yaml delete mode 100644 launch/sick_safetyscanner_rear.launch diff --git a/config/rear_lidar.yaml b/config/rear_lidar.yaml deleted file mode 100644 index 728a71c..0000000 --- a/config/rear_lidar.yaml +++ /dev/null @@ -1,19 +0,0 @@ -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 -tcp_request_retry_ms : 1000 -tcp_max_request_retries : 65535 -udp_connection_monitor : True -udp_connection_monitor_watchdog_timeout_ms : 5000 \ No newline at end of file diff --git a/launch/sick_safetyscanner_rear.launch b/launch/sick_safetyscanner_rear.launch deleted file mode 100644 index 84719f7..0000000 --- a/launch/sick_safetyscanner_rear.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - From 2ac945a55506122be6c6e81784bd626ffbce8428 Mon Sep 17 00:00:00 2001 From: smalik007 Date: Tue, 20 Jun 2023 14:53:11 +0200 Subject: [PATCH 11/11] further code improvements --- config/sick_safety_scanner.yaml | 4 +- .../sick_safetyscanners/SickSafetyscanners.h | 2 +- .../SickSafetyscannersRos.h | 9 +- src/SickSafetyscanners.cpp | 330 ++++++++---------- src/SickSafetyscannersRos.cpp | 128 +++---- src/cola2/Cola2Session.cpp | 11 +- src/communication/AsyncTCPClient.cpp | 2 +- 7 files changed, 205 insertions(+), 281 deletions(-) diff --git a/config/sick_safety_scanner.yaml b/config/sick_safety_scanner.yaml index 7697c14..1c290a4 100644 --- a/config/sick_safety_scanner.yaml +++ b/config/sick_safety_scanner.yaml @@ -16,7 +16,5 @@ measurement_data: True intrusion_data: True application_io_data: True use_persistent_config: False -tcp_request_retry_ms : 1000 -tcp_max_request_retries : 10 udp_connection_monitor : True -udp_connection_monitor_watchdog_timeout_ms : 5000 \ No newline at end of file +udp_connection_monitor_watchdog_timeout_ms : 5000 diff --git a/include/sick_safetyscanners/SickSafetyscanners.h b/include/sick_safetyscanners/SickSafetyscanners.h index 6e42656..9aca50e 100644 --- a/include/sick_safetyscanners/SickSafetyscanners.h +++ b/include/sick_safetyscanners/SickSafetyscanners.h @@ -214,7 +214,7 @@ class SickSafetyscanners void processTCPPacket(const sick::datastructure::PacketBuffer& buffer); bool startTCPConnection(const sick::datastructure::CommSettings& settings); bool changeCommSettingsInColaSession(const datastructure::CommSettings& settings); - void stopTCPConnection(); + bool stopTCPConnection(); bool requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code); bool requestFieldDataInColaSession(std::vector& fields); bool requestDeviceNameInColaSession(datastructure::DeviceName& device_name); diff --git a/include/sick_safetyscanners/SickSafetyscannersRos.h b/include/sick_safetyscanners/SickSafetyscannersRos.h index c01b659..e1017d5 100644 --- a/include/sick_safetyscanners/SickSafetyscannersRos.h +++ b/include/sick_safetyscanners/SickSafetyscannersRos.h @@ -42,6 +42,7 @@ #include #include #include +#include // STD #include @@ -134,6 +135,7 @@ 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; @@ -173,10 +175,7 @@ class SickSafetyscannersRos bool m_use_sick_angles; float m_angle_offset; bool m_use_pers_conf; - bool m_connection_status; - - uint32_t m_tcp_request_retry_ms; - int m_tcp_max_request_retries; + bool m_connected; bool m_udp_connection_monitor; uint32_t m_connection_monitor_watchdog_timeout_ms; @@ -239,7 +238,7 @@ class SickSafetyscannersRos sick_safetyscanners::FieldData::Response& res); void udpConnectionMonitorHandler(); - void setCommunicationSettingScanner(); + bool setCommunicationSettingScanner(); bool getConfigMetadata(sick_safetyscanners::ConfigMetadata::Request& req, sick_safetyscanners::ConfigMetadata::Response& res); diff --git a/src/SickSafetyscanners.cpp b/src/SickSafetyscanners.cpp index 38083df..6d85d81 100644 --- a/src/SickSafetyscanners.cpp +++ b/src/SickSafetyscanners.cpp @@ -103,265 +103,212 @@ void SickSafetyscanners::processTCPPacket(const sick::datastructure::PacketBuffe bool SickSafetyscanners::changeSensorSettings(const datastructure::CommSettings& settings) { - bool status = false; + if (!startTCPConnection(settings)) + return false; - status = startTCPConnection(settings); - if (true == status) - { - changeCommSettingsInColaSession(settings); - stopTCPConnection(); - } + if (!changeCommSettingsInColaSession(settings)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::FindSensor(const datastructure::CommSettings& settings, uint16_t blink_time) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - FindSensorInColaSession(blink_time); - stopTCPConnection(); - } + if (!FindSensorInColaSession(blink_time)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestTypeCode(const datastructure::CommSettings& settings, sick::datastructure::TypeCode& type_code) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestTypeCodeInColaSession(type_code); - stopTCPConnection(); - } + if (!requestTypeCodeInColaSession(type_code)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestApplicationName( const datastructure::CommSettings& settings, sick::datastructure::ApplicationName& application_name) { + if (!startTCPConnection(settings)) + return false; - bool status = false; - status = startTCPConnection(settings); - - if (true == status) - { - requestApplicationNameInColaSession(application_name); - stopTCPConnection(); - } + if (!requestApplicationNameInColaSession(application_name)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestFieldData(const datastructure::CommSettings& settings, std::vector& field_data) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestFieldDataInColaSession(field_data); - stopTCPConnection(); - } + if (!requestFieldDataInColaSession(field_data)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestMonitoringCases( const datastructure::CommSettings& settings, std::vector& monitoring_cases) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestMonitoringCaseDataInColaSession(monitoring_cases); - stopTCPConnection(); - } + if (!requestMonitoringCaseDataInColaSession(monitoring_cases)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestDeviceName(const datastructure::CommSettings& settings, datastructure::DeviceName& device_name) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestDeviceNameInColaSession(device_name); - stopTCPConnection(); - } + if (!requestDeviceNameInColaSession(device_name)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestSerialNumber(const datastructure::CommSettings& settings, datastructure::SerialNumber& serial_number) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestSerialNumberInColaSession(serial_number); - stopTCPConnection(); - } + if (!requestSerialNumberInColaSession(serial_number)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestOrderNumber(const datastructure::CommSettings& settings, datastructure::OrderNumber& order_number) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestOrderNumberInColaSession(order_number); - stopTCPConnection(); - } + if (!requestOrderNumberInColaSession(order_number)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestProjectName(const datastructure::CommSettings& settings, datastructure::ProjectName& project_name) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestProjectNameInColaSession(project_name); - stopTCPConnection(); - } + if (!requestProjectNameInColaSession(project_name)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestUserName(const datastructure::CommSettings& settings, datastructure::UserName& user_name) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestUserNameInColaSession(user_name); - stopTCPConnection(); - } + if (!requestUserNameInColaSession(user_name)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestFirmwareVersion(const datastructure::CommSettings& settings, datastructure::FirmwareVersion& firmware_version) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestFirmwareVersionInColaSession(firmware_version); - stopTCPConnection(); - } + if (!requestFirmwareVersionInColaSession(firmware_version)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestPersistentConfig(const datastructure::CommSettings& settings, sick::datastructure::ConfigData& config_data) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestPersistentConfigInColaSession(config_data); - stopTCPConnection(); - } + if (!requestPersistentConfigInColaSession(config_data)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestConfigMetadata(const datastructure::CommSettings& settings, sick::datastructure::ConfigMetadata& config_metadata) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestConfigMetadataInColaSession(config_metadata); - stopTCPConnection(); - } + if (!requestConfigMetadataInColaSession(config_metadata)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestStatusOverview(const datastructure::CommSettings& settings, sick::datastructure::StatusOverview& status_overview) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestStatusOverviewInColaSession(status_overview); - stopTCPConnection(); - } + if (!requestStatusOverviewInColaSession(status_overview)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestDeviceStatus(const datastructure::CommSettings& settings, sick::datastructure::DeviceStatus& device_status) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestDeviceStatusInColaSession(device_status); - stopTCPConnection(); - } + if (!requestDeviceStatusInColaSession(device_status)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::requestRequiredUserAction( const datastructure::CommSettings& settings, sick::datastructure::RequiredUserAction& required_user_action) { - bool status = false; - status = startTCPConnection(settings); + if (!startTCPConnection(settings)) + return false; - if (true == status) - { - requestRequiredUserActionInColaSession(required_user_action); - stopTCPConnection(); - } + if (!requestRequiredUserActionInColaSession(required_user_action)) + return false; - return status; + return stopTCPConnection(); } bool SickSafetyscanners::startTCPConnection(const sick::datastructure::CommSettings& settings) { - bool status = false; std::shared_ptr async_tcp_client = std::make_shared( boost::bind(&SickSafetyscanners::processTCPPacket, this, _1), @@ -369,17 +316,14 @@ bool SickSafetyscanners::startTCPConnection(const sick::datastructure::CommSetti settings.getSensorIp(), settings.getSensorTcpPort()); - status = async_tcp_client->doConnect(); - - if (true == status) + if(!async_tcp_client->doConnect()) { - m_session_ptr.reset(); - m_session_ptr = std::make_shared(async_tcp_client); - - status = m_session_ptr->open(); + return false; } - return status; + m_session_ptr.reset(); + m_session_ptr = std::make_shared(async_tcp_client); + return m_session_ptr->open(); } bool SickSafetyscanners::changeCommSettingsInColaSession( @@ -393,8 +337,6 @@ bool SickSafetyscanners::changeCommSettingsInColaSession( bool SickSafetyscanners::requestFieldDataInColaSession( std::vector& fields) { - bool status = false; - sick::cola2::Cola2Session::CommandPtr command_ptr; for (int i = 0; i < 128; i++) @@ -404,13 +346,18 @@ bool SickSafetyscanners::requestFieldDataInColaSession( command_ptr = std::make_shared( boost::ref(*m_session_ptr), field_data, i); - status = m_session_ptr->executeCommand(command_ptr); + /* return early if fails to execute */ + if (!m_session_ptr->executeCommand(command_ptr)) + return false; if (field_data.getIsValid()) { command_ptr = std::make_shared( boost::ref(*m_session_ptr), field_data, i); - status = m_session_ptr->executeCommand(command_ptr); + + /* return early if fails to execute */ + if (!m_session_ptr->executeCommand(command_ptr)) + return false; fields.push_back(field_data); } @@ -420,14 +367,12 @@ bool SickSafetyscanners::requestFieldDataInColaSession( } } - return status; + return true; } bool SickSafetyscanners::requestMonitoringCaseDataInColaSession( std::vector& monitoring_cases) { - bool status = false; - sick::cola2::Cola2Session::CommandPtr command_ptr; for (int i = 0; i < 254; i++) { @@ -435,7 +380,11 @@ bool SickSafetyscanners::requestMonitoringCaseDataInColaSession( command_ptr = std::make_shared( boost::ref(*m_session_ptr), monitoring_case_data, i); - status = m_session_ptr->executeCommand(command_ptr); + + /* Early return on failure */ + if(!m_session_ptr->executeCommand(command_ptr)) + return false; + if (monitoring_case_data.getIsValid()) { monitoring_cases.push_back(monitoring_case_data); @@ -446,7 +395,7 @@ bool SickSafetyscanners::requestMonitoringCaseDataInColaSession( } } - return status; + return true; } bool SickSafetyscanners::FindSensorInColaSession(uint16_t blink_time) @@ -458,100 +407,105 @@ bool SickSafetyscanners::FindSensorInColaSession(uint16_t blink_time) bool SickSafetyscanners::requestDeviceNameInColaSession(datastructure::DeviceName& device_name) { - bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), device_name); - status = m_session_ptr->executeCommand(command_ptr); - ROS_INFO("Device name: %s", device_name.getDeviceName().c_str()); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; - return status; + ROS_INFO("Device name: %s", device_name.getDeviceName().c_str()); + return true; } bool SickSafetyscanners::requestApplicationNameInColaSession( datastructure::ApplicationName& application_name) { - bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), application_name); - status = m_session_ptr->executeCommand(command_ptr); - ROS_INFO("Application name: %s", application_name.getApplicationName().c_str()); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; - return status; + ROS_INFO("Application name: %s", application_name.getApplicationName().c_str()); + return true; } bool SickSafetyscanners::requestSerialNumberInColaSession( datastructure::SerialNumber& serial_number) { - bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), serial_number); - status = m_session_ptr->executeCommand(command_ptr); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("Serial Number: %s", serial_number.getSerialNumber().c_str()); - return status; + return true; } bool SickSafetyscanners::requestFirmwareVersionInColaSession( datastructure::FirmwareVersion& firmware_version) { - bool status = false; - sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), firmware_version); - status = m_session_ptr->executeCommand(command_ptr); - ROS_INFO("Firmware Version: %s", firmware_version.getFirmwareVersion().c_str()); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; - return status; + ROS_INFO("Firmware Version: %s", firmware_version.getFirmwareVersion().c_str()); + return true; } bool SickSafetyscanners::requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code) { - bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), type_code); - status = m_session_ptr->executeCommand(command_ptr); - ROS_INFO("Type Code: %s", type_code.getTypeCode().c_str()); - return status; + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + + ROS_INFO("Type Code: %s", type_code.getTypeCode().c_str()); + return true; } bool SickSafetyscanners::requestOrderNumberInColaSession( sick::datastructure::OrderNumber& order_number) { - bool status = false; + sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), order_number); - status = m_session_ptr->executeCommand(command_ptr); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("Order Number: %s", order_number.getOrderNumber().c_str()); - return status; + return true; } bool SickSafetyscanners::requestProjectNameInColaSession( sick::datastructure::ProjectName& project_name) { - bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), project_name); - status = m_session_ptr->executeCommand(command_ptr); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("Project Name: %s", project_name.getProjectName().c_str()); - return status; + return true; } bool SickSafetyscanners::requestUserNameInColaSession(sick::datastructure::UserName& user_name) { - bool status = false; sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), user_name); - status = m_session_ptr->executeCommand(command_ptr); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("User Name: %s", user_name.getUserName().c_str()); - return status; + return true; } bool SickSafetyscanners::requestConfigMetadataInColaSession( @@ -599,10 +553,12 @@ bool SickSafetyscanners::requestPersistentConfigInColaSession( return m_session_ptr->executeCommand(command_ptr); } -void SickSafetyscanners::stopTCPConnection() +bool SickSafetyscanners::stopTCPConnection() { - m_session_ptr->close(); - m_session_ptr->doDisconnect(); + if (!m_session_ptr->close()) + return false; + + return m_session_ptr->doDisconnect(); } diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index dfea7f7..4dc5ebb 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -47,7 +47,7 @@ SickSafetyscannersRos::SickSafetyscannersRos() , m_range_max(0.0) , m_angle_offset(-90.0) , m_use_pers_conf(false) - , m_connection_status(false) + , m_connected(false) { dynamic_reconfigure::Server< sick_safetyscanners::SickSafetyscannersConfigurationConfig>::CallbackType reconf_callback = @@ -66,6 +66,9 @@ SickSafetyscannersRos::SickSafetyscannersRos() m_raw_data_publisher = m_nh.advertise("raw_data", 100); m_output_path_publisher = m_nh.advertise("output_paths", 100); + + m_connection_status_publisher = m_nh.advertise("connection_status", 1, true); // latched + m_field_service_server = m_nh.advertiseService("field_data", &SickSafetyscannersRos::getFieldData, this); @@ -91,7 +94,11 @@ SickSafetyscannersRos::SickSafetyscannersRos() m_interface_ip); m_device->run(); - setCommunicationSettingScanner(); + if(!setCommunicationSettingScanner()) + { + ROS_WARN("Couldn't establish TCP connection"); + } + if (m_udp_connection_monitor) { @@ -104,39 +111,36 @@ SickSafetyscannersRos::SickSafetyscannersRos() } m_initialised = true; - m_connection_status = true; ROS_INFO("Successfully launched node."); } bool SickSafetyscannersRos::readTypeCodeSettings() { - bool status = false; ROS_INFO("Reading Type code settings"); sick::datastructure::TypeCode type_code; - status = m_device->requestTypeCode(m_communication_settings, type_code); - if (true == status) - { - m_communication_settings.setEInterfaceType(type_code.getInterfaceType()); - m_range_min = 0.1; - m_range_max = type_code.getMaxRange(); - } + + if (!m_device->requestTypeCode(m_communication_settings, type_code)) + return false; + + m_communication_settings.setEInterfaceType(type_code.getInterfaceType()); + m_range_min = 0.1; + m_range_max = type_code.getMaxRange(); - return status; + return true; } bool SickSafetyscannersRos::readPersistentConfig() { - bool status = false; ROS_INFO("Reading Persistent Configuration"); sick::datastructure::ConfigData config_data; - status = m_device->requestPersistentConfig(m_communication_settings, config_data); - if (true == status) - { - m_communication_settings.setStartAngle(config_data.getStartAngle()); - m_communication_settings.setEndAngle(config_data.getEndAngle()); - } - return status; + if (!m_device->requestPersistentConfig(m_communication_settings, config_data)) + return false; + + m_communication_settings.setStartAngle(config_data.getStartAngle()); + m_communication_settings.setEndAngle(config_data.getEndAngle()); + + return true; } void SickSafetyscannersRos::reconfigureCallback( @@ -272,28 +276,6 @@ bool SickSafetyscannersRos::readParameters() m_communication_settings.setFeatures( general_system_state, derived_settings, measurement_data, intrusion_data, application_io_data); - int tcp_request_retry_ms = 1000; - if (!m_private_nh.getParam("tcp_request_retry_ms", tcp_request_retry_ms)) - { - ROS_WARN("Using default tcp request retry time: %d ms", tcp_request_retry_ms); - } - else - { - ROS_INFO("tcp request retry time : %d ms", tcp_request_retry_ms); - } - m_tcp_request_retry_ms = tcp_request_retry_ms; - - int tcp_max_request_retries = 10; - if (!m_private_nh.getParam("tcp_max_request_retries", tcp_max_request_retries)) - { - ROS_WARN("Using default tcp max request retries: %d", tcp_max_request_retries); - } - else - { - ROS_INFO("max tcp request retries : %d", tcp_max_request_retries); - } - m_tcp_max_request_retries = tcp_max_request_retries; - m_udp_connection_monitor = false; if (!m_private_nh.getParam("udp_connection_monitor", m_udp_connection_monitor)) { @@ -679,7 +661,7 @@ SickSafetyscannersRos::createGeneralSystemStateMessage(const sick::datastructure msg.current_monitoring_case_no_table_4 = general_system_state->getCurrentMonitoringCaseNoTable4(); - msg.connection_status = m_connection_status; + msg.connection_status = m_connected; msg.application_error = general_system_state->getApplicationError(); msg.device_error = general_system_state->getDeviceError(); } @@ -923,54 +905,48 @@ bool SickSafetyscannersRos::getFieldData(sick_safetyscanners::FieldData::Request 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)) + /* Re-establish connection only when scanner was previously connected */ + if (((time_now - m_last_udp_pkt_received) > (m_connection_monitor_watchdog_timeout_ms/1000)) && m_connected) { ROS_WARN("No udp packet received for %f , Trying to re-establish connection", time_now - m_last_udp_pkt_received); - m_connection_status = false; - m_last_raw_data.general_system_state.connection_status = m_connection_status; + + m_connected = false; + m_last_raw_data.general_system_state.connection_status = m_connected; m_raw_data_publisher.publish(m_last_raw_data); m_diagnostic_updater.update(); + + std_msgs::Bool connection_status; + connection_status.data = m_connected; + m_connection_status_publisher.publish(connection_status); + setCommunicationSettingScanner(); + } + else if (!m_connected) /* Timer-CB and scanner still not connected, so try again */ + { setCommunicationSettingScanner(); } } -void SickSafetyscannersRos::setCommunicationSettingScanner() +bool SickSafetyscannersRos::setCommunicationSettingScanner() { - bool status = false; - int retries = 0; + /* If tcp request fails return early */ + if (!readTypeCodeSettings()) + return false; - while (!status && retries < m_tcp_max_request_retries) + if (m_use_pers_conf) { - status = readTypeCodeSettings(); - if (m_use_pers_conf && status) - { - status = readPersistentConfig(); + readPersistentConfig(); } - if (status) - { - status = m_device->changeSensorSettings(m_communication_settings); - status = m_device->requestConfigMetadata(m_communication_settings, m_config_meta_data); - status = m_device->requestFirmwareVersion(m_communication_settings, m_firmware_version); - } + m_device->changeSensorSettings(m_communication_settings); + m_device->requestConfigMetadata(m_communication_settings, m_config_meta_data); + m_device->requestFirmwareVersion(m_communication_settings, m_firmware_version); - if (!status) - { - ROS_WARN("Retry tcp request after %d ms", m_tcp_request_retry_ms); - std::this_thread::sleep_for (std::chrono::milliseconds(m_tcp_request_retry_ms)); - } - retries++; - } + m_connected = true; + std_msgs::Bool connection_status; + connection_status.data = m_connected; + m_connection_status_publisher.publish(connection_status); - if (false == status && retries >= m_tcp_max_request_retries) - { - ROS_WARN("Could not establish connection, max retries exhausted : %d, Shutting down the node", m_tcp_max_request_retries); - ros::requestShutdown(); - } - else - { - m_connection_status = true; - } + return true; } bool SickSafetyscannersRos::getConfigMetadata(sick_safetyscanners::ConfigMetadata::Request& req, diff --git a/src/cola2/Cola2Session.cpp b/src/cola2/Cola2Session.cpp index 0529fcb..84335bc 100644 --- a/src/cola2/Cola2Session.cpp +++ b/src/cola2/Cola2Session.cpp @@ -66,15 +66,10 @@ bool Cola2Session::doDisconnect() bool Cola2Session::executeCommand(const CommandPtr& command) { - bool status = false; - status = addCommand(command->getRequestID(), command); - - if (true == status) - { - status = sendTelegramAndListenForAnswer(command); - } + if (!addCommand(command->getRequestID(), command)) + return false; - return status; + return sendTelegramAndListenForAnswer(command); } bool Cola2Session::sendTelegramAndListenForAnswer(const CommandPtr& command) diff --git a/src/communication/AsyncTCPClient.cpp b/src/communication/AsyncTCPClient.cpp index d68605d..f28fdf9 100644 --- a/src/communication/AsyncTCPClient.cpp +++ b/src/communication/AsyncTCPClient.cpp @@ -69,7 +69,7 @@ bool AsyncTCPClient::doDisconnect() if (ec != boost::system::errc::success) { ROS_ERROR("Error shutting socket down: %i", ec.value()); - status = false; + return false; /* early return, as if socket shutdown fails, close will fail too */ } else {