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

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

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

@@ -173,11 +173,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_;