From 8f2dfa97eee6ce443f5eb43a611f3690e5c7d19e Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 8 Oct 2024 17:38:02 +0200 Subject: [PATCH] Allow setting the analog output domain when setting an analog IO (#1123) --- ur_controllers/doc/index.rst | 2 + .../ur_controllers/gpio_controller.hpp | 6 +++ ur_controllers/src/gpio_controller.cpp | 47 ++++++++++++++++++- .../ur_robot_driver/hardware_interface.hpp | 1 + ur_robot_driver/src/hardware_interface.cpp | 12 ++++- ur_robot_driver/urdf/ur.ros2_control.xacro | 1 + 6 files changed, 67 insertions(+), 2 deletions(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 099b3f8f4..bc4d7c147 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -129,6 +129,8 @@ Advertised services * ``~/resend_robot_program [std_srvs/srv/Trigger]``: When :ref:`headless_mode` is used, this service can be used to restart the *External Control* program on the robot. * ``~/set_io [ur_msgs/srv/SetIO]``: Set an output pin on the robot. +* ``~/set_analog_output [ur_msgs/srv/SetAnalogOutput]``: Set an analog output on the robot. This + also allows specifying the domain. * ``~/set_payload [ur_msgs/srv/SetPayload]``: Change the robot's payload on-the-fly. * ``~/set_speed_slider [ur_msgs/srv/SetSpeedSliderFraction]``: Set the value of the speed slider. * ``~/zero_ftsensor [std_srvs/srv/Trigger]``: Zeroes the reported wrench of the force torque diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp index c20bd680e..b06b5139a 100644 --- a/ur_controllers/include/ur_controllers/gpio_controller.hpp +++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp @@ -51,6 +51,7 @@ #include "ur_dashboard_msgs/msg/robot_mode.hpp" #include "ur_dashboard_msgs/msg/safety_mode.hpp" #include "ur_msgs/srv/set_io.hpp" +#include "ur_msgs/srv/set_analog_output.hpp" #include "ur_msgs/srv/set_speed_slider_fraction.hpp" #include "ur_msgs/srv/set_payload.hpp" #include "rclcpp/time.hpp" @@ -79,6 +80,7 @@ enum CommandInterfaces ZERO_FTSENSOR_ASYNC_SUCCESS = 32, HAND_BACK_CONTROL_CMD = 33, HAND_BACK_CONTROL_ASYNC_SUCCESS = 34, + ANALOG_OUTPUTS_DOMAIN = 35, }; enum StateInterfaces @@ -122,6 +124,9 @@ class GPIOController : public controller_interface::ControllerInterface private: bool setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs::srv::SetIO::Response::SharedPtr resp); + bool setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req, + ur_msgs::srv::SetAnalogOutput::Response::SharedPtr resp); + bool setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Request::SharedPtr req, ur_msgs::srv::SetSpeedSliderFraction::Response::SharedPtr resp); @@ -161,6 +166,7 @@ class GPIOController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr hand_back_control_srv_; rclcpp::Service::SharedPtr set_speed_slider_srv_; rclcpp::Service::SharedPtr set_io_srv_; + rclcpp::Service::SharedPtr set_analog_output_srv_; rclcpp::Service::SharedPtr set_payload_srv_; rclcpp::Service::SharedPtr tare_sensor_srv_; diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp index d5e6d16ee..188542d0e 100644 --- a/ur_controllers/src/gpio_controller.cpp +++ b/ur_controllers/src/gpio_controller.cpp @@ -97,6 +97,8 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd"); config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success"); + config.names.emplace_back(tf_prefix + "gpio/analog_output_domain_cmd"); + return config; } @@ -293,6 +295,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre program_state_pub_ = get_node()->create_publisher("~/robot_program_running", qos_latched); set_io_srv_ = get_node()->create_service( "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2)); + set_analog_output_srv_ = get_node()->create_service( + "~/set_analog_output", + std::bind(&GPIOController::setAnalogOutput, this, std::placeholders::_1, std::placeholders::_2)); set_speed_slider_srv_ = get_node()->create_service( "~/set_speed_slider", @@ -357,7 +362,7 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->pin].set_value(static_cast(req->state)); - RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%1.0f'.", req->pin, req->state); + RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f'.", req->pin, req->state); if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " @@ -385,6 +390,46 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs: } } +bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::SharedPtr req, + ur_msgs::srv::SetAnalogOutput::Response::SharedPtr resp) +{ + std::string domain_string = "UNKNOWN"; + switch (req->data.domain) { + case ur_msgs::msg::Analog::CURRENT: + domain_string = "CURRENT"; + break; + case ur_msgs::msg::Analog::VOLTAGE: + domain_string = "VOLTAGE"; + break; + default: + RCLCPP_ERROR(get_node()->get_logger(), "Domain must be either 0 (CURRENT) or 1 (VOLTAGE)"); + resp->success = false; + return false; + } + + if (req->data.pin < 0 || req->data.pin > 1) { + RCLCPP_ERROR(get_node()->get_logger(), "Invalid pin selected. Only pins 0 and 1 are allowed."); + resp->success = false; + return false; + } + + command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING); + command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->data.pin].set_value( + static_cast(req->data.state)); + command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_DOMAIN].set_value(static_cast(req->data.domain)); + + RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f' in domain %s.", req->data.pin, + req->data.state, domain_string.c_str()); + + if (!waitForAsyncCommand([&]() { return command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value(); })) { + RCLCPP_WARN(get_node()->get_logger(), "Could not verify that io was set. (This might happen when using the " + "mocked interface)"); + } + + resp->success = static_cast(command_interfaces_[IO_ASYNC_SUCCESS].get_value()); + return resp->success; +} + bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Request::SharedPtr req, ur_msgs::srv::SetSpeedSliderFraction::Response::SharedPtr resp) { 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 81fe254a1..48b5405af 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -177,6 +177,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // asynchronous commands std::array standard_dig_out_bits_cmd_; std::array standard_analog_output_cmd_; + double analog_output_domain_cmd_; double tool_voltage_cmd_; double io_async_success_; double target_speed_fraction_cmd_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 69fbba02e..e742ac7a6 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -310,6 +310,8 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "gpio", "standard_analog_output_cmd_" + std::to_string(i), &standard_analog_output_cmd_[i])); } + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + "gpio", "analog_output_domain_cmd", &analog_output_domain_cmd_)); command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "gpio", "tool_voltage_cmd", &tool_voltage_cmd_)); @@ -685,6 +687,8 @@ void URPositionHardwareInterface::initAsyncIO() standard_analog_output_cmd_[i] = NO_NEW_CMD_; } + analog_output_domain_cmd_ = NO_NEW_CMD_; + tool_voltage_cmd_ = NO_NEW_CMD_; payload_mass_ = NO_NEW_CMD_; @@ -714,7 +718,13 @@ void URPositionHardwareInterface::checkAsyncIO() for (size_t i = 0; i < 2; ++i) { if (!std::isnan(standard_analog_output_cmd_[i]) && ur_driver_ != nullptr) { - io_async_success_ = ur_driver_->getRTDEWriter().sendStandardAnalogOutput(i, standard_analog_output_cmd_[i]); + urcl::AnalogOutputType domain = urcl::AnalogOutputType::SET_ON_TEACH_PENDANT; + if (!std::isnan(analog_output_domain_cmd_) && ur_driver_ != nullptr) { + domain = static_cast(analog_output_domain_cmd_); + analog_output_domain_cmd_ = NO_NEW_CMD_; + } + io_async_success_ = + ur_driver_->getRTDEWriter().sendStandardAnalogOutput(i, standard_analog_output_cmd_[i], domain); standard_analog_output_cmd_[i] = NO_NEW_CMD_; } } diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 580107df6..b013ab368 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -114,6 +114,7 @@ +