From c83cfb38b228c47fab4bf937b4cb5517c6fe4c81 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 12 Feb 2025 20:11:36 +0100 Subject: [PATCH 1/4] Fix use of M_PI in steering_controllers_library and tricycle_controller (#1536) --- steering_controllers_library/src/steering_odometry.cpp | 2 ++ steering_controllers_library/test/test_steering_odometry.cpp | 2 ++ tricycle_controller/src/tricycle_controller.cpp | 2 ++ 3 files changed, 6 insertions(+) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index b2bf100255..b8cf847033 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -17,6 +17,8 @@ * Author: Dr. Ing. Denis Stogl */ +#define _USE_MATH_DEFINES + #include "steering_controllers_library/steering_odometry.hpp" #include diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3d6d97f15f..a211ac8bf4 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#define _USE_MATH_DEFINES + #include #include "steering_controllers_library/steering_odometry.hpp" diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 9630d88b44..716583021f 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -16,6 +16,8 @@ * Author: Tony Najjar */ +#define _USE_MATH_DEFINES + #include #include #include From 6d681ef4dbe6027f1315c909b10e2ef8231df87d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 13 Feb 2025 12:16:10 +0100 Subject: [PATCH 2/4] Cleanup wrong lifecycle transitions in tests and unnecessary checks (#1534) --- .../diff_drive_controller.hpp | 2 -- .../src/diff_drive_controller.cpp | 26 +------------- .../test/test_diff_drive_controller.cpp | 11 ++++-- .../src/forward_command_controller.cpp | 5 --- ...i_interface_forward_command_controller.cpp | 5 --- .../src/gpio_command_controller.cpp | 5 --- .../gripper_action_controller_impl.hpp | 5 --- .../src/joint_state_broadcaster.cpp | 5 --- .../test/test_joint_state_broadcaster.cpp | 17 --------- .../src/joint_trajectory_controller.cpp | 10 ------ .../test/test_trajectory_actions.cpp | 3 -- .../test/test_trajectory_controller.cpp | 36 +------------------ ...arallel_gripper_action_controller_impl.hpp | 5 --- .../test/test_parallel_gripper_controller.cpp | 10 ------ .../tricycle_controller.hpp | 2 -- .../src/tricycle_controller.cpp | 13 +------ .../test/test_tricycle_controller.cpp | 11 +++--- 17 files changed, 17 insertions(+), 154 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 11ecd9337b..665203c1c9 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -141,8 +141,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED}; - bool is_halted = false; - bool reset(); void halt(); diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 3156d0d80f..8eb5ba837e 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -101,15 +101,6 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto logger = get_node()->get_logger(); - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { - halt(); - is_halted = true; - } - return controller_interface::return_type::OK; - } const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); @@ -149,15 +140,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = get_node()->get_logger(); - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { - halt(); - is_halted = true; - } - return controller_interface::return_type::OK; - } // command may be limited further by SpeedLimit, // without affecting the stored twist command @@ -562,7 +544,6 @@ controller_interface::CallbackReturn DiffDriveController::on_activate( return controller_interface::CallbackReturn::ERROR; } - is_halted = false; subscriber_is_active_ = true; RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); @@ -573,11 +554,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate( const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; - if (!is_halted) - { - halt(); - is_halted = true; - } + halt(); reset_buffers(); registered_left_wheel_handles_.clear(); registered_right_wheel_handles_.clear(); @@ -616,7 +593,6 @@ bool DiffDriveController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - is_halted = false; return true; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 25cf2405f2..25c3add506 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -719,11 +719,16 @@ TEST_F(TestDiffDriveController, cleanup) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // should be moving + EXPECT_LT(0.0, left_wheel_vel_cmd_.get_value()); + EXPECT_LT(0.0, right_wheel_vel_cmd_.get_value()); + state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + + // should be stopped + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()"; state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 78fe8c9191..bc0a2589f5 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -31,11 +31,6 @@ void ForwardCommandController::declare_parameters() controller_interface::CallbackReturn ForwardCommandController::read_parameters() { - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); if (params_.joints.empty()) diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 06fe395cb4..692cc2ce0a 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -32,11 +32,6 @@ void MultiInterfaceForwardCommandController::declare_parameters() controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() { - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); if (params_.joint.empty()) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index a509eae774..fdecf71c30 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -158,11 +158,6 @@ controller_interface::return_type GpioCommandController::update( bool GpioCommandController::update_dynamic_map_parameters() { auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(logger, "Error encountered during init"); - return false; - } // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); // get parameters from the listener in case they were updated diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 2c115091af..034f7e43dd 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -206,11 +206,6 @@ controller_interface::CallbackReturn GripperActionController: const rclcpp_lifecycle::State &) { const auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); // Action status checking update rate diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5b72b9f35a..5a9d1caeef 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -90,11 +90,6 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf controller_interface::CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); if (use_all_available_interfaces()) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3ec7ff4ac2..370965e99e 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -149,23 +149,6 @@ void JointStateBroadcasterTest::assign_state_interfaces( state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } -TEST_F(JointStateBroadcasterTest, ConfigureErrorTest) -{ - // publishers not initialized yet - ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); - ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); - - // configure failed - ASSERT_THROW(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), std::exception); - - SetUpStateBroadcaster(); - // check state remains unchanged - - // publishers still not initialized - ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); - ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); -} - TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) { // publishers not initialized yet diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bbe9afde75..550ab9b476 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -144,10 +144,6 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - return controller_interface::return_type::OK; - } auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) @@ -655,12 +651,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(logger, "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } - // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 6f846703f9..894d887a22 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -1020,9 +1020,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, deactivate_controller_aborts_acti auto state_ref = traj_controller_->get_state_reference(); auto state = traj_controller_->get_state_feedback(); - // run an update - updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - // There will be no active trajectory upon deactivation, so we can't use the expectCommandPoint // method. if (traj_controller_->has_position_command_interface()) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21d0277541..34f44e2d8c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -46,39 +46,6 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description) SetUpTrajectoryControllerLocal({}, "")); } -TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) -{ - rclcpp::executors::MultiThreadedExecutor executor; - SetUpTrajectoryController(executor); - traj_controller_->get_node()->set_parameter( - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - - const auto state = traj_controller_->configure(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - - // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; - // *INDENT-OFF* - std::vector> points{ - {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; - // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); - - traj_controller_->update( - rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); - - // hw position == 0 because controller is not activated - EXPECT_EQ(0.0, joint_pos_[0]); - EXPECT_EQ(0.0, joint_pos_[1]); - EXPECT_EQ(0.0, joint_pos_[2]); - - executor.cancel(); -} - TEST_P(TrajectoryControllerTestParameterized, check_interface_names) { rclcpp::executors::MultiThreadedExecutor executor; @@ -148,7 +115,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) auto state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -223,8 +189,8 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param std::vector deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + // it should be holding the current point - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); expectHoldingPointDeactivated(deactivated_positions); // reactivate diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index d37cb87fc8..5b7e760ca2 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -226,11 +226,6 @@ controller_interface::CallbackReturn GripperActionController::on_configure( const rclcpp_lifecycle::State &) { const auto logger = get_node()->get_logger(); - if (!param_listener_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); - return controller_interface::CallbackReturn::ERROR; - } params_ = param_listener_->get_params(); // Action status checking update rate diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index 417d87d40f..23c13b157a 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -67,11 +67,6 @@ TEST_F(GripperControllerTest, ParametersNotSet) this->SetUpController( "test_gripper_action_position_controller_no_parameters", controller_interface::return_type::ERROR); - - // configure failed, 'joints' parameter not set - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); } TEST_F(GripperControllerTest, JointParameterIsEmpty) @@ -79,11 +74,6 @@ TEST_F(GripperControllerTest, JointParameterIsEmpty) this->SetUpController( "test_gripper_action_position_controller_empty_joint", controller_interface::return_type::ERROR); - - // configure failed, 'joints' is empty - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); } TEST_F(GripperControllerTest, ConfigureParamsSuccess) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 3a1c996b78..60590012b9 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -135,8 +135,6 @@ class TricycleController : public controller_interface::ControllerInterface TractionLimiter limiter_traction_; SteeringLimiter limiter_steering_; - bool is_halted = false; - void reset_odometry( const std::shared_ptr request_header, const std::shared_ptr req, diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 716583021f..02f5449b4c 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -88,16 +88,6 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const controller_interface::return_type TricycleController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) - { - if (!is_halted) - { - halt(); - is_halted = true; - } - return controller_interface::return_type::OK; - } - // if the mutex is unable to lock, last_command_msg_ won't be updated received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) { last_command_msg_ = msg; }); @@ -382,7 +372,6 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - is_halted = false; subscriber_is_active_ = true; RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); @@ -392,6 +381,7 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; + halt(); return CallbackReturn::SUCCESS; } @@ -438,7 +428,6 @@ bool TricycleController::reset() velocity_command_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); - is_halted = false; return true; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 5e868ebeea..d8d4f2cf63 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -282,9 +282,10 @@ TEST_F(TestTricycleController, cleanup) state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ASSERT_EQ( - controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + + // should be stopped + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -333,11 +334,11 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) // deactivated // wait so controller process the second point when deactivated std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = controller_->get_node()->deactivate(); - ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; From 0d4b7e8f4867d0771aba36651e87facabac1dfde Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Feb 2025 16:57:28 +0100 Subject: [PATCH 3/4] Fix the exported interface naming in the chainable controllers (#1528) --- .../test_ackermann_steering_controller.cpp | 15 ++++++----- .../test_ackermann_steering_controller.hpp | 3 +-- ...kermann_steering_controller_preceeding.cpp | 15 ++++++----- .../src/admittance_controller.cpp | 4 +-- .../test/test_admittance_controller.cpp | 21 ++++++++++++++++ .../test/test_bicycle_steering_controller.cpp | 15 ++++++----- .../test/test_bicycle_steering_controller.hpp | 3 +-- ...bicycle_steering_controller_preceeding.cpp | 15 ++++++----- .../src/diff_drive_controller.cpp | 4 +-- .../test/test_diff_drive_controller.cpp | 21 ++++++++-------- .../src/force_torque_sensor_broadcaster.cpp | 25 +++++++++++++------ .../test_force_torque_sensor_broadcaster.cpp | 19 ++++++++++++++ .../src/mecanum_drive_controller.cpp | 6 ++--- .../test/test_mecanum_drive_controller.cpp | 12 +++++---- .../test/test_mecanum_drive_controller.hpp | 3 +-- pid_controller/src/pid_controller.cpp | 6 +++-- pid_controller/test/test_pid_controller.cpp | 25 +++++++++++++++++-- .../test/test_pid_controller_preceding.cpp | 11 +++++--- .../src/steering_controllers_library.cpp | 4 +-- .../test_steering_controllers_library.cpp | 15 ++++++----- .../test_steering_controllers_library.hpp | 3 +-- .../test_tricycle_steering_controller.cpp | 19 ++++++++------ .../test_tricycle_steering_controller.hpp | 2 +- ...ricycle_steering_controller_preceeding.cpp | 18 +++++++------ 24 files changed, 189 insertions(+), 95 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c434f73467..15eb39f1a4 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_ackermann_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_ackermann_steering_controller.hpp" + class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture { @@ -87,11 +88,13 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 2e7d688471..4657ee2c95 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -295,8 +295,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; - std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 4f67beaa07..7a0e4d430e 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_ackermann_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_ackermann_steering_controller.hpp" + class AckermannSteeringControllerTest : public AckermannSteeringControllerFixture { @@ -89,11 +90,13 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 7bcd770a52..1ef76d1f61 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -147,9 +147,9 @@ AdmittanceController::on_export_reference_interfaces() { velocity_reference_.emplace_back(reference_interfaces_[index]); } - const auto full_name = joint + "/" + interface; + const auto exported_prefix = std::string(get_node()->get_name()) + "/" + joint; chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( - std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + exported_prefix, interface, reference_interfaces_.data() + index)); index++; } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 5c4bbe9438..f6e28d599e 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -168,6 +168,27 @@ TEST_F(AdmittanceControllerTest, check_interfaces) ASSERT_EQ( controller_->state_interfaces_.size(), state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); + + const auto reference_interfaces = controller_->ordered_exported_reference_interfaces_; + ASSERT_EQ(reference_interfaces.size(), 2 * joint_names_.size()); + for (auto i = 0ul; i < joint_names_.size(); i++) + { + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_names_[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_POSITION); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_POSITION); + EXPECT_EQ( + reference_interfaces[i + joint_names_.size()]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i + joint_names_.size()]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ( + reference_interfaces[i + joint_names_.size()]->get_interface_name(), + hardware_interface::HW_IF_VELOCITY); + } } TEST_F(AdmittanceControllerTest, activate_success) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 7ec0983f93..9c41f5fd47 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_bicycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_bicycle_steering_controller.hpp" + class BicycleSteeringControllerTest : public BicycleSteeringControllerFixture { @@ -71,11 +72,13 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 25fec7fdf1..b6e5d616a7 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -263,8 +263,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {{3.3, 0.5}}; std::array joint_command_values_ = {{1.1, 2.2}}; - std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 0a321009c2..500bb36c1c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_bicycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_bicycle_steering_controller.hpp" + class BicycleSteeringControllerTest : public BicycleSteeringControllerFixture { @@ -75,11 +76,13 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 8eb5ba837e..537e32d3e1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -700,11 +700,11 @@ DiffDriveController::on_export_reference_interfaces() reference_interfaces.reserve(reference_interfaces_.size()); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 25c3add506..80fc1bf3b8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -976,23 +976,22 @@ TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported) ASSERT_EQ(reference_interfaces.size(), 2) << "Expected exactly 2 reference interfaces: linear and angular"; - const std::string expected_prefix_name = std::string(controller_->get_node()->get_name()); - const std::string expected_linear_interface_name = - std::string("linear/") + hardware_interface::HW_IF_VELOCITY; - const std::string expected_angular_interface_name = - std::string("angular/") + hardware_interface::HW_IF_VELOCITY; + const std::string expected_linear_prefix_name = + std::string(controller_->get_node()->get_name()) + std::string("/linear"); + const std::string expected_angular_prefix_name = + std::string(controller_->get_node()->get_name()) + std::string("/angular"); const std::string expected_linear_name = - expected_prefix_name + std::string("/") + expected_linear_interface_name; + expected_linear_prefix_name + std::string("/") + hardware_interface::HW_IF_VELOCITY; const std::string expected_angular_name = - expected_prefix_name + std::string("/") + expected_angular_interface_name; + expected_angular_prefix_name + std::string("/") + hardware_interface::HW_IF_VELOCITY; ASSERT_EQ(reference_interfaces[0]->get_name(), expected_linear_name); ASSERT_EQ(reference_interfaces[1]->get_name(), expected_angular_name); - EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_prefix_name); - EXPECT_EQ(reference_interfaces[0]->get_interface_name(), expected_linear_interface_name); - EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_prefix_name); - EXPECT_EQ(reference_interfaces[1]->get_interface_name(), expected_angular_interface_name); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_linear_prefix_name); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_angular_prefix_name); + EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } // Make sure that the controller is properly reset when deactivated diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index ae105a511c..5b4ac3368c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -176,6 +176,7 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() std::vector torque_names( {params_.interface_names.torque.x, params_.interface_names.torque.y, params_.interface_names.torque.z}); + std::string export_prefix = get_node()->get_name(); if (!params_.sensor_name.empty()) { const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names(); @@ -183,37 +184,47 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin()); std::copy( semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin()); + + // Update the prefix and get the proper force and torque names + export_prefix = export_prefix + "/" + params_.sensor_name; + // strip "/" and get the second part of the information + // e.g. /ft_sensor/force.x -> force.x + std::for_each( + force_names.begin(), force_names.end(), + [](std::string & name) { name = name.substr(name.find_last_of("/") + 1); }); + std::for_each( + torque_names.begin(), torque_names.end(), + [](std::string & name) { name = name.substr(name.find_last_of("/") + 1); }); } - const std::string controller_name = get_node()->get_name(); if (!force_names[0].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + export_prefix, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); } if (!force_names[1].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + export_prefix, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); } if (!force_names[2].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + export_prefix, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); } if (!torque_names[0].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + export_prefix, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); } if (!torque_names[1].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + export_prefix, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); } if (!torque_names[2].empty()) { exported_state_interfaces.emplace_back(hardware_interface::StateInterface( - controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + export_prefix, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); } return exported_state_interfaces; } diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 26f959e88f..46c396f7b1 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -324,8 +324,16 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); ASSERT_EQ( exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "torque.z"); for (size_t i = 0; i < 6; ++i) { + ASSERT_EQ( + exported_state_interfaces[i]->get_prefix_name(), controller_name + "/" + sensor_name_); ASSERT_EQ( exported_state_interfaces[i]->get_value(), sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); @@ -361,6 +369,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) const std::string controller_name = fts_broadcaster_->get_node()->get_name(); ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name); + ASSERT_EQ(exported_state_interfaces[1]->get_prefix_name(), controller_name); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/torque.z"); ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); } @@ -402,8 +414,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x"); ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y"); ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "fts_sensor/torque.z"); for (size_t i = 0; i < 6; ++i) { + ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name); ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); } } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 2fd5fdbdcb..93e607bd08 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -279,13 +279,13 @@ MecanumDriveController::on_export_reference_interfaces() reference_interfaces.reserve(reference_interfaces_.size()); - std::vector reference_interface_names = { - "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + std::vector reference_interface_names = {"/linear/x", "/linear/y", "/angular/z"}; for (size_t i = 0; i < reference_interfaces_.size(); ++i) { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); + get_node()->get_name() + reference_interface_names[i], hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[i])); } return reference_interfaces; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index bbddcec06a..079a431e03 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -108,11 +108,13 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex for (size_t i = 0; i < reference_interface_names.size(); ++i) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + - std::string("/") + reference_interface_names[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[i]); + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 5f7cf36dc7..766c3ec554 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -269,8 +269,7 @@ class MecanumDriveControllerFixture : public ::testing::Test } protected: - std::vector reference_interface_names = { - "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + std::vector reference_interface_names = {"linear/x", "linear/y", "angular/z"}; static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 521227b49c..2816d5a5ec 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -392,7 +392,8 @@ std::vector PidController::on_export_refer for (const auto & dof_name : reference_and_state_dof_names_) { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); + std::string(get_node()->get_name()) + "/" + dof_name, interface, + &reference_interfaces_[index])); ++index; } } @@ -414,7 +415,8 @@ std::vector PidController::on_export_state_i for (const auto & dof_name : reference_and_state_dof_names_) { state_interfaces.push_back(hardware_interface::StateInterface( - get_node()->get_name(), dof_name + "/" + interface, &state_interfaces_values_[index])); + std::string(get_node()->get_name()) + "/" + dof_name, interface, + &state_interfaces_values_[index])); ++index; } } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 60d484e463..a8a703e8a4 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -98,11 +98,32 @@ TEST_F(PidControllerTest, check_exported_interfaces) const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ( + ref_if_conf[ri_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), interface); ++ri_index; } } + + // check exported state interfaces + auto state_ifs = controller_->export_state_interfaces(); + ASSERT_EQ(state_ifs.size(), dof_state_values_.size()); + size_t s_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string state_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(state_ifs[s_index]->get_name(), state_itf_name); + EXPECT_EQ( + state_ifs[s_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(state_ifs[s_index]->get_interface_name(), interface); + ++s_index; + } + } } TEST_F(PidControllerTest, activate_success) diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index ee5fe46754..602303ef9e 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -85,8 +85,10 @@ TEST_F(PidControllerTest, check_exported_interfaces) const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ( + ref_if_conf[ri_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), interface); ++ri_index; } } @@ -103,8 +105,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; EXPECT_EQ(exported_state_itfs[esi_index]->get_name(), state_itf_name); EXPECT_EQ( - exported_state_itfs[esi_index]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), dof_name + "/" + interface); + exported_state_itfs[esi_index]->get_prefix_name(), + std::string(controller_->get_node()->get_name()) + "/" + dof_name); + EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), interface); ++esi_index; } } diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index a91cb87105..ed0346cbbc 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -317,11 +317,11 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.reserve(nr_ref_itfs_); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 3378efbef8..10a0f00376 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_steering_controllers_library.hpp" - #include #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_steering_controllers_library.hpp" + class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture { @@ -68,11 +69,13 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 20d136ab32..52bd69744f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -317,8 +317,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; - std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 8e29314f8e..93c92c865f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_tricycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_tricycle_steering_controller.hpp" + class TricycleSteeringControllerTest : public TricycleSteeringControllerFixture { @@ -75,15 +76,17 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index cfecf96cc8..4a914120ef 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -280,7 +280,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_{{0.5, 0.5, 0.0}}; std::array joint_command_values_{{1.1, 3.3, 2.2}}; - std::array joint_reference_interfaces_{{"linear/velocity", "angular/velocity"}}; + std::array joint_reference_interfaces_{{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 566169e34e..f9b9afb67c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_tricycle_steering_controller.hpp" - #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_tricycle_steering_controller.hpp" class TricycleSteeringControllerTest : public TricycleSteeringControllerFixture { @@ -78,15 +78,17 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } From cf8b96ede01a88e78ef7e7a79eb1d9ff2b723456 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Thu, 13 Feb 2025 09:05:40 -0800 Subject: [PATCH 4/4] [pid_controller] Fix logic for feedforward_mode with single reference interface (#1520) --- pid_controller/src/pid_controller.cpp | 23 +-- .../test/pid_controller_params.yaml | 13 ++ pid_controller/test/test_pid_controller.cpp | 131 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 2 + 4 files changed, 161 insertions(+), 8 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 2816d5a5ec..dd4d6df185 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -500,20 +500,27 @@ controller_interface::return_type PidController::update_and_write_commands( for (size_t i = 0; i < dof_; ++i) { - double tmp_command = std::numeric_limits::quiet_NaN(); + double tmp_command = 0.0; - // Using feedforward if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) { // calculate feed-forward if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) { - tmp_command = reference_interfaces_[dof_ + i] * - params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; - } - else - { - tmp_command = 0.0; + // two interfaces + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + if (std::isfinite(reference_interfaces_[dof_ + i])) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + } + else // one interface + { + tmp_command = reference_interfaces_[i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } } double error = reference_interfaces_[i] - measured_state_values_[i]; diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 7555cfc156..cf0a3f4173 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -9,3 +9,16 @@ test_pid_controller: gains: joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} + + +test_pid_controller_with_feedforward_gain: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a8a703e8a4..67ac680eae 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -542,6 +542,137 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) } } +/** + * @brief check chained pid controller with feedforward and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward gain 1.0, the command value should be 1.95 + 1.0 * 5.0 = 6.95 + const double target_value = 5.0; + const double expected_command_value = 6.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // turn on feedforward + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // send a message to update reference interface + std::shared_ptr msg = std::make_shared(); + msg->dof_names = controller_->params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); +} + +/** + * @brief check chained pid controller with feedforward OFF and gain as non-zero, single interface + */ +TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) +{ + // state interface value is 1.1 as defined in test fixture + // given target value 5.0 + // with p gain 0.5, the command value should be 0.5 * (5.0 - 1.1) = 1.95 + // with feedforward off, the command value should be still 1.95 even though feedforward gain + // is 1.0 + const double target_value = 5.0; + const double expected_command_value = 1.95; + + SetUpController("test_pid_controller_with_feedforward_gain"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check on interfaces & pid gain parameters + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 0.5); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 1.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + // setup executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + controller_->set_chained_mode(true); + + // activate controller + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // feedforward by default is OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // send a message to update reference interface + std::shared_ptr msg = std::make_shared(); + msg->dof_names = controller_->params_.dof_names; + msg->values.resize(msg->dof_names.size(), 0.0); + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + msg->values[i] = target_value; + } + msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + ASSERT_EQ( + controller_->update_reference_from_subscribers( + rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // run update + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check on result from update + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 4471f35a12..dd5ae029b9 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -59,6 +59,8 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on); FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_with_gain); + FRIEND_TEST(PidControllerTest, test_update_chained_feedforward_off_with_gain); public: controller_interface::CallbackReturn on_configure(