From c6a5df9fdc1681bef0d896a4bc8cca9d22b863ae Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 1 Aug 2019 12:54:39 +0200 Subject: [PATCH] extended SetIO service to correctly set configurable and tool digital output --- ur_rtde_driver/resources/rtde_input_recipe.txt | 4 ++++ ur_rtde_driver/src/ros/hardware_interface.cpp | 10 +++++++++- ur_rtde_driver/src/rtde/rtde_writer.cpp | 4 ++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/ur_rtde_driver/resources/rtde_input_recipe.txt b/ur_rtde_driver/resources/rtde_input_recipe.txt index c96d81b..c00dc05 100644 --- a/ur_rtde_driver/resources/rtde_input_recipe.txt +++ b/ur_rtde_driver/resources/rtde_input_recipe.txt @@ -2,6 +2,10 @@ speed_slider_mask speed_slider_fraction standard_digital_output_mask standard_digital_output +configurable_digital_output_mask +configurable_digital_output +tool_digital_output_mask +tool_digital_output standard_analog_output_mask standard_analog_output_type standard_analog_output_0 diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 69c7dc9..b823db6 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -547,7 +547,15 @@ bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse { if (req.fun == req.FUN_SET_DIGITAL_OUT) { - res.success = ur_driver_->getRTDEWriter().sendStandardDigitalOutput(req.pin, req.state); + if (req.pin <= 7) + { + res.success = ur_driver_->getRTDEWriter().sendStandardDigitalOutput(req.pin, req.state); + } else if (req.pin <= 15) + { + res.success = ur_driver_->getRTDEWriter().sendConfigurableDigitalOutput(req.pin-8, req.state); + } else { + res.success = ur_driver_->getRTDEWriter().sendToolDigitalOutput(req.pin-16, req.state); + } } else if (req.fun == req.FUN_SET_ANALOG_OUT) { diff --git a/ur_rtde_driver/src/rtde/rtde_writer.cpp b/ur_rtde_driver/src/rtde/rtde_writer.cpp index e1342cc..a8e5fb8 100644 --- a/ur_rtde_driver/src/rtde/rtde_writer.cpp +++ b/ur_rtde_driver/src/rtde/rtde_writer.cpp @@ -151,8 +151,8 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) { digital_output = 0; } - success = package->setData("tooldigital_output_mask", mask); - success = success && package->setData("tooldigital_output", digital_output); + success = package->setData("tool_digital_output_mask", mask); + success = success && package->setData("tool_digital_output", digital_output); if (success) {