From c0f5e4a8d9d23586b008d615702d345a79dfe23b Mon Sep 17 00:00:00 2001 From: tombrodie Date: Fri, 28 Apr 2023 14:21:07 +1000 Subject: [PATCH 1/4] Handle disconnected lidar sensor on init and during runtime --- launch/record.launch | 22 +++++----- launch/sensor.launch | 26 +++++------ launch/sensor_mtp.launch | 18 ++++---- src/os_sensor_nodelet.cpp | 90 +++++++++++++++++++++++++++------------ 4 files changed, 99 insertions(+), 57 deletions(-) diff --git a/launch/record.launch b/launch/record.launch index ee7f590f..7c16df51 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -1,11 +1,12 @@ - - - - - + + + + + - - - - - + + + + + + diff --git a/launch/sensor.launch b/launch/sensor.launch index 5d086f56..a5a811a8 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -1,17 +1,18 @@ - - - - - + + + + + - - - - - - + + + + + doc="The request no bond setup when nodelets are created"/> @@ -48,6 +49,7 @@ args="load nodelets_os/OusterSensor os_nodelet_mgr $(arg _no_bond)"> + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 8a0cb711..62d709a4 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -1,9 +1,10 @@ - - - + + + @@ -15,7 +16,7 @@ RNG19_RFL8_SIG16_NIR16, RNG15_RFL8_NIR8 }"/> - - - - + + - + + diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index efb89d5f..39bfc7f3 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -36,10 +36,9 @@ class OusterSensor : public OusterClientBase { virtual void onInit() override { auto& pnh = getPrivateNodeHandle(); sensor_hostname = get_sensor_hostname(pnh); - sensor::sensor_config config; - uint8_t flags; - std::tie(config, flags) = create_sensor_config_rosparams(pnh); - configure_sensor(sensor_hostname, config, flags); + bool retry_configuration = false; + std::tie(config, flags) = create_sensor_config_rosparams(pnh, retry_configuration); + configure_sensor(sensor_hostname, config, flags, retry_configuration); sensor_client = create_sensor_client(sensor_hostname, config); auto& nh = getNodeHandle(); create_metadata_publisher(nh); @@ -141,7 +140,7 @@ class OusterSensor : public OusterClientBase { } try { - configure_sensor(sensor_hostname, config, 0); + configure_sensor(sensor_hostname, config, 0, false); } catch (const std::exception& e) { return false; } @@ -190,7 +189,7 @@ class OusterSensor : public OusterClientBase { } std::pair create_sensor_config_rosparams( - ros::NodeHandle& nh) { + ros::NodeHandle& nh, bool& retry_configuration) { auto udp_dest = nh.param("udp_dest", std::string{}); auto mtp_dest_arg = nh.param("mtp_dest", std::string{}); auto mtp_main_arg = nh.param("mtp_main", false); @@ -201,6 +200,8 @@ class OusterSensor : public OusterClientBase { auto udp_profile_lidar_arg = nh.param("udp_profile_lidar", std::string{}); + retry_configuration = nh.param("retry_configuration", false); + if (lidar_port < 0 || lidar_port > 65535) { auto error_msg = "Invalid lidar port number! port value should be in the range " @@ -312,31 +313,48 @@ class OusterSensor : public OusterClientBase { return std::make_pair(config, config_flags); } - void configure_sensor(const std::string& hostname, - sensor::sensor_config& config, int config_flags) { - if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) && - !mtp_main) { - if (!get_config(hostname, config, true)) { - NODELET_ERROR("Error getting active config"); - } else { - NODELET_INFO("Retrived active config of sensor"); + bool configure_sensor(const std::string& hostname, + sensor::sensor_config& config, + const int config_flags, + const bool retry_configuration) { + bool is_configured = false; + bool is_first_attempt = true; + do { + // Throttling + if (!is_first_attempt) { + ros::Duration(0.01).sleep(); + ros::spinOnce(); } - return; - } - try { - if (!set_config(hostname, config, config_flags)) { - auto err_msg = "Error connecting to sensor " + hostname; - NODELET_ERROR_STREAM(err_msg); - throw std::runtime_error(err_msg); + if (config.udp_dest && + sensor::in_multicast(config.udp_dest.value()) && + !mtp_main) { + if (!get_config(hostname, config, true)) { + NODELET_ERROR("Error getting active config"); + } else { + NODELET_INFO("Retrieved active config of sensor"); + } + } else { + try { + if (!set_config(hostname, config, config_flags)) { + auto err_msg = "Error connecting to sensor " + hostname; + NODELET_ERROR_STREAM(err_msg); + } else { + is_configured = true; + } + } catch (const std::exception& e) { + NODELET_ERROR("Error setting config: %s", e.what()); + } } - } catch (const std::exception& e) { - NODELET_ERROR("Error setting config: %s", e.what()); - throw; - } + + is_first_attempt = false; + } while (!is_configured && retry_configuration); NODELET_INFO_STREAM("Sensor " << hostname - << " was configured successfully"); + << " was " + << (is_configured ? "" : "not ") << "configured successfully"); + + return is_configured; } bool load_config_file(const std::string& config_file, @@ -438,8 +456,22 @@ class OusterSensor : public OusterClientBase { return; } if (state & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) + if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) { + had_reconnection_success = false; + first_lidar_data_rx = ros::Time::now(); lidar_packet_pub.publish(lidar_packet); + } + } + else if (!cached_config.empty() && + !had_reconnection_success && + (first_lidar_data_rx != ros::Time(0.0)) && + (ros::Time::now().toSec() - first_lidar_data_rx.toSec()) > 3.0) { + NODELET_ERROR("poll_client: attempting reconnection"); + had_reconnection_success = configure_sensor(sensor_hostname, config, flags, false); + + if (had_reconnection_success) { + sensor_client = create_sensor_client(sensor_hostname, config); + } } if (state & sensor::IMU_DATA) { if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf)) @@ -457,9 +489,13 @@ class OusterSensor : public OusterClientBase { std::string sensor_hostname; ros::ServiceServer get_config_srv; ros::ServiceServer set_config_srv; + ros::Time first_lidar_data_rx = ros::Time(0.0); std::string cached_config; std::string mtp_dest; bool mtp_main; + bool had_reconnection_success = false; + uint8_t flags; + sensor::sensor_config config; }; } // namespace nodelets_os From 1bbc83d6a9a4649b4daf09d7d4a8465ee1c9c17e Mon Sep 17 00:00:00 2001 From: tombrodie Date: Mon, 1 May 2023 09:32:23 +1000 Subject: [PATCH 2/4] Remove unnecessary check, remove magic number --- src/os_sensor_nodelet.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index 39bfc7f3..c4aa6f0d 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -446,6 +446,8 @@ class OusterSensor : public OusterClientBase { } void connection_loop(sensor::client& cli, const sensor::packet_format& pf) { + static constexpr double TIMEOUT = 3.0; + auto state = sensor::poll_client(cli); if (state == sensor::EXIT) { NODELET_INFO("poll_client: caught signal, exiting"); @@ -462,10 +464,9 @@ class OusterSensor : public OusterClientBase { lidar_packet_pub.publish(lidar_packet); } } - else if (!cached_config.empty() && - !had_reconnection_success && + else if (!had_reconnection_success && (first_lidar_data_rx != ros::Time(0.0)) && - (ros::Time::now().toSec() - first_lidar_data_rx.toSec()) > 3.0) { + (ros::Time::now().toSec() - first_lidar_data_rx.toSec()) > TIMEOUT) { NODELET_ERROR("poll_client: attempting reconnection"); had_reconnection_success = configure_sensor(sensor_hostname, config, flags, false); From 8cfe967300fb6be2d159ce9fb8ce5f4942ebb451 Mon Sep 17 00:00:00 2001 From: tombrodie Date: Mon, 8 May 2023 12:15:50 +1000 Subject: [PATCH 3/4] Remove hanging spin --- src/os_sensor_nodelet.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index c4aa6f0d..bd633f67 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -323,7 +323,6 @@ class OusterSensor : public OusterClientBase { // Throttling if (!is_first_attempt) { ros::Duration(0.01).sleep(); - ros::spinOnce(); } if (config.udp_dest && From bfe481c95347ed97899e35e4c0b841cd3f0b4eb1 Mon Sep 17 00:00:00 2001 From: tombrodie Date: Mon, 8 May 2023 12:16:04 +1000 Subject: [PATCH 4/4] Simplify logging step --- src/os_sensor_nodelet.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index bd633f67..217bf182 100644 --- a/src/os_sensor_nodelet.cpp +++ b/src/os_sensor_nodelet.cpp @@ -336,8 +336,7 @@ class OusterSensor : public OusterClientBase { } else { try { if (!set_config(hostname, config, config_flags)) { - auto err_msg = "Error connecting to sensor " + hostname; - NODELET_ERROR_STREAM(err_msg); + NODELET_ERROR("Error connecting to sensor: '%s'", hostname.c_str()); } else { is_configured = true; }