From f43e183ee19de34ee06a1f1b40c48caf4d7ea911 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Tue, 12 Nov 2024 22:29:25 +0100 Subject: [PATCH 01/13] use latest kinematics_interface version --- cartesian_controllers_ros2.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartesian_controllers_ros2.repos b/cartesian_controllers_ros2.repos index f967445..e78b80e 100644 --- a/cartesian_controllers_ros2.repos +++ b/cartesian_controllers_ros2.repos @@ -7,4 +7,4 @@ repositories: external/ros-controls/kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git - version: 1.1.0 + version: master From ccc8c101a9c40be9e42ddee22bdfe20eaba463c8 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Tue, 12 Nov 2024 22:29:52 +0100 Subject: [PATCH 02/13] temporary point to dynamics_interface version for kinematics_interface 1.2.0 --- cartesian_controllers_ros2.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartesian_controllers_ros2.repos b/cartesian_controllers_ros2.repos index e78b80e..2563711 100644 --- a/cartesian_controllers_ros2.repos +++ b/cartesian_controllers_ros2.repos @@ -2,7 +2,7 @@ repositories: external/dynamics_interface: type: git url: https://github.com/tpoignonec/dynamics_interface.git - version: main + version: tpoignonec-refractor_for_kinematics_interface_1.2.0 external/ros-controls/kinematics_interface: type: git From acd4cda653b481cc9b46e0984d203ecfd308a9b6 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Tue, 12 Nov 2024 23:04:55 +0100 Subject: [PATCH 03/13] use new "kyn->initialize()" API --- .../src/cartesian_state_broadcaster.cpp | 13 +++++++++++-- cartesian_vic_controller/src/cartesian_vic_rule.cpp | 12 +++++++++++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp b/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp index 55d8fd3..182ad5b 100644 --- a/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp +++ b/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp @@ -53,6 +53,16 @@ CallbackReturn CartesianStateBroadcaster::on_configure( auto kinematics_plugin_package = get_node()->get_parameter("kinematics_plugin_package").as_string(); + // Load URDF + auto robot_param = rclcpp::Parameter(); + if (!get_node()->get_parameter("robot_description", robot_param)) { + RCLCPP_ERROR( + rclcpp::get_logger("CartesianStateBroadcaster"), + "parameter robot_description not set"); + return CallbackReturn::ERROR; + } + auto robot_description = robot_param.as_string(); + // Load the differential IK plugin if (!kinematics_plugin_name.empty()) { try { @@ -62,8 +72,7 @@ CallbackReturn CartesianStateBroadcaster::on_configure( kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(kinematics_plugin_name)); if (!kinematics_->initialize( - get_node()->get_node_parameters_interface(), - end_effector_name_)) + robot_description, get_node()->get_node_parameters_interface(), "kinematics")) { RCLCPP_ERROR( rclcpp::get_logger("CartesianStateBroadcaster"), diff --git a/cartesian_vic_controller/src/cartesian_vic_rule.cpp b/cartesian_vic_controller/src/cartesian_vic_rule.cpp index 9767fb3..a5591f2 100644 --- a/cartesian_vic_controller/src/cartesian_vic_rule.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_rule.cpp @@ -61,6 +61,15 @@ CartesianVicRule::configure( num_joints_ = num_joints; // reset vic state reset(num_joints); + + // Load URDF + auto robot_param = rclcpp::Parameter(); + if (!parameters_interface->get_parameter("robot_description", robot_param)) { + RCLCPP_ERROR(logger_, "parameter robot_description not set"); + return controller_interface::return_type::ERROR; + } + auto robot_description = robot_param.as_string(); + // Load the dynamics (also used for IK) plugin if (!parameters_.dynamics.plugin_name.empty()) { try { @@ -70,7 +79,8 @@ CartesianVicRule::configure( "dynamics_interface::DynamicsInterface"); dynamics_ = std::unique_ptr( dynamics_loader_->createUnmanagedInstance(parameters_.dynamics.plugin_name)); - if (!dynamics_->initialize(parameters_interface, parameters_.dynamics.tip)) { + if (!dynamics_->initialize( + robot_description, parameters_interface, "dynamics")) { return controller_interface::return_type::ERROR; } } catch (pluginlib::PluginlibException & ex) { From 144b840dfdc82afc97440a2ee3b445ca42918263 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Tue, 12 Nov 2024 23:05:36 +0100 Subject: [PATCH 04/13] make CI format happier --- cartesian_vic_controller/src/cartesian_vic_rule.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cartesian_vic_controller/src/cartesian_vic_rule.cpp b/cartesian_vic_controller/src/cartesian_vic_rule.cpp index a5591f2..f29a61b 100644 --- a/cartesian_vic_controller/src/cartesian_vic_rule.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_rule.cpp @@ -80,7 +80,8 @@ CartesianVicRule::configure( dynamics_ = std::unique_ptr( dynamics_loader_->createUnmanagedInstance(parameters_.dynamics.plugin_name)); if (!dynamics_->initialize( - robot_description, parameters_interface, "dynamics")) { + robot_description, parameters_interface, "dynamics")) + { return controller_interface::return_type::ERROR; } } catch (pluginlib::PluginlibException & ex) { From c504b2e21cbdd3a9e2015469702ef5acbb639968 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:21:08 +0100 Subject: [PATCH 05/13] use main branch of "dynamics_interface" pkg --- cartesian_controllers_ros2.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartesian_controllers_ros2.repos b/cartesian_controllers_ros2.repos index 2563711..e78b80e 100644 --- a/cartesian_controllers_ros2.repos +++ b/cartesian_controllers_ros2.repos @@ -2,7 +2,7 @@ repositories: external/dynamics_interface: type: git url: https://github.com/tpoignonec/dynamics_interface.git - version: tpoignonec-refractor_for_kinematics_interface_1.2.0 + version: main external/ros-controls/kinematics_interface: type: git From 2995dcc13735a8b9efea8a783e46eb805551e9a6 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:26:22 +0100 Subject: [PATCH 06/13] use realtime_publisher.hpp instead of obsolete realtime_publisher.h --- .../cartesian_state_broadcaster/cartesian_state_broadcaster.hpp | 2 +- .../cartesian_vic_controller/cartesian_vic_controller.hpp | 2 +- .../include/cartesian_vic_servo/vic_servo_node.hpp | 2 +- .../cartesian_vic_teleop_controller.hpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp b/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp index 54fb639..34b62eb 100644 --- a/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp +++ b/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp @@ -21,7 +21,7 @@ #include #include -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "cartesian_state_broadcaster/visibility_control.h" #include "controller_interface/controller_interface.hpp" diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp index e2d161e..c3c42d2 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp @@ -38,7 +38,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" // Ros msgs diff --git a/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp b/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp index 04ff5f0..cbb7496 100644 --- a/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp +++ b/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp index 0ff6a23..1c5a2a1 100644 --- a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp @@ -29,7 +29,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" // Base class for VIC controller #include "cartesian_vic_controller/cartesian_vic_controller.hpp" From eb696fce0fce6393f8e2f353819d75f6f4aaf09d Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:28:31 +0100 Subject: [PATCH 07/13] fix call to moveit_servo::Servo::getCurrentRobotState() --- cartesian_vic_servo/src/vic_servo_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartesian_vic_servo/src/vic_servo_node.cpp b/cartesian_vic_servo/src/vic_servo_node.cpp index f960acd..f9900b6 100644 --- a/cartesian_vic_servo/src/vic_servo_node.cpp +++ b/cartesian_vic_servo/src/vic_servo_node.cpp @@ -271,7 +271,7 @@ bool CartesianVicServo::start() } // Reset command queue - auto current_state = servo_->getCurrentRobotState(); + auto current_state = servo_->getCurrentRobotState(false); moveit_servo::updateSlidingWindow( current_state, joint_cmd_rolling_window_, max_expected_latency_, this->now()); From de258d63a2e655b8a1cec9b8dba964ff831ddc7d Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:35:11 +0100 Subject: [PATCH 08/13] oups, forgot one... --- cartesian_vic_servo/src/vic_servo_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartesian_vic_servo/src/vic_servo_node.cpp b/cartesian_vic_servo/src/vic_servo_node.cpp index f9900b6..67de4f6 100644 --- a/cartesian_vic_servo/src/vic_servo_node.cpp +++ b/cartesian_vic_servo/src/vic_servo_node.cpp @@ -651,7 +651,7 @@ bool CartesianVicServo::send_twist_command( } else { // if joint_cmd_rolling_window_ is empty or all commands are outdated, use current robot state joint_cmd_rolling_window_.clear(); - current_state = servo_->getCurrentRobotState(); + current_state = servo_->getCurrentRobotState(false); current_state.velocities *= 0.0; } From 95cfadf1e4bf4f41371465fc70748b4677b84e3b Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:41:00 +0100 Subject: [PATCH 09/13] again --- cartesian_vic_servo/src/demo_twist.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cartesian_vic_servo/src/demo_twist.cpp b/cartesian_vic_servo/src/demo_twist.cpp index f2205f7..768583c 100644 --- a/cartesian_vic_servo/src/demo_twist.cpp +++ b/cartesian_vic_servo/src/demo_twist.cpp @@ -101,7 +101,7 @@ int main(int argc, char * argv[]) // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(false); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); From abb837c115888ffc184a6de7235a2016b28e3ad5 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:51:04 +0100 Subject: [PATCH 10/13] update CI --- .github/workflows/ci.yml | 73 +++++++++------------------------------- 1 file changed, 16 insertions(+), 57 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4021de7..4ee8df2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,68 +1,27 @@ -name: CI +name: CI (jazzy) on: push: branches: [ main ] pull_request: branches: [ main ] + workflow_dispatch: + schedule: + - cron: '0 0 * * 0' jobs: - CI: + build: runs-on: ubuntu-latest - permissions: - contents: read - issues: read - checks: write - pull-requests: write + container: + image: ubuntu:noble steps: - - name: Prepare - run: | - mkdir -p ${{github.workspace}}/src - uses: actions/checkout@v4 + - uses: ros-tooling/setup-ros@v0.7 with: - path: src/cartesian_controllers_ros2 - - - name: Build Docker Image - uses: docker/build-push-action@v6 - with: - tags: cartesian_controllers_ros2:jazzy - file: .docker/ci/Dockerfile - push: false - - - name: Build - uses: addnab/docker-run-action@v3 - with: - image: cartesian_controllers_ros2:jazzy - options: -v ${{github.workspace}}/:/ros/ - run: | - cd /ros # structure = <...>/ros/src/cartesian_controllers_ros2/<...> - . /opt/ros/jazzy/setup.sh - vcs import src < src/cartesian_controllers_ros2/cartesian_controllers_ros2.repos - rosdep install --ignore-src --from-paths . -y -r - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install - - - name: Tests - uses: addnab/docker-run-action@v3 - with: - image: cartesian_controllers_ros2:jazzy - options: -v ${{github.workspace}}/:/ros/ - run: | - cd /ros - . /opt/ros/jazzy/setup.sh - vcs import src < src/cartesian_controllers_ros2/cartesian_controllers_ros2.repos - rosdep install --ignore-src --from-paths . -y -r - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install - source install/setup.bash - colcon test --event-handlers console_direct+ - colcon test-result - - - name: Upload Tests to Artifacts - uses: actions/upload-artifact@v4 - if: always() - with: - name: test-results - path: build/*/test_results/*/*.xml - - - name: Publish Unit Test Results - uses: EnricoMi/publish-unit-test-result-action@v2 - if: always() + use-ros2-testing: true + - name: Print ls and pwd + run: | + ls + pwd + - uses: ros-tooling/action-ros-ci@v0.3 with: - files: build/*/test_results/*/*.xml + target-ros2-distro: jazzy + vcs-repo-file-url: ./cartesian_controllers_ros2.repos \ No newline at end of file From 95eca738b749aaecc9d80a1a5501f908c31d93d4 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 21:52:47 +0100 Subject: [PATCH 11/13] lint CI --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4ee8df2..b7aaade 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -24,4 +24,4 @@ jobs: - uses: ros-tooling/action-ros-ci@v0.3 with: target-ros2-distro: jazzy - vcs-repo-file-url: ./cartesian_controllers_ros2.repos \ No newline at end of file + vcs-repo-file-url: ./cartesian_controllers_ros2.repos From fff2d2997c281a505c8b054443e557f05a4b525f Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 22:01:31 +0100 Subject: [PATCH 12/13] Revert "update CI" This reverts commit abb837c115888ffc184a6de7235a2016b28e3ad5. --- .github/workflows/ci.yml | 73 +++++++++++++++++++++++++++++++--------- 1 file changed, 57 insertions(+), 16 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b7aaade..4021de7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,27 +1,68 @@ -name: CI (jazzy) +name: CI on: push: branches: [ main ] pull_request: branches: [ main ] - workflow_dispatch: - schedule: - - cron: '0 0 * * 0' jobs: - build: + CI: runs-on: ubuntu-latest - container: - image: ubuntu:noble + permissions: + contents: read + issues: read + checks: write + pull-requests: write steps: + - name: Prepare + run: | + mkdir -p ${{github.workspace}}/src - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@v0.7 with: - use-ros2-testing: true - - name: Print ls and pwd - run: | - ls - pwd - - uses: ros-tooling/action-ros-ci@v0.3 + path: src/cartesian_controllers_ros2 + + - name: Build Docker Image + uses: docker/build-push-action@v6 + with: + tags: cartesian_controllers_ros2:jazzy + file: .docker/ci/Dockerfile + push: false + + - name: Build + uses: addnab/docker-run-action@v3 + with: + image: cartesian_controllers_ros2:jazzy + options: -v ${{github.workspace}}/:/ros/ + run: | + cd /ros # structure = <...>/ros/src/cartesian_controllers_ros2/<...> + . /opt/ros/jazzy/setup.sh + vcs import src < src/cartesian_controllers_ros2/cartesian_controllers_ros2.repos + rosdep install --ignore-src --from-paths . -y -r + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install + + - name: Tests + uses: addnab/docker-run-action@v3 + with: + image: cartesian_controllers_ros2:jazzy + options: -v ${{github.workspace}}/:/ros/ + run: | + cd /ros + . /opt/ros/jazzy/setup.sh + vcs import src < src/cartesian_controllers_ros2/cartesian_controllers_ros2.repos + rosdep install --ignore-src --from-paths . -y -r + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install + source install/setup.bash + colcon test --event-handlers console_direct+ + colcon test-result + + - name: Upload Tests to Artifacts + uses: actions/upload-artifact@v4 + if: always() + with: + name: test-results + path: build/*/test_results/*/*.xml + + - name: Publish Unit Test Results + uses: EnricoMi/publish-unit-test-result-action@v2 + if: always() with: - target-ros2-distro: jazzy - vcs-repo-file-url: ./cartesian_controllers_ros2.repos + files: build/*/test_results/*/*.xml From dc8282ebd047ce05a5f0210b3e735064cbf0e857 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Sat, 18 Jan 2025 22:08:34 +0100 Subject: [PATCH 13/13] ignore deps during tests --- .github/workflows/ci.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4021de7..13216b3 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -51,7 +51,8 @@ jobs: rosdep install --ignore-src --from-paths . -y -r colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install source install/setup.bash - colcon test --event-handlers console_direct+ + DEPS_PKG_NAMES=$(colcon list -n --base-paths src/external) + colcon test --event-handlers console_direct+ console_package_list+ --packages-skip $DEPS_PKG_NAMES colcon test-result - name: Upload Tests to Artifacts