From 3809db7351fc06d1623e6d08400f8844d57662b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 15 Jan 2024 11:13:39 +0100 Subject: [PATCH 01/63] Update mergify.yml (#974) --- .github/mergify.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 0a6e425a30..e2e225980f 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -32,13 +32,13 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @dstogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? - name: development targets master branch conditions: - base!=master - author!=bmagyar - - author!=dstogl + - author!=destogl - author!=christophfroehlich - author!=mergify[bot] - author!=dependabot[bot] From d5f1eabb92bed62543f2223008963170b7542f19 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 15 Jan 2024 18:29:23 +0100 Subject: [PATCH 02/63] [PID] Remove joint_jog include (#975) --- pid_controller/include/pid_controller/pid_controller.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index a34dc9f87f..f7b8cc4491 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -35,7 +35,6 @@ #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" From 2f7a7552bdd485b540b292d7e541e17519915166 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 17 Jan 2024 07:41:18 +0000 Subject: [PATCH 03/63] Bump actions/upload-artifact from 4.0.0 to 4.1.0 (#976) --- .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 a1d159541d..4b52ea32a3 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@v4.0.0 + - uses: actions/upload-artifact@v4.1.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 08e6eafd82..0720291062 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@v4.0.0 + - uses: actions/upload-artifact@v4.1.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 f785652989..c65ee8339b 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@v4.0.0 + - uses: actions/upload-artifact@v4.1.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 fcc1a297fd..9157fa6edb 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@v4.0.0 + - uses: actions/upload-artifact@v4.1.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 833ed7fbf67f0443ca28a1d24e035a64c99f2640 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 23 Jan 2024 00:08:43 +0100 Subject: [PATCH 04/63] [JTC] Convert lambda to class functions (#945) * Convert lambdas to member functions * Make member function const * Add a test for compute_error function * Make reference_wrapper argument const * Iterate over error fields --- .../joint_trajectory_controller.hpp | 35 +++- .../tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 70 +++---- .../test/test_trajectory_controller.cpp | 174 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 24 +++ 5 files changed, 261 insertions(+), 44 deletions(-) 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 0ed2ce5688..b9097b1ce3 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 @@ -209,6 +209,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void goal_accepted_callback( std::shared_ptr> goal_handle); + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + + /** + * Computes the error for a specific joint in the trajectory. + * + * @param[out] error The computed error for the joint. + * @param[in] index The index of the joint in the trajectory. + * @param[in] current The current state of the joints. + * @param[in] desired The desired state of the joints. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const; // fill trajectory_msg so it matches joints controlled by this controller // positions set to current position, velocities, accelerations and efforts to 0.0 JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -217,7 +231,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // sorts the joints of the incoming message to our local order JOINT_TRAJECTORY_CONTROLLER_PUBLIC void sort_to_local_joint_order( - std::shared_ptr trajectory_msg); + std::shared_ptr trajectory_msg) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -251,7 +265,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_active_trajectory() const; - using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, @@ -283,6 +296,24 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + + /** + * @brief Assigns the values from a trajectory point interface to a joint interface. + * + * @tparam T The type of the joint interface. + * @param[out] joint_interface The reference_wrapper to assign the values to + * @param[in] trajectory_point_interface Containing the values to assign. + * @todo: Use auto in parameter declaration with c++20 + */ + template + void assign_interface_from_point( + const T & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + } }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 2157fdf2a6..c46b1c297f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -147,7 +147,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "State tolerances failed for joint %ld:", joint_idx); + RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", 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 4a2a801e51..a8c7562b2f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -122,35 +122,6 @@ controller_interface::return_type JointTrajectoryController::update( } } - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, size_t index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) - { - // error defined as the difference between current and desired - if (joints_angle_wraparound_[index]) - { - // if desired, the shortest_angular_distance is calculated, i.e., the error is - // normalized between -piupdate(*new_external_msg); } - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; - // current state update state_current_.time_from_start.set__sec(0); read_state_from_state_interfaces(state_current_); @@ -1191,6 +1151,34 @@ void JointTrajectoryController::goal_accepted_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } +void JointTrajectoryController::compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const +{ + // error defined as the difference between current and desired + if (joints_angle_wraparound_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -pi trajectory_msg) const { @@ -1252,7 +1240,7 @@ void JointTrajectoryController::fill_partial_goal( } void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) + std::shared_ptr trajectory_msg) const { // rearrange all points in the trajectory message based on mapping std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 72820c0295..dce04bf43a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -461,6 +461,180 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) // Floating-point value comparison threshold const double EPS = 1e-6; + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=true + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, true /* angle_wraparound */); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } + } + + // angle wraparound of position error + desired.positions[0] += 3.0 * M_PI_2; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + if (i == 0) + { + EXPECT_NEAR( + error.positions[i], desired.positions[i] - current.positions[i] - 2.0 * M_PI, EPS); + } + else + { + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + } + + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } + } + + executor.cancel(); +} + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=false + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, false /* angle_wraparound */); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } + } + + // no normalization of position error + desired.positions[0] += 3.0 * M_PI_4; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } + } + + executor.cancel(); +} + /** * @brief check if position error of revolute joints are angle_wraparound if not configured so */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index e636fad4b7..f98fd3e286 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -107,6 +107,13 @@ class TestableJointTrajectoryController void trigger_declare_parameters() { param_listener_->declare_params(); } + void testable_compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) + { + compute_error_for_joint(error, index, current, desired); + } + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; @@ -154,6 +161,23 @@ class TestableJointTrajectoryController trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + /** + * a copy of the private member function + */ + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) + { + point.positions.resize(size, 0.0); + if (has_velocity_state_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_state_interface_) + { + point.accelerations.resize(size, 0.0); + } + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; From d69676709afe8f82bdeb44f665dad66eaca3d5da Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 23 Jan 2024 09:43:12 +0000 Subject: [PATCH 05/63] Bump actions/upload-artifact from 4.1.0 to 4.2.0 (#984) --- .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 4b52ea32a3..a195702d8c 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@v4.1.0 + - uses: actions/upload-artifact@v4.2.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 0720291062..7febd4417c 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@v4.1.0 + - uses: actions/upload-artifact@v4.2.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 c65ee8339b..c00f15e4a6 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@v4.1.0 + - uses: actions/upload-artifact@v4.2.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 9157fa6edb..0f20551214 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@v4.1.0 + - uses: actions/upload-artifact@v4.2.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 3ab375942e2c0086d086d1d83a470449ae25351f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 23 Jan 2024 10:50:42 +0100 Subject: [PATCH 06/63] Update mergify.yml (#982) --- .github/mergify.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index e2e225980f..39ee6b6bc0 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -46,5 +46,5 @@ pull_request_rules: 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` + 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 0bcb77bf083894d8208d6bfe7ae560d7dbf90aab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 25 Jan 2024 18:19:27 +0100 Subject: [PATCH 07/63] [CI] debian + RHEL updates (#985) --- .github/workflows/humble-debian-build.yml | 30 +++++++++++++++++++ .../workflows/humble-rhel-binary-build.yml | 20 ++++++------- .github/workflows/iron-debian-build.yml | 30 +++++++++++++++++++ .github/workflows/iron-rhel-binary-build.yml | 18 +++++++---- .github/workflows/rolling-debian-build.yml | 30 +++++++++++++++++++ .../workflows/rolling-rhel-binary-build.yml | 18 +++++++---- 6 files changed, 123 insertions(+), 23 deletions(-) create mode 100644 .github/workflows/humble-debian-build.yml create mode 100644 .github/workflows/iron-debian-build.yml create mode 100644 .github/workflows/rolling-debian-build.yml diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml new file mode 100644 index 0000000000..e8deb2caa5 --- /dev/null +++ b/.github/workflows/humble-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Humble Build +on: + workflow_dispatch: + pull_request: + branches: + - humble + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + humble_debian: + name: Humble debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: humble + container: ghcr.io/ros-controls/ros:humble-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_controllers + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs + colcon test-result --verbose diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 9da6059892..cd9b85b2e1 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -1,19 +1,13 @@ -name: Humble RHEL Binary Build +name: RHEL Humble Binary Build on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble - push: - branches: - - humble schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' - jobs: humble_rhel_binary: name: Humble RHEL binary build @@ -25,9 +19,13 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers - - run: | + - name: Install dependencies + run: | rosdep update - rosdep install -iy --from-path src/ros2_controllers + rosdep install -iyr --from-path src/ros2_controllers || true + - name: Build and test + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + colcon build --packages-skip rqt_joint_trajectory_controller + colcon test --packages-skip rqt_joint_trajectory_controller + colcon test-result --verbose diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml new file mode 100644 index 0000000000..09dbd051b2 --- /dev/null +++ b/.github/workflows/iron-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Iron Build +on: + workflow_dispatch: + pull_request: + branches: + - iron + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + iron_debian: + name: Iron debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: iron + container: ghcr.io/ros-controls/ros:iron-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_controllers + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs + colcon test-result --verbose diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index 5664d61768..0eb28b9673 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -1,7 +1,7 @@ -name: Iron RHEL Binary Build +name: RHEL Iron Binary Build on: workflow_dispatch: - push: + pull_request: branches: - iron schedule: @@ -20,9 +20,15 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers - - run: | + - name: Install dependencies + run: | rosdep update - rosdep install -iy --from-path src/ros2_controllers + rosdep install -iyr --from-path src/ros2_controllers || true + - name: Build and test + # source also underlay workspace with generate_parameter_library on rhel9 + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + source /opt/ros2_ws/install/setup.bash + colcon build --packages-skip rqt_joint_trajectory_controller + colcon test --packages-skip rqt_joint_trajectory_controller + colcon test-result --verbose diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml new file mode 100644 index 0000000000..b6d0a4193a --- /dev/null +++ b/.github/workflows/rolling-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Rolling Build +on: + workflow_dispatch: + pull_request: + branches: + - master + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + rolling_debian: + name: Rolling debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: rolling + container: ghcr.io/ros-controls/ros:rolling-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_controllers + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller + colcon test-result --verbose diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 04dc58775f..dece43b673 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,7 +1,7 @@ -name: Rolling RHEL Binary Build +name: RHEL Rolling Binary Build on: workflow_dispatch: - push: + pull_request: branches: - master schedule: @@ -20,9 +20,15 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers - - run: | + - name: Install dependencies + run: | rosdep update - rosdep install -iy --from-path src/ros2_controllers + rosdep install -iyr --from-path src/ros2_controllers || true + - name: Build and test + # source also underlay workspace with generate_parameter_library on rhel9 + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + source /opt/ros2_ws/install/setup.bash + colcon build --packages-skip rqt_joint_trajectory_controller + colcon test --packages-skip rqt_joint_trajectory_controller + colcon test-result --verbose From 9f7e9e916ce8ae1403f0f0f5d108c80ea0242a9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 25 Jan 2024 19:02:41 +0100 Subject: [PATCH 08/63] Revert "[JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (#967)" (#978) This reverts commit 6e2736b0ed521e1b236dcf8ab2ede4ef565e97d4. --- .../src/joint_trajectory_controller_parameters.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index a6358d1015..4981489d36 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -3,6 +3,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of joints used by the controller", + read_only: true, validation: { unique<>: null, } @@ -20,6 +21,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of command interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], @@ -30,6 +32,7 @@ joint_trajectory_controller: type: string_array, default_value: [], description: "Names of state interfaces to claim", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], From 7fee9401d68302e27ffa78efe6d1dc82237c39e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jan 2024 18:55:33 +0100 Subject: [PATCH 09/63] Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (#698)" (#988) This reverts commit 32aaef7552638826aba0b3f3a72b1c1453739afa. --- .../src/force_torque_sensor_broadcaster.cpp | 16 +++-- .../test_force_torque_sensor_broadcaster.cpp | 67 +++++++++---------- 2 files changed, 39 insertions(+), 44 deletions(-) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index e9a173380b..9b570d353f 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -29,12 +29,6 @@ ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() } controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() -{ - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { try { @@ -43,10 +37,18 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( } catch (const std::exception & e) { - fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + const bool no_interface_names_defined = params_.interface_names.force.x.empty() && params_.interface_names.force.y.empty() && params_.interface_names.force.z.empty() && params_.interface_names.torque.x.empty() && diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index ce6a04ec1f..f06252d83a 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -113,12 +113,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'interface_names' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, both 'sensor_name' and 'interface_names' supplied ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -129,7 +128,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe SetUpFTSBroadcaster(); // set the 'sensor_name' empty - fts_broadcaster_->get_node()->declare_parameter("sensor_name", ""); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); // configure failed, 'sensor_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -140,8 +139,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe SetUpFTSBroadcaster(); // set the 'interface_names' empty - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", ""); - fts_broadcaster_->get_node()->declare_parameter("interface_names.torque.z", ""); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""}); // configure failed, 'interface_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -152,10 +151,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -172,12 +171,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // set the 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -188,8 +186,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -216,8 +214,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -232,10 +230,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -249,8 +246,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -272,10 +269,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -297,16 +293,13 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set all the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.y", "fts_sensor/force.y"); - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.z", "fts_sensor/force.z"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.x", "fts_sensor/torque.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.y", "fts_sensor/torque.y"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); From 8d732f1a3bc2879f89162e7d0ddae275180451c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Jan 2024 18:58:52 +0100 Subject: [PATCH 10/63] [tricycle_controller] Use generate_parameter_library (#957) --- tricycle_controller/CMakeLists.txt | 13 +- tricycle_controller/doc/userdoc.rst | 10 +- .../tricycle_controller.hpp | 27 +--- tricycle_controller/package.xml | 1 + .../src/tricycle_controller.cpp | 151 +++++------------- .../src/tricycle_controller_parameter.yaml | 149 +++++++++++++++++ .../test/test_load_tricycle_controller.cpp | 9 +- .../test/test_tricycle_controller.cpp | 115 ++++++------- 8 files changed, 275 insertions(+), 200 deletions(-) create mode 100644 tricycle_controller/src/tricycle_controller_parameter.yaml diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 4f6d064dc8..cdc69f182a 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces controller_interface geometry_msgs + generate_parameter_library hardware_interface nav_msgs pluginlib @@ -29,6 +30,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(tricycle_controller_parameters + src/tricycle_controller_parameter.yaml +) + add_library(tricycle_controller SHARED src/tricycle_controller.cpp src/odometry.cpp @@ -40,6 +45,7 @@ target_include_directories(tricycle_controller PUBLIC $ $ ) +target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters) ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) @@ -50,8 +56,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_tricycle_controller - test/test_tricycle_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + test/test_tricycle_controller.cpp) target_link_libraries(test_tricycle_controller tricycle_controller ) @@ -66,8 +71,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_tricycle_controller + add_rostest_with_parameters_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml ) target_link_libraries(test_load_tricycle_controller tricycle_controller @@ -85,6 +91,7 @@ install( install( TARGETS tricycle_controller + tricycle_controller_parameters EXPORT export_tricycle_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index d153aeacba..991627eca2 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -5,7 +5,8 @@ tricycle_controller ===================== -Controller for mobile robots with tricycle drive. +Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle. + Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. @@ -24,3 +25,10 @@ Other features Odometry publishing Velocity, acceleration and jerk limits Automatic stop after command timeout + +Parameters +-------------- + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/tricycle_controller_parameter.yaml diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index cef90d026a..24aaf89de9 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -45,6 +45,9 @@ #include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" +// auto-generated by generate_parameter_library +#include "tricycle_controller_parameters.hpp" + namespace tricycle_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -109,31 +112,14 @@ class TricycleController : public controller_interface::ControllerInterface double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); std::tuple twist_to_ackermann(double linear_command, double angular_command); - std::string traction_joint_name_; - std::string steering_joint_name_; + // Parameters from ROS for tricycle_controller + std::shared_ptr param_listener_; + Params params_; // HACK: put into vector to avoid initializing structs because they have no default constructors std::vector traction_joint_; std::vector steering_joint_; - struct WheelParams - { - double wheelbase = 0.0; - double radius = 0.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in separate node - std::string base_frame_id = "base_link"; - std::string odom_frame_id = "odom"; - std::array pose_covariance_diagonal; - std::array twist_covariance_diagonal; - } odom_params_; - - bool publish_ackermann_command_ = false; std::shared_ptr> ackermann_command_publisher_ = nullptr; std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; @@ -168,7 +154,6 @@ class TricycleController : public controller_interface::ControllerInterface SteeringLimiter limiter_steering_; bool is_halted = false; - bool use_stamped_vel_ = true; void reset_odometry( const std::shared_ptr request_header, diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cebd332a60..cc50f0d58c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -10,6 +10,7 @@ Tony Najjar ament_cmake + generate_parameter_library ackermann_msgs backward_ros diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index d6aeec0af1..07be8c2aae 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -52,41 +52,9 @@ CallbackReturn TricycleController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare("traction_joint_name", std::string()); - auto_declare("steering_joint_name", std::string()); - - auto_declare("wheelbase", wheel_params_.wheelbase); - auto_declare("wheel_radius", wheel_params_.radius); - - auto_declare("odom_frame_id", odom_params_.odom_frame_id); - auto_declare("base_frame_id", odom_params_.base_frame_id); - auto_declare>("pose_covariance_diagonal", std::vector()); - auto_declare>("twist_covariance_diagonal", std::vector()); - auto_declare("open_loop", odom_params_.open_loop); - auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); - auto_declare("odom_only_twist", odom_params_.odom_only_twist); - - auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count())); - auto_declare("publish_ackermann_command", publish_ackermann_command_); - auto_declare("velocity_rolling_window_size", 10); - auto_declare("use_stamped_vel", use_stamped_vel_); - - auto_declare("traction.max_velocity", NAN); - auto_declare("traction.min_velocity", NAN); - auto_declare("traction.max_acceleration", NAN); - auto_declare("traction.min_acceleration", NAN); - auto_declare("traction.max_deceleration", NAN); - auto_declare("traction.min_deceleration", NAN); - auto_declare("traction.max_jerk", NAN); - auto_declare("traction.min_jerk", NAN); - - auto_declare("steering.max_position", NAN); - auto_declare("steering.min_position", NAN); - auto_declare("steering.max_velocity", NAN); - auto_declare("steering.min_velocity", NAN); - auto_declare("steering.max_acceleration", NAN); - auto_declare("steering.min_acceleration", NAN); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -101,8 +69,8 @@ InterfaceConfiguration TricycleController::command_interface_configuration() con { InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + command_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return command_interfaces_config; } @@ -110,8 +78,8 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const { InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + state_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return state_interfaces_config; } @@ -151,7 +119,7 @@ controller_interface::return_type TricycleController::update( double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - if (odom_params_.open_loop) + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); } @@ -172,7 +140,7 @@ controller_interface::return_type TricycleController::update( { auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) + if (!params_.odom_only_twist) { odometry_message.pose.pose.position.x = odometry_.getX(); odometry_message.pose.pose.position.y = odometry_.getY(); @@ -186,7 +154,7 @@ controller_interface::return_type TricycleController::update( realtime_odometry_publisher_->unlockAndPublish(); } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; @@ -239,7 +207,7 @@ controller_interface::return_type TricycleController::update( previous_commands_.emplace(ackermann_command); // Publish ackermann command - if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) + if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel @@ -258,74 +226,40 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { auto logger = get_node()->get_logger(); - // update parameters - traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); - steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); - if (traction_joint_name_.empty()) + // update parameters if they have changed + if (param_listener_->is_old(params_)) { - RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); - return CallbackReturn::ERROR; + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); } - if (steering_joint_name_.empty()) - { - RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); - return CallbackReturn::ERROR; - } - - wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); - odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); + odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); - odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - - auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); - std::copy( - pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - - auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); - std::copy( - twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - - odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); - odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); - odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - - cmd_vel_timeout_ = - std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; - publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; + params_.publish_ackermann_command = + get_node()->get_parameter("publish_ackermann_command").as_bool(); + params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool(); try { limiter_traction_ = TractionLimiter( - get_node()->get_parameter("traction.min_velocity").as_double(), - get_node()->get_parameter("traction.max_velocity").as_double(), - get_node()->get_parameter("traction.min_acceleration").as_double(), - get_node()->get_parameter("traction.max_acceleration").as_double(), - get_node()->get_parameter("traction.min_deceleration").as_double(), - get_node()->get_parameter("traction.max_deceleration").as_double(), - get_node()->get_parameter("traction.min_jerk").as_double(), - get_node()->get_parameter("traction.max_jerk").as_double()); + params_.traction.min_velocity, params_.traction.max_velocity, + params_.traction.min_acceleration, params_.traction.max_acceleration, + params_.traction.min_deceleration, params_.traction.max_deceleration, + params_.traction.min_jerk, params_.traction.max_jerk); } catch (const std::invalid_argument & e) { RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); return CallbackReturn::ERROR; } - try { limiter_steering_ = SteeringLimiter( - get_node()->get_parameter("steering.min_position").as_double(), - get_node()->get_parameter("steering.max_position").as_double(), - get_node()->get_parameter("steering.min_velocity").as_double(), - get_node()->get_parameter("steering.max_velocity").as_double(), - get_node()->get_parameter("steering.min_acceleration").as_double(), - get_node()->get_parameter("steering.max_acceleration").as_double()); + params_.steering.min_position, params_.steering.max_position, params_.steering.min_velocity, + params_.steering.max_velocity, params_.steering.min_acceleration, + params_.steering.max_acceleration); } catch (const std::invalid_argument & e) { @@ -347,7 +281,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & previous_commands_.emplace(empty_ackermann_drive); // initialize ackermann command publisher - if (publish_ackermann_command_) + if (params_.publish_ackermann_command) { ackermann_command_publisher_ = get_node()->create_publisher( DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -357,7 +291,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (use_stamped_vel_) + if (params_.use_stamped_vel) { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -409,8 +343,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_publisher_); auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_params_.odom_frame_id; - odometry_message.child_frame_id = odom_params_.base_frame_id; + odometry_message.header.frame_id = params_.odom_frame_id; + odometry_message.child_frame_id = params_.base_frame_id; // initialize odom values zeros odometry_message.twist = @@ -421,13 +355,12 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = - odom_params_.twist_covariance_diagonal[index]; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message - if (odom_params_.enable_odom_tf) + if (params_.enable_odom_tf) { odometry_transform_publisher_ = get_node()->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -438,8 +371,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & // keeping track of odom and base_link transforms only auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; } // Create odom reset service @@ -456,8 +389,8 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); // Initialize the joints - const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); - const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + const auto wheel_front_result = get_traction(params_.traction_joint_name, traction_joint_); + const auto steering_result = get_steering(params_.steering_joint_name, steering_joint_); if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) { return CallbackReturn::ERROR; @@ -644,12 +577,12 @@ std::tuple TricycleController::twist_to_ackermann(double Vx, dou if (Vx == 0 && theta_dot != 0) { // is spin action alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + Ws = abs(theta_dot) * params_.wheelbase / params_.wheel_radius; return std::make_tuple(alpha, Ws); } - alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); - Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, params_.wheelbase); + Ws = Vx / (params_.wheel_radius * std::cos(alpha)); return std::make_tuple(alpha, Ws); } diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml new file mode 100644 index 0000000000..68fc2202c2 --- /dev/null +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -0,0 +1,149 @@ +tricycle_controller: + traction_joint_name: { + type: string, + default_value: "", + description: "Name of the traction joint", + validation: { + not_empty<>: [] + } + } + steering_joint_name: { + type: string, + default_value: "", + description: "Name of the steering joint", + validation: { + not_empty<>: [] + } + } + wheelbase: { + type: double, + default_value: 0.0, + description: "Shortest distance between the front wheel and the rear axle. If this parameter is wrong, the robot will not behave correctly in curves.", + } + wheel_radius: { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: { + type: string, + default_value: "base_link", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + enable_odom_tf: { + type: bool, + default_value: false, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + odom_only_twist: { + type: bool, + default_value: false, + description: "for doing the pose integration in separate node.", + } + cmd_vel_timeout: { + type: int, + default_value: 500, # milliseconds + description: "Timeout in milliseconds, after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_ackermann_command: { + type: bool, + default_value: false, + description: "Publish limited commands.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", + validation: { + gt<>: [0] + } + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + traction: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_deceleration: { + type: double, + default_value: .NAN, + } + min_deceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + steering: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + max_position: { + type: double, + default_value: .NAN, + } + min_position: { + type: double, + default_value: .NAN, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 9298fae574..245523c844 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -26,8 +26,6 @@ TEST(TestLoadTricycleController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -39,6 +37,13 @@ TEST(TestLoadTricycleController, load_controller) ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), nullptr); +} +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/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index d8beedae42..018727e260 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -41,6 +41,12 @@ using lifecycle_msgs::msg::State; using testing::SizeIs; using testing::UnorderedElementsAre; +namespace +{ +const char traction_joint_name[] = "traction_joint"; +const char steering_joint_name[] = "steering_joint"; +} // namespace + class TestableTricycleController : public tricycle_controller::TricycleController { public: @@ -146,11 +152,28 @@ class TestTricycleController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::string traction_joint_name_init = traction_joint_name, + const std::string steering_joint_name_init = steering_joint_name, + const std::vector & parameters = {}) + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name_init))); + parameter_overrides.push_back( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, "", node_options); + } + const std::string controller_name = "test_tricycle_controller"; std::unique_ptr controller_; - const std::string traction_joint_name = "traction_joint"; - const std::string steering_joint_name = "steering_joint"; double position_ = 0.1; double velocity_ = 0.2; @@ -172,42 +195,24 @@ class TestTricycleController : public ::testing::Test const std::string urdf_ = ""; }; -TEST_F(TestTricycleController, configure_fails_without_parameters) +TEST_F(TestTricycleController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) +TEST_F(TestTricycleController, init_fails_if_only_traction_or_steering_side_defined) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ( + InitController(traction_joint_name, std::string()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + InitController(std::string(), steering_joint_name), controller_interface::return_type::ERROR); } TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -215,26 +220,22 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) auto cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - cmd_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + cmd_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - state_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + state_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -242,15 +243,9 @@ TEST_F(TestTricycleController, activate_fails_without_resources_assigned) TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResources(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -258,15 +253,11 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 1.2), rclcpp::Parameter("wheel_radius", 0.12)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -307,15 +298,11 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); From ab6835144d9d60a38e778484039349ff44844f32 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 Jan 2024 17:09:15 +0100 Subject: [PATCH 11/63] Bump ros-tooling/action-ros-ci from 0.3.5 to 0.3.6 (#994) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.3.5 to 0.3.6. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.3.5...0.3.6) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci 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/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 a195702d8c..cf98b2194e 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 7febd4417c..41e40a5cb5 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index c00f15e4a6..7d12c7f2ad 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 0f20551214..76c3a70781 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} From d375e4348347139b6dfce86fcda0a75ce24d0b7b Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 Jan 2024 17:09:25 +0100 Subject: [PATCH 12/63] Bump codecov/codecov-action from 3.1.4 to 3.1.5 (#993) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.4 to 3.1.5. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.4...v3.1.5) --- updated-dependencies: - dependency-name: codecov/codecov-action 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 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index cf98b2194e..f1a0941f20 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v3.1.5 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 41e40a5cb5..1d21f3b78b 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v3.1.5 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 7d12c7f2ad..e6a5b74d06 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v3.1.5 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 021ccab8c7a4a75392bd8903aafa1e7c9e23bc01 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 30 Jan 2024 17:09:34 +0100 Subject: [PATCH 13/63] Bump actions/upload-artifact from 4.2.0 to 4.3.0 (#992) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 4.2.0 to 4.3.0. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v4.2.0...v4.3.0) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-type: direct:production update-type: version-update:semver-minor ... 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/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 f1a0941f20..c503e090ab 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@v4.2.0 + - uses: actions/upload-artifact@v4.3.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 1d21f3b78b..62ff5f34fe 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@v4.2.0 + - uses: actions/upload-artifact@v4.3.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 e6a5b74d06..3f6dff5ab8 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@v4.2.0 + - uses: actions/upload-artifact@v4.3.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 76c3a70781..2a3aa21325 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@v4.2.0 + - uses: actions/upload-artifact@v4.3.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 99fadceed82918975e3471d615e897b3facd536e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 30 Jan 2024 17:44:14 +0100 Subject: [PATCH 14/63] [JTC] Invalidate empty trajectory messages (#902) --- .../src/joint_trajectory_controller.cpp | 6 +++ .../test/test_trajectory_controller.cpp | 42 ++++++++++++++++++- 2 files changed, 46 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a8c7562b2f..a22fe7f2d1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1363,6 +1363,12 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + if (trajectory.points.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; + } + if (!params_.allow_nonzero_velocity_at_trajectory_end) { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index dce04bf43a..fbd6d7aea5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1366,6 +1366,11 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg.joint_names = {"bad_name"}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // empty message + traj_msg = good_traj_msg; + traj_msg.points.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // No position data traj_msg = good_traj_msg; traj_msg.points[0].positions.clear(); @@ -1402,8 +1407,41 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); } -/// With allow_integration_in_goal_trajectories parameter trajectory missing position or -/// velocities are accepted +/** + * @brief Test invalid velocity at trajectory end with parameter set to false + */ +TEST_P( + TrajectoryControllerTestParameterized, + expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) +{ + rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg; + traj_msg.joint_names = joint_names_; + traj_msg.header.stamp = rclcpp::Time(0); + + // empty message (no throw!) + ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg)); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Nonzero velocity at trajectory end! + traj_msg.points.resize(1); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(1); + traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + traj_msg.points[0].velocities.resize(1); + traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes + * + * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or + * velocities are accepted + */ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) { rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); From f375c698aa6b241b596a42c9d6e879708be4526f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:35:22 +0100 Subject: [PATCH 15/63] Let sphinx add parameter description with nested structures to documentation (#652) --- admittance_controller/doc/userdoc.rst | 10 +- .../doc/parameters_context.yaml | 9 ++ diff_drive_controller/doc/userdoc.rst | 17 +- .../doc/parameters_context.yaml | 12 ++ .../doc/userdoc.rst | 26 ++- ..._torque_sensor_broadcaster_parameters.yaml | 3 +- gripper_controllers/doc/userdoc.rst | 17 +- imu_sensor_broadcaster/doc/userdoc.rst | 13 +- ...nt_state_broadcaster_parameter_context.yml | 46 ++++++ joint_state_broadcaster/doc/userdoc.rst | 71 ++------- .../joint_state_broadcaster_parameters.yaml | 11 ++ .../doc/parameters.rst | 148 ++---------------- .../doc/parameters_context.yaml | 13 ++ ...oint_trajectory_controller_parameters.yaml | 40 +++-- range_sensor_broadcaster/doc/userdoc.rst | 16 ++ 15 files changed, 204 insertions(+), 248 deletions(-) create mode 100644 diff_drive_controller/doc/parameters_context.yaml create mode 100644 force_torque_sensor_broadcaster/doc/parameters_context.yaml create mode 100644 joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml create mode 100644 joint_trajectory_controller/doc/parameters_context.yaml diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 0e4469cd50..8056a017d7 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -17,10 +17,14 @@ ROS 2 interface of the controller Parameters ^^^^^^^^^^^ -The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. -The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -An example parameter file can be found in the `test folder of the controller `_ +The admittance controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +.. generate_parameter_library_details:: ../src/admittance_controller_parameters.yaml + +An example parameter file for this controller can be found in `the test folder `_: + +.. literalinclude:: ../test/test_params.yaml + :language: yaml Topics ^^^^^^^ diff --git a/diff_drive_controller/doc/parameters_context.yaml b/diff_drive_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..81e92806f5 --- /dev/null +++ b/diff_drive_controller/doc/parameters_context.yaml @@ -0,0 +1,9 @@ +linear.x: | + Joint limits structure for the linear ``x``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +angular.z: | + Joint limits structure for the rotation about ``z``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index d2dd284cf3..70d0d7ca5c 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -64,17 +64,12 @@ Publishers Parameters ,,,,,,,,,,,, -Check `parameter definition file for details `_. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -Note that the documentation on parameters for joint limits can be found in `their header file `_. -Those parameters are: +.. generate_parameter_library_details:: ../src/diff_drive_controller_parameter.yaml + parameters_context.yaml -linear.x [JointLimits structure] - Joint limits structure for the linear X-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +An example parameter file for this controller can be found in `the test directory `_: -angular.z [JointLimits structure] - Joint limits structure for the rotation about Z-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +.. literalinclude:: ../test/config/test_diff_drive_controller.yaml + :language: yaml diff --git a/force_torque_sensor_broadcaster/doc/parameters_context.yaml b/force_torque_sensor_broadcaster/doc/parameters_context.yaml new file mode 100644 index 0000000000..6991427316 --- /dev/null +++ b/force_torque_sensor_broadcaster/doc/parameters_context.yaml @@ -0,0 +1,12 @@ +interface_names: | + (optional) Defines custom, per axis interface names. + This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. + It is sufficient that only one ``interface_name`` is defined. + This enables the broadcaster to use force sensing cells with less than six measuring axes. + An example definition is: + + .. code-block:: yaml + + interface_names: + force: + x: example_name/example_interface diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index 053723e8f0..df0430e3bb 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -12,25 +12,17 @@ The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see Parameters ^^^^^^^^^^^ -The interfaces can be defined in two ways, using ``sensor_name`` or ``interface_names`` parameter. -Those two parameters can not be defined at the same time +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -frame_id (mandatory) - Frame in which the output message will be published. +The interfaces can be defined in two ways, using the ``sensor_name`` or the ``interface_names`` parameter: +Those two parameters cannot be defined at the same time. -sensor_name (optional) - Defines sensor name used as prefix for its interfaces. - If used standard interface names for a 6D FTS will be used: /force.x, ..., /torque.z. +Full list of parameters: -interface_names.[force|torque].[x|y|z] (optional) - Defines custom, per axis interface names. - This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. - It is sufficient that only one ``interface_name`` is defined. - This enables broadcaster use for force sensing cells with less then six measuring axes. - Example definition is: +.. generate_parameter_library_details:: ../src/force_torque_sensor_broadcaster_parameters.yaml + parameters_context.yaml - .. code-block:: yaml +An example parameter file for this controller can be found in `the test directory `_: - interface_names: - force: - x: example_name/example_interface +.. literalinclude:: ../test/force_torque_sensor_broadcaster_params.yaml + :language: yaml diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 68a85d9d8e..3e75ab6012 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -10,7 +10,8 @@ force_torque_sensor_broadcaster: sensor_name: { type: string, default_value: "", - description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined. + If used, standard interface names for a 6D FTS will be used: ``/force.x, ..., /torque.z``", } interface_names: force: diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst index 28262e90f9..7f51c8f4ac 100644 --- a/gripper_controllers/doc/userdoc.rst +++ b/gripper_controllers/doc/userdoc.rst @@ -5,10 +5,23 @@ Gripper Action Controller -------------------------------- -Controller for executing a gripper command action for simple single-dof grippers. +Controllers for executing a gripper command action for simple single-dof grippers: + +- ``position_controllers/GripperActionController`` +- ``effort_controllers/GripperActionController`` Parameters ^^^^^^^^^^^ -This controller uses the `generate_parameter_library `_ to handle its parameters. +These controllers use the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= .. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/gripper_action_controller_parameters.yaml diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 3b8ae172db..09b51a7ecb 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -11,6 +11,17 @@ The controller is a wrapper around ``IMUSensor`` semantic component (see ``contr Parameters ^^^^^^^^^^^ -This controller uses the `generate_parameter_library `_ to handle its parameters. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +List of parameters +========================= .. generate_parameter_library_details:: ../src/imu_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/imu_sensor_broadcaster_params.yaml + :language: yaml diff --git a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml new file mode 100644 index 0000000000..c96f8301ae --- /dev/null +++ b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml @@ -0,0 +1,46 @@ +map_interface_to_joint_state: | + Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. + Usecases: + + #. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. + Typically one would map both values in separate interfaces in the framework. + To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. + #. A robot provides multiple measuring techniques for its joint values which results in slightly different values. + Typically one would use separate interface for providing those values in the framework. + Using multiple joint_state_broadcaster instances we could publish and show both in RViz. + + Format (each line is optional): + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: + \t\tvelocity: + \t\teffort: + + + Examples: + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: kf_estimated_position + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tvelocity: derived_velocity + \t\teffort: derived_effort + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: torque_sensor + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: current_sensor diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index c9164ec723..c7bf4fa9a1 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -22,70 +22,19 @@ If none of the requested interface are not defined, the controller returns error Parameters ---------- +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -use_local_topics - Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. +List of parameters +,,,,,,,,,,,,,,,,,, -joints - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``interfaces`` parameter. - Joint state broadcaster asks for access to all defined interfaces on all defined joints. +.. generate_parameter_library_details:: + ../src/joint_state_broadcaster_parameters.yaml + joint_state_broadcaster_parameter_context.yml -interfaces - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``joints`` parameter. +An example parameter file +,,,,,,,,,,,,,,,,,,,,,,,,, - -extra_joints - Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. - - -map_interface_to_joint_state - Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. - Usecases: - - 1. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. - Typically one would map both values in separate interfaces in the framework. - To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. - - 1. A robot provides multiple measuring techniques for its joint values which results in slightly different values. - Typically one would use separate interface for providing those values in the framework. - Using multiple joint_state_broadcaster instances we could publish and show both in RViz. - - Format (each line is optional): - - .. code-block:: yaml - - map_interface_to_joint_state: - position: - velocity: - effort: - - - Examples: - - .. code-block:: yaml - - map_interface_to_joint_state: - position: kf_estimated_position - - - .. code-block:: yaml - - map_interface_to_joint_state: - velocity: derived_velocity - effort: derived_effort - - - .. code-block:: yaml - - map_interface_to_joint_state: - effort: torque_sensor - - - .. code-block:: yaml - - map_interface_to_joint_state: - effort: current_sensor +.. generate_parameter_library_default:: + ../src/joint_state_broadcaster_parameters.yaml diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index ba0d4f0051..8f0d4da6c5 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -2,18 +2,29 @@ joint_state_broadcaster: use_local_topics: { type: bool, default_value: false, + description: "Defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``." } joints: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``interfaces`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published. + Joint state broadcaster asks for access to all defined interfaces on all defined joints." } extra_joints: { type: string_array, default_value: [], + description: "Names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0." } interfaces: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``joints`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published." } map_interface_to_joint_state: position: { diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 8ad2b406d6..dbb50fcbeb 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -5,146 +5,18 @@ Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joints (list(string)) - Joint names to control and listen to. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -command_joints (list(string)) - Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. -command_interface (list(string)) - Command interfaces provided by the hardware interface for all joints. +List of parameters +========================= - Values: [position | velocity | acceleration] (multiple allowed) +.. generate_parameter_library_details:: + ../src/joint_trajectory_controller_parameters.yaml + parameters_context.yaml -state_interfaces (list(string)) - State interfaces provided by the hardware for all joints. +An example parameter file +========================= - Values: position (mandatory) [velocity, [acceleration]]. - Acceleration interface can only be used in combination with position and velocity. - -action_monitor_rate (double) - Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). - - Default: 20.0 - -allow_partial_joints_goal (boolean) - Allow joint goals defining trajectory for only some joints. - - Default: false - -allow_integration_in_goal_trajectories (boolean) - Allow integration in goal trajectories to accept goals without position or velocity specified - - Default: false - -interpolation_method (string) - The type of interpolation to use, if any. Can be "splines" or "none". - - Default: splines - -open_loop_control (boolean) - Use controller in open-loop control mode: - - * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. - * It deactivates the feedback control, see the ``gains`` structure. - - This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). - - .. Note:: - If this flag is set, the controller tries to read the values from the command interfaces on activation. - If they have real numeric values, those will be used instead of state interfaces. - Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. - - Default: false - -start_with_holding (bool) - If true, start with holding position after activation. Otherwise, no command will be sent until - the first trajectory is received. - - Default: true - -allow_nonzero_velocity_at_trajectory_end (boolean) - If false, the last velocity point has to be zero or the goal will be rejected. - - Default: false - -cmd_timeout (double) - Timeout after which the input command is considered stale. - Timeout is counted from the end of the trajectory (the last point). - ``cmd_timeout`` must be greater than ``constraints.goal_time``, - otherwise ignored. - - If zero, timeout is deactivated" - - Default: 0.0 - -constraints (structure) - Default values for tolerances if no explicit values are states in JointTrajectory message. - -constraints.stopped_velocity_tolerance (double) - Default value for end velocity deviation. - - Default: 0.01 - -constraints.goal_time (double) - Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. - If set to zero, the controller will wait a potentially infinite amount of time. - - Default: 0.0 (not checked) - -constraints..trajectory (double) - Maximally allowed deviation from the target trajectory for a given joint. - - Default: 0.0 (tolerance is not enforced) - -constraints..goal (double) - Maximally allowed deviation from the goal (end of the trajectory) for a given joint. - - Default: 0.0 (tolerance is not enforced) - -gains (structure) - Only relevant, if ``open_loop_control`` is not set. - - If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. - This structure contains the controller gains for every joint with the control law - - .. math:: - - u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), - the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. - -gains..p (double) - Proportional gain :math:`k_p` for PID - - Default: 0.0 - -gains..i (double) - Integral gain :math:`k_i` for PID - - Default: 0.0 - -gains..d (double) - Derivative gain :math:`k_d` for PID - - Default: 0.0 - -gains..i_clamp (double) - Integral clamp. Symmetrical in both positive and negative direction. - - Default: 0.0 - -gains..ff_velocity_scale (double) - Feed-forward scaling :math:`k_{ff}` of velocity - - Default: 0.0 - -gains..angle_wraparound (bool) - For joints that wrap around (without end stop, ie. are continuous), - where the shortest rotation to the target position is the desired motion. - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured - position :math:`s` from the state interface. - - Default: false +.. generate_parameter_library_default:: + ../src/joint_trajectory_controller_parameters.yaml diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..2ffe8aed36 --- /dev/null +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -0,0 +1,13 @@ +constraints: + Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message. + +gains: | + Only relevant, if ``open_loop_control`` is not set. + + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. + This structure contains the controller gains for every joint with the control law + + .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 4981489d36..ded5bb7ca3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -2,7 +2,7 @@ joint_trajectory_controller: joints: { type: string_array, default_value: [], - description: "Names of joints used by the controller", + description: "Joint names to control and listen to", read_only: true, validation: { unique<>: null, @@ -11,7 +11,7 @@ joint_trajectory_controller: command_joints: { type: string_array, default_value: [], - description: "Names of the commanding joints used by the controller", + description: "Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.", read_only: true, validation: { unique<>: null, @@ -20,7 +20,7 @@ joint_trajectory_controller: command_interfaces: { type: string_array, default_value: [], - description: "Names of command interfaces to claim", + description: "Command interfaces provided by the hardware interface for all joints", read_only: true, validation: { unique<>: null, @@ -31,7 +31,7 @@ joint_trajectory_controller: state_interfaces: { type: string_array, default_value: [], - description: "Names of state interfaces to claim", + description: "State interfaces provided by the hardware for all joints.", read_only: true, validation: { unique<>: null, @@ -42,12 +42,21 @@ joint_trajectory_controller: allow_partial_joints_goal: { type: bool, default_value: false, - description: "Goals with partial set of joints are allowed", + description: "Allow joint goals defining trajectory for only some joints.", } open_loop_control: { type: bool, default_value: false, - description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + description: "Use controller in open-loop control mode + \n\n + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n + * It deactivates the feedback control, see the ``gains`` structure. + \n\n + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + \n\n + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started.\n", read_only: true, } allow_integration_in_goal_trajectories: { @@ -58,7 +67,7 @@ joint_trajectory_controller: action_monitor_rate: { type: double, default_value: 20.0, - description: "Rate status changes will be monitored", + description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory)", read_only: true, validation: { gt_eq: [0.1] @@ -83,7 +92,7 @@ joint_trajectory_controller: default_value: 0.0, # seconds description: "Timeout after which the input command is considered stale. Timeout is counted from the end of the trajectory (the last point). - cmd_timeout must be greater than constraints.goal_time, otherwise ignored. + ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored. If zero, timeout is deactivated", } gains: @@ -91,17 +100,17 @@ joint_trajectory_controller: p: { type: double, default_value: 0.0, - description: "Proportional gain for PID" + description: "Proportional gain :math:`k_p` for PID" } i: { type: double, default_value: 0.0, - description: "Integral gain for PID" + description: "Integral gain :math:`k_i` for PID" } d: { type: double, default_value: 0.0, - description: "Derivative gain for PID" + description: "Derivative gain :math:`k_d` for PID" } i_clamp: { type: double, @@ -111,13 +120,16 @@ joint_trajectory_controller: ff_velocity_scale: { type: double, default_value: 0.0, - description: "Feed-forward scaling of velocity." + description: "Feed-forward scaling :math:`k_{ff}` of velocity" } angle_wraparound: { type: bool, default_value: false, - description: "For joints that wrap around (ie. are continuous). - Normalizes position-error to -pi to pi." + description: 'For joints that wrap around (without end stop, ie. are continuous), + where the shortest rotation to the target position is the desired motion. + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface.' } constraints: stopped_velocity_tolerance: { diff --git a/range_sensor_broadcaster/doc/userdoc.rst b/range_sensor_broadcaster/doc/userdoc.rst index e35bec67ad..39fb98b3aa 100644 --- a/range_sensor_broadcaster/doc/userdoc.rst +++ b/range_sensor_broadcaster/doc/userdoc.rst @@ -11,5 +11,21 @@ The controller is a wrapper around ``RangeSensor`` semantic component (see ``con Parameters ^^^^^^^^^^^ +The Range Sensor Broadcaster uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= .. generate_parameter_library_details:: ../src/range_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/range_sensor_broadcaster_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/range_sensor_broadcaster_params.yaml + :language: yaml From f2f36c0c65607659cb5961f1354a64b3d1a6866c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:40:02 +0100 Subject: [PATCH 16/63] Add tests for `interface_configuration_type` consistently (#899) --- .../test_ackermann_steering_controller.cpp | 28 +++++++------- .../test_ackermann_steering_controller.hpp | 2 +- ...kermann_steering_controller_preceeding.cpp | 28 +++++++------- .../test/test_admittance_controller.cpp | 3 ++ .../test/test_bicycle_steering_controller.cpp | 22 +++++------ .../test/test_bicycle_steering_controller.hpp | 2 +- ...bicycle_steering_controller_preceeding.cpp | 22 +++++------ .../test/test_diff_drive_controller.cpp | 12 +++--- .../test_force_torque_sensor_broadcaster.cpp | 6 +++ .../test/test_forward_command_controller.cpp | 9 +++++ ...i_interface_forward_command_controller.cpp | 2 + .../test/test_imu_sensor_broadcaster.cpp | 6 +++ .../test/test_joint_state_broadcaster.cpp | 18 +++++++++ .../test/test_trajectory_controller.cpp | 12 +++--- pid_controller/test/test_pid_controller.cpp | 29 ++++++++------- .../test/test_pid_controller_preceding.cpp | 29 ++++++++------- .../test/test_range_sensor_broadcaster.cpp | 4 ++ .../test_steering_controllers_library.cpp | 28 +++++++------- .../test_steering_controllers_library.hpp | 2 +- .../test_tricycle_steering_controller.cpp | 37 ++++++++++--------- .../test_tricycle_steering_controller.hpp | 2 +- ...ricycle_steering_controller_preceeding.cpp | 34 +++++++++-------- 22 files changed, 198 insertions(+), 139 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 480e90e166..ef5454a16c 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -46,41 +46,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index a2849d5742..7c279d6323 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -62,7 +62,7 @@ class TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController { FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 2d951588c5..1a16bed838 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -48,41 +48,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index fe1d3214e0..6b03249df8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -157,12 +157,15 @@ TEST_F(AdmittanceControllerTest, check_interfaces) auto command_interfaces = controller_->command_interface_configuration(); ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->state_interfaces_.size(), diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 06b0c7e846..3dcdc0b1db 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -44,29 +44,29 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 521506762b..6e84342bea 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -60,7 +60,7 @@ class TestableBicycleSteeringController : public bicycle_steering_controller::BicycleSteeringController { FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(BicycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(BicycleSteeringControllerTest, activate_success); FRIEND_TEST(BicycleSteeringControllerTest, update_success); FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 875910ba23..bc3a182753 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -46,31 +46,31 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], + cmd_if_conf.names[CMD_TRACTION_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); - EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index eb970d34a3..4ad293298f 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -249,12 +249,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_THAT( - controller_->state_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); - ASSERT_THAT( - controller_->command_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index f06252d83a..8b994307fa 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -162,8 +162,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -196,8 +198,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // deactivate passed ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -205,8 +209,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) // check interface configuration cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 236cb14edd..937144b48c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -137,6 +137,7 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -189,6 +190,7 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), @@ -197,8 +199,10 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -352,6 +356,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); @@ -359,8 +364,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -383,8 +390,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 2d3b61e211..f8f073c103 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -297,6 +297,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // send command auto command_ptr = std::make_shared(); @@ -322,6 +323,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 83edc044d3..62179a99ff 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -119,8 +119,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -138,8 +140,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // deactivate passed ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -147,8 +151,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3e45740bbc..c10f51aaa2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -180,8 +180,10 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -227,8 +229,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -274,8 +278,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -321,8 +327,10 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -360,9 +368,11 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT( state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -381,8 +391,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -455,8 +467,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -504,8 +518,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; @@ -547,8 +563,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index fbd6d7aea5..24988618bb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -120,13 +120,13 @@ TEST_P(TrajectoryControllerTestParameterized, activate) 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( - cmd_interface_config.names.size(), joint_names_.size() * command_interface_types_.size()); + auto cmd_if_conf = traj_controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_names_.size() * command_interface_types_.size()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_interface_config = traj_controller_->state_interface_configuration(); - ASSERT_EQ( - state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); + auto state_if_conf = traj_controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_names_.size() * state_interface_types_.size()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state = ActivateTrajectoryController(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 11f703a1a4..a44347f5f1 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -61,34 +61,36 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_FALSE(controller_->params_.use_external_measured_states); } -TEST_F(PidControllerTest, check_exported_intefaces) +TEST_F(PidControllerTest, check_exported_interfaces) { 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) + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { - EXPECT_EQ(command_interfaces.names[i], dof_names_[i] + "/" + command_interface_); + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.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); + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); ++si_index; } } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -96,10 +98,9 @@ TEST_F(PidControllerTest, check_exported_intefaces) { 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); + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index e0d3051226..3e17e69286 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -49,34 +49,36 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.command_interface, command_interface_); } -TEST_F(PidControllerTest, check_exported_intefaces) +TEST_F(PidControllerTest, check_exported_interfaces) { 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) + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.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); + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); ++si_index; } } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -84,10 +86,9 @@ TEST_F(PidControllerTest, check_exported_intefaces) { 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); + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 7b8e6d0e02..64f68a7e7a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -154,8 +154,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), @@ -164,8 +166,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 81075d1082..0217567a26 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -28,41 +28,43 @@ class SteeringControllersLibraryTest }; // checking if all interfaces, command, state and reference are exported as expected -TEST_F(SteeringControllersLibraryTest, check_exported_intefaces) +TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 5caf347ac1..83e6054bd4 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -73,7 +73,7 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { - FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); public: diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 82ba924305..c555de53de 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -45,46 +45,47 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index c5f6985d4e..e97e2a45bd 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -61,7 +61,7 @@ class TestableTricycleSteeringController : public tricycle_steering_controller::TricycleSteeringController { FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(TricycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(TricycleSteeringControllerTest, activate_success); FRIEND_TEST(TricycleSteeringControllerTest, update_success); FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index dd72332875..6f2913aeb8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -47,46 +47,48 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } From 2705ca8530ab9dae7bc77747541a7c35ceb4d1f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:47:09 +0100 Subject: [PATCH 17/63] [diff_drive] Remove unused parameter and add simple validation #abi-breaking (#958) --- diff_drive_controller/CMakeLists.txt | 6 +- .../diff_drive_controller.hpp | 6 + .../src/diff_drive_controller.cpp | 16 +- .../src/diff_drive_controller_parameter.yaml | 15 +- .../config/test_diff_drive_controller.yaml | 3 +- .../test/test_diff_drive_controller.cpp | 306 +++++++----------- .../test/test_load_diff_drive_controller.cpp | 10 +- 7 files changed, 143 insertions(+), 219 deletions(-) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 436832c523..d67815b5e0 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -53,8 +53,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) + test/test_diff_drive_controller.cpp) target_link_libraries(test_diff_drive_controller diff_drive_controller ) @@ -69,8 +68,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_diff_drive_controller + add_rostest_with_parameters_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml ) ament_target_dependencies(test_load_diff_drive_controller controller_manager diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 554bedba59..72b38f7d2d 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface // Parameters from ROS for diff_drive_controller std::shared_ptr param_listener_; Params params_; + /* Number of wheels on each side of the robot. This is important to take the wheels slip into + * account when multiple wheels on each side are present. If there are more wheels then control + * signals for each side, you should enter number for control signals. For example, Husky has two + * wheels on each side, but they use one control signal, in this case '1' is the correct value of + * the parameter. */ + int wheels_per_side_; Odometry odometry_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index ea08aef89b..42b6cda8e1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -149,7 +149,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -166,8 +166,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= static_cast(params_.wheels_per_side); - right_feedback_mean /= static_cast(params_.wheels_per_side); + left_feedback_mean /= static_cast(wheels_per_side_); + right_feedback_mean /= static_cast(wheels_per_side_); if (params_.position_feedback) { @@ -257,7 +257,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -286,12 +286,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.left_wheel_names.empty()) - { - RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); - return controller_interface::CallbackReturn::ERROR; - } - const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; @@ -320,7 +314,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - params_.wheels_per_side = params_.left_wheel_names.size(); + wheels_per_side_ = static_cast(params_.left_wheel_names.size()); if (publish_limited_velocity_) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 0c0285e7c2..9720e068e1 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -2,23 +2,24 @@ diff_drive_controller: left_wheel_names: { type: string_array, default_value: [], - description: "Link names of the left side wheels", + description: "Names of the left side wheels' joints", + validation: { + not_empty<>: [] + } } right_wheel_names: { type: string_array, default_value: [], - description: "Link names of the right side wheels", + description: "Names of the right side wheels' joints", + validation: { + not_empty<>: [] + } } wheel_separation: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", } - wheels_per_side: { - type: int, - default_value: 0, - description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", - } wheel_radius: { type: double, default_value: 0.0, diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index a2149eb6bc..bfbf8f2d19 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -2,7 +2,6 @@ test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] - write_op_modes: ["motor_controller"] wheel_separation: 0.40 wheels_per_side: 1 # actually 2, but both are controlled by 1 signal @@ -21,7 +20,7 @@ test_diff_drive_controller: open_loop: true enable_odom_tf: true - cmd_vel_timeout: 500 # milliseconds + cmd_vel_timeout: 0.5 # seconds publish_limited_velocity: true velocity_rolling_window_size: 10 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4ad293298f..43dae41a9b 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -36,6 +36,12 @@ using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -166,11 +172,28 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, ns, node_options); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; - const std::vector left_wheel_names = {"left_wheel_joint"}; - const std::vector right_wheel_names = {"right_wheel_joint"}; std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; @@ -193,59 +216,31 @@ class TestDiffDriveController : public ::testing::Test const std::string urdf_ = ""; }; -TEST_F(TestDiffDriveController, configure_fails_without_parameters) +TEST_F(TestDiffDriveController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - - auto extended_right_wheel_names = right_wheel_names; - extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); + ASSERT_EQ( + InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -259,26 +254,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -292,26 +279,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -327,26 +306,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -363,26 +334,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -398,26 +362,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -435,26 +392,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -469,13 +419,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -483,15 +427,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -499,15 +437,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -516,15 +450,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -533,15 +463,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -550,15 +476,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -599,15 +521,11 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 1eb8939031..983ec6d98f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -21,8 +21,6 @@ TEST(TestLoadDiffDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -33,6 +31,14 @@ TEST(TestLoadDiffDriveController, load_controller) ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); rclcpp::shutdown(); + return result; } From 67dbf7b4128f75f8d907d8ca8d480e3108bf9bca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:48:22 +0100 Subject: [PATCH 18/63] [JTC] Fill action error_strings (#887) --- .../src/joint_trajectory_controller.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a22fe7f2d1..b0e93b6ecd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -294,6 +294,7 @@ controller_interface::return_type JointTrajectoryController::update( { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + result->set__error_string("Aborted due to path tolerance violation"); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 @@ -310,9 +311,10 @@ controller_interface::return_type JointTrajectoryController::update( { if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + result->set__error_string("Goal successfully reached!"); + active_goal->setSucceeded(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); @@ -325,17 +327,19 @@ controller_interface::return_type JointTrajectoryController::update( } else if (!within_goal_time) { + const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " + + std::to_string(time_difference) + " seconds"; + auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + result->set__error_string(error_string); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); From 08ea6cf3885bd286cfd17fb1b32b40a763afae61 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 31 Jan 2024 20:54:51 +0000 Subject: [PATCH 19/63] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 7 +++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ joint_state_broadcaster/CHANGELOG.rst | 6 ++++++ joint_trajectory_controller/CHANGELOG.rst | 10 ++++++++++ pid_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 6 ++++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 107 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a314802327..83e135693c 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 826a7aa6be..47f5d2ac30 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index a1f79c922b..c44e1a9157 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 316cd9e52d..d3f89a9eea 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [diff_drive] Remove unused parameter and add simple validation #abi-breaking (`#958 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 54cdc18d69..debb1f7717 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 58191c2f42..5b0be8fdbe 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_)" (`#988 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.4.0 (2024-01-11) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e6f5e3d52f..6241e56ce1 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 677d52f784..5143c40545 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 821ef8b385..4d3b88cbd9 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 0650f4e762..d9dc1645e2 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5eb851589c..b6d85a29b5 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Fill action error_strings (`#887 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* [JTC] Invalidate empty trajectory messages (`#902 `_) +* Revert "[JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (`#967 `_)" (`#978 `_) +* [JTC] Convert lambda to class functions (`#945 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García + 4.4.0 (2024-01-11) ------------------ * Cancel goal in on_deactivate (`#962 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index b492f15efa..c2a5e3a117 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* [PID] Remove joint_jog include (`#975 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 7c3392dae9..2268ccc082 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 77a365009a..6d280dcd72 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index bdb3c388dd..6f3c02f142 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index eab2bcfcb3..17033eae28 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.4.0 (2024-01-11) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0558e15d49..504366c761 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.4.0 (2024-01-11) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ec986caddc..3483a6d388 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a0dccedfe5..075b1f516e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [tricycle_controller] Use generate_parameter_library (`#957 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 3be8c9ec03..184ffd20d5 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 8cc9c869ef..25bec1853d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.4.0 (2024-01-11) ------------------ From fd8b91f74e9d6436b618f7b0daa252db55e90c73 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 31 Jan 2024 20:55:20 +0000 Subject: [PATCH 20/63] 4.5.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 83e135693c..3521258b1a 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 6c318219b2..366c5c31cf 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.4.0 + 4.5.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 47f5d2ac30..ba1a25c0ab 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index fd7cf32401..d379438824 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.4.0 + 4.5.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 c44e1a9157..44d8e5051d 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 2082124de6..cddfcf3975 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.4.0 + 4.5.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 d3f89a9eea..4a517f8a6d 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.5.0 (2024-01-31) +------------------ * [diff_drive] Remove unused parameter and add simple validation #abi-breaking (`#958 `_) * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9f79dabea6..4531a5337d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.4.0 + 4.5.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 debb1f7717..2cb40f6e36 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ee5504c672..ce2700ef01 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.4.0 + 4.5.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 5b0be8fdbe..fc05235ade 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_)" (`#988 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index be9fc21d16..1ebd0c4a79 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.4.0 + 4.5.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 6241e56ce1..d0e32e8793 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c0827e9e20..cae9f877b7 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5143c40545..9013897769 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 9488804f1d..348cf6dd93 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.4.0 + 4.5.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 4d3b88cbd9..61d843c659 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 184f9f7dd7..b6fc59d85c 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.4.0 + 4.5.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 d9dc1645e2..cd80339155 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 70d0f7daca..43dafb66b0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.4.0 + 4.5.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 b6d85a29b5..a41c70cbc7 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.5.0 (2024-01-31) +------------------ * [JTC] Fill action error_strings (`#887 `_) * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index bc6e43a1d5..030d91d3e9 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.4.0 + 4.5.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 c2a5e3a117..40bfd292f5 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * [PID] Remove joint_jog include (`#975 `_) * Contributors: Christoph Fröhlich diff --git a/pid_controller/package.xml b/pid_controller/package.xml index dc7f9c13be..4aa553f31a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.4.0 + 4.5.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 2268ccc082..e62920bf7f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b714afc89f..e6a98ef7d2 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.4.0 + 4.5.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 6d280dcd72..09ce2703a7 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Let sphinx add parameter description with nested structures to documentation (`#652 `_) * Contributors: Christoph Fröhlich diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index d8213112da..c74acce857 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.4.0 + 4.5.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 6f3c02f142..8c96dc0ac0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 265adde781..d8477b3ccd 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.4.0 + 4.5.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 17033eae28..ab8b17ea7a 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.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f34838f5ff..4c99d9c18a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.4.0 + 4.5.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 d9c72db506..4c5afefe0a 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.4.0", + version="4.5.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 504366c761..c143c7c6ad 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.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 29db146abc..0929c2aaa3 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.4.0 + 4.5.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 02f32191d5..399ca590f9 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.4.0", + version="4.5.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 3483a6d388..8d67b186b6 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 90cb82ac8e..66a968b71f 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.4.0 + 4.5.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 075b1f516e..289ff3e6b0 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ * [tricycle_controller] Use generate_parameter_library (`#957 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cc50f0d58c..d53e8473a1 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.4.0 + 4.5.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 184ffd20d5..4f8d2be9f4 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.5.0 (2024-01-31) +------------------ * Add tests for `interface_configuration_type` consistently (`#899 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index ad1c07d396..0263f8f9fe 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.4.0 + 4.5.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 25bec1853d..cbf4e9d068 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.5.0 (2024-01-31) +------------------ 4.4.0 (2024-01-11) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 5319f55a97..94d61b8ae1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 2817f2779673e43d1c63ee0d8b35c8ed6f85410d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 1 Feb 2024 11:19:13 +0100 Subject: [PATCH 21/63] Fix tests for using new `get_node_options` API (#840) --- .../test/test_ackermann_steering_controller.hpp | 4 +++- .../test/test_bicycle_steering_controller.hpp | 4 +++- .../test/test_diff_drive_controller.cpp | 3 ++- doc/writing_new_controller.rst | 4 +++- .../test/test_joint_group_effort_controller.cpp | 3 ++- .../test/test_force_torque_sensor_broadcaster.cpp | 4 +++- .../test/test_forward_command_controller.cpp | 3 ++- .../test_multi_interface_forward_command_controller.cpp | 4 +++- gripper_controllers/test/test_gripper_controllers.cpp | 3 ++- .../test/test_imu_sensor_broadcaster.cpp | 3 ++- .../test/test_joint_state_broadcaster.cpp | 3 ++- .../test/test_trajectory_controller_utils.hpp | 9 ++++++++- pid_controller/test/test_pid_controller.hpp | 4 +++- .../test/test_joint_group_position_controller.cpp | 3 ++- .../test/test_range_sensor_broadcaster.cpp | 3 ++- .../test/test_steering_controllers_library.hpp | 4 +++- tricycle_controller/test/test_tricycle_controller.cpp | 3 ++- .../test/test_tricycle_steering_controller.hpp | 4 +++- .../test/test_joint_group_velocity_controller.cpp | 3 ++- 19 files changed, 52 insertions(+), 19 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 7c279d6323..a047186d14 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -147,7 +147,9 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 6e84342bea..5e21ff228c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -144,7 +144,9 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 43dae41a9b..9ab3022a9f 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -218,7 +218,8 @@ class TestDiffDriveController : public ::testing::Test TEST_F(TestDiffDriveController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 501c231def..1a9ffce714 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -42,7 +42,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. - 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. + 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``. + + 7. (Optional) Often, controllers accept lists of joint names and interface names as parameters. If so, you can add two protected string vectors to store those values. 4. **Adding definitions into source file (.cpp)** diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index f9d72ab202..200a1beda8 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -54,7 +54,8 @@ void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_effort_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_effort_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 8b994307fa..2412361352 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -55,7 +55,9 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { - const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", "", 0); + const auto result = fts_broadcaster_->init( + "test_force_torque_sensor_broadcaster", "", 0, "", + fts_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 937144b48c..c31c8a964c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -62,7 +62,8 @@ void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { - const auto result = controller_->init("forward_command_controller", "", 0); + const auto result = controller_->init( + "forward_command_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index f8f073c103..7879d5c1d7 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -64,7 +64,9 @@ void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset( void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { - const auto result = controller_->init("multi_interface_forward_command_controller", "", 0); + const auto result = controller_->init( + "multi_interface_forward_command_controller", "", 0, "", + controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index da9e15840e..9f7e024917 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -62,7 +62,8 @@ void GripperControllerTest::TearDown() template void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller", "", 0); + const auto result = + controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 62179a99ff..25a39a8b4d 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -55,7 +55,8 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { - const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0); + const auto result = imu_broadcaster_->init( + "test_imu_sensor_broadcaster", "", 0, "", imu_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index c10f51aaa2..2faa55f467 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,7 +69,8 @@ void JointStateBroadcasterTest::SetUpStateBroadcaster( void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( const std::vector & joint_names, const std::vector & interfaces) { - const auto result = state_broadcaster_->init("joint_state_broadcaster", "", 0); + const auto result = state_broadcaster_->init( + "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f98fd3e286..6978d0e452 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -66,6 +66,8 @@ class TestableJointTrajectoryController return ret; } + rclcpp::NodeOptions define_custom_node_options() const override { return node_options_; } + /** * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the @@ -157,6 +159,8 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } @@ -179,6 +183,7 @@ class TestableJointTrajectoryController } rclcpp::WaitSet joint_cmd_sub_wait_set_; + rclcpp::NodeOptions node_options_; }; class TrajectoryControllerTest : public ::testing::Test @@ -233,8 +238,10 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); + traj_controller_->set_node_options(node_options); - return traj_controller_->init(controller_name_, "", 0, "", node_options); + return traj_controller_->init( + controller_name_, "", 0, "", traj_controller_->define_custom_node_options()); } void SetPidParameters( diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index ab32f5cb48..1c356263e7 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -146,7 +146,9 @@ class PidControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_pid_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(dof_names_.size()); diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 3b4f00be12..60bff556db 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -54,7 +54,8 @@ void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupPositionControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_position_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_position_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 64f68a7e7a..010f18c1a6 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -37,7 +37,8 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( std::string broadcaster_name) { controller_interface::return_type result = controller_interface::return_type::ERROR; - result = range_broadcaster_->init(broadcaster_name, "", 0); + result = range_broadcaster_->init( + broadcaster_name, "", 0, "", range_broadcaster_->define_custom_node_options()); if (controller_interface::return_type::OK == result) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 83e6054bd4..1284b72096 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -168,7 +168,9 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 018727e260..5280aaf244 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -197,7 +197,8 @@ class TestTricycleController : public ::testing::Test TEST_F(TestTricycleController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); ASSERT_EQ(ret, controller_interface::return_type::ERROR); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index e97e2a45bd..6a516691b8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -146,7 +146,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 4cbf1b7342..a99ffaeebf 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -54,7 +54,8 @@ void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupVelocityControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_velocity_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_velocity_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; From 737a45b170e79c0f6bd033b37c0def6d00ab4adf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 07:22:08 +0100 Subject: [PATCH 22/63] Use correct ref for scheduled workflows (#1013) --- .github/workflows/humble-debian-build.yml | 1 + .github/workflows/humble-rhel-binary-build.yml | 1 + .github/workflows/iron-debian-build.yml | 1 + .github/workflows/iron-rhel-binary-build.yml | 1 + .github/workflows/rolling-debian-build.yml | 2 ++ .github/workflows/rolling-rhel-binary-build.yml | 2 ++ 6 files changed, 8 insertions(+) diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index e8deb2caa5..426b935fa4 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index cd9b85b2e1..933486ba50 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -19,6 +19,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - name: Install dependencies run: | rosdep update diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index 09dbd051b2..c47fbe5cd9 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index 0eb28b9673..c3bc1e6def 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -20,6 +20,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - name: Install dependencies run: | rosdep update diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index b6d0a4193a..9169494b00 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -20,6 +20,8 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - name: Build and test shell: bash run: | diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index dece43b673..98c02b72a3 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -20,6 +20,8 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_controllers + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - name: Install dependencies run: | rosdep update From 8bf379b2726900adec452193a777d900ba152e89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 09:38:34 +0100 Subject: [PATCH 23/63] Add test_depend on `hardware_interface_testing` (#1018) --- ackermann_steering_controller/package.xml | 1 + admittance_controller/package.xml | 1 + bicycle_steering_controller/package.xml | 1 + diff_drive_controller/test/test_load_diff_drive_controller.cpp | 3 +++ effort_controllers/package.xml | 2 ++ .../test/test_load_joint_group_effort_controller.cpp | 2 ++ force_torque_sensor_broadcaster/package.xml | 1 + forward_command_controller/package.xml | 1 + gripper_controllers/package.xml | 1 + .../test/test_load_gripper_action_controllers.cpp | 2 ++ imu_sensor_broadcaster/package.xml | 1 + joint_state_broadcaster/package.xml | 1 + joint_trajectory_controller/package.xml | 1 + pid_controller/package.xml | 1 + position_controllers/package.xml | 2 ++ .../test/test_load_joint_group_position_controller.cpp | 2 ++ range_sensor_broadcaster/package.xml | 2 +- tricycle_controller/package.xml | 1 + tricycle_controller/test/test_load_tricycle_controller.cpp | 2 ++ tricycle_steering_controller/package.xml | 2 +- velocity_controllers/package.xml | 1 + .../test/test_load_joint_group_velocity_controller.cpp | 2 ++ 22 files changed, 31 insertions(+), 2 deletions(-) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 366c5c31cf..fe22ca10b8 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index d379438824..e690330aa0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -34,6 +34,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing kinematics_interface_kdl ros2_control_test_assets diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index cddfcf3975..8bb6ac79fa 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -28,6 +28,7 @@ ament_cmake_gmock controller_manager hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 983ec6d98f..4c9d2f984f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -16,6 +16,9 @@ #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" diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ce2700ef01..279d5fbf43 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 61bb1ddf9a..52f1f9934a 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -17,7 +17,9 @@ #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(TestLoadJointGroupVelocityController, load_controller) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1ebd0c4a79..0791eb5d16 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -23,6 +23,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index cae9f877b7..8950a9a3e9 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -22,6 +22,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 348cf6dd93..a35fce7894 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 130b12e0bb..0ef5f0bcb2 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -17,7 +17,9 @@ #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(TestLoadGripperActionControllers, load_controller) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b6fc59d85c..5694e1cee7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -25,6 +25,7 @@ ament_lint_auto ament_lint_common controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 43dafb66b0..6dd8b4b61e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -25,6 +25,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface rclcpp ros2_control_test_assets diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 030d91d3e9..8cd2e5becc 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -28,6 +28,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 4aa553f31a..70c7bfa987 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e6a98ef7d2..e67d3d8a46 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index fe61039fdb..bc27b5e629 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -17,7 +17,9 @@ #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(TestLoadJointGroupPositionController, load_controller) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index c74acce857..2d865c1d7f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -22,7 +22,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index d53e8473a1..4a8725810b 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -30,6 +30,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 245523c844..bd54459780 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -21,7 +21,9 @@ #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(TestLoadTricycleController, load_controller) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 0263f8f9fe..16bfd522f7 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -29,7 +29,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 94d61b8ae1..3e28f7736e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -17,6 +17,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 1872b5f746..e426349f96 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -17,7 +17,9 @@ #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(TestLoadJointGroupVelocityController, load_controller) From c786604f10516379d9c5460eb5356920d923ae03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 5 Feb 2024 11:04:49 +0100 Subject: [PATCH 24/63] Add test_depend on `hardware_interface_testing` also for diff_drive (#1021) --- diff_drive_controller/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4531a5337d..e87ab85471 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -26,6 +26,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets From 5cde4fc9545fe5f14ad0fa59db67f022c476061e Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 6 Feb 2024 17:04:42 +0100 Subject: [PATCH 25/63] Bump codecov/codecov-action from 3.1.5 to 4.0.1 (#1023) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.5 to 4.0.1. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.5...v4.0.1) --- updated-dependencies: - dependency-name: codecov/codecov-action 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-coverage-build-humble.yml | 2 +- .github/workflows/ci-coverage-build-iron.yml | 2 +- .github/workflows/ci-coverage-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index c503e090ab..931a715a17 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 62ff5f34fe..06eadf320d 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 3f6dff5ab8..2fdbf677e7 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -55,7 +55,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.5 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 89109a0809fe00e2fbb7fd420bbc4c288ebb3852 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 6 Feb 2024 17:05:48 +0100 Subject: [PATCH 26/63] Bump actions/upload-artifact from 4.3.0 to 4.3.1 (#1024) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 4.3.0 to 4.3.1. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v4.3.0...v4.3.1) --- updated-dependencies: - dependency-name: actions/upload-artifact 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> Co-authored-by: Dr. Denis --- .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 931a715a17..357ee3dfa8 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@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 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 06eadf320d..7914a1acb0 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@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 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 2fdbf677e7..b96276ca5a 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@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 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 2a3aa21325..3d5bc1cf35 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@v4.3.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 1266941cefcf02e2a409303a4e5f975e87e4d358 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 9 Feb 2024 11:51:38 +0100 Subject: [PATCH 27/63] [CI] Improvements and Cleanups (#1028) --- .github/workflows/ci-ros-lint.yml | 70 +++++++------------ .../workflows/humble-binary-build-main.yml | 2 - .../workflows/humble-binary-build-testing.yml | 2 - .github/workflows/humble-debian-build.yml | 25 ++----- .../workflows/humble-rhel-binary-build.yml | 25 ++----- .../humble-semi-binary-build-main.yml | 2 - .../humble-semi-binary-build-testing.yml | 2 - .github/workflows/humble-source-build.yml | 2 - .github/workflows/iron-binary-build-main.yml | 4 -- .../workflows/iron-binary-build-testing.yml | 4 -- .github/workflows/iron-debian-build.yml | 25 ++----- .github/workflows/iron-rhel-binary-build.yml | 27 ++----- .../workflows/iron-semi-binary-build-main.yml | 4 -- .../iron-semi-binary-build-testing.yml | 4 -- .github/workflows/iron-source-build.yml | 2 - .github/workflows/reusable-debian-build.yml | 63 +++++++++++++++++ .../reusable-industrial-ci-with-cache.yml | 4 +- .../workflows/reusable-rhel-binary-build.yml | 69 ++++++++++++++++++ .../workflows/rolling-binary-build-main.yml | 4 -- .../rolling-binary-build-testing.yml | 4 -- .github/workflows/rolling-debian-build.yml | 26 ++----- .../workflows/rolling-rhel-binary-build.yml | 28 ++------ .../rolling-semi-binary-build-main.yml | 4 -- .../rolling-semi-binary-build-testing.yml | 4 -- 24 files changed, 199 insertions(+), 207 deletions(-) create mode 100644 .github/workflows/reusable-debian-build.yml create mode 100644 .github/workflows/reusable-rhel-binary-build.yml diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index f6b9c027c9..df17d11dc4 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -2,6 +2,30 @@ name: ROS Lint on: pull_request: +env: + package-name: + ackermann_steering_controller + admittance_controller + bicycle_steering_controller + diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + gripper_controllers + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + pid_controller + position_controllers + range_sensor_broadcaster + ros2_controllers + ros2_controllers_test_nodes + rqt_joint_trajectory_controller + steering_controllers_library + tricycle_controller + tricycle_steering_controller + velocity_controllers + jobs: ament_lint: name: ament_${{ matrix.linter }} @@ -19,28 +43,7 @@ jobs: with: distribution: rolling linter: ${{ matrix.linter }} - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - pid_controller - position_controllers - range_sensor_broadcaster - ros2_controllers - ros2_controllers_test_nodes - rqt_joint_trajectory_controller - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers + package-name: ${{ env.package-name }} ament_lint_100: @@ -58,25 +61,4 @@ jobs: distribution: rolling linter: cpplint arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - pid_controller - position_controllers - range_sensor_broadcaster - ros2_controllers - ros2_controllers_test_nodes - rqt_joint_trajectory_controller - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers + package-name: ${{ env.package-name }} diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml index 64d78f281a..9c634b372a 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build-main.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml index 524cacd685..b662543959 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/humble-binary-build-testing.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index 426b935fa4..c236aecf64 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Humble Build +name: Debian Humble Source Build on: workflow_dispatch: pull_request: @@ -12,20 +12,9 @@ on: jobs: humble_debian: name: Humble debian build - runs-on: ubuntu-latest - env: - ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:humble-debian - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - - name: Build and test - shell: bash - run: | - source /opt/ros2_ws/install/setup.bash - vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs - colcon test-result --verbose + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 933486ba50..db503d1993 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -11,22 +11,9 @@ on: jobs: humble_rhel_binary: name: Humble RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:humble-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - - name: Install dependencies - run: | - rosdep update - rosdep install -iyr --from-path src/ros2_controllers || true - - name: Build and test - run: | - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build --packages-skip rqt_joint_trajectory_controller - colcon test --packages-skip rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml index 863df79a22..bfe83392ea 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml index 6286636e1f..3a66c0b74d 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index ff0fd62e05..a40d53f8e3 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,8 +1,6 @@ name: Humble Source Build on: workflow_dispatch: - branches: - - humble push: branches: - humble diff --git a/.github/workflows/iron-binary-build-main.yml b/.github/workflows/iron-binary-build-main.yml index ef35397855..bb1997bd48 100644 --- a/.github/workflows/iron-binary-build-main.yml +++ b/.github/workflows/iron-binary-build-main.yml @@ -4,10 +4,6 @@ name: Iron Binary Build - main on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml index 25a693dc23..37e3524ccd 100644 --- a/.github/workflows/iron-binary-build-testing.yml +++ b/.github/workflows/iron-binary-build-testing.yml @@ -4,10 +4,6 @@ name: Iron Binary Build - testing on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index c47fbe5cd9..58787a804c 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Iron Build +name: Debian Iron Source Build on: workflow_dispatch: pull_request: @@ -12,20 +12,9 @@ on: jobs: iron_debian: name: Iron debian build - runs-on: ubuntu-latest - env: - ROS_DISTRO: iron - container: ghcr.io/ros-controls/ros:iron-debian - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - - name: Build and test - shell: bash - run: | - source /opt/ros2_ws/install/setup.bash - vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller control_msgs controller_manager_msgs - colcon test-result --verbose + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: iron + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index c3bc1e6def..90dac67a44 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -12,24 +12,9 @@ on: jobs: iron_rhel_binary: name: Iron RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: iron - container: ghcr.io/ros-controls/ros:iron-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - - name: Install dependencies - run: | - rosdep update - rosdep install -iyr --from-path src/ros2_controllers || true - - name: Build and test - # source also underlay workspace with generate_parameter_library on rhel9 - run: | - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - source /opt/ros2_ws/install/setup.bash - colcon build --packages-skip rqt_joint_trajectory_controller - colcon test --packages-skip rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: iron + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build-main.yml index 2224a59f0e..ed90a46ea8 100644 --- a/.github/workflows/iron-semi-binary-build-main.yml +++ b/.github/workflows/iron-semi-binary-build-main.yml @@ -3,10 +3,6 @@ name: Iron Semi-Binary Build - main on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml index c5ff430c89..d06a20443d 100644 --- a/.github/workflows/iron-semi-binary-build-testing.yml +++ b/.github/workflows/iron-semi-binary-build-testing.yml @@ -3,10 +3,6 @@ name: Iron Semi-Binary Build - testing on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 1e9d865c49..34372a4178 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -1,8 +1,6 @@ name: Iron Source Build on: workflow_dispatch: - branches: - - iron push: branches: - iron diff --git a/.github/workflows/reusable-debian-build.yml b/.github/workflows/reusable-debian-build.yml new file mode 100644 index 0000000000..b406fe6eaa --- /dev/null +++ b/.github/workflows/reusable-debian-build.yml @@ -0,0 +1,63 @@ +name: Reusable Debian Source Build +# Reusable action to simplify dealing with debian source builds +# author: Christoph Froehlich + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + upstream_workspace: + description: 'Path to local .repos file.' + default: '' + required: false + type: string + skip_packages: + description: 'Packages to skip from build and test' + default: '' + required: false + type: string + + +jobs: + debian_source: + name: ${{ inputs.ros_distro }} debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: ${{ inputs.ros_distro }} + path: src/ros2_controllers + container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-debian + steps: + - name: Checkout default ref when build is not scheduled + if: ${{ github.event_name != 'schedule' }} + uses: actions/checkout@v4 + with: + path: ${{ env.path }} + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build + if: ${{ github.event_name == 'schedule' }} + uses: actions/checkout@v4 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + path: ${{ env.path }} + - name: Build workspace + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + if [[ -n "${{ inputs.upstream_workspace }}" ]]; then + vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} + fi + colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros2_ws/install/setup.bash + colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + colcon test-result --verbose diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index acefeebfac..234ec27677 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -56,10 +56,10 @@ jobs: BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - - name: Checkout ${{ inputs.ref }} when build is not scheduled + - name: Checkout default ref when build is not scheduled if: ${{ github.event_name != 'schedule' }} uses: actions/checkout@v4 - - name: Checkout ${{ inputs.ref }} on scheduled build + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build if: ${{ github.event_name == 'schedule' }} uses: actions/checkout@v4 with: diff --git a/.github/workflows/reusable-rhel-binary-build.yml b/.github/workflows/reusable-rhel-binary-build.yml new file mode 100644 index 0000000000..be4eabb76b --- /dev/null +++ b/.github/workflows/reusable-rhel-binary-build.yml @@ -0,0 +1,69 @@ +name: Reusable RHEL Binary Build +# Reusable action to simplify dealing with RHEL binary builds +# author: Christoph Froehlich + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + upstream_workspace: + description: 'Path to local .repos file.' + default: '' + required: false + type: string + skip_packages: + description: 'Packages to skip from build and test' + default: '' + required: false + type: string + +jobs: + rhel_binary: + name: ${{ inputs.ros_distro }} RHEL binary build + runs-on: ubuntu-latest + env: + path: src/ros2_controllers + container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-rhel + steps: + - name: Checkout default ref when build is not scheduled + if: ${{ github.event_name != 'schedule' }} + uses: actions/checkout@v4 + with: + path: ${{ env.path }} + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build + if: ${{ github.event_name == 'schedule' }} + uses: actions/checkout@v4 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + path: ${{ env.path }} + - name: Install dependencies + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + if [[ -n "${{ inputs.upstream_workspace }}" ]]; then + vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} + fi + rosdep update + rosdep install -iyr --from-path src || true + - name: Build workspace + # source also underlay workspace with generate_parameter_library on rhel9 + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + colcon test-result --verbose diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml index 793db5d7e5..729d5e38ba 100644 --- a/.github/workflows/rolling-binary-build-main.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -4,10 +4,6 @@ name: Rolling Binary Build - main on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml index 9b480d99c3..0596aeec56 100644 --- a/.github/workflows/rolling-binary-build-testing.yml +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -4,10 +4,6 @@ name: Rolling Binary Build - testing on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 9169494b00..153f4df681 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Rolling Build +name: Debian Rolling Source Build on: workflow_dispatch: pull_request: @@ -12,21 +12,9 @@ on: jobs: rolling_debian: name: Rolling debian build - runs-on: ubuntu-latest - env: - ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:rolling-debian - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - # default behavior is correct on master branch - # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - - name: Build and test - shell: bash - run: | - source /opt/ros2_ws/install/setup.bash - vcs import src < src/ros2_controllers/ros2_controllers.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test --packages-skip rqt_controller_manager rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 98c02b72a3..31c133ab69 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -12,25 +12,9 @@ on: jobs: rolling_rhel_binary: name: Rolling RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:rolling-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - # default behavior is correct on master branch - # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - - name: Install dependencies - run: | - rosdep update - rosdep install -iyr --from-path src/ros2_controllers || true - - name: Build and test - # source also underlay workspace with generate_parameter_library on rhel9 - run: | - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - source /opt/ros2_ws/install/setup.bash - colcon build --packages-skip rqt_joint_trajectory_controller - colcon test --packages-skip rqt_joint_trajectory_controller - colcon test-result --verbose + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build-main.yml index 8b395e5163..206ca8bd52 100644 --- a/.github/workflows/rolling-semi-binary-build-main.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -3,10 +3,6 @@ name: Rolling Semi-Binary Build - main on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml index 630881dc0a..b284c0b7d4 100644 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -3,10 +3,6 @@ name: Rolling Semi-Binary Build - testing on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master From e1647c7d7312c6c6967488ab7d1c8e2e3e1573fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 4 Dec 2023 18:25:23 +0100 Subject: [PATCH 28/63] Make some methods virtual for extending of JTC. Extend caculation of values deduction. --- .../joint_trajectory_controller.hpp | 4 +-- .../joint_trajectory_plugin.xml | 6 ++--- .../src/trajectory.cpp | 26 +++++++++++++++++++ 3 files changed, 31 insertions(+), 5 deletions(-) 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 b9097b1ce3..a033e5cd1b 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 @@ -235,7 +235,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void add_new_trajectory_msg( + virtual void add_new_trajectory_msg( const std::shared_ptr & traj_msg); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( @@ -270,7 +270,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - void read_state_from_state_interfaces(JointTrajectoryPoint & state); + virtual void read_state_from_state_interfaces(JointTrajectoryPoint & state); /** Assign values from the command interfaces as state. * This is only possible if command AND state interfaces exist for the same type, diff --git a/joint_trajectory_controller/joint_trajectory_plugin.xml b/joint_trajectory_controller/joint_trajectory_plugin.xml index 27930380ea..ceed825f4c 100644 --- a/joint_trajectory_controller/joint_trajectory_plugin.xml +++ b/joint_trajectory_controller/joint_trajectory_plugin.xml @@ -1,7 +1,7 @@ - - The joint trajectory controller executes joint-space trajectories on a set of joints - + + The joint trajectory controller executes joint-space trajectories on a set of joints. + diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index fae4c41ff9..a1fc2d105e 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -331,6 +331,32 @@ void Trajectory::deduce_from_derivatives( (first_state.velocities[i] + second_state.velocities[i]) * 0.5 * delta_t; } } + else + { + for (size_t i = 0; i < dim; ++i) + { + if (std::isnan(second_state.positions[i])) + { + double first_state_velocity = + first_state.velocities.empty() ? 0.0 : first_state.velocities[i]; + if (std::isnan(first_state_velocity)) + { + first_state.velocities[i] = 0.0; + first_state_velocity = 0.0; + } + double second_state_velocity = + second_state.velocities.empty() ? 0.0 : second_state.velocities[i]; + if (std::isnan(second_state_velocity)) + { + second_state.velocities[i] = 0.0; + second_state_velocity = 0.0; + } + + second_state.positions[i] = + first_state.positions[i] + (first_state_velocity + second_state_velocity) * 0.5 * delta_t; + } + } + } } TrajectoryPointConstIter Trajectory::begin() const From 7dfd7ea370d88e09f8fdeb5beb6009b36c95b0db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 5 Dec 2023 14:25:03 +0100 Subject: [PATCH 29/63] Add structures for joint limits in the controller. --- joint_trajectory_controller/CMakeLists.txt | 1 + .../joint_trajectory_controller.hpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5cddb1bf4d..7c4d8a6684 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library hardware_interface + joint_limits pluginlib rclcpp rclcpp_lifecycle 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 a033e5cd1b..27c90538fa 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 @@ -27,6 +27,7 @@ #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/trajectory.hpp" @@ -194,6 +195,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + std::vector joint_limits_; + // callback for topic interface JOINT_TRAJECTORY_CONTROLLER_PUBLIC void topic_callback(const std::shared_ptr msg); From c0a38aca0c86f9d56bf63d1e0c84018a471c8c09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 5 Dec 2023 15:03:45 +0100 Subject: [PATCH 30/63] Added Cartesian Trajectory Generator (CTG). --- joint_trajectory_controller/CMakeLists.txt | 32 ++ .../cartesian_trajectory_generator.hpp | 85 ++++ .../joint_trajectory_plugin.xml | 7 + joint_trajectory_controller/package.xml | 4 + .../src/cartesian_trajectory_generator.cpp | 434 ++++++++++++++++++ ...st_load_cartesian_trajectory_generator.cpp | 43 ++ 6 files changed, 605 insertions(+) create mode 100644 joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp create mode 100644 joint_trajectory_controller/src/cartesian_trajectory_generator.cpp create mode 100644 joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 7c4d8a6684..99771efde1 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS generate_parameter_library hardware_interface joint_limits + nav_msgs pluginlib rclcpp rclcpp_lifecycle @@ -20,6 +21,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rsl tl_expected trajectory_msgs + tf2 + tf2_geometry_msgs ) find_package(ament_cmake REQUIRED) @@ -50,6 +53,20 @@ ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCL # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") + +add_library(cartesian_trajectory_generator SHARED + src/cartesian_trajectory_generator.cpp +) +target_compile_features(cartesian_trajectory_generator PUBLIC cxx_std_17) +target_include_directories(cartesian_trajectory_generator PUBLIC + $ + $ +) +target_link_libraries(cartesian_trajectory_generator PUBLIC + joint_trajectory_controller +) +ament_target_dependencies(cartesian_trajectory_generator PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) if(BUILD_TESTING) @@ -87,6 +104,20 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) + + ament_add_gmock(test_load_cartesian_trajectory_generator + test/test_load_cartesian_trajectory_generator + ) + target_link_libraries(test_load_cartesian_trajectory_generator + cartesian_trajectory_generator + ) + ament_target_dependencies(test_load_cartesian_trajectory_generator + controller_manager + control_toolbox + realtime_tools + ros2_control_test_assets + ) + endif() @@ -95,6 +126,7 @@ install( DESTINATION include/joint_trajectory_controller ) install(TARGETS + cartesian_trajectory_generator joint_trajectory_controller joint_trajectory_controller_parameters EXPORT export_joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp new file mode 100644 index 0000000000..e7820caff3 --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -0,0 +1,85 @@ +// 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 JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/srv/reset_axis.hpp" +#include "control_msgs/srv/set_dof_limits.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace cartesian_trajectory_generator +{ +class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTrajectoryController +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + CartesianTrajectoryGenerator(); + + /** + * @brief command_interface_configuration This controller requires the position and velocity + * state interfaces for the controlled joints + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + using ControllerReferenceMsg = trajectory_msgs::msg::MultiDOFJointTrajectoryPoint; + using ControllerFeedbackMsg = nav_msgs::msg::Odometry; + using ControllerModeSrvType = control_msgs::srv::ResetAxis; + using SetLimitsModeSrvType = control_msgs::srv::SetDOFLimits; + +protected: + void read_state_from_hardware(JointTrajectoryPoint & state) override; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_reliable_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Subscription::SharedPtr feedback_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> feedback_; + + rclcpp::Service::SharedPtr set_joint_limits_service_; + +private: + void reference_callback(const std::shared_ptr msg); + + void set_joint_limits_service_callback( + const std::shared_ptr request, + std::shared_ptr response); + + std::vector configured_joint_limits_; +}; + +} // namespace cartesian_trajectory_generator + +#endif // JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ diff --git a/joint_trajectory_controller/joint_trajectory_plugin.xml b/joint_trajectory_controller/joint_trajectory_plugin.xml index ceed825f4c..9e179bfdcc 100644 --- a/joint_trajectory_controller/joint_trajectory_plugin.xml +++ b/joint_trajectory_controller/joint_trajectory_plugin.xml @@ -5,3 +5,10 @@ + + + + The cartesian trajectory generator executes cartesian-space trajectories on a set of input points. + + + diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 8cd2e5becc..7e189399e5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -18,6 +18,7 @@ control_toolbox generate_parameter_library hardware_interface + nav_msgs pluginlib rclcpp rclcpp_lifecycle @@ -25,6 +26,9 @@ rsl tl_expected trajectory_msgs + tf2 + tf2_geometry_msgs + ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp new file mode 100644 index 0000000000..4f6b15f031 --- /dev/null +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -0,0 +1,434 @@ +// 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. + +#include "joint_trajectory_controller/cartesian_trajectory_generator.hpp" + +#include "tf2/transform_datatypes.h" + +#include "controller_interface/helpers.hpp" +#include "joint_trajectory_controller/trajectory.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +void reset_twist_msg(geometry_msgs::msg::Twist & msg) +{ + msg.linear.x = std::numeric_limits::quiet_NaN(); + msg.linear.y = std::numeric_limits::quiet_NaN(); + msg.linear.z = std::numeric_limits::quiet_NaN(); + msg.angular.x = std::numeric_limits::quiet_NaN(); + msg.angular.y = std::numeric_limits::quiet_NaN(); + msg.angular.z = std::numeric_limits::quiet_NaN(); +} + +using ControllerReferenceMsg = + cartesian_trajectory_generator::CartesianTrajectoryGenerator::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg(ControllerReferenceMsg & msg) +{ + msg.transforms.resize(1); + msg.transforms[0].translation.x = std::numeric_limits::quiet_NaN(); + msg.transforms[0].translation.y = std::numeric_limits::quiet_NaN(); + msg.transforms[0].translation.z = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.x = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.y = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.z = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.w = std::numeric_limits::quiet_NaN(); + + msg.velocities.resize(1); + reset_twist_msg(msg.velocities[0]); + + msg.accelerations.resize(1); + reset_twist_msg(msg.accelerations[0]); +} + +void reset_controller_reference_msg(const std::shared_ptr & msg) +{ + reset_controller_reference_msg(*msg); +} + +using ControllerFeedbackMsg = + cartesian_trajectory_generator::CartesianTrajectoryGenerator::ControllerFeedbackMsg; + +// called from RT control loop +void reset_controller_feedback_msg(ControllerFeedbackMsg & msg) +{ + msg.pose.pose.position.x = std::numeric_limits::quiet_NaN(); + msg.pose.pose.position.y = std::numeric_limits::quiet_NaN(); + msg.pose.pose.position.z = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); + msg.pose.covariance.fill(std::numeric_limits::quiet_NaN()); + + reset_twist_msg(msg.twist.twist); + msg.twist.covariance.fill(std::numeric_limits::quiet_NaN()); +} +void reset_controller_feedback_msg(const std::shared_ptr & msg) +{ + reset_controller_feedback_msg(*msg); +} +} // namespace + +namespace cartesian_trajectory_generator +{ +CartesianTrajectoryGenerator::CartesianTrajectoryGenerator() +: joint_trajectory_controller::JointTrajectoryController() +{ +} + +controller_interface::InterfaceConfiguration +CartesianTrajectoryGenerator::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::NONE; + return conf; +} + +controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // store joint limits for later + configured_joint_limits_ = joint_limits_; + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + auto subscribers_reliable_qos = rclcpp::SystemDefaultsQoS(); + subscribers_reliable_qos.keep_all(); + subscribers_reliable_qos.reliable(); + + // Reference Subscribers (reliable channel also for updates not to be missed) + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); + ref_subscriber_reliable_ = get_node()->create_subscription( + "~/reference_reliable", subscribers_qos, + std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg); + input_ref_.writeFromNonRT(msg); + + // Odometry feedback + auto feedback_callback = [&](const std::shared_ptr msg) -> void + { feedback_.writeFromNonRT(msg); }; + feedback_subscriber_ = get_node()->create_subscription( + "~/feedback", subscribers_qos, feedback_callback); + std::shared_ptr feedback_msg = std::make_shared(); + reset_controller_feedback_msg(feedback_msg); + feedback_.writeFromNonRT(feedback_msg); + + // service QoS + auto services_qos = rclcpp::SystemDefaultsQoS(); // message queue depth + services_qos.keep_all(); + services_qos.reliable(); + services_qos.durability_volatile(); + + set_joint_limits_service_ = get_node()->create_service( + "~/set_joint_limits", + std::bind( + &CartesianTrajectoryGenerator::set_joint_limits_service_callback, this, std::placeholders::_1, + std::placeholders::_2), + services_qos); + + return CallbackReturn::SUCCESS; +} + +void CartesianTrajectoryGenerator::reference_callback( + const std::shared_ptr msg) +{ + // store input ref for later use + input_ref_.writeFromNonRT(msg); + + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, dof_); + read_state_from_hardware(state); + + // assume for now that we are working with trajectories with one point - we don't know exactly + // where we are in the trajectory before sampling - nevertheless this should work for the use case + auto new_traj_msg = std::make_shared(); + new_traj_msg->joint_names = params_.joints; + new_traj_msg->points.resize(1); + new_traj_msg->points[0].positions.resize( + params_.joints.size(), std::numeric_limits::quiet_NaN()); + new_traj_msg->points[0].velocities.resize( + params_.joints.size(), std::numeric_limits::quiet_NaN()); + new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_seconds(0.01); + + // just pass input into trajectory message + auto assign_value_from_input = [&]( + const double pos_from_msg, const double vel_from_msg, + const std::string & joint_name, const size_t index) + { + new_traj_msg->points[0].positions[index] = pos_from_msg; + new_traj_msg->points[0].velocities[index] = vel_from_msg; + if (std::isnan(pos_from_msg) && std::isnan(vel_from_msg)) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Input position and velocity for %s is NaN", joint_name.c_str()); + } + }; + + assign_value_from_input( + msg->transforms[0].translation.x, msg->velocities[0].linear.x, params_.joints[0], 0); + assign_value_from_input( + msg->transforms[0].translation.y, msg->velocities[0].linear.y, params_.joints[1], 1); + assign_value_from_input( + msg->transforms[0].translation.z, msg->velocities[0].linear.z, params_.joints[2], 2); + assign_value_from_input( + msg->transforms[0].rotation.x, msg->velocities[0].angular.x, params_.joints[3], 3); + assign_value_from_input( + msg->transforms[0].rotation.y, msg->velocities[0].angular.y, params_.joints[4], 4); + assign_value_from_input( + msg->transforms[0].rotation.z, msg->velocities[0].angular.z, params_.joints[5], 5); + + add_new_trajectory_msg(new_traj_msg); +} + +void CartesianTrajectoryGenerator::set_joint_limits_service_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + response->ok = true; + if (request->names.size() != request->limits.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Fields name and limits have size %zu and %zu. Their size should be equal. Ignoring " + "service call!", + request->names.size(), request->limits.size()); + response->ok = false; + } + + // start with current limits + auto new_joint_limits = joint_limits_; + + // lambda for setting new limit + auto update_limit_from_request = []( + double & new_limit_value, bool & new_limit_has_limits, + const double request_limit_value, + const double configured_limit_value) + { + // request is to reset to configured limit + if (std::isnan(request_limit_value)) + { + new_limit_value = configured_limit_value; + } + // new limit is requested + else + { + new_limit_value = request_limit_value; + } + new_limit_has_limits = !std::isnan(new_limit_value); + }; + + // lambda for setting new position limits + auto update_pos_limits_from_request = + []( + double & new_min_limit_value, double & new_max_limit_value, bool & new_limit_has_limits, + const double request_min_limit_value, const double request_max_limit_value, + const double configured_min_limit_value, const double configured_max_limit_value) + { + // request is to reset to configured min position limit + if (std::isnan(request_min_limit_value)) + { + new_min_limit_value = configured_min_limit_value; + } + // new min position limit is requested + else + { + new_min_limit_value = request_min_limit_value; + } + + // request is to reset to configured max position limit + if (std::isnan(request_max_limit_value)) + { + new_max_limit_value = configured_max_limit_value; + } + // new max position limit is requested + else + { + new_max_limit_value = request_max_limit_value; + } + + // has limits only if one of the min or max position limits is set + new_limit_has_limits = !std::isnan(new_min_limit_value) || !std::isnan(new_max_limit_value); + }; + + for (size_t i = 0; i < request->names.size(); ++i) + { + auto it = + std::find(command_joint_names_.begin(), command_joint_names_.end(), request->names[i]); + if (it != command_joint_names_.end()) + { + auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); + + update_pos_limits_from_request( + new_joint_limits[cmd_itf_index].min_position, new_joint_limits[cmd_itf_index].max_position, + new_joint_limits[cmd_itf_index].has_position_limits, request->limits[i].min_position, + request->limits[i].max_position, configured_joint_limits_[cmd_itf_index].min_position, + configured_joint_limits_[cmd_itf_index].max_position); + update_limit_from_request( + new_joint_limits[cmd_itf_index].max_velocity, + new_joint_limits[cmd_itf_index].has_velocity_limits, request->limits[i].max_velocity, + configured_joint_limits_[cmd_itf_index].max_velocity); + update_limit_from_request( + new_joint_limits[cmd_itf_index].max_acceleration, + new_joint_limits[cmd_itf_index].has_acceleration_limits, + request->limits[i].max_acceleration, + configured_joint_limits_[cmd_itf_index].max_acceleration); + update_limit_from_request( + new_joint_limits[cmd_itf_index].max_jerk, new_joint_limits[cmd_itf_index].has_jerk_limits, + request->limits[i].max_jerk, configured_joint_limits_[cmd_itf_index].max_jerk); + update_limit_from_request( + new_joint_limits[cmd_itf_index].max_effort, + new_joint_limits[cmd_itf_index].has_effort_limits, request->limits[i].max_effort, + configured_joint_limits_[cmd_itf_index].max_effort); + + RCLCPP_INFO( + get_node()->get_logger(), "New limits for joint %zu (%s) are: \n%s", cmd_itf_index, + command_joint_names_[cmd_itf_index].c_str(), + new_joint_limits[cmd_itf_index].to_string().c_str()); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), "Name '%s' is not command interface. Ignoring this entry.", + request->names[i].c_str()); + response->ok = false; + } + } + + // TODO(destogl): use buffers to sync comm between threads + joint_limits_ = new_joint_limits; +} + +controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( + const rclcpp_lifecycle::State &) +{ + // order all joints in the storage + for (const auto & interface : params_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + interface.c_str(), joint_command_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; + } + } + // for (const auto & interface : state_interface_types_) + // { + // auto it = + // std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + // auto index = std::distance(allowed_interface_types_.begin(), it); + // if (!controller_interface::get_ordered_interfaces( + // state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + // { + // RCLCPP_ERROR( + // get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, + // interface.c_str(), joint_state_interface_[index].size()); + // return controller_interface::CallbackReturn::ERROR; + // } + // } + + // Store 'home' pose + traj_msg_home_ptr_ = std::make_shared(); + traj_msg_home_ptr_->header.stamp.sec = 0; + traj_msg_home_ptr_->header.stamp.nanosec = 0; + traj_msg_home_ptr_->points.resize(1); + traj_msg_home_ptr_->points[0].time_from_start.sec = 0; + traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; + traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); + for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) + { + traj_msg_home_ptr_->points[0].positions[index] = + joint_state_interface_[0][index].get().get_value(); + } + + traj_external_point_ptr_ = std::make_shared(); + traj_home_point_ptr_ = std::make_shared(); + traj_msg_external_point_ptr_.writeFromNonRT( + std::shared_ptr()); + + subscriber_is_active_ = true; + traj_point_active_ptr_ = &traj_external_point_ptr_; + + // Initialize current state storage if hardware state has tracking offset + read_state_from_hardware(state_current_); + read_state_from_hardware(state_desired_); + read_state_from_hardware(last_commanded_state_); + // Handle restart of controller by reading from commands if + // those are not nan + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, dof_); + if (read_state_from_command_interfaces(state)) + { + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + } + + return CallbackReturn::SUCCESS; +} + +void CartesianTrajectoryGenerator::read_state_from_hardware(JointTrajectoryPoint & state) +{ + std::array orientation_angles; + const auto measured_state = *(feedback_.readFromRT()); + tf2::Quaternion measured_q; + tf2::fromMsg(measured_state->pose.pose.orientation, measured_q); + tf2::Matrix3x3 m(measured_q); + m.getRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); + + // Assign values from the hardware + // Position states always exist + state.positions[0] = measured_state->pose.pose.position.x; + state.positions[1] = measured_state->pose.pose.position.y; + state.positions[2] = measured_state->pose.pose.position.z; + state.positions[3] = orientation_angles[0]; + state.positions[4] = orientation_angles[1]; + state.positions[5] = orientation_angles[2]; + + state.velocities[0] = measured_state->twist.twist.linear.x; + state.velocities[1] = measured_state->twist.twist.linear.y; + state.velocities[2] = measured_state->twist.twist.linear.z; + state.velocities[3] = measured_state->twist.twist.angular.x; + state.velocities[4] = measured_state->twist.twist.angular.y; + state.velocities[5] = measured_state->twist.twist.angular.z; + + state.accelerations.clear(); +} +} // namespace cartesian_trajectory_generator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_trajectory_generator::CartesianTrajectoryGenerator, + controller_interface::ControllerInterface) diff --git a/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp b/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp new file mode 100644 index 0000000000..84454b57f7 --- /dev/null +++ b/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.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. + +#include + +#include "gmock/gmock.h" + +#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(TestLoadJointStateController, 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_cartesian_trajectory_generator", + "trajectory_interpolators/CartesianTrajectoryGenerator")); + + rclcpp::shutdown(); +} From 133de371d01675cb8679646e5658dde9c3a3be1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 6 Apr 2023 17:20:14 +0200 Subject: [PATCH 31/63] Add reseting of individual DoFs to state values in JTC when using in open_loop_mode. --- .../cartesian_trajectory_generator.hpp | 2 - .../joint_trajectory_controller.hpp | 6 ++ joint_trajectory_controller/package.xml | 1 + .../src/joint_trajectory_controller.cpp | 60 +++++++++++++++++++ 4 files changed, 67 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp index e7820caff3..0b8dde5dfe 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -20,7 +20,6 @@ #include #include -#include "control_msgs/srv/reset_axis.hpp" #include "control_msgs/srv/set_dof_limits.hpp" #include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -54,7 +53,6 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr using ControllerReferenceMsg = trajectory_msgs::msg::MultiDOFJointTrajectoryPoint; using ControllerFeedbackMsg = nav_msgs::msg::Odometry; - using ControllerModeSrvType = control_msgs::srv::ResetAxis; using SetLimitsModeSrvType = control_msgs::srv::SetDOFLimits; protected: 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 27c90538fa..e6a5159647 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 @@ -24,6 +24,7 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" +#include "control_msgs/srv/reset_dofs.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -195,6 +196,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + using ControllerResetDofsSrvType = control_msgs::srv::ResetDofs; + + realtime_tools::RealtimeBuffer> reset_dofs_flags_; + rclcpp::Service::SharedPtr reset_dofs_service_; + std::vector joint_limits_; // callback for topic interface diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 7e189399e5..78606f2a28 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -18,6 +18,7 @@ control_toolbox generate_parameter_library hardware_interface + joint_limits nav_msgs pluginlib rclcpp diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b0e93b6ecd..a1b98c690c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -153,6 +153,25 @@ controller_interface::return_type JointTrajectoryController::update( first_sample = true; if (params_.open_loop_control) { + auto reset_flags = reset_dofs_flags_.readFromRT(); + for (size_t i = 0; i < dof_; ++i) + { + if (reset_flags->at(i)) + { + last_commanded_state_.positions[i] = state_current_.positions[i]; + if (has_velocity_state_interface_) + { + last_commanded_state_.velocities[i] = state_current_.velocities[i]; + } + if (has_acceleration_state_interface_) + { + last_commanded_state_.accelerations[i] = state_current_.accelerations[i]; + } + + reset_flags->at(i) = false; // reset flag in the buffer for one-shot execution + } + } + traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_); } else @@ -840,6 +859,47 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + // Initialize memory and RT buffer for DoF reset flags + std::vector reset_flags; + reset_flags.resize(dof_, false); + reset_dofs_flags_.writeFromNonRT(reset_flags); + + // Control mode service + auto reset_dofs_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + response->ok = true; + + std::vector reset_flags; + reset_flags.resize(dof_, false); + + for (size_t i = 0; i < request->names.size(); ++i) + { + auto it = + std::find(command_joint_names_.begin(), command_joint_names_.end(), request->names[i]); + if (it != command_joint_names_.end()) + { + RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'.", request->names[i].c_str()); + auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); + reset_flags[cmd_itf_index] = true; + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), "Name '%s' is not command interface. Ignoring this entry.", + request->names[i].c_str()); + response->ok = false; + } + } + + reset_dofs_flags_.writeFromNonRT(reset_flags); + }; + + reset_dofs_service_ = get_node()->create_service( + "~/reset_dofs", reset_dofs_service_callback); + return CallbackReturn::SUCCESS; } From 45443c549a7c2aea31566320db01d18aa97397d6 Mon Sep 17 00:00:00 2001 From: Bijou Date: Fri, 7 Apr 2023 09:04:29 -0700 Subject: [PATCH 32/63] Fix missing .cpp extension in CMakeLists for CTG --- joint_trajectory_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 99771efde1..1006e6bc7a 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -106,7 +106,7 @@ if(BUILD_TESTING) ) ament_add_gmock(test_load_cartesian_trajectory_generator - test/test_load_cartesian_trajectory_generator + test/test_load_cartesian_trajectory_generator.cpp ) target_link_libraries(test_load_cartesian_trajectory_generator cartesian_trajectory_generator From 484e0f6661717a3a91c052f71b744874ad10dea3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 25 Jul 2022 17:38:02 +0200 Subject: [PATCH 33/63] Added structures for joint limits. Small fixes for JTC. Enalbe definition of all fields in JointTrajectory message when using test node. Use more command_joint_names instead of joint_names. Debugging Rucking smoothing. Updated files to follow joint limits Use velocity ration for its reduction. Fixup integration in trajectories with non 0 velocity at the end. Solved jumps at the end of a trajectory. Add debuging output to JTC with Ruckig. Enable disabling closed-loop PID in JTC when velocity- and effort-only interfaces are used. --- .../trajectory.hpp | 5 +- joint_trajectory_controller/package.xml | 1 - .../src/joint_trajectory_controller.cpp | 80 +++++++++++++++++-- .../src/trajectory.cpp | 15 ++-- .../test/test_trajectory.cpp | 1 + .../publisher_joint_trajectory_controller.py | 31 ++++--- 6 files changed, 109 insertions(+), 24 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 3bd4873a31..a6f924ee7e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -18,11 +18,13 @@ #include #include +#include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/time.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" + namespace joint_trajectory_controller { using TrajectoryPointIter = std::vector::iterator; @@ -89,7 +91,8 @@ class Trajectory const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const rclcpp::Duration & period, const std::vector & joint_limits); /** * Do interpolation between 2 states given a time in between their respective timestamps diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 78606f2a28..1a2c8e68f0 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -30,7 +30,6 @@ tf2 tf2_geometry_msgs - ament_cmake_gmock controller_manager hardware_interface_testing diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a1b98c690c..f5a5e8847e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -27,9 +27,9 @@ #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits_rosparam.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" @@ -136,7 +136,7 @@ controller_interface::return_type JointTrajectoryController::update( fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); + traj_external_point_ptr_->update(*new_external_msg, joint_limits_, period); } // current state update @@ -151,6 +151,7 @@ controller_interface::return_type JointTrajectoryController::update( if (!traj_external_point_ptr_->is_sampled_already()) { first_sample = true; + if (params_.open_loop_control) { auto reset_flags = reset_dofs_flags_.readFromRT(); @@ -183,7 +184,8 @@ controller_interface::return_type JointTrajectoryController::update( // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; const bool valid_point = traj_external_point_ptr_->sample( - time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr, period, + joint_limits_); if (valid_point) { @@ -387,7 +389,9 @@ controller_interface::return_type JointTrajectoryController::update( } } - publish_state(time, state_desired_, state_current_, state_error_); + publish_state( + time, state_desired_, state_current_, state_error_, splines_state_, ruckig_state_, + ruckig_input_state_); return controller_interface::return_type::OK; } @@ -591,9 +595,10 @@ void JointTrajectoryController::query_state_service( if (has_active_trajectory()) { TrajectoryPointConstIter start_segment_itr, end_segment_itr; + const rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01); response->success = traj_external_point_ptr_->sample( static_cast(request->time), interpolation_method_, state_requested, - start_segment_itr, end_segment_itr); + start_segment_itr, end_segment_itr, period, joint_limits_); // If the requested sample time precedes the trajectory finish time respond as failure if (response->success) { @@ -685,7 +690,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( has_effort_command_interface_ = contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); - // if there is only velocity or if there is effort command interface // then use also PID adapter use_closed_loop_pid_adapter_ = (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && @@ -709,6 +713,19 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( joints_angle_wraparound_[i] = gains.angle_wraparound; } + // Initialize joint limits + joint_limits_.resize(dof_); + for (size_t i = 0; i < joint_limits_.size(); ++i) + { + if (joint_limits::declare_parameters(command_joint_names_[i], get_node())) + { + joint_limits::get_joint_limits(command_joint_names_[i], get_node(), joint_limits_[i]); + RCLCPP_INFO( + get_node()->get_logger(), "Limits for joint %zu (%s) are: \n%s", i, + command_joint_names_[i].c_str(), joint_limits_[i].to_string().c_str()); + } + } + if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); @@ -740,6 +757,16 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "'position' state interfaces are present"); return CallbackReturn::FAILURE; } + if ( + has_acceleration_command_interface_ && + (!has_velocity_command_interface_ || !has_position_command_interface_)) + { + RCLCPP_ERROR( + logger, + "'acceleration' command interface can only be used if 'velocity' and " + "'position' command interfaces are present"); + return CallbackReturn::FAILURE; + } // effort is always used alone so no need for size check if ( @@ -1093,7 +1120,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( void JointTrajectoryController::publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, - const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, + const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, + const JointTrajectoryPoint & ruckig_input) { if (state_publisher_->trylock()) { @@ -1120,6 +1149,39 @@ void JointTrajectoryController::publish_state( state_publisher_->unlockAndPublish(); } + + if (splines_output_publisher_ && splines_output_publisher_->trylock()) + { + splines_output_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp; + splines_output_publisher_->msg_.actual.positions = splines_output.positions; + splines_output_publisher_->msg_.actual.velocities = splines_output.velocities; + splines_output_publisher_->msg_.actual.accelerations = splines_output.accelerations; + splines_output_publisher_->msg_.actual.effort = splines_output.effort; + + splines_output_publisher_->unlockAndPublish(); + } + + if (ruckig_input_publisher_ && ruckig_input_publisher_->trylock()) + { + ruckig_input_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp; + ruckig_input_publisher_->msg_.actual.positions = ruckig_input.positions; + ruckig_input_publisher_->msg_.actual.velocities = ruckig_input.velocities; + ruckig_input_publisher_->msg_.actual.accelerations = ruckig_input.accelerations; + ruckig_input_publisher_->msg_.actual.effort = ruckig_input.effort; + + ruckig_input_publisher_->unlockAndPublish(); + } + + if (ruckig_input_target_publisher_ && ruckig_input_target_publisher_->trylock()) + { + ruckig_input_target_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp; + ruckig_input_target_publisher_->msg_.actual.positions = ruckig_input_target.positions; + ruckig_input_target_publisher_->msg_.actual.velocities = ruckig_input_target.velocities; + ruckig_input_target_publisher_->msg_.actual.accelerations = ruckig_input_target.accelerations; + ruckig_input_target_publisher_->msg_.actual.effort = ruckig_input_target.effort; + + ruckig_input_target_publisher_->unlockAndPublish(); + } } void JointTrajectoryController::topic_callback( @@ -1319,7 +1381,9 @@ void JointTrajectoryController::sort_to_local_joint_order( if (to_remap.size() != mapping.size()) { RCLCPP_WARN( - get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + get_node()->get_logger(), + "Invalid input size for sorting. Values have size %zu and mapping size %zu", + to_remap.size(), mapping.size()); return to_remap; } static std::vector output(dof_, 0.0); diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index a1fc2d105e..a04f295ad3 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -23,11 +23,15 @@ namespace joint_trajectory_controller { -Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} +Trajectory::Trajectory() +: trajectory_start_time_(0), time_before_traj_msg_(0), sampled_already_(false), +{ +} Trajectory::Trajectory(std::shared_ptr joint_trajectory) : trajectory_msg_(joint_trajectory), - trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) + trajectory_start_time_(static_cast(joint_trajectory->header.stamp)), + sampled_already_(false), { } @@ -61,7 +65,8 @@ bool Trajectory::sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const rclcpp::Duration & period, const std::vector & joint_limits) { THROW_ON_NULLPTR(trajectory_msg_) @@ -89,6 +94,7 @@ bool Trajectory::sample( return false; } + // TODO(anyone): this shouldn't be initialized at runtime output_state = trajectory_msgs::msg::JointTrajectoryPoint(); auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Time first_point_timestamp = @@ -139,8 +145,7 @@ bool Trajectory::sample( else { // it changes points only if position and velocity do not exist, but their derivatives - deduce_from_derivatives( - point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds()); + deduce_from_derivatives(point, next_point, point.positions.size(), (t1 - t0).seconds()); interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); } diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index b52aa67a04..d60e5c696d 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -51,6 +51,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + // traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, output_point, start, end, ); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index cb66f58468..c038ab8f5b 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -16,8 +16,8 @@ # import rclpy +from rclpy.duration import Duration from rclpy.node import Node -from builtin_interfaces.msg import Duration from rcl_interfaces.msg import ParameterDescriptor from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint @@ -29,7 +29,9 @@ def __init__(self): super().__init__("publisher_position_trajectory_controller") # Declare all parameters self.declare_parameter("controller_name", "position_trajectory_controller") - self.declare_parameter("wait_sec_between_publish", 6) + self.declare_parameter("wait_sec_between_publish", 6.0) + self.declare_parameter("repeat_the_same_goal", 1) + self.declare_parameter("goal_time_from_start", 4.0) self.declare_parameter("goal_names", ["pos1", "pos2"]) self.declare_parameter("joints", [""]) self.declare_parameter("check_starting_point", False) @@ -37,6 +39,8 @@ def __init__(self): # Read parameters controller_name = self.get_parameter("controller_name").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + self.repeat_the_same_goal = self.get_parameter("repeat_the_same_goal").value + goal_time_from_start = Duration(seconds=self.get_parameter("goal_time_from_start").value) goal_names = self.get_parameter("goal_names").value self.joints = self.get_parameter("joints").value self.check_starting_point = self.get_parameter("check_starting_point").value @@ -73,9 +77,13 @@ def __init__(self): def get_sub_param(sub_param): param_name = name + "." + sub_param - self.declare_parameter(param_name, [float()]) + # use default value different than 0.0 + self.declare_parameter(param_name, [float(1234567890)]) param_value = self.get_parameter(param_name).value + if len(param_value) == 1 and param_value[0] == 1234567890: + param_value.clear() + float_values = [] if len(param_value) != len(self.joints): @@ -108,7 +116,7 @@ def get_sub_param(sub_param): one_ok = True if one_ok: - point.time_from_start = Duration(sec=4) + point.time_from_start = goal_time_from_start.to_msg() self.goals.append(point) self.get_logger().info(f'Goal "{name}" has definition {point}') @@ -135,21 +143,26 @@ def get_sub_param(sub_param): self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) - self.i = 0 + self.current_goal = 0 + self.repetition = 0 def timer_callback(self): if self.starting_point_ok: - self.get_logger().info(f"Sending goal {self.goals[self.i]}.") + self.get_logger().info(f"Sending goal {self.goals[self.current_goal]}.") traj = JointTrajectory() traj.joint_names = self.joints - traj.points.append(self.goals[self.i]) + traj.points.append(self.goals[self.current_goal]) self.publisher_.publish(traj) - self.i += 1 - self.i %= len(self.goals) + self.repetition += 1 + self.repetition %= self.repeat_the_same_goal + + if self.repetition == 0: + self.current_goal += 1 + self.current_goal %= len(self.goals) elif self.check_starting_point and not self.joint_state_msg_received: self.get_logger().warn( From 94c4c2c8ff9014f9575408cc948b40ec0a8ed53b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 4 May 2023 17:56:38 +0200 Subject: [PATCH 34/63] Integrate SimpleJointLimiter in to JTC. --- .../joint_trajectory_controller.hpp | 10 ++- .../trajectory.hpp | 5 +- .../src/joint_trajectory_controller.cpp | 35 +++++---- ...oint_trajectory_controller_parameters.yaml | 8 ++ .../src/trajectory.cpp | 76 ++++++++++++++++--- 5 files changed, 109 insertions(+), 25 deletions(-) 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 e6a5159647..b77db585e2 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 @@ -28,11 +28,13 @@ #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limiter_interface.hpp" #include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" +#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -166,6 +168,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa double cmd_timeout_; // True if holding position or repeating last trajectory point in case of success realtime_tools::RealtimeBuffer rt_is_holding_; + + // joint limiter configuration for JTC + using JointLimiter = joint_limits::JointLimiterInterface; + std::shared_ptr> joint_limiter_loader_; + std::unique_ptr joint_limiter_; + // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = @@ -201,8 +209,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa realtime_tools::RealtimeBuffer> reset_dofs_flags_; rclcpp::Service::SharedPtr reset_dofs_service_; - std::vector joint_limits_; - // callback for topic interface JOINT_TRAJECTORY_CONTROLLER_PUBLIC void topic_callback(const std::shared_ptr msg); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index a6f924ee7e..c59fc03d75 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -18,6 +18,7 @@ #include #include +#include "joint_limits/joint_limiter_interface.hpp" #include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/visibility_control.h" @@ -92,7 +93,9 @@ class Trajectory const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, - const rclcpp::Duration & period, const std::vector & joint_limits); + const rclcpp::Duration & period, + std::unique_ptr> & + joint_limiter); /** * Do interpolation between 2 states given a time in between their respective timestamps diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f5a5e8847e..748472f1c8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -27,7 +27,6 @@ #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_limits/joint_limits_rosparam.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/logging.hpp" @@ -51,6 +50,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); + + joint_limiter_loader_ = std::make_shared>( + "joint_limits", "joint_limits::JointLimiterInterface"); + RCLCPP_DEBUG(get_node()->get_logger(), "Available joint limiter classes:"); + for (const auto & available_class : joint_limiter_loader_->getDeclaredClasses()) + { + RCLCPP_DEBUG(get_node()->get_logger(), " %s", available_class.c_str()); + } } catch (const std::exception & e) { @@ -136,7 +143,7 @@ controller_interface::return_type JointTrajectoryController::update( fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg, joint_limits_, period); + traj_external_point_ptr_->update(*new_external_msg); } // current state update @@ -185,7 +192,7 @@ controller_interface::return_type JointTrajectoryController::update( TrajectoryPointConstIter start_segment_itr, end_segment_itr; const bool valid_point = traj_external_point_ptr_->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr, period, - joint_limits_); + joint_limiter_); if (valid_point) { @@ -598,7 +605,7 @@ void JointTrajectoryController::query_state_service( const rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01); response->success = traj_external_point_ptr_->sample( static_cast(request->time), interpolation_method_, state_requested, - start_segment_itr, end_segment_itr, period, joint_limits_); + start_segment_itr, end_segment_itr, period, joint_limiter_); // If the requested sample time precedes the trajectory finish time respond as failure if (response->success) { @@ -714,16 +721,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } // Initialize joint limits - joint_limits_.resize(dof_); - for (size_t i = 0; i < joint_limits_.size(); ++i) + if (!params_.joint_limiter_type.empty()) { - if (joint_limits::declare_parameters(command_joint_names_[i], get_node())) - { - joint_limits::get_joint_limits(command_joint_names_[i], get_node(), joint_limits_[i]); - RCLCPP_INFO( - get_node()->get_logger(), "Limits for joint %zu (%s) are: \n%s", i, - command_joint_names_[i].c_str(), joint_limits_[i].to_string().c_str()); - } + RCLCPP_INFO( + get_node()->get_logger(), "Using joint limiter plugin: '%s'", + params_.joint_limiter_type.c_str()); + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_->createUnmanagedInstance(params_.joint_limiter_type)); + joint_limiter_->init(command_joint_names_, get_node()); + } + else + { + RCLCPP_INFO(get_node()->get_logger(), "Not using joint limiter plugin as none defined."); } if (params_.state_interfaces.empty()) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index ded5bb7ca3..b93ec99940 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -157,3 +157,11 @@ joint_trajectory_controller: default_value: 0.0, description: "Per-joint trajectory offset tolerance at the goal position.", } + + joint_limiter_type: { + type: string, + default_value: "joint_limits/SimpleJointLimiter", + description: "The type of joint limiter to use. If empty, no joint limiter will be used. + Per default, the simple joint limiter implementation from 'joint_limits' package is used. + For other official implementations see 'joint_limiters' package in 'ros2_control' repository.", + } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index a04f295ad3..6a65ba4c23 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -66,7 +66,8 @@ bool Trajectory::sample( const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, - const rclcpp::Duration & period, const std::vector & joint_limits) + const rclcpp::Duration & period, + std::unique_ptr> & joint_limiter) { THROW_ON_NULLPTR(trajectory_msg_) @@ -121,6 +122,12 @@ bool Trajectory::sample( } start_segment_itr = begin(); // no segments before the first end_segment_itr = begin(); + + if (joint_limiter) + { + joint_limiter->enforce(state_before_traj_msg_, output_state, period); + } + previous_state_ = output_state; return true; } @@ -151,23 +158,74 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); + + if (joint_limiter) + { + joint_limiter->enforce(previous_state_, output_state, period); + } + previous_state_ = output_state; return true; } } - // whole animation has played out - start_segment_itr = --end(); - end_segment_itr = end(); - output_state = (*start_segment_itr); + // whole animation has played out - but still continue s interpolating and smoothing + auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1]; + const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; + + // FIXME(destogl): this is from backport - check if needed + // // whole animation has played out + // start_segment_itr = --end(); + // end_segment_itr = end(); + // expected_state = (*start_segment_itr); + // + // // TODO: Add and test enforceJointLimits? Unsure if needed for end of animation + // // Yes, call enforceJointLimits to handle halting in servo, which has time_from_start == 1ns + // (does not enforce vel/acc limits) if(last_idx == 0) { + // // Enforce limits from current state, not the trajectory's single point, because the point + // from servo halting violates limits if (joint_limiter) + // { + // joint_limiter->enforce( + // state_before_traj_msg_, expected_state, (sample_time - time_before_traj_msg_)); + // } + // } + + const size_t dim = last_point.positions.size(); + // the trajectories in msg may have empty velocities/accel, so resize them - if (output_state.velocities.empty()) + if (last_point.velocities.empty()) { - output_state.velocities.resize(output_state.positions.size(), 0.0); + last_point.velocities.resize(dim, 0.0); } - if (output_state.accelerations.empty()) + if (last_point.accelerations.empty()) { - output_state.accelerations.resize(output_state.positions.size(), 0.0); + last_point.accelerations.resize(dim, 0.0); } + + // integrate velocities and positions because acc and vel don't have to be 0 between points + for (size_t dim_i = 0; dim_i < dim; ++dim_i) + { + if (last_point.accelerations[dim_i] != 0) + { + last_point.velocities[dim_i] += last_point.accelerations[dim_i] * period.seconds(); + // remember velocity over multiple calls + } + if (last_point.velocities[dim_i] != 0) + { + last_point.positions[dim_i] += last_point.velocities[dim_i] * period.seconds(); + // remember velocity over multiple calls + } + } + + // do not do splines when trajectory has finished because the time is achieved + interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state) + + if (joint_limiter) + { + // TODO(destogl): use here output state + joint_limiter->enforce(previous_state_, output_state, period); + } + previous_state_ = output_state; + return true; } From 79039e247e569fb787691fe6cc8c4eebaabf5153 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 4 May 2023 18:58:42 +0200 Subject: [PATCH 35/63] Adjust default value of limiters. --- .../src/joint_trajectory_controller_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index b93ec99940..167a9cfb2e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -160,7 +160,7 @@ joint_trajectory_controller: joint_limiter_type: { type: string, - default_value: "joint_limits/SimpleJointLimiter", + default_value: "", description: "The type of joint limiter to use. If empty, no joint limiter will be used. Per default, the simple joint limiter implementation from 'joint_limits' package is used. For other official implementations see 'joint_limiters' package in 'ros2_control' repository.", From 59c542f989ac4cc2234b94dba7d9b231a911e191 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 6 Dec 2023 14:10:08 +0100 Subject: [PATCH 36/63] JTC extending for overriding. --- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 9 deletions(-) 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 b77db585e2..b5cb5a299d 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 @@ -285,7 +285,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - virtual void read_state_from_state_interfaces(JointTrajectoryPoint & state); + virtual bool read_state_from_state_interfaces(JointTrajectoryPoint & state); /** Assign values from the command interfaces as state. * This is only possible if command AND state interfaces exist for the same type, diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 748472f1c8..1476881718 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -148,7 +148,8 @@ controller_interface::return_type JointTrajectoryController::update( // current state update state_current_.time_from_start.set__sec(0); - read_state_from_state_interfaces(state_current_); + if (!read_state_from_state_interfaces(state_current_)) + return controller_interface::return_type::OK; // currently carrying out a trajectory if (has_active_trajectory()) @@ -402,7 +403,7 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } -void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) +bool JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { auto assign_point_from_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) @@ -437,6 +438,7 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory state.velocities.clear(); state.accelerations.clear(); } + return true; } bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) @@ -991,17 +993,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); + if (!read_state_from_state_interfaces(state)) return CallbackReturn::ERROR; + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + + // Handle restart of controller by reading from commands if those are not NaN if (read_state_from_command_interfaces(state)) { state_current_ = state; + state_desired_ = state; last_commanded_state_ = state; } - else - { - // Initialize current state storage from hardware - read_state_from_state_interfaces(state_current_); - read_state_from_state_interfaces(last_commanded_state_); - } // The controller should start by holding position at the beginning of active state add_new_trajectory_msg(set_hold_position()); From 3aaf3889a0b2a7196504cfa67d47d2d9601e5de4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 6 Dec 2023 14:10:39 +0100 Subject: [PATCH 37/63] Adjust method names to the JTC updates and manage errors in feedback values. --- .../cartesian_trajectory_generator.hpp | 2 +- .../src/cartesian_trajectory_generator.cpp | 23 +++++++++---------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp index 0b8dde5dfe..794101e515 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -56,7 +56,7 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr using SetLimitsModeSrvType = control_msgs::srv::SetDOFLimits; protected: - void read_state_from_hardware(JointTrajectoryPoint & state) override; + bool read_state_from_state_interfaces(JointTrajectoryPoint & state) override; // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index 4f6b15f031..c065f5f5b3 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -137,8 +137,8 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( feedback_subscriber_ = get_node()->create_subscription( "~/feedback", subscribers_qos, feedback_callback); std::shared_ptr feedback_msg = std::make_shared(); - reset_controller_feedback_msg(feedback_msg); - feedback_.writeFromNonRT(feedback_msg); + // initialize feedback to null pointer since it is used to determine if we have valid data or not + feedback_.writeFromNonRT(nullptr); // service QoS auto services_qos = rclcpp::SystemDefaultsQoS(); // message queue depth @@ -162,10 +162,6 @@ void CartesianTrajectoryGenerator::reference_callback( // store input ref for later use input_ref_.writeFromNonRT(msg); - trajectory_msgs::msg::JointTrajectoryPoint state; - resize_joint_trajectory_point(state, dof_); - read_state_from_hardware(state); - // assume for now that we are working with trajectories with one point - we don't know exactly // where we are in the trajectory before sampling - nevertheless this should work for the use case auto new_traj_msg = std::make_shared(); @@ -381,13 +377,14 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( traj_point_active_ptr_ = &traj_external_point_ptr_; // Initialize current state storage if hardware state has tracking offset - read_state_from_hardware(state_current_); - read_state_from_hardware(state_desired_); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading from commands if - // those are not nan trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); + if (!read_state_from_state_interfaces(state)) return CallbackReturn::ERROR; + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + // Handle restart of controller by reading from commands if + // those are not nan if (read_state_from_command_interfaces(state)) { state_current_ = state; @@ -398,10 +395,11 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( return CallbackReturn::SUCCESS; } -void CartesianTrajectoryGenerator::read_state_from_hardware(JointTrajectoryPoint & state) +bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTrajectoryPoint & state) { std::array orientation_angles; const auto measured_state = *(feedback_.readFromRT()); + if (!measured_state) return false; tf2::Quaternion measured_q; tf2::fromMsg(measured_state->pose.pose.orientation, measured_q); tf2::Matrix3x3 m(measured_q); @@ -424,6 +422,7 @@ void CartesianTrajectoryGenerator::read_state_from_hardware(JointTrajectoryPoint state.velocities[5] = measured_state->twist.twist.angular.z; state.accelerations.clear(); + return true; } } // namespace cartesian_trajectory_generator From 27dcfb817bf1b0aa6fb799677f1786190f1ce58b Mon Sep 17 00:00:00 2001 From: Bijou Date: Mon, 8 May 2023 14:30:12 -0700 Subject: [PATCH 38/63] Remove unused variable --- .../src/cartesian_trajectory_generator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index c065f5f5b3..e48c266da8 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -136,7 +136,6 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( { feedback_.writeFromNonRT(msg); }; feedback_subscriber_ = get_node()->create_subscription( "~/feedback", subscribers_qos, feedback_callback); - std::shared_ptr feedback_msg = std::make_shared(); // initialize feedback to null pointer since it is used to determine if we have valid data or not feedback_.writeFromNonRT(nullptr); From fdb6e35e0cef1f93854362be39dbdb27854732b9 Mon Sep 17 00:00:00 2001 From: Bijou Date: Fri, 12 May 2023 14:01:21 -0700 Subject: [PATCH 39/63] Remove deprecated rclcpp/qos_event.hpp --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1476881718..4626a0186c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -29,6 +29,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" From 5ba276e78b7eba87a5755498b3352b753ad92109 Mon Sep 17 00:00:00 2001 From: Bijou Date: Fri, 12 May 2023 14:01:46 -0700 Subject: [PATCH 40/63] Remove unused free functions for resetting controlled feedback messages --- .../src/cartesian_trajectory_generator.cpp | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index e48c266da8..c9f30bf93c 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -59,29 +59,6 @@ void reset_controller_reference_msg(const std::shared_ptr::quiet_NaN(); - msg.pose.pose.position.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.position.z = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); - msg.pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); - msg.pose.covariance.fill(std::numeric_limits::quiet_NaN()); - - reset_twist_msg(msg.twist.twist); - msg.twist.covariance.fill(std::numeric_limits::quiet_NaN()); -} -void reset_controller_feedback_msg(const std::shared_ptr & msg) -{ - reset_controller_feedback_msg(*msg); -} } // namespace namespace cartesian_trajectory_generator From b90c3e3d0e1d74067cdf7cedbf9c65ab6c181cb4 Mon Sep 17 00:00:00 2001 From: bijoua Date: Tue, 29 Aug 2023 19:23:47 -0700 Subject: [PATCH 41/63] Use time from start passed in from input reference if non-zero in CTG --- .../src/cartesian_trajectory_generator.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index c9f30bf93c..fb0f8d422d 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -147,7 +147,15 @@ void CartesianTrajectoryGenerator::reference_callback( params_.joints.size(), std::numeric_limits::quiet_NaN()); new_traj_msg->points[0].velocities.resize( params_.joints.size(), std::numeric_limits::quiet_NaN()); - new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_seconds(0.01); + if (msg->time_from_start.nanosec == 0) + { + new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_seconds(0.01); + } + else + { + new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_nanoseconds( + static_cast(msg->time_from_start.nanosec)); + } // just pass input into trajectory message auto assign_value_from_input = [&]( From e6fe00b5021cebc17b0696c0ba4822a5f91a7417 Mon Sep 17 00:00:00 2001 From: Bijou Date: Tue, 5 Sep 2023 12:33:57 -0700 Subject: [PATCH 42/63] Modify CTG to expect twist in measured state input to be in body frame --- .../src/cartesian_trajectory_generator.cpp | 29 +++++++++++++++---- 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index fb0f8d422d..faa2ad4035 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -14,6 +14,7 @@ #include "joint_trajectory_controller/cartesian_trajectory_generator.hpp" +#include "eigen3/Eigen/Eigen" #include "tf2/transform_datatypes.h" #include "controller_interface/helpers.hpp" @@ -398,12 +399,28 @@ bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTraject state.positions[4] = orientation_angles[1]; state.positions[5] = orientation_angles[2]; - state.velocities[0] = measured_state->twist.twist.linear.x; - state.velocities[1] = measured_state->twist.twist.linear.y; - state.velocities[2] = measured_state->twist.twist.linear.z; - state.velocities[3] = measured_state->twist.twist.angular.x; - state.velocities[4] = measured_state->twist.twist.angular.y; - state.velocities[5] = measured_state->twist.twist.angular.z; + ////// Convert measured twist which is in body frame to world frame since CTG/JTC expects state in world frame + + Eigen::Quaterniond q_body_in_world( + measured_state->pose.pose.orientation.w, measured_state->pose.pose.orientation.x, + measured_state->pose.pose.orientation.y, measured_state->pose.pose.orientation.z); + + Eigen::Vector3d linear_vel_body( + measured_state->twist.twist.linear.x, measured_state->twist.twist.linear.y, + measured_state->twist.twist.linear.z); + auto linear_vel_world = q_body_in_world * linear_vel_body; + + Eigen::Vector3d angular_vel_body( + measured_state->twist.twist.angular.x, measured_state->twist.twist.angular.y, + measured_state->twist.twist.angular.z); + auto angular_vel_world = q_body_in_world * angular_vel_body; + + state.velocities[0] = linear_vel_world[0]; + state.velocities[1] = linear_vel_world[1]; + state.velocities[2] = linear_vel_world[2]; + state.velocities[3] = angular_vel_world[0]; + state.velocities[4] = angular_vel_world[1]; + state.velocities[5] = angular_vel_world[2]; state.accelerations.clear(); return true; From 177a2428246db7139c7017a45be0bcb4f3b9db1f Mon Sep 17 00:00:00 2001 From: bijoua Date: Tue, 12 Sep 2023 12:06:11 -0700 Subject: [PATCH 43/63] Handle NaNs in measured state in CTG --- .../src/cartesian_trajectory_generator.cpp | 45 ++++++++++++++----- 1 file changed, 35 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index faa2ad4035..042caf3b00 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -385,16 +385,37 @@ bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTraject std::array orientation_angles; const auto measured_state = *(feedback_.readFromRT()); if (!measured_state) return false; + tf2::Quaternion measured_q; - tf2::fromMsg(measured_state->pose.pose.orientation, measured_q); + + if ( + std::isnan(measured_state->pose.pose.orientation.w) || + std::isnan(measured_state->pose.pose.orientation.x) || + std::isnan(measured_state->pose.pose.orientation.y) || + std::isnan(measured_state->pose.pose.orientation.z)) + { + // if any of the orientation is NaN, revert to previous orientation + measured_q.setRPY(state.positions[3], state.positions[4], state.positions[5]); + } + else + { + tf2::fromMsg(measured_state->pose.pose.orientation, measured_q); + } tf2::Matrix3x3 m(measured_q); m.getRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); // Assign values from the hardware // Position states always exist - state.positions[0] = measured_state->pose.pose.position.x; - state.positions[1] = measured_state->pose.pose.position.y; - state.positions[2] = measured_state->pose.pose.position.z; + // if any measured position is NaN, keep previous value + state.positions[0] = std::isnan(measured_state->pose.pose.position.x) + ? state.positions[0] + : measured_state->pose.pose.position.x; + state.positions[1] = std::isnan(measured_state->pose.pose.position.y) + ? state.positions[1] + : measured_state->pose.pose.position.y; + state.positions[2] = std::isnan(measured_state->pose.pose.position.z) + ? state.positions[2] + : measured_state->pose.pose.position.z; state.positions[3] = orientation_angles[0]; state.positions[4] = orientation_angles[1]; state.positions[5] = orientation_angles[2]; @@ -402,17 +423,21 @@ bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTraject ////// Convert measured twist which is in body frame to world frame since CTG/JTC expects state in world frame Eigen::Quaterniond q_body_in_world( - measured_state->pose.pose.orientation.w, measured_state->pose.pose.orientation.x, - measured_state->pose.pose.orientation.y, measured_state->pose.pose.orientation.z); + measured_q.w(), measured_q.x(), measured_q.y(), measured_q.z()); + // if any measured linear velocity is NaN, set to zero Eigen::Vector3d linear_vel_body( - measured_state->twist.twist.linear.x, measured_state->twist.twist.linear.y, - measured_state->twist.twist.linear.z); + std::isnan(measured_state->twist.twist.linear.x) ? 0.0 : measured_state->twist.twist.linear.x, + std::isnan(measured_state->twist.twist.linear.y) ? 0.0 : measured_state->twist.twist.linear.y, + std::isnan(measured_state->twist.twist.linear.z) ? 0.0 : measured_state->twist.twist.linear.z); auto linear_vel_world = q_body_in_world * linear_vel_body; + // if any measured angular velocity is NaN, set to zero Eigen::Vector3d angular_vel_body( - measured_state->twist.twist.angular.x, measured_state->twist.twist.angular.y, - measured_state->twist.twist.angular.z); + std::isnan(measured_state->twist.twist.angular.x) ? 0.0 : measured_state->twist.twist.angular.x, + std::isnan(measured_state->twist.twist.angular.y) ? 0.0 : measured_state->twist.twist.angular.y, + std::isnan(measured_state->twist.twist.angular.z) ? 0.0 + : measured_state->twist.twist.angular.z); auto angular_vel_world = q_body_in_world * angular_vel_body; state.velocities[0] = linear_vel_world[0]; From e178549b3989f822b64ad1cc24d0ce1bc28e93b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 7 Dec 2023 16:13:50 +0100 Subject: [PATCH 44/63] Update JTC to be more extensible. --- .../joint_trajectory_controller.hpp | 25 +++++++++++++++++++ .../src/joint_trajectory_controller.cpp | 18 ------------- 2 files changed, 25 insertions(+), 18 deletions(-) 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 b5cb5a299d..b3d5499b71 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 @@ -140,6 +140,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // For convenience, for each type the interfaces are ordered so that i-th position // matches i-th index in joint_names_ template + using JointInterfaceRefs = std::vector>; + template using InterfaceReferences = std::vector>>; InterfaceReferences joint_command_interface_; @@ -285,6 +287,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC virtual bool read_state_from_state_interfaces(JointTrajectoryPoint & state); /** Assign values from the command interfaces as state. @@ -300,6 +303,28 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const std::shared_ptr request, std::shared_ptr response); + // BEGIN: helper methods + template + void assign_point_from_interface( + std::vector & trajectory_point_interface, const JointInterfaceRefs & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; + + template + void assign_interface_from_point( + JointInterfaceRefs & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + }; + // END: helper methods + private: void update_pids(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4626a0186c..f376bd5c0d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -406,15 +406,6 @@ controller_interface::return_type JointTrajectoryController::update( bool JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; - // Assign values from the hardware // Position states always exist assign_point_from_interface(state.positions, joint_state_interface_[0]); @@ -446,15 +437,6 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto { bool has_values = true; - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; - auto interface_has_values = [](const auto & joint_interface) { return std::find_if( From 2dfedc8f3c04afa5663f4cc1c76b1b9f10134a09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 7 Dec 2023 16:14:50 +0100 Subject: [PATCH 45/63] Updated CTG to have custom state message. --- .../cartesian_trajectory_generator.hpp | 24 +++- .../src/cartesian_trajectory_generator.cpp | 136 ++++++++++++++++-- 2 files changed, 147 insertions(+), 13 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp index 794101e515..918b9e3280 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -20,7 +20,10 @@ #include #include +#include "control_msgs/msg/cartesian_trajectory_generator_state.hpp" #include "control_msgs/srv/set_dof_limits.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -58,16 +61,26 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr protected: bool read_state_from_state_interfaces(JointTrajectoryPoint & state) override; + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void publish_state( + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, + const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, + const JointTrajectoryPoint & ruckig_input) override; + // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_reliable_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeBuffer> reference_world_; rclcpp::Subscription::SharedPtr feedback_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> feedback_; rclcpp::Service::SharedPtr set_joint_limits_service_; + trajectory_msgs::msg::JointTrajectoryPoint control_output_local_; + private: void reference_callback(const std::shared_ptr msg); @@ -75,7 +88,16 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr const std::shared_ptr request, std::shared_ptr response); + using CartControllerStateMsg = control_msgs::msg::CartesianTrajectoryGeneratorState; + using CartStatePublisher = realtime_tools::RealtimePublisher; + using CartStatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr cart_publisher_; + CartStatePublisherPtr cart_state_publisher_; + std::vector configured_joint_limits_; + + // storage of last received measured position to + geometry_msgs::msg::Pose last_received_measured_position_; }; } // namespace cartesian_trajectory_generator diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index 042caf3b00..01a009f4c1 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -14,11 +14,12 @@ #include "joint_trajectory_controller/cartesian_trajectory_generator.hpp" -#include "eigen3/Eigen/Eigen" -#include "tf2/transform_datatypes.h" - #include "controller_interface/helpers.hpp" +#include "eigen3/Eigen/Eigen" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/vector3.hpp" #include "joint_trajectory_controller/trajectory.hpp" +#include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace @@ -60,6 +61,15 @@ void reset_controller_reference_msg(const std::shared_ptr & orientation_angles, geometry_msgs::msg::Quaternion & quaternion_msg) +{ + // convert quaternion to euler angles + tf2::Quaternion quaternion; + quaternion.setRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); + quaternion_msg = tf2::toMsg(quaternion); +} } // namespace namespace cartesian_trajectory_generator @@ -90,30 +100,30 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( configured_joint_limits_ = joint_limits_; // topics QoS - auto subscribers_qos = rclcpp::SystemDefaultsQoS(); - subscribers_qos.keep_last(1); - subscribers_qos.best_effort(); + auto qos_best_effort_history_depth_one = rclcpp::SystemDefaultsQoS(); + qos_best_effort_history_depth_one.keep_last(1); + qos_best_effort_history_depth_one.best_effort(); auto subscribers_reliable_qos = rclcpp::SystemDefaultsQoS(); subscribers_reliable_qos.keep_all(); subscribers_reliable_qos.reliable(); // Reference Subscribers (reliable channel also for updates not to be missed) ref_subscriber_ = get_node()->create_subscription( - "~/reference", subscribers_qos, + "~/reference", qos_best_effort_history_depth_one, std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); ref_subscriber_reliable_ = get_node()->create_subscription( - "~/reference_reliable", subscribers_qos, + "~/reference_reliable", subscribers_reliable_qos, std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); reset_controller_reference_msg(msg); - input_ref_.writeFromNonRT(msg); + reference_world_.writeFromNonRT(msg); // Odometry feedback auto feedback_callback = [&](const std::shared_ptr msg) -> void { feedback_.writeFromNonRT(msg); }; feedback_subscriber_ = get_node()->create_subscription( - "~/feedback", subscribers_qos, feedback_callback); + "~/feedback", qos_best_effort_history_depth_one, feedback_callback); // initialize feedback to null pointer since it is used to determine if we have valid data or not feedback_.writeFromNonRT(nullptr); @@ -130,6 +140,35 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( std::placeholders::_2), services_qos); + cart_publisher_ = get_node()->create_publisher( + "~/controller_state_cartesian", qos_best_effort_history_depth_one); + cart_state_publisher_ = std::make_unique(cart_publisher_); + + cart_state_publisher_->lock(); + cart_state_publisher_->msg_.dof_names = params_.joints; + cart_state_publisher_->msg_.reference_world.transforms.resize(1); + cart_state_publisher_->msg_.reference_world.velocities.resize(1); + cart_state_publisher_->msg_.reference_world.accelerations.resize(1); + cart_state_publisher_->msg_.reference_local.transforms.resize(1); + cart_state_publisher_->msg_.reference_local.velocities.resize(1); + cart_state_publisher_->msg_.reference_local.accelerations.resize(1); + cart_state_publisher_->msg_.feedback.transforms.resize(1); + cart_state_publisher_->msg_.feedback.velocities.resize(1); + cart_state_publisher_->msg_.feedback.accelerations.resize(1); + cart_state_publisher_->msg_.feedback_local.transforms.resize(1); + cart_state_publisher_->msg_.feedback_local.velocities.resize(1); + cart_state_publisher_->msg_.feedback_local.accelerations.resize(1); + cart_state_publisher_->msg_.error.transforms.resize(1); + cart_state_publisher_->msg_.error.velocities.resize(1); + cart_state_publisher_->msg_.error.accelerations.resize(1); + cart_state_publisher_->msg_.output_world.transforms.resize(1); + cart_state_publisher_->msg_.output_world.velocities.resize(1); + cart_state_publisher_->msg_.output_world.accelerations.resize(1); + cart_state_publisher_->msg_.output_local.transforms.resize(1); + cart_state_publisher_->msg_.output_local.velocities.resize(1); + cart_state_publisher_->msg_.output_local.accelerations.resize(1); + cart_state_publisher_->unlock(); + return CallbackReturn::SUCCESS; } @@ -137,7 +176,7 @@ void CartesianTrajectoryGenerator::reference_callback( const std::shared_ptr msg) { // store input ref for later use - input_ref_.writeFromNonRT(msg); + reference_world_.writeFromNonRT(msg); // assume for now that we are working with trajectories with one point - we don't know exactly // where we are in the trajectory before sampling - nevertheless this should work for the use case @@ -420,7 +459,8 @@ bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTraject state.positions[4] = orientation_angles[1]; state.positions[5] = orientation_angles[2]; - ////// Convert measured twist which is in body frame to world frame since CTG/JTC expects state in world frame + // Convert measured twist which is in body frame to world frame since CTG/JTC expects state + // in world frame Eigen::Quaterniond q_body_in_world( measured_q.w(), measured_q.x(), measured_q.y(), measured_q.z()); @@ -450,6 +490,78 @@ bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTraject state.accelerations.clear(); return true; } + +void CartesianTrajectoryGenerator::publish_state( + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, + const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, + const JointTrajectoryPoint & ruckig_input) +{ + joint_trajectory_controller::JointTrajectoryController::publish_state( + time, desired_state, current_state, state_error, splines_output, ruckig_input_target, + ruckig_input); + + if (cart_state_publisher_->trylock()) + { + cart_state_publisher_->msg_.header.stamp = time; + cart_state_publisher_->msg_.reference_world = *(*reference_world_.readFromRT()); + + auto set_multi_dof_point = + [&]( + trajectory_msgs::msg::MultiDOFJointTrajectoryPoint & multi_dof_point, + const JointTrajectoryPoint & traj_point) + { + if (traj_point.positions.size() == 6) + { + multi_dof_point.transforms[0].translation.x = traj_point.positions[0]; + multi_dof_point.transforms[0].translation.y = traj_point.positions[1]; + multi_dof_point.transforms[0].translation.z = traj_point.positions[2]; + + std::array orientation_angles = { + traj_point.positions[3], traj_point.positions[4], traj_point.positions[5]}; + geometry_msgs::msg::Quaternion quaternion; + rpy_to_quaternion(orientation_angles, quaternion); + multi_dof_point.transforms[0].rotation = quaternion; + } + if (traj_point.velocities.size() == 6) + { + multi_dof_point.velocities[0].linear.x = traj_point.velocities[0]; + multi_dof_point.velocities[0].linear.y = traj_point.velocities[1]; + multi_dof_point.velocities[0].linear.z = traj_point.velocities[2]; + multi_dof_point.velocities[0].angular.x = traj_point.velocities[3]; + multi_dof_point.velocities[0].angular.y = traj_point.velocities[4]; + multi_dof_point.velocities[0].angular.z = traj_point.velocities[5]; + } + if (traj_point.accelerations.size() == 6) + { + multi_dof_point.accelerations[0].linear.x = traj_point.accelerations[0]; + multi_dof_point.accelerations[0].linear.y = traj_point.accelerations[1]; + multi_dof_point.accelerations[0].linear.z = traj_point.accelerations[2]; + multi_dof_point.accelerations[0].angular.x = traj_point.accelerations[3]; + multi_dof_point.accelerations[0].angular.y = traj_point.accelerations[4]; + multi_dof_point.accelerations[0].angular.z = traj_point.accelerations[5]; + } + }; + + set_multi_dof_point(cart_state_publisher_->msg_.feedback_local, current_state); + set_multi_dof_point(cart_state_publisher_->msg_.error, state_error); + set_multi_dof_point(cart_state_publisher_->msg_.output_world, desired_state); + + const auto measured_state = *(feedback_.readFromRT()); + cart_state_publisher_->msg_.feedback.transforms[0].translation.x = + measured_state->pose.pose.position.x; + cart_state_publisher_->msg_.feedback.transforms[0].translation.y = + measured_state->pose.pose.position.y; + cart_state_publisher_->msg_.feedback.transforms[0].translation.z = + measured_state->pose.pose.position.z; + cart_state_publisher_->msg_.feedback.transforms[0].rotation = + measured_state->pose.pose.orientation; + cart_state_publisher_->msg_.feedback.velocities[0] = measured_state->twist.twist; + + cart_state_publisher_->unlockAndPublish(); + } +} + } // namespace cartesian_trajectory_generator #include "pluginlib/class_list_macros.hpp" From 9119f51384cf8769d93092d07d570655b25f381f Mon Sep 17 00:00:00 2001 From: bijoua Date: Mon, 25 Sep 2023 16:17:36 -0700 Subject: [PATCH 46/63] Fix stair step in JTC when new trajectory message received --- .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) 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 b3d5499b71..a87b3505c7 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 @@ -132,6 +132,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa Params params_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + rclcpp::Time last_commanded_time_; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f376bd5c0d..05bddf9bde 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -182,7 +182,9 @@ controller_interface::return_type JointTrajectoryController::update( } } - traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_); + if (last_commanded_time_.nanoseconds() == 0) last_commanded_time_ = time; + traj_external_point_ptr_->set_point_before_trajectory_msg( + last_commanded_time_, last_commanded_state_); } else { @@ -305,6 +307,7 @@ controller_interface::return_type JointTrajectoryController::update( // store the previous command. Used in open-loop control mode last_commanded_state_ = state_desired_; + last_commanded_time_ = time; } if (active_goal) From e506ba6cf9db2697f92052196b2d211161bc6844 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 26 Sep 2023 18:03:16 +0200 Subject: [PATCH 47/63] Reset initial value of last commanded time to 0. --- .../src/joint_trajectory_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 05bddf9bde..cc9e6152e3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -182,7 +182,10 @@ controller_interface::return_type JointTrajectoryController::update( } } - if (last_commanded_time_.nanoseconds() == 0) last_commanded_time_ = time; + if (fabs(last_commanded_time_.seconds()) < std::numeric_limits::epsilon()) + { + last_commanded_time_ = time; + } traj_external_point_ptr_->set_point_before_trajectory_msg( last_commanded_time_, last_commanded_state_); } @@ -991,6 +994,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_desired_ = state; last_commanded_state_ = state; } + last_commanded_time_ = rclcpp::Time(); // The controller should start by holding position at the beginning of active state add_new_trajectory_msg(set_hold_position()); From 72874bef6e6870e81185bb88ee89690682b11e1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 28 Sep 2023 19:43:48 +0200 Subject: [PATCH 48/63] Use previous state for limiters. --- joint_trajectory_controller/src/trajectory.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 6a65ba4c23..b13f8ec876 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -108,6 +108,7 @@ bool Trajectory::sample( if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) { output_state = state_before_traj_msg_; + previous_state_ = state_before_traj_msg_; } else { @@ -125,7 +126,7 @@ bool Trajectory::sample( if (joint_limiter) { - joint_limiter->enforce(state_before_traj_msg_, output_state, period); + joint_limiter->enforce(previous_state_, output_state, period); } previous_state_ = output_state; return true; From 3f10cd1a95d495b95bfdfac68c4541970d370c9b Mon Sep 17 00:00:00 2001 From: bijoua Date: Tue, 17 Oct 2023 18:05:27 -0700 Subject: [PATCH 49/63] Use values, if present in reset dofs service, instead of fb to reset --- .../joint_trajectory_controller.hpp | 9 ++- .../src/joint_trajectory_controller.cpp | 60 +++++++++++++++---- 2 files changed, 58 insertions(+), 11 deletions(-) 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 a87b3505c7..570d8452bd 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 @@ -209,7 +209,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using ControllerResetDofsSrvType = control_msgs::srv::ResetDofs; - realtime_tools::RealtimeBuffer> reset_dofs_flags_; + struct ResetDofsData + { + bool reset; + double position; + double velocity; + double acceleration; + }; + realtime_tools::RealtimeBuffer> reset_dofs_flags_; rclcpp::Service::SharedPtr reset_dofs_service_; // callback for topic interface diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index cc9e6152e3..ddf09ded0b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -166,19 +166,25 @@ controller_interface::return_type JointTrajectoryController::update( auto reset_flags = reset_dofs_flags_.readFromRT(); for (size_t i = 0; i < dof_; ++i) { - if (reset_flags->at(i)) + if (reset_flags->at(i).reset) { - last_commanded_state_.positions[i] = state_current_.positions[i]; + last_commanded_state_.positions[i] = std::isnan(reset_flags->at(i).position) + ? state_current_.positions[i] + : reset_flags->at(i).position; if (has_velocity_state_interface_) { - last_commanded_state_.velocities[i] = state_current_.velocities[i]; + last_commanded_state_.velocities[i] = std::isnan(reset_flags->at(i).velocity) + ? state_current_.velocities[i] + : reset_flags->at(i).velocity; } if (has_acceleration_state_interface_) { - last_commanded_state_.accelerations[i] = state_current_.accelerations[i]; + last_commanded_state_.accelerations[i] = std::isnan(reset_flags->at(i).acceleration) + ? state_current_.accelerations[i] + : reset_flags->at(i).acceleration; } - reset_flags->at(i) = false; // reset flag in the buffer for one-shot execution + reset_flags->at(i).reset = false; // reset flag in the buffer for one-shot execution } } @@ -887,8 +893,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); // Initialize memory and RT buffer for DoF reset flags - std::vector reset_flags; - reset_flags.resize(dof_, false); + std::vector reset_flags; + reset_flags.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); reset_dofs_flags_.writeFromNonRT(reset_flags); // Control mode service @@ -899,8 +907,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { response->ok = true; - std::vector reset_flags; - reset_flags.resize(dof_, false); + std::vector reset_flags; + reset_flags.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + + if ( + (request->positions.size() != request->velocities.size()) || + (request->velocities.size() != request->accelerations.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has different values size for positions %ld, velocities %ld, " + "accelerations %ld", + request->positions.size(), request->velocities.size(), request->accelerations.size()); + response->ok = false; + return; + } + + if ((request->positions.size() > 0) && (request->names.size() != request->positions.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has names size %ld different than positions size %ld", + request->names.size(), request->positions.size()); + response->ok = false; + return; + } for (size_t i = 0; i < request->names.size(); ++i) { @@ -910,7 +943,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'.", request->names[i].c_str()); auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); - reset_flags[cmd_itf_index] = true; + double pos = (request->positions.size() != 0) ? request->positions[i] + : std::numeric_limits::quiet_NaN(); + double vel = (request->velocities.size() != 0) ? request->velocities[i] + : std::numeric_limits::quiet_NaN(); + double accel = (request->accelerations.size() != 0) + ? request->accelerations[i] + : std::numeric_limits::quiet_NaN(); + reset_flags[cmd_itf_index] = {true, pos, vel, accel}; } else { From 7b2b0891f2088fa8123e89825a8171b69ac29cd4 Mon Sep 17 00:00:00 2001 From: bijoua Date: Thu, 19 Oct 2023 19:07:20 -0700 Subject: [PATCH 50/63] Fix bug when two reset dofs req sent without being processed in between --- .../src/joint_trajectory_controller.cpp | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ddf09ded0b..86792fa97a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -171,6 +171,10 @@ controller_interface::return_type JointTrajectoryController::update( last_commanded_state_.positions[i] = std::isnan(reset_flags->at(i).position) ? state_current_.positions[i] : reset_flags->at(i).position; + RCLCPP_INFO_STREAM( + get_node()->get_logger(), command_joint_names_[i] + << ": last commanded state position set to " + << last_commanded_state_.positions[i]); if (has_velocity_state_interface_) { last_commanded_state_.velocities[i] = std::isnan(reset_flags->at(i).velocity) @@ -907,11 +911,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { response->ok = true; - std::vector reset_flags; - reset_flags.resize( - dof_, {false, std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); - if ( (request->positions.size() != request->velocities.size()) || (request->velocities.size() != request->accelerations.size())) @@ -935,16 +934,40 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return; } + std::vector reset_flags_reset; + reset_flags_reset.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + + // Here we read current reset dofs flags and clear it. This is done so we can add this request + // to the existing reset flags. This logic prevents this request from overwriting any previous request + // that hasn't been processed yet. + // The assumption here is that the current reset flags are not going to be processed + // between the two calls here to read and reset, which is a highly unlikely scenario. + auto reset_flags = *reset_dofs_flags_.readFromNonRT(); + reset_dofs_flags_.writeFromNonRT(reset_flags_reset); + + // add/update reset dofs flags from request for (size_t i = 0; i < request->names.size(); ++i) { auto it = std::find(command_joint_names_.begin(), command_joint_names_.end(), request->names[i]); if (it != command_joint_names_.end()) { - RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'.", request->names[i].c_str()); auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); double pos = (request->positions.size() != 0) ? request->positions[i] : std::numeric_limits::quiet_NaN(); + + if (request->positions.size() != 0) + { + RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'", request->names[i].c_str()); + } + else + { + RCLCPP_INFO( + get_node()->get_logger(), "Resetting dof '%s' position to %f", + request->names[i].c_str(), pos); + } double vel = (request->velocities.size() != 0) ? request->velocities[i] : std::numeric_limits::quiet_NaN(); double accel = (request->accelerations.size() != 0) From e4054adea7a709e6d1f4b4b201b039e2adcbd96d Mon Sep 17 00:00:00 2001 From: Bijou Date: Thu, 19 Oct 2023 19:16:19 -0700 Subject: [PATCH 51/63] Update reset dofs output messages in JTC --- .../src/joint_trajectory_controller.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 86792fa97a..540e0a7223 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -173,7 +173,7 @@ controller_interface::return_type JointTrajectoryController::update( : reset_flags->at(i).position; RCLCPP_INFO_STREAM( get_node()->get_logger(), command_joint_names_[i] - << ": last commanded state position set to " + << ": last commanded state position reset to " << last_commanded_state_.positions[i]); if (has_velocity_state_interface_) { @@ -939,11 +939,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( dof_, {false, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); - // Here we read current reset dofs flags and clear it. This is done so we can add this request - // to the existing reset flags. This logic prevents this request from overwriting any previous request + // Here we read current reset dofs flags and clear it. This is done so we can add this new request + // to the existing reset flags. This logic prevents this new request from overwriting any previous request // that hasn't been processed yet. - // The assumption here is that the current reset flags are not going to be processed - // between the two calls here to read and reset, which is a highly unlikely scenario. + // The one assumption made here is that the current reset flags are not going to be processed + // between the two calls here to read and reset, which is a highly unlikely scenario. Even if it was, + // the behavior is fairly benign in that the dofs in the previous request will be reset twice. auto reset_flags = *reset_dofs_flags_.readFromNonRT(); reset_dofs_flags_.writeFromNonRT(reset_flags_reset); From e5edfe56f634cb26a09756de7e5c15a02c6c9d08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 1 Nov 2023 16:43:02 +0100 Subject: [PATCH 52/63] Remove service for setting joint limits in CTG. --- .../cartesian_trajectory_generator.hpp | 8 -- .../src/cartesian_trajectory_generator.cpp | 125 ------------------ 2 files changed, 133 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp index 918b9e3280..621c747439 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -21,7 +21,6 @@ #include #include "control_msgs/msg/cartesian_trajectory_generator_state.hpp" -#include "control_msgs/srv/set_dof_limits.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "joint_limits/joint_limits.hpp" @@ -56,7 +55,6 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr using ControllerReferenceMsg = trajectory_msgs::msg::MultiDOFJointTrajectoryPoint; using ControllerFeedbackMsg = nav_msgs::msg::Odometry; - using SetLimitsModeSrvType = control_msgs::srv::SetDOFLimits; protected: bool read_state_from_state_interfaces(JointTrajectoryPoint & state) override; @@ -77,17 +75,11 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr rclcpp::Subscription::SharedPtr feedback_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> feedback_; - rclcpp::Service::SharedPtr set_joint_limits_service_; - trajectory_msgs::msg::JointTrajectoryPoint control_output_local_; private: void reference_callback(const std::shared_ptr msg); - void set_joint_limits_service_callback( - const std::shared_ptr request, - std::shared_ptr response); - using CartControllerStateMsg = control_msgs::msg::CartesianTrajectoryGeneratorState; using CartStatePublisher = realtime_tools::RealtimePublisher; using CartStatePublisherPtr = std::unique_ptr; diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index 01a009f4c1..128f3d54c9 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -133,13 +133,6 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( services_qos.reliable(); services_qos.durability_volatile(); - set_joint_limits_service_ = get_node()->create_service( - "~/set_joint_limits", - std::bind( - &CartesianTrajectoryGenerator::set_joint_limits_service_callback, this, std::placeholders::_1, - std::placeholders::_2), - services_qos); - cart_publisher_ = get_node()->create_publisher( "~/controller_state_cartesian", qos_best_effort_history_depth_one); cart_state_publisher_ = std::make_unique(cart_publisher_); @@ -227,124 +220,6 @@ void CartesianTrajectoryGenerator::reference_callback( add_new_trajectory_msg(new_traj_msg); } -void CartesianTrajectoryGenerator::set_joint_limits_service_callback( - const std::shared_ptr request, - std::shared_ptr response) -{ - response->ok = true; - if (request->names.size() != request->limits.size()) - { - RCLCPP_WARN( - get_node()->get_logger(), - "Fields name and limits have size %zu and %zu. Their size should be equal. Ignoring " - "service call!", - request->names.size(), request->limits.size()); - response->ok = false; - } - - // start with current limits - auto new_joint_limits = joint_limits_; - - // lambda for setting new limit - auto update_limit_from_request = []( - double & new_limit_value, bool & new_limit_has_limits, - const double request_limit_value, - const double configured_limit_value) - { - // request is to reset to configured limit - if (std::isnan(request_limit_value)) - { - new_limit_value = configured_limit_value; - } - // new limit is requested - else - { - new_limit_value = request_limit_value; - } - new_limit_has_limits = !std::isnan(new_limit_value); - }; - - // lambda for setting new position limits - auto update_pos_limits_from_request = - []( - double & new_min_limit_value, double & new_max_limit_value, bool & new_limit_has_limits, - const double request_min_limit_value, const double request_max_limit_value, - const double configured_min_limit_value, const double configured_max_limit_value) - { - // request is to reset to configured min position limit - if (std::isnan(request_min_limit_value)) - { - new_min_limit_value = configured_min_limit_value; - } - // new min position limit is requested - else - { - new_min_limit_value = request_min_limit_value; - } - - // request is to reset to configured max position limit - if (std::isnan(request_max_limit_value)) - { - new_max_limit_value = configured_max_limit_value; - } - // new max position limit is requested - else - { - new_max_limit_value = request_max_limit_value; - } - - // has limits only if one of the min or max position limits is set - new_limit_has_limits = !std::isnan(new_min_limit_value) || !std::isnan(new_max_limit_value); - }; - - for (size_t i = 0; i < request->names.size(); ++i) - { - auto it = - std::find(command_joint_names_.begin(), command_joint_names_.end(), request->names[i]); - if (it != command_joint_names_.end()) - { - auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); - - update_pos_limits_from_request( - new_joint_limits[cmd_itf_index].min_position, new_joint_limits[cmd_itf_index].max_position, - new_joint_limits[cmd_itf_index].has_position_limits, request->limits[i].min_position, - request->limits[i].max_position, configured_joint_limits_[cmd_itf_index].min_position, - configured_joint_limits_[cmd_itf_index].max_position); - update_limit_from_request( - new_joint_limits[cmd_itf_index].max_velocity, - new_joint_limits[cmd_itf_index].has_velocity_limits, request->limits[i].max_velocity, - configured_joint_limits_[cmd_itf_index].max_velocity); - update_limit_from_request( - new_joint_limits[cmd_itf_index].max_acceleration, - new_joint_limits[cmd_itf_index].has_acceleration_limits, - request->limits[i].max_acceleration, - configured_joint_limits_[cmd_itf_index].max_acceleration); - update_limit_from_request( - new_joint_limits[cmd_itf_index].max_jerk, new_joint_limits[cmd_itf_index].has_jerk_limits, - request->limits[i].max_jerk, configured_joint_limits_[cmd_itf_index].max_jerk); - update_limit_from_request( - new_joint_limits[cmd_itf_index].max_effort, - new_joint_limits[cmd_itf_index].has_effort_limits, request->limits[i].max_effort, - configured_joint_limits_[cmd_itf_index].max_effort); - - RCLCPP_INFO( - get_node()->get_logger(), "New limits for joint %zu (%s) are: \n%s", cmd_itf_index, - command_joint_names_[cmd_itf_index].c_str(), - new_joint_limits[cmd_itf_index].to_string().c_str()); - } - else - { - RCLCPP_WARN( - get_node()->get_logger(), "Name '%s' is not command interface. Ignoring this entry.", - request->names[i].c_str()); - response->ok = false; - } - } - - // TODO(destogl): use buffers to sync comm between threads - joint_limits_ = new_joint_limits; -} - controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( const rclcpp_lifecycle::State &) { From d2717a367a660b829b89a8148d10e1500bcd9c1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 16 Nov 2023 19:54:19 +0100 Subject: [PATCH 53/63] Cleanup rucking code and add simple joint limiter. --- .../src/trajectory.cpp | 57 ++++++++++++------- 1 file changed, 37 insertions(+), 20 deletions(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index b13f8ec876..ce6ed72bc5 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -97,6 +97,7 @@ bool Trajectory::sample( // TODO(anyone): this shouldn't be initialized at runtime output_state = trajectory_msgs::msg::JointTrajectoryPoint(); + auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + first_point_in_msg.time_from_start; @@ -171,7 +172,6 @@ bool Trajectory::sample( // whole animation has played out - but still continue s interpolating and smoothing auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1]; - const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; // FIXME(destogl): this is from backport - check if needed // // whole animation has played out @@ -222,10 +222,26 @@ bool Trajectory::sample( if (joint_limiter) { - // TODO(destogl): use here output state - joint_limiter->enforce(previous_state_, output_state, period); + // When running Joint Limiter we might not get to the last_point in time - so SplineIt! + interpolate_between_points( + sample_time - period, previous_state_, sample_time, last_point, sample_time, output_state); + + // if limits are enforced time of the second point is in the future + if (joint_limiter->enforce(previous_state_, output_state, period)) + { + // TODO(destogl): spline it again to avoid oscillations in output from the filter + // interpolate_between_points( + // sample_time-period, previous_state_, sample_time + period, output_state, + // sample_time, output_state); + } + previous_state_ = output_state; + } + else + { + const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; + // do not do splines when trajectory has finished because the time is achieved + interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state); } - previous_state_ = output_state; return true; } @@ -239,6 +255,7 @@ void Trajectory::interpolate_between_points( rclcpp::Duration duration_btwn_points = time_b - time_a; const size_t dim = state_a.positions.size(); + // TODO(anyone): this shouldn't be resized at runtime output.positions.resize(dim, 0.0); output.velocities.resize(dim, 0.0); output.accelerations.resize(dim, 0.0); @@ -292,7 +309,6 @@ void Trajectory::interpolate_between_points( // do cubic interpolation double T[4]; generate_powers(3, duration_btwn_points.seconds(), T); - for (size_t i = 0; i < dim; ++i) { double start_pos = state_a.positions[i]; @@ -399,23 +415,24 @@ void Trajectory::deduce_from_derivatives( { for (size_t i = 0; i < dim; ++i) { - if (std::isnan(second_state.positions[i])) + // reset velocities always to 0 if it is empty or NaN + double first_state_velocity = + first_state.velocities.empty() ? 0.0 : first_state.velocities[i]; + if (std::isnan(first_state_velocity)) + { + first_state.velocities[i] = 0.0; + first_state_velocity = 0.0; + } + double second_state_velocity = + second_state.velocities.empty() ? 0.0 : second_state.velocities[i]; + if (std::isnan(second_state_velocity)) { - double first_state_velocity = - first_state.velocities.empty() ? 0.0 : first_state.velocities[i]; - if (std::isnan(first_state_velocity)) - { - first_state.velocities[i] = 0.0; - first_state_velocity = 0.0; - } - double second_state_velocity = - second_state.velocities.empty() ? 0.0 : second_state.velocities[i]; - if (std::isnan(second_state_velocity)) - { - second_state.velocities[i] = 0.0; - second_state_velocity = 0.0; - } + second_state.velocities[i] = 0.0; + second_state_velocity = 0.0; + } + if (std::isnan(second_state.positions[i])) + { second_state.positions[i] = first_state.positions[i] + (first_state_velocity + second_state_velocity) * 0.5 * delta_t; } From e31691f467a1c5994b354b37c1b7099684060a61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 7 Dec 2023 18:15:53 +0100 Subject: [PATCH 54/63] Fixup JTC and Trajectory to compile. --- .../joint_trajectory_controller.hpp | 7 +- .../trajectory.hpp | 1 + .../src/joint_trajectory_controller.cpp | 53 ++--------- .../src/trajectory.cpp | 9 +- .../test/test_trajectory.cpp | 92 +++++++++++-------- 5 files changed, 70 insertions(+), 92 deletions(-) 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 570d8452bd..f7e4b6e5e0 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 @@ -291,7 +291,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_active_trajectory() const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void publish_state( + virtual void publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); @@ -331,6 +331,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa joint_interface[index].get().set_value(trajectory_point_interface[index]); } }; + + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); // END: helper methods private: @@ -340,8 +343,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const std::vector & interface_type_list, const std::string & interface_type); void init_hold_position_msg(); - void resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index c59fc03d75..f199262f07 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -161,6 +161,7 @@ class Trajectory trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; bool sampled_already_ = false; + trajectory_msgs::msg::JointTrajectoryPoint previous_state_; }; /** diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 540e0a7223..b1de16274f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -414,9 +414,7 @@ controller_interface::return_type JointTrajectoryController::update( } } - publish_state( - time, state_desired_, state_current_, state_error_, splines_state_, ruckig_state_, - ruckig_input_state_); + publish_state(time, state_desired_, state_current_, state_error_); return controller_interface::return_type::OK; } @@ -939,12 +937,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( dof_, {false, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); - // Here we read current reset dofs flags and clear it. This is done so we can add this new request - // to the existing reset flags. This logic prevents this new request from overwriting any previous request - // that hasn't been processed yet. - // The one assumption made here is that the current reset flags are not going to be processed - // between the two calls here to read and reset, which is a highly unlikely scenario. Even if it was, - // the behavior is fairly benign in that the dofs in the previous request will be reset twice. + // Here we read current reset dofs flags and clear it. This is done so we can add this new + // request to the existing reset flags. This logic prevents this new request from overwriting + // any previous request that hasn't been processed yet. The one assumption made here is that the + // current reset flags are not going to be processed between the two calls here to read and + // reset, which is a highly unlikely scenario. Even if it was, the behavior is fairly benign in + // that the dofs in the previous request will be reset twice. auto reset_flags = *reset_dofs_flags_.readFromNonRT(); reset_dofs_flags_.writeFromNonRT(reset_flags_reset); @@ -1186,9 +1184,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( void JointTrajectoryController::publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, - const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, - const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, - const JointTrajectoryPoint & ruckig_input) + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) { if (state_publisher_->trylock()) { @@ -1215,39 +1211,6 @@ void JointTrajectoryController::publish_state( state_publisher_->unlockAndPublish(); } - - if (splines_output_publisher_ && splines_output_publisher_->trylock()) - { - splines_output_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp; - splines_output_publisher_->msg_.actual.positions = splines_output.positions; - splines_output_publisher_->msg_.actual.velocities = splines_output.velocities; - splines_output_publisher_->msg_.actual.accelerations = splines_output.accelerations; - splines_output_publisher_->msg_.actual.effort = splines_output.effort; - - splines_output_publisher_->unlockAndPublish(); - } - - if (ruckig_input_publisher_ && ruckig_input_publisher_->trylock()) - { - ruckig_input_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp; - ruckig_input_publisher_->msg_.actual.positions = ruckig_input.positions; - ruckig_input_publisher_->msg_.actual.velocities = ruckig_input.velocities; - ruckig_input_publisher_->msg_.actual.accelerations = ruckig_input.accelerations; - ruckig_input_publisher_->msg_.actual.effort = ruckig_input.effort; - - ruckig_input_publisher_->unlockAndPublish(); - } - - if (ruckig_input_target_publisher_ && ruckig_input_target_publisher_->trylock()) - { - ruckig_input_target_publisher_->msg_.header.stamp = state_publisher_->msg_.header.stamp; - ruckig_input_target_publisher_->msg_.actual.positions = ruckig_input_target.positions; - ruckig_input_target_publisher_->msg_.actual.velocities = ruckig_input_target.velocities; - ruckig_input_target_publisher_->msg_.actual.accelerations = ruckig_input_target.accelerations; - ruckig_input_target_publisher_->msg_.actual.effort = ruckig_input_target.effort; - - ruckig_input_target_publisher_->unlockAndPublish(); - } } void JointTrajectoryController::topic_callback( diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index ce6ed72bc5..7615eb7c5a 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -24,14 +24,14 @@ namespace joint_trajectory_controller { Trajectory::Trajectory() -: trajectory_start_time_(0), time_before_traj_msg_(0), sampled_already_(false), +: trajectory_start_time_(0), time_before_traj_msg_(0), sampled_already_(false) { } Trajectory::Trajectory(std::shared_ptr joint_trajectory) : trajectory_msg_(joint_trajectory), trajectory_start_time_(static_cast(joint_trajectory->header.stamp)), - sampled_already_(false), + sampled_already_(false) { } @@ -217,10 +217,7 @@ bool Trajectory::sample( } } - // do not do splines when trajectory has finished because the time is achieved - interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state) - - if (joint_limiter) + if (joint_limiter) { // When running Joint Limiter we might not get to the last_point in time - so SplineIt! interpolate_between_points( diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index d60e5c696d..2a48aa1f2b 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -21,6 +21,7 @@ #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" +#include "joint_limits/joint_limiter_interface.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -36,6 +37,7 @@ namespace { // Floating-point value comparison threshold const double EPS = 1e-8; +std::unique_ptr> joint_limiter_; } // namespace TEST(TestTrajectory, initialize_trajectory) @@ -50,7 +52,9 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample( + clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); // traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, output_point, start, end, ); EXPECT_EQ(traj.end(), start); @@ -67,7 +71,9 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample( + clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -112,7 +118,9 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -124,7 +132,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(result, false); } @@ -132,7 +140,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -145,7 +153,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -157,7 +165,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -168,7 +176,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } @@ -177,7 +185,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -185,7 +193,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, - start, end); + start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -196,7 +204,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -353,7 +361,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -365,7 +375,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(result, false); } @@ -373,7 +383,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -393,7 +403,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -407,7 +417,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -427,7 +437,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -441,7 +451,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -461,7 +471,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -474,7 +484,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, - start, end); + start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -526,7 +536,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -539,7 +551,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(result, false); } @@ -547,7 +559,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -567,7 +579,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -618,7 +630,9 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -631,7 +645,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(result, false); } @@ -645,7 +659,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -661,7 +675,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -677,7 +691,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -689,7 +703,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, - start, end); + start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -735,7 +749,9 @@ TEST(TestTrajectory, skip_interpolation) // sample at trajectory starting time { - traj.sample(time_now, no_interpolation, expected_state, start, end); + traj.sample( + time_now, no_interpolation, expected_state, start, end, rclcpp::Duration::from_seconds(0.1), + joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -750,7 +766,7 @@ TEST(TestTrajectory, skip_interpolation) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(result, false); } @@ -758,7 +774,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); // For passthrough, this should just return the first waypoint @@ -774,7 +790,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -789,7 +805,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -799,7 +815,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -807,7 +823,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -815,7 +831,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); From 753d83b365c19019800009a69dc80c9546902ff0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 7 Dec 2023 18:16:10 +0100 Subject: [PATCH 55/63] Fixup CTG to compile. --- .../cartesian_trajectory_generator.hpp | 7 +- .../src/cartesian_trajectory_generator.cpp | 69 +++++++++++-------- 2 files changed, 43 insertions(+), 33 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp index 621c747439..4ff40c3730 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -23,7 +23,6 @@ #include "control_msgs/msg/cartesian_trajectory_generator_state.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "nav_msgs/msg/odometry.hpp" #include "trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp" @@ -63,9 +62,7 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, - const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, - const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, - const JointTrajectoryPoint & ruckig_input) override; + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) override; // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; @@ -86,8 +83,6 @@ class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTr rclcpp::Publisher::SharedPtr cart_publisher_; CartStatePublisherPtr cart_state_publisher_; - std::vector configured_joint_limits_; - // storage of last received measured position to geometry_msgs::msg::Pose last_received_measured_position_; }; diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index 128f3d54c9..604ef6fecf 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -96,9 +96,6 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( return ret; } - // store joint limits for later - configured_joint_limits_ = joint_limits_; - // topics QoS auto qos_best_effort_history_depth_one = rclcpp::SystemDefaultsQoS(); qos_best_effort_history_depth_one.keep_last(1); @@ -223,6 +220,15 @@ void CartesianTrajectoryGenerator::reference_callback( controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( const rclcpp_lifecycle::State &) { + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(params_); + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { @@ -253,43 +259,55 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( // } // } - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); traj_msg_external_point_ptr_.writeFromNonRT( std::shared_ptr()); subscriber_is_active_ = true; - traj_point_active_ptr_ = &traj_external_point_ptr_; - // Initialize current state storage if hardware state has tracking offset + // Handle restart of controller by reading from commands if those are not NaN (a controller was + // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); if (!read_state_from_state_interfaces(state)) return CallbackReturn::ERROR; state_current_ = state; state_desired_ = state; last_commanded_state_ = state; - // Handle restart of controller by reading from commands if - // those are not nan + + // Handle restart of controller by reading from commands if those are not NaN if (read_state_from_command_interfaces(state)) { state_current_ = state; state_desired_ = state; last_commanded_state_ = state; } + last_commanded_time_ = rclcpp::Time(); + + // 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 + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + get_node()->get_logger(), + "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, + default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } return CallbackReturn::SUCCESS; } @@ -368,13 +386,10 @@ bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTraject void CartesianTrajectoryGenerator::publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, - const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error, - const JointTrajectoryPoint & splines_output, const JointTrajectoryPoint & ruckig_input_target, - const JointTrajectoryPoint & ruckig_input) + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) { joint_trajectory_controller::JointTrajectoryController::publish_state( - time, desired_state, current_state, state_error, splines_output, ruckig_input_target, - ruckig_input); + time, desired_state, current_state, state_error); if (cart_state_publisher_->trylock()) { From 23508107a6c75618c8b76fcc6ab69703756187ce Mon Sep 17 00:00:00 2001 From: Alex White Date: Wed, 19 Apr 2023 19:07:05 -0700 Subject: [PATCH 56/63] Added support for save integral term parameter Added save_iterm parameter Added check for default false in tests Applied suggestions from code review More comment updates --- pid_controller/src/pid_controller.yaml | 5 +++++ pid_controller/test/test_pid_controller.cpp | 1 + 2 files changed, 6 insertions(+) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index f645738862..1946070bad 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -74,6 +74,11 @@ pid_controller: default_value: 0.0, description: "Lower integral clamp. Only used if antiwindup is activated." } + save_iterm: { + type: bool, + default_value: false, + description: "Enables save of integral term on pid reset. Integrator error will not be set to 0. This is useful for pid loops that are enabled/disabled with constant steady-state external disturbance." + } feedforward_gain: { type: double, default_value: 0.0, diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a44347f5f1..39c37689f0 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -52,6 +52,7 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) 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_FALSE(controller_->params_.gains.dof_names_map[dof_name].save_iterm); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); } ASSERT_EQ(controller_->params_.command_interface, command_interface_); From f7d7466d40b3d5dd99aa76cc0d8735af48dec9fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 18 Dec 2023 18:56:25 +0100 Subject: [PATCH 57/63] Fix JTC crashes --- joint_trajectory_controller/src/trajectory.cpp | 18 ++++++++++-------- .../test/test_trajectory.cpp | 7 ++++--- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 7615eb7c5a..0fa195258e 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -143,7 +143,7 @@ bool Trajectory::sample( const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start; const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start; - if (sample_time >= t0 && sample_time < t1) + if (sample_time >= t0 && sample_time <= t1) { // If interpolation is disabled, just forward the next waypoint if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) @@ -174,10 +174,6 @@ bool Trajectory::sample( auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1]; // FIXME(destogl): this is from backport - check if needed - // // whole animation has played out - // start_segment_itr = --end(); - // end_segment_itr = end(); - // expected_state = (*start_segment_itr); // // // TODO: Add and test enforceJointLimits? Unsure if needed for end of animation // // Yes, call enforceJointLimits to handle halting in servo, which has time_from_start == 1ns @@ -235,9 +231,15 @@ bool Trajectory::sample( } else { - const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; - // do not do splines when trajectory has finished because the time is achieved - interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state); + // OLD: the following 3 lines --> maybe to delete + // const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; + // // do not do splines when trajectory has finished because the time is achieved + // interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state); + + // whole animation has played out + start_segment_itr = --end(); + end_segment_itr = end(); + output_state = (*start_segment_itr); } return true; diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 2a48aa1f2b..f69f2dd62e 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -196,6 +196,7 @@ TEST(TestTrajectory, sample_trajectory_positions) start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); + std::cout << "Expected state size: " << expected_state.positions.size() << std::endl; EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -477,17 +478,17 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); // the goal is reached so no acceleration anymore - EXPECT_NEAR(0, expected_state.accelerations[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); } - // sample past given points - movement virtually stops + // sample past given points - if there is velocity continue sampling { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); - EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(position_third_seg + p3.velocities[0] * 0.1, expected_state.positions[0], EPS); EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); } From b20386900c2074fcb0dd06ea44d70a522c0ddeef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 18 Dec 2023 19:55:49 +0100 Subject: [PATCH 58/63] Fixup one crash of JTC. --- joint_trajectory_controller/src/trajectory.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0fa195258e..bcf5ac12c1 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -213,6 +213,11 @@ bool Trajectory::sample( } } + // whole animation has played out + start_segment_itr = --end(); + end_segment_itr = end(); + output_state = (*start_segment_itr); + if (joint_limiter) { // When running Joint Limiter we might not get to the last_point in time - so SplineIt! @@ -235,11 +240,6 @@ bool Trajectory::sample( // const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; // // do not do splines when trajectory has finished because the time is achieved // interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state); - - // whole animation has played out - start_segment_itr = --end(); - end_segment_itr = end(); - output_state = (*start_segment_itr); } return true; From cc5fc7a0951a3b13c07cf336415ac9cefcda7f2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 31 Jan 2024 15:29:29 +0100 Subject: [PATCH 59/63] Move resetting of the axis values from lambda to a method. --- .../joint_trajectory_controller.hpp | 6 + .../src/joint_trajectory_controller.cpp | 171 +++++++++--------- 2 files changed, 91 insertions(+), 86 deletions(-) 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 f7e4b6e5e0..1f72df30b8 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 @@ -234,6 +234,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void goal_accepted_callback( std::shared_ptr> goal_handle); + // Callback for services + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void reset_dofs_service_callback( + const std::shared_ptr request, + std::shared_ptr response); + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; /** diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b1de16274f..33775bab4c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -901,93 +901,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); reset_dofs_flags_.writeFromNonRT(reset_flags); - // Control mode service - auto reset_dofs_service_callback = - [&]( - const std::shared_ptr request, - std::shared_ptr response) - { - response->ok = true; - - if ( - (request->positions.size() != request->velocities.size()) || - (request->velocities.size() != request->accelerations.size())) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Reset dofs service call has different values size for positions %ld, velocities %ld, " - "accelerations %ld", - request->positions.size(), request->velocities.size(), request->accelerations.size()); - response->ok = false; - return; - } - - if ((request->positions.size() > 0) && (request->names.size() != request->positions.size())) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Reset dofs service call has names size %ld different than positions size %ld", - request->names.size(), request->positions.size()); - response->ok = false; - return; - } - - std::vector reset_flags_reset; - reset_flags_reset.resize( - dof_, {false, std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); - - // Here we read current reset dofs flags and clear it. This is done so we can add this new - // request to the existing reset flags. This logic prevents this new request from overwriting - // any previous request that hasn't been processed yet. The one assumption made here is that the - // current reset flags are not going to be processed between the two calls here to read and - // reset, which is a highly unlikely scenario. Even if it was, the behavior is fairly benign in - // that the dofs in the previous request will be reset twice. - auto reset_flags = *reset_dofs_flags_.readFromNonRT(); - reset_dofs_flags_.writeFromNonRT(reset_flags_reset); - - // add/update reset dofs flags from request - for (size_t i = 0; i < request->names.size(); ++i) - { - auto it = - std::find(command_joint_names_.begin(), command_joint_names_.end(), request->names[i]); - if (it != command_joint_names_.end()) - { - auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); - double pos = (request->positions.size() != 0) ? request->positions[i] - : std::numeric_limits::quiet_NaN(); - - if (request->positions.size() != 0) - { - RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'", request->names[i].c_str()); - } - else - { - RCLCPP_INFO( - get_node()->get_logger(), "Resetting dof '%s' position to %f", - request->names[i].c_str(), pos); - } - double vel = (request->velocities.size() != 0) ? request->velocities[i] - : std::numeric_limits::quiet_NaN(); - double accel = (request->accelerations.size() != 0) - ? request->accelerations[i] - : std::numeric_limits::quiet_NaN(); - reset_flags[cmd_itf_index] = {true, pos, vel, accel}; - } - else - { - RCLCPP_WARN( - get_node()->get_logger(), "Name '%s' is not command interface. Ignoring this entry.", - request->names[i].c_str()); - response->ok = false; - } - } - - reset_dofs_flags_.writeFromNonRT(reset_flags); - }; - reset_dofs_service_ = get_node()->create_service( - "~/reset_dofs", reset_dofs_service_callback); + "~/reset_dofs", + std::bind(&JointTrajectoryController::reset_dofs_service_callback, this, _1, _2)); return CallbackReturn::SUCCESS; } @@ -1306,6 +1222,89 @@ void JointTrajectoryController::goal_accepted_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } +void JointTrajectoryController::reset_dofs_service_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + response->ok = true; + + if ( + (request->positions.size() != request->velocities.size()) || + (request->velocities.size() != request->accelerations.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has different values size for positions %ld, velocities %ld, " + "accelerations %ld", + request->positions.size(), request->velocities.size(), request->accelerations.size()); + response->ok = false; + return; + } + + if ((request->positions.size() > 0) && (request->names.size() != request->positions.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has names size %ld different than positions size %ld", + request->names.size(), request->positions.size()); + response->ok = false; + return; + } + + std::vector reset_flags_reset; + reset_flags_reset.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + + // Here we read current reset dofs flags and clear it. This is done so we can add this new + // request to the existing reset flags. This logic prevents this new request from overwriting + // any previous request that hasn't been processed yet. The one assumption made here is that the + // current reset flags are not going to be processed between the two calls here to read and + // reset, which is a highly unlikely scenario. Even if it was, the behavior is fairly benign in + // that the dofs in the previous request will be reset twice. + auto reset_flags = *reset_dofs_flags_.readFromNonRT(); + reset_dofs_flags_.writeFromNonRT(reset_flags_reset); + + // add/update reset dofs flags from request + for (size_t i = 0; i < request->names.size(); ++i) + { + auto it = + std::find(command_joint_names_.begin(), command_joint_names_.end(), request->names[i]); + if (it != command_joint_names_.end()) + { + auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); + double pos = (request->positions.size() != 0) ? request->positions[i] + : std::numeric_limits::quiet_NaN(); + + if (request->positions.size() != 0) + { + RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'", request->names[i].c_str()); + } + else + { + RCLCPP_INFO( + get_node()->get_logger(), "Resetting dof '%s' position to %f", request->names[i].c_str(), + pos); + } + double vel = (request->velocities.size() != 0) ? request->velocities[i] + : std::numeric_limits::quiet_NaN(); + double accel = (request->accelerations.size() != 0) + ? request->accelerations[i] + : std::numeric_limits::quiet_NaN(); + reset_flags[cmd_itf_index] = {true, pos, vel, accel}; + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), "Name '%s' is not command interface. Ignoring this entry.", + request->names[i].c_str()); + response->ok = false; + } + } + + reset_dofs_flags_.writeFromNonRT(reset_flags); +} + void JointTrajectoryController::compute_error_for_joint( JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) const From 1ed60ebb30e98d6b0ab63c4841b19f322afb8714 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 31 Jan 2024 15:30:12 +0100 Subject: [PATCH 60/63] Adjust variable name to avoid shadowing. --- .../src/cartesian_trajectory_generator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index 604ef6fecf..a67980b57f 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -112,9 +112,9 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( "~/reference_reliable", subscribers_reliable_qos, std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg); - reference_world_.writeFromNonRT(msg); + std::shared_ptr msg_reset = std::make_shared(); + reset_controller_reference_msg(msg_reset); + reference_world_.writeFromNonRT(msg_reset); // Odometry feedback auto feedback_callback = [&](const std::shared_ptr msg) -> void From 54b195fd3bf43e1763e934b2054b4c5f2ff16ba4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 1 Feb 2024 18:34:10 +0100 Subject: [PATCH 61/63] Bugfixing. --- .../src/joint_trajectory_controller.cpp | 22 +++++++++++++++++++ .../src/trajectory.cpp | 9 ++++++-- 2 files changed, 29 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 33775bab4c..95912fb9e6 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -717,6 +717,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { const auto & gains = params_.gains.joints_map.at(params_.joints[i]); joints_angle_wraparound_[i] = gains.angle_wraparound; + RCLCPP_INFO( + get_node()->get_logger(), "Angle Wraparound for joint '%s' is '%s'", + params_.joints[i].c_str(), joints_angle_wraparound_[i] ? "true" : "false"); } // Initialize joint limits @@ -1312,6 +1315,15 @@ void JointTrajectoryController::compute_error_for_joint( // error defined as the difference between current and desired if (joints_angle_wraparound_[index]) { + RCLCPP_WARN( + get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.", + current.positions.size(), desired.positions.size()); + RCLCPP_WARN( + get_node()->get_logger(), "Calculating angular distance between current: %.5f.", + current.positions[index]); + RCLCPP_WARN( + get_node()->get_logger(), "Calculating angular distance between desired: %.5f.", + desired.positions[index]); // if desired, the shortest_angular_distance is calculated, i.e., the error is // normalized between -piget_logger(), "Calculating for index: %zu.", index); + RCLCPP_WARN( + get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.", + current.positions.size(), desired.positions.size()); + RCLCPP_WARN( + get_node()->get_logger(), "Calculating angular distance between current: %.5f.", + current.positions[index]); + RCLCPP_WARN( + get_node()->get_logger(), "Calculating angular distance between desired: %.5f.", + desired.positions[index]); error.positions[index] = desired.positions[index] - current.positions[index]; } if ( diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index bcf5ac12c1..ca7059232e 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -71,6 +71,8 @@ bool Trajectory::sample( { THROW_ON_NULLPTR(trajectory_msg_) + RCUTILS_LOG_ERROR_NAMED("trajectory", "Sampling Trajectory"); + if (trajectory_msg_->points.empty()) { start_segment_itr = end(); @@ -105,6 +107,7 @@ bool Trajectory::sample( // current time hasn't reached traj time of the first point in the msg yet if (sample_time < first_point_timestamp) { + RCUTILS_LOG_ERROR_NAMED("trajectory", "Before Start time"); // If interpolation is disabled, just forward the next waypoint if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) { @@ -170,6 +173,8 @@ bool Trajectory::sample( } } + RCUTILS_LOG_ERROR_NAMED("trajectory", "Managing last point"); + // whole animation has played out - but still continue s interpolating and smoothing auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1]; @@ -237,9 +242,9 @@ bool Trajectory::sample( else { // OLD: the following 3 lines --> maybe to delete - // const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; + const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; // // do not do splines when trajectory has finished because the time is achieved - // interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state); + interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state); } return true; From 34efa25d3d4aa59fb3e3e8fe95316b981cb44bbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 9 Feb 2024 14:11:51 +0100 Subject: [PATCH 62/63] Increasing code readability in JTC --- .../joint_trajectory_controller.hpp | 6 +- .../src/cartesian_trajectory_generator.cpp | 7 +- .../src/joint_trajectory_controller.cpp | 143 ++++++++---------- 3 files changed, 74 insertions(+), 82 deletions(-) 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 1f72df30b8..130dbfbc3a 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 @@ -131,8 +131,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr param_listener_; Params params_; + // variables for storing internal data for open-loop control trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; rclcpp::Time last_commanded_time_; + /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; @@ -184,9 +186,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; - std::shared_ptr traj_external_point_ptr_ = nullptr; + std::shared_ptr current_trajectory_ = nullptr; realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; + new_trajectory_msg_; std::shared_ptr hold_position_msg_ptr_ = nullptr; diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index a67980b57f..9c73b56568 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -214,7 +214,7 @@ void CartesianTrajectoryGenerator::reference_callback( assign_value_from_input( msg->transforms[0].rotation.z, msg->velocities[0].angular.z, params_.joints[5], 5); - add_new_trajectory_msg(new_traj_msg); + topic_callback(new_traj_msg); } controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( @@ -259,9 +259,8 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( // } // } - traj_external_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); + current_trajectory_ = std::make_shared(); + new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); subscriber_is_active_ = true; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 95912fb9e6..8f522b7642 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -133,31 +133,33 @@ controller_interface::return_type JointTrajectoryController::update( // don't update goal after we sampled the trajectory to avoid any racecondition const auto active_goal = *rt_active_goal_.readFromRT(); - // Check if a new external message has been received from nonRT threads - auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); + // Check if a new trajectory message has been received from Non-RT threads + const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg(); + auto new_external_msg = new_trajectory_msg_.readFromRT(); // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) if ( - current_external_msg != *new_external_msg && + current_trajectory_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); + current_trajectory_->update(*new_external_msg); } // current state update state_current_.time_from_start.set__sec(0); if (!read_state_from_state_interfaces(state_current_)) + { return controller_interface::return_type::OK; + } // currently carrying out a trajectory if (has_active_trajectory()) { bool first_sample = false; // if sampling the first time, set the point before you sample - if (!traj_external_point_ptr_->is_sampled_already()) + if (!current_trajectory_->is_sampled_already()) { first_sample = true; @@ -196,24 +198,24 @@ controller_interface::return_type JointTrajectoryController::update( { last_commanded_time_ = time; } - traj_external_point_ptr_->set_point_before_trajectory_msg( + current_trajectory_->set_point_before_trajectory_msg( last_commanded_time_, last_commanded_state_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); + current_trajectory_->set_point_before_trajectory_msg(time, state_current_); } } // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( + const bool valid_point = current_trajectory_->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr, period, joint_limiter_); if (valid_point) { - const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); + const rclcpp::Time traj_start = current_trajectory_->time_from_start(); // this is the time instance // - started with the first segment: when the first point will be reached (in the future) // - later: when the point of the current segment was reached @@ -225,7 +227,7 @@ controller_interface::return_type JointTrajectoryController::update( bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + const bool before_last_point = end_segment_itr != current_trajectory_->end(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -235,8 +237,8 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // Check state/goal tolerance @@ -349,8 +351,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // check goal tolerance else if (!before_last_point) @@ -368,8 +370,8 @@ 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_success_trajectory_point()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -387,8 +389,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } } } @@ -397,16 +399,16 @@ controller_interface::return_type JointTrajectoryController::update( // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied (will stay in this state until new message arrives) @@ -602,13 +604,13 @@ void JointTrajectoryController::query_state_service( { TrajectoryPointConstIter start_segment_itr, end_segment_itr; const rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01); - response->success = traj_external_point_ptr_->sample( + response->success = current_trajectory_->sample( static_cast(request->time), interpolation_method_, state_requested, start_segment_itr, end_segment_itr, period, joint_limiter_); // If the requested sample time precedes the trajectory finish time respond as failure if (response->success) { - if (end_segment_itr == traj_external_point_ptr_->end()) + if (end_segment_itr == current_trajectory_->end()) { RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); response->success = false; @@ -953,9 +955,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - traj_external_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); + current_trajectory_ = std::make_shared(); + new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); subscriber_is_active_ = true; @@ -1054,7 +1055,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return CallbackReturn::SUCCESS; } @@ -1088,7 +1089,7 @@ bool JointTrajectoryController::reset() } } - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return true; } @@ -1315,15 +1316,6 @@ void JointTrajectoryController::compute_error_for_joint( // error defined as the difference between current and desired if (joints_angle_wraparound_[index]) { - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.", - current.positions.size(), desired.positions.size()); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between current: %.5f.", - current.positions[index]); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between desired: %.5f.", - desired.positions[index]); // if desired, the shortest_angular_distance is calculated, i.e., the error is // normalized between -piget_logger(), "Calculating for index: %zu.", index); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.", - current.positions.size(), desired.positions.size()); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between current: %.5f.", - current.positions[index]); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between desired: %.5f.", - desired.positions[index]); error.positions[index] = desired.positions[index] - current.positions[index]; } if ( @@ -1488,6 +1470,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( bool JointTrajectoryController::validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory) const { + // CHECK: Partial joint goals // If partial joints goals are not allowed, goal should specify all controller joints if (!params_.allow_partial_joints_goal) { @@ -1500,33 +1483,21 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if joint names are provided if (trajectory.joint_names.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); return false; } - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) + // CHECK: if provided trajectory has points + if (trajectory.points.empty()) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } - if (trajectory_end_time < get_node()->now()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non-zero start time (%f) that ends in the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); - return false; - } + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; } + // CHECK: If joint names are matching the joints defined for the controller for (size_t i = 0; i < trajectory.joint_names.size(); ++i) { const std::string & incoming_joint_name = trajectory.joint_names[i]; @@ -1541,12 +1512,7 @@ bool JointTrajectoryController::validate_trajectory_msg( } } - if (trajectory.points.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); - return false; - } - + // CHECK: if trajectory ends with non-zero velocity (when option is disabled) if (!params_.allow_nonzero_velocity_at_trajectory_end) { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) @@ -1562,9 +1528,32 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if trajectory end time is in the past (if start time defined) + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) + { + trajectory_end_time += p.time_from_start; + } + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); + return false; + } + } + rclcpp::Duration previous_traj_time(0ms); for (size_t i = 0; i < trajectory.points.size(); ++i) { + // CHECK: if time of points in the trajectory is monotonous if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( @@ -1578,6 +1567,8 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; + + // CHECK: if all required data are provided in the trajectory // This currently supports only position, velocity and acceleration inputs if (params_.allow_integration_in_goal_trajectories) { @@ -1620,7 +1611,7 @@ bool JointTrajectoryController::validate_trajectory_msg( void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { - traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + new_trajectory_msg_.writeFromNonRT(traj_msg); } void JointTrajectoryController::preempt_active_goal() @@ -1653,7 +1644,7 @@ 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] = current_trajectory_->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 @@ -1706,7 +1697,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( bool JointTrajectoryController::has_active_trajectory() const { - return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); + return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg(); } void JointTrajectoryController::update_pids() From 1ca51d16b7a1e007f24a45d3c67fc029f03d2c30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 9 Feb 2024 14:12:11 +0100 Subject: [PATCH 63/63] Adjusting CTG with limiters. --- joint_trajectory_controller/src/trajectory.cpp | 11 +++-------- .../test/test_trajectory_controller_utils.hpp | 4 ++-- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index ca7059232e..ffcd93dfcd 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -52,6 +52,7 @@ void Trajectory::set_point_before_trajectory_msg( { time_before_traj_msg_ = current_time; state_before_traj_msg_ = current_point; + previous_state_ = current_point; } void Trajectory::update(std::shared_ptr joint_trajectory) @@ -71,8 +72,6 @@ bool Trajectory::sample( { THROW_ON_NULLPTR(trajectory_msg_) - RCUTILS_LOG_ERROR_NAMED("trajectory", "Sampling Trajectory"); - if (trajectory_msg_->points.empty()) { start_segment_itr = end(); @@ -107,16 +106,14 @@ bool Trajectory::sample( // current time hasn't reached traj time of the first point in the msg yet if (sample_time < first_point_timestamp) { - RCUTILS_LOG_ERROR_NAMED("trajectory", "Before Start time"); // If interpolation is disabled, just forward the next waypoint if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) { output_state = state_before_traj_msg_; - previous_state_ = state_before_traj_msg_; } else { - // it changes points only if position and velocity do not exist, but their derivatives + // it changes a point only if position and velocity do not exist, but their derivatives deduce_from_derivatives( state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(), (first_point_timestamp - time_before_traj_msg_).seconds()); @@ -130,7 +127,7 @@ bool Trajectory::sample( if (joint_limiter) { - joint_limiter->enforce(previous_state_, output_state, period); + joint_limiter->enforce(state_before_traj_msg_, output_state, period); } previous_state_ = output_state; return true; @@ -173,8 +170,6 @@ bool Trajectory::sample( } } - RCUTILS_LOG_ERROR_NAMED("trajectory", "Managing last point"); - // whole animation has played out - but still continue s interpolating and smoothing auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1]; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 6978d0e452..485cdff884 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -149,12 +149,12 @@ class TestableJointTrajectoryController bool has_trivial_traj() const { - return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; + return has_active_trajectory() && current_trajectory_->has_nontrivial_msg() == false; } bool has_nontrivial_traj() { - return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(); + return has_active_trajectory() && current_trajectory_->has_nontrivial_msg(); } double get_cmd_timeout() { return cmd_timeout_; }