Added a subscriber to send custom script commands to the robot

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -34,6 +34,7 @@
 #include <algorithm>
 #include <std_msgs/Bool.h>
 #include <std_msgs/Float64.h>
+#include <std_msgs/String.h>
 #include <std_srvs/Trigger.h>
 #include <realtime_tools/realtime_publisher.h>
 #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<UrDriver> 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_;

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_interface::RTDEClient> rtde_client_;
   std::unique_ptr<comm::ReverseInterface> reverse_interface_;
   std::unique_ptr<comm::ScriptSender> script_sender_;
+  std::unique_ptr<comm::URStream<ur_driver::primary_interface::PackageHeader>> primary_stream_;
 
   double servoj_time_;
   uint32_t servoj_gain_;

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<std::string>("tf_prefix", "");
 
   program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("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

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<ur_driver::primary_interface::PackageHeader>(
+      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<ur_driver::primary_interface::PackageHeader> stream(robot_ip_,
+  if (primary_stream_ == nullptr)
-                                                                     ur_driver::primary_interface::UR_PRIMARY_PORT);
+  {
+    throw std::runtime_error("checkCalibration() called without a primary interface connection being established.");
+  }
   primary_interface::PrimaryParser parser;
-  comm::URProducer<ur_driver::primary_interface::PackageHeader> prod(stream, parser);
+  comm::URProducer<ur_driver::primary_interface::PackageHeader> 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<const uint8_t*>(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