From f628ab353f14aeb0469a73bac6eb6f75e7dc4f4e Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Wed, 31 Jul 2019 14:57:06 +0200 Subject: [PATCH] implemented setting of standard digital and analog outputs via SetIO service --- .../ur_rtde_driver/ros/hardware_interface.h | 3 + .../include/ur_rtde_driver/rtde/rtde_writer.h | 3 +- .../resources/rtde_input_recipe.txt | 6 ++ ur_rtde_driver/src/ros/hardware_interface.cpp | 20 +++++ ur_rtde_driver/src/rtde/data_package.cpp | 2 + ur_rtde_driver/src/rtde/rtde_writer.cpp | 73 +++++++++++++++++-- 6 files changed, 98 insertions(+), 9 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index b0beb7a..96321cc 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -111,6 +112,7 @@ protected: std::bitset& data); void speedScalingCallback(const std_msgs::Float64::ConstPtr& msg); + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res); std::unique_ptr ur_driver_; @@ -154,6 +156,7 @@ protected: std::unique_ptr> tool_data_pub_; ros::Subscriber speed_slider_sub_; + ros::ServiceServer set_io_srv_; uint32_t runtime_state_; bool position_controller_running_; diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h index e7d3128..0a03038 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h +++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_writer.h @@ -52,9 +52,10 @@ public: bool sendStandardDigitalOutput(uint8_t output_pin, bool value); bool sendConfigurableDigitalOutput(uint8_t output_pin, bool value); bool sendToolDigitalOutput(bool value); - bool sendStandardAnalogOuput(uint8_t output_pin, bool value); + bool sendStandardAnalogOuput(uint8_t output_pin, double value); private: + uint8_t pinToMask(uint8_t pin); comm::URStream* stream_; std::vector recipe_; uint8_t recipe_id_; diff --git a/ur_rtde_driver/resources/rtde_input_recipe.txt b/ur_rtde_driver/resources/rtde_input_recipe.txt index 8f30e31..c96d81b 100644 --- a/ur_rtde_driver/resources/rtde_input_recipe.txt +++ b/ur_rtde_driver/resources/rtde_input_recipe.txt @@ -1,2 +1,8 @@ speed_slider_mask speed_slider_fraction +standard_digital_output_mask +standard_digital_output +standard_analog_output_mask +standard_analog_output_type +standard_analog_output_0 +standard_analog_output_1 diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index aee0d1a..69c7dc9 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -218,6 +218,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h speed_slider_sub_ = robot_hw_nh.subscribe("set_speed_slider", 10, &HardwareInterface::speedScalingCallback, this); + set_io_srv_ = robot_hw_nh.advertiseService("set_io", &HardwareInterface::setIO, this); + deactivate_srv_ = robot_hw_nh.advertiseService("hand_back_control", &HardwareInterface::stopControl, this); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface"); @@ -540,4 +542,22 @@ void HardwareInterface::speedScalingCallback(const std_msgs::Float64::ConstPtr& { ur_driver_->getRTDEWriter().sendSpeedSlider(msg->data); } + +bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res) +{ + if (req.fun == req.FUN_SET_DIGITAL_OUT) + { + res.success = ur_driver_->getRTDEWriter().sendStandardDigitalOutput(req.pin, req.state); + } + else if (req.fun == req.FUN_SET_ANALOG_OUT) + { + res.success = ur_driver_->getRTDEWriter().sendStandardAnalogOuput(req.pin, req.state); + } + else + { + res.success = false; + } + + return true; +} } // namespace ur_driver diff --git a/ur_rtde_driver/src/rtde/data_package.cpp b/ur_rtde_driver/src/rtde/data_package.cpp index a2edce5..13b3246 100644 --- a/ur_rtde_driver/src/rtde/data_package.cpp +++ b/ur_rtde_driver/src/rtde/data_package.cpp @@ -220,6 +220,8 @@ std::unordered_map DataPackage::g_ { "tool_digital_output", uint8_t() }, { "standard_analog_output_mask", uint8_t() }, { "standard_analog_output_type", uint8_t() }, + { "standard_analog_output_0", double() }, + { "standard_analog_output_1", double() }, }; void rtde_interface::DataPackage::initEmpty() diff --git a/ur_rtde_driver/src/rtde/rtde_writer.cpp b/ur_rtde_driver/src/rtde/rtde_writer.cpp index 66ea04e..71f6bdb 100644 --- a/ur_rtde_driver/src/rtde/rtde_writer.cpp +++ b/ur_rtde_driver/src/rtde/rtde_writer.cpp @@ -63,19 +63,47 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) package.reset(new DataPackage(recipe_)); package->initEmpty(); uint32_t mask = 1; - package->setData("speed_slider_mask", mask); - package->setData("speed_slider_fraction", speed_slider_fraction); + bool success = true; + success = package->setData("speed_slider_mask", mask); + success = success && package->setData("speed_slider_fraction", speed_slider_fraction); - if (!queue_.tryEnqueue(std::move(package))) + if (success) { - return false; + if (!queue_.tryEnqueue(std::move(package))) + { + return false; + } } - return true; + return success; } bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) { - return false; + std::unique_ptr package; + package.reset(new DataPackage(recipe_)); + package->initEmpty(); + uint8_t mask = pinToMask(output_pin); + bool success = true; + uint8_t digital_output; + if (value) + { + digital_output = 255; + } + else + { + digital_output = 0; + } + success = package->setData("standard_digital_output_mask", mask); + success = success && package->setData("standard_digital_output", digital_output); + + if (success) + { + if (!queue_.tryEnqueue(std::move(package))) + { + return false; + } + } + return success; } bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) @@ -88,9 +116,38 @@ bool RTDEWriter::sendToolDigitalOutput(bool value) return false; } -bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, bool value) +bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, double value) { - return false; + std::unique_ptr package; + package.reset(new DataPackage(recipe_)); + package->initEmpty(); + uint8_t mask = pinToMask(output_pin); + // default to current for now, as no functionality to choose included in set io service + uint8_t output_type = 0; + bool success = true; + success = package->setData("standard_analog_output_mask", mask); + success = success && package->setData("standard_analog_output_type", output_type); + success = success && package->setData("standard_analog_output_0", value); + success = success && package->setData("standard_analog_output_1", value); + + if (success) + { + if (!queue_.tryEnqueue(std::move(package))) + { + return false; + } + } + return success; +} + +uint8_t RTDEWriter::pinToMask(uint8_t pin) +{ + if (pin > 7) + { + return 0; + } + + return 1 << pin; } } // namespace rtde_interface