diff --git a/config/sick_safety_scanner.yaml b/config/sick_safety_scanner.yaml new file mode 100644 index 0000000..1c290a4 --- /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 diff --git a/include/sick_safetyscanners/SickSafetyscanners.h b/include/sick_safetyscanners/SickSafetyscanners.h index e059249..9aca50e 100644 --- a/include/sick_safetyscanners/SickSafetyscanners.h +++ b/include/sick_safetyscanners/SickSafetyscanners.h @@ -114,22 +114,25 @@ 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); /*! @@ -137,34 +140,32 @@ class SickSafetyscanners * \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& 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,7 +183,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. @@ -190,7 +191,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); @@ -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& 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& 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 80734c5..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,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 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); 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/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 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - 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/SickSafetyscanners.cpp b/src/SickSafetyscanners.cpp index 534b443..6d85d81 100644 --- a/src/SickSafetyscanners.cpp +++ b/src/SickSafetyscanners.cpp @@ -101,143 +101,213 @@ 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(); + if (!startTCPConnection(settings)) + return false; + + if (!changeCommSettingsInColaSession(settings)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::FindSensor(const datastructure::CommSettings& settings, +bool SickSafetyscanners::FindSensor(const datastructure::CommSettings& settings, uint16_t blink_time) { - startTCPConnection(settings); - FindSensorInColaSession(blink_time); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!FindSensorInColaSession(blink_time)) + return false; + + return stopTCPConnection(); } -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(); + if (!startTCPConnection(settings)) + return false; + + if (!requestTypeCodeInColaSession(type_code)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestApplicationName( +bool SickSafetyscanners::requestApplicationName( const datastructure::CommSettings& settings, sick::datastructure::ApplicationName& application_name) { - startTCPConnection(settings); - requestApplicationNameInColaSession(application_name); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestApplicationNameInColaSession(application_name)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestFieldData(const datastructure::CommSettings& settings, + +bool SickSafetyscanners::requestFieldData(const datastructure::CommSettings& settings, std::vector& field_data) { - startTCPConnection(settings); - requestFieldDataInColaSession(field_data); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestFieldDataInColaSession(field_data)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestMonitoringCases( +bool SickSafetyscanners::requestMonitoringCases( const datastructure::CommSettings& settings, std::vector& monitoring_cases) { - startTCPConnection(settings); - requestMonitoringCaseDataInColaSession(monitoring_cases); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestMonitoringCaseDataInColaSession(monitoring_cases)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestDeviceName(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestDeviceName(const datastructure::CommSettings& settings, datastructure::DeviceName& device_name) { - startTCPConnection(settings); - requestDeviceNameInColaSession(device_name); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestDeviceNameInColaSession(device_name)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestSerialNumber(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestSerialNumber(const datastructure::CommSettings& settings, datastructure::SerialNumber& serial_number) { - startTCPConnection(settings); - requestSerialNumberInColaSession(serial_number); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestSerialNumberInColaSession(serial_number)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestOrderNumber(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestOrderNumber(const datastructure::CommSettings& settings, datastructure::OrderNumber& order_number) { - startTCPConnection(settings); - requestOrderNumberInColaSession(order_number); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestOrderNumberInColaSession(order_number)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestProjectName(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestProjectName(const datastructure::CommSettings& settings, datastructure::ProjectName& project_name) { - startTCPConnection(settings); - requestProjectNameInColaSession(project_name); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestProjectNameInColaSession(project_name)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestUserName(const datastructure::CommSettings& settings, +bool SickSafetyscanners::requestUserName(const datastructure::CommSettings& settings, datastructure::UserName& user_name) { - startTCPConnection(settings); - requestUserNameInColaSession(user_name); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestUserNameInColaSession(user_name)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestFirmwareVersion(const datastructure::CommSettings& settings, + +bool SickSafetyscanners::requestFirmwareVersion(const datastructure::CommSettings& settings, datastructure::FirmwareVersion& firmware_version) { - startTCPConnection(settings); - requestFirmwareVersionInColaSession(firmware_version); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestFirmwareVersionInColaSession(firmware_version)) + return false; + + return stopTCPConnection(); } -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(); + if (!startTCPConnection(settings)) + return false; + + if (!requestPersistentConfigInColaSession(config_data)) + return false; + + return stopTCPConnection(); } -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(); + if (!startTCPConnection(settings)) + return false; + + if (!requestConfigMetadataInColaSession(config_metadata)) + return false; + + return stopTCPConnection(); } -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(); + if (!startTCPConnection(settings)) + return false; + + if (!requestStatusOverviewInColaSession(status_overview)) + return false; + + return stopTCPConnection(); } -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(); + if (!startTCPConnection(settings)) + return false; + + if (!requestDeviceStatusInColaSession(device_status)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::requestRequiredUserAction( +bool SickSafetyscanners::requestRequiredUserAction( const datastructure::CommSettings& settings, sick::datastructure::RequiredUserAction& required_user_action) { - startTCPConnection(settings); - requestRequiredUserActionInColaSession(required_user_action); - stopTCPConnection(); + if (!startTCPConnection(settings)) + return false; + + if (!requestRequiredUserActionInColaSession(required_user_action)) + return false; + + return stopTCPConnection(); } -void SickSafetyscanners::startTCPConnection(const sick::datastructure::CommSettings& settings) +bool SickSafetyscanners::startTCPConnection(const sick::datastructure::CommSettings& settings) { std::shared_ptr async_tcp_client = std::make_shared( @@ -245,23 +315,26 @@ void SickSafetyscanners::startTCPConnection(const sick::datastructure::CommSetti boost::ref(*m_io_service_ptr), settings.getSensorIp(), settings.getSensorTcpPort()); - async_tcp_client->doConnect(); - - m_session_ptr.reset(); - m_session_ptr = std::make_shared(async_tcp_client); + + if(!async_tcp_client->doConnect()) + { + return false; + } - m_session_ptr->open(); + m_session_ptr.reset(); + m_session_ptr = std::make_shared(async_tcp_client); + return m_session_ptr->open(); } -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) { sick::cola2::Cola2Session::CommandPtr command_ptr; @@ -272,13 +345,19 @@ void SickSafetyscanners::requestFieldDataInColaSession( command_ptr = std::make_shared( boost::ref(*m_session_ptr), field_data, i); - 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); - 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); } @@ -287,9 +366,11 @@ void SickSafetyscanners::requestFieldDataInColaSession( break; // skip other requests after first invalid } } + + return true; } -void SickSafetyscanners::requestMonitoringCaseDataInColaSession( +bool SickSafetyscanners::requestMonitoringCaseDataInColaSession( std::vector& monitoring_cases) { sick::cola2::Cola2Session::CommandPtr command_ptr; @@ -299,7 +380,11 @@ void SickSafetyscanners::requestMonitoringCaseDataInColaSession( command_ptr = std::make_shared( boost::ref(*m_session_ptr), monitoring_case_data, i); - 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); @@ -309,140 +394,171 @@ void SickSafetyscanners::requestMonitoringCaseDataInColaSession( break; // skip other requests after first invalid } } + + return true; } -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) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), device_name); - m_session_ptr->executeCommand(command_ptr); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("Device name: %s", device_name.getDeviceName().c_str()); + return true; } -void SickSafetyscanners::requestApplicationNameInColaSession( +bool SickSafetyscanners::requestApplicationNameInColaSession( datastructure::ApplicationName& application_name) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), application_name); - m_session_ptr->executeCommand(command_ptr); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("Application name: %s", application_name.getApplicationName().c_str()); + return true; } -void SickSafetyscanners::requestSerialNumberInColaSession( +bool SickSafetyscanners::requestSerialNumberInColaSession( datastructure::SerialNumber& serial_number) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), serial_number); - 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 true; } -void SickSafetyscanners::requestFirmwareVersionInColaSession( +bool SickSafetyscanners::requestFirmwareVersionInColaSession( datastructure::FirmwareVersion& firmware_version) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), firmware_version); - m_session_ptr->executeCommand(command_ptr); + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("Firmware Version: %s", firmware_version.getFirmwareVersion().c_str()); + return true; } -void SickSafetyscanners::requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code) +bool SickSafetyscanners::requestTypeCodeInColaSession(sick::datastructure::TypeCode& type_code) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), type_code); - m_session_ptr->executeCommand(command_ptr); + + if (!m_session_ptr->executeCommand(command_ptr)) + return false; + ROS_INFO("Type Code: %s", type_code.getTypeCode().c_str()); + return true; } -void SickSafetyscanners::requestOrderNumberInColaSession( +bool SickSafetyscanners::requestOrderNumberInColaSession( sick::datastructure::OrderNumber& order_number) { + sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), order_number); - 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 true; } -void SickSafetyscanners::requestProjectNameInColaSession( +bool SickSafetyscanners::requestProjectNameInColaSession( sick::datastructure::ProjectName& project_name) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), project_name); - 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 true; } -void SickSafetyscanners::requestUserNameInColaSession(sick::datastructure::UserName& user_name) +bool SickSafetyscanners::requestUserNameInColaSession(sick::datastructure::UserName& user_name) { sick::cola2::Cola2Session::CommandPtr command_ptr = std::make_shared(boost::ref(*m_session_ptr), user_name); - 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 true; } -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() +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 5eb2ec8..4dc5ebb 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -34,7 +34,8 @@ #include "sick_safetyscanners/SickSafetyscannersRos.h" - +#include +#include namespace sick { @@ -46,6 +47,7 @@ SickSafetyscannersRos::SickSafetyscannersRos() , m_range_max(0.0) , m_angle_offset(-90.0) , m_use_pers_conf(false) + , m_connected(false) { dynamic_reconfigure::Server< sick_safetyscanners::SickSafetyscannersConfigurationConfig>::CallbackType reconf_callback = @@ -64,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); @@ -88,38 +93,54 @@ SickSafetyscannersRos::SickSafetyscannersRos() &m_communication_settings, m_interface_ip); m_device->run(); - readTypeCodeSettings(); - - if (m_use_pers_conf) + + if(!setCommunicationSettingScanner()) { - readPersistentConfig(); + ROS_WARN("Couldn't establish TCP connection"); } - m_device->changeSensorSettings(m_communication_settings); - m_device->requestConfigMetadata(m_communication_settings, config_meta_data); - m_device->requestFirmwareVersion(m_communication_settings, firmware_version); + + 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."); } -void SickSafetyscannersRos::readTypeCodeSettings() +bool SickSafetyscannersRos::readTypeCodeSettings() { ROS_INFO("Reading Type code settings"); sick::datastructure::TypeCode type_code; - m_device->requestTypeCode(m_communication_settings, type_code); + + 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 true; +} -void SickSafetyscannersRos::readPersistentConfig() +bool SickSafetyscannersRos::readPersistentConfig() { ROS_INFO("Reading Persistent Configuration"); sick::datastructure::ConfigData config_data; - m_device->requestPersistentConfig(m_communication_settings, config_data); + + 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( @@ -254,6 +275,31 @@ 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 timeout : %d ms", udp_connection_timeout); + } + + m_connection_monitor_watchdog_timeout_ms = udp_connection_timeout; + } m_private_nh.getParam("frame_id", m_frame_id); @@ -288,6 +334,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) @@ -310,12 +357,12 @@ void SickSafetyscannersRos::sensorDiagnostics( diagnostic_status.addf("Version major version", "%u", header.version_major_version); diagnostic_status.addf("Version minor version", "%u", header.version_minor_version); diagnostic_status.addf("Version release", "%u", header.version_release); - diagnostic_status.addf("Firmware version", "%s", firmware_version.getFirmwareVersion().c_str()); + diagnostic_status.addf("Firmware version", "%s", m_firmware_version.getFirmwareVersion().c_str()); diagnostic_status.addf("Serial number of device", "%u", header.serial_number_of_device); diagnostic_status.addf( "Serial number of channel plug", "%u", header.serial_number_of_channel_plug); - diagnostic_status.addf("App checksum", "%X", config_meta_data.getAppChecksum()); - diagnostic_status.addf("Overall checksum", "%X", config_meta_data.getOverallChecksum()); + diagnostic_status.addf("App checksum", "%X", m_config_meta_data.getAppChecksum()); + diagnostic_status.addf("Overall checksum", "%X", m_config_meta_data.getOverallChecksum()); diagnostic_status.addf("Channel number", "%u", header.channel_number); diagnostic_status.addf("Sequence number", "%u", header.sequence_number); @@ -338,6 +385,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)); @@ -613,6 +661,7 @@ SickSafetyscannersRos::createGeneralSystemStateMessage(const sick::datastructure msg.current_monitoring_case_no_table_4 = general_system_state->getCurrentMonitoringCaseNoTable4(); + msg.connection_status = m_connected; msg.application_error = general_system_state->getApplicationError(); msg.device_error = general_system_state->getDeviceError(); } @@ -853,6 +902,53 @@ bool SickSafetyscannersRos::getFieldData(sick_safetyscanners::FieldData::Request return true; } +void SickSafetyscannersRos::udpConnectionMonitorHandler() +{ + double time_now = ros::Time::now().toSec(); + /* 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_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(); + } +} + +bool SickSafetyscannersRos::setCommunicationSettingScanner() +{ + /* If tcp request fails return early */ + if (!readTypeCodeSettings()) + return false; + + if (m_use_pers_conf) + { + readPersistentConfig(); + } + + 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); + + m_connected = true; + std_msgs::Bool connection_status; + connection_status.data = m_connected; + m_connection_status_publisher.publish(connection_status); + + return true; +} + bool SickSafetyscannersRos::getConfigMetadata(sick_safetyscanners::ConfigMetadata::Request& req, sick_safetyscanners::ConfigMetadata::Response& res) { diff --git a/src/cola2/Cola2Session.cpp b/src/cola2/Cola2Session.cpp index 6fcc056..84335bc 100644 --- a/src/cola2/Cola2Session.cpp +++ b/src/cola2/Cola2Session.cpp @@ -59,16 +59,17 @@ 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; + if (!addCommand(command->getRequestID(), command)) + return false; + + return sendTelegramAndListenForAnswer(command); } bool Cola2Session::sendTelegramAndListenForAnswer(const CommandPtr& command) diff --git a/src/communication/AsyncTCPClient.cpp b/src/communication/AsyncTCPClient.cpp index d37c6fb..f28fdf9 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()); + return false; /* early return, as if socket shutdown fails, close will fail too */ } 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; }