Load the script from an external file

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
-  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)
+ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file)
   : 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