Create a new MutuallyExclusive callback group for every timer, use a …

…multithreaded executor

ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h

@@ -348,6 +348,11 @@ class AirsimROSWrapper
     rclcpp::TimerBase::SharedPtr airsim_control_update_timer_;
     rclcpp::TimerBase::SharedPtr airsim_lidar_update_timer_;
 
+    /// Callback groups
+    std::vector<rclcpp::CallbackGroup::SharedPtr> airsim_img_callback_groups_;
+    rclcpp::CallbackGroup::SharedPtr airsim_control_callback_group_;
+    std::vector<rclcpp::CallbackGroup::SharedPtr> airsim_lidar_callback_groups_;
+
     typedef std::pair<std::vector<ImageRequest>, std::string> airsim_img_request_vehicle_name_pair;
     std::vector<airsim_img_request_vehicle_name_pair> airsim_img_request_vehicle_name_pair_vec_;
     std::vector<image_transport::Publisher> image_pub_vec_;

ros2/src/airsim_ros_pkgs/src/airsim_node.cpp

@@ -1,35 +1,19 @@
 #include <rclcpp/rclcpp.hpp>
 #include "airsim_ros_wrapper.h"
 
-int main(int argc, char** argv)
-{
+int main(int argc, char **argv) {
     rclcpp::init(argc, argv);
     rclcpp::NodeOptions node_options;
     node_options.automatically_declare_parameters_from_overrides(true);
-    std::shared_ptr<rclcpp::Node> nh = rclcpp::Node::make_shared("airsim_node", node_options);
-    std::shared_ptr<rclcpp::Node> nh_img = nh->create_sub_node("img");
-    std::shared_ptr<rclcpp::Node> nh_lidar = nh->create_sub_node("lidar");
+    std::shared_ptr <rclcpp::Node> nh = rclcpp::Node::make_shared("airsim_node", node_options);
+    std::shared_ptr <rclcpp::Node> nh_img = nh->create_sub_node("img");
+    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);
-
-    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

@@ -109,7 +109,8 @@ 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_callback_group_ = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    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_callback_group_);
 }
 
 void AirsimROSWrapper::create_ros_pubs_from_settings_json()
@@ -309,16 +310,19 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
     if (!airsim_img_request_vehicle_name_pair_vec_.empty()) {
         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));
+        auto cb = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+        airsim_img_callback_groups_.push_back(cb);
+        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), cb);
         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));
+        auto cb = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+        airsim_lidar_callback_groups_.push_back(cb);
+        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), cb);
         is_used_lidar_timer_cb_queue_ = true;
     }