From 35556854883b546143d7773c43053650a8b23840 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 30 Nov 2023 16:26:33 +0100 Subject: [PATCH 01/24] added documentation on common controller parameters (#855) --- doc/controllers_index.rst | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 9fceb90fd3..5f8780d961 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -72,3 +72,12 @@ In the sense of ros2_control, broadcasters are still controllers using the same IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + + +Common Controller Parameters +**************************** + +Every controller and broadcaster has a few common parameters. They are optional, but if needed they have to be set before ``onConfigure`` transition to ``inactive`` state, see `lifecycle documents `__. Once the controllers are already loaded, this transition is done using the service ``configure_controller`` of the controller_manager. + +* ``update_rate``: An unsigned integer parameter representing the rate at which every controller/broadcaster runs its update cycle. When unspecified, they run at the same frequency as the controller_manager. +* ``is_async``: A boolean parameter that is needed to specify if the controller update needs to run asynchronously. From fcc0847958c17a942e2b233d3bea1ab2d2ac05dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 30 Nov 2023 17:36:36 +0100 Subject: [PATCH 02/24] [JTC] Activate checks for parameter validation (#857) --- .../test/test_trajectory_controller.cpp | 157 +++++++++--------- .../test/test_trajectory_controller_utils.hpp | 22 ++- 2 files changed, 91 insertions(+), 88 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7c026d5e7a..0b6651e79f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -122,8 +122,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - traj_controller_->get_node()->configure(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_interface_config = traj_controller_->command_interface_configuration(); ASSERT_EQ( @@ -133,8 +133,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) ASSERT_EQ( state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); - ActivateTrajectoryController(); - ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE); + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); executor.cancel(); } @@ -194,13 +194,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); // This call is replacing the way parameters are set via launch - traj_controller_->configure(); - auto state = traj_controller_->get_state(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - ActivateTrajectoryController(); - - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); @@ -245,8 +242,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait so controller would have processed the third point when reactivated -> but it shouldn't std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - ActivateTrajectoryController(false, deactivated_positions); - state = traj_controller_->get_state(); + state = ActivateTrajectoryController(false, deactivated_positions); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // it should still be holding the position at time of deactivation @@ -1918,72 +1914,73 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"effort"}), std::vector({"position", "velocity", "acceleration"})))); -// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` -// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) -// { -// auto set_parameter_and_check_result = [&]() -// { -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// SetParameters(); // This call is replacing the way parameters are set via launch -// traj_controller_->get_node()->configure(); -// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); -// }; -// -// SetUpTrajectoryController(false); -// -// // command interfaces: empty -// command_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // command interfaces: bad_name -// command_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // command interfaces: effort has to be only -// command_interface_types_ = {"effort", "position"}; -// set_parameter_and_check_result(); -// -// // command interfaces: velocity - position not present -// command_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // command interfaces: acceleration without position and velocity -// command_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: empty -// state_interface_types_ = {}; -// set_parameter_and_check_result(); -// -// // state interfaces: cannot not be effort -// state_interface_types_ = {"effort"}; -// set_parameter_and_check_result(); -// -// // state interfaces: bad name -// state_interface_types_ = {"bad_name"}; -// set_parameter_and_check_result(); -// -// // state interfaces: velocity - position not present -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity", "acceleration"}; -// set_parameter_and_check_result(); -// -// // state interfaces: acceleration without position and velocity -// state_interface_types_ = {"acceleration"}; -// set_parameter_and_check_result(); -// -// // velocity-only command interface: position - velocity not present -// command_interface_types_ = {"velocity"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// -// // effort-only command interface: position - velocity not present -// command_interface_types_ = {"effort"}; -// state_interface_types_ = {"position"}; -// set_parameter_and_check_result(); -// state_interface_types_ = {"velocity"}; -// set_parameter_and_check_result(); -// } +/** + * @brief see if parameter validation is correct + * + * Note: generate_parameter_library validates parameters itself during on_init() method, but + * combinations of parameters are checked from JTC during on_configure() + */ +TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +{ + // command interfaces: empty + command_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + auto state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + + // command interfaces: bad_name + command_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: effort has to be only + command_interface_types_ = {"effort", "position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: velocity - position not present + command_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // command interfaces: acceleration without position and velocity + command_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: empty + state_interface_types_ = {}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: cannot not be effort + state_interface_types_ = {"effort"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: bad name + state_interface_types_ = {"bad_name"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: velocity - position not present + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + state_interface_types_ = {"velocity", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // state interfaces: acceleration without position and velocity + state_interface_types_ = {"acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 3c8e252067..b530517517 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -188,6 +188,17 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}) + { + auto ret = SetUpTrajectoryControllerLocal(parameters); + if (ret != controller_interface::return_type::OK) + { + FAIL(); + } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + } + + controller_interface::return_type SetUpTrajectoryControllerLocal( + const std::vector & parameters = {}) { traj_controller_ = std::make_shared(); @@ -200,12 +211,7 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); - auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options); - if (ret != controller_interface::return_type::OK) - { - FAIL(); - } - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + return traj_controller_->init(controller_name_, "", 0, "", node_options); } void SetPidParameters( @@ -262,7 +268,7 @@ class TrajectoryControllerTest : public ::testing::Test initial_eff_joints); } - void ActivateTrajectoryController( + rclcpp_lifecycle::State ActivateTrajectoryController( bool separate_cmd_and_state_values = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, @@ -320,7 +326,7 @@ class TrajectoryControllerTest : public ::testing::Test } traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); - traj_controller_->get_node()->activate(); + return traj_controller_->get_node()->activate(); } static void TearDownTestCase() { rclcpp::shutdown(); } From 62ce4879e45fdec2f462f14a695cf579afbfe866 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Dec 2023 14:39:06 +0100 Subject: [PATCH 03/24] [JTC] Remove start_with_holding option (#839) --- .../src/joint_trajectory_controller.cpp | 7 ++---- ...oint_trajectory_controller_parameters.yaml | 5 ---- .../test/test_trajectory_controller.cpp | 23 ++----------------- 3 files changed, 4 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index dae7f13148..d24fd5a34a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -944,11 +944,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(last_commanded_state_); } - // Should the controller start by holding position at the beginning of active state? - if (params_.start_with_holding) - { - add_new_trajectory_msg(set_hold_position()); - } + // The controller should start by holding position at the beginning of active state + add_new_trajectory_msg(set_hold_position()); rt_is_holding_.writeFromNonRT(true); // parse timeout parameter diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 87ca1daea5..4981489d36 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -73,11 +73,6 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } - start_with_holding: { - type: bool, - default_value: true, - description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", - } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: false, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b6651e79f..0b049000e3 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -444,24 +444,6 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) executor.cancel(); } -/** - * @brief check if hold on startup is deactivated - */ -TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) -{ - rclcpp::executors::MultiThreadedExecutor executor; - - rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); - - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup without start_with_holding being set, we expect no active trajectory - ASSERT_FALSE(traj_controller_->has_active_traj()); - - executor.cancel(); -} - /** * @brief check if hold on startup */ @@ -469,12 +451,11 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) { rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup with start_with_holding being set, we expect an active trajectory: + // after startup, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; From e4f1700ef653268130e25e65e351dc4d5572b54d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Dec 2023 14:40:56 +0100 Subject: [PATCH 04/24] [JTC] Continue with last trajectory-point on success (#842) --- joint_trajectory_controller/doc/userdoc.rst | 4 +- .../joint_trajectory_controller.hpp | 12 +- .../src/joint_trajectory_controller.cpp | 16 ++- .../test/test_trajectory_actions.cpp | 136 +++++++++++++----- .../test/test_trajectory_controller.cpp | 14 +- .../test/test_trajectory_controller_utils.hpp | 56 +++----- 6 files changed, 162 insertions(+), 76 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 58ba734b45..6a34185437 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -150,12 +150,14 @@ Actions [#f1]_ /follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] Action server for commanding the controller - The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. + Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. +The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances. + .. _Subscriber: Subscriber [#f1]_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 397ccf347c..d16e4f8267 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -174,7 +174,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Timeout to consider commands old double cmd_timeout_; - // Are we holding position? + // True if holding position or repeating last trajectory point in case of success realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; @@ -244,9 +244,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); + + /** @brief set the current position with zero velocity and acceleration as new command + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr set_hold_position(); + /** @brief set last trajectory point to be repeated at success + * + * no matter if it has nonzero velocity or acceleration + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + std::shared_ptr set_success_trajectory_point(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d24fd5a34a..9306b07354 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -362,7 +362,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -1466,6 +1466,20 @@ JointTrajectoryController::set_hold_position() return hold_position_msg_ptr_; } +std::shared_ptr +JointTrajectoryController::set_success_trajectory_point() +{ + // set last command to be repeated at success, no matter if it has nonzero velocity or + // acceleration + hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); + + // set flag, otherwise tolerances will be checked with success_trajectory_point too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; +} + bool JointTrajectoryController::contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index ed13a24485..87d557df1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,12 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, double kp = 0.0) + bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); + SetUpAndActivateTrajectoryController( + executor_, parameters, separate_cmd_and_state_values, kp, ff); SetUpActionClient(); @@ -218,7 +219,10 @@ class TestTrajectoryActionsTestParameterized TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; @@ -228,8 +232,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - point.positions = point_positions; points.push_back(point); @@ -247,20 +249,53 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) // note: the action goal also is a trivial trajectory - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(point_positions); - } - else + expectCommandPoint(point_positions); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + std::vector point_velocities{1.0, 1.0, 1.0}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions = point_positions; + point.velocities = point_velocities; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectCommandPoint(point_positions, point_velocities); } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -277,15 +312,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); - point1.positions.resize(joint_names_.size()); - point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; point2.time_from_start = rclcpp::Duration::from_seconds(0.3); - point2.positions.resize(joint_names_.size()); - point2.positions = points_positions.at(1); points.push_back(point2); @@ -302,15 +333,57 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(points_positions.at(1)); - } - else + expectCommandPoint(points_positions.at(1)); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions = points_positions.at(0); + point1.velocities = points_velocities.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions = points_positions.at(1); + point2.velocities = points_velocities.at(1); + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + expectCommandPoint(points_positions.at(1), points_velocities.at(1)); } /** @@ -354,7 +427,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(0)); + expectCommandPoint(points_positions.at(0)); } /** @@ -413,7 +486,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(1)); + expectCommandPoint(points_positions.at(1)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -464,8 +537,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -513,8 +585,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -559,8 +630,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -607,7 +677,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) // it should be holding the last position, // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(cancelled_position); + expectCommandPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0b049000e3..05010c562c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -248,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // it should still be holding the position at time of deactivation // i.e., active but trivial trajectory (one point only) traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - expectHoldingPoint(deactivated_positions); + expectCommandPoint(deactivated_positions); executor.cancel(); } @@ -459,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(initial_positions); executor.cancel(); } @@ -819,12 +819,12 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // should hold last position with zero velocity if (traj_controller_->has_position_command_interface()) { - expectHoldingPoint(points.at(2)); + expectCommandPoint(points.at(2)); } else { // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + expectCommandPoint(INITIAL_POS_JOINTS); } executor.cancel(); @@ -1795,7 +1795,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); } TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) @@ -1827,14 +1827,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point - expectHoldingPoint(hold_position); + expectCommandPoint(hold_position); } // position controllers diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b530517517..4a65b4ad51 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -521,7 +521,8 @@ class TrajectoryControllerTest : public ::testing::Test return state_msg_; } - void expectHoldingPoint(std::vector point) + void expectCommandPoint( + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only) @@ -531,16 +532,16 @@ class TrajectoryControllerTest : public ::testing::Test { if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); + EXPECT_EQ(velocity.at(0), joint_vel_[0]); + EXPECT_EQ(velocity.at(1), joint_vel_[1]); + EXPECT_EQ(velocity.at(2), joint_vel_[2]); } if (traj_controller_->has_acceleration_command_interface()) @@ -557,40 +558,29 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_EQ(0.0, joint_eff_[2]); } } - else + else // traj_controller_->use_closed_loop_pid_adapter() == true { // velocity or effort PID? - // velocity setpoint is always zero -> feedforward term does not have an effect // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", velocity command is " << joint_vel_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", velocity command is " << joint_vel_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", velocity command is " << joint_vel_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + } } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", effort command is " << joint_eff_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", effort command is " << joint_eff_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", effort command is " << joint_eff_[2]; + for (size_t i = 0; i < 3; i++) + { + EXPECT_TRUE(is_same_sign_or_zero( + position.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + } } } } From 757310073cadb20e551c407354f5a40e09e39a2a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 1 Dec 2023 13:45:13 +0000 Subject: [PATCH 05/24] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 8 ++++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 20 files changed, 80 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7a94360e4b..ebd6d5ef43 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index b94892ac83..19fbe5864d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 278921e18b..dbfe611d52 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5f57b7e833..e0166e9dd8 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6c7d7d6fa3..963c8916db 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 17a6d4f280..005b4ba6ac 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 941b849edd..f65fefd016 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f35dcd1a32..3e06cc1830 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4d2d2d01b3..b05b8ce192 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ce89b01912..9f80626bb2 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a4486e4e98..1ad5b44a48 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Continue with last trajectory-point on success (`#842 `_) +* [JTC] Remove start_with_holding option (`#839 `_) +* [JTC] Activate checks for parameter validation (`#857 `_) +* [JTC] Improve update methods for tests (`#858 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 26462d51a9..caf8f3925c 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 71ee9a7119..d15eb127e6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 2c2e5ec358..9ae67679d7 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e8eaf9c382..c3f8302d06 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0c08780904..58694e910d 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 1a76469d6c..b8f6854520 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 6bc3d26f53..9e67804591 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase test coverage of interface configuration getters (`#856 `_) +* Contributors: Christoph Fröhlich + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 0ade9bca61..bd2fc2d678 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index bb7e231222..cb05fb45f1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ * fix tests for API break of passing controller manager update rate in init method (`#854 `_) From 23f944f26e7e21764be3acdb6610cd59866c9bfc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 1 Dec 2023 13:45:35 +0000 Subject: [PATCH 06/24] 4.1.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 42 files changed, 62 insertions(+), 62 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index ebd6d5ef43..a1cbe9596e 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 421f098f96..476c4d3050 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.0.0 + 4.1.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 19fbe5864d..615a457992 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index a5f7e7e353..da5b652bd0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.0.0 + 4.1.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index dbfe611d52..bee3606ec8 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index e3063672d6..c627577ee8 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.0.0 + 4.1.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e0166e9dd8..8ee42d701f 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 56b9552f93..03dffe764d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.0.0 + 4.1.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 963c8916db..f4496d3927 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ef2610d34f..e55e095c57 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 005b4ba6ac..b87aaa2641 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4a1df9ff11..48db0e49b6 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.0.0 + 4.1.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index f65fefd016..b38f021587 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0fe44110b1..c500c1f714 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3e06cc1830..798a0f3e67 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 08b0298e1f..34c7ba663e 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.0.0 + 4.1.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index b05b8ce192..2c311626cf 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 1819b974e2..3ab9c5fff6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.0.0 + 4.1.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9f80626bb2..f2295beca3 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * joint_state_broadcaster: Add proper subscription to TestCustomInterfaceMappingUpdate (`#859 `_) * Contributors: Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 1364b1a164..37ba7b4912 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.0.0 + 4.1.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1ad5b44a48..7e4c25a474 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * [JTC] Continue with last trajectory-point on success (`#842 `_) * [JTC] Remove start_with_holding option (`#839 `_) * [JTC] Activate checks for parameter validation (`#857 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fe10d9583d..b41a7cad0c 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.0.0 + 4.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index caf8f3925c..6f0870c9ae 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 2112f7d8c5..1aec89e59d 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index d15eb127e6..be35427e46 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3ece4bda4b..222d87e3bb 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.0.0 + 4.1.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 9ae67679d7..46f994c792 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6a213308c5..d34a5b1375 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.0.0 + 4.1.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c3f8302d06..a7a2f7a1fd 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 5698930bff..16dd709c25 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.0.0 + 4.1.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index a129b2d0a7..baa01b97c2 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.0.0", + version="4.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 58694e910d..ee80d4b30d 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 41cf5e85d7..24ed2c9d63 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.0.0 + 4.1.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 5a5b979d41..2d855d9255 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.0.0", + version="4.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index b8f6854520..bae8e6e557 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 932fda9a35..5de0ddc872 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.0.0 + 4.1.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 9e67804591..0203298259 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ * Increase test coverage of interface configuration getters (`#856 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index a663dcacde..5f382bf8d2 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.0.0 + 4.1.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index bd2fc2d678..4abf0788fb 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 7f9d14c2fe..f565ef0c02 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.0.0 + 4.1.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cb05fb45f1..91575e6075 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-12-01) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 6ce13e6adf..d7376e8e6b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.0.0 + 4.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 038a54794ef06147e7a4ab5298b8c9eabe65e287 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 4 Dec 2023 20:01:15 +0100 Subject: [PATCH 07/24] =?UTF-8?q?=F0=9F=9A=80=20Add=20PID=20controller=20?= =?UTF-8?q?=F0=9F=8E=89=20(#434)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/controllers_index.rst | 1 + pid_controller/CMakeLists.txt | 108 ++++ pid_controller/README.md | 7 + pid_controller/doc/userdoc.rst | 86 +++ .../include/pid_controller/pid_controller.hpp | 141 +++++ .../pid_controller/visibility_control.h | 49 ++ pid_controller/package.xml | 35 ++ pid_controller/pid_controller.xml | 8 + pid_controller/src/pid_controller.cpp | 521 +++++++++++++++++ pid_controller/src/pid_controller.yaml | 87 +++ .../test/pid_controller_params.yaml | 11 + .../test/pid_controller_preceding_params.yaml | 11 + .../test/test_load_pid_controller.cpp | 43 ++ pid_controller/test/test_pid_controller.cpp | 531 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 285 ++++++++++ .../test/test_pid_controller_preceding.cpp | 103 ++++ ros2_controllers/package.xml | 1 + 17 files changed, 2028 insertions(+) create mode 100644 pid_controller/CMakeLists.txt create mode 100644 pid_controller/README.md create mode 100644 pid_controller/doc/userdoc.rst create mode 100644 pid_controller/include/pid_controller/pid_controller.hpp create mode 100644 pid_controller/include/pid_controller/visibility_control.h create mode 100644 pid_controller/package.xml create mode 100644 pid_controller/pid_controller.xml create mode 100644 pid_controller/src/pid_controller.cpp create mode 100644 pid_controller/src/pid_controller.yaml create mode 100644 pid_controller/test/pid_controller_params.yaml create mode 100644 pid_controller/test/pid_controller_preceding_params.yaml create mode 100644 pid_controller/test/test_load_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.hpp create mode 100644 pid_controller/test/test_pid_controller_preceding.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 5f8780d961..d042fe79dd 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -55,6 +55,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt new file mode 100644 index 0000000000..81cbe6f006 --- /dev/null +++ b/pid_controller/CMakeLists.txt @@ -0,0 +1,108 @@ +cmake_minimum_required(VERSION 3.16) +project(pid_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pid_controller_parameters + src/pid_controller.yaml +) + +add_library(pid_controller SHARED + src/pid_controller.cpp +) +target_compile_features(pid_controller PUBLIC cxx_std_17) +target_include_directories(pid_controller PUBLIC + $ + $ +) +target_link_libraries(pid_controller PUBLIC + pid_controller_parameters +) +ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_pid_controller + test/test_pid_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml + ) + target_include_directories(test_pid_controller PRIVATE include) + target_link_libraries(test_pid_controller pid_controller) + ament_target_dependencies( + test_pid_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_pid_controller_preceding + test/test_pid_controller_preceding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceding_params.yaml + ) + target_include_directories(test_pid_controller_preceding PRIVATE include) + target_link_libraries(test_pid_controller_preceding pid_controller) + ament_target_dependencies( + test_pid_controller_preceding + controller_interface + hardware_interface + ) + + ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) + target_include_directories(test_load_pid_controller PRIVATE include) + ament_target_dependencies( + test_load_pid_controller + controller_manager + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/pid_controller +) + +install(TARGETS + pid_controller + pid_controller_parameters + EXPORT export_pid_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pid_controller/README.md b/pid_controller/README.md new file mode 100644 index 0000000000..01e8fddac8 --- /dev/null +++ b/pid_controller/README.md @@ -0,0 +1,7 @@ +pid_controller +========================================== + +Controller based on PID implementation from control_toolbox package. + +Pluginlib-Library: pid_controller +Plugin: pid_controller/PidController (controller_interface::ControllerInterface) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5570f523fe --- /dev/null +++ b/pid_controller/doc/userdoc.rst @@ -0,0 +1,86 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/doc/userdoc.rst + +.. _pid_controller_userdoc: + +PID Controller +-------------------------------- + +PID Controller implementation that uses PidROS implementation from `control_toolbox `_ package. +The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers. +It also enables to use the first derivative of the reference and its feedback to have second-order PID control. + +Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example: + +- reference/state POSITION; command VELOCITY --> PI CONTROLLER +- reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER + +- reference/state VELOCITY; command POSITION --> PD CONTROLLER +- reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER + +- reference/state POSITION; command POSITION --> PID CONTROLLER +- reference/state VELOCITY; command VELOCITY --> PID CONTROLLER +- reference/state ACCELERATION; command ACCELERATION --> PID CONTROLLER +- reference/state EFFORT; command EFFORT --> PID CONTROLLER + +.. note:: + + Theoretically one can misuse :ref:`Joint Trajectory Controller (JTC)` for the same purpose by sending only one reference point into it. + Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn't do that. + PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware. + +Execution logic of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The controller can be also used in "feed-forward" mode where feed-forward gain is used to increase controllers dynamics. +If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type. +For example a valid combination would be ``position`` and ``velocity`` interface types. + +Using the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Pluginlib-Library: pid_controller +Plugin name: pid_controller/PidController + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + +Commands +,,,,,,,,, +- / [double] + +States +,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + + +Subscribers +,,,,,,,,,,,, +If controller is not in chained mode (``in_chained_mode == false``): + +- /reference [control_msgs/msg/MultiDOFCommand] + +If controller parameter ``use_external_measured_states`` is true: + +- /measured_state [control_msgs/msg/MultiDOFCommand] + +Services +,,,,,,,,,,, + +- /set_feedforward_control [std_srvs/srv/SetBool] + +Publishers +,,,,,,,,,,, +- /controller_state [control_msgs/msg/MultiDOFStateStamped] + +Parameters +,,,,,,,,,,, + +The PID controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/pid_controller.yaml diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp new file mode 100644 index 0000000000..a34dc9f87f --- /dev/null +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -0,0 +1,141 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER__PID_CONTROLLER_HPP_ + +#include +#include +#include + +#include "control_msgs/msg/multi_dof_command.hpp" +#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "control_toolbox/pid_ros.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "pid_controller/visibility_control.h" +#include "pid_controller_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +#include "control_msgs/msg/pid_state.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace pid_controller +{ + +enum class feedforward_mode_type : std::uint8_t +{ + OFF = 0, + ON = 1, +}; + +class PidController : public controller_interface::ChainableControllerInterface +{ +public: + PID_CONTROLLER__VISIBILITY_PUBLIC + PidController(); + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand; + using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; + +protected: + std::shared_ptr param_listener_; + pid_controller::Params params_; + + std::vector reference_and_state_dof_names_; + size_t dof_; + std::vector measured_state_values_; + + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector feedforward_gain_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> measured_state_; + + rclcpp::Service::SharedPtr set_feedforward_control_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + // internal methods + void update_parameters(); + controller_interface::CallbackReturn configure_parameters(); + +private: + // callback for topic interface + PID_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace pid_controller + +#endif // PID_CONTROLLER__PID_CONTROLLER_HPP_ diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h new file mode 100644 index 0000000000..1509364b5a --- /dev/null +++ b/pid_controller/include/pid_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PID_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PID_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define PID_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define PID_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef PID_CONTROLLER__VISIBILITY_BUILDING_DLL +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_EXPORT +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_IMPORT +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define PID_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // PID_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/pid_controller/package.xml b/pid_controller/package.xml new file mode 100644 index 0000000000..7d52121582 --- /dev/null +++ b/pid_controller/package.xml @@ -0,0 +1,35 @@ + + + + pid_controller + 0.0.0 + Controller based on PID implememenation from control_toolbox package. + Bence Magyar + Denis Štogl + Denis Štogl + Apache-2.0 + + ament_cmake + + generate_parameter_library + + angles + control_msgs + control_toolbox + controller_interface + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/pid_controller/pid_controller.xml b/pid_controller/pid_controller.xml new file mode 100644 index 0000000000..5b90741ae6 --- /dev/null +++ b/pid_controller/pid_controller.xml @@ -0,0 +1,8 @@ + + + + PidController ros2_control controller. + + + diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp new file mode 100644 index 0000000000..373e941d96 --- /dev/null +++ b/pid_controller/src/pid_controller.cpp @@ -0,0 +1,521 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "pid_controller/pid_controller.hpp" + +#include +#include +#include +#include + +#include "angles/angles.h" +#include "control_msgs/msg/single_dof_state.hpp" +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); + +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + msg->dof_names = dof_names; + msg->values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg->values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); +} + +void reset_controller_measured_state_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + reset_controller_reference_msg(msg, dof_names); +} + +} // namespace + +namespace pid_controller +{ +PidController::PidController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn PidController::on_init() +{ + control_mode_.initRT(feedforward_mode_type::OFF); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::update_parameters() +{ + if (!param_listener_->is_old(params_)) + { + return; + } + params_ = param_listener_->get_params(); +} + +controller_interface::CallbackReturn PidController::configure_parameters() +{ + update_parameters(); + + if (!params_.reference_and_state_dof_names.empty()) + { + reference_and_state_dof_names_ = params_.reference_and_state_dof_names; + } + else + { + reference_and_state_dof_names_ = params_.dof_names; + } + + if (params_.dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'dof_names' (%zu) and 'reference_and_state_dof_names' (%zu) parameters has to be " + "the same!", + params_.dof_names.size(), reference_and_state_dof_names_.size()); + return CallbackReturn::FAILURE; + } + + dof_ = params_.dof_names.size(); + + // TODO(destogl): is this even possible? Test it... + if (params_.gains.dof_names_map.size() != dof_) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'gains' (%zu) map and number or 'dof_names' (%zu) have to be the same!", + params_.gains.dof_names_map.size(), dof_); + return CallbackReturn::FAILURE; + } + + pids_.resize(dof_); + + 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); + if (!pids_[i]->initPid()) + { + return CallbackReturn::FAILURE; + } + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reference_and_state_dof_names_.clear(); + pids_.clear(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto ret = configure_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&PidController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + input_ref_.writeFromNonRT(msg); + + // input state Subscriber and callback + if (params_.use_external_measured_states) + { + auto measured_state_callback = + [&](const std::shared_ptr msg) -> void + { + // TODO(destogl): Sort the input values based on joint and interface names + measured_state_.writeFromNonRT(msg); + }; + measured_state_subscriber_ = get_node()->create_subscription( + "~/measured_state", subscribers_qos, measured_state_callback); + } + std::shared_ptr measured_state_msg = + std::make_shared(); + reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); + measured_state_.writeFromNonRT(measured_state_msg); + + measured_state_values_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + auto set_feedforward_control_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->data) + { + control_mode_.writeFromNonRT(feedforward_mode_type::ON); + } + else + { + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + } + response->success = true; + }; + + set_feedforward_control_service_ = get_node()->create_service( + "~/set_feedforward_control", set_feedforward_control_callback, qos_services); + + try + { + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Reserve memory in state publisher + state_publisher_->lock(); + state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); + for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) + { + state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; + } + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::reference_callback(const std::shared_ptr msg) +{ + if (msg->dof_names.empty() && msg->values.size() == reference_and_state_dof_names_.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Reference massage does not have DoF names defined. " + "Assuming that value have order as defined state DoFs"); + auto ref_msg = msg; + ref_msg->dof_names = reference_and_state_dof_names_; + input_ref_.writeFromNonRT(ref_msg); + } + else if ( + msg->dof_names.size() == reference_and_state_dof_names_.size() && + msg->values.size() == reference_and_state_dof_names_.size()) + { + auto ref_msg = msg; // simple initialization + + // sort values in the ref_msg + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + + bool all_found = true; + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + auto found_it = + std::find(ref_msg->dof_names.begin(), ref_msg->dof_names.end(), msg->dof_names[i]); + if (found_it == msg->dof_names.end()) + { + all_found = false; + RCLCPP_WARN( + get_node()->get_logger(), "DoF name '%s' not found in the defined list of state DoFs.", + msg->dof_names[i].c_str()); + break; + } + + auto position = std::distance(ref_msg->dof_names.begin(), found_it); + ref_msg->values[position] = msg->values[i]; + ref_msg->values_dot[position] = msg->values_dot[i]; + } + + if (all_found) + { + input_ref_.writeFromNonRT(ref_msg); + } + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) and/or values (%zu) is not matching the expected size (%zu).", + msg->dof_names.size(), msg->values.size(), reference_and_state_dof_names_.size()); + } +} + +controller_interface::InterfaceConfiguration PidController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params_.dof_names.size()); + for (const auto & dof_name : params_.dof_names) + { + command_interfaces_config.names.push_back(dof_name + "/" + params_.command_interface); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PidController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + if (params_.use_external_measured_states) + { + state_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + } + else + { + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(dof_ * params_.reference_and_state_interfaces.size()); + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces_config.names.push_back(dof_name + "/" + interface); + } + } + } + + return state_interfaces_config; +} + +std::vector PidController::on_export_reference_interfaces() +{ + reference_interfaces_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + 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])); + ++index; + } + } + + return reference_interfaces; +} + +bool PidController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn PidController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command (the same number as state interfaces) + reset_controller_reference_msg(*(input_ref_.readFromRT()), reference_and_state_dof_names_); + reset_controller_measured_state_msg( + *(measured_state_.readFromRT()), reference_and_state_dof_names_); + + reference_interfaces_.assign( + reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + measured_state_values_.assign( + measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PidController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto current_ref = input_ref_.readFromRT(); + + for (size_t i = 0; i < dof_; ++i) + { + if (!std::isnan((*current_ref)->values[i])) + { + reference_interfaces_[i] = (*current_ref)->values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + { + reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + } + + (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + } + } + return controller_interface::return_type::OK; +} + +controller_interface::return_type PidController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // check for any parameter updates + update_parameters(); + + if (params_.use_external_measured_states) + { + const auto measured_state = *(measured_state_.readFromRT()); + for (size_t i = 0; i < dof_; ++i) + { + measured_state_values_[i] = measured_state->values[i]; + if (measured_state_values_.size() == 2 * dof_) + { + measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + } + } + } + else + { + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + measured_state_values_[i] = state_interfaces_[i].get_value(); + } + } + + for (size_t i = 0; i < dof_; ++i) + { + double tmp_command = std::numeric_limits::quiet_NaN(); + + // 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; + } + + double error = reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -picomputeCommand( + error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); + } + else + { + // Fallback to calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + } + else + { + // use calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + + // write calculated values + command_interfaces_[i].set_value(tmp_command); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + for (size_t i = 0; i < dof_; ++i) + { + state_publisher_->msg_.dof_states[i].reference = reference_interfaces_[i]; + state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[i]; + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].error = + reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -pimsg_.dof_states[i].error = + angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]); + } + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].error_dot = + reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].time_step = period.seconds(); + // Command can store the old calculated values. This should be obvious because at least one + // another value is NaN. + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + } + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace pid_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + pid_controller::PidController, controller_interface::ChainableControllerInterface) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml new file mode 100644 index 0000000000..f645738862 --- /dev/null +++ b/pid_controller/src/pid_controller.yaml @@ -0,0 +1,87 @@ +pid_controller: + dof_names: { + type: string_array, + default_value: [], + description: "Specifies dof_names or axes used by the controller. If 'reference_and_state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + reference_and_state_dof_names: { + type: string_array, + default_value: [], + description: "(optional) Specifies dof_names or axes for getting reference and reading states. This parameter is only relevant when state dof names are different then command dof names, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + command_interface: { + type: string, + default_value: "", + description: "Name of the interface used by the controller for writing commands to the hardware.", + read_only: true, + validation: { + not_empty<>: null, + } + } + reference_and_state_interfaces: { + type: string_array, + default_value: [], + description: "Name of the interfaces used by the controller getting hardware states and reference commands. The second interface should be the derivative of the first.", + read_only: true, + validation: { + not_empty<>: null, + size_lt<>: 3, + } + } + use_external_measured_states: { + type: bool, + default_value: false, + description: "Use external states from a topic instead from state interfaces." + } + gains: + __map_dof_names: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + antiwindup: { + type: bool, + default_value: false, + description: "Antiwindup functionality." + } + i_clamp_max: { + type: double, + default_value: 0.0, + description: "Upper integral clamp. Only used if antiwindup is activated." + } + i_clamp_min: { + type: double, + default_value: 0.0, + description: "Lower integral clamp. Only used if antiwindup is activated." + } + feedforward_gain: { + type: double, + default_value: 0.0, + description: "Gain for the feed-forward part." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (i.e., are continuous). + Normalizes position-error to -pi to pi." + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml new file mode 100644 index 0000000000..7555cfc156 --- /dev/null +++ b/pid_controller/test/pid_controller_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml new file mode 100644 index 0000000000..673abfe08e --- /dev/null +++ b/pid_controller/test/pid_controller_preceding_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + reference_and_state_dof_names: + - joint1state diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp new file mode 100644 index 0000000000..3a75f6e170 --- /dev/null +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPidController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); + + rclcpp::shutdown(); +} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp new file mode 100644 index 0000000000..11f703a1a4 --- /dev/null +++ b/pid_controller/test/test_pid_controller.cpp @@ -0,0 +1,531 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.0); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.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); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < command_interfaces.names.size(); ++i) + { + EXPECT_EQ(command_interfaces.names[i], dof_names_[i] + "/" + command_interface_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +TEST_F(PidControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_EQ((*msg)->values.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values_dot) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(PidControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, test_feedforward_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are not wrapped +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are wrapped +} + +TEST_F(PidControllerTest, subscribe_and_get_messages_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); + } + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + ASSERT_EQ(msg.dof_states[i].reference, 0.45); + ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp new file mode 100644 index 0000000000..ab32f5cb48 --- /dev/null +++ b/pid_controller/test/test_pid_controller.hpp @@ -0,0 +1,285 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef TEST_PID_CONTROLLER_HPP_ +#define TEST_PID_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "pid_controller/pid_controller.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; +using ControllerModeSrvType = pid_controller::PidController::ControllerModeSrvType; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestablePidController : public pid_controller::PidController +{ + FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(PidControllerTest, activate_success); + FRIEND_TEST(PidControllerTest, reactivate_success); + FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_off); + 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); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = pid_controller::PidController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return pid_controller::PidController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class PidControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + dof_names_ = {"joint1"}; + command_interface_ = "position"; + state_interfaces_ = {"position"}; + dof_state_values_ = {1.1}; + dof_command_values_ = {101.101}; + reference_and_state_dof_names_ = {"joint1state"}; + + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + feedforward_service_client_ = service_caller_node_->create_client( + "/test_pid_controller/set_feedforward_control"); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_pid_controller") + { + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(dof_names_.size()); + command_ifs.reserve(dof_names_.size()); + + for (size_t i = 0; i < dof_names_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + dof_names_[i], command_interface_, &dof_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_ifs.reserve(dof_names_.size() * state_interfaces_.size()); + state_itfs_.reserve(dof_names_.size() * state_interfaces_.size()); + size_t index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index])); + state_ifs.emplace_back(state_itfs_.back()); + ++index; + } + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_pid_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands( + const std::vector & values = {0.45}, const std::vector & values_dot = {0.0}) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.dof_names = dof_names_; + msg.values = values; + msg.values_dot = values_dot; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool feedforward, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = feedforward; + + bool wait_for_service_ret = + feedforward_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Service is not available!"); + } + auto result = feedforward_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::vector dof_names_; + std::string command_interface_; + std::vector state_interfaces_; + std::vector dof_state_values_; + std::vector dof_command_values_; + std::vector reference_and_state_dof_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr feedforward_service_client_; +}; + +#endif // TEST_PID_CONTROLLER_HPP_ diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp new file mode 100644 index 0000000000..e0d3051226 --- /dev/null +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -0,0 +1,103 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_THAT( + controller_->params_.reference_and_state_dof_names, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_THAT( + controller_->reference_and_state_dof_names_, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_EQ(controller_->params_.command_interface, command_interface_); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index d34a5b1375..3b77b7cc69 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -20,6 +20,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library From 71b06d01c1374914b93f202c80182210b385fb5f Mon Sep 17 00:00:00 2001 From: Abishalini Sivaraman Date: Mon, 4 Dec 2023 13:21:13 -0700 Subject: [PATCH 08/24] Fix floating point comparison in JTC (#879) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix floating point comparison in JTC * Fix format --------- Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9306b07354..59e2c408c5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1369,10 +1369,11 @@ bool JointTrajectoryController::validate_trajectory_msg( { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) { - if (trajectory.points.back().velocities.at(i) != 0.) + if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits::epsilon()) { RCLCPP_ERROR( - get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f", + get_node()->get_logger(), + "Velocity of last trajectory point of joint %s is not zero: %.15f", trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i)); return false; } From 4b267648fa1ac10120cb597e7f6ee23593b6633d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 5 Dec 2023 17:53:25 +0100 Subject: [PATCH 09/24] Bump ros-tooling/setup-ros from 0.7.0 to 0.7.1 (#881) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.7.0 to 0.7.1. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.7.0...0.7.1) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index 76b2c22e53..f263cd2da3 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: humble steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 1aaf30c639..720f0416c9 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: iron steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 72aa9af66c..e55285b179 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 5789c2dee4..60cf153978 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -14,7 +14,7 @@ jobs: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 6b1af2b86c..f0ac7b447d 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v4 From 925a690fcb469cb705ec67185cd67c6f1481dd61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Dec 2023 22:52:01 +0100 Subject: [PATCH 10/24] Add pid_controller to CI jobs (#883) --- .github/workflows/ci-coverage-build-humble.yml | 1 + .github/workflows/ci-coverage-build-iron.yml | 1 + .github/workflows/ci-coverage-build.yml | 1 + .github/workflows/ci-ros-lint.yml | 2 ++ 4 files changed, 5 insertions(+) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index f263cd2da3..ece507aaa1 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 720f0416c9..8d77e65b6b 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index e55285b179..bf9ad847b4 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -38,6 +38,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 60cf153978..f6b9c027c9 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -31,6 +31,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster ros2_controllers @@ -69,6 +70,7 @@ jobs: imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster ros2_controllers From e8a127f2b8bc6b5561f54d4afcb30a641f60ec3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Dec 2023 13:42:43 +0100 Subject: [PATCH 11/24] Fix rqt jtc bugs for continuous joints and other minor bugs (#890) --- .../joint_limits_urdf.py | 57 ++++++++++++------- .../joint_trajectory_controller.py | 10 +++- 2 files changed, 42 insertions(+), 25 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 7914a76b17..5655e12f7a 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -14,6 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +# Code inspired on the joint_state_publisher package by David Lu!!! +# https://github.com/ros/robot_model/blob/indigo-devel/ +# joint_state_publisher/joint_state_publisher/joint_state_publisher + # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got # Exception: Required attribute not set in XML: upper # upper is an optional attribute, so I don't understand what's going on @@ -33,21 +37,24 @@ def callback(msg): description = msg.data -def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True): - global description - use_small = use_smallest_joint_limits - use_mimic = True - - # Code inspired on the joint_state_publisher package by David Lu!!! - # https://github.com/ros/robot_model/blob/indigo-devel/ - # joint_state_publisher/joint_state_publisher/joint_state_publisher - +def subscribe_to_robot_description(node, key="robot_description"): qos_profile = rclpy.qos.QoSProfile(depth=1) qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE - node.create_subscription(String, "/robot_description", callback, qos_profile) - rclpy.spin_once(node) + node.create_subscription(String, key, callback, qos_profile) + + +def get_joint_limits(node, use_smallest_joint_limits=True): + global description + use_small = use_smallest_joint_limits + use_mimic = True + + count = 0 + while description == "" and count < 10: + print("Waiting for the robot_description!") + count += 1 + rclpy.spin_once(node, timeout_sec=1.0) free_joints = {} dependent_joints = {} @@ -66,21 +73,27 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr name = child.getAttribute("name") try: limit = child.getElementsByTagName("limit")[0] - except IndexError: - continue - if jtype == "continuous": - minval = -pi - maxval = pi - else: try: minval = float(limit.getAttribute("lower")) maxval = float(limit.getAttribute("upper")) except ValueError: - continue - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: - continue + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: + raise Exception( + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + except IndexError: + raise Exception( + f"Missing limits tag for the joint : {name} in the robot_description!" + ) safety_tags = child.getElementsByTagName("safety_controller") if use_small and len(safety_tags) == 1: tag = safety_tags[0] diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 99f43e125e..162977cdfe 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -29,7 +29,7 @@ from .utils import ControllerLister, ControllerManagerLister from .double_editor import DoubleEditor -from .joint_limits_urdf import get_joint_limits +from .joint_limits_urdf import get_joint_limits, subscribe_to_robot_description from .update_combo import update_combo # TODO: @@ -170,6 +170,9 @@ def __init__(self, context): self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() + # subscriptions + subscribe_to_robot_description(self._node) + # Signal connections w = self._widget w.enable_button.toggled.connect(self._on_jtc_enabled) @@ -460,8 +463,9 @@ def _jtc_joint_names(jtc_info): joint_names = [] for interface in jtc_info.claimed_interfaces: - name = interface.split("/")[0] - joint_names.append(name) + name = interface.split("/")[-2] + if name not in joint_names: + joint_names.append(name) return joint_names From 960e073185b15f330d5d9e1b7af2682cc7c2e8b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 8 Dec 2023 19:47:20 +0100 Subject: [PATCH 12/24] Use more mergify features (#888) --- .github/mergify.yml | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 3aaaab2001..692c346ab4 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,7 +8,6 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion conditions: - base=master @@ -21,7 +20,29 @@ pull_request_rules: - name: Ask to resolve conflict conditions: - conflict - - author!=mergify + - author!=mergify[bot] actions: comment: message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: Ask to resolve conflict for backports + conditions: + - conflict + - author=mergify[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @bmagyar @dstogl @christophfroehlich? + + - name: development targets master branch + conditions: + - base!=master + - author!=bmagyar + - author!=dstogl + - author!=christophfroehlich + - author!=mergify[bot] + actions: + comment: + message: | + @{{author}}, all pull requests must be targeted towards the `master` development branch. + Once merged into `master`, it is possible to backport to @{{base}}, but it must be in `master` + to have these changes reflected into new distributions. From 999eae5491a79f9734f8d5e4e18ffea3e7a04e2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 9 Dec 2023 21:09:32 +0100 Subject: [PATCH 13/24] Update good-first-issue.md (#895) --- .github/ISSUE_TEMPLATE/good-first-issue.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 4a2664918a..4de9ad8d30 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -55,6 +55,6 @@ Don’t hesitate to ask questions or to get help if you feel like you are gettin Furthermore, you find helpful resources here: * [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) * [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) -* [ROS Answers](https://answers.ros.org/questions/) +* [Robotics Stack Exchange](https://robotics.stackexchange.com) **Good luck with your first issue!** From cb56213c7836caa208c6049048088d33a72b6aff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 17:52:20 +0100 Subject: [PATCH 14/24] Add configs for humble and iron (#901) --- .github/dependabot.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 05a48fc654..aafd67c236 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -11,3 +11,17 @@ updates: directory: "/" schedule: interval: "weekly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "humble" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "iron" From 52d3d3f2f64c357361c8f99f018bbaa49584e27f Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Dec 2023 18:05:29 +0100 Subject: [PATCH 15/24] Bump actions/setup-python from 4.7.1 to 5.0.0 (#906) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4.7.1 to 5.0.0. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4.7.1...v5.0.0) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 5d801016f9..9f090b48ca 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.1 + - uses: actions/setup-python@v5.0.0 with: python-version: '3.10' - name: Install system hooks From 0a96cbb513282880d2d64adfb8c3c8c0ffc84b5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 19:08:10 +0100 Subject: [PATCH 16/24] Update list of reviewers (#884) --- .github/reviewer-lottery.yml | 29 +++++++++++------------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 84b156f5a1..c6580eacd4 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -13,28 +13,21 @@ groups: - name: reviewers reviewers: 5 usernames: - - rosterloh - - progtologist + - aprotyas - arne48 + - bijoua29 - christophfroehlich - DasRoteSkelett - - sgmurray - - harderthan - - jaron-l - - malapatiravi + - duringhof - erickisos - - sachinkum0009 - - qiayuanliao - - homalozoa - - anfemosa - - jackcenter - - VX792 - - mhubii + - fmauch + - jaron-l - livanov93 - - aprotyas + - mcbed + - moriarty + - olivier-stasse - peterdavidfagan - - duringhof + - progtologist + - saikishor - VanshGehlot - - bijoua29 - - LukasMacha97 - - mcbed + - VX792 From e8e091da251778985ec1c36b24874f8ba5886d93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Dec 2023 08:12:43 +0100 Subject: [PATCH 17/24] mergify: Ignore dependabot[bot] (#916) --- .github/mergify.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/mergify.yml b/.github/mergify.yml index 692c346ab4..0a6e425a30 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -21,6 +21,7 @@ pull_request_rules: conditions: - conflict - author!=mergify[bot] + - author!=dependabot[bot] actions: comment: message: This pull request is in conflict. Could you fix it @{{author}}? @@ -40,6 +41,7 @@ pull_request_rules: - author!=dstogl - author!=christophfroehlich - author!=mergify[bot] + - author!=dependabot[bot] actions: comment: message: | From 1d0d7531ef8d24ea52e761a872b8bf277652dd33 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 12 Dec 2023 09:51:22 +0100 Subject: [PATCH 18/24] Cleanup package.xml und clarify tests of JTC. (#889) * Cleanup package.xml und clarify tests of JTC. * Update joint_trajectory_controller/package.xml Co-authored-by: Bence Magyar --------- Co-authored-by: Bence Magyar --- joint_trajectory_controller/package.xml | 15 +++++---------- .../test/test_trajectory_controller_utils.hpp | 8 ++++---- 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index b41a7cad0c..70d9bd51d2 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -4,29 +4,24 @@ 4.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar - Jordan Palacios - Karsten Knese + Dr. Denis Štogl + Christoph Froehlich Apache License 2.0 ament_cmake - angles - pluginlib - realtime_tools - - angles - pluginlib - realtime_tools - + angles backward_ros controller_interface control_msgs control_toolbox generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_lifecycle + realtime_tools rsl tl_expected trajectory_msgs diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 4a65b4ad51..7b68d882ff 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -215,7 +215,7 @@ class TrajectoryControllerTest : public ::testing::Test } void SetPidParameters( - double p_default = 0.0, double ff_default = 1.0, bool angle_wraparound_default = false) + double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -223,13 +223,13 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", p_default); + const rclcpp::Parameter k_p(prefix + ".p", p_value); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); const rclcpp::Parameter angle_wraparound( - prefix + ".angle_wraparound", angle_wraparound_default); + prefix + ".angle_wraparound", angle_wraparound_value); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); } } From 364df81e9947671e6e51d45ce306b93afceebec8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:07:22 +0000 Subject: [PATCH 19/24] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 + admittance_controller/CHANGELOG.rst | 3 + bicycle_steering_controller/CHANGELOG.rst | 3 + diff_drive_controller/CHANGELOG.rst | 3 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 3 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 3 + imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 3 + joint_trajectory_controller/CHANGELOG.rst | 6 + pid_controller/CHANGELOG.rst | 164 ++++++++++++++++++ pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 3 + range_sensor_broadcaster/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 5 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 5 + steering_controllers_library/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 3 + tricycle_steering_controller/CHANGELOG.rst | 3 + velocity_controllers/CHANGELOG.rst | 3 + 22 files changed, 232 insertions(+), 1 deletion(-) create mode 100644 pid_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a1cbe9596e..15e7408f00 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 615a457992..c1053639c0 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index bee3606ec8..89d3f6291d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 8ee42d701f..646903a5ac 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index f4496d3927..7cf9e2f114 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index b87aaa2641..027158d18c 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index b38f021587..e23dc6db47 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 798a0f3e67..4c492457f2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2c311626cf..55c324f22c 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index f2295beca3..d46f807ab5 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 7e4c25a474..f2406abef0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Cleanup package.xml und clarify tests of JTC. (`#889 `_) +* Fix floating point comparison in JTC (`#879 `_) +* Contributors: Abishalini Sivaraman, Dr. Denis + 4.1.0 (2023-12-01) ------------------ * [JTC] Continue with last trajectory-point on success (`#842 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst new file mode 100644 index 0000000000..af41de496e --- /dev/null +++ b/pid_controller/CHANGELOG.rst @@ -0,0 +1,164 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pid_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* 🚀 Add PID controller 🎉 (`#434 `_) +* Contributors: Dr. Denis + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 7d52121582..c955c4747a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 0.0.0 + 4.1.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 6f0870c9ae..ef93a8b3d9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index be35427e46..b530b7c721 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 46f994c792..03064c6f95 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 🚀 Add PID controller 🎉 (`#434 `_) +* Contributors: Dr. Denis + 4.1.0 (2023-12-01) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a7a2f7a1fd..b6dcdc8d3a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index ee80d4b30d..5e0c8f82da 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) +* Contributors: Sai Kishor Kothakota + 4.1.0 (2023-12-01) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index bae8e6e557..44f3fa515f 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0203298259..98957eb42f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ * Increase test coverage of interface configuration getters (`#856 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4abf0788fb..cb3a5af24d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 91575e6075..8b84ddf784 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-12-01) ------------------ From 2bcff2f87505bbc32fdfaa8a2938cecddcadc3f8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:07:51 +0000 Subject: [PATCH 20/24] 4.2.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 15e7408f00..975ecbf5a6 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 476c4d3050..20bfca8003 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.1.0 + 4.2.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index c1053639c0..86ba49d92a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index da5b652bd0..478b10bb55 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.1.0 + 4.2.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 89d3f6291d..d7259fbc75 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index c627577ee8..2573605890 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.1.0 + 4.2.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 646903a5ac..eb40ec1db1 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 03dffe764d..5cacf41dad 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.1.0 + 4.2.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 7cf9e2f114..6cd3b74a95 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e55e095c57..e42456c828 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 027158d18c..a808f82c70 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 48db0e49b6..154e597c5f 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.1.0 + 4.2.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e23dc6db47..45e5a8081c 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c500c1f714..324ec1c5be 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 4c492457f2..ff2046a41b 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 34c7ba663e..ce981141a5 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.1.0 + 4.2.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 55c324f22c..e8be87d1f3 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 3ab9c5fff6..48a26ead3f 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.1.0 + 4.2.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index d46f807ab5..4e92131965 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 37ba7b4912..300f8b2238 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.1.0 + 4.2.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index f2406abef0..1b3559af58 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * Cleanup package.xml und clarify tests of JTC. (`#889 `_) * Fix floating point comparison in JTC (`#879 `_) * Contributors: Abishalini Sivaraman, Dr. Denis diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 70d9bd51d2..1d0929b8b5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.1.0 + 4.2.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index af41de496e..5924bc6d94 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * 🚀 Add PID controller 🎉 (`#434 `_) * Contributors: Dr. Denis diff --git a/pid_controller/package.xml b/pid_controller/package.xml index c955c4747a..7caa3f7856 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.1.0 + 4.2.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ef93a8b3d9..2e1e38fa43 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 1aec89e59d..8ed752c536 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b530b7c721..c92b4867b5 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 222d87e3bb..08e8fd9506 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.1.0 + 4.2.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 03064c6f95..5839d43b68 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * 🚀 Add PID controller 🎉 (`#434 `_) * Contributors: Dr. Denis diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3b77b7cc69..793aea9140 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.1.0 + 4.2.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index b6dcdc8d3a..d7c37b2fee 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 16dd709c25..84ebaea6fa 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.1.0 + 4.2.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index baa01b97c2..b9aa443081 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.1.0", + version="4.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5e0c8f82da..67d9eb85df 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * Fix rqt jtc bugs for continuous joints and other minor bugs (`#890 `_) * Contributors: Sai Kishor Kothakota diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 24ed2c9d63..4e7c622798 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.1.0 + 4.2.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 2d855d9255..1e49983693 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.1.0", + version="4.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 44f3fa515f..fa8cd4ee2f 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5de0ddc872..85398f1d90 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.1.0 + 4.2.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 98957eb42f..d7424ca31a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 5f382bf8d2..4f131da79c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.1.0 + 4.2.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index cb3a5af24d..3d04f2ba8d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f565ef0c02..f6eb3fff49 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.1.0 + 4.2.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 8b84ddf784..98d6523639 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-12-01) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d7376e8e6b..3d901585db 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.1.0 + 4.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 13a43c32b5b4d0040c96d4e192db4f1ed6d4f046 Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Wed, 13 Dec 2023 07:16:11 -0500 Subject: [PATCH 21/24] Changing default int values to double in steering controller's yaml file (#927) --- .../src/steering_controllers_library.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 8acdfb1448..a9f7fa75fb 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -1,7 +1,7 @@ steering_controllers_library: reference_timeout: { type: double, - default_value: 1, + default_value: 1.0, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } @@ -93,14 +93,14 @@ steering_controllers_library: twist_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], description: "diagonal values of twist covariance matrix.", read_only: false, } pose_covariance_diagonal: { type: double_array, - default_value: [0, 7, 14, 21, 28, 35], + default_value: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0], description: "diagonal values of pose covariance matrix.", read_only: false, } From 691175034c8de94dc70318fa3edb9f3e889434d5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 21 Dec 2023 11:23:49 +0000 Subject: [PATCH 22/24] Bump actions/upload-artifact from 3.1.3 to 4.0.0 (#934) --- .github/workflows/ci-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index ece507aaa1..a1d159541d 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 8d77e65b6b..08e6eafd82 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index bf9ad847b4..f785652989 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -60,7 +60,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index f0ac7b447d..fcc1a297fd 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.3 + - uses: actions/upload-artifact@v4.0.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 4c6d7236245c527f275b1e03a7e5242b658dc782 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 21 Dec 2023 12:29:46 +0100 Subject: [PATCH 23/24] [JTC] Add console output for tolerance checks (#932) --- .../joint_trajectory_controller/tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b4cb029dd9..b5e660be54 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -150,7 +150,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "Path state tolerances failed:"); + RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 59e2c408c5..c6a5169855 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -247,20 +247,21 @@ controller_interface::return_type JointTrajectoryController::update( // Always check the state tolerance on the first sample in case the first sample // is the last point + // print output per default, goal will be aborted afterwards if ( - (before_last_point || first_sample) && + (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.state_tolerance[index], + true /* show_errors */)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance if ( - !before_last_point && + !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.goal_state_tolerance[index], + false /* show_errors */)) { outside_goal_tolerance = true; @@ -269,6 +270,10 @@ controller_interface::return_type JointTrajectoryController::update( if (time_difference > default_tolerances_.goal_time_tolerance) { within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], + true /* show_errors */); } } } From 3c3f61437b89d53f01918d6e49f4428112609824 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 23 Dec 2023 08:53:42 +0100 Subject: [PATCH 24/24] Do not run reviewer lottery for dependabot and mergify (#946) --- .github/workflows/reviewer_lottery.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 2edbc9b59e..ed28964e01 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -6,6 +6,7 @@ on: jobs: test: runs-on: ubuntu-latest + if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]' steps: - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v3