fix(ros2): salvage reentrant timer callback group (credit @nikola-j #4559)

Reentrant callback group + MultiThreadedExecutor for img/lidar/control timers.

Signed-off-by: Bartok9 <[email protected]>

Author: Bartok9

ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h

@@ -119,7 +119,7 @@ class AirsimROSWrapper
         CAR
     };
 
-    AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const std::shared_ptr<rclcpp::Node> nh_img, const std::shared_ptr<rclcpp::Node> nh_lidar, const std::string& host_ip);
+    AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const std::shared_ptr<rclcpp::Node> nh_img, const std::shared_ptr<rclcpp::Node> nh_lidar, const std::string& host_ip, const std::shared_ptr<rclcpp::CallbackGroup> callback_group);
     ~AirsimROSWrapper(){};
 
     void initialize_airsim();
@@ -317,6 +317,7 @@ class AirsimROSWrapper
     std::shared_ptr<rclcpp::Node> nh_;
     std::shared_ptr<rclcpp::Node> nh_img_;
     std::shared_ptr<rclcpp::Node> nh_lidar_;
+    std::shared_ptr<rclcpp::CallbackGroup> callback_group_;
 
     // todo not sure if async spinners shuold be inside this class, or should be instantiated in airsim_node.cpp, and cb queues should be public
     // todo for multiple drones with multiple sensors, this won't scale. make it a part of VehicleROS?

ros2/src/airsim_ros_pkgs/src/airsim_node.cpp

@@ -11,25 +11,12 @@ int main(int argc, char** argv)
     std::shared_ptr<rclcpp::Node> nh_lidar = nh->create_sub_node("lidar");
     std::string host_ip;
     nh->get_parameter("host_ip", host_ip);
-    AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, host_ip);
+    auto callback_group = nh->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+    AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, host_ip, callback_group);
 
-    if (airsim_ros_wrapper.is_used_img_timer_cb_queue_) {
-        rclcpp::executors::SingleThreadedExecutor executor;
-        executor.add_node(nh_img);
-        while (rclcpp::ok()) {
-            executor.spin();
-        }
-    }
-
-    if (airsim_ros_wrapper.is_used_lidar_timer_cb_queue_) {
-        rclcpp::executors::SingleThreadedExecutor executor;
-        executor.add_node(nh_lidar);
-        while (rclcpp::ok()) {
-            executor.spin();
-        }
-    }
-
-    rclcpp::spin(nh);
+    rclcpp::executors::MultiThreadedExecutor executor;
+    executor.add_node(nh);
+    executor.spin();
 
     return 0;
-}
+}

ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp

@@ -24,7 +24,7 @@ const std::unordered_map<int, std::string> AirsimROSWrapper::image_type_int_to_s
     { 7, "Infrared" }
 };
 
-AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const std::shared_ptr<rclcpp::Node> nh_img, const std::shared_ptr<rclcpp::Node> nh_lidar, const std::string& host_ip)
+AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const std::shared_ptr<rclcpp::Node> nh_img, const std::shared_ptr<rclcpp::Node> nh_lidar, const std::string& host_ip, const std::shared_ptr<rclcpp::CallbackGroup> callback_group)
     : is_used_lidar_timer_cb_queue_(false)
     , is_used_img_timer_cb_queue_(false)
     , airsim_settings_parser_(host_ip)
@@ -35,6 +35,7 @@ AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const
     , nh_(nh)
     , nh_img_(nh_img)
     , nh_lidar_(nh_lidar)
+    , callback_group_(callback_group)
     , isENU_(false)
     , publish_clock_(false)
 {
@@ -109,7 +110,7 @@ void AirsimROSWrapper::initialize_ros()
 
     nh_->declare_parameter("vehicle_name", rclcpp::ParameterValue(""));
     create_ros_pubs_from_settings_json();
-    airsim_control_update_timer_ = nh_->create_wall_timer(std::chrono::duration<double>(update_airsim_control_every_n_sec), std::bind(&AirsimROSWrapper::drone_state_timer_cb, this));
+    airsim_control_update_timer_ = nh_->create_wall_timer(std::chrono::duration<double>(update_airsim_control_every_n_sec), std::bind(&AirsimROSWrapper::drone_state_timer_cb, this), callback_group_);
 }
 
 void AirsimROSWrapper::create_ros_pubs_from_settings_json()
@@ -310,15 +311,15 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
         double update_airsim_img_response_every_n_sec;
         nh_->get_parameter("update_airsim_img_response_every_n_sec", update_airsim_img_response_every_n_sec);
 
-        airsim_img_response_timer_ = nh_img_->create_wall_timer(std::chrono::duration<double>(update_airsim_img_response_every_n_sec), std::bind(&AirsimROSWrapper::img_response_timer_cb, this));
+        airsim_img_response_timer_ = nh_img_->create_wall_timer(std::chrono::duration<double>(update_airsim_img_response_every_n_sec), std::bind(&AirsimROSWrapper::img_response_timer_cb, this), callback_group_);
         is_used_img_timer_cb_queue_ = true;
     }
 
     // lidars update on their own callback/thread at a given rate
     if (lidar_cnt > 0) {
         double update_lidar_every_n_sec;
         nh_->get_parameter("update_lidar_every_n_sec", update_lidar_every_n_sec);
-        airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration<double>(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this));
+        airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration<double>(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this), callback_group_);
         is_used_lidar_timer_cb_queue_ = true;
     }