@@ -542,6 +542,137 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status)
542
542
}
543
543
}
544
544
545
+ /* *
546
+ * @brief check chained pid controller with feedforward and gain as non-zero, single interface
547
+ */
548
+ TEST_F (PidControllerTest, test_update_chained_feedforward_with_gain)
549
+ {
550
+ // state interface value is 1.1 as defined in test fixture
551
+ // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95
552
+ // with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95
553
+ const double target_value = 5.0 ;
554
+ const double expected_command_value = 6.95 ;
555
+
556
+ SetUpController (" test_pid_controller_with_feedforward_gain" );
557
+ ASSERT_EQ (controller_->on_configure (rclcpp_lifecycle::State ()), NODE_SUCCESS);
558
+
559
+ // check on interfaces & pid gain parameters
560
+ for (const auto & dof_name : dof_names_)
561
+ {
562
+ ASSERT_EQ (controller_->params_ .gains .dof_names_map [dof_name].p , 0.5 );
563
+ ASSERT_EQ (controller_->params_ .gains .dof_names_map [dof_name].feedforward_gain , 1.0 );
564
+ }
565
+ ASSERT_EQ (controller_->params_ .command_interface , command_interface_);
566
+ EXPECT_THAT (
567
+ controller_->params_ .reference_and_state_interfaces ,
568
+ testing::ElementsAreArray (state_interfaces_));
569
+ ASSERT_FALSE (controller_->params_ .use_external_measured_states );
570
+
571
+ // setup executor
572
+ rclcpp::executors::MultiThreadedExecutor executor;
573
+ executor.add_node (controller_->get_node ()->get_node_base_interface ());
574
+ executor.add_node (service_caller_node_->get_node_base_interface ());
575
+
576
+ controller_->set_chained_mode (true );
577
+
578
+ // activate controller
579
+ ASSERT_EQ (controller_->on_activate (rclcpp_lifecycle::State ()), NODE_SUCCESS);
580
+ ASSERT_TRUE (controller_->is_in_chained_mode ());
581
+
582
+ // turn on feedforward
583
+ controller_->control_mode_ .writeFromNonRT (feedforward_mode_type::ON);
584
+ ASSERT_EQ (*(controller_->control_mode_ .readFromRT ()), feedforward_mode_type::ON);
585
+
586
+ // send a message to update reference interface
587
+ std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
588
+ msg->dof_names = controller_->params_ .dof_names ;
589
+ msg->values .resize (msg->dof_names .size (), 0.0 );
590
+ for (size_t i = 0 ; i < msg->dof_names .size (); ++i)
591
+ {
592
+ msg->values [i] = target_value;
593
+ }
594
+ msg->values_dot .resize (msg->dof_names .size (), std::numeric_limits<double >::quiet_NaN ());
595
+ controller_->input_ref_ .writeFromNonRT (msg);
596
+ ASSERT_EQ (
597
+ controller_->update_reference_from_subscribers (
598
+ rclcpp::Time (0 ), rclcpp::Duration::from_seconds (0.01 )),
599
+ controller_interface::return_type::OK);
600
+
601
+ // run update
602
+ ASSERT_EQ (
603
+ controller_->update (rclcpp::Time (0 ), rclcpp::Duration::from_seconds (0.01 )),
604
+ controller_interface::return_type::OK);
605
+
606
+ // check on result from update
607
+ ASSERT_EQ (controller_->command_interfaces_ [0 ].get_value (), expected_command_value);
608
+ }
609
+
610
+ /* *
611
+ * @brief check chained pid controller with feedforward OFF and gain as non-zero, single interface
612
+ */
613
+ TEST_F (PidControllerTest, test_update_chained_feedforward_off_with_gain)
614
+ {
615
+ // state interface value is 1.1 as defined in test fixture
616
+ // given target value 5.0
617
+ // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95
618
+ // with feedforward off, the command value should be still 1.95 even though feedforward gain
619
+ // is 1.0
620
+ const double target_value = 5.0 ;
621
+ const double expected_command_value = 1.95 ;
622
+
623
+ SetUpController (" test_pid_controller_with_feedforward_gain" );
624
+ ASSERT_EQ (controller_->on_configure (rclcpp_lifecycle::State ()), NODE_SUCCESS);
625
+
626
+ // check on interfaces & pid gain parameters
627
+ for (const auto & dof_name : dof_names_)
628
+ {
629
+ ASSERT_EQ (controller_->params_ .gains .dof_names_map [dof_name].p , 0.5 );
630
+ ASSERT_EQ (controller_->params_ .gains .dof_names_map [dof_name].feedforward_gain , 1.0 );
631
+ }
632
+ ASSERT_EQ (controller_->params_ .command_interface , command_interface_);
633
+ EXPECT_THAT (
634
+ controller_->params_ .reference_and_state_interfaces ,
635
+ testing::ElementsAreArray (state_interfaces_));
636
+ ASSERT_FALSE (controller_->params_ .use_external_measured_states );
637
+
638
+ // setup executor
639
+ rclcpp::executors::MultiThreadedExecutor executor;
640
+ executor.add_node (controller_->get_node ()->get_node_base_interface ());
641
+ executor.add_node (service_caller_node_->get_node_base_interface ());
642
+
643
+ controller_->set_chained_mode (true );
644
+
645
+ // activate controller
646
+ ASSERT_EQ (controller_->on_activate (rclcpp_lifecycle::State ()), NODE_SUCCESS);
647
+ ASSERT_TRUE (controller_->is_in_chained_mode ());
648
+
649
+ // feedforward by default is OFF
650
+ ASSERT_EQ (*(controller_->control_mode_ .readFromRT ()), feedforward_mode_type::OFF);
651
+
652
+ // send a message to update reference interface
653
+ std::shared_ptr<ControllerCommandMsg> msg = std::make_shared<ControllerCommandMsg>();
654
+ msg->dof_names = controller_->params_ .dof_names ;
655
+ msg->values .resize (msg->dof_names .size (), 0.0 );
656
+ for (size_t i = 0 ; i < msg->dof_names .size (); ++i)
657
+ {
658
+ msg->values [i] = target_value;
659
+ }
660
+ msg->values_dot .resize (msg->dof_names .size (), std::numeric_limits<double >::quiet_NaN ());
661
+ controller_->input_ref_ .writeFromNonRT (msg);
662
+ ASSERT_EQ (
663
+ controller_->update_reference_from_subscribers (
664
+ rclcpp::Time (0 ), rclcpp::Duration::from_seconds (0.01 )),
665
+ controller_interface::return_type::OK);
666
+
667
+ // run update
668
+ ASSERT_EQ (
669
+ controller_->update (rclcpp::Time (0 ), rclcpp::Duration::from_seconds (0.01 )),
670
+ controller_interface::return_type::OK);
671
+
672
+ // check on result from update
673
+ ASSERT_EQ (controller_->command_interfaces_ [0 ].get_value (), expected_command_value);
674
+ }
675
+
545
676
int main (int argc, char ** argv)
546
677
{
547
678
::testing::InitGoogleTest (&argc, argv);
0 commit comments