Allow setting the analog output domain when setting an analog IO (#1123)

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

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<std_srvs::srv::Trigger>::SharedPtr hand_back_control_srv_;
  rclcpp::Service<ur_msgs::srv::SetSpeedSliderFraction>::SharedPtr set_speed_slider_srv_;
  rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
+ rclcpp::Service<ur_msgs::srv::SetAnalogOutput>::SharedPtr set_analog_output_srv_;
  rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
 

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<std_msgs::msg::Bool>("~/robot_program_running", qos_latched);
  set_io_srv_ = get_node()->create_service<ur_msgs::srv::SetIO>(
  "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2));
+ set_analog_output_srv_ = get_node()->create_service<ur_msgs::srv::SetAnalogOutput>(
+ "~/set_analog_output",
+ std::bind(&GPIOController::setAnalogOutput, this, std::placeholders::_1, std::placeholders::_2));
 
  set_speed_slider_srv_ = get_node()->create_service<ur_msgs::srv::SetSpeedSliderFraction>(
  "~/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<double>(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<float>(req->data.state));
+ command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_DOMAIN].set_value(static_cast<double>(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<bool>(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)
 {

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -177,6 +177,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
  // asynchronous commands
  std::array<double, 18> standard_dig_out_bits_cmd_;
  std::array<double, 2> standard_analog_output_cmd_;
+ double analog_output_domain_cmd_;
  double tool_voltage_cmd_;
  double io_async_success_;
  double target_speed_fraction_cmd_;

ur_robot_driver/src/hardware_interface.cpp

@@ -310,6 +310,8 @@ std::vector<hardware_interface::CommandInterface> 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<urcl::AnalogOutputType>(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_;
  }
  }

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -114,6 +114,7 @@
 
  <command_interface name="standard_analog_output_cmd_0"/>
  <command_interface name="standard_analog_output_cmd_1"/>
+ <command_interface name="analog_output_domain_cmd"/>
 
  <command_interface name="tool_voltage_cmd"/>