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 5c56ffd..33f5d68 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 @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include "tf2_msgs/TFMessage.h" @@ -114,6 +115,7 @@ protected: bool setSpeedSlider(ur_rtde_msgs::SetSpeedSliderRequest& req, ur_rtde_msgs::SetSpeedSliderResponse& res); bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res); + void commandCallback(const std_msgs::StringConstPtr& msg); std::unique_ptr ur_driver_; @@ -158,6 +160,7 @@ protected: ros::ServiceServer set_speed_slider_srv_; ros::ServiceServer set_io_srv_; + ros::Subscriber command_sub_; uint32_t runtime_state_; bool position_controller_running_; diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h index 22615b1..13659a4 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h @@ -119,6 +119,17 @@ public: rtde_interface::RTDEWriter& getRTDEWriter(); + /*! + * \brief Sends a custom script program to the robot. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param program URScript code that shall be executed by the robot. + * + * \returns true on successful upload, false otherwise. + */ + bool sendScript(const std::string& program); + private: std::string readScriptFile(const std::string& filename); std::string readKeepalive(); @@ -128,6 +139,7 @@ private: std::unique_ptr rtde_client_; std::unique_ptr reverse_interface_; std::unique_ptr script_sender_; + std::unique_ptr> primary_stream_; double servoj_time_; uint32_t servoj_gain_; diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 07a5af2..2a68be6 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -77,6 +77,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h std::string tf_prefix = robot_hw_nh.param("tf_prefix", ""); program_state_pub_ = robot_hw_nh.advertise("robot_program_running", 10, true); + command_sub_ = robot_hw_nh.subscribe("script_command", 1, &HardwareInterface::commandCallback, this); tcp_transform_.header.frame_id = "base"; tcp_transform_.child_frame_id = "tool0_controller"; @@ -579,4 +580,24 @@ bool HardwareInterface::setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse return true; } + +void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) +{ + std::string str = msg->data; + if (str.back() != '\n') + { + str.append("\n"); + } + + // TODO: Check whether we can currently send code + + if (ur_driver_->sendScript(str)) + { + ROS_DEBUG_STREAM("Sent script to robot"); + } + else + { + ROS_ERROR_STREAM("Error sending script to robot"); + } +} } // namespace ur_driver diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index f30eabd..f7f3465 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -61,6 +61,8 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc LOG_DEBUG("Initializing RTDE client"); rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file)); + primary_stream_.reset(new comm::URStream( + robot_ip_, ur_driver::primary_interface::UR_PRIMARY_PORT)); LOG_INFO("Checking if calibration data matches connected robot."); checkCalibration(calibration_checksum); @@ -216,10 +218,12 @@ std::string UrDriver::readKeepalive() void UrDriver::checkCalibration(const std::string& checksum) { - comm::URStream stream(robot_ip_, - ur_driver::primary_interface::UR_PRIMARY_PORT); + if (primary_stream_ == nullptr) + { + throw std::runtime_error("checkCalibration() called without a primary interface connection being established."); + } primary_interface::PrimaryParser parser; - comm::URProducer prod(stream, parser); + comm::URProducer prod(*primary_stream_, parser); CalibrationChecker consumer(checksum); @@ -240,4 +244,23 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() return rtde_client_->getWriter(); } +bool UrDriver::sendScript(const std::string& program) +{ + if (primary_stream_ == nullptr) + { + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " + "should not happen."); + } + size_t len = program.size(); + const uint8_t* data = reinterpret_cast(program.c_str()); + size_t written; + + if (primary_stream_->write(data, len, written)) + { + LOG_DEBUG("Sent program to robot"); + return true; + } + LOG_ERROR("Could not send program to robot"); + return false; +} } // namespace ur_driver