From 8bcbdf229ca4b7393c128092c85aac0f9d30bd16 Mon Sep 17 00:00:00 2001 From: Max Date: Thu, 21 Jul 2022 23:05:38 +0200 Subject: [PATCH 1/8] Remove node_ prefix. --- .../launch/complementary_filter.launch.py | 2 +- imu_filter_madgwick/launch/imu_filter.launch.py | 4 ++-- .../launch/imu_filter_component.launch.py | 10 +++++----- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/imu_complementary_filter/launch/complementary_filter.launch.py b/imu_complementary_filter/launch/complementary_filter.launch.py index 71fc01a7..b6cce836 100644 --- a/imu_complementary_filter/launch/complementary_filter.launch.py +++ b/imu_complementary_filter/launch/complementary_filter.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): [ Node( package='imu_complementary_filter', - node_executable='complementary_filter_node', + executable='complementary_filter_node', name='complementary_filter_gain_node', output='screen', parameters=[ diff --git a/imu_filter_madgwick/launch/imu_filter.launch.py b/imu_filter_madgwick/launch/imu_filter.launch.py index 930f928f..90d123bb 100644 --- a/imu_filter_madgwick/launch/imu_filter.launch.py +++ b/imu_filter_madgwick/launch/imu_filter.launch.py @@ -14,8 +14,8 @@ def generate_launch_description(): [ launch_ros.actions.Node( package='imu_filter_madgwick', - node_executable='imu_filter_madgwick_node', - node_name='imu_filter', + executable='imu_filter_madgwick_node', + name='imu_filter', output='screen', parameters=[os.path.join(config_dir, 'imu_filter.yaml')], ) diff --git a/imu_filter_madgwick/launch/imu_filter_component.launch.py b/imu_filter_madgwick/launch/imu_filter_component.launch.py index acfe3663..e346c05e 100644 --- a/imu_filter_madgwick/launch/imu_filter_component.launch.py +++ b/imu_filter_madgwick/launch/imu_filter_component.launch.py @@ -21,15 +21,15 @@ def generate_launch_description(): params = yaml.safe_load(f)['imu_filter']['ros__parameters'] container = ComposableNodeContainer( - node_name='imu_filter_container', - node_namespace='', + name='imu_filter_container', + namespace='', package='rclcpp_components', - node_executable='component_container', + executable='component_container', composable_node_descriptions=[ ComposableNode( package='imu_filter_madgwick', - node_plugin='ImuFilterMadgwickRos', - node_name='imu_filter', + plugin='ImuFilterMadgwickRos', + name='imu_filter', parameters=[params], ) ], From e084d56338c8dc63a0c5408b6f329c331d27c682 Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Thu, 16 Nov 2023 10:48:31 +0100 Subject: [PATCH 2/8] Update qos to sensor_data. --- imu_complementary_filter/src/complementary_filter_ros.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index 96f1bdaa..dae12aaa 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -69,12 +69,13 @@ ComplementaryFilterROS::ComplementaryFilterROS() } // Register IMU raw data subscriber. - imu_subscriber_.reset(new ImuSubscriber(this, "/imu/data_raw")); + rmw_qos_profile_t qos = rmw_qos_profile_sensor_data; + imu_subscriber_.reset(new ImuSubscriber(this, "/imu/data_raw", qos)); // Register magnetic data subscriber. if (use_mag_) { - mag_subscriber_.reset(new MagSubscriber(this, "/imu/mag")); + mag_subscriber_.reset(new MagSubscriber(this, "/imu/mag", qos)); sync_.reset(new Synchronizer(SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_)); From 8ace1351919addf19eb3f5b22c4f5ba417bd3b06 Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Thu, 16 Nov 2023 18:34:34 +0100 Subject: [PATCH 3/8] Rotate to ENU. --- imu_complementary_filter/src/complementary_filter_ros.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index dae12aaa..00d9835b 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -242,6 +242,12 @@ void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) filter_.getOrientation(q0, q1, q2, q3); tf2::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3); + //Rotate orientation to report in ENU coordinate convention instead of NWU. -- Alpistinho + tf2::Quaternion qRot; + qRot.setRPY( 0, 0, M_PI/2.0 ); + q = qRot*q; + q.normalize(); + // Create and publish fitlered IMU message. ImuMsg::SharedPtr imu_msg = std::make_shared(*imu_msg_raw); imu_msg->orientation.x = q1; From be735ccc147dc0f33fec6f47f9cb8c31ed43f537 Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Thu, 16 Nov 2023 19:39:10 +0100 Subject: [PATCH 4/8] Update rotation. --- .../src/complementary_filter_ros.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index 00d9835b..f1380587 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -244,16 +244,16 @@ void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) //Rotate orientation to report in ENU coordinate convention instead of NWU. -- Alpistinho tf2::Quaternion qRot; - qRot.setRPY( 0, 0, M_PI/2.0 ); + qRot.setRPY( 0, 0, -M_PI/2.0 ); q = qRot*q; q.normalize(); // Create and publish fitlered IMU message. ImuMsg::SharedPtr imu_msg = std::make_shared(*imu_msg_raw); - imu_msg->orientation.x = q1; - imu_msg->orientation.y = q2; - imu_msg->orientation.z = q3; - imu_msg->orientation.w = q0; + imu_msg->orientation.x = q.x(); + imu_msg->orientation.y = q.y(); + imu_msg->orientation.z = q.z(); + imu_msg->orientation.w = q.w(); imu_msg->orientation_covariance[0] = orientation_variance_; imu_msg->orientation_covariance[1] = 0.0; From 6373e518aff85dd163041beccc8e706dbef0d923 Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Thu, 16 Nov 2023 19:39:35 +0100 Subject: [PATCH 5/8] Use mag. --- .../launch/complementary_filter.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/imu_complementary_filter/launch/complementary_filter.launch.py b/imu_complementary_filter/launch/complementary_filter.launch.py index b6cce836..f7a89bfe 100644 --- a/imu_complementary_filter/launch/complementary_filter.launch.py +++ b/imu_complementary_filter/launch/complementary_filter.launch.py @@ -13,9 +13,9 @@ def generate_launch_description(): parameters=[ {'do_bias_estimation': True}, {'do_adaptive_gain': True}, - {'use_mag': False}, - {'gain_acc': 0.01}, - {'gain_mag': 0.01}, + {'use_mag': True}, + {'gain_acc': 0.1}, + {'gain_mag': 0.1}, ], ) ] From 8f09a8e46464c1b5c911970a68a457787e23ba5c Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Thu, 16 Nov 2023 19:48:50 +0100 Subject: [PATCH 6/8] +pi/2 --- imu_complementary_filter/src/complementary_filter_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index f1380587..441b3177 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -244,7 +244,7 @@ void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) //Rotate orientation to report in ENU coordinate convention instead of NWU. -- Alpistinho tf2::Quaternion qRot; - qRot.setRPY( 0, 0, -M_PI/2.0 ); + qRot.setRPY( 0, 0, M_PI/2.0 ); q = qRot*q; q.normalize(); From b3a02a1f89fa30c45524ffe0680784a17d6a8277 Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Thu, 16 Nov 2023 21:31:33 +0100 Subject: [PATCH 7/8] Use negative to have same as madgwick. --- imu_complementary_filter/src/complementary_filter_ros.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index 441b3177..b74a686d 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -250,10 +250,10 @@ void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) // Create and publish fitlered IMU message. ImuMsg::SharedPtr imu_msg = std::make_shared(*imu_msg_raw); - imu_msg->orientation.x = q.x(); - imu_msg->orientation.y = q.y(); - imu_msg->orientation.z = q.z(); - imu_msg->orientation.w = q.w(); + imu_msg->orientation.x = -q.x(); + imu_msg->orientation.y = -q.y(); + imu_msg->orientation.z = -q.z(); + imu_msg->orientation.w = -q.w(); imu_msg->orientation_covariance[0] = orientation_variance_; imu_msg->orientation_covariance[1] = 0.0; From 59580e7c77f87e0f73183aa896918a824a07edea Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Sun, 19 Nov 2023 12:22:33 +0100 Subject: [PATCH 8/8] Remove the minus. --- imu_complementary_filter/src/complementary_filter_ros.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/imu_complementary_filter/src/complementary_filter_ros.cpp b/imu_complementary_filter/src/complementary_filter_ros.cpp index b74a686d..441b3177 100644 --- a/imu_complementary_filter/src/complementary_filter_ros.cpp +++ b/imu_complementary_filter/src/complementary_filter_ros.cpp @@ -250,10 +250,10 @@ void ComplementaryFilterROS::publish(ImuMsg::ConstSharedPtr imu_msg_raw) // Create and publish fitlered IMU message. ImuMsg::SharedPtr imu_msg = std::make_shared(*imu_msg_raw); - imu_msg->orientation.x = -q.x(); - imu_msg->orientation.y = -q.y(); - imu_msg->orientation.z = -q.z(); - imu_msg->orientation.w = -q.w(); + imu_msg->orientation.x = q.x(); + imu_msg->orientation.y = q.y(); + imu_msg->orientation.z = q.z(); + imu_msg->orientation.w = q.w(); imu_msg->orientation_covariance[0] = orientation_variance_; imu_msg->orientation_covariance[1] = 0.0;