From e45fd8543e25f23510b94f2199e13393cca39a3b Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 7 Nov 2025 12:40:05 +0100 Subject: [PATCH 1/2] 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 a0851f7b..371fc538 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 @@ -83,9 +83,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_; @@ -121,7 +119,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 a363a60c..98fe3a8e 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -186,12 +186,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); @@ -547,42 +543,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 8bbc08118d1ab10c237c7e21ada8bbd8dd6b764d Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 10 Nov 2025 09:54:30 +0100 Subject: [PATCH 2/2] 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 98fe3a8e..d841bd7b 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -187,7 +187,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); @@ -550,12 +550,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")