SW-4997: Switch from using ROS timer to threads for polling lidar data (

#139)

* Switch from using ROS timer to thread for polling data
* Specify param defaults for non-required params

ouster-ros/config/parameters.yaml

@@ -15,4 +15,4 @@ ouster/os_cloud:
     sensor_frame: 'os_sensor'
     lidar_frame: 'os_lidar'
     imu_frame: 'os_imu'
-    timestamp_mode: ''
+    timestamp_mode: ''  # this value needs to match os_sensor/timestamp_mode

ouster-ros/src/os_cloud_node.cpp

@@ -113,10 +113,10 @@ class OusterCloud : public OusterProcessingNodeBase {
     }
 
     void declare_parameters() {
-        declare_parameter("sensor_frame");
-        declare_parameter("lidar_frame");
-        declare_parameter("imu_frame");
-        declare_parameter("timestamp_mode");
+        declare_parameter("sensor_frame", "os_sensor");
+        declare_parameter("lidar_frame", "os_lidar");
+        declare_parameter("imu_frame", "os_imu");
+        declare_parameter("timestamp_mode", "");
     }
 
     void parse_parameters() {

ouster-ros/src/os_sensor_node.cpp

@@ -90,21 +90,8 @@ class OusterSensor : public OusterSensorNodeBase {
         LifecycleNode::on_activate(state);
         lidar_packet_pub->on_activate();
         imu_packet_pub->on_activate();
-
         allocate_buffers();
-        if (!connection_loop_timer) {
-            // TOOD: replace with a thread instead?
-            connection_loop_timer =
-                rclcpp::create_timer(this, get_clock(), 0s, [this]() {
-                    if (reset_in_progress) return;
-                    auto& pf = sensor::get_format(info);
-                    connection_loop(*sensor_client, pf);
-                });
-        } else {
-            connection_loop_timer->reset();
-        }
-
-        reset_in_progress = false;
+        start_sensor_connection_thread();
         return LifecycleNodeInterface::CallbackReturn::SUCCESS;
     }
 
@@ -115,12 +102,29 @@ class OusterSensor : public OusterSensorNodeBase {
         return LifecycleNodeInterface::CallbackReturn::FAILURE;
     }
 
+    void start_sensor_connection_thread() {
+        sensor_connection_active = true;
+        sensor_connection_thread = std::make_unique<std::thread>([this]() {
+            auto& pf = sensor::get_format(info);
+            while (sensor_connection_active) {
+                connection_loop(*sensor_client, pf);
+            }
+            RCLCPP_INFO(get_logger(), "connection_loop DONE.");
+        });
+    }
+
+    void stop_sensor_connection_thread() {
+        if (sensor_connection_thread->joinable()) {
+            sensor_connection_active = false;
+            sensor_connection_thread->join();
+        }
+    }
+
     LifecycleNodeInterface::CallbackReturn on_deactivate(
         const rclcpp_lifecycle::State& state) {
         RCLCPP_DEBUG(get_logger(), "on_deactivate() is called.");
         LifecycleNode::on_deactivate(state);
-        connection_loop_timer->cancel();
-
+        stop_sensor_connection_thread();
         return LifecycleNodeInterface::CallbackReturn::SUCCESS;
     }
 
@@ -150,8 +154,7 @@ class OusterSensor : public OusterSensorNodeBase {
         }
 
         if (state.label() == "active") {
-            // stop the timer first then perform cleanup
-            connection_loop_timer->cancel();
+            stop_sensor_connection_thread();
         }
 
         // whether state was 'active' or 'inactive' do cleanup
@@ -171,15 +174,15 @@ class OusterSensor : public OusterSensorNodeBase {
    private:
     void declare_parameters() {
         declare_parameter("sensor_hostname");
-        declare_parameter("metadata");
-        declare_parameter("udp_dest");
-        declare_parameter("mtp_dest");
+        declare_parameter("metadata", "");
+        declare_parameter("udp_dest", "");
+        declare_parameter("mtp_dest", "");
         declare_parameter("mtp_main", false);
         declare_parameter("lidar_port", 0);
         declare_parameter("imu_port", 0);
-        declare_parameter("lidar_mode");
-        declare_parameter("timestamp_mode");
-        declare_parameter("udp_profile_lidar");
+        declare_parameter("lidar_mode", "");
+        declare_parameter("timestamp_mode", "");
+        declare_parameter("udp_profile_lidar", "");
     }
 
     std::string get_sensor_hostname() {
@@ -312,16 +315,14 @@ class OusterSensor : public OusterSensorNodeBase {
 
     // param init_id_reset is overriden to true when force_reinit is true
     void reset_sensor(bool force_reinit, bool init_id_reset = false) {
-        if (reset_in_progress) {
-            RCLCPP_WARN(
-                get_logger(),
-                "sensor reset is already in progress, ignoring second call!");
+        if (!sensor_connection_active) {
+            RCLCPP_WARN(get_logger(),
+                        "sensor reset is invoked but sensor connection is not "
+                        "active, ignoring call!");
             return;
         }
 
-        reset_in_progress = true;
         force_sensor_reinit = force_reinit;
-        connection_loop_timer->cancel();
         reset_last_init_id = force_reinit ? true : init_id_reset;
         auto request_transitions = std::vector<uint8_t>{
             lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE,
@@ -333,15 +334,14 @@ class OusterSensor : public OusterSensorNodeBase {
 
     // TODO: need to notify dependent node(s) of the update
     void reactivate_sensor(bool init_id_reset = false) {
-        if (reset_in_progress) {
-            RCLCPP_WARN(
-                get_logger(),
-                "sensor reset is already in progress, ignoring second call!");
+        if (!sensor_connection_active) {
+            // This may indicate that we are in the process of re-activation
+            RCLCPP_WARN(get_logger(),
+                        "sensor reactivate is invoked but sensor connection is "
+                        "not active, ignoring call!");
             return;
         }
 
-        reset_in_progress = true;
-        connection_loop_timer->cancel();
         reset_last_init_id = init_id_reset;
         update_config_and_metadata(*sensor_client);
         publish_metadata();
@@ -800,7 +800,7 @@ class OusterSensor : public OusterSensorNodeBase {
         get_metadata_srv.reset();
         get_config_srv.reset();
         set_config_srv.reset();
-        connection_loop_timer.reset();
+        sensor_connection_thread.reset();
     }
 
    private:
@@ -818,11 +818,11 @@ class OusterSensor : public OusterSensorNodeBase {
     rclcpp::Service<GetConfig>::SharedPtr get_config_srv;
     rclcpp::Service<SetConfig>::SharedPtr set_config_srv;
     std::shared_ptr<rclcpp::Client<ChangeState>> change_state_client;
-    std::shared_ptr<rclcpp::TimerBase> connection_loop_timer;
+    std::unique_ptr<std::thread> sensor_connection_thread;
 
     bool force_sensor_reinit = false;
     bool reset_last_init_id = true;
-    std::atomic<bool> reset_in_progress = {false};
+    std::atomic<bool> sensor_connection_active = {false};
 
     bool last_init_id_initialized = false;
     uint32_t last_init_id;