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 8be8a0b..b0beb7a 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 @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include "tf2_msgs/TFMessage.h" @@ -109,6 +110,8 @@ protected: void readBitsetData(const std::unique_ptr& data_pkg, const std::string& var_name, std::bitset& data); + void speedScalingCallback(const std_msgs::Float64::ConstPtr& msg); + std::unique_ptr ur_driver_; ros::ServiceServer deactivate_srv_; @@ -150,6 +153,8 @@ protected: std::unique_ptr> io_pub_; std::unique_ptr> tool_data_pub_; + ros::Subscriber speed_slider_sub_; + 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 ab7f4d4..e7d3128 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 @@ -30,7 +30,10 @@ #include "ur_rtde_driver/rtde/package_header.h" #include "ur_rtde_driver/rtde/rtde_package.h" +#include "ur_rtde_driver/rtde/data_package.h" #include "ur_rtde_driver/comm/stream.h" +#include "ur_rtde_driver/queue/readerwriterqueue.h" +#include namespace ur_driver { @@ -40,10 +43,10 @@ class RTDEWriter { public: RTDEWriter() = delete; - RTDEWriter(comm::URStream* stream, const std::string& recipe_file); + RTDEWriter(comm::URStream* stream, const std::vector& recipe); ~RTDEWriter() = default; - bool init(uint8_t recipe_id); - bool start(); + void init(uint8_t recipe_id); + void run(); bool sendSpeedSlider(double speed_slider_fraction); bool sendStandardDigitalOutput(uint8_t output_pin, bool value); @@ -55,6 +58,8 @@ private: comm::URStream* stream_; std::vector recipe_; uint8_t recipe_id_; + moodycamel::BlockingReaderWriterQueue> queue_; + std::thread writer_thread_; }; } // namespace rtde_interface diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 0dc4a0c..aee0d1a 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -216,6 +216,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h } tool_data_pub_.reset(new realtime_tools::RealtimePublisher(robot_hw_nh, "tool_data", 1)); + speed_slider_sub_ = robot_hw_nh.subscribe("set_speed_slider", 10, &HardwareInterface::speedScalingCallback, 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"); @@ -533,4 +535,9 @@ bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::Tri } return true; } + +void HardwareInterface::speedScalingCallback(const std_msgs::Float64::ConstPtr& msg) +{ + ur_driver_->getRTDEWriter().sendSpeedSlider(msg->data); +} } // namespace ur_driver diff --git a/ur_rtde_driver/src/rtde/rtde_client.cpp b/ur_rtde_driver/src/rtde/rtde_client.cpp index 9f256a1..89092f9 100644 --- a/ur_rtde_driver/src/rtde/rtde_client.cpp +++ b/ur_rtde_driver/src/rtde/rtde_client.cpp @@ -40,7 +40,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , parser_(recipe_) , prod_(stream_, parser_) , pipeline_(prod_, PIPELINE_NAME, notifier) - , writer_(&stream_, input_recipe_file) + , writer_(&stream_, input_recipe_) , max_frequency_(URE_MAX_FREQUENCY) { } diff --git a/ur_rtde_driver/src/rtde/rtde_writer.cpp b/ur_rtde_driver/src/rtde/rtde_writer.cpp index 4d28b0e..66ea04e 100644 --- a/ur_rtde_driver/src/rtde/rtde_writer.cpp +++ b/ur_rtde_driver/src/rtde/rtde_writer.cpp @@ -31,35 +31,63 @@ namespace ur_driver { namespace rtde_interface { -RTDEWriter::RTDEWriter(comm::URStream* stream, const std::string& recipe_file) : stream_(stream) +RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector& recipe) + : stream_(stream), recipe_(recipe), queue_{ 32 } { } -bool RTDEWriter::init(uint8_t recipe_id) +void RTDEWriter::init(uint8_t recipe_id) { - return false; + recipe_id_ = recipe_id; + writer_thread_ = std::thread(&RTDEWriter::run, this); } -bool RTDEWriter::start() + +void RTDEWriter::run() { - return false; + uint8_t buffer[4096]; + size_t size; + size_t written; + std::unique_ptr package; + while (true) + { + queue_.waitDequeue(package); + package->setRecipeID(recipe_id_); + size = package->serializePackage(buffer); + stream_->write(buffer, size, written); + } } bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) { - return false; + std::unique_ptr package; + 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); + + if (!queue_.tryEnqueue(std::move(package))) + { + return false; + } + return true; } + bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) { return false; } + bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) { return false; } + bool RTDEWriter::sendToolDigitalOutput(bool value) { return false; } + bool RTDEWriter::sendStandardAnalogOuput(uint8_t output_pin, bool value) { return false;