Merge branch 'main' into freedrive_mode

UniversalRobots · Dec 18, 2024 · 49c265d · 49c265d
2 parents d4c0ed5 + d465561
commit 49c265d
Show file tree

Hide file tree

Showing 12 changed files with 100 additions and 40 deletions.
diff --git a/.github/workflows/coverage-build.yml b/.github/workflows/coverage-build.yml
@@ -19,32 +19,36 @@ jobs:
       - uses: ros-tooling/setup-ros@v0.7
         with:
           required-ros-distributions: ${{ env.ROS_DISTRO }}
+          use-ros2-testing: true
       - uses: actions/checkout@v4
       - uses: ros-tooling/action-ros-ci@v0.3
         with:
           target-ros2-distro: ${{ env.ROS_DISTRO }}
           # build all packages listed in the meta package
           package-name:
-            ur
+            ur_calibration
             ur_controllers
-            ur_dashboard_msgs
-            ur_moveit_config
             ur_robot_driver
           vcs-repo-file-url: |
             https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/Universal_Robots_ROS2_Driver-not-released.${{ env.ROS_DISTRO }}.repos
           colcon-defaults: |
             {
               "build": {
-                "mixin": ["coverage-gcc"]
+                "mixin": ["coverage-gcc", "coverage-pytest"]
+              },
+              "test": {
+                "mixin": ["coverage-pytest"]
               }
             }
           colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
-          skip-tests: true
-      - uses: codecov/codecov-action@v3
+          skip-tests: false
+      - uses: codecov/codecov-action@v5
         with:
+          fail_ci_if_error: true
           file: ros_ws/lcov/total_coverage.info
           flags: unittests
           name: codecov-umbrella
+          token: ${{ secrets.CODECOV_TOKEN }}
       - uses: actions/upload-artifact@v4
         with:
           name: colcon-logs-${{ matrix.os }}

diff --git a/Universal_Robots_ROS2_Driver.jazzy.repos b/Universal_Robots_ROS2_Driver.jazzy.repos
@@ -27,6 +27,10 @@ repositories:
     type: git
     url: https://github.com/ros-controls/control_msgs.git
     version: master
+  control_toolbox:
+    type: git
+    url: https://github.com/ros-controls/control_toolbox.git
+    version: ros2-master
   realtime_tools:
     type: git
     url: https://github.com/ros-controls/realtime_tools.git

diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos
@@ -27,6 +27,10 @@ repositories:
     type: git
     url: https://github.com/ros-controls/control_msgs.git
     version: master
+  control_toolbox:
+    type: git
+    url: https://github.com/ros-controls/control_toolbox.git
+    version: ros2-master
   realtime_tools:
     type: git
     url: https://github.com/ros-controls/realtime_tools.git

diff --git a/ur/package.xml b/ur/package.xml
@@ -4,8 +4,9 @@
   <name>ur</name>
   <version>2.4.13</version>
   <description>Metapackage for universal robots</description>
-  <maintainer email="exner@fzi.de">Felix Exner</maintainer>
-  <maintainer email="dipentima@fzi.de">Vincenzo Di Pentima</maintainer>
+  <maintainer email="feex@universal-robots.com">Felix Exner</maintainer>
+  <maintainer email="rsk@universal-robots.com">Rune Søe-Knudsen</maintainer>
+  <maintainer email="ros@universal-robots.com">Universal Robots A/S</maintainer>
   <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml
@@ -4,8 +4,9 @@
   <version>2.4.13</version>
   <description>Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF</description>
 
-  <maintainer email="exner@fzi.de">Felix Exner</maintainer>
-  <maintainer email="dipentima@fzi.de">Vincenzo Di Pentima</maintainer>
+  <maintainer email="feex@universal-robots.com">Felix Exner</maintainer>
+  <maintainer email="rsk@universal-robots.com">Rune Søe-Knudsen</maintainer>
+  <maintainer email="ros@universal-robots.com">Universal Robots A/S</maintainer>
 
   <license>BSD-3-Clause</license>
 

diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml
@@ -5,8 +5,9 @@
   <version>2.4.13</version>
   <description>Provides controllers that use the speed scaling interface of Universal Robots.</description>
 
-  <maintainer email="exner@fzi.de">Felix Exner</maintainer>
-  <maintainer email="dipentima@fzi.de">Vincenzo Di Pentima</maintainer>
+  <maintainer email="feex@universal-robots.com">Felix Exner</maintainer>
+  <maintainer email="rsk@universal-robots.com">Rune Søe-Knudsen</maintainer>
+  <maintainer email="ros@universal-robots.com">Universal Robots A/S</maintainer>
 
   <license>BSD-3-Clause</license>
 
@@ -18,6 +19,7 @@
   <author email="grosse@fzi.de">Marvin Große Besselmann</author>
   <author email="lovro.ivanov@gmail.com">Lovro Ivanov</author>
   <author email="zelenak@picknik.ai">Andy Zelenak</author>
+  <author email="dipentima@fzi.de">Vincenzo Di Pentima</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp
@@ -44,6 +44,7 @@
 
 #include <controller_interface/controller_interface.hpp>
 #include <rclcpp/logging.hpp>
+#include <rclcpp/version.h>
 #include <builtin_interfaces/msg/duration.hpp>
 #include <lifecycle_msgs/msg/state.hpp>
 
@@ -327,8 +328,12 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
         scaling_factor_ = scaling_state_interface_->get().get_value();
       }
 
+#if RCLCPP_VERSION_MAJOR >= 17
       active_trajectory_elapsed_time_ += period * scaling_factor_;
-
+#else
+      // This is kept for Humble compatibility
+      active_trajectory_elapsed_time_ = active_trajectory_elapsed_time_ + period * scaling_factor_;
+#endif
       // RCLCPP_INFO(get_node()->get_logger(), "Elapsed trajectory time: %f. Scaling factor: %f, period: %f",
       // active_trajectory_elapsed_time_.seconds(), scaling_factor_, period.seconds());
 

diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml
@@ -5,8 +5,9 @@
   <version>2.4.13</version>
   <description>Messages around the UR Dashboard server.</description>
 
-  <maintainer email="exner@fzi.de">Felix Exner</maintainer>
-  <maintainer email="dipentima@fzi.de">Vincenzo Di Pentima</maintainer>
+  <maintainer email="feex@universal-robots.com">Felix Exner</maintainer>
+  <maintainer email="rsk@universal-robots.com">Rune Søe-Knudsen</maintainer>
+  <maintainer email="ros@universal-robots.com">Universal Robots A/S</maintainer>
 
   <license>BSD-3-Clause</license>
 

diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml
@@ -6,15 +6,17 @@
   <description>
      An example package with MoveIt2 configurations for UR robots.
   </description>
-  <maintainer email="exner@fzi.de">Felix Exner</maintainer>
-  <maintainer email="dipentima@fzi.de">Vincenzo Di Pentima</maintainer>
+  <maintainer email="feex@universal-robots.com">Felix Exner</maintainer>
+  <maintainer email="rsk@universal-robots.com">Rune Søe-Knudsen</maintainer>
+  <maintainer email="ros@universal-robots.com">Universal Robots A/S</maintainer>
 
   <license>Apache2.0</license>
 
   <author email="wilbrandt@fzi.de">Robert Wilbrandt</author>
   <author email="denis@stoglrobotics.de">Denis Stogl</author>
   <author email="lovro.ivanov@gmail.com">Lovro Ivanov</author>
   <author email="zelenak@picknik.ai">Andy Zelenak</author>
+  <author email="dipentima@fzi.de">Vincenzo Di Pentima</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml
@@ -5,8 +5,9 @@
   <version>2.4.13</version>
   <description>The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series.</description>
 
-  <maintainer email="exner@fzi.de">Felix Exner</maintainer>
-  <maintainer email="dipentima@fzi.de">Vincenzo Di Pentima</maintainer>
+  <maintainer email="feex@universal-robots.com">Felix Exner</maintainer>
+  <maintainer email="rsk@universal-robots.com">Rune Søe-Knudsen</maintainer>
+  <maintainer email="ros@universal-robots.com">Universal Robots A/S</maintainer>
 
   <license>BSD-3-Clause</license>
 
@@ -18,6 +19,7 @@
   <author email="grosse@fzi.de">Marvin Große Besselmann</author>
   <author email="lovro.ivanov@gmail.com">Lovro Ivanov</author>
   <author email="zelenak@picknik.ai">Andy Zelenak</author>
+  <author email="dipentima@fzi.de">Vincenzo Di Pentima</author>
   <author>Thomas Timm Andersen</author>
   <author>Simon Rasmussen</author>
   <author>Felix Exner</author>

diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
@@ -765,14 +765,14 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp:
       check_passthrough_trajectory_controller();
     } else {
       ur_driver_->writeKeepalive();
+    }
 
-      if (!std::isnan(force_mode_task_frame_[0]) && !std::isnan(force_mode_selection_vector_[0]) &&
-          !std::isnan(force_mode_wrench_[0]) && !std::isnan(force_mode_type_) && !std::isnan(force_mode_limits_[0]) &&
-          !std::isnan(force_mode_damping_) && !std::isnan(force_mode_gain_scaling_) && ur_driver_ != nullptr) {
-        start_force_mode();
-      } else if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr && force_mode_async_success_ == 2.0) {
-        stop_force_mode();
-      }
+    if (!std::isnan(force_mode_task_frame_[0]) && !std::isnan(force_mode_selection_vector_[0]) &&
+        !std::isnan(force_mode_wrench_[0]) && !std::isnan(force_mode_type_) && !std::isnan(force_mode_limits_[0]) &&
+        !std::isnan(force_mode_damping_) && !std::isnan(force_mode_gain_scaling_) && ur_driver_ != nullptr) {
+      start_force_mode();
+    } else if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr && force_mode_async_success_ == 2.0) {
+      stop_force_mode();
     }
 
     packet_read_ = false;

diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py
@@ -116,19 +116,9 @@ def lookup_tcp_in_base(self, tf_prefix, timepoint):
                 pass
         return trans
 
-    def test_force_mode_controller(self, tf_prefix):
-        self.assertTrue(
-            self._controller_manager_interface.switch_controller(
-                strictness=SwitchController.Request.BEST_EFFORT,
-                activate_controllers=[
-                    "force_mode_controller",
-                ],
-                deactivate_controllers=[
-                    "scaled_joint_trajectory_controller",
-                    "joint_trajectory_controller",
-                ],
-            ).ok
-        )
+    # Implementation of force mode test to be reused
+    # todo: If we move to pytest this could be done using parametrization
+    def run_force_mode(self, tf_prefix):
         self._force_mode_controller_interface = ForceModeInterface(self.node)
 
         # Create task frame for force mode
@@ -237,6 +227,49 @@ def test_force_mode_controller(self, tf_prefix):
             ).ok
         )
 
+    def test_force_mode_controller(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.run_force_mode(tf_prefix)
+
+    def test_force_mode_controller_with_passthrough_controller(self, tf_prefix):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "passthrough_trajectory_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        time.sleep(1)
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=[
+                    "force_mode_controller",
+                ],
+                deactivate_controllers=[
+                    "scaled_joint_trajectory_controller",
+                    "joint_trajectory_controller",
+                ],
+            ).ok
+        )
+        self.run_force_mode(tf_prefix)
+
     def test_illegal_force_mode_types(self, tf_prefix):
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
@@ -443,7 +476,8 @@ def test_deactivating_controller_stops_force_mode(self, tf_prefix):
         trans_after_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
 
         self.assertAlmostEqual(
-            trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z
+            trans_before_wait.transform.translation.z,
+            trans_after_wait.transform.translation.z,
         )
 
     def test_params_out_of_range_fails(self, tf_prefix):