Skip to content

Commit dc53641

Browse files
pascalaupascalau
authored
[Pid] Add enable_feedforward parameter (ros-controls#1553)
1 parent d8dc4b1 commit dc53641

File tree

8 files changed

+89
-39
lines changed

8 files changed

+89
-39
lines changed

‎pid_controller/doc/userdoc.rst‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,9 @@ Services
7474

7575
- <controller_name>/set_feedforward_control [std_srvs/srv/SetBool]
7676

77+
.. warning::
78+
This service is being deprecated in favour of setting the ``feedforward_gain`` parameter to a non-zero value.
79+
7780
Publishers
7881
,,,,,,,,,,,
7982
- <controller_name>/controller_state [control_msgs/msg/MultiDOFStateStamped]
@@ -85,6 +88,10 @@ The PID controller uses the `generate_parameter_library <https://github.com/Pick
8588

8689
List of parameters
8790
=========================
91+
.. warning::
92+
The parameter ``enable_feedforward`` is being deprecated in favor of setting the ``feedforward_gain`` parameter to a non-zero value.
93+
This might cause different behavior in the future if currently the ``feedforward_gain`` is set to a non-zero value and not activated.
94+
8895
.. generate_parameter_library_details:: ../src/pid_controller.yaml
8996

9097

‎pid_controller/include/pid_controller/pid_controller.hpp‎

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -36,12 +36,6 @@
3636
namespace pid_controller
3737
{
3838

39-
enum class feedforward_mode_type : std::uint8_t
40-
{
41-
OFF = 0,
42-
ON = 1,
43-
};
44-
4539
class PidController : public controller_interface::ChainableControllerInterface
4640
{
4741
public:
@@ -94,7 +88,7 @@ class PidController : public controller_interface::ChainableControllerInterface
9488
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerMeasuredStateMsg>> measured_state_;
9589

9690
rclcpp::Service<ControllerModeSrvType>::SharedPtr set_feedforward_control_service_;
97-
realtime_tools::RealtimeBuffer<feedforward_mode_type> control_mode_;
91+
realtime_tools::RealtimeBuffer<bool> feedforward_mode_enabled_;
9892

9993
using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
10094

‎pid_controller/src/pid_controller.cpp‎

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ PidController::PidController() : controller_interface::ChainableControllerInterf
7474

7575
controller_interface::CallbackReturn PidController::on_init()
7676
{
77-
control_mode_.initRT(feedforward_mode_type::OFF);
77+
feedforward_mode_enabled_.initRT(false);
7878

7979
try
8080
{
@@ -96,6 +96,8 @@ void PidController::update_parameters()
9696
return;
9797
}
9898
params_ = param_listener_->get_params();
99+
100+
feedforward_mode_enabled_.writeFromNonRT(params_.enable_feedforward);
99101
}
100102

101103
controller_interface::CallbackReturn PidController::configure_parameters()
@@ -230,6 +232,7 @@ controller_interface::CallbackReturn PidController::on_configure(
230232
measured_state_subscriber_ = get_node()->create_subscription<ControllerMeasuredStateMsg>(
231233
"~/measured_state", subscribers_qos, measured_state_callback);
232234
}
235+
233236
std::shared_ptr<ControllerMeasuredStateMsg> measured_state_msg =
234237
std::make_shared<ControllerMeasuredStateMsg>();
235238
reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_);
@@ -243,14 +246,13 @@ controller_interface::CallbackReturn PidController::on_configure(
243246
const std::shared_ptr<ControllerModeSrvType::Request> request,
244247
std::shared_ptr<ControllerModeSrvType::Response> response)
245248
{
246-
if (request->data)
247-
{
248-
control_mode_.writeFromNonRT(feedforward_mode_type::ON);
249-
}
250-
else
251-
{
252-
control_mode_.writeFromNonRT(feedforward_mode_type::OFF);
253-
}
249+
feedforward_mode_enabled_.writeFromNonRT(request->data);
250+
251+
RCLCPP_WARN(
252+
get_node()->get_logger(),
253+
"This service will be deprecated in favour of setting the ``feedforward_gain`` parameter to "
254+
"a non-zero value.");
255+
254256
response->success = true;
255257
};
256258

@@ -510,7 +512,7 @@ controller_interface::return_type PidController::update_and_write_commands(
510512
if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i]))
511513
{
512514
// calculate feed-forward
513-
if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON)
515+
if (*(feedforward_mode_enabled_.readFromRT()))
514516
{
515517
// two interfaces
516518
if (reference_interfaces_.size() == 2 * dof_)

‎pid_controller/src/pid_controller.yaml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,11 @@ pid_controller:
4343
default_value: false,
4444
description: "Use external states from a topic instead from state interfaces."
4545
}
46+
enable_feedforward: {
47+
type: bool,
48+
default_value: false,
49+
description: "Enables feedforward gain. (Will be deprecated in favour of setting feedforward_gain to a non-zero value.)"
50+
}
4651
gains:
4752
__map_dof_names:
4853
p: {

‎pid_controller/test/test_pid_controller.cpp‎

Lines changed: 61 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,6 @@
2121
#include <string>
2222
#include <vector>
2323

24-
using pid_controller::feedforward_mode_type;
25-
2624
class PidControllerTest : public PidControllerFixture<TestablePidController>
2725
{
2826
};
@@ -205,21 +203,68 @@ TEST_F(PidControllerTest, test_feedforward_mode_service)
205203
executor.add_node(service_caller_node_->get_node_base_interface());
206204

207205
// initially set to OFF
208-
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
206+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
209207

210208
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
211209
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
212210

213211
// should stay false
214-
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
212+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
215213

216214
// set to true
217215
ASSERT_NO_THROW(call_service(true, executor));
218-
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
216+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
219217

220218
// set back to false
221219
ASSERT_NO_THROW(call_service(false, executor));
222-
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
220+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
221+
}
222+
223+
TEST_F(PidControllerTest, test_feedforward_mode_parameter)
224+
{
225+
SetUpController();
226+
227+
// Check updating mode during on_configure
228+
229+
// initially set to OFF
230+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
231+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
232+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
233+
234+
// Reconfigure after setting parameter to true
235+
ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS);
236+
EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful);
237+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
238+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
239+
ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS);
240+
EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful);
241+
242+
// initially set to ON
243+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
244+
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
245+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
246+
247+
// Check updating mode during update_and_write_commands
248+
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
249+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
250+
251+
// Switch to ON
252+
ASSERT_EQ(
253+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
254+
controller_interface::return_type::OK);
255+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
256+
EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful);
257+
ASSERT_EQ(
258+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
259+
controller_interface::return_type::OK);
260+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
261+
262+
// Switch to OFF
263+
EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful);
264+
ASSERT_EQ(
265+
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
266+
controller_interface::return_type::OK);
267+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
223268
}
224269

225270
/**
@@ -239,7 +284,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off)
239284
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
240285
ASSERT_FALSE(controller_->is_in_chained_mode());
241286
EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0]));
242-
EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
287+
EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
243288
for (const auto & interface : controller_->reference_interfaces_)
244289
{
245290
EXPECT_TRUE(std::isnan(interface));
@@ -258,7 +303,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off)
258303
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
259304
controller_interface::return_type::OK);
260305

261-
EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
306+
EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
262307
EXPECT_EQ(
263308
controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size());
264309
EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
@@ -293,16 +338,16 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward
293338
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
294339
ASSERT_FALSE(controller_->is_in_chained_mode());
295340
EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0]));
296-
EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
341+
EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
297342
for (const auto & interface : controller_->reference_interfaces_)
298343
{
299344
EXPECT_TRUE(std::isnan(interface));
300345
}
301346

302347
controller_->set_reference(dof_command_values_);
303348

304-
controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON);
305-
EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
349+
controller_->feedforward_mode_enabled_.writeFromNonRT(true);
350+
EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
306351

307352
for (size_t i = 0; i < dof_command_values_.size(); ++i)
308353
{
@@ -315,7 +360,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward
315360
controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
316361
controller_interface::return_type::OK);
317362

318-
EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
363+
EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
319364
EXPECT_EQ(
320365
controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size());
321366
EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size());
@@ -355,7 +400,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update)
355400
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
356401
ASSERT_TRUE(controller_->is_in_chained_mode());
357402
// feedforward mode is off as default, use this for convenience
358-
EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
403+
EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
359404

360405
// update reference interface which will be used for calculation
361406
const double ref_interface_value = 5.0;
@@ -567,8 +612,8 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain)
567612
ASSERT_TRUE(controller_->is_in_chained_mode());
568613

569614
// turn on feedforward
570-
controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON);
571-
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
615+
controller_->feedforward_mode_enabled_.writeFromNonRT(true);
616+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
572617

573618
// send a message to update reference interface
574619
controller_->set_reference({target_value});
@@ -626,7 +671,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain)
626671
ASSERT_TRUE(controller_->is_in_chained_mode());
627672

628673
// feedforward by default is OFF
629-
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF);
674+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false);
630675

631676
// send a message to update reference interface
632677
controller_->set_reference({target_value});

‎pid_controller/test/test_pid_controller.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@ class TestablePidController : public pid_controller::PidController
5454
FRIEND_TEST(PidControllerTest, activate_success);
5555
FRIEND_TEST(PidControllerTest, reactivate_success);
5656
FRIEND_TEST(PidControllerTest, test_feedforward_mode_service);
57+
FRIEND_TEST(PidControllerTest, test_feedforward_mode_parameter);
5758
FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off);
5859
FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain);
5960
FRIEND_TEST(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update);

‎pid_controller/test/test_pid_controller_dual_interface.cpp‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,6 @@
2020
#include <string>
2121
#include <vector>
2222

23-
using pid_controller::feedforward_mode_type;
24-
2523
class PidControllerDualInterfaceTest : public PidControllerFixture<TestablePidController>
2624
{
2725
public:
@@ -80,8 +78,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i
8078
ASSERT_TRUE(controller_->is_in_chained_mode());
8179

8280
// turn on feedforward
83-
controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON);
84-
ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON);
81+
controller_->feedforward_mode_enabled_.writeFromNonRT(true);
82+
ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true);
8583

8684
// set up the reference interface,
8785
controller_->reference_interfaces_ = {

‎pid_controller/test/test_pid_controller_preceding.cpp‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,6 @@
2121
#include <string>
2222
#include <vector>
2323

24-
using pid_controller::feedforward_mode_type;
25-
2624
class PidControllerTest : public PidControllerFixture<TestablePidController>
2725
{
2826
};

0 commit comments

Comments
 (0)