Added services to set tool voltage and zero force torque sensor (#466) (#582)

Added launch arguments for reverse ip and script command interface port.

(cherry picked from commit c196eb4)

Co-authored-by: Mads Holm Peters <[email protected]>

Author: mergify[bot]

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -63,16 +63,19 @@ enum CommandInterfaces
 {
   DIGITAL_OUTPUTS_CMD = 0u,
   ANALOG_OUTPUTS_CMD = 18,
-  IO_ASYNC_SUCCESS = 20,
-  TARGET_SPEED_FRACTION_CMD = 21,
-  TARGET_SPEED_FRACTION_ASYNC_SUCCESS = 22,
-  RESEND_ROBOT_PROGRAM_CMD = 23,
-  RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS = 24,
-  PAYLOAD_MASS = 25,
-  PAYLOAD_COG_X = 26,
-  PAYLOAD_COG_Y = 27,
-  PAYLOAD_COG_Z = 28,
-  PAYLOAD_ASYNC_SUCCESS = 29,
+  TOOL_VOLTAGE_CMD = 20,
+  IO_ASYNC_SUCCESS = 21,
+  TARGET_SPEED_FRACTION_CMD = 22,
+  TARGET_SPEED_FRACTION_ASYNC_SUCCESS = 23,
+  RESEND_ROBOT_PROGRAM_CMD = 24,
+  RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS = 25,
+  PAYLOAD_MASS = 26,
+  PAYLOAD_COG_X = 27,
+  PAYLOAD_COG_Y = 28,
+  PAYLOAD_COG_Z = 29,
+  PAYLOAD_ASYNC_SUCCESS = 30,
+  ZERO_FTSENSOR_CMD = 31,
+  ZERO_FTSENSOR_ASYNC_SUCCESS = 32,
 };
 
 enum StateInterfaces
@@ -125,6 +128,8 @@ class GPIOController : public controller_interface::ControllerInterface
   bool setPayload(const ur_msgs::srv::SetPayload::Request::SharedPtr req,
                   ur_msgs::srv::SetPayload::Response::SharedPtr resp);
 
+  bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
+
   void publishIO();
 
   void publishToolData();
@@ -150,6 +155,7 @@ class GPIOController : public controller_interface::ControllerInterface
   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::SetPayload>::SharedPtr set_payload_srv_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
 
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;

ur_controllers/src/gpio_controller.cpp

@@ -61,6 +61,8 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
     config.names.emplace_back("gpio/standard_analog_output_cmd_" + std::to_string(i));
   }
 
+  config.names.emplace_back("gpio/tool_voltage_cmd");
+
   config.names.emplace_back("gpio/io_async_success");
 
   config.names.emplace_back("speed_scaling/target_speed_fraction_cmd");
@@ -78,6 +80,10 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
   config.names.emplace_back("payload/cog.z");
   config.names.emplace_back("payload/payload_async_success");
 
+  // zero ft sensor
+  config.names.emplace_back("zero_ftsensor/zero_ftsensor_cmd");
+  config.names.emplace_back("zero_ftsensor/zero_ftsensor_async_success");
+
   return config;
 }
 
@@ -274,6 +280,10 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
 
     set_payload_srv_ = get_node()->create_service<ur_msgs::srv::SetPayload>(
         "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2));
+
+    tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
+        "~/zero_ftsensor",
+        std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
   } catch (...) {
     return LifecycleNodeInterface::CallbackReturn::ERROR;
   }
@@ -326,6 +336,19 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
       std::this_thread::sleep_for(std::chrono::milliseconds(50));
     }
 
+    resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
+    return resp->success;
+  } else if (req->fun == req->FUN_SET_TOOL_VOLTAGE) {
+    command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+    command_interfaces_[CommandInterfaces::TOOL_VOLTAGE_CMD].set_value(static_cast<double>(req->state));
+
+    RCLCPP_INFO(get_node()->get_logger(), "Setting tool voltage to: '%1.0f'.", req->state);
+
+    while (command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
+      // Asynchronous wait until the hardware interface has set the io value
+      std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
     resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
     return resp->success;
   } else {
@@ -413,6 +436,31 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
   return true;
 }
 
+bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
+                                  std_srvs::srv::Trigger::Response::SharedPtr resp)
+{
+  // reset success flag
+  command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  // call the service in the hardware
+  command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_CMD].set_value(1.0);
+
+  while (command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
+    // Asynchronous wait until the hardware interface has set the slider value
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  }
+
+  resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value());
+
+  if (resp->success) {
+    RCLCPP_INFO(get_node()->get_logger(), "Successfully zeroed the force torque sensor");
+  } else {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not zero the force torque sensor");
+    return false;
+  }
+
+  return true;
+}
+
 void GPIOController::initMsgs()
 {
   io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size());

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -174,11 +174,14 @@ 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 tool_voltage_cmd_;
   double io_async_success_;
   double target_speed_fraction_cmd_;
   double scaling_async_success_;
   double resend_robot_program_cmd_;
   double resend_robot_program_async_success_;
+  double zero_ftsensor_cmd_;
+  double zero_ftsensor_async_success_;
   bool first_pass_;
   bool initialized_;
   double system_interface_initialized_;

ur_robot_driver/launch/ur_control.launch.py

@@ -69,6 +69,8 @@ def launch_setup(context, *args, **kwargs):
     tool_device_name = LaunchConfiguration("tool_device_name")
     tool_tcp_port = LaunchConfiguration("tool_tcp_port")
     tool_voltage = LaunchConfiguration("tool_voltage")
+    reverse_ip = LaunchConfiguration("reverse_ip")
+    script_command_port = LaunchConfiguration("script_command_port")
 
     joint_limit_params = PathJoinSubstitution(
         [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"]
@@ -173,6 +175,12 @@ def launch_setup(context, *args, **kwargs):
             "tool_voltage:=",
             tool_voltage,
             " ",
+            "reverse_ip:=",
+            reverse_ip,
+            " ",
+            "script_command_port:=",
+            script_command_port,
+            " ",
         ]
     )
     robot_description = {"robot_description": robot_description_content}
@@ -540,5 +548,19 @@ def generate_launch_description():
             description="Tool voltage that will be setup.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "reverse_ip",
+            default_value="0.0.0.0",
+            description="IP that will be used for the robot controller to communicate back to the driver.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "script_command_port",
+            default_value="50004",
+            description="Port that will be opened to forward script commands from the driver to the robot",
+        )
+    )
 
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

ur_robot_driver/src/hardware_interface.cpp

@@ -264,6 +264,14 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
         "gpio", "standard_analog_output_cmd_" + std::to_string(i), &standard_analog_output_cmd_[i]));
   }
 
+  command_interfaces.emplace_back(hardware_interface::CommandInterface("gpio", "tool_voltage_cmd", &tool_voltage_cmd_));
+
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("zero_ftsensor", "zero_ftsensor_cmd", &zero_ftsensor_cmd_));
+
+  command_interfaces.emplace_back(hardware_interface::CommandInterface("zero_ftsensor", "zero_ftsensor_async_success",
+                                                                       &zero_ftsensor_async_success_));
+
   return command_interfaces;
 }
 
@@ -289,6 +297,16 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
   int reverse_port = stoi(info_.hardware_parameters["reverse_port"]);
   // The driver will offer an interface to receive the program's URScript on this port.
   int script_sender_port = stoi(info_.hardware_parameters["script_sender_port"]);
+  // IP that will be used for the robot controller to communicate back to the driver.
+  std::string reverse_ip = info_.hardware_parameters["reverse_ip"];
+  if (reverse_ip == "0.0.0.0") {
+    reverse_ip = "";
+  }
+  // Port that will be opened to send trajectory points from the driver to the robot. Note: this feature hasn't been
+  // implemented in ROS2
+  int trajectory_port = 50003;
+  // Port that will be opened to forward script commands from the driver to the robot
+  int script_command_port = stoi(info_.hardware_parameters["script_command_port"]);
   //  std::string tf_prefix = info_.hardware_parameters["tf_prefix"];
   //  std::string tf_prefix;
 
@@ -381,7 +399,7 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
         robot_ip, script_filename, output_recipe_filename, input_recipe_filename,
         std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1), headless_mode,
         std::move(tool_comm_setup), (uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
-        servoj_lookahead_time, non_blocking_read_);
+        servoj_lookahead_time, non_blocking_read_, reverse_ip, trajectory_port, script_command_port);
   } catch (urcl::ToolCommNotAvailable& e) {
     RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication");
 
@@ -541,6 +559,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
       urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
       target_speed_fraction_cmd_ = NO_NEW_CMD_;
       resend_robot_program_cmd_ = NO_NEW_CMD_;
+      zero_ftsensor_cmd_ = NO_NEW_CMD_;
       initialized_ = true;
     }
 
@@ -594,6 +613,8 @@ void URPositionHardwareInterface::initAsyncIO()
     standard_analog_output_cmd_[i] = NO_NEW_CMD_;
   }
 
+  tool_voltage_cmd_ = NO_NEW_CMD_;
+
   payload_mass_ = NO_NEW_CMD_;
   payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
 }
@@ -623,6 +644,11 @@ void URPositionHardwareInterface::checkAsyncIO()
     }
   }
 
+  if (!std::isnan(tool_voltage_cmd_) && ur_driver_ != nullptr) {
+    io_async_success_ = ur_driver_->setToolVoltage(static_cast<urcl::ToolVoltage>(tool_voltage_cmd_));
+    tool_voltage_cmd_ = NO_NEW_CMD_;
+  }
+
   if (!std::isnan(target_speed_fraction_cmd_) && ur_driver_ != nullptr) {
     scaling_async_success_ = ur_driver_->getRTDEWriter().sendSpeedSlider(target_speed_fraction_cmd_);
     target_speed_fraction_cmd_ = NO_NEW_CMD_;
@@ -640,22 +666,15 @@ void URPositionHardwareInterface::checkAsyncIO()
   if (!std::isnan(payload_mass_) && !std::isnan(payload_center_of_gravity_[0]) &&
       !std::isnan(payload_center_of_gravity_[1]) && !std::isnan(payload_center_of_gravity_[2]) &&
       ur_driver_ != nullptr) {
-    try {
-      // create command as string from interfaces
-      // ROS1 driver hardware_interface.cpp#L450-L456
-      std::stringstream str_command;
-      str_command.imbue(std::locale::classic());
-      str_command << "sec setup():" << std::endl
-                  << " set_payload(" << payload_mass_ << ", [" << payload_center_of_gravity_[0] << ", "
-                  << payload_center_of_gravity_[1] << ", " << payload_center_of_gravity_[2] << "])" << std::endl
-                  << "end";
-      payload_async_success_ = ur_driver_->sendScript(str_command.str());
-    } catch (const urcl::UrException& e) {
-      RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Service Call failed: '%s'", e.what());
-    }
+    payload_async_success_ = ur_driver_->setPayload(payload_mass_, payload_center_of_gravity_);
     payload_mass_ = NO_NEW_CMD_;
     payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
   }
+
+  if (!std::isnan(zero_ftsensor_cmd_) && ur_driver_ != nullptr) {
+    zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor();
+    zero_ftsensor_cmd_ = NO_NEW_CMD_;
+  }
 }
 
 void URPositionHardwareInterface::updateNonDoubleValues()