From d5b432eee11bc644e72f0e06a6c39d235dc3e5d0 Mon Sep 17 00:00:00 2001 From: rostest Date: Wed, 23 Sep 2020 19:13:33 +0200 Subject: [PATCH] Configuration Cola-Timeouts, README.md update --- README.md | 116 +++++++++++----------- launch/sim_loc_driver.launch.py | 4 +- src/cola_services.cpp | 10 +- src/driver_monitor.cpp | 10 +- src/time_sync_service.cpp | 20 ++-- test/ros2_scripts/send_cola_advanced.bash | 12 +-- yaml/sim_loc_driver.yaml | 2 +- 7 files changed, 87 insertions(+), 87 deletions(-) diff --git a/README.md b/README.md index 445391c..19fc432 100644 --- a/README.md +++ b/README.md @@ -111,64 +111,66 @@ https://supportportal.sick.com/Product_notes/lls-telegram-listing/. ### LiDAR-LOC specialized ROS services The operations below are implemented by specialized ros services defined in folder srv. These services create and -transmit the corresponding cola telegram to the controller and converts the parameter. +transmit the corresponding cola telegram to the controller and converts the parameter. Naming convention: +A service named is defined in file srv/Srv.srv, f.e. command "LocStartLocalizing" is implemented +by ros service "SickLocStartLocalizing" defined in file "srv/SickLocStartLocalizingSrv.srv". -Request | ros service | Interface definition | Description ---- | --- | --- | --- -**States Telegrams**||| -IsSystemReady | SickLocIsSystemReady | [srv/SickLocIsSystemReadySrv.srv](srv/SickLocIsSystemReadySrv.srv) | Check if the system is ready -LocState | SickLocState | [srv/SickLocStateSrv.srv](srv/SickLocStateSrv.srv) | Read localization state -LocStartLocalizing | SickLocStartLocalizing | [srv/SickLocStartLocalizingSrv.srv](srv/SickLocStartLocalizingSrv.srv) | Start localization -LocStop | SickLocStop | [srv/SickLocStopSrv.srv](srv/SickLocStopSrv.srv) | Stop localization or demo mapping -**Result Output Configuration Telegrams**||| -LocSetResultPort | SickLocSetResultPort | [srv/SickLocSetResultPortSrv.srv](srv/SickLocSetResultPortSrv.srv) | Set TCP-port for result output (default: 2201) -LocSetResultMode | SickLocSetResultMode | [srv/SickLocSetResultModeSrv.srv](srv/SickLocSetResultModeSrv.srv) | Set mode of result output (default: stream) -LocSetResultPoseEnabled | SickLocSetResultPoseEnabled | [srv/SickLocSetResultPoseEnabledSrv.srv](srv/SickLocSetResultPoseEnabledSrv.srv) | Disable/enable result output -LocSetResultEndianness | SickLocSetResultEndianness | [srv/SickLocSetResultEndiannessSrv.srv](srv/SickLocSetResultEndiannessSrv.srv) | Set endianness of result output -LocSetResultPoseInterval | SickLocSetResultPoseInterval | [srv/SickLocSetResultPoseIntervalSrv.srv](srv/SickLocSetResultPoseIntervalSrv.srv) | Set interval of result output -LocRequestResultData | SickLocRequestResultData | [srv/SickLocRequestResultDataSrv.srv](srv/SickLocRequestResultDataSrv.srv) | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. -**SetPose Telegrams**||| -LocSetPose | SickLocSetPose | [srv/SickLocSetPoseSrv.srv](srv/SickLocSetPoseSrv.srv) | Initialize vehicle pose -**Timestamp Telegrams**||| -LocRequestTimestamp | SickLocRequestTimestamp | [srv/SickLocRequestTimestampSrv.srv](srv/SickLocRequestTimestampSrv.srv) | Query timestamp, see "Time synchronization" -**Advanced services in release 4 and later**||| -DevGetLidarConfig | SickDevGetLidarConfig | [srv/SickDevGetLidarConfigSrv.srv](srv/SickDevGetLidarConfigSrv.srv) | Reads the configuration for a lidar -DevGetLidarIdent | SickDevGetLidarIdent | [srv/SickDevGetLidarIdentSrv.srv](srv/SickDevGetLidarIdentSrv.srv) | Returns the scanner ident of the specified lidar. -DevGetLidarState | SickDevGetLidarState | [srv/SickDevGetLidarStateSrv.srv](srv/SickDevGetLidarStateSrv.srv) | Returns the lidar state of the given lidar. -DevIMUActive | SickDevIMUActive | [srv/SickDevIMUActiveSrv.srv](srv/SickDevIMUActiveSrv.srv) | Read IMU Active status -DevSetIMUActive | SickDevSetIMUActive | [srv/SickDevSetIMUActiveSrv.srv](srv/SickDevSetIMUActiveSrv.srv) | Set IMU Active -DevSetLidarConfig | SickDevSetLidarConfig | [srv/SickDevSetLidarConfigSrv.srv](srv/SickDevSetLidarConfigSrv.srv) | Sets the configuration for a lidar -GetSoftwareVersion | SickGetSoftwareVersion | [srv/SickGetSoftwareVersionSrv.srv](srv/SickGetSoftwareVersionSrv.srv) | Returns the version string of the localization system. -LocAutoStartActive | SickLocAutoStartActive | [srv/SickLocAutoStartActiveSrv.srv](srv/SickLocAutoStartActiveSrv.srv) | Returns whether autostart is used or not. -LocAutoStartSavePose | SickLocAutoStartSavePose | [srv/SickLocAutoStartSavePoseSrv.srv](srv/SickLocAutoStartSavePoseSrv.srv) | Saves the current pose. If no further pose writing occurs the system will reinitialize itself at this position on restart if LocAutoStart is enabled. -LocAutoStartSavePoseInterval | SickLocAutoStartSavePoseInterval | [srv/SickLocAutoStartSavePoseIntervalSrv.srv](srv/SickLocAutoStartSavePoseIntervalSrv.srv) | Returns the interval in seconds for saving the pose automatically for auto start while localizing -LocForceUpdate | SickLocForceUpdate | [srv/SickLocForceUpdateSrv.srv](srv/SickLocForceUpdateSrv.srv) | Forces an update of the map localization with the next scan. This should be used with care because it is not garanteed that this converges to the correct pose. Moving the LiDAR instead should be preferred because it produces more robust updates. -LocInitializePose | SickLocInitializePose | [srv/SickLocInitializePoseSrv.srv](srv/SickLocInitializePoseSrv.srv) | Automatically adjusts the given input pose according to the map of the environment and the current LiDAR measurements. -LocInitialPose | SickLocInitialPose | [srv/SickLocInitialPoseSrv.srv](srv/SickLocInitialPoseSrv.srv) | Reads the initial pose -LocMap | SickLocMap | [srv/SickLocMapSrv.srv](srv/SickLocMapSrv.srv) | Returns the currently loaded map -LocMapState | SickLocMapState | [srv/SickLocMapStateSrv.srv](srv/SickLocMapStateSrv.srv) | Reads the current state of the map. 0: not active, 1: active. -LocOdometryActive | SickLocOdometryActive | [srv/SickLocOdometryActiveSrv.srv](srv/SickLocOdometryActiveSrv.srv) | Returns the usage of odometry data in Scan Matching -LocOdometryPort | SickLocOdometryPort | [srv/SickLocOdometryPortSrv.srv](srv/SickLocOdometryPortSrv.srv) | Returns the UDP port of the UDP socket for odometry measurement input. -LocOdometryRestrictYMotion | SickLocOdometryRestrictYMotion | [srv/SickLocOdometryRestrictYMotionSrv.srv](srv/SickLocOdometryRestrictYMotionSrv.srv) | Returns the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. -LocReflectorsForSupportActive | SickLocReflectorsForSupportActive | [srv/SickLocReflectorsForSupportActiveSrv.srv](srv/SickLocReflectorsForSupportActiveSrv.srv) | Returns usage of mapped reflectors for map based localization robustification -LocResultEndianness | SickLocResultEndianness | [srv/SickLocResultEndiannessSrv.srv](srv/SickLocResultEndiannessSrv.srv) | Returns the endianness of the result port: 0 BIG_ENDIAN (default), 1 LITTLE_ENDIAN -LocResultMode | SickLocResultMode | [srv/SickLocResultModeSrv.srv](srv/SickLocResultModeSrv.srv) | Returns the current result mode: 0 STREAM (default), 1 POLL -LocResultPort | SickLocResultPort | [srv/SickLocResultPortSrv.srv](srv/SickLocResultPortSrv.srv) | Read the TCP port of the localization controller for result output. -LocResultPoseInterval | SickLocResultPoseInterval | [srv/SickLocResultPoseIntervalSrv.srv](srv/SickLocResultPoseIntervalSrv.srv) | Reads the interval of the pose result output. -LocResultState | SickLocResultState | [srv/SickLocResultStateSrv.srv](srv/SickLocResultStateSrv.srv) | Returns the output state of the result interface -LocRingBufferRecordingActive | SickLocRingBufferRecordingActive | [srv/SickLocRingBufferRecordingActiveSrv.srv](srv/SickLocRingBufferRecordingActiveSrv.srv) | Returns whether rolling buffer log file recording is activated or deactivated -LocSaveRingBufferRecording | SickLocSaveRingBufferRecording | [srv/SickLocSaveRingBufferRecordingSrv.srv](srv/SickLocSaveRingBufferRecordingSrv.srv) | Saves the current sensor data ring buffer as log file. The log file can be downloaded via FTP. -LocSetAutoStartActive | SickLocSetAutoStartActive | [srv/SickLocSetAutoStartActiveSrv.srv](srv/SickLocSetAutoStartActiveSrv.srv) | Sets whether autostart should be used or not. -LocSetAutoStartSavePoseInterval | SickLocSetAutoStartSavePoseInterval | [srv/SickLocSetAutoStartSavePoseIntervalSrv.srv](srv/SickLocSetAutoStartSavePoseIntervalSrv.srv) | Set the interval in seconds for saving the pose automatically for auto start while localizing -LocSetMap | SickLocSetMap | [srv/SickLocSetMapSrv.srv](srv/SickLocSetMapSrv.srv) | Loads a given map -LocSetOdometryActive | SickLocSetOdometryActive | [srv/SickLocSetOdometryActiveSrv.srv](srv/SickLocSetOdometryActiveSrv.srv) | Enables or disables the usage of odometry data in Scan Matching -LocSetOdometryPort | SickLocSetOdometryPort | [srv/SickLocSetOdometryPortSrv.srv](srv/SickLocSetOdometryPortSrv.srv) | Sets the UDP port of the UDP socket for odometry measurement input. -LocSetOdometryRestrictYMotion | SickLocSetOdometryRestrictYMotion | [srv/SickLocSetOdometryRestrictYMotionSrv.srv](srv/SickLocSetOdometryRestrictYMotionSrv.srv) | Method to set the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. -LocSetReflectorsForSupportActive | SickLocSetReflectorsForSupportActive | [srv/SickLocSetReflectorsForSupportActiveSrv.srv](srv/SickLocSetReflectorsForSupportActiveSrv.srv) | Enables or disables usage of mapped reflectors for map based localization robustification -LocSetRingBufferRecordingActive | SickLocSetRingBufferRecordingActive | [srv/SickLocSetRingBufferRecordingActiveSrv.srv](srv/SickLocSetRingBufferRecordingActiveSrv.srv) | Activates or deactivates rolling buffer log file recording on error report -LocStartDemoMapping | SickLocStartDemoMapping | [srv/SickLocStartDemoMappingSrv.srv](srv/SickLocStartDemoMappingSrv.srv) | If all conditions are met, starts the demo mapping system. -ReportUserMessage | SickReportUserMessage | [srv/SickReportUserMessageSrv.srv](srv/SickReportUserMessageSrv.srv) | Method to report messages to the localization system. -SavePermanent | SickSavePermanent | [srv/SickSavePermanentSrv.srv](srv/SickSavePermanentSrv.srv) | Saves the parameters permanently on the device. They will be reloaded on reboot. +Request : ros service | Description +--- | --- +**States Telegrams**| +IsSystemReady : [SickLocIsSystemReady](srv/SickLocIsSystemReadySrv.srv) | Check if the system is ready +LocState : [SickLocState](srv/SickLocStateSrv.srv) | Read localization state +LocStartLocalizing : [SickLocStartLocalizing](srv/SickLocStartLocalizingSrv.srv) | Start localization +LocStop : [SickLocStop](srv/SickLocStopSrv.srv) | Stop localization or demo mapping +**Result Output Configuration Telegrams**|| +LocSetResultPort : [SickLocSetResultPort](srv/SickLocSetResultPortSrv.srv) | Set TCP-port for result output (default: 2201) +LocSetResultMode : [SickLocSetResultMode](srv/SickLocSetResultModeSrv.srv) | Set mode of result output (default: stream) +LocSetResultPoseEnabled : [SickLocSetResultPoseEnabled](srv/SickLocSetResultPoseEnabledSrv.srv) | Disable/enable result output +LocSetResultEndianness : [SickLocSetResultEndianness](srv/SickLocSetResultEndiannessSrv.srv) | Set endianness of result output +LocSetResultPoseInterval : [SickLocSetResultPoseInterval](srv/SickLocSetResultPoseIntervalSrv.srv) | Set interval of result output +LocRequestResultData : [SickLocRequestResultData](srv/SickLocRequestResultDataSrv.srv) | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. +**SetPose Telegrams**| +LocSetPose : [SickLocSetPose](srv/SickLocSetPoseSrv.srv) | Initialize vehicle pose +**Timestamp Telegrams**| +LocRequestTimestamp : [SickLocRequestTimestamp](srv/SickLocRequestTimestampSrv.srv) | Query timestamp, see "Time synchronization" +**Advanced services in release 4 and later**| +DevGetLidarConfig : [SickDevGetLidarConfig](srv/SickDevGetLidarConfigSrv.srv) | Reads the configuration for a lidar +DevGetLidarIdent : [SickDevGetLidarIdent](srv/SickDevGetLidarIdentSrv.srv) | Returns the scanner ident of the specified lidar. +DevGetLidarState : [SickDevGetLidarState](srv/SickDevGetLidarStateSrv.srv) | Returns the lidar state of the given lidar. +DevIMUActive : [SickDevIMUActive](srv/SickDevIMUActiveSrv.srv) | Read IMU Active status +DevSetIMUActive : [SickDevSetIMUActive](srv/SickDevSetIMUActiveSrv.srv) | Set IMU Active +DevSetLidarConfig : [SickDevSetLidarConfig](srv/SickDevSetLidarConfigSrv.srv) | Sets the configuration for a lidar +GetSoftwareVersion : [SickGetSoftwareVersion](srv/SickGetSoftwareVersionSrv.srv) | Returns the version string of the localization system. +LocAutoStartActive : [SickLocAutoStartActive](srv/SickLocAutoStartActiveSrv.srv) | Returns whether autostart is used or not. +LocAutoStartSavePose : [SickLocAutoStartSavePose](srv/SickLocAutoStartSavePoseSrv.srv) | Saves the current pose. If no further pose writing occurs the system will reinitialize itself at this position on restart if LocAutoStart is enabled. +LocAutoStartSavePoseInterval : [SickLocAutoStartSavePoseInterval](srv/SickLocAutoStartSavePoseIntervalSrv.srv) | Returns the interval in seconds for saving the pose automatically for auto start while localizing +LocForceUpdate : [SickLocForceUpdate](srv/SickLocForceUpdateSrv.srv) | Forces an update of the map localization with the next scan. This should be used with care because it is not garanteed that this converges to the correct pose. Moving the LiDAR instead should be preferred because it produces more robust updates. +LocInitializePose : [SickLocInitializePose](srv/SickLocInitializePoseSrv.srv) | Automatically adjusts the given input pose according to the map of the environment and the current LiDAR measurements. +LocInitialPose : [SickLocInitialPose](srv/SickLocInitialPoseSrv.srv) | Reads the initial pose +LocMap : [SickLocMap](srv/SickLocMapSrv.srv) | Returns the currently loaded map +LocMapState : [SickLocMapState](srv/SickLocMapStateSrv.srv) | Reads the current state of the map. 0: not active, 1: active. +LocOdometryActive : [SickLocOdometryActive](srv/SickLocOdometryActiveSrv.srv) | Returns the usage of odometry data in Scan Matching +LocOdometryPort : [SickLocOdometryPort](srv/SickLocOdometryPortSrv.srv) | Returns the UDP port of the UDP socket for odometry measurement input. +LocOdometryRestrictYMotion : [SickLocOdometryRestrictYMotion](srv/SickLocOdometryRestrictYMotionSrv.srv) | Returns the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. +LocReflectorsForSupportActive : [SickLocReflectorsForSupportActive](srv/SickLocReflectorsForSupportActiveSrv.srv) | Returns usage of mapped reflectors for map based localization robustification +LocResultEndianness : [SickLocResultEndianness](srv/SickLocResultEndiannessSrv.srv) | Returns the endianness of the result port: 0 BIG_ENDIAN (default), 1 LITTLE_ENDIAN +LocResultMode : [SickLocResultMode](srv/SickLocResultModeSrv.srv) | Returns the current result mode: 0 STREAM (default), 1 POLL +LocResultPort : [SickLocResultPort](srv/SickLocResultPortSrv.srv) | Read the TCP port of the localization controller for result output. +LocResultPoseInterval : [SickLocResultPoseInterval](srv/SickLocResultPoseIntervalSrv.srv) | Reads the interval of the pose result output. +LocResultState : [SickLocResultState](srv/SickLocResultStateSrv.srv) | Returns the output state of the result interface +LocRingBufferRecordingActive : [SickLocRingBufferRecordingActive](srv/SickLocRingBufferRecordingActiveSrv.srv) | Returns whether rolling buffer log file recording is activated or deactivated +LocSaveRingBufferRecording : [SickLocSaveRingBufferRecording](srv/SickLocSaveRingBufferRecordingSrv.srv) | Saves the current sensor data ring buffer as log file. The log file can be downloaded via FTP. +LocSetAutoStartActive : [SickLocSetAutoStartActive](srv/SickLocSetAutoStartActiveSrv.srv) | Sets whether autostart should be used or not. +LocSetAutoStartSavePoseInterval : [SickLocSetAutoStartSavePoseInterval](srv/SickLocSetAutoStartSavePoseIntervalSrv.srv) | Set the interval in seconds for saving the pose automatically for auto start while localizing +LocSetMap : [SickLocSetMap](srv/SickLocSetMapSrv.srv) | Loads a given map +LocSetOdometryActive : [SickLocSetOdometryActive](srv/SickLocSetOdometryActiveSrv.srv) | Enables or disables the usage of odometry data in Scan Matching +LocSetOdometryPort : [SickLocSetOdometryPort](srv/SickLocSetOdometryPortSrv.srv) | Sets the UDP port of the UDP socket for odometry measurement input. +LocSetOdometryRestrictYMotion : [SickLocSetOdometryRestrictYMotion](srv/SickLocSetOdometryRestrictYMotionSrv.srv) | Method to set the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. +LocSetReflectorsForSupportActive : [SickLocSetReflectorsForSupportActive](srv/SickLocSetReflectorsForSupportActiveSrv.srv) | Enables or disables usage of mapped reflectors for map based localization robustification +LocSetRingBufferRecordingActive : [SickLocSetRingBufferRecordingActive](srv/SickLocSetRingBufferRecordingActiveSrv.srv) | Activates or deactivates rolling buffer log file recording on error report +LocStartDemoMapping : [SickLocStartDemoMapping](srv/SickLocStartDemoMappingSrv.srv) | If all conditions are met, starts the demo mapping system. +ReportUserMessage : [SickReportUserMessage](srv/SickReportUserMessageSrv.srv) | Method to report messages to the localization system. +SavePermanent : [SickSavePermanent](srv/SickSavePermanentSrv.srv) | Saves the parameters permanently on the device. They will be reloaded on reboot. The following examples show how to call these services under ROS1: diff --git a/launch/sim_loc_driver.launch.py b/launch/sim_loc_driver.launch.py index ab664c8..21918d7 100755 --- a/launch/sim_loc_driver.launch.py +++ b/launch/sim_loc_driver.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): {'/sick_lidar_localization/driver/monitoring_rate': 1.0}, # frequency to monitor driver messages, once per second by default {'/sick_lidar_localization/driver/monitoring_message_timeout': 1.0}, # timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, 1 second by default # Configuration for time sync service - {'/sick_lidar_localization/time_sync/cola_response_timeout': 1.0}, # Timeout in seconds for cola responses from localization controller + {'/sick_lidar_localization/time_sync/cola_response_timeout': 10.0}, # Timeout in seconds for cola responses from localization controller {'/sick_lidar_localization/time_sync/software_pll_fifo_length': 7}, # Length of software pll fifo, default: 7 {'/sick_lidar_localization/time_sync/time_sync_rate': 0.1}, # Frequency to request timestamps from localization controller using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 {'/sick_lidar_localization/time_sync/time_sync_initial_rate': 1.0}, # Frequency to request timestamps and to update software pll during initialization phase, default: 1.0 (LocRequestTimestamp queries every second) @@ -34,7 +34,7 @@ def generate_launch_description(): {'/sick_lidar_localization/driver/tf_parent_frame_id': "tf_demo_map"}, # parent frame of tf messages of of vehicles pose (typically frame of the loaded map) {'/sick_lidar_localization/driver/tf_child_frame_id': "tf_sick_lidar_localization"}, # child frame of tf messages of of vehicles pose # Configuration for cola service - {'/sick_lidar_localization/time_sync/cola_response_timeout': 1.0}, # Timeout in seconds for cola responses from localization controller + {'/sick_lidar_localization/time_sync/cola_response_timeout': 10.0}, # Timeout in seconds for cola responses from localization controller {'/sick_lidar_localization/driver/tcp_connection_retry_delay': 1.0}, # Delay in seconds to retry to connect to the localization controller, default 1 second # Odometry configuration {'/sick_lidar_localization/driver/odom_telegrams_udp_port': 3000}, # Udp port to send odom packages to the localization controller diff --git a/src/cola_services.cpp b/src/cola_services.cpp index 7ab9de2..9674894 100644 --- a/src/cola_services.cpp +++ b/src/cola_services.cpp @@ -74,12 +74,12 @@ /*! * Constructor */ -sick_lidar_localization::ColaServices::ColaServices(ROS::NodePtr nh, sick_lidar_localization::DriverMonitor* driver_monitor) : m_nh(nh), m_driver_monitor(driver_monitor), m_cola_response_timeout(1.0) +sick_lidar_localization::ColaServices::ColaServices(ROS::NodePtr nh, sick_lidar_localization::DriverMonitor* driver_monitor) : m_nh(nh), m_driver_monitor(driver_monitor), m_cola_response_timeout(10.0) { if(nh) { ROS::param(nh, "/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); - + // Advertise ros services supported by release 3 and later #if defined __ROS_VERSION && __ROS_VERSION == 1 m_srv_server_01 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocIsSystemReadySrv, "SickLocIsSystemReady",&sick_lidar_localization::ColaServices::serviceCbIsSystemReady, this); @@ -105,7 +105,7 @@ sick_lidar_localization::ColaServices::ColaServices(ROS::NodePtr nh, sick_lidar_ m_srv_server_10 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultPoseIntervalSrv, "SickLocSetResultPoseInterval",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseIntervalROS2, this); m_srv_server_11 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocRequestResultDataSrv, "SickLocRequestResultData",&sick_lidar_localization::ColaServices::serviceCbLocRequestResultDataROS2, this); m_srv_server_12 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetPoseSrv, "SickLocSetPose",&sick_lidar_localization::ColaServices::serviceCbLocSetPoseROS2, this); -#endif +#endif // Advertise ros services supported by release 4 and later #if defined __ROS_VERSION && __ROS_VERSION == 1 m_SickDevSetLidarConfigSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevSetLidarConfigSrv, "SickDevSetLidarConfig", &sick_lidar_localization::ColaServices::serviceCbDevSetLidarConfig, this); @@ -212,7 +212,7 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaSer #elif defined __ROS_VERSION && __ROS_VERSION == 2 std::shared_ptr cola_telegram_request = std::make_shared(); std::shared_ptr cola_telegram_response = std::make_shared(); -#endif +#endif cola_telegram_request->cola_ascii_request = cola_ascii_request; cola_telegram_request->wait_response_timeout = m_cola_response_timeout; try @@ -226,7 +226,7 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaSer bool service_call_ok = m_driver_monitor->serviceCbColaTelegram(*cola_telegram_request, *cola_telegram_response); if (!service_call_ok || cola_telegram_response->cola_ascii_response.empty()) { - ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(): calling ros service \"SickLocColaTelegram\" failed with request: \"" + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(): calling ros service \"SickLocColaTelegram\" failed with request: \"" << cola_telegram_request->cola_ascii_request << "\", response: \"" << cola_telegram_response->cola_ascii_response << "\"" << " after " << ROS::seconds(ROS::now() - start_request_timestamp) << " sec (timeout: " << m_cola_response_timeout << " sec, " << __FILE__ << ":" << __LINE__ << ")"); return sick_lidar_localization::SickLocColaTelegramMsg(); diff --git a/src/driver_monitor.cpp b/src/driver_monitor.cpp index 7153a3e..5c5ac3e 100644 --- a/src/driver_monitor.cpp +++ b/src/driver_monitor.cpp @@ -76,7 +76,7 @@ */ sick_lidar_localization::DriverMonitor::DriverMonitor(ROS::NodePtr nh, const std::string & server_adress, int ip_port_results, int ip_port_cola) : m_initialized(false), m_nh(nh), m_server_adress(server_adress), m_ip_port_results(ip_port_results), m_ip_port_cola(ip_port_cola), m_cola_binary(false), - m_monitoring_thread_running(false), m_monitoring_thread(0), m_monitoring_rate(1.0), m_receive_telegrams_timeout(1.0), m_cola_response_timeout(1.0), m_cola_transmitter(0) + m_monitoring_thread_running(false), m_monitoring_thread(0), m_monitoring_rate(1.0), m_receive_telegrams_timeout(1.0), m_cola_response_timeout(10.0), m_cola_transmitter(0) { if(m_nh) { @@ -219,7 +219,7 @@ bool sick_lidar_localization::DriverMonitor::serviceCbColaTelegram(sick_lidar_lo if(!m_cola_transmitter->waitPopResponse(binary_response, cola_request.wait_response_timeout, receive_timestamp) || binary_response.size() < 2) // at least 2 byte stx and etx { ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: receive() failed by localization server " << m_server_adress << ":" << m_ip_port_cola - << "(" << binary_response.size() << " bytes received, configured timeout: " << cola_request.wait_response_timeout << " sec, receive failed after " + << "(" << binary_response.size() << " bytes received, configured timeout: " << cola_request.wait_response_timeout << " sec, receive failed after " << ROS::seconds(ROS::now() - waitResponseStartTime) << " sec)"); stopColaTransmitter(); return false; @@ -302,15 +302,15 @@ bool sick_lidar_localization::DriverMonitor::resultTelegramsReceiveStatusIsOk(vo ROS_INFO_STREAM("DriverMonitor: result telegrams deactivated, no result telegrams received (cola response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ")"); return true; // OK: LocResultState == "0": Result telegrams deactivated } - + // Check timestamp of last result telegram ROS::Time driver_message_recv_timestamp = m_driver_message_recv_timestamp.get(); ROS::Time current_time = ROS::now(); if(ROS::seconds(current_time - driver_message_recv_timestamp) <= m_receive_telegrams_timeout) return true; // OK: result telegram received within timeout // Timeout error: Localization and result telegrams activated, but no result telegrams received - ROS_WARN_STREAM("## ERROR DriverMonitor: Localization and result telegrams activated, timeout while waiting for result telegrams, current time: " - << ROS::secondsSinceStart(current_time) << " last message received: " << ROS::secondsSinceStart(driver_message_recv_timestamp) << ", delta_time: " + ROS_WARN_STREAM("## ERROR DriverMonitor: Localization and result telegrams activated, timeout while waiting for result telegrams, current time: " + << ROS::secondsSinceStart(current_time) << " last message received: " << ROS::secondsSinceStart(driver_message_recv_timestamp) << ", delta_time: " << ROS::seconds(current_time - driver_message_recv_timestamp) << ", configured timeout: " << m_receive_telegrams_timeout << " sec"); return false; } diff --git a/src/time_sync_service.cpp b/src/time_sync_service.cpp index ca8f624..aad18af 100644 --- a/src/time_sync_service.cpp +++ b/src/time_sync_service.cpp @@ -93,7 +93,7 @@ */ sick_lidar_localization::TimeSyncService::TimeSyncService(ROS::NodePtr nh, sick_lidar_localization::DriverMonitor* driver_monitor) : m_nh(nh), m_driver_monitor(driver_monitor), m_time_sync_thread_running(false), m_time_sync_thread(0), m_cola_binary(false), m_cola_binary_mode(0), m_software_pll_fifo_length(7), - m_time_sync_rate(0.1), m_time_sync_initial_rate(1.0), m_time_sync_initial_length(10), m_cola_response_timeout(1.0) + m_time_sync_rate(0.1), m_time_sync_initial_rate(1.0), m_time_sync_initial_length(10), m_cola_response_timeout(10.0) { if(nh) { @@ -152,7 +152,7 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li #elif defined __ROS_VERSION && __ROS_VERSION == 2 std::shared_ptr cola_telegram_request = std::make_shared(); std::shared_ptr cola_telegram_response = std::make_shared(); -#endif +#endif cola_telegram_request->cola_ascii_request = "sMN LocRequestTimestamp"; cola_telegram_request->wait_response_timeout = m_cola_response_timeout; // cola_telegram_request.send_binary = m_cola_binary; @@ -169,7 +169,7 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li { ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): calling ros service \"SickLocColaTelegram\" failed with request: \"" << cola_telegram_request->cola_ascii_request << "\" response: \"" << cola_telegram_response->cola_ascii_response << "\" after " - << ROS::seconds(ROS::now() - start_request_timestamp) << " sec (status: " << service_call_ok << ", timeout: " << m_cola_response_timeout + << ROS::seconds(ROS::now() - start_request_timestamp) << " sec (status: " << service_call_ok << ", timeout: " << m_cola_response_timeout << " sec, " << __FILE__ << ":" << __LINE__ << ")"); return false; } @@ -180,7 +180,7 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): request \"" << cola_telegram_request->cola_ascii_request << "\" failed, exception " << exc.what()); return false; } - + // Decode response, get timestamp_lidar_ms from parameter sick_lidar_localization::SickLocColaTelegramMsg cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram_response->cola_ascii_response); if(cola_response.command_name != "LocRequestTimestamp" || cola_response.parameter.size() != 1) @@ -204,23 +204,23 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): invalid timestamps in cola response " << sick_lidar_localization::Utils::flattenToString(cola_response)); return false; } - + // Set timestamps in service_response service_response.send_time_vehicle_sec = cola_telegram_response->send_timestamp_sec; // Vehicle timestamp when sending LocRequestTimestamp (seconds part of ros timestamp immediately before tcp send) service_response.send_time_vehicle_nsec = cola_telegram_response->send_timestamp_nsec; // Vehicle timestamp when sending LocRequestTimestamp (nano seconds part of ros timestamp immediately before tcp send) service_response.receive_time_vehicle_sec = cola_telegram_response->receive_timestamp_sec; // Vehicle timestamp when receiving the LocRequestTimestamp response (seconds part of ros timestamp immediately after first response byte received) service_response.receive_time_vehicle_nsec = cola_telegram_response->receive_timestamp_nsec; // Vehicle timestamp when receiving the LocRequestTimestamp response (nano seconds part of ros timestamp immediately after first response byte received) - + // Calculate time offset uint64_t send_time_vehicle_nsec = service_response.send_time_vehicle_sec * 1000000000UL + service_response.send_time_vehicle_nsec; uint64_t receive_time_vehicle_nsec = service_response.receive_time_vehicle_sec * 1000000000UL + service_response.receive_time_vehicle_nsec; uint64_t mean_time_vehicle_nsec = send_time_vehicle_nsec / 2 + receive_time_vehicle_nsec / 2; service_response.mean_time_vehicle_ms = mean_time_vehicle_nsec / 1000000; // Vehicle mean timestamp in milliseconds: (send_time_vehicle + receive_time_vehicle) / 2 service_response.delta_time_ms = service_response.mean_time_vehicle_ms - service_response.timestamp_lidar_ms; // Time offset: mean_time_vehicle_ms - timestamp_lidar_ms - + // Update software pll updateSoftwarePll(service_response); - + // Get system timestamp from ticks via ros service "SickLocTimeSync" sick_lidar_localization::SickLocTimeSyncSrv::Request time_sync_msg_request; sick_lidar_localization::SickLocTimeSyncSrv::Response time_sync_msg_response; @@ -231,7 +231,7 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): service \"SickLocTimeSync\" failed, could not get system time from ticks"); else ROS_INFO_STREAM("TimeSyncService::serviceCbRequestTimestamp(): no system time from ticks, software pll still initializing"); - + return true; } @@ -354,7 +354,7 @@ void sick_lidar_localization::TimeSyncService::runTimeSyncThreadCb(void) sick_lidar_localization::SickLocRequestTimestampSrv::Request* timestamp_service_request = ×tamp_service.request; sick_lidar_localization::SickLocRequestTimestampSrv::Response* timestamp_service_response = ×tamp_service.response; // bool service_call_ok = m_request_timestamp_client.call(timestamp_service); -#elif defined __ROS_VERSION && __ROS_VERSION == 2 +#elif defined __ROS_VERSION && __ROS_VERSION == 2 std::shared_ptr timestamp_service_request = std::make_shared(); std::shared_ptr timestamp_service_response = std::make_shared(); #endif diff --git a/test/ros2_scripts/send_cola_advanced.bash b/test/ros2_scripts/send_cola_advanced.bash index 4c65e35..f04080c 100755 --- a/test/ros2_scripts/send_cola_advanced.bash +++ b/test/ros2_scripts/send_cola_advanced.bash @@ -49,18 +49,16 @@ for ((n=0;n<1;n++)) ; do call_service SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" "success=True" call_service SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" "success=True" # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" - call_service SickDevSetLidarConfig sick_lidar_localization/srv/SickDevSetLidarConfigSrv "{index: 0, minrange: 100, maxrange: 200000, minangle: -15000, maxangle: 15000, x: 1000, y: -1000, yaw: 2000, upsidedown: true, ip: 192.168.1.30, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set=True, executed=True" - call_service SickDevGetLidarConfig sick_lidar_localization/srv/SickDevGetLidarConfigSrv "{scannerindex: 0}" "minrange=100, maxrange=200000, minangle=-15000, maxangle=15000, x=1000, y=-1000, yaw=2000, upsidedown=True, ip='192.168.1.30', port=2111, interfacetype=0, maplayer=0, active=True" - # call_service SickDevSetLidarConfig sick_lidar_localization/srv/SickDevSetLidarConfigSrv "{index: 0, minrange: 1, maxrange: 3, minangle: 0, maxangle: 2, x: 0, y: 0, yaw: 0, upsidedown: true, ip: 192.168.0.30, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set=True, executed=True" - # call_service SickDevGetLidarConfig sick_lidar_localization/srv/SickDevGetLidarConfigSrv "{scannerindex: 0}" "minrange=1, maxrange=3, minangle=0, maxangle=2, x=0, y=0, yaw=0, upsidedown=True, ip='192.168.0.30', port=2111, interfacetype=0, maplayer=0, active=True" - # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN DevSetLidarConfig +0 +300 +100000 -180000 +180000 +1000 -300 -45000 0 +12 192.168.0.30 +2122 0 0 1', wait_response_timeout: 1}" + call_service SickDevSetLidarConfig sick_lidar_localization/srv/SickDevSetLidarConfigSrv "{index: 0, minrange: 100, maxrange: 12400, minangle: 0, maxangle: 180000, x: 1000, y: -1000, yaw: 0, upsidedown: false, ip: 192.168.1.30, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set=True, executed=True" + call_service SickDevGetLidarConfig sick_lidar_localization/srv/SickDevGetLidarConfigSrv "{scannerindex: 0}" "minrange=100, maxrange=12400, minangle=0, maxangle=180000, x=1000, y=-1000, yaw=0, upsidedown=False, ip='192.168.1.30', port=2111, interfacetype=0, maplayer=0, active=True" + # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN DevSetLidarConfig +0 +300 +100000 -180000 +180000 +1000 -300 -45000 0 +12 192.168.1.30 +2111 0 0 1', wait_response_timeout: 1}" # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN DevGetLidarConfig 0', wait_response_timeout: 1}" call_service SickLocSetMap sick_lidar_localization/srv/SickLocSetMapSrv "{mapfilename: demo_map.smap}" "set=True, executed=True" call_service SickLocMap sick_lidar_localization/srv/SickLocMapSrv "{}" "map='demo_map.smap', success=True" call_service SickLocMapState sick_lidar_localization/srv/SickLocMapStateSrv "{}" "mapstate=True, success=True" - call_service SickLocInitializePose sick_lidar_localization/srv/SickLocInitializePoseSrv "{x: 100, y: -100, yaw: 2000, sigmatranslation: 1000}" "success=True" + call_service SickLocInitializePose sick_lidar_localization/srv/SickLocInitializePoseSrv "{x: 100, y: -100, yaw: 10, sigmatranslation: 1000}" "success=True" # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN LocInitializePose +10300 -5200 +30000 +1000', wait_response_timeout: 1}" - call_service SickLocInitialPose sick_lidar_localization/srv/SickLocInitialPoseSrv "{}" "x=100, y=-100, yaw=2000, sigmatranslation=1000, success=True" + call_service SickLocInitialPose sick_lidar_localization/srv/SickLocInitialPoseSrv "{}" "x=100, y=-100, yaw=10, sigmatranslation=1000, success=True" call_service SickLocSetReflectorsForSupportActive sick_lidar_localization/srv/SickLocSetReflectorsForSupportActiveSrv "{active: 1}" "success=True" call_service SickLocReflectorsForSupportActive sick_lidar_localization/srv/SickLocReflectorsForSupportActiveSrv "{}" "active=True, success=True" call_service SickLocSetOdometryActive sick_lidar_localization/srv/SickLocSetOdometryActiveSrv "{active: 1}" "set=True, executed=True" diff --git a/yaml/sim_loc_driver.yaml b/yaml/sim_loc_driver.yaml index 9f3dcd9..ad0e061 100644 --- a/yaml/sim_loc_driver.yaml +++ b/yaml/sim_loc_driver.yaml @@ -26,7 +26,7 @@ sick_lidar_localization: # Configuration for time sync service time_sync: - cola_response_timeout: 1.0 # Timeout in seconds for cola responses from localization controller + cola_response_timeout: 10.0 # Timeout in seconds for cola responses from localization controller software_pll_fifo_length: 7 # Length of software pll fifo, default: 7 time_sync_rate: 0.1 # Frequency to request timestamps from localization controller using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 time_sync_initial_rate: 1.0 # Frequency to request timestamps and to update software pll during initialization phase, default: 1.0 (LocRequestTimestamp queries every second)