diff --git a/doc/migration.rst b/doc/migration.rst index f264e650ef..72de6cabb0 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -17,3 +17,4 @@ pid_controller * Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are removed. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. +* PID state publisher topic changed to ```` namespace and is initially turned off. It can be turned on by using ``activate_state_publisher`` parameter. (`#1823 `_). diff --git a/doc/release_notes.rst b/doc/release_notes.rst index cc4a2bd5d4..87bf9f3eef 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,3 +22,4 @@ pid_controller * Output clamping via ``u_clamp_max`` and ``u_clamp_min`` was added, allowing users to bound the controller output. * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy. * A new ``error_deadband`` parameter stops integration when the error is within a specified range. +* PID state publisher can be turned off or on by using ``activate_state_publisher`` parameter. (`#1823 `_). diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index a643905aea..a479366ae4 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -72,6 +72,9 @@ If controller parameter ``use_external_measured_states`` is true: Publishers ,,,,,,,,,,, - /controller_state [control_msgs/msg/MultiDOFStateStamped] +- //pid_state [control_msgs/msg/PidState] + +Initially the PidState publisher is turned off. It can be turned on by using ``gains..activate_state_publisher`` parameter. Parameters ,,,,,,,,,,, diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index d8caac42c1..25cc136ab1 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -126,9 +126,8 @@ controller_interface::CallbackReturn PidController::configure_parameters() for (size_t i = 0; i < dof_; ++i) { - // prefix should be interpreted as parameters prefix - pids_[i] = - std::make_shared(get_node(), "gains." + params_.dof_names[i], true); + pids_[i] = std::make_shared( + get_node(), "gains." + params_.dof_names[i], "~/" + params_.dof_names[i], false); if (!pids_[i]->initialize_from_ros_parameters()) { return CallbackReturn::FAILURE; diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 7ea8ec25b0..947bb7e9df 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -130,3 +130,8 @@ pid_controller: default_value: true, description: "Indicating if integral term is retained after re-activation" } + activate_state_publisher: { + type: bool, + default_value: false, + description: "Individual state publisher activation for each DOF. If true, the controller will publish the state of each DOF to the topic `///pid_state`." + }