Send control type from hardware interface

TODO:
 - Documentation of function members
 - Using enums for control modes

ur_robot_driver/include/ur_robot_driver/comm/reverse_interface.h

@@ -37,6 +37,18 @@ namespace ur_driver
 {
 namespace comm
 {
+/*!
+ * \brief Control modes as interpreted from the script runnning on the robot.
+ */
+enum class ControlMode : int32_t
+{
+  MODE_STOPPED = -2,        ///< When this is set, the program is expected to stop and exit.
+  MODE_UNINITIALIZED = -1,  ///< Startup default until another mode is sent to the script.
+  MODE_IDLE = 0,            ///< Set when no controller is currently active controlling the robot.
+  MODE_SERVOJ = 1,          ///< Set when servoj control is active.
+  MODE_SPEEDJ = 2           ///< Set when speedj control is active.
+};
+
 /*!
  * \brief The ReverseInterface class handles communication to the robot. It starts a server and
  * waits for the robot to connect via its URCaps program.
@@ -72,17 +84,19 @@ public:
   /*!
    * \brief Writes needed information to the robot to be read by the URCaps program.
    *
-   * \param positions A vector of joint position targets for the robot
-   * \param type An additional integer used to command the program to end when needed
+   * \param positions A vector of joint targets for the robot
+   * \param control_mode Control mode assigned to this command. See documentation of ::ControlMode
+   * for details on possible values.
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool write(const vector6d_t* positions, const int32_t type = 2)
+  bool write(const vector6d_t* positions, const ControlMode control_mode = ControlMode::MODE_IDLE)
   {
-    uint8_t buffer[sizeof(uint32_t) * 7];
+    uint8_t buffer[sizeof(int32_t) * 8];
     uint8_t* b_pos = buffer;
 
-    int32_t val = htobe32(type);
+    // The first element is always the keepalive signal.
+    int32_t val = htobe32(1);
     b_pos += append(b_pos, val);
 
     if (positions != nullptr)
@@ -94,6 +108,13 @@ public:
         b_pos += append(b_pos, val);
       }
     }
+    else
+    {
+      b_pos += 6 * sizeof(int32_t);
+    }
+
+    val = htobe32(toUnderlying(control_mode));
+    b_pos += append(b_pos, val);
 
     size_t written;
 

ur_robot_driver/include/ur_robot_driver/types.h

@@ -46,4 +46,17 @@ std::ostream& operator<<(std::ostream& out, const std::array<T, N>& item)
   out << "]";
   return out;
 }
+
+/*!
+ * \brief Converts an enum type to its underlying type
+ *
+ * \param e Enum value that should be converted
+ *
+ * \returns Enum value converted to underlying type
+ */
+template <typename E>
+constexpr typename std::underlying_type<E>::type toUnderlying(const E e) noexcept
+{
+  return static_cast<typename std::underlying_type<E>::type>(e);
+}
 }  // namespace ur_driver

ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h

@@ -122,10 +122,11 @@ public:
    * the robot.
    *
    * \param values Desired joint positions
+   * \param control_mode Control mode this command is assigned to.
    *
    * \returns True on successful write.
    */
-  bool writeJointCommand(const vector6d_t& values);
+  bool writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode);
 
   /*!
    * \brief Write a keepalive signal only.

ur_robot_driver/launch/ur_control.launch

@@ -15,7 +15,7 @@
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
   <arg name="controllers" default="joint_state_controller vel_based_pos_traj_controller force_torque_sensor_controller"/>
   <arg name="stopped_controllers" default="joint_group_vel_controller"/>
-  <arg name="urscript_file" default="$(find ur_robot_driver)/resources/speedj.urscript"/>
+  <arg name="urscript_file" default="$(find ur_robot_driver)/resources/servoj.urscript"/>
   <arg name="rtde_output_recipe_file" default="$(find ur_robot_driver)/resources/rtde_output_recipe.txt" doc="Recipe file used for the RTDE-outputs. Only change this if you know what you're doing."/>
   <arg name="rtde_input_recipe_file" default="$(find ur_robot_driver)/resources/rtde_input_recipe.txt" doc="Recipe file used for the RTDE-inputs. Only change this if you know what you're doing."/>
   <arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>

ur_robot_driver/resources/servoj.urscript

 {{BEGIN_REPLACE}}
 
 steptime = get_steptime()
+speedj_time = steptime - 0.1*steptime
 textmsg("steptime=", steptime)
 MULT_jointstate = {{JOINT_STATE_REPLACE}}
 
 #Constants
-SERVO_STOPPED = -2
 SERVO_UNINITIALIZED = -1
 SERVO_IDLE = 0
 SERVO_RUNNING = 1
 
+MODE_STOPPED = -2
+MODE_UNINITIALIZED = -1
+MODE_IDLE = 0
+MODE_SERVOJ = 1
+MODE_SPEEDJ = 2
+
 #Global variables are also showed in the Teach pendants variable list
 global cmd_servo_state = SERVO_UNINITIALIZED
+global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 global cmd_servo_q = get_actual_joint_positions()
 global cmd_servo_q_last = get_actual_joint_positions()
 global extrapolate_count = 0
 global extrapolate_max_count = 0
+global control_mode = MODE_UNINITIALIZED
+cmd_speedj_active = True
 
 def set_servo_setpoint(q):
   cmd_servo_state = SERVO_RUNNING
@@ -32,8 +41,9 @@ def extrapolate():
 end
 
 thread servoThread():
+  textmsg("Starting servo thread")
   state = SERVO_IDLE
-  while state > SERVO_STOPPED:
+  while control_mode == MODE_SERVOJ:
     enter_critical
     q = cmd_servo_q
     do_extrapolate = False
@@ -57,7 +67,6 @@ thread servoThread():
     elif state == SERVO_RUNNING:
       extrapolate_count = 0
       servoj(q, t=steptime, {{SERVO_J_REPLACE}})
-
     else:
       extrapolate_count = 0
       sync()
@@ -67,24 +76,55 @@ thread servoThread():
   textmsg("servo thread ended")
   stopj(0.1)
 end
+
+# Helpers for speed control
+def set_speed(qd):
+  cmd_servo_qd = qd
+  control_mode = MODE_SPEEDJ
+end
+
+thread speedThread():
+  textmsg("Starting speed thread")
+  while control_mode == MODE_SPEEDJ:
+    enter_critical
+    qd = cmd_servo_qd
+    exit_critical
+    # Having this shortly under steptime seems to help
+    speedj(qd, 40.0, speedj_time)
+    sync()
+  end
+  textmsg("speedj thread ended")
+  stopj(5.0)
+end
+
 socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
 
-thread_servo = run servoThread()
+control_mode = MODE_UNINITIALIZED
+thread_move = 0
 keepalive = -2
 textmsg("External control active")
-params_mult = socket_read_binary_integer(1+6, "reverse_socket")
+params_mult = socket_read_binary_integer(1+6+1, "reverse_socket")
 keepalive = params_mult[1]
-while keepalive > 0:
+while keepalive > 0 and control_mode > MODE_STOPPED:
   enter_critical
   socket_send_line(1, "reverse_socket")
-  params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
+  params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
   if params_mult[0] > 0:
     keepalive = params_mult[1]
-    if params_mult[1] > 1:
+    if control_mode != params_mult[8]:
+      control_mode = params_mult[8]
+      if control_mode == MODE_SERVOJ:
+        thread_move = run servoThread()
+      elif control_mode == MODE_SPEEDJ:
+        thread_move = run speedThread()
+      end
+    end
+    if control_mode == MODE_SERVOJ:
       q = [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, params_mult[7] / MULT_jointstate]
       set_servo_setpoint(q)
-    else:
-      set_servo_setpoint(get_actual_joint_positions())
+    elif control_mode == MODE_SPEEDJ:
+      qd = [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, params_mult[7] / MULT_jointstate]
+      set_speed(qd)
     end
   else:
     keepalive = keepalive - 1
@@ -92,8 +132,6 @@ while keepalive > 0:
   exit_critical
 end
 
-textmsg("Stopping communication and servoing")
-cmd_servo_state = SERVO_STOPPED
-sleep(.1)
+textmsg("Stopping communication and control")
+sleep(0.1)
 socket_close("reverse_socket")
-kill thread_servo

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -476,22 +476,22 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
   {
     if (position_controller_running_)
     {
-      ur_driver_->writeJointCommand(joint_position_command_);
+      ur_driver_->writeJointCommand(joint_position_command_, comm::ControlMode::MODE_SERVOJ);
     }
     else if (velocity_controller_running_)
     {
-      ur_driver_->writeJointCommand(joint_velocity_command_);
-    }
-    else if (robot_program_running_)
-    {
-      ur_driver_->writeKeepalive();
+      ur_driver_->writeJointCommand(joint_velocity_command_, comm::ControlMode::MODE_SPEEDJ);
     }
     else
     {
-      ur_driver_->stopControl();
+      ur_driver_->writeKeepalive();
     }
     packet_read_ = false;
   }
+  else
+  {
+    ur_driver_->stopControl();
+  }
 }
 
 bool HardwareInterface::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,

ur_robot_driver/src/ur/ur_driver.cpp

@@ -167,11 +167,11 @@ std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage
   return rtde_client_->getDataPackage(timeout);
 }
 
-bool UrDriver::writeJointCommand(const vector6d_t& values)
+bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode)
 {
   if (reverse_interface_active_)
   {
-    return reverse_interface_->write(&values);
+    return reverse_interface_->write(&values, control_mode);
   }
   return false;
 }
@@ -181,7 +181,7 @@ bool UrDriver::writeKeepalive()
   if (reverse_interface_active_)
   {
     vector6d_t* fake = nullptr;
-    return reverse_interface_->write(fake, 1);
+    return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE);
   }
   return false;
 }
@@ -196,7 +196,7 @@ bool UrDriver::stopControl()
   if (reverse_interface_active_)
   {
     vector6d_t* fake = nullptr;
-    return reverse_interface_->write(fake, 0);
+    return reverse_interface_->write(fake, comm::ControlMode::MODE_STOPPED);
   }
   return false;
 }