Upload URScript via primary interface

Note: This will probably be changed later, but currently that's the
easiest way to handle this.

CMakeLists.txt

@@ -71,13 +71,13 @@ include_directories(
 
 add_library(ur_rtde_driver
     src/comm/tcp_socket.cpp
+    src/comm/server.cpp
     #src/ros/service_stopper.cpp
     #src/ur/commander.cpp
     #src/ur/master_board.cpp
     #src/ur/messages.cpp
     #src/ur/robot_mode.cpp
     #src/ur/rt_state.cpp
-    #src/ur/server.cpp
     src/primary/primary_package.cpp
     src/primary/robot_message.cpp
     src/primary/robot_message/version_message.cpp

include/ur_rtde_driver/comm/stream.h

@@ -49,7 +49,7 @@ public:
    * \param host IP address of the remote host
    * \param port Port on which the socket shall be connected
    */
-  URStream(std::string& host, int port) : host_(host), port_(port)
+  URStream(const std::string& host, int port) : host_(host), port_(port)
   {
   }
 

include/ur_rtde_driver/ur/ur_driver.h

@@ -26,6 +26,7 @@
 //----------------------------------------------------------------------
 
 #include "ur_rtde_driver/rtde/rtde_client.h"
+#include "ur_rtde_driver/comm/reverse_interface.h"
 
 namespace ur_driver
 {
@@ -60,10 +61,13 @@ public:
     return rtde_frequency_;
   }
 
+  bool writeJointCommand(const vector6d_t& values);
+
 private:
   //! This frequency is used for the rtde interface. By default, it will be 125Hz on CB3 and 500Hz on the e-series.
   uint32_t rtde_frequency_;
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
+  std::unique_ptr<comm::ReverseInterface> reverse_interface_;
 };
 }  // namespace ur_driver

launch/ur10_ros_control.launch

@@ -16,7 +16,7 @@
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
   <arg name="shutdown_on_disconnect" default="true" />
-  <arg name="controllers" default="joint_state_controller"/>
+  <arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller"/>
   <arg name="stopped_controllers" default=""/>
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">

src/comm/server.cpp

@@ -18,7 +18,7 @@
  * limitations under the License.
  */
 
-#include "ur_rtde_driver/ur/server.h"
+#include "ur_rtde_driver/comm/server.h"
 #include <arpa/inet.h>
 #include <netinet/tcp.h>
 #include <unistd.h>

src/ros/hardware_interface.cpp

@@ -101,7 +101,7 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
 
 void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
 {
-  // TODO: Implement
+  ur_driver_->writeJointCommand(joint_position_command_);
 }
 
 bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,

src/ur/ur_driver.cpp

@@ -30,6 +30,66 @@
 
 namespace ur_driver
 {
+static const std::string POSITION_PROGRAM = R"(
+def myProg():
+  textmsg("hello")
+  MULT_jointstate = 1000000
+
+  SERVO_IDLE = 0
+  SERVO_RUNNING = 1
+  cmd_servo_state = SERVO_IDLE
+  cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  def set_servo_setpoint(q):
+    enter_critical
+    cmd_servo_state = SERVO_RUNNING
+    cmd_servo_q = q
+    exit_critical
+  end
+
+  thread servoThread():
+    state = SERVO_IDLE
+    while True:
+      enter_critical
+      q = cmd_servo_q
+      do_brake = False
+      if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
+        do_brake = True
+      end
+      state = cmd_servo_state
+      cmd_servo_state = SERVO_IDLE
+      exit_critical
+      if do_brake:
+        stopj(1.0)
+        sync()
+      elif state == SERVO_RUNNING:
+        servoj(q, t=0.008, lookahead_time=0.03, gain=300)
+      else:
+        sync()
+      end
+    end
+  end
+  socket_open("192.168.56.1", 50001)
+
+  thread_servo = run servoThread()
+  keepalive = -2
+  params_mult = socket_read_binary_integer(6+1)
+  keepalive = params_mult[7]
+  while keepalive > 0:
+    params_mult = socket_read_binary_integer(6+1)
+    keepalive = params_mult[7]
+    if keepalive > 0:
+      if params_mult[0] > 0:
+        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)
+      end
+    end
+  end
+  sleep(.1)
+  socket_close()
+  kill thread_servo
+end
+)";
+
 ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125)  // conservative CB3 default.
 {
   ROS_INFO_STREAM("Initializing RTDE client");
@@ -39,6 +99,26 @@ ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125
   {
     throw std::runtime_error("initialization went wrong");  // TODO: be less harsh
   }
+
+  comm::URStream<rtde_interface::PackageHeader> stream(ROBOT_IP, 30001);
+  stream.connect();
+
+  size_t len = POSITION_PROGRAM.size();
+  const uint8_t* data = reinterpret_cast<const uint8_t*>(POSITION_PROGRAM.c_str());
+  size_t written;
+
+  if (stream.write(data, len, written))
+  {
+    LOG_INFO("Sent program to robot");
+  }
+  else
+  {
+    LOG_ERROR("Could not send program to robot");
+  }
+
+  uint32_t reverse_port = 50001;  // TODO: Make this a parameter
+  reverse_interface_.reset(new comm::ReverseInterface(reverse_port));
+
   rtde_client_->start();  // TODO: Add extra start method (also to HW-Interface)
 }
 
@@ -59,4 +139,18 @@ std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage
   }
   return nullptr;
 }
+
+bool UrDriver::writeJointCommand(const vector6d_t& values)
+{
+  if (reverse_interface_)
+  {
+    reverse_interface_->write(values);
+  }
+  else
+  {
+    return false;
+  }
+
+  return true;
+}
 }  // namespace ur_driver