From 78020666bf22b69b4287259f023864dcc6f17780 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 25 Sep 2024 15:02:38 +0200 Subject: [PATCH 1/9] Auto-update pre-commit hooks (backport of #1094) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7393b848f..72f0efc09 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -120,7 +120,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: 0.9.0a1 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] From 72087b1a5822b10a9d642f32fe0069083553f3c1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:16:07 +0200 Subject: [PATCH 2/9] Assure the description is loaded as string (backport of #1107) If this isn't explicitly specified, the description string might be interpreted as a yaml content, which leads to problems, obviously. --- ur_robot_driver/launch/ur_control.launch.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index bbcdd5fd0..9c1a036a7 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -30,7 +30,7 @@ # Author: Denis Stogl from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.parameter_descriptions import ParameterFile, ParameterValue from launch_ros.substitutions import FindPackageShare from launch import LaunchDescription @@ -200,7 +200,9 @@ def launch_setup(context, *args, **kwargs): " ", ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = { + "robot_description": ParameterValue(value=robot_description_content, value_type=str) + } initial_joint_controllers = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", controllers_file] From 75c37f1f6eb3c1ae2541df7d0e4177d651db7b2d Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 7 Oct 2024 10:26:21 +0200 Subject: [PATCH 3/9] Auto-update pre-commit hooks (backport of #1118) --- .pre-commit-config.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 72f0efc09..cc6a89647 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -69,7 +69,7 @@ repos: - id: ament_cppcheck name: ament_cppcheck description: Static code analysis of C/C++ files. - stages: [commit] + stages: [pre-commit] entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -80,7 +80,7 @@ repos: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] + stages: [pre-commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -102,7 +102,7 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] + stages: [pre-commit] entry: ament_lint_cmake language: system files: CMakeLists.txt$ @@ -113,7 +113,7 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] + stages: [pre-commit] entry: ament_copyright language: system args: ['--exclude', 'ur_robot_driver/doc/conf.py', 'ur_calibration/doc/conf.py'] From 6be649f6e10aa034e1259bb5fd89deeab923d632 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 8 Oct 2024 12:48:59 +0200 Subject: [PATCH 4/9] Service to get software version of robot (backport of #964) * Implemented get_version service * Implemented test of get_version service and integrated it with the test of tool contact * Update ur_robot_driver/test/test_common.py Co-authored-by: Felix Exner (fexner) * Renamed service to "GetRobotSoftwareVersion" everywhere. Also moved the version information from being stored in the command interface to be stored in the state interface * Remove tool contact from test. * Implemented test of get_version service and integrated it with the test of tool contact * Renamed service to "GetRobotSoftwareVersion" everywhere. Also moved the version information from being stored in the command interface to be stored in the state interface * Remove tool contact from test. * Create new URConfigurationController And moved the get_robot_software_version service in to it. * Make configuration controller thread safe Also minor cleanup and add testing of the robot software version service * Use ptr-safe RealTimeBoxBestEffort the RealTimeBox used before is not really real-time safe and the way it was implemented there was unnecessary data allocation in both, the activate method and the service callback. Using the RealTimeBoxBestEffort makes allocating additional memory unnecessary and makes things really thread-safe. * Added back files that were mistakenly deleted --- ur_controllers/CMakeLists.txt | 9 +- ur_controllers/controller_plugins.xml | 5 + .../ur_configuration_controller.hpp | 105 ++++++++++++++ .../src/ur_configuration_controller.cpp | 128 ++++++++++++++++++ ...r_configuration_controller_parameters.yaml | 6 + ur_robot_driver/config/ur_controllers.yaml | 6 + .../ur_robot_driver/hardware_interface.hpp | 4 + ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/src/hardware_interface.cpp | 20 +++ ur_robot_driver/test/robot_driver.py | 7 + ur_robot_driver/test/test_common.py | 15 +- 11 files changed, 304 insertions(+), 3 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/ur_configuration_controller.hpp create mode 100644 ur_controllers/src/ur_configuration_controller.cpp create mode 100644 ur_controllers/src/ur_configuration_controller_parameters.yaml diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..5ea7bbe86 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -54,10 +54,16 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + ur_configuration_controller_parameters + src/ur_configuration_controller_parameters.yaml +) + add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp - src/gpio_controller.cpp) + src/gpio_controller.cpp + src/ur_configuration_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include @@ -66,6 +72,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + ur_configuration_controller_parameters ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f0058ab55..fa4b63987 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,4 +14,9 @@ This controller publishes the Tool IO. + + + Controller used to get and change the configuration of the robot + + diff --git a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp new file mode 100644 index 000000000..3775ee72c --- /dev/null +++ b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp @@ -0,0 +1,105 @@ +// Copyright 2024, Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Jacob Larsen jala@universal-robots.com + * \date 2024-07-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ +#define UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ + +// TODO(fmauch): Currently, the realtime_box_best_effort doesn't include this +#include +#include // NOLINT + +#include + +#include + +#include "ur_msgs/srv/get_robot_software_version.hpp" +#include "ur_configuration_controller_parameters.hpp" + +namespace ur_controllers +{ + +// Struct to hold version information +struct VersionInformation +{ + uint32_t major = 0, minor = 0, build = 0, bugfix = 0; +}; + +// Enum for indexing into state interfaces. +enum StateInterfaces +{ + ROBOT_VERSION_MAJOR = 0, + ROBOT_VERSION_MINOR = 1, + ROBOT_VERSION_BUILD = 2, + ROBOT_VERSION_BUGFIX = 3, +}; + +class URConfigurationController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + CallbackReturn on_init() override; + +private: + realtime_tools::RealtimeBoxBestEffort> robot_software_version_{ + std::make_shared() + }; + + rclcpp::Service::SharedPtr get_robot_software_version_srv_; + + bool getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr req, + ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp); + + std::shared_ptr param_listener_; + ur_configuration_controller::Params params_; +}; +} // namespace ur_controllers + +#endif // UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_ diff --git a/ur_controllers/src/ur_configuration_controller.cpp b/ur_controllers/src/ur_configuration_controller.cpp new file mode 100644 index 000000000..a6ec2d24b --- /dev/null +++ b/ur_controllers/src/ur_configuration_controller.cpp @@ -0,0 +1,128 @@ +// Copyright 2024, Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Jacob Larsen jala@universal-robots.com + * \date 2024-07-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include +namespace ur_controllers +{ + +controller_interface::CallbackReturn URConfigurationController::on_init() +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +URConfigurationController::on_configure(const rclcpp_lifecycle::State& /* previous_state */) +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + + get_robot_software_version_srv_ = get_node()->create_service( + "~/get_robot_software_version", std::bind(&URConfigurationController::getRobotSoftwareVersion, this, + std::placeholders::_1, std::placeholders::_2)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration URConfigurationController::command_interface_configuration() const +{ + // No command interfaces currently + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + return config; +} + +controller_interface::InterfaceConfiguration URConfigurationController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = params_.tf_prefix; + + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_minor"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_build"); + config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_bugfix"); + + return config; +} + +controller_interface::return_type URConfigurationController::update(const rclcpp::Time& /* time */, + const rclcpp::Duration& /* period */) +{ + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */) +{ + robot_software_version_.set([this](const std::shared_ptr ptr) { + ptr->major = state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value(); + ptr->minor = state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value(); + ptr->build = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value(); + ptr->bugfix = state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value(); + }); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +URConfigurationController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +bool URConfigurationController::getRobotSoftwareVersion( + ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/, + ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp) +{ + std::shared_ptr temp; + return robot_software_version_.tryGet([resp](const std::shared_ptr ptr) { + resp->major = ptr->major; + resp->minor = ptr->minor; + resp->build = ptr->build; + resp->bugfix = ptr->bugfix; + }); +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::URConfigurationController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/ur_configuration_controller_parameters.yaml b/ur_controllers/src/ur_configuration_controller_parameters.yaml new file mode 100644 index 000000000..4cbdf8aa1 --- /dev/null +++ b/ur_controllers/src/ur_configuration_controller_parameters.yaml @@ -0,0 +1,6 @@ +ur_configuration_controller: + tf_prefix: { + type: string, + default_value: "", + description: "URDF prefix of the corresponding arm" + } diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index a512dc1ca..544dd1fbd 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,8 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + ur_configuration_controller: + type: ur_controllers/URConfigurationController speed_scaling_state_broadcaster: ros__parameters: @@ -34,6 +36,10 @@ io_and_status_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" +ur_configuration_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + force_torque_sensor_broadcaster: ros__parameters: sensor_name: $(var tf_prefix)tcp_fts_sensor diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 67e27a222..6fc227766 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -191,6 +191,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; + double get_robot_software_version_major_; + double get_robot_software_version_minor_; + double get_robot_software_version_bugfix_; + double get_robot_software_version_build_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 9c1a036a7..053ed4996 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -295,6 +295,7 @@ def launch_setup(context, *args, **kwargs): "force_torque_sensor_broadcaster", "joint_state_broadcaster", "speed_scaling_state_broadcaster", + "ur_configuration_controller", ] }, ], @@ -337,6 +338,7 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "ur_configuration_controller", ] controllers_inactive = ["forward_position_controller"] diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 6404e69fc..b682bfd9b 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -44,6 +44,7 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/ur/tool_communication.h" +#include "ur_client_library/ur/version_information.h" #include "rclcpp/rclcpp.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -231,6 +232,18 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_major", &get_robot_software_version_major_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_minor", &get_robot_software_version_minor_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_bugfix", &get_robot_software_version_bugfix_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "get_robot_software_version", "get_version_build", &get_robot_software_version_build_)); + return state_interfaces; } @@ -454,6 +467,13 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou "README.md] for details."); } + // Export version information to state interfaces + urcl::VersionInformation version_info = ur_driver_->getVersion(); + get_robot_software_version_major_ = version_info.major; + get_robot_software_version_minor_ = version_info.minor; + get_robot_software_version_build_ = version_info.build; + get_robot_software_version_bugfix_ = version_info.bugfix; + async_thread_ = std::make_shared(&URPositionHardwareInterface::asyncThread, this); RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!"); diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 96c859947..88fcffce3 100644 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -49,6 +49,7 @@ ControllerManagerInterface, DashboardInterface, IoStatusInterface, + ConfigurationInterface, generate_driver_test_description, ) @@ -92,6 +93,7 @@ def init_robot(self): self._dashboard_interface = DashboardInterface(self.node) self._controller_manager_interface = ControllerManagerInterface(self.node) self._io_status_controller_interface = IoStatusInterface(self.node) + self._configuration_controller_interface = ConfigurationInterface(self.node) self._scaled_follow_joint_trajectory = ActionInterface( self.node, @@ -108,6 +110,11 @@ def setUp(self): # Test functions # + def test_get_robot_software_version(self): + self.assertNotEqual( + self._configuration_controller_interface.get_robot_software_version().major, 0 + ) + def test_start_scaled_jtc_controller(self): self.assertTrue( self._controller_manager_interface.switch_controller( diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index f1eac75af..8db7b6835 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -52,7 +52,7 @@ IsProgramRunning, Load, ) -from ur_msgs.srv import SetIO +from ur_msgs.srv import SetIO, GetRobotSoftwareVersion TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. @@ -240,7 +240,18 @@ class IoStatusInterface( _ServiceInterface, namespace="/io_and_status_controller", initial_services={"set_io": SetIO}, - services={"resend_robot_program": Trigger}, + services={ + "resend_robot_program": Trigger, + }, +): + pass + + +class ConfigurationInterface( + _ServiceInterface, + namespace="/ur_configuration_controller", + initial_services={"get_robot_software_version": GetRobotSoftwareVersion}, + services={}, ): pass From aca2a0e0baff7fcf2269f68f5a9e73e7ccb6f568 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 8 Oct 2024 13:43:00 +0200 Subject: [PATCH 5/9] Update ur_msgs upstream branch (#1126) --- Universal_Robots_ROS2_Driver.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos index 61ee8ef3a..14de188d5 100644 --- a/Universal_Robots_ROS2_Driver.humble.repos +++ b/Universal_Robots_ROS2_Driver.humble.repos @@ -10,7 +10,7 @@ repositories: ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git - version: foxy-devel + version: humble-devel ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git From 1a5c861f57bac8b187efc1d140180a0a86ba64d1 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Thu, 10 Oct 2024 11:42:44 +0200 Subject: [PATCH 6/9] [moveit] Disable execution_duration_monitoring by default (#1133) * [moveit] Disable execution_duration_monitoring by default * Add note about disabling TEM in docs * Update documentation --- ur_moveit_config/launch/ur_moveit.launch.py | 2 ++ ur_robot_driver/doc/usage.rst | 20 ++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index 4226dfa52..dba83fc74 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -188,6 +188,8 @@ def launch_setup(context, *args, **kwargs): "trajectory_execution.allowed_execution_duration_scaling": 1.2, "trajectory_execution.allowed_goal_duration_margin": 0.5, "trajectory_execution.allowed_start_tolerance": 0.01, + # Execution time monitoring can be incompatible with the scaled JTC + "trajectory_execution.execution_duration_monitoring": False, } planning_scene_monitor_parameters = { diff --git a/ur_robot_driver/doc/usage.rst b/ur_robot_driver/doc/usage.rst index 014c0fd23..208177d8d 100644 --- a/ur_robot_driver/doc/usage.rst +++ b/ur_robot_driver/doc/usage.rst @@ -191,6 +191,26 @@ To test the driver with the example MoveIt-setup, first start the driver as desc Now you should be able to use the MoveIt Plugin in rviz2 to plan and execute trajectories with the robot as explained `here `_. +.. 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:: ../../ur_moveit_config/launch/ur_moveit.launch.py + :language: python + :start-at: trajectory_execution = + :end-at: execution_duration_monitoring": False + :append: } + :dedent: 4 + :caption: ur_moveit_config/launch/ur_moveit.launch.py + Fake hardware on ROS2 Galactic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 60b09db5ddcd22a476d56765ff84632836ff6ba8 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 16 Oct 2024 09:30:49 +0200 Subject: [PATCH 7/9] README: Center family photo (backport pf #1122) Increased size of family photo (cherry picked from commit 980e7fbf9940870e1d9b35944c2a84a51eb0dc39) Co-authored-by: URJala <159417921+URJala@users.noreply.github.com> --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2d50dff45..432604d11 100644 --- 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. -
Universal Robot family
+
Universal Robot family
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 From 255355d7876f1cec996f957107c548f9848cf7ed Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 16 Oct 2024 09:32:15 +0200 Subject: [PATCH 8/9] Auto-update pre-commit hooks (backport of #1147) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cc6a89647..c83afa27e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,13 +33,13 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.7.2 + rev: v3.18.0 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.3.0 + rev: 24.10.0 hooks: - id: black args: ["--line-length=100"] From 84e371ca4074cfca63f8fd5b6295ed2db20f009c Mon Sep 17 00:00:00 2001 From: Gregory LeMasurier Date: Wed, 16 Oct 2024 17:49:19 -0400 Subject: [PATCH 9/9] revert previous merge --- README.md | 2 +- ci_status.md | 142 ++++++++++++++++ ur/CHANGELOG.rst | 3 + ur/package.xml | 2 +- ur_bringup/CHANGELOG.rst | 3 + ur_bringup/package.xml | 2 +- ur_calibration/CHANGELOG.rst | 3 + ur_calibration/package.xml | 2 +- ur_controllers/CHANGELOG.rst | 5 + ur_controllers/package.xml | 2 +- .../scaled_joint_trajectory_controller.cpp | 58 ++++--- ur_dashboard_msgs/CHANGELOG.rst | 3 + ur_dashboard_msgs/package.xml | 2 +- ur_moveit_config/CHANGELOG.rst | 3 + ur_moveit_config/config/controllers.yaml | 8 - ur_moveit_config/config/joint_limits.yaml | 70 +------- ur_moveit_config/config/kinematics.yaml | 1 - ur_moveit_config/launch/ur_moveit.launch.py | 15 +- ur_moveit_config/package.xml | 2 +- ur_moveit_config/srdf/ur_macro.srdf.xacro | 154 ------------------ ur_robot_driver/CHANGELOG.rst | 6 + ur_robot_driver/CMakeLists.txt | 4 + ur_robot_driver/config/ur_controllers.yaml | 23 --- ur_robot_driver/launch/ur_control.launch.py | 32 ++-- ur_robot_driver/package.xml | 2 +- ur_robot_driver/test/launch_args.py | 148 +++++++++++++++++ ur_robot_driver/test/test_common.py | 25 +-- 27 files changed, 414 insertions(+), 308 deletions(-) create mode 100644 ci_status.md create mode 100644 ur_robot_driver/test/launch_args.py diff --git a/README.md b/README.md index 0019e56a9..432604d11 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ This is one of the very first ROS2 manipulator drivers. Some of the new features This driver is developed on top of [Universal_Robots_Client_Library](https://github.com/UniversalRobots/Universal_Robots_Client_Library) and support some key cobot functionalities like; pause at emergency stop, safeguard stop, automatic speed scaling to avoid violate the safety setting and manually speed scaling from the teach pendant. In addition the externalControl URCap makes it possible to include ROS2 behaviors in the robot program. -The driver is compatible across the entire line of UR robots -- from 3 kg payload to 16 kg payload and includes both the CB3 and the E-series. +The driver is compatible across the entire line of UR robots -- from 3 kg payload to 30 kg payload and includes all robots from the CB3 series and newer. Check also [presentations and videos](ur_robot_driver/doc/resources/README.md) about this driver. diff --git a/ci_status.md b/ci_status.md new file mode 100644 index 000000000..4df0e8f42 --- /dev/null +++ b/ci_status.md @@ -0,0 +1,142 @@ +# Build Status + +This page gives a detailed overview of the build status of this repository. Please note that due to +upstream changes some pipelines might turn red temporarily which can be expected behavior. For each +red pipeline there should be a corresponding issue labeled with [ci-failure](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues?q=is%3Aissue+is%3Aopen+label%3Aci-failure). + + + + + + + + + + + + + + + + + + + + + + + + + + +
HumbleIronJazzyRolling
humbleironmainmain
+ + Humble Binary Main +
+ + Humble Binary Testing +
+ + Humble Semi-Binary Main +
+ + Humble Semi-Binary Testing + + 		+ + Iron Binary Main +
+ + Iron Binary Testing +
+ + Iron Semi-Binary Main +
+ + Iron Semi-Binary Testing + + 		+ + Jazzy Binary Main +
+ + Jazzy Binary Testing +
+ + Jazzy Semi-Binary Main +
+ + Jazzy Semi-Binary Testing + + 		+ + Rolling Binary Main +
+ + Rolling Binary Testing +
+ + Rolling Semi-Binary Main +
+ + Rolling Semi-Binary Testing + +
+
+ + + + + 		+
+ + + + + 		+
+ + + + + 		+
+ + + + +
+ +**NOTE**: There are two build stages checking current and future compatibility of the driver. + +1. Binary builds - against released packages (main and testing) in ROS distributions. Shows that + direct local build is possible and integration tests work against the released state. + + Uses repos file: `src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver-not-released..repos` + +1. Semi-binary builds - against released core ROS packages (main and testing), but the immediate dependencies are pulled from source. + Shows that local build with dependencies is possible and if this fails we can expect that after the next package sync we will not be able to build. + + Uses repos file: `src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver.repos` + +Each of these stages also performs integration tests using ursim. In order to execute these tests locally, they have to be enabled: + + ``` + colcon build --packages-select ur_robot_driver --cmake-args -DUR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS=On + ``` diff --git a/ur/CHANGELOG.rst b/ur/CHANGELOG.rst index 59ce6572e..9bfd506ca 100644 --- a/ur/CHANGELOG.rst +++ b/ur/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur ^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.15 (2024-07-26) +------------------- + 2.2.14 (2024-07-01) ------------------- diff --git a/ur/package.xml b/ur/package.xml index 32bc61108..c36b73c0c 100644 --- a/ur/package.xml +++ b/ur/package.xml @@ -2,7 +2,7 @@ ur - 2.2.14 + 2.2.15 Metapackage for universal robots Felix Exner Robert Wilbrandt diff --git a/ur_bringup/CHANGELOG.rst b/ur_bringup/CHANGELOG.rst index 847e4d6ed..ad09d48af 100644 --- a/ur_bringup/CHANGELOG.rst +++ b/ur_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.15 (2024-07-26) +------------------- + 2.2.14 (2024-07-01) ------------------- diff --git a/ur_bringup/package.xml b/ur_bringup/package.xml index 648b6e13f..9aad69454 100644 --- a/ur_bringup/package.xml +++ b/ur_bringup/package.xml @@ -2,7 +2,7 @@ ur_bringup - 2.2.14 + 2.2.15 Launch file and run-time configurations, e.g. controllers. Denis Stogl diff --git a/ur_calibration/CHANGELOG.rst b/ur_calibration/CHANGELOG.rst index 07be7a2c9..953daf0bf 100644 --- a/ur_calibration/CHANGELOG.rst +++ b/ur_calibration/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_calibration ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.15 (2024-07-26) +------------------- + 2.2.14 (2024-07-01) ------------------- diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index ea8fa6ad6..3d3cb6973 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -1,7 +1,7 @@ ur_calibration - 2.2.14 + 2.2.15 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 Felix Exner diff --git a/ur_controllers/CHANGELOG.rst b/ur_controllers/CHANGELOG.rst index 47b572159..2d3ad37b6 100644 --- a/ur_controllers/CHANGELOG.rst +++ b/ur_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ur_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.15 (2024-07-26) +------------------- +* Updated scaled JTC to latest upstream updates (`#1067 `_) +* Contributors: Felix Exner (fexner) + 2.2.14 (2024-07-01) ------------------- diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index 7212a849a..e5b7197e8 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -2,7 +2,7 @@ ur_controllers - 2.2.14 + 2.2.15 Provides controllers that use the speed scaling interface of Universal Robots. Denis Stogl diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 4ccd1ec12..d9009898b 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -86,10 +86,11 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } + auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); } auto compute_error_for_joint = [&](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current, @@ -138,6 +139,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // currently carrying out a trajectory if (has_active_trajectory()) { + // Main Speed scaling difference... // Adjust time with scaling factor TimeData time_data; time_data.time = time; @@ -153,9 +155,10 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const if (!traj_external_point_ptr_->is_sampled_already()) { first_sample = true; if (params_.open_loop_control) { - traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_); + traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_, + joints_angle_wraparound_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_); + traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_, joints_angle_wraparound_); } } @@ -178,12 +181,13 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const bool outside_goal_tolerance = false; bool within_goal_time = true; const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + auto active_tol = active_tolerances_.readFromRT(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances if (!before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + RCLCPP_WARN(logger, "Aborted due to command timeout"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -195,21 +199,25 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const // Always check the state tolerance on the first sample in case the first sample // is the last point - if ((before_last_point || first_sample) && - !check_state_tolerance_per_joint(state_error_, index, default_tolerances_.state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) { + // print output per default, goal will be aborted afterwards + if ((before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && + !check_state_tolerance_per_joint(state_error_, index, active_tol->state_tolerance[index], + true /* show_errors */)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance - if (!before_last_point && - !check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index], - false) && - *(rt_is_holding_.readFromRT()) == false) { + if (!before_last_point && *(rt_is_holding_.readFromRT()) == false && + !check_state_tolerance_per_joint(state_error_, index, active_tol->goal_state_tolerance[index], + false /* show_errors */)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) { - if (time_difference > default_tolerances_.goal_time_tolerance) { + if (active_tol->goal_time_tolerance != 0.0) { + // if we exceed goal_time_tolerance set it to aborted + if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index], + true /* show_errors */); } } } @@ -263,41 +271,47 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const if (tolerance_violated_while_moving) { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + result->set__error_string("Aborted due to path tolerance violation"); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); } else if (!before_last_point) { + // check goal tolerance if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + result->set__error_string("Goal successfully reached!"); + active_goal->setSucceeded(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { + const std::string error_string = + "Aborted due to goal_time_tolerance exceeding by " + std::to_string(time_difference) + " seconds"; + auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + result->set__error_string(error_string); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + RCLCPP_WARN(logger, error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -305,12 +319,12 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const } } else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); } else if (!before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { - RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); diff --git a/ur_dashboard_msgs/CHANGELOG.rst b/ur_dashboard_msgs/CHANGELOG.rst index fe03cb0ff..95a696584 100644 --- a/ur_dashboard_msgs/CHANGELOG.rst +++ b/ur_dashboard_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_dashboard_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.15 (2024-07-26) +------------------- + 2.2.14 (2024-07-01) ------------------- diff --git a/ur_dashboard_msgs/package.xml b/ur_dashboard_msgs/package.xml index 35df29681..ea0298e30 100644 --- a/ur_dashboard_msgs/package.xml +++ b/ur_dashboard_msgs/package.xml @@ -2,7 +2,7 @@ ur_dashboard_msgs - 2.2.14 + 2.2.15 Messages around the UR Dashboard server. Felix Exner diff --git a/ur_moveit_config/CHANGELOG.rst b/ur_moveit_config/CHANGELOG.rst index 48996f7d9..0ee305cef 100644 --- a/ur_moveit_config/CHANGELOG.rst +++ b/ur_moveit_config/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ur_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.2.15 (2024-07-26) +------------------- + 2.2.14 (2024-07-01) ------------------- diff --git a/ur_moveit_config/config/controllers.yaml b/ur_moveit_config/config/controllers.yaml index 0c58c3b1a..784ebd315 100644 --- a/ur_moveit_config/config/controllers.yaml +++ b/ur_moveit_config/config/controllers.yaml @@ -1,7 +1,6 @@ controller_names: - scaled_joint_trajectory_controller - joint_trajectory_controller - - robotiq_gripper_controller scaled_joint_trajectory_controller: @@ -28,10 +27,3 @@ joint_trajectory_controller: - wrist_1_joint - wrist_2_joint - wrist_3_joint - -robotiq_gripper_controller: - action_ns: gripper_cmd - type: GripperCommand - default: true - joints: - - robotiq_85_left_knuckle_joint diff --git a/ur_moveit_config/config/joint_limits.yaml b/ur_moveit_config/config/joint_limits.yaml index c8b4bcd4e..dc2169495 100644 --- a/ur_moveit_config/config/joint_limits.yaml +++ b/ur_moveit_config/config/joint_limits.yaml @@ -5,73 +5,21 @@ # cases. For specific applications, higher values might lead to better execution performance. joint_limits: - elbow_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false + shoulder_pan_joint: + has_acceleration_limits: true max_acceleration: 5.0 - robotiq_85_left_finger_tip_joint: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - robotiq_85_left_inner_knuckle_joint: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - robotiq_85_left_knuckle_joint: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: false - max_acceleration: 0 - robotiq_85_right_finger_tip_joint: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - robotiq_85_right_inner_knuckle_joint: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - robotiq_85_right_knuckle_joint: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: false - max_acceleration: 0 shoulder_lift_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false + has_acceleration_limits: true + max_acceleration: 5.0 + elbow_joint: + has_acceleration_limits: true max_acceleration: 5.0 - max_position: -1.0335224310504358 - min_position: -2.7769482771502894 - shoulder_pan_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false - max_acceleration: 5.0 - max_position: 4.71238899 - min_position: 1.57079633 wrist_1_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 5.0 - min_position: -3.1415926535897931 - max_position: -1.2 wrist_2_joint: - has_velocity_limits: true - max_velocity: 3.1415926535897931 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 5.0 - max_position: 0.0 - min_position: -3.1415926535897931 wrist_3_joint: - has_velocity_limits: true - max_velocity: 10.0 - has_acceleration_limits: false + has_acceleration_limits: true max_acceleration: 5.0 - max_position: 3.66519143 - min_position: -0.52359877559 diff --git a/ur_moveit_config/config/kinematics.yaml b/ur_moveit_config/config/kinematics.yaml index 7681fe635..af3d30e89 100644 --- a/ur_moveit_config/config/kinematics.yaml +++ b/ur_moveit_config/config/kinematics.yaml @@ -6,4 +6,3 @@ kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 - \ No newline at end of file diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index bcc2a8893..dba83fc74 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/ur_moveit_config/launch/ur_moveit.launch.py @@ -52,6 +52,7 @@ def launch_setup(context, *args, **kwargs): # General arguments description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") + _publish_robot_description_semantic = LaunchConfiguration("publish_robot_description_semantic") moveit_config_package = LaunchConfiguration("moveit_config_package") moveit_joint_limits_file = LaunchConfiguration("moveit_joint_limits_file") moveit_config_file = LaunchConfiguration("moveit_config_file") @@ -143,6 +144,10 @@ def launch_setup(context, *args, **kwargs): ) robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + publish_robot_description_semantic = { + "publish_robot_description_semantic": _publish_robot_description_semantic + } + robot_description_kinematics = PathJoinSubstitution( [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"] ) @@ -207,6 +212,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ robot_description, robot_description_semantic, + publish_robot_description_semantic, robot_description_kinematics, robot_description_planning, ompl_planning_pipeline_config, @@ -310,10 +316,17 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="ur5_robotiq85_gripper.urdf.xacro", + default_value="ur.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "publish_robot_description_semantic", + default_value="True", + description="Whether to publish the SRDF description on topic /robot_description_semantic.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "moveit_config_package", diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml index bfacdd816..5c159c00d 100644 --- a/ur_moveit_config/package.xml +++ b/ur_moveit_config/package.xml @@ -2,7 +2,7 @@ ur_moveit_config - 2.2.14 + 2.2.15 An example package with MoveIt2 configurations for UR robots. diff --git a/ur_moveit_config/srdf/ur_macro.srdf.xacro b/ur_moveit_config/srdf/ur_macro.srdf.xacro index 747fcab91..04554f33c 100644 --- a/ur_moveit_config/srdf/ur_macro.srdf.xacro +++ b/ur_moveit_config/srdf/ur_macro.srdf.xacro @@ -13,23 +13,6 @@ - - - - - - - - - - - - - - - - - @@ -55,41 +38,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -104,108 +53,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ur_robot_driver/CHANGELOG.rst b/ur_robot_driver/CHANGELOG.rst index 861890d66..45aa60b5b 100644 --- a/ur_robot_driver/CHANGELOG.rst +++ b/ur_robot_driver/CHANGELOG.rst @@ -1,3 +1,9 @@ +2.2.15 (2024-07-26) +------------------- +* Fix passing launch_dashobard_client launch argument (backport of `#1057 `_) +* Updated the UR family photo on the readme (backport of `#1064 `_) +* Contributors: Rune Søoe-Knudsen, Felix Exner + 2.2.14 (2024-07-01) ------------------- * Add sleep between controller stopper's controller queries (backport of `#1038 `_) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index c2eed8a8e..6577e8e87 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -180,6 +180,10 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake) if(${UR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS}) + add_launch_test(test/launch_args.py + TIMEOUT + 180 + ) add_launch_test(test/dashboard_client.py TIMEOUT 180 diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index adc29e4dd..544dd1fbd 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -26,11 +26,6 @@ controller_manager: ur_configuration_controller: type: ur_controllers/URConfigurationController - - robotiq_gripper_controller: - type: position_controllers/GripperActionController - robotiq_activation_controller: - type: robotiq_controllers/RobotiqActivationController speed_scaling_state_broadcaster: ros__parameters: @@ -135,21 +130,3 @@ forward_position_controller: - $(var tf_prefix)wrist_1_joint - $(var tf_prefix)wrist_2_joint - $(var tf_prefix)wrist_3_joint - -robotiq_gripper_controller: - ros__parameters: - default: true - joint: robotiq_85_left_knuckle_joint - use_effort_interface: true - use_speed_interface: true - - action_monitor_rate: 20.0 - allow_stalling: false - goal_tolerance: 0.01 - max_effort: 50.0 - stall_timeout: 1.0 - stall_velocity_threshold: 0.001 - -robotiq_activation_controller: - ros__parameters: - default: true \ No newline at end of file diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 87a2cf710..053ed4996 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -36,7 +36,14 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + AndSubstitution, + Command, + FindExecutable, + LaunchConfiguration, + NotSubstitution, + PathJoinSubstitution, +) def launch_setup(context, *args, **kwargs): @@ -240,7 +247,9 @@ def launch_setup(context, *args, **kwargs): dashboard_client_node = Node( package="ur_robot_driver", - condition=IfCondition(launch_dashboard_client) and UnlessCondition(use_fake_hardware), + condition=IfCondition( + AndSubstitution(launch_dashboard_client, NotSubstitution(use_fake_hardware)) + ), executable="dashboard_client", name="dashboard_client", output="screen", @@ -308,18 +317,6 @@ def launch_setup(context, *args, **kwargs): arguments=["-d", rviz_config_file], ) - #robotiq_gripper_controller_spawner = Node( - # package="controller_manager", - # executable="spawner", - # arguments=["robotiq_gripper_controller", "-c", "/controller_manager"], - #) - - #robotiq_activation_controller_spawner = Node( - # package="controller_manager", - # executable="spawner", - # arguments=["robotiq_activation_controller", "-c", "/controller_manager"], - #) - # Spawn controllers def controller_spawner(controllers, active=True): inactive_flags = ["--inactive"] if not active else [] @@ -342,9 +339,6 @@ def controller_spawner(controllers, active=True): "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", "ur_configuration_controller", - - "robotiq_gripper_controller", - "robotiq_activation_controller", ] controllers_inactive = ["forward_position_controller"] @@ -458,7 +452,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="ur5_robotiq85_gripper.urdf.xacro", + default_value="ur.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) @@ -470,7 +464,7 @@ def generate_launch_description(): FindPackageShare(LaunchConfiguration("description_package")), "config", LaunchConfiguration("ur_type"), - "hrilab_kinematics.yaml", + "default_kinematics.yaml", ] ), description="The calibration configuration of the actual robot used.", diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 7fd4ef3b6..90924da23 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -2,7 +2,7 @@ ur_robot_driver - 2.2.14 + 2.2.15 The new driver for Universal Robots UR3, UR5 and UR10 robots with CB3 controllers and the e-series. Denis Stogl diff --git a/ur_robot_driver/test/launch_args.py b/ur_robot_driver/test/launch_args.py new file mode 100644 index 000000000..eba542fff --- /dev/null +++ b/ur_robot_driver/test/launch_args.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python +# Copyright 2024, FZI Forschungszentrum Informatik +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import logging +import os +import pytest +import sys +import time +import unittest + +import rclpy +from rclpy.node import Node + + +from launch import LaunchDescription +from launch.actions import ( + ExecuteProcess, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackagePrefix, FindPackageShare + +import launch_testing +from launch_testing.actions import ReadyToTest + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + _declare_launch_arguments, + _ursim_action, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "launch_dashboard_client", + [("true"), ("false")], +) +def generate_test_description(launch_dashboard_client): + ur_type = LaunchConfiguration("ur_type") + launch_arguments = { + "robot_ip": "192.168.56.101", + "ur_type": ur_type, + "launch_rviz": "false", + "controller_spawner_timeout": str(120), + "initial_joint_controller": "scaled_joint_trajectory_controller", + "headless_mode": "true", + "launch_dashboard_client": launch_dashboard_client, + "start_joint_controller": "false", + } + + robot_driver = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] + ) + ), + launch_arguments=launch_arguments.items(), + ) + wait_dashboard_server = ExecuteProcess( + cmd=[ + PathJoinSubstitution( + [ + FindPackagePrefix("ur_robot_driver"), + "bin", + "wait_dashboard_server.sh", + ] + ) + ], + name="wait_dashboard_server", + output="screen", + ) + driver_starter = RegisterEventHandler( + OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) + ) + + return LaunchDescription( + _declare_launch_arguments() + + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter] + ) + + +class DashboardClientTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_launch_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + # This waits for the controller_manager to be available + self._controller_manager_interface = ControllerManagerInterface(self.node) + + def setUp(self): + pass + + def test_dashboard_client_exists(self, launch_dashboard_client): + # Use the get_node_names_and_namespaces() method to get the list of nodes and their namespaces + nodes_with_ns = self.node.get_node_names_and_namespaces() + + nodes = [namespace + name for (name, namespace) in nodes_with_ns] + + for node in nodes: + print(node) + + # Print out the nodes and their namespaces + logging.info("Discovered ROS nodes:") + if launch_dashboard_client == "true": + self.assertIn("/dashboard_client", nodes) + else: + self.assertNotIn("/dashboard_client", nodes) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 4d8a28449..8db7b6835 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -318,23 +318,26 @@ def generate_driver_test_description( ): ur_type = LaunchConfiguration("ur_type") + launch_arguments = { + "robot_ip": "192.168.56.101", + "ur_type": ur_type, + "launch_rviz": "false", + "controller_spawner_timeout": str(controller_spawner_timeout), + "initial_joint_controller": "scaled_joint_trajectory_controller", + "headless_mode": "true", + "launch_dashboard_client": "true", + "start_joint_controller": "false", + } + if tf_prefix: + launch_arguments["tf_prefix"] = tf_prefix + robot_driver = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] ) ), - launch_arguments={ - "robot_ip": "192.168.56.101", - "ur_type": ur_type, - "launch_rviz": "false", - "controller_spawner_timeout": str(controller_spawner_timeout), - "initial_joint_controller": "scaled_joint_trajectory_controller", - "headless_mode": "true", - "launch_dashboard_client": "false", - "start_joint_controller": "false", - "tf_prefix": tf_prefix, - }.items(), + launch_arguments=launch_arguments.items(), ) wait_dashboard_server = ExecuteProcess( cmd=[