Merge remote-tracking branch 'origin/main' into force_mode_arguments

Felix Exner · Felix Exner · commit 577e9fcce9cb · 2024-10-16T15:04:14.000+02:00
diff --git a/.github/helpers/check_urls.sh b/.github/helpers/check_urls.sh
@@ -0,0 +1,67 @@
+#!/bin/bash
+
+set -e
+
+IGNORE_FILES=""
+IGNORE_PATTERNS=""
+
+while getopts ":f:d:p:" opt; do
+    case "${opt}" in
+        f)
+            IGNORE_FILES="${OPTARG}";;
+        d)
+            IGNORE_DIRS="${OPTARG}";;
+        p)
+            IGNORE_PATTERNS="${OPTARG}";;
+        \?)
+            echo "Invalid option -$OPTARG"
+            exit;;
+    esac
+done
+
+read -r -a ignore_files <<<"$IGNORE_FILES"
+read -r -a ignore_dirs <<<"$IGNORE_DIRS"
+read -r -a ignore_patterns <<<"$IGNORE_PATTERNS"
+
+IGNORE_FILES_ARG=""
+for item in "${ignore_files[@]}"; do
+  IGNORE_FILES_ARG="$IGNORE_FILES_ARG --exclude=$item"
+done
+IGNORE_DIRS_ARG=""
+for item in "${ignore_dirs[@]}"; do
+  IGNORE_DIRS_ARG="$IGNORE_DIRS_ARG --exclude-dir=$item"
+done
+
+#Find URLs in code:
+urls=$(grep -oP '(http|ftp|https):\/\/([a-zA-Z0-9_-]+(?:(?:\.[a-zA-Z0-9_-]+)+))([a-zA-Z0-9_.,@?^=%&:\/~+#-]*[a-zA-Z0-9_@?^=%&\/~+#-])?' -rI $IGNORE_FILES_ARG $IGNORE_DIRS_ARG)
+
+fail_counter=0
+
+FAILED_LINKS=()
+for item in $urls; do
+#     echo $item
+    skip=0
+    for pattern in "${ignore_patterns[@]}"; do
+      [[ "$item" =~ $pattern ]] && skip=1
+    done
+
+    if [[ $skip == 1 ]]; then
+      echo "SKIPPING $item"
+      continue
+    fi
+
+    filename=$(echo "$item" | cut -d':' -f1)
+    url=$(echo "$item" | cut -d':' -f2-)
+    echo -n "Checking $url from file $filename"
+    if ! curl --head --silent --fail "$url" 2>&1 > /dev/null; then
+        echo -e " \033[0;31mNOT FOUND\033[32m\n"
+        FAILED_LINKS+=("$url from file $filename")
+        ((fail_counter=fail_counter+1))
+    else
+        printf " \033[32mok\033[0m\n"
+    fi
+done
+
+echo "Failed files:"
+printf '%s\n' "${FAILED_LINKS[@]}"
+exit $fail_counter
diff --git a/.github/workflows/check_links.yml b/.github/workflows/check_links.yml
@@ -0,0 +1,19 @@
+name: Check Links
+on:
+  workflow_dispatch:
+  pull_request:
+  push:
+    branches:
+      - main
+
+jobs:
+  check_links:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - name: Check URLs
+        run: |
+          .github/helpers/check_urls.sh \
+            -d ".git build CMakeModules debian" \
+            -f "package.xml ursim_docker.rst" \
+            -p "vnc\.html"
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -33,13 +33,13 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.17.0
+    rev: v3.18.0
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
 
   - repo: https://github.com/psf/black
-    rev: 24.8.0
+    rev: 24.10.0
     hooks:
       - id: black
         args: ["--line-length=100"]
diff --git a/README.md b/README.md
@@ -1,7 +1,7 @@
 # Universal Robots ROS2 Driver
 
 Universal Robots has become a dominant supplier of lightweight, robotic manipulators for industry, as well as for scientific research and education.
-<center><img src="ur_robot_driver/doc/installation/initial_setup_images/family_photo.png" alt="Universal Robot family" style="width: 80%;"/></center>
+<div align="center"><img src="ur_robot_driver/doc/installation/initial_setup_images/family_photo.png" alt="Universal Robot family" style="width: 90%;"/></div>
 
 This is one of the very first ROS2 manipulator drivers. Some of the new features are enabled by ROS2 and include decreased latency, improved security, and more flexibility regarding middleware configuration. The package contains launch files to quickly get started using the driver as a standalone version or in combination with MoveIt2
 
@@ -103,24 +103,24 @@ For getting started, you'll basically need three steps:
    ```bash
    sudo apt-get install ros-rolling-ur
    ```
-   See the [installation instructions](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/installation/installation.html) for more details and source-build instructions.
+   See the [installation instructions](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/installation/installation.html) for more details and source-build instructions.
 
 2. **Start & Setup the robot**. Once you've installed the driver, [setup the
-   robot](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/installation/robot_setup.html)
+   robot](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/installation/robot_setup.html)
    and [create a program for external
-   control](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/installation/install_urcap_e_series.html).
+   control](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/installation/install_urcap_e_series.html).
 
    Please do this step carefully and extract the calibration as explained
-   [here](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/installation/robot_setup.html#extract-calibration-information).
+   [here](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/installation/robot_setup.html#extract-calibration-information).
    Otherwise the TCP's pose will not be correct inside the ROS ecosystem.
 
    If no real robot is required, you can [use a simulated
-   robot](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/usage.html#usage-with-official-ur-simulator)
+   robot](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/usage/simulation.html#usage-with-official-ur-simulator)
    that will behave almost exactly like the real robot.
 
 
 3. **Start the driver**. See the [usage
-   documentation](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/usage.html) for
+   documentation](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/usage/toc.html) for
    details.
 
    ```bash
@@ -129,7 +129,7 @@ For getting started, you'll basically need three steps:
    ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101
    ```
 
-4. Unless started in [headless mode](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/ROS_INTERFACE.html#headless-mode): Run the external_control program by **pressing `play` on the teach pendant**.
+4. Unless started in [headless mode](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/operation_modes.html#headless-mode): Run the external_control program by **pressing `play` on the teach pendant**.
 
 
 ## MoveIt! support
@@ -142,9 +142,9 @@ Watch MoveIt in action with the Universal Robots ROS2 driver:
   *The video shows free-space trajectory planning around a modeled collision scene object using the MoveIt2 MotionPlanning widget for Rviz2.*
 
 See the [MoveIt!
-section](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/usage.html#using-moveit)
+section](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/usage/move.html#using-moveit)
 of the [Usage
-guide](https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/usage.html) for details.
+guide](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_robot_driver/ur_robot_driver/doc/usage/toc.html) for details.
 
 ## Expected Changes in the Near Future
 
diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package ur
 ^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.12 (2024-10-14)
+-------------------
+
+2.4.11 (2024-10-10)
+-------------------
+
 2.4.10 (2024-09-11)
 -------------------
 * Update maintainers team (`#1088 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1088>`_)
diff --git a/ur/package.xml b/ur/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ur</name>
-  <version>2.4.10</version>
+  <version>2.4.12</version>
   <description>Metapackage for universal robots</description>
   <maintainer email="exner@fzi.de">Felix Exner</maintainer>
   <maintainer email="dipentima@fzi.de">Vincenzo Di Pentima</maintainer>
diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package ur_calibration
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.12 (2024-10-14)
+-------------------
+
+2.4.11 (2024-10-10)
+-------------------
+
 2.4.10 (2024-09-11)
 -------------------
 * Update maintainers team (`#1088 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1088>`_)
diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ur_calibration</name>
-  <version>2.4.10</version>
+  <version>2.4.12</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>
diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst
@@ -2,6 +2,16 @@
 Changelog for package ur_controllers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.12 (2024-10-14)
+-------------------
+
+2.4.11 (2024-10-10)
+-------------------
+* Allow setting the analog output domain when setting an analog IO (`#1123 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1123>`_)
+* Service to get software version of robot (`#964 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/964>`_)
+* Improve usage documentation (`#1110 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1110>`_)
+* Contributors: Felix Exner (fexner), URJala, Rune Søe-Knudsen
+
 2.4.10 (2024-09-11)
 -------------------
 * Updated get_state to get_lifecycle_state (`#1087 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1087>`_)
diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
@@ -37,6 +37,8 @@
 #ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 #define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 
+#include <optional>
+#include <memory>
 #include "angles/angles.h"
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
@@ -73,9 +75,12 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
   };
 
 private:
-  double scaling_factor_{};
+  double scaling_factor_{ 1.0 };
   realtime_tools::RealtimeBuffer<TimeData> time_data_;
 
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> scaling_state_interface_ =
+      std::nullopt;
+
   std::shared_ptr<scaled_joint_trajectory_controller::ParamListener> scaled_param_listener_;
   scaled_joint_trajectory_controller::Params scaled_params_;
 };
diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ur_controllers</name>
-  <version>2.4.10</version>
+  <version>2.4.12</version>
   <description>Provides controllers that use the speed scaling interface of Universal Robots.</description>
 
   <maintainer email="exner@fzi.de">Felix Exner</maintainer>
diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
@@ -50,6 +50,12 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_init()
   // Create the parameter listener and get the parameters
   scaled_param_listener_ = std::make_shared<scaled_joint_trajectory_controller::ParamListener>(get_node());
   scaled_params_ = scaled_param_listener_->get_params();
+  if (!scaled_params_.speed_scaling_interface_name.empty()) {
+    RCLCPP_INFO(get_node()->get_logger(), "Using scaling state from the hardware from interface %s.",
+                scaled_params_.speed_scaling_interface_name.c_str());
+  } else {
+    RCLCPP_INFO(get_node()->get_logger(), "No scaling interface set. This controller will not use speed scaling.");
+  }
 
   return JointTrajectoryController::on_init();
 }
@@ -58,7 +64,10 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st
 {
   controller_interface::InterfaceConfiguration conf;
   conf = JointTrajectoryController::state_interface_configuration();
-  conf.names.push_back(scaled_params_.speed_scaling_interface_name);
+
+  if (!scaled_params_.speed_scaling_interface_name.empty()) {
+    conf.names.push_back(scaled_params_.speed_scaling_interface_name);
+  }
 
   return conf;
 }
@@ -70,17 +79,27 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat
   time_data.period = rclcpp::Duration::from_nanoseconds(0);
   time_data.uptime = get_node()->now();
   time_data_.initRT(time_data);
+
+  // Set scaling interfaces
+  if (!scaled_params_.speed_scaling_interface_name.empty()) {
+    auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) {
+      return (interface.get_name() == scaled_params_.speed_scaling_interface_name);
+    });
+    if (it != state_interfaces_.end()) {
+      scaling_state_interface_ = *it;
+    } else {
+      RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces.");
+    }
+  }
+
   return JointTrajectoryController::on_activate(state);
 }
 
 controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
                                                                           const rclcpp::Duration& period)
 {
-  if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) {
-    scaling_factor_ = state_interfaces_.back().get_value();
-  } else {
-    RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.",
-                 scaled_params_.speed_scaling_interface_name.c_str());
+  if (scaling_state_interface_.has_value()) {
+    scaling_factor_ = scaling_state_interface_->get().get_value();
   }
 
   if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package ur_dashboard_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.12 (2024-10-14)
+-------------------
+
+2.4.11 (2024-10-10)
+-------------------
+
 2.4.10 (2024-09-11)
 -------------------
 * Update maintainers team (`#1088 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1088>`_)
diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ur_dashboard_msgs</name>
-  <version>2.4.10</version>
+  <version>2.4.12</version>
   <description>Messages around the UR Dashboard server.</description>
 
   <maintainer email="exner@fzi.de">Felix Exner</maintainer>
diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst
@@ -2,6 +2,15 @@
 Changelog for package ur_moveit_config
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.12 (2024-10-14)
+-------------------
+
+2.4.11 (2024-10-10)
+-------------------
+* Add note about TEM (`#1136 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1136>`_)
+* [moveit] Add config for trajectory execution and disable execution monitoring by default (`#1132 <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1132>`_)
+* Contributors: Felix Exner (fexner), G.A. vd. Hoorn
+
 2.4.10 (2024-09-11)
 -------------------
 
diff --git a/ur_moveit_config/config/moveit_controllers.yaml b/ur_moveit_config/config/moveit_controllers.yaml
@@ -1,6 +1,12 @@
 # MoveIt uses this configuration for controller management
 moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
 
+trajectory_execution:
+  allowed_execution_duration_scaling: 1.2
+  allowed_goal_duration_margin: 0.5
+  allowed_start_tolerance: 0.01
+  execution_duration_monitoring: false # May lead to unexpectedly aborted goals with scaled JTC
+
 moveit_simple_controller_manager:
   controller_names:
     - scaled_joint_trajectory_controller
diff --git a/ur_moveit_config/doc/index.rst b/ur_moveit_config/doc/index.rst
@@ -23,3 +23,21 @@ interaction using
 
 Now you should be able to use the MoveIt Plugin in rviz2 to plan and execute trajectories with the
 robot as explained `here <https://moveit.picknik.ai/main/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_.
+
+.. note::
+   The MoveIt configuration provided here has Trajectory Execution Monitoring (TEM) *disabled*, as the
+   Scaled Joint Trajectory Controller may cause trajectories to be executed at a lower velocity
+   than they were originally planned by MoveIt. MoveIt's TEM however is not aware of this
+   deliberate slow-down due to scaling and will in most cases unnecessarily (and unexpectedly)
+   abort goals.
+
+   Until this incompatibility is resolved, the default value for ``execution_duration_monitoring``
+   is set to ``false``. Users who wish to temporarily (re)enable TEM at runtime (for use with
+   other, non-scaling controllers) can do so using the ROS 2 parameter services supported by
+   MoveIt.
+
+   .. literalinclude:: ../config/moveit_controllers.yaml
+      :language: yaml
+      :start-at: trajectory_execution:
+      :end-at: execution_duration_monitoring
+      :caption: moveit_controllers.yaml
diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py
@@ -175,6 +175,9 @@ def generate_launch_description():
             moveit_config.planning_pipelines,
             moveit_config.joint_limits,
             warehouse_ros_config,
+            {
+                "use_sim_time": use_sim_time,
+            },
         ],
     )
 
diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml
diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst
diff --git a/ur_robot_driver/doc/usage/move.rst b/ur_robot_driver/doc/usage/move.rst
diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
