Load the script from an external file

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -43,8 +43,9 @@ public:
    * \brief Constructs a new UrDriver object.
    *
    * \param robot_ip IP-address under which the robot is reachable.
+   * \param script_file URScript file that should be sent to the robot
    */
-  UrDriver(const std::string& robot_ip);
+  UrDriver(const std::string& robot_ip, const std::string& script_file);
   virtual ~UrDriver() = default;
 
   /*!
@@ -64,6 +65,8 @@ public:
   bool writeJointCommand(const vector6d_t& values);
 
 private:
+  std::string readScriptFile(const std::string& filename);
+
   int rtde_frequency_;
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;

ur_rtde_driver/launch/ur10_ros_control.launch

@@ -18,6 +18,7 @@
   <arg name="shutdown_on_disconnect" default="true" />
   <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller"/>
   <arg name="stopped_controllers" default=""/>
+  <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -25,8 +26,9 @@
 
 
   <!-- Load hardware interface -->
-  <node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)">
+  <node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="true">
     <param name="robot_ip" type="str" value="$(arg robot_ip)"/>
+    <param name="script_file" value="$(arg urscript_file)"/>
     <!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->
     <!--<param name="min_payload" type="double" value="$(arg min_payload)"/>-->
     <!--<param name="max_payload" type="double" value="$(arg max_payload)"/>-->

ur_rtde_driver/resources/servoj.urscript

@@ -0,0 +1,76 @@
+def myProg():
+  global steptime = get_steptime()
+  textmsg("steptime=", steptime)
+  MULT_jointstate = {{JOINT_STATE_REPLACE}}
+
+  SERVO_UNINITIALIZED = -1
+  SERVO_IDLE = 0
+  SERVO_RUNNING = 1
+  cmd_servo_state = SERVO_UNINITIALIZED
+  cmd_servo_q = get_actual_joint_positions()
+  cmd_servo_q_last = get_actual_joint_positions()
+  def set_servo_setpoint(q):
+    enter_critical
+    cmd_servo_state = SERVO_RUNNING
+    cmd_servo_q_last = cmd_servo_q
+    cmd_servo_q = q
+    exit_critical
+  end
+
+  def extrapolate():
+    enter_critical
+    diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
+    cmd_servo_q_last = cmd_servo_q
+    cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
+    exit_critical
+
+    return cmd_servo_q
+  end
+
+  thread servoThread():
+    state = SERVO_IDLE
+    while True:
+      enter_critical
+      q = cmd_servo_q
+      do_extrapolate = False
+      if (cmd_servo_state == SERVO_IDLE):
+        do_extrapolate = True
+      end
+      state = cmd_servo_state
+      cmd_servo_state = SERVO_IDLE
+      exit_critical
+      if do_extrapolate:
+        textmsg("No new setpoint received. Extrapolating.")
+        q = extrapolate()
+        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+      elif state == SERVO_RUNNING:
+        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
+      else:
+        textmsg("Should not be here")
+        sync()
+      end
+    end
+    stopj(0.1)
+  end
+  socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
+
+  thread_servo = run servoThread()
+  keepalive = -2
+  params_mult = socket_read_binary_integer(6+1, "reverse_socket")
+  keepalive = params_mult[7]
+  while keepalive > 0:
+    params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.2) # steptime could work as well, but does not work in simulation
+    if params_mult[0] > 0:
+      keepalive = params_mult[7]
+      q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
+      set_servo_setpoint(q)
+    else:
+      # TODO: Extrapolation goes here
+      keepalive = keepalive - 1
+    end
+  end
+  sleep(.1)
+  socket_close()
+  kill thread_servo
+end
+

ur_rtde_driver/src/main_plain_driver.cpp

@@ -18,14 +18,15 @@ using namespace ur_driver;
 
 int main(int argc, char* argv[])
 {
-  std::string ROBOT_IP = "192.168.56.101";
+  std::string robot_ip = "192.168.56.101";
+  std::string script_filename = "urprog.urscript";
 
-  if (argc > 1)
+  if (argc > 2)
   {
-    ROBOT_IP = argv[1];
+    robot_ip = argv[1];
   }
 
-  UrDriver driver(ROBOT_IP);
+  UrDriver driver(robot_ip, script_filename);
 
   while (true)
   {

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -45,9 +45,15 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } };
   joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } };
   std::string robot_ip = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
+  std::string script_filename;
+  if (!robot_hw_nh.getParam("script_file", script_filename))
+  {
+    ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("script_file") << " not given.");
+    return false;
+  }
 
   ROS_INFO_STREAM("Initializing urdriver");
-  ur_driver_.reset(new UrDriver(robot_ip));
+  ur_driver_.reset(new UrDriver(robot_ip, script_filename));
 
   if (!root_nh.getParam("hardware_interface/joints", joint_names_))
   {

ur_rtde_driver/src/ros/hardware_interface_node.cpp

@@ -64,7 +64,11 @@ int main(int argc, char** argv)
 
   hw_interface.reset(new ur_driver::HardwareInterface);
 
-  hw_interface->init(nh, nh_priv);
+  if (!hw_interface->init(nh, nh_priv))
+  {
+    ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting");
+    exit(1);
+  }
   ROS_INFO_STREAM("initialized hw interface");
   controller_manager::ControllerManager cm(hw_interface.get(), nh);
   ROS_INFO_STREAM("started controller manager");

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -41,85 +41,8 @@ static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
 static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
 static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
 static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
-static const std::string POSITION_PROGRAM = R"(
-def myProg():
-  global steptime = get_steptime()
-  textmsg("steptime=", steptime)
-  MULT_jointstate = {{JOINT_STATE_REPLACE}}
 
-  SERVO_UNINITIALIZED = -1
+ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file)
-  SERVO_IDLE = 0
-  SERVO_RUNNING = 1
-  cmd_servo_state = SERVO_UNINITIALIZED
-  cmd_servo_q = get_actual_joint_positions()
-  cmd_servo_q_last = get_actual_joint_positions()
-  def set_servo_setpoint(q):
-    enter_critical
-    cmd_servo_state = SERVO_RUNNING
-    cmd_servo_q_last = cmd_servo_q
-    cmd_servo_q = q
-    exit_critical
-  end
-
-  def extrapolate():
-    enter_critical
-    diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
-    cmd_servo_q_last = cmd_servo_q
-    cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
-    exit_critical
-
-    return cmd_servo_q
-  end
-
-  thread servoThread():
-    state = SERVO_IDLE
-    while True:
-      enter_critical
-      q = cmd_servo_q
-      do_extrapolate = False
-      if (cmd_servo_state == SERVO_IDLE):
-        do_extrapolate = True
-      end
-      state = cmd_servo_state
-      cmd_servo_state = SERVO_IDLE
-      exit_critical
-      if do_extrapolate:
-        textmsg("No new setpoint received. Extrapolating.")
-        q = extrapolate()
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-      elif state == SERVO_RUNNING:
-        servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-      else:
-        textmsg("Should not be here")
-        sync()
-      end
-    end
-    stopj(0.1)
-  end
-  socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
-
-  thread_servo = run servoThread()
-  keepalive = -2
-  params_mult = socket_read_binary_integer(6+1, "reverse_socket")
-  keepalive = params_mult[7]
-  while keepalive > 0:
-    params_mult = socket_read_binary_integer(6+1, "reverse_socket", steptime)
-    if params_mult[0] > 0:
-      keepalive = params_mult[7]
-      q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
-      set_servo_setpoint(q)
-    else:
-      # TODO: Extrapolation goes here
-      keepalive = keepalive - 1
-    end
-  end
-  sleep(.1)
-  socket_close()
-  kill thread_servo
-end
-)";
-
-ur_driver::UrDriver::UrDriver(const std::string& robot_ip)
   : servoj_time_(0.008), servoj_gain_(750), servoj_lookahead_time_(0.03)
 {
   LOG_INFO("Initializing RTDE client");
@@ -141,7 +64,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip)
 
   uint32_t reverse_port = 50001;  // TODO: Make this a parameter
 
-  std::string prog = POSITION_PROGRAM;
+  std::string prog = readScriptFile(script_file);
   prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
   std::ostringstream out;
   out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
@@ -201,4 +124,12 @@ bool UrDriver::writeJointCommand(const vector6d_t& values)
 
   return true;
 }
+
+std::string UrDriver::readScriptFile(const std::string& filename)
+{
+  std::ifstream ifs(filename);
+  std::string content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+
+  return content;
+}
 }  // namespace ur_driver