implemented initialization of rs485 interface.

ur_rtde_driver/CMakeLists.txt

@@ -96,6 +96,7 @@ add_library(ur_rtde_driver
     src/rtde/text_message.cpp
     src/rtde/rtde_client.cpp
     src/ur/ur_driver.cpp
+    src/ur/tool_communication.cpp
 )
 target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES})
 add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

ur_rtde_driver/config/ur10e_controllers.yaml

@@ -0,0 +1,113 @@
+# Settings for ros_control control loop
+hardware_control_loop:
+   loop_hz: 125
+
+# Settings for ros_control hardware interface
+hardware_interface:
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+
+# Publish all joint states ----------------------------------
+joint_state_controller:
+   type:         joint_state_controller/JointStateController
+   publish_rate: 125
+
+# Publish wrench ----------------------------------
+force_torque_sensor_controller:
+   type:         force_torque_sensor_controller/ForceTorqueSensorController
+   publish_rate: 125
+
+# Publish speed_scaling factor
+speed_scaling_state_controller:
+   type:         ur_controllers/SpeedScalingStateController
+   publish_rate: 125
+
+# Joint Trajectory Controller - position based -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+pos_based_pos_traj_controller:
+   #type: position_controllers/JointTrajectoryController
+   type: position_controllers/ScaledJointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.4, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.4, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate: 500
+   action_monitor_rate: 10
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #stop_trajectory_duration: 0 # Defaults to 0.0
+
+# Joint Trajectory Controller -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+vel_based_pos_traj_controller:
+   type: velocity_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate:  125
+   action_monitor_rate: 10
+   gains:
+      #!!These values have not been optimized!!
+      shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+
+   # Use a feedforward term to reduce the size of PID gains
+   velocity_ff:
+      shoulder_pan_joint: 1.0
+      shoulder_lift_joint: 1.0
+      elbow_joint: 1.0
+      wrist_1_joint: 1.0
+      wrist_2_joint: 1.0
+      wrist_3_joint: 1.0
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #stop_trajectory_duration: 0 # Defaults to 0.0
+
+# Pass an array of joint velocities directly to the joints
+joint_group_vel_controller:
+   type: velocity_controllers/JointGroupVelocityController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint

ur_rtde_driver/include/ur_rtde_driver/ur/tool_communication.h

@@ -0,0 +1,168 @@
+
+// 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-06-06
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED
+#define UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED
+
+#include "ur_rtde_driver/types.h"
+#include <set>
+
+namespace ur_driver
+{
+enum class ToolVoltage : int
+{
+  OFF = 0,
+  _12V = 12,
+  _24V = 24
+};
+
+enum class Parity : int
+{
+  NONE = 0,
+  ODD = 1,
+  EVEN = 2
+};
+
+/*!
+ * \brief Helper class that represents a numeric value with a lower and an upper boundary.
+ *
+ * @tparam T any type for which a comparison exists.
+ */
+template <class T>
+class Limited
+{
+public:
+  Limited() = delete;
+  ~Limited() = default;
+
+  using Datatype = T;
+
+  Limited(const T lower, const T upper) : lower_(lower), upper_(upper)
+  {
+    data_ = lower_;
+  }
+
+  void setData(const T data)
+  {
+    if (data >= lower_ && data <= upper_)
+    {
+      data_ = data;
+    }
+    else
+    {
+      throw std::runtime_error("Given data is out of range");
+    }
+  }
+
+  T getData() const
+  {
+    return data_;
+  }
+
+private:
+  T data_;
+  const T lower_;
+  const T upper_;
+};
+
+/*!
+ * \brief Class holding a tool communication configuration
+ */
+class ToolCommSetup
+{
+public:
+  ToolCommSetup();
+  ~ToolCommSetup() = default;
+
+  using StopBitsT = Limited<uint32_t>;
+  using RxIdleCharsT = Limited<float>;
+  using TxIdleCharsT = Limited<float>;
+
+  void setToolVoltage(const ToolVoltage tool_voltage)
+  {
+    tool_voltage_ = tool_voltage;
+  }
+  ToolVoltage getToolVoltage() const
+  {
+    return tool_voltage_;
+  }
+
+  void setParity(const Parity parity)
+  {
+    parity_ = parity;
+  }
+  Parity getParity() const
+  {
+    return parity_;
+  }
+
+  void setBaudRate(const uint32_t baud_rate);
+  uint32_t getBaudRate() const
+  {
+    return baud_rate_;
+  };
+
+  void setStopBits(const StopBitsT::Datatype stop_bits)
+  {
+    stop_bits_.setData(stop_bits);
+  }
+  StopBitsT::Datatype getStopBits() const
+  {
+    return stop_bits_.getData();
+  }
+
+  void setRxIdleChars(const RxIdleCharsT::Datatype rx_idle_chars)
+  {
+    rx_idle_chars_.setData(rx_idle_chars);
+  }
+  RxIdleCharsT::Datatype getRxIdleChars() const
+  {
+    return rx_idle_chars_.getData();
+  }
+
+  void setTxIdleChars(const TxIdleCharsT::Datatype tx_idle_chars)
+  {
+    tx_idle_chars_.setData(tx_idle_chars);
+  }
+  TxIdleCharsT::Datatype getTxIdleChars() const
+  {
+    return tx_idle_chars_.getData();
+  }
+
+private:
+  const std::set<uint32_t> baud_rates_{ 9600, 19200, 38400, 57600, 115200, 10000000, 2000000, 5000000 };
+
+  ToolVoltage tool_voltage_;
+  Parity parity_;
+  uint32_t baud_rate_;
+  StopBitsT stop_bits_;
+  RxIdleCharsT rx_idle_chars_;
+  TxIdleCharsT tx_idle_chars_;
+};
+}  // namespace ur_driver
+#endif  // ifndef UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -30,6 +30,7 @@
 #include "ur_rtde_driver/rtde/rtde_client.h"
 #include "ur_rtde_driver/comm/reverse_interface.h"
 #include "ur_rtde_driver/comm/script_sender.h"
+#include "ur_rtde_driver/ur/tool_communication.h"
 
 namespace ur_driver
 {
@@ -47,9 +48,22 @@ public:
    *
    * \param robot_ip IP-address under which the robot is reachable.
    * \param script_file URScript file that should be sent to the robot
+   * \param tool_comm_setup Configuration for using the tool communication
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file,
-           std::function<void(bool)> handle_program_state);
+           std::function<void(bool)> handle_program_state, std::unique_ptr<ToolCommSetup> tool_comm_setup);
+  /*!
+   * \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, const std::string& script_file, const std::string& recipe_file,
+           std::function<void(bool)> handle_program_state)
+    : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr<ToolCommSetup>{})
+  {
+  }
+
   virtual ~UrDriver() = default;
 
   /*!

ur_rtde_driver/launch/ur10_ros_control.launch

@@ -31,6 +31,7 @@
     <param name="robot_ip" type="str" value="$(arg robot_ip)"/>
     <param name="script_file" value="$(arg urscript_file)"/>
     <param name="recipe_file" value="$(arg rtde_recipe_file)"/>
+    <param name="use_tool_communication" value="false"/>
     <!--<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/launch/ur10e_ros_control.launch

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- GDB functionality -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <arg name="robot_ip"/>
+  <arg name="reverse_port" default="50001"/>
+  <arg name="limited" default="false"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="3.0"/>
+  <arg name="prefix" default="" />
+  <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
+  <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 pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
+  <arg name="stopped_controllers" default=""/>
+  <arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
+  <arg name="rtde_recipe_file" default="$(find ur_rtde_driver)/resources/rtde_recipe.txt"/>
+  <arg name="use_tool_communication" default="true"/>
+  <arg name="tool_voltage" default="12"/>
+  <arg name="tool_parity" default="1"/>
+  <arg name="tool_baud_rate" default="57600"/>
+  <arg name="tool_stop_bits" default="1"/>
+  <arg name="tool_rx_idle_chars" default="1.5"/>
+  <arg name="tool_tx_idle_chars" default="3.5"/>
+
+  <!-- robot model -->
+  <include file="$(find ur_e_description)/launch/ur10e_upload.launch">
+    <arg name="limited" value="$(arg limited)"/>
+  </include>
+
+
+  <!-- Load hardware interface -->
+  <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="recipe_file" value="$(arg rtde_recipe_file)"/>
+    <param name="use_tool_communication" value="$(arg use_tool_communication)"/>
+    <param name="tool_voltage" value="$(arg tool_voltage)"/>
+    <param name="tool_parity" value="$(arg tool_parity)"/>
+    <param name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
+    <param name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
+    <param name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
+    <param name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
+  </node>
+
+  <!-- Load controller settings -->
+  <rosparam file="$(find ur_rtde_driver)/config/ur10e_controllers.yaml" command="load"/>
+
+  <!-- spawn controller manager -->
+  <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="$(arg controllers)" />
+
+  <!-- load other controller -->
+  <!--<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"-->
+    <!--output="screen" args="load $(arg stopped_controllers)" />-->
+
+  <!-- Convert joint states to /tf tranforms -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+</launch>

ur_rtde_driver/resources/servoj.urscript

@@ -1,3 +1,5 @@
+{{BEGIN_REPLACE}}
+
 global steptime = get_steptime()
 textmsg("steptime=", steptime)
 global MULT_jointstate = {{JOINT_STATE_REPLACE}}

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -26,6 +26,7 @@
 //----------------------------------------------------------------------
 
 #include "ur_rtde_driver/ros/hardware_interface.h"
+#include "ur_rtde_driver/ur/tool_communication.h"
 
 #include <Eigen/Geometry>
 
@@ -66,9 +67,67 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   tcp_link_ = robot_hw_nh.param<std::string>("tcp_link", "tool0");
   program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("robot_program_running", 10, true);
 
+  bool use_tool_communication = robot_hw_nh.param<bool>("use_tool_communication", "false");
+  std::unique_ptr<ToolCommSetup> tool_comm_setup;
+  if (use_tool_communication)
+  {
+    tool_comm_setup.reset(new ToolCommSetup());
+
+    using ToolVoltageT = std::underlying_type<ToolVoltage>::type;
+    ToolVoltageT tool_voltage;
+    if (!robot_hw_nh.getParam("tool_voltage", tool_voltage))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_voltage") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setToolVoltage(static_cast<ToolVoltage>(tool_voltage));
+
+    using ParityT = std::underlying_type<Parity>::type;
+    ParityT parity;
+    if (!robot_hw_nh.getParam("tool_parity", parity))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_parity") << " not given.");
+      return false;
+    }
+
+    int baud_rate;
+    if (!robot_hw_nh.getParam("tool_baud_rate", baud_rate))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_baud_rate") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setBaudRate(baud_rate);
+
+    int stop_bits;
+    if (!robot_hw_nh.getParam("tool_stop_bits", stop_bits))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_stop_bits") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setStopBits(stop_bits);
+
+    int rx_idle_chars;
+    if (!robot_hw_nh.getParam("tool_rx_idle_chars", rx_idle_chars))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_rx_idle_chars") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setRxIdleChars(rx_idle_chars);
+    tool_comm_setup->setParity(static_cast<Parity>(parity));
+
+    int tx_idle_chars;
+    if (!robot_hw_nh.getParam("tool_tx_idle_chars", tx_idle_chars))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_tx_idle_chars") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setTxIdleChars(tx_idle_chars);
+  }
+
   ROS_INFO_STREAM("Initializing urdriver");
   ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename,
-                                std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1)));
+                                std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
+                                std::move(tool_comm_setup)));
 
   if (!root_nh.getParam("hardware_interface/joints", joint_names_))
   {

ur_rtde_driver/src/ur/tool_communication.cpp

@@ -0,0 +1,53 @@
+// 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-06-06
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/ur/tool_communication.h"
+
+namespace ur_driver
+{
+ToolCommSetup::ToolCommSetup()
+  : tool_voltage_(ToolVoltage::OFF)
+  , parity_(Parity::ODD)
+  , baud_rate_(9600)
+  , stop_bits_(1, 2)
+  , rx_idle_chars_(1.0, 40.0)
+  , tx_idle_chars_(0.0, 40.0)
+{
+}
+
+void ToolCommSetup::setBaudRate(const uint32_t baud_rate)
+{
+  if (baud_rates_.find(baud_rate) != baud_rates_.end())
+  {
+    baud_rate_ = baud_rate;
+  }
+  else
+  {
+    throw std::runtime_error("Provided baud rate is not allowed");
+  }
+}
+}  // namespace ur_driver

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -38,13 +38,15 @@
 namespace ur_driver
 {
 static const int32_t MULT_JOINTSTATE = 1000000;
+static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}");
 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}}");
 
 ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file,
-                              const std::string& recipe_file, std::function<void(bool)> handle_program_state)
+                              const std::string& recipe_file, std::function<void(bool)> handle_program_state,
+                              std::unique_ptr<ToolCommSetup> tool_comm_setup)
   : servoj_time_(0.008)
   , servoj_gain_(2000)
   , servoj_lookahead_time_(0.03)
@@ -80,6 +82,20 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
   prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
 
+  std::stringstream begin_replace;
+  if (tool_comm_setup != nullptr)
+  {
+    begin_replace << "set_tool_voltage("
+                  << static_cast<std::underlying_type<ToolVoltage>::type>(tool_comm_setup->getToolVoltage()) << ")\n";
+    begin_replace << "set_tool_communication("
+                  << "True"
+                  << ", " << tool_comm_setup->getBaudRate() << ", "
+                  << static_cast<std::underlying_type<Parity>::type>(tool_comm_setup->getParity()) << ", "
+                  << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", "
+                  << tool_comm_setup->getTxIdleChars() << ")";
+  }
+  prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str());
+
   script_sender_.reset(new comm::ScriptSender(script_sender_port, prog));
   script_sender_->start();
   LOG_INFO("Created script sender");