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 15798c349..81fe254a1 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_; // Passthrough trajectory controller interface values urcl::vector6d_t passthrough_trajectory_positions_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index fd456e2e3..6af726a08 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -130,6 +130,7 @@ def launch_setup(context): "force_torque_sensor_broadcaster", "joint_state_broadcaster", "speed_scaling_state_broadcaster", + "ur_configuration_controller", ] }, ], @@ -165,6 +166,7 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "ur_configuration_controller", ] controllers_inactive = [ "scaled_joint_trajectory_controller", diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 14a8ef48d..69fbba02e 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; } @@ -463,6 +476,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 100755 --- 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