moved driver to subfolder

ur_rtde_driver/src/ur/commander.cpp

@@ -0,0 +1,173 @@
+/*
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_rtde_driver/ur/commander.h"
+#include "ur_rtde_driver/log.h"
+
+namespace ur_driver
+{
+bool URCommander::write(const std::string& s)
+{
+  size_t len = s.size();
+  const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
+  size_t written;
+  return stream_.write(data, len, written);
+}
+
+void URCommander::formatArray(std::ostringstream& out, std::array<double, 6>& values)
+{
+  std::string mod("[");
+  for (auto const& val : values)
+  {
+    out << mod << val;
+    mod = ",";
+  }
+  out << "]";
+}
+
+bool URCommander::uploadProg(const std::string& s)
+{
+  LOG_DEBUG("Sending program [%s]", s.c_str());
+  return write(s);
+}
+
+bool URCommander::setToolVoltage(uint8_t voltage)
+{
+  if (voltage != 0 || voltage != 12 || voltage != 24)
+    return false;
+
+  std::ostringstream out;
+  out << "set_tool_voltage(" << (int)voltage << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander::setFlag(uint8_t pin, bool value)
+{
+  std::ostringstream out;
+  out << "set_flag(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+bool URCommander::setPayload(double value)
+{
+  std::ostringstream out;
+  out << "set_payload(" << std::fixed << std::setprecision(5) << value << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander::stopj(double a)
+{
+  std::ostringstream out;
+  out << "stopj(" << std::fixed << std::setprecision(5) << a << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V1_X::speedj(std::array<double, 6>& speeds, double acceleration)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(5);
+  out << "speedj(";
+  formatArray(out, speeds);
+  out << "," << acceleration << "," << 0.02 << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
+{
+  std::ostringstream out;
+  out << "sec io_fun():\n"
+      << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"
+      << "end\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
+{
+  std::ostringstream out;
+  out << "sec io_fun():\n"
+      << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
+      << "end\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
+{
+  std::ostringstream out;
+  out << "sec io_fun():\n"
+      << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n"
+      << "end\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
+{
+  std::ostringstream out;
+  std::string func;
+
+  if (pin < 8)
+  {
+    func = "set_standard_digital_out";
+  }
+  else if (pin < 16)
+  {
+    func = "set_configurable_digital_out";
+    pin -= 8;
+  }
+  else if (pin < 18)
+  {
+    func = "set_tool_digital_out";
+    pin -= 16;
+  }
+  else
+    return false;
+
+  out << "sec io_fun():\n"
+      << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
+      << "end\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V3_1__2::speedj(std::array<double, 6>& speeds, double acceleration)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(5);
+  out << "speedj(";
+  formatArray(out, speeds);
+  out << "," << acceleration << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+
+bool URCommander_V3_3::speedj(std::array<double, 6>& speeds, double acceleration)
+{
+  std::ostringstream out;
+  out << std::fixed << std::setprecision(5);
+  out << "speedj(";
+  formatArray(out, speeds);
+  out << "," << acceleration << "," << 0.008 << ")\n";
+  std::string s(out.str());
+  return write(s);
+}
+}  // namespace ur_driver

ur_rtde_driver/src/ur/master_board.cpp

@@ -0,0 +1,124 @@
+/*
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_rtde_driver/ur/master_board.h"
+#include "ur_rtde_driver/ur/consumer.h"
+
+namespace ur_driver
+{
+bool SharedMasterBoardData::parseWith(BinParser& bp)
+{
+  bp.parse(analog_input_range0);
+  bp.parse(analog_input_range1);
+  bp.parse(analog_input0);
+  bp.parse(analog_input1);
+  bp.parse(analog_output_domain0);
+  bp.parse(analog_output_domain1);
+  bp.parse(analog_output0);
+  bp.parse(analog_output1);
+  bp.parse(master_board_temperature);
+  bp.parse(robot_voltage_48V);
+  bp.parse(robot_current);
+  bp.parse(master_IO_current);
+  return true;
+}
+
+bool MasterBoardData_V1_X::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<MasterBoardData_V1_X>())
+    return false;
+
+  bp.parse<uint16_t, 10>(digital_input_bits);
+  bp.parse<uint16_t, 10>(digital_output_bits);
+
+  SharedMasterBoardData::parseWith(bp);
+
+  bp.parse(master_safety_state);
+  bp.parse(master_on_off_state);
+  bp.parse(euromap67_interface_installed);
+
+  if (euromap67_interface_installed)
+  {
+    if (!bp.checkSize(MasterBoardData_V1_X::EURO_SIZE))
+      return false;
+
+    bp.parse(euromap_input_bits);
+    bp.parse(euromap_output_bits);
+    bp.parse(euromap_voltage);
+    bp.parse(euromap_current);
+  }
+
+  return true;
+}
+
+bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<MasterBoardData_V3_0__1>())
+    return false;
+
+  bp.parse<uint32_t, 18>(digital_input_bits);
+  bp.parse<uint32_t, 18>(digital_output_bits);
+
+  SharedMasterBoardData::parseWith(bp);
+
+  bp.parse(safety_mode);
+  bp.parse(in_reduced_mode);
+  bp.parse(euromap67_interface_installed);
+
+  if (euromap67_interface_installed)
+  {
+    if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
+      return false;
+
+    bp.parse(euromap_input_bits);
+    bp.parse(euromap_output_bits);
+    bp.parse(euromap_voltage);
+    bp.parse(euromap_current);
+  }
+
+  bp.consume(sizeof(uint32_t));
+
+  return true;
+}
+
+bool MasterBoardData_V3_2::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<MasterBoardData_V3_2>())
+    return false;
+
+  MasterBoardData_V3_0__1::parseWith(bp);
+
+  bp.parse(operational_mode_selector_input);
+  bp.parse(three_position_enabling_device_input);
+
+  return true;
+}
+
+bool MasterBoardData_V1_X::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool MasterBoardData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool MasterBoardData_V3_2::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+}  // namespace ur_driver

ur_rtde_driver/src/ur/messages.cpp

@@ -0,0 +1,40 @@
+/*
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_rtde_driver/ur/messages.h"
+#include "ur_rtde_driver/ur/consumer.h"
+
+namespace ur_driver
+{
+bool VersionMessage::parseWith(BinParser& bp)
+{
+  bp.parse(project_name);
+  bp.parse(major_version);
+  bp.parse(minor_version);
+  bp.parse(svn_version);
+  bp.consume(sizeof(uint32_t));  // undocumented field??
+  bp.parseRemainder(build_date);
+
+  return true;  // not really possible to check dynamic size packets
+}
+
+bool VersionMessage::consumeWith(URMessagePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+}  // namespace ur_driver

ur_rtde_driver/src/ur/robot_mode.cpp

@@ -0,0 +1,105 @@
+/*
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_rtde_driver/ur/robot_mode.h"
+#include "ur_rtde_driver/ur/consumer.h"
+
+namespace ur_driver
+{
+bool SharedRobotModeData::parseWith(BinParser& bp)
+{
+  bp.parse(timestamp);
+  bp.parse(physical_robot_connected);
+  bp.parse(real_robot_enabled);
+  bp.parse(robot_power_on);
+  bp.parse(emergency_stopped);
+  bp.parse(protective_stopped);
+  bp.parse(program_running);
+  bp.parse(program_paused);
+  return true;
+}
+
+bool RobotModeData_V1_X::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RobotModeData_V1_X>())
+    return false;
+
+  SharedRobotModeData::parseWith(bp);
+
+  bp.parse(robot_mode);
+  bp.parse(speed_fraction);
+
+  return true;
+}
+
+bool RobotModeData_V3_0__1::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RobotModeData_V3_0__1>())
+    return false;
+
+  SharedRobotModeData::parseWith(bp);
+
+  bp.parse(robot_mode);
+  bp.parse(control_mode);
+  bp.parse(target_speed_fraction);
+  bp.parse(speed_scaling);
+
+  return true;
+}
+
+bool RobotModeData_V3_2::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RobotModeData_V3_2>())
+    return false;
+
+  RobotModeData_V3_0__1::parseWith(bp);
+
+  bp.parse(target_speed_fraction_limit);
+
+  return true;
+}
+
+bool RobotModeData_V3_5::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RobotModeData_V3_5>())
+    return false;
+
+  RobotModeData_V3_2::parseWith(bp);
+
+  bp.parse(unknown_internal_use);
+
+  return true;
+}
+
+bool RobotModeData_V1_X::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool RobotModeData_V3_0__1::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool RobotModeData_V3_2::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool RobotModeData_V3_5::consumeWith(URStatePacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+}  // namespace ur_driver

ur_rtde_driver/src/ur/rt_state.cpp

@@ -0,0 +1,138 @@
+/*
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ur_rtde_driver/ur/rt_state.h"
+#include "ur_rtde_driver/ur/consumer.h"
+
+namespace ur_driver
+{
+void RTShared::parse_shared1(BinParser& bp)
+{
+  bp.parse(time);
+  bp.parse(q_target);
+  bp.parse(qd_target);
+  bp.parse(qdd_target);
+  bp.parse(i_target);
+  bp.parse(m_target);
+  bp.parse(q_actual);
+  bp.parse(qd_actual);
+  bp.parse(i_actual);
+}
+
+void RTShared::parse_shared2(BinParser& bp)
+{
+  bp.parse(digital_inputs);
+  bp.parse(motor_temperatures);
+  bp.parse(controller_time);
+  bp.consume(sizeof(double));  // Unused "Test value" field
+  bp.parse(robot_mode);
+}
+
+bool RTState_V1_6__7::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RTState_V1_6__7>())
+    return false;
+
+  parse_shared1(bp);
+
+  bp.parse(tool_accelerometer_values);
+  bp.consume(sizeof(double) * 15);
+  bp.parse(tcp_force);
+  bp.parse(tool_vector_actual);
+  bp.parse(tcp_speed_actual);
+
+  parse_shared2(bp);
+
+  return true;
+}
+
+bool RTState_V1_8::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RTState_V1_8>())
+    return false;
+
+  RTState_V1_6__7::parseWith(bp);
+
+  bp.parse(joint_modes);
+
+  return true;
+}
+
+bool RTState_V3_0__1::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RTState_V3_0__1>())
+    return false;
+
+  parse_shared1(bp);
+
+  bp.parse(i_control);
+  bp.parse(tool_vector_actual);
+  bp.parse(tcp_speed_actual);
+  bp.parse(tcp_force);
+  bp.parse(tool_vector_target);
+  bp.parse(tcp_speed_target);
+
+  parse_shared2(bp);
+
+  bp.parse(joint_modes);
+  bp.parse(safety_mode);
+  bp.consume(sizeof(double[6]));  // skip undocumented
+  bp.parse(tool_accelerometer_values);
+  bp.consume(sizeof(double[6]));  // skip undocumented
+  bp.parse(speed_scaling);
+  bp.parse(linear_momentum_norm);
+  bp.consume(sizeof(double));  // skip undocumented
+  bp.consume(sizeof(double));  // skip undocumented
+  bp.parse(v_main);
+  bp.parse(v_robot);
+  bp.parse(i_robot);
+  bp.parse(v_actual);
+
+  return true;
+}
+
+bool RTState_V3_2__3::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RTState_V3_2__3>())
+    return false;
+
+  RTState_V3_0__1::parseWith(bp);
+
+  bp.parse(digital_outputs);
+  bp.parse(program_state);
+
+  return true;
+}
+
+bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
+}  // namespace ur_driver

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -0,0 +1,156 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/ur/ur_driver.h"
+#include <memory>
+
+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");
+  rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_));
+
+  if (!rtde_client_->init())
+  {
+    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)
+}
+
+std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
+{
+  // TODO: This goes into the rtde_client
+  std::unique_ptr<comm::URPackage<rtde_interface::PackageHeader>> urpackage;
+  uint32_t period_ms = (1.0 / rtde_frequency_) * 1000;
+  std::chrono::milliseconds timeout(period_ms);
+  if (rtde_client_->getDataPackage(urpackage, timeout))
+  {
+    rtde_interface::DataPackage* tmp = dynamic_cast<rtde_interface::DataPackage*>(urpackage.get());
+    if (tmp != nullptr)
+    {
+      urpackage.release();
+      return std::move(std::unique_ptr<rtde_interface::DataPackage>(tmp));
+    }
+  }
+  return nullptr;
+}
+
+bool UrDriver::writeJointCommand(const vector6d_t& values)
+{
+  if (reverse_interface_)
+  {
+    reverse_interface_->write(values);
+  }
+  else
+  {
+    return false;
+  }
+
+  return true;
+}
+}  // namespace ur_driver