Add a set_analog_output service (UniversalRobots#714)

* Add a set_analog_output service This service allows setting an analog output with an output domain. * Fix formatting
shadow-robot · Sep 30, 2024 · b70c5b2 · b70c5b2
1 parent 27dbac5
commit b70c5b2
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 1 deletion.
diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h
@@ -51,6 +51,7 @@
 #include <ur_client_library/control/trajectory_point_interface.h>
 #include <ur_msgs/IOStates.h>
 #include <ur_msgs/ToolDataMsg.h>
+#include <ur_msgs/SetAnalogOutput.h>
 #include <ur_msgs/SetIO.h>
 #include <ur_msgs/SetSpeedSliderFraction.h>
 #include <ur_msgs/SetPayload.h>
@@ -211,6 +212,7 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   bool setSpeedSlider(ur_msgs::SetSpeedSliderFractionRequest& req, ur_msgs::SetSpeedSliderFractionResponse& res);
   bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res);
+  bool setAnalogOutput(ur_msgs::SetAnalogOutputRequest& req, ur_msgs::SetAnalogOutputResponse& res);
   bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
   bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
   void commandCallback(const std_msgs::StringConstPtr& msg);
@@ -319,6 +321,7 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   ros::ServiceServer set_speed_slider_srv_;
   ros::ServiceServer set_io_srv_;
+  ros::ServiceServer set_analog_output_srv_;
   ros::ServiceServer resend_robot_program_srv_;
   ros::Subscriber command_sub_;
 

diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
@@ -435,8 +435,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   // doing. Using this with other controllers might lead to unexpected behaviors.
   set_speed_slider_srv_ = robot_hw_nh.advertiseService("set_speed_slider", &HardwareInterface::setSpeedSlider, this);
 
-  // Service to set any of the robot's IOs
+  // Services to set any of the robot's IOs
   set_io_srv_ = robot_hw_nh.advertiseService("set_io", &HardwareInterface::setIO, this);
+  set_analog_output_srv_ = robot_hw_nh.advertiseService("set_analog_output", &HardwareInterface::setAnalogOutput, this);
 
   if (headless_mode)
   {
@@ -1135,6 +1136,16 @@ bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse
   return true;
 }
 
+bool HardwareInterface::setAnalogOutput(ur_msgs::SetAnalogOutputRequest& req, ur_msgs::SetAnalogOutputResponse& res)
+{
+  if (ur_driver_)
+  {
+    res.success = ur_driver_->getRTDEWriter().sendStandardAnalogOutput(
+        req.data.pin, req.data.state, static_cast<urcl::AnalogOutputType>(req.data.domain));
+  }
+  return true;
+}
+
 bool HardwareInterface::resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res)
 {
   res.success = ur_driver_->sendRobotProgram();