diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index a34dc9f87f..a4b1ef94da 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -80,10 +80,6 @@ class PidController : public controller_interface::ChainableControllerInterface controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - PID_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -94,6 +90,8 @@ class PidController : public controller_interface::ChainableControllerInterface using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; protected: + controller_interface::return_type update_reference_from_subscribers() override; + std::shared_ptr param_listener_; pid_controller::Params params_; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 373e941d96..53f7e38cc4 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -30,10 +30,16 @@ namespace { // utility // Changed services history QoS to keep all so we don't lose any client service calls -rclcpp::QoS qos_services = - rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) - .reliable() - .durability_volatile(); +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; @@ -377,8 +383,7 @@ controller_interface::CallbackReturn PidController::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type PidController::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +controller_interface::return_type PidController::update_reference_from_subscribers() { auto current_ref = input_ref_.readFromRT();