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..d841bd7b 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, _1)); // **** register publishers imu_publisher_ = create_publisher("imu/data", 5); @@ -547,42 +543,37 @@ 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; - - 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.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; } }