Skip to content

Commit

Permalink
Configuration Cola-Timeouts, README.md update
Browse files Browse the repository at this point in the history
  • Loading branch information
@rostest
rostest committed Sep 23, 2020
1 parent b5cf0af commit d5b432e
Show file tree
Hide file tree
Showing 7 changed files with 87 additions and 87 deletions.
116 changes: 59 additions & 57 deletions README.md
View file

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions launch/sim_loc_driver.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down
10 changes: 5 additions & 5 deletions src/cola_services.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>(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);
Expand All @@ -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);
Expand Down Expand Up @@ -212,7 +212,7 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaSer
#elif defined __ROS_VERSION && __ROS_VERSION == 2
std::shared_ptr<sick_lidar_localization::SickLocColaTelegramSrv::Request> cola_telegram_request = std::make_shared<sick_lidar_localization::SickLocColaTelegramSrv::Request>();
std::shared_ptr<sick_lidar_localization::SickLocColaTelegramSrv::Response> cola_telegram_response = std::make_shared<sick_lidar_localization::SickLocColaTelegramSrv::Response>();
#endif
#endif
cola_telegram_request->cola_ascii_request = cola_ascii_request;
cola_telegram_request->wait_response_timeout = m_cola_response_timeout;
try
Expand All @@ -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();
Expand Down
10 changes: 5 additions & 5 deletions src/driver_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down
20 changes: 10 additions & 10 deletions src/time_sync_service.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -152,7 +152,7 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li
#elif defined __ROS_VERSION && __ROS_VERSION == 2
std::shared_ptr<sick_lidar_localization::SickLocColaTelegramSrv::Request> cola_telegram_request = std::make_shared<sick_lidar_localization::SickLocColaTelegramSrv::Request>();
std::shared_ptr<sick_lidar_localization::SickLocColaTelegramSrv::Response> cola_telegram_response = std::make_shared<sick_lidar_localization::SickLocColaTelegramSrv::Response>();
#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;
Expand All @@ -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;
}
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -354,7 +354,7 @@ void sick_lidar_localization::TimeSyncService::runTimeSyncThreadCb(void)
sick_lidar_localization::SickLocRequestTimestampSrv::Request* timestamp_service_request = &timestamp_service.request;
sick_lidar_localization::SickLocRequestTimestampSrv::Response* timestamp_service_response = &timestamp_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<sick_lidar_localization::SickLocRequestTimestampSrv::Request> timestamp_service_request = std::make_shared<sick_lidar_localization::SickLocRequestTimestampSrv::Request>();
std::shared_ptr<sick_lidar_localization::SickLocRequestTimestampSrv::Response> timestamp_service_response = std::make_shared<sick_lidar_localization::SickLocRequestTimestampSrv::Response>();
#endif
Expand Down
Loading

0 comments on commit d5b432e

Please sign in to comment.