From 16a1013ba77f03857ef75fba3ce347ab0a4df03e Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 7 Nov 2025 12:40:05 +0100 Subject: [PATCH 1/3] use modern postSetParametersCallback Also solves the bug causing every Double parameters change in the entire system to be printed as a change in this node --- .../imu_filter_madgwick/imu_filter_ros.h | 6 +-- imu_filter_madgwick/src/imu_filter_ros.cpp | 37 ++++++++----------- 2 files changed, 17 insertions(+), 26 deletions(-) diff --git a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h index cc5880e..7331c26 100644 --- a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h +++ b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h @@ -73,9 +73,7 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode rclcpp::TimerBase::SharedPtr check_topics_timer_; // Subscription for parameter change - rclcpp::AsyncParametersClient::SharedPtr parameters_client_; - rclcpp::Subscription::SharedPtr - parameter_event_sub_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_parameters_callback_handle_; // **** paramaters WorldFrame::WorldFrame world_frame_; @@ -108,7 +106,7 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode void publishRawMsg(const rclcpp::Time& t, float roll, float pitch, float yaw); - void reconfigCallback(rcl_interfaces::msg::ParameterEvent::SharedPtr event); + void postSetParametersCallback(const std::vector& parameters); void checkTopicsTimerCallback(); void applyYawOffset(double& q0, double& q1, double& q2, double& q3); diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index db389bd..63fa843 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -176,12 +176,8 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options) mag_bias_.y, mag_bias_.z); // **** register dynamic reconfigure - parameters_client_ = std::make_shared( - this->get_node_base_interface(), this->get_node_topics_interface(), - this->get_node_graph_interface(), this->get_node_services_interface()); - - parameter_event_sub_ = parameters_client_->on_parameter_event( - std::bind(&ImuFilterMadgwickRos::reconfigCallback, this, _1)); + post_set_parameters_callback_handle_ = + this->add_post_set_parameters_callback(std::bind(&ImuFilterMadgwickRos::postSetParametersCallback, this, std::placeholders::_1)); // **** register publishers imu_publisher_ = create_publisher("imu/data", 5); @@ -498,42 +494,39 @@ void ImuFilterMadgwickRos::publishRawMsg(const rclcpp::Time &t, float roll, rpy_raw_debug_publisher_->publish(rpy); } -void ImuFilterMadgwickRos::reconfigCallback( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +void ImuFilterMadgwickRos::postSetParametersCallback( + const std::vector& parameters) { - double gain, zeta; std::lock_guard lock(mutex_); - for (auto &changed_parameter : event->changed_parameters) + for (const auto& changed_parameter : parameters) { - const auto &type = changed_parameter.value.type; - const auto &name = changed_parameter.name; - const auto &value = changed_parameter.value; + const auto &type = changed_parameter.get_type(); + const auto &name = changed_parameter.get_name(); + const auto &value = changed_parameter.get_value(); if (type == ParameterType::PARAMETER_DOUBLE) { RCLCPP_INFO(get_logger(), "Parameter %s set to %f", name.c_str(), - value.double_value); + value); if (name == "gain") { - gain = value.double_value; - filter_.setAlgorithmGain(gain); + filter_.setAlgorithmGain(value); } else if (name == "zeta") { - zeta = value.double_value; - filter_.setDriftBiasGain(zeta); + filter_.setDriftBiasGain(value); } else if (name == "mag_bias_x") { - mag_bias_.x = value.double_value; + mag_bias_.x = value; } else if (name == "mag_bias_y") { - mag_bias_.y = value.double_value; + mag_bias_.y = value; } else if (name == "mag_bias_z") { - mag_bias_.z = value.double_value; + mag_bias_.z = value; } else if (name == "orientation_stddev") { - double orientation_stddev = value.double_value; + double orientation_stddev = value; orientation_variance_ = orientation_stddev * orientation_stddev; } } From 6a3ac45e270bc3d3388273833a2e91470afe2d62 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 10 Nov 2025 09:54:30 +0100 Subject: [PATCH 2/3] clean up a bit --- imu_filter_madgwick/src/imu_filter_ros.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index 63fa843..4a7081f 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -177,7 +177,7 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options) // **** register dynamic reconfigure post_set_parameters_callback_handle_ = - this->add_post_set_parameters_callback(std::bind(&ImuFilterMadgwickRos::postSetParametersCallback, this, std::placeholders::_1)); + this->add_post_set_parameters_callback(std::bind(&ImuFilterMadgwickRos::postSetParametersCallback, this, _1)); // **** register publishers imu_publisher_ = create_publisher("imu/data", 5); @@ -501,12 +501,10 @@ void ImuFilterMadgwickRos::postSetParametersCallback( for (const auto& changed_parameter : parameters) { - const auto &type = changed_parameter.get_type(); - const auto &name = changed_parameter.get_name(); - const auto &value = changed_parameter.get_value(); - - if (type == ParameterType::PARAMETER_DOUBLE) + if (changed_parameter.get_type() == ParameterType::PARAMETER_DOUBLE) { + const auto &name = changed_parameter.get_name(); + const auto &value = changed_parameter.get_value(); RCLCPP_INFO(get_logger(), "Parameter %s set to %f", name.c_str(), value); if (name == "gain") From 0a8961f18cadb981ff9ba7bb737d22cfd12c5547 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 11 Nov 2025 16:01:44 +0100 Subject: [PATCH 3/3] fix clang-format --- .../include/imu_filter_madgwick/imu_filter_ros.h | 6 ++++-- imu_filter_madgwick/src/imu_filter_ros.cpp | 7 ++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h index 7331c26..7ddca4c 100644 --- a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h +++ b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h @@ -73,7 +73,8 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode rclcpp::TimerBase::SharedPtr check_topics_timer_; // Subscription for parameter change - rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_parameters_callback_handle_; + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr + post_set_parameters_callback_handle_; // **** paramaters WorldFrame::WorldFrame world_frame_; @@ -106,7 +107,8 @@ class ImuFilterMadgwickRos : public imu_filter::BaseNode void publishRawMsg(const rclcpp::Time& t, float roll, float pitch, float yaw); - void postSetParametersCallback(const std::vector& parameters); + void postSetParametersCallback( + const std::vector& parameters); void checkTopicsTimerCallback(); void applyYawOffset(double& q0, double& q1, double& q2, double& q3); diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index 4a7081f..ab33c82 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -177,7 +177,8 @@ ImuFilterMadgwickRos::ImuFilterMadgwickRos(const rclcpp::NodeOptions &options) // **** register dynamic reconfigure post_set_parameters_callback_handle_ = - this->add_post_set_parameters_callback(std::bind(&ImuFilterMadgwickRos::postSetParametersCallback, this, _1)); + this->add_post_set_parameters_callback(std::bind( + &ImuFilterMadgwickRos::postSetParametersCallback, this, _1)); // **** register publishers imu_publisher_ = create_publisher("imu/data", 5); @@ -495,11 +496,11 @@ void ImuFilterMadgwickRos::publishRawMsg(const rclcpp::Time &t, float roll, } void ImuFilterMadgwickRos::postSetParametersCallback( - const std::vector& parameters) + const std::vector ¶meters) { std::lock_guard lock(mutex_); - for (const auto& changed_parameter : parameters) + for (const auto &changed_parameter : parameters) { if (changed_parameter.get_type() == ParameterType::PARAMETER_DOUBLE) {