Add QoS overriding options (#207)

* add yaml file with parameters and include it in CMakeLists * include the yaml in the launch file * add qos overriding options and change topic namespaces * fix typos in param names * add imu/mag topic params to yaml config file * format launch.py file * format files with clang-format --------- Signed-off-by: Aleksander Szymański <bitterisland6@gmail.com>
CCNYRoboticsLab · Oct 1, 2024 · 11bf900 · 11bf900
1 parent f089c6d
commit 11bf900
Show file tree

Hide file tree

Showing 4 changed files with 56 additions and 20 deletions.
diff --git a/imu_complementary_filter/CMakeLists.txt b/imu_complementary_filter/CMakeLists.txt
@@ -63,7 +63,7 @@ install(
   DESTINATION include
 )
 
-install(DIRECTORY launch
+install(DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}
 )
 

diff --git a/imu_complementary_filter/config/filter_config.yaml b/imu_complementary_filter/config/filter_config.yaml
@@ -0,0 +1,28 @@
+complementary_filter_gain_node:
+  ros__parameters:
+    gain_acc: 0.01
+    gain_mag: 0.01
+    bias_alpha: 0.01
+    do_bias_estimation: true
+    do_adaptive_gain: true
+    use_mag: false
+    fixed_frame: "odom"
+    publish_tf: false
+    reverse_tf: false
+    constant_dt: 0.0
+    publish_debug_topics: false
+
+    qos_overrides:
+      /imu/data_raw:
+        subscription:
+          depth: 10
+          durability: volatile
+          history: keep_last
+          reliability: reliable
+
+      /imu/mag:
+        subscription:
+          depth: 10
+          durability: volatile
+          history: keep_last
+          reliability: reliable
diff --git a/imu_complementary_filter/launch/complementary_filter.launch.py b/imu_complementary_filter/launch/complementary_filter.launch.py
@@ -1,22 +1,20 @@
+import os
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    return LaunchDescription(
-        [
-            Node(
-                package='imu_complementary_filter',
-                executable='complementary_filter_node',
-                name='complementary_filter_gain_node',
-                output='screen',
-                parameters=[
-                    {'do_bias_estimation': True},
-                    {'do_adaptive_gain': True},
-                    {'use_mag': False},
-                    {'gain_acc': 0.01},
-                    {'gain_mag': 0.01},
-                ],
-            )
-        ]
+    ld = LaunchDescription()
+
+    config = os.path.join(get_package_share_directory('imu_complementary_filter'), 'config', 'filter_config.yaml')
+
+    node = Node(
+        package='imu_complementary_filter',
+        executable='complementary_filter_node',
+        name='complementary_filter_gain_node',
+        output='screen',
+        parameters=[config],
     )
+    ld.add_action(node)
+    return ld
diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp
@@ -64,17 +64,27 @@ ComplementaryFilterROS::ComplementaryFilterROS()
         if (filter_.getDoBiasEstimation())
         {
             state_publisher_ = this->create_publisher<std_msgs::msg::Bool>(
-                "/imu/steady_state", queue_size);
+                "imu/steady_state", queue_size);
         }
     }
 
     // Register IMU raw data subscriber.
-    imu_subscriber_.reset(new ImuSubscriber(this, "/imu/data_raw"));
+    rclcpp::SubscriptionOptions sub_opts;
+    sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions{{
+        rclcpp::QosPolicyKind::Depth,
+        rclcpp::QosPolicyKind::Durability,
+        rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Reliability,
+    }};
+
+    imu_subscriber_.reset(new ImuSubscriber(this, "imu/data_raw",
+                                            rmw_qos_profile_default, sub_opts));
 
     // Register magnetic data subscriber.
     if (use_mag_)
     {
-        mag_subscriber_.reset(new MagSubscriber(this, "/imu/mag"));
+        mag_subscriber_.reset(new MagSubscriber(
+            this, "imu/mag", rmw_qos_profile_default, sub_opts));
 
         sync_.reset(new Synchronizer(SyncPolicy(queue_size), *imu_subscriber_,
                                      *mag_subscriber_));