From a6c1c0c1c209fcfee8ed81825e7c7e7784d54eee Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Mon, 9 Dec 2024 11:56:32 +0100 Subject: [PATCH 1/5] Add wrench offset for admittance controller (#1249) --- admittance_controller/doc/userdoc.rst | 3 ++ .../admittance_controller.hpp | 3 ++ .../src/admittance_controller.cpp | 40 ++++++++++++++++++- doc/release_notes.rst | 1 + 4 files changed, 46 insertions(+), 1 deletion(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 8056a017d7..c624269d86 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -32,6 +32,9 @@ Topics ~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] Target joint commands when controller is not in chained mode. +~/wrench_reference (input topic) [geometry_msgs::msg::WrenchStamped] + Target wrench offset (WrenchStamped has to be in the frame of the FT-sensor). + ~/state (output topic) [control_msgs::msg::AdmittanceControllerState] Topic publishing internal states. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 89cd195ce4..17b70b264d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -133,6 +133,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_; + rclcpp::Subscription::SharedPtr + input_wrench_command_subscriber_; rclcpp::Publisher::SharedPtr s_publisher_; // admittance parameters @@ -144,6 +146,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // real-time buffer realtime_tools::RealtimeBuffer> input_joint_command_; + realtime_tools::RealtimeBuffer input_wrench_command_; std::unique_ptr> state_publisher_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e0e38140a..58a5bcd6e2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -27,6 +27,23 @@ namespace admittance_controller { + +geometry_msgs::msg::Wrench add_wrenches( + const geometry_msgs::msg::Wrench & a, const geometry_msgs::msg::Wrench & b) +{ + geometry_msgs::msg::Wrench res; + + res.force.x = a.force.x + b.force.x; + res.force.y = a.force.y + b.force.y; + res.force.z = a.force.z + b.force.z; + + res.torque.x = a.torque.x + b.torque.x; + res.torque.y = a.torque.y + b.torque.y; + res.torque.z = a.torque.z + b.torque.z; + + return res; +} + controller_interface::CallbackReturn AdmittanceController::on_init() { // initialize controller config @@ -116,6 +133,7 @@ AdmittanceController::on_export_reference_interfaces() reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); position_reference_ = {}; velocity_reference_ = {}; + input_wrench_command_.reset(); // assign reference interfaces auto index = 0ul; @@ -265,6 +283,24 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + + input_wrench_command_subscriber_ = + get_node()->create_subscription( + "~/wrench_reference", rclcpp::SystemDefaultsQoS(), + [&](const geometry_msgs::msg::WrenchStamped & msg) + { + if ( + msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && + !msg.header.frame_id.empty()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " + << msg.header.frame_id << ". Expected reference frame: " + << admittance_->parameters_.ft_sensor.frame.id); + return; + } + input_wrench_command_.writeFromNonRT(msg); + }); s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = @@ -398,8 +434,10 @@ controller_interface::return_type AdmittanceController::update_and_write_command // get all controller inputs read_state_from_hardware(joint_state_, ft_values_); + auto offsetted_ft_values = add_wrenches(ft_values_, input_wrench_command_.readFromRT()->wrench); + // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + admittance_->update(joint_state_, offsetted_ft_values, reference_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 974bca880f..c7149faf03 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -7,6 +7,7 @@ This list summarizes the changes between Iron (previous) and Jazzy (current) rel admittance_controller ************************ * Remove ``robot_description`` parameter from parameter YAML, because it is not used at all (`#963 `_). +* Added ``~/wrench_reference`` input topic which allows to provide a force-torque offset as WrenchStamped (`#1249 `_). diff_drive_controller ***************************** From de42962cdab61c110cf913846fde087c119e76de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 9 Dec 2024 11:57:24 +0100 Subject: [PATCH 2/5] Add missing plugins to ros2_controllers dependencies (#1413) --- ros2_controllers/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ec612a3139..294d851e40 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -25,11 +25,14 @@ force_torque_sensor_broadcaster forward_command_controller gpio_controllers + gripper_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller + parallel_gripper_controller pid_controller + pose_broadcaster position_controllers range_sensor_broadcaster steering_controllers_library From 330b9e4e4ea18b423ff1cc0f8c8f7f0724ee33f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 9 Dec 2024 13:04:12 +0100 Subject: [PATCH 3/5] Add sai (#1416) --- .github/mergify.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index c2eda7385b..84ad380e90 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -23,7 +23,7 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? - name: development targets master branch conditions: @@ -31,6 +31,7 @@ pull_request_rules: - author!=bmagyar - author!=destogl - author!=christophfroehlich + - author!=saikishor - author!=mergify[bot] - author!=dependabot[bot] actions: From 66d160c6ae2b596dd3749e658cb01e2ac00d35a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 9 Dec 2024 17:15:51 +0100 Subject: [PATCH 4/5] [CI] Add clang job and setup concurrency (#1407) --- .../workflows/humble-abi-compatibility.yml | 5 +++ .github/workflows/humble-binary-build.yml | 5 +++ .github/workflows/humble-check-docs.yml | 4 +++ .github/workflows/humble-coverage-build.yml | 5 +++ .github/workflows/humble-debian-build.yml | 4 +++ .github/workflows/humble-pre-commit.yml | 4 +++ .../humble-rhel-semi-binary-build.yml | 5 +++ .../workflows/humble-semi-binary-build.yml | 22 +++++++++--- .github/workflows/jazzy-abi-compatibility.yml | 5 +++ .github/workflows/jazzy-binary-build.yml | 5 +++ .github/workflows/jazzy-check-docs.yml | 4 +++ .github/workflows/jazzy-coverage-build.yml | 5 +++ .github/workflows/jazzy-debian-build.yml | 4 +++ .github/workflows/jazzy-pre-commit.yml | 4 +++ .../jazzy-rhel-semi-binary-build.yml | 4 +++ .github/workflows/jazzy-semi-binary-build.yml | 22 +++++++++--- .../workflows/rolling-abi-compatibility.yml | 5 +++ .github/workflows/rolling-binary-build.yml | 5 +++ .github/workflows/rolling-check-docs.yml | 4 +++ ...ling-compatibility-humble-binary-build.yml | 14 ++++---- ...lling-compatibility-jazzy-binary-build.yml | 14 ++++---- .github/workflows/rolling-coverage-build.yml | 5 +++ .github/workflows/rolling-debian-build.yml | 4 +++ .github/workflows/rolling-pre-commit.yml | 4 +++ .../rolling-rhel-semi-binary-build.yml | 4 +++ .../workflows/rolling-semi-binary-build.yml | 22 +++++++++--- .github/workflows/rosdoc2.yml | 3 ++ .../admittance_controller/admittance_rule.hpp | 9 ++--- .../admittance_rule_impl.hpp | 35 +++++++++++-------- .../src/admittance_controller.cpp | 4 +-- .../src/diff_drive_controller.cpp | 2 +- .../test_load_forward_command_controller.cpp | 2 +- .../test/test_joint_state_broadcaster.cpp | 4 +-- .../tolerances.hpp | 4 +-- .../trajectory.hpp | 4 +-- .../src/joint_trajectory_controller.cpp | 6 ++-- .../src/trajectory.cpp | 4 +-- .../test/test_trajectory_controller_utils.hpp | 4 ++- pid_controller/src/pid_controller.cpp | 2 +- ros2_controllers.humble.repos | 2 +- .../src/steering_controllers_library.cpp | 3 +- .../src/steering_odometry.cpp | 5 ++- .../src/tricycle_controller.cpp | 2 +- 43 files changed, 214 insertions(+), 69 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 247371ba7d..6cfd2c6068 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -15,6 +15,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index a031d823cf..f013f18880 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -34,6 +34,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml index 0eeeaedf40..448ff4084b 100644 --- a/.github/workflows/humble-check-docs.yml +++ b/.github/workflows/humble-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index d0dcb9c350..40d1de7052 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -30,6 +30,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index cd25248caf..e52ffe6842 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: humble_debian: diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml index 5bb2408578..38a76ee025 100644 --- a/.github/workflows/humble-pre-commit.yml +++ b/.github/workflows/humble-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - humble +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index 25c755ada5..6019d08e46 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -18,6 +18,11 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: humble_rhel_binary: name: Humble RHEL binary build diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index c53c46032a..2d0437782b 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -33,15 +33,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: humble - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.humble.repos ref_for_scheduled_build: humble + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: humble + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 8e557b5ee3..5da5fb9d00 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.jazzy.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index cda4969abf..7dd294a55b 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -35,6 +35,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml index 2f9eaf18bd..cfc67a3eac 100644 --- a/.github/workflows/jazzy-check-docs.yml +++ b/.github/workflows/jazzy-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml index c96ac88c32..2f587c7003 100644 --- a/.github/workflows/jazzy-coverage-build.yml +++ b/.github/workflows/jazzy-coverage-build.yml @@ -33,6 +33,11 @@ on: # - '**/CMakeLists.txt' # - 'ros2_controllers.jazzy.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_jazzy: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index e3e3b8a353..b1a60f1528 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: jazzy_debian: diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml index d9ec610bbc..aab5ba52ac 100644 --- a/.github/workflows/jazzy-pre-commit.yml +++ b/.github/workflows/jazzy-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml index 1c62fcf2ac..f39c9cc570 100644 --- a/.github/workflows/jazzy-rhel-semi-binary-build.yml +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: jazzy_rhel: diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index a462d9040a..e38fd5b7ba 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -35,15 +35,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: jazzy - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.jazzy.repos ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: jazzy + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index da3337554c..1f0ec61d40 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index b512eb9db7..a3686144f3 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -36,6 +36,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index 15b7eff57e..1144c4e6c0 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -11,6 +11,10 @@ on: - '**.yaml' - '.github/workflows/rolling-check-docs.yml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 7aab02fa62..6370db30d1 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -33,16 +33,16 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [humble] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: humble + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index 37ffbb84a4..4da98f2d09 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -33,16 +33,16 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-jazzy: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [jazzy] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: jazzy + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 058ff9bb33..abea8a90db 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -30,6 +30,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_rolling: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index d7efd3ea0a..8f9394762b 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: rolling_debian: diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 7bc07ba802..792278d6d2 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index 29a53b810a..0f03ad02c2 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: rolling_rhel: diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index db0dd1fe64..06f4c55612 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -35,15 +35,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: rolling - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: rolling + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rosdoc2.yml b/.github/workflows/rosdoc2.yml index 0771248e79..434344fcaa 100644 --- a/.github/workflows/rosdoc2.yml +++ b/.github/workflows/rosdoc2.yml @@ -8,6 +8,9 @@ on: - ros2_controllers/rosdoc2.yaml - ros2_controllers/package.xml +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true jobs: check: diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a326b663d0..bfb3a612c7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -63,10 +63,11 @@ struct AdmittanceState mass_inv.setZero(); stiffness.setZero(); selected_axes.setZero(); - current_joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_vel = Eigen::VectorXd::Zero(num_joints); - joint_acc = Eigen::VectorXd::Zero(num_joints); + auto idx = static_cast(num_joints); + current_joint_pos = Eigen::VectorXd::Zero(idx); + joint_pos = Eigen::VectorXd::Zero(idx); + joint_vel = Eigen::VectorXd::Zero(idx); + joint_acc = Eigen::VectorXd::Zero(idx); } Eigen::VectorXd current_joint_pos; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 13d4e67fbc..859c7e3336 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -136,9 +136,11 @@ void AdmittanceRule::apply_parameters_update() for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { - admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; - admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * - sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + auto idx = static_cast(i); + admittance_state_.mass_inv[idx] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[idx] = + parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[idx] * admittance_state_.stiffness[idx]); } } @@ -216,12 +218,13 @@ controller_interface::return_type AdmittanceRule::update( // update joint desired joint state for (size_t i = 0; i < num_joints_; ++i) { + auto idx = static_cast(i); desired_joint_state.positions[i] = - reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + reference_joint_state.positions[i] + admittance_state_.joint_pos[idx]; desired_joint_state.velocities[i] = - reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + reference_joint_state.velocities[i] + admittance_state_.joint_vel[idx]; desired_joint_state.accelerations[i] = - reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[idx]; } return controller_interface::return_type::OK; @@ -334,7 +337,7 @@ void AdmittanceRule::process_wrench_measurements( new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); // apply smoothing filter - for (size_t i = 0; i < 6; ++i) + for (Eigen::Index i = 0; i < 6; ++i) { wrench_world_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); @@ -345,18 +348,20 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control { for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { - state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - state_message_.damping.data[i] = admittance_state_.damping[i]; - state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); - state_message_.mass.data[i] = admittance_state_.mass[i]; + auto idx = static_cast(i); + state_message_.stiffness.data[i] = admittance_state_.stiffness[idx]; + state_message_.damping.data[i] = admittance_state_.damping[idx]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[idx]); + state_message_.mass.data[i] = admittance_state_.mass[idx]; } for (size_t i = 0; i < parameters_.joints.size(); ++i) { + auto idx = static_cast(i); state_message_.joint_state.name[i] = parameters_.joints[i]; - state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; - state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; - state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + state_message_.joint_state.position[i] = admittance_state_.joint_pos[idx]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[idx]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[idx]; } state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; @@ -411,7 +416,7 @@ void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) { for (auto row = 0; row < matrix.rows(); row++) { - matrix(row, col) = data[row + col * matrix.rows()]; + matrix(row, col) = data[static_cast(row + col * matrix.rows())]; } } } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 58a5bcd6e2..468c02624b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -340,7 +340,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, admittance_->parameters_.joints, interface, joint_state_interface_[index])) @@ -355,7 +355,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d710c81257..a93d3d3812 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -292,7 +292,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); - odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index c192f1eb5f..ad2e53162a 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadForwardCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index de534c00b6..3ec7ff4ac2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -1091,10 +1091,10 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); - const auto index_in_original_order = std::distance( + const auto index_in_original_order = static_cast(std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i]))); ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 4902fd1dcc..b33a2bf5c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -199,7 +199,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(joints.cbegin(), it); + auto i = static_cast(std::distance(joints.cbegin(), it)); std::string interface = ""; try { @@ -246,7 +246,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(joints.cbegin(), it); + auto i = static_cast(std::distance(joints.cbegin(), it)); std::string interface = ""; try { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d650c369d0..74d4e28f3a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -206,8 +206,8 @@ inline std::vector mapping(const T & t1, const T & t2) } else { - const size_t t1_dist = std::distance(t1.begin(), t1_it); - const size_t t2_dist = std::distance(t2.begin(), t2_it); + const size_t t1_dist = static_cast(std::distance(t1.begin(), t1_it)); + const size_t t2_dist = static_cast(std::distance(t2.begin(), t2_it)); mapping_vector[t1_dist] = t2_dist; } } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 82d9c94aab..365ce993d0 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -385,7 +385,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(logger, error_string.c_str()); + RCLCPP_WARN(logger, "%s", error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -907,7 +907,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { @@ -921,7 +921,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 1c50d26c62..57366f88a8 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -175,8 +175,8 @@ bool Trajectory::sample( interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); } - start_segment_itr = begin() + i; - end_segment_itr = begin() + (i + 1); + start_segment_itr = begin() + static_cast(i); + end_segment_itr = begin() + static_cast(i + 1); if (search_monotonically_increasing) { last_sample_idx_ = i; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7dff81f08f..b0e66394d1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -467,7 +467,9 @@ class TrajectoryControllerTest : public ::testing::Test if (joint_names.empty()) { traj_msg.joint_names = { - joint_names_.begin(), joint_names_.begin() + points_positions[0].size()}; + joint_names_.begin(), + joint_names_.begin() + + static_cast::difference_type>(points_positions[0].size())}; } else { diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 032dc1d666..f3b2ba33a0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -320,7 +320,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.begin(), found_it); + auto position = static_cast(std::distance(ref_msg->dof_names.begin(), found_it)); ref_msg->values[position] = msg->values[i]; ref_msg->values_dot[position] = msg->values_dot[i]; } diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index a0432f65b3..548c95b44a 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -6,7 +6,7 @@ repositories: realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master + version: humble kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 836574f150..3c14013f40 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -81,7 +81,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + odometry_.set_velocity_rolling_window_size( + static_cast(params_.velocity_rolling_window_size)); configure_odometry(); diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index dbe210ed41..8b8bb8bfd8 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -204,7 +204,10 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ reset_accumulators(); } -void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } +void SteeringOdometry::set_odometry_type(const unsigned int type) +{ + config_type_ = static_cast(type); +} double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 1d6daba958..bc04451ad6 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -236,7 +236,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); - odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; params_.publish_ackermann_command = From 950c9c1a056c8d8861000a8f11facb7e1cd9f8c9 Mon Sep 17 00:00:00 2001 From: Rafal Gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Tue, 10 Dec 2024 17:57:39 +0100 Subject: [PATCH 5/5] Improve tf_prefix based on namespace (#1420) Co-authored-by: Kees van Teeffelen <107179662+kjvanteeffelen@users.noreply.github.com> --- diff_drive_controller/src/diff_drive_controller.cpp | 9 +++++---- .../test/test_diff_drive_controller.cpp | 6 +++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a93d3d3812..7c3c9864d2 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -375,13 +375,14 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( tf_prefix = std::string(get_node()->get_namespace()); } - if (tf_prefix == "/") + // Make sure prefix does not start with '/' and always ends with '/' + if (tf_prefix.back() != '/') { - tf_prefix = ""; + tf_prefix = tf_prefix + "/"; } - else + if (tf_prefix.front() == '/') { - tf_prefix = tf_prefix + "/"; + tf_prefix.erase(0, 1); } } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index e29c6bbfee..a984c5a5c3 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -392,7 +392,6 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; @@ -412,10 +411,11 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; + std::string ns_prefix = test_namespace.erase(0, 1) + "/"; /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to the * frame id's */ - ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id); + ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id); } TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned)