Implemented optional headless mode that directly sends the robot program via its interface, avoiding URCaps usage

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -53,12 +53,13 @@ public:
    * \param output_recipe_file Filename where the output recipe is stored in.
    * \param input_recipe_file Filename where the input recipe is stored in.
    * \param handle_program_state Function handle to a callback on program state changes.
+   * \param headless_mode Parameter to control if the driver should be started in headless mode.
    * \param tool_comm_setup Configuration for using the tool communication.
    * \param calibration_checksum Expected checksum of calibration. Will be matched against the
    * calibration reported by the robot.
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
-           const std::string& input_recipe_file, std::function<void(bool)> handle_program_state,
+           const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "");
   /*!
    * \brief Constructs a new UrDriver object.
@@ -68,13 +69,14 @@ public:
    * \param output_recipe_file Filename where the output recipe is stored in.
    * \param input_recipe_file Filename where the input recipe is stored in.
    * \param handle_program_state Function handle to a callback on program state changes.
+   * \param headless_mode Parameter to control if the driver should be started in headless mode.
    * \param calibration_checksum Expected checksum of calibration. Will be matched against the
    * calibration reported by the robot.
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
-           const std::string& input_recipe_file, std::function<void(bool)> handle_program_state,
+           const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            const std::string& calibration_checksum = "")
-    : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state,
+    : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
                std::unique_ptr<ToolCommSetup>{}, calibration_checksum)
   {
   }
@@ -154,6 +156,15 @@ public:
    */
   bool sendScript(const std::string& program);
 
+  /*!
+   * \brief Sends the external control program to the robot.
+   *
+   * Only for use in headless mode, as it replaces the use of the URCaps program.
+   *
+   * \returns true on successful upload, false otherwise
+   */
+  bool sendRobotProgram();
+
 private:
   std::string readScriptFile(const std::string& filename);
   std::string readKeepalive();
@@ -176,6 +187,8 @@ private:
   std::function<void(bool)> handle_program_state_;
 
   std::string robot_ip_;
+  bool in_headless_mode_;
+  std::string full_robot_program_;
 };
 }  // namespace ur_driver
 #endif  // ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED

ur_rtde_driver/launch/ur10_bringup.launch

@@ -9,6 +9,7 @@
   <arg name="robot_description_file" default="$(find ur_description)/launch/ur10_upload.launch" doc="Robot description launch file."/>
   <arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
+  <arg name="headless_mode" default="false"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
@@ -21,6 +22,7 @@
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+    <arg name="headless_mode" value="$(arg headless_mode)"/>
   </include>
 
 </launch>

ur_rtde_driver/launch/ur10e_bringup.launch

@@ -20,6 +20,7 @@
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
 
+  <arg name="headless_mode" default="false"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
@@ -32,6 +33,7 @@
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+    <arg name="headless_mode" value="$(arg headless_mode)"/>
     <arg name="tool_voltage" value="$(arg tool_voltage)"/>
     <arg name="tool_parity" value="$(arg tool_parity)"/>
     <arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>

ur_rtde_driver/launch/ur3_bringup.launch

@@ -9,6 +9,7 @@
   <arg name="robot_description_file" default="$(find ur_description)/launch/ur3_upload.launch" doc="Robot description launch file."/>
   <arg name="kinematics_config" default="$(find ur_description)/config/ur3_default.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
+  <arg name="headless_mode" default="false"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
@@ -21,6 +22,7 @@
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+    <arg name="headless_mode" value="$(arg headless_mode)"/>
   </include>
 
 </launch>

ur_rtde_driver/launch/ur3e_bringup.launch

@@ -18,6 +18,7 @@
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
+  <arg name="headless_mode" default="false"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
@@ -30,6 +31,7 @@
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+    <arg name="headless_mode" value="$(arg headless_mode)"/>
     <arg name="tool_voltage" value="$(arg tool_voltage)"/>
     <arg name="tool_parity" value="$(arg tool_parity)"/>
     <arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>

ur_rtde_driver/launch/ur5_bringup.launch

@@ -9,6 +9,7 @@
   <arg name="robot_description_file" default="$(find ur_description)/launch/ur5_upload.launch" doc="Robot description launch file."/>
   <arg name="kinematics_config" default="$(find ur_description)/config/ur5_default.yaml" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
+  <arg name="headless_mode" default="false"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
@@ -21,6 +22,7 @@
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+    <arg name="headless_mode" value="$(arg headless_mode)"/>
   </include>
 
 </launch>

ur_rtde_driver/launch/ur5e_bringup.launch

@@ -18,6 +18,7 @@
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
+  <arg name="headless_mode" default="false"/>
 
   <include file="$(find ur_rtde_driver)/launch/ur_common.launch">
     <arg name="debug" value="$(arg debug)"/>
@@ -30,6 +31,7 @@
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+    <arg name="headless_mode" value="$(arg headless_mode)"/>
     <arg name="tool_voltage" value="$(arg tool_voltage)"/>
     <arg name="tool_parity" value="$(arg tool_parity)"/>
     <arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>

ur_rtde_driver/launch/ur_common.launch

@@ -18,6 +18,7 @@
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
   <arg name="robot_description_file" doc="Robot description launch file."/>
   <arg name="limited" default="false" doc="Use the description in limited mode (Every axis rotates from -PI to PI)"/>
+  <arg name="headless_mode" default="false"/>
 
   <!-- robot model -->
   <include file="$(arg robot_description_file)">
@@ -37,6 +38,7 @@
     <arg name="tf_prefix" value="$(arg tf_prefix)"/>
     <arg name="controllers" value="$(arg controllers)"/>
     <arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
+    <arg name="headless_mode" value="$(arg headless_mode)"/>
     <arg name="tool_voltage" value="$(arg tool_voltage)"/>
     <arg name="tool_parity" value="$(arg tool_parity)"/>
     <arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>

ur_rtde_driver/launch/ur_control.launch

@@ -24,6 +24,7 @@
   <arg name="tool_tx_idle_chars" default="3.5" doc="Number of idle chars in TX channel used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_device_name" default="/tmp/ttyUR" doc="Local device name used for tool communication. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_tcp_port" default="54321" doc="Port on which the robot controller publishes the tool comm interface. Only used, when `use_tool_communication` is set to true."/>
+  <arg name="headless_mode" default="false"/>
 
   <!-- Load hardware interface -->
   <node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="true">
@@ -32,6 +33,7 @@
     <param name="script_file" value="$(arg urscript_file)"/>
     <param name="output_recipe_file" value="$(arg rtde_output_recipe_file)"/>
     <param name="input_recipe_file" value="$(arg rtde_input_recipe_file)"/>
+    <param name="headless_mode" value="$(arg headless_mode)"/>
     <param name="tf_prefix" value="$(arg tf_prefix)"/>
     <param name="use_tool_communication" value="$(arg use_tool_communication)"/>
     <!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -89,6 +89,13 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
     return false;
   }
 
+  bool headless_mode;
+  if (!robot_hw_nh.getParam("headless_mode", headless_mode))
+  {
+    ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("headless_mode") << " not given.");
+    return false;
+  }
+
   // Whenever the runtime state of the "External Control" program node in the UR-program changes, a
   // message gets published here. So this is equivalent to the information whether the robot accepts
   // commands from ROS side.
@@ -195,7 +202,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   {
     ur_driver_.reset(new UrDriver(robot_ip_, script_filename, output_recipe_filename, input_recipe_filename,
                                   std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
-                                  std::move(tool_comm_setup), calibration_checksum));
+                                  headless_mode, std::move(tool_comm_setup), calibration_checksum));
   }
   catch (ur_driver::ToolCommNotAvailable& e)
   {

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -34,6 +34,7 @@
 #include "ur_rtde_driver/exceptions.h"
 #include "ur_rtde_driver/primary/primary_parser.h"
 #include <memory>
+#include <sstream>
 
 #include <ur_rtde_driver/ur/calibration_checker.h>
 
@@ -48,7 +49,7 @@ static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
 
 ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file,
                               const std::string& output_recipe_file, const std::string& input_recipe_file,
-                              std::function<void(bool)> handle_program_state,
+                              std::function<void(bool)> handle_program_state, bool headless_mode,
                               std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum)
   : servoj_time_(0.008)
   , servoj_gain_(2000)
@@ -113,9 +114,25 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   }
   prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str());
 
-  script_sender_.reset(new comm::ScriptSender(script_sender_port, prog));
-  script_sender_->start();
-  LOG_DEBUG("Created script sender");
+  in_headless_mode_ = headless_mode;
+  if (in_headless_mode_)
+  {
+    full_robot_program_ = "def externalControl():\n";
+    std::istringstream prog_stream(prog);
+    std::string line;
+    while (std::getline(prog_stream, line))
+    {
+      full_robot_program_ += "\t" + line + "\n";
+    }
+    full_robot_program_ += "end\n";
+    sendRobotProgram();
+  }
+  else
+  {
+    script_sender_.reset(new comm::ScriptSender(script_sender_port, prog));
+    script_sender_->start();
+    LOG_DEBUG("Created script sender");
+  }
 
   reverse_port_ = reverse_port;
   watchdog_thread_ = std::thread(&UrDriver::startWatchdog, this);
@@ -256,4 +273,17 @@ bool UrDriver::sendScript(const std::string& program)
   LOG_ERROR("Could not send program to robot");
   return false;
 }
+
+bool UrDriver::sendRobotProgram()
+{
+  if (in_headless_mode_)
+  {
+    return sendScript(full_robot_program_);
+  }
+  else
+  {
+    LOG_ERROR("Tried to send robot program directly while not in headless mode");
+    return false;
+  }
+}
 }  // namespace ur_driver