Add EKI based hardware interface (#110)

Squashed commits:

* eki_hw_interface: Initial commit. An EKI-based hardware interface.
* PR fix: copyright
* PR fix: add_compile_options
* PR fix: header whitespace and readme backticks / syntax highlighting
* PR fix: (invalid) descript ip address placeholders for EKI xml docs
* PR fix: removing depend on std_msgs
* PR fix: use angles package for deg/rad conversions
* PR fix: use 2 threads for spinning
* PR fix: improve err msg for missing ip or port parameters
* PR fix: ros_control pass read/write params by reference
* PR fix: socket_read_state return/fail early
* PR fix: TODO's -> Github issues...
* PR fix: make socket_read_state in_buffer static
* PR fix: removing unused velocity/effort command vectors
* PR fix: krl/kuka_eki_hw_interface.src cleanup
* PR fix: Adding velocity and effort to transmitted state
  Note: velocty and effort value population in the krl interrupt currently disabled due to it inducing stuttering.  Interrupt service needs to be shorter...  could re-add possibly after some optimization and further testing...
* PR fix: Adding velocity and effort to transmitted state
* PR fix: combining state and command servers into single EkiHwInterface server
* PR fix: return/fail early in KRL
* PR fix: use init() in place of configure() for future Kinetic compatibility
* PR fix: Check for null FirstChildElement results
* PR fix: adding comments to EKI xml configuration file
* PR fix: adding timeout (5 seconds) to eki_read_state() socket call
* PR fix: making socket timeout optionally settable via parameter (with default)

kuka_eki_hw_interface/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(kuka_eki_hw_interface)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  angles
+  cmake_modules
+  controller_manager
+  hardware_interface
+  joint_limits_interface
+  roscpp
+)
+
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(TinyXML REQUIRED)
+
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    kuka_eki_hw_interface
+  CATKIN_DEPENDS
+    controller_manager
+    hardware_interface
+    joint_limits_interface
+    roscpp
+  DEPENDS
+    TinyXML
+)
+
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${TinyXML_INCLUDE_DIRS}
+)
+
+add_library(kuka_eki_hw_interface
+  src/kuka_eki_hw_interface.cpp
+)
+
+target_link_libraries(kuka_eki_hw_interface
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+
+add_executable(kuka_eki_hw_interface_node
+  src/kuka_eki_hw_interface_node.cpp
+)
+
+target_link_libraries(kuka_eki_hw_interface_node
+  kuka_eki_hw_interface
+)
+
+install(
+  TARGETS kuka_eki_hw_interface
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+install(
+  TARGETS kuka_eki_hw_interface_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

kuka_eki_hw_interface/config/controller_joint_names.yaml

@@ -0,0 +1 @@
+controller_joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"]

kuka_eki_hw_interface/config/hardware_controllers.yaml

@@ -0,0 +1,18 @@
+#Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# Joint trajectory controller
+position_trajectory_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - joint_a1
+    - joint_a2
+    - joint_a3
+    - joint_a4
+    - joint_a5
+    - joint_a6
+
+  state_publish_rate: 50 # Defaults to 50
+  action_monitor_rate: 20 # Defaults to 20

kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h

@@ -0,0 +1,104 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, 3M
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *      * Redistributions of source code must retain the above copyright
+ *        notice, this list of conditions and the following disclaimer.
+ *      * Redistributions in binary form must reproduce the above copyright
+ *        notice, this list of conditions and the following disclaimer in the
+ *        documentation and/or other materials provided with the distribution.
+ *      * Neither the name of the copyright holder, nor the names of its
+ *        contributors may be used to endorse or promote products derived
+ *        from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+// Author: Brett Hemes (3M) <[email protected]>
+
+
+#ifndef KUKA_EKI_HW_INTERFACE
+#define KUKA_EKI_HW_INTERFACE
+
+#include <vector>
+#include <string>
+
+#include <boost/asio.hpp>
+
+#include <ros/ros.h>
+#include <controller_manager/controller_manager.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+
+
+namespace kuka_eki_hw_interface
+{
+
+class KukaEkiHardwareInterface : public hardware_interface::RobotHW
+{
+private:
+  ros::NodeHandle nh_;
+
+  const unsigned int n_dof_ = 6;
+  std::vector<std::string> joint_names_;
+  std::vector<double> joint_position_;
+  std::vector<double> joint_velocity_;
+  std::vector<double> joint_effort_;
+  std::vector<double> joint_position_command_;
+
+  // EKI
+  std::string eki_server_address_;
+  std::string eki_server_port_;
+
+  // Timing
+  ros::Duration control_period_;
+  ros::Duration elapsed_time_;
+  double loop_hz_;
+
+  // Interfaces
+  hardware_interface::JointStateInterface joint_state_interface_;
+  hardware_interface::PositionJointInterface position_joint_interface_;
+
+  // EKI socket read/write
+  int eki_read_state_timeout_ = 5;  // [s]; settable by parameter (default = 5)
+  boost::asio::io_service ios_;
+  boost::asio::deadline_timer deadline_;
+  boost::asio::ip::udp::endpoint eki_server_endpoint_;
+  boost::asio::ip::udp::socket eki_server_socket_;
+  void eki_check_read_state_deadline();
+  static void eki_handle_receive(const boost::system::error_code &ec, size_t length,
+                                 boost::system::error_code* out_ec, size_t* out_length);
+  bool eki_read_state(std::vector<double> &joint_position, std::vector<double> &joint_velocity,
+                      std::vector<double> &joint_effort);
+  bool eki_write_command(const std::vector<double> &joint_position);
+
+public:
+
+  KukaEkiHardwareInterface();
+  ~KukaEkiHardwareInterface();
+
+  void init();
+  void start();
+  void read(const ros::Time &time, const ros::Duration &period);
+  void write(const ros::Time &time, const ros::Duration &period);
+};
+
+} // namespace kuka_eki_hw_interface
+
+#endif  // KUKA_EKI_HW_INTERFACE

kuka_eki_hw_interface/krl/EkiHwInterface.xml

@@ -0,0 +1,69 @@
+<ETHERNETKRL>
+   <CONFIGURATION>
+      <EXTERNAL>
+         <TYPE>Client</TYPE>                  <!-- Users connect as clients -->
+      </EXTERNAL>
+      <INTERNAL>
+         <ENVIRONMENT>Program</ENVIRONMENT>   <!-- Server run via robot interpreter -->
+         <BUFFERING Limit="512" />            <!-- Allow buffering of up to 512 messages (system max) -->
+         <ALIVE Set_Flag="1" />               <!-- Use $flag[1] to indicate alive/good connection status -->
+         <IP>address.of.robot.controller</IP> <!-- IP address for EKI interface on robot controller (KRC) -->
+         <PORT>54600</PORT>                   <!-- Port of EKI interface on robot controller (in [54600, 54615]) -->
+         <PROTOCOL>UDP</PROTOCOL>             <!-- Use UDP protocol -->
+      </INTERNAL>
+   </CONFIGURATION>
+
+   <!-- Configured XML structure for data reception (external client to robot)
+        <RobotCommand>
+           <Pos A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
+           </Pos>
+        </RobotCommand>
+   -->
+   <RECEIVE>
+      <XML>
+         <!-- Joint position command -->
+         <ELEMENT Tag="RobotCommand/Pos/@A1" Type="REAL" />
+         <ELEMENT Tag="RobotCommand/Pos/@A2" Type="REAL" />
+         <ELEMENT Tag="RobotCommand/Pos/@A3" Type="REAL" />
+         <ELEMENT Tag="RobotCommand/Pos/@A4" Type="REAL" />
+         <ELEMENT Tag="RobotCommand/Pos/@A5" Type="REAL" />
+         <ELEMENT Tag="RobotCommand/Pos/@A6" Type="REAL" />
+      </XML>
+   </RECEIVE>
+
+   <!-- Configured XML structure for data transmission (robot to external client)
+        <RobotState>
+           <Pos A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
+           </Pos>
+           <Vel A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
+           </Vel>
+           <Eff A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
+           </Eff>
+        </RobotState>
+   -->
+   <SEND>
+      <XML>
+         <!-- Joint state positions -->
+         <ELEMENT Tag="RobotState/Pos/@A1"/>
+         <ELEMENT Tag="RobotState/Pos/@A2"/>
+         <ELEMENT Tag="RobotState/Pos/@A3"/>
+         <ELEMENT Tag="RobotState/Pos/@A4"/>
+         <ELEMENT Tag="RobotState/Pos/@A5"/>
+         <ELEMENT Tag="RobotState/Pos/@A6"/>
+         <!-- Joint state velocities -->
+         <ELEMENT Tag="RobotState/Vel/@A1"/>
+         <ELEMENT Tag="RobotState/Vel/@A2"/>
+         <ELEMENT Tag="RobotState/Vel/@A3"/>
+         <ELEMENT Tag="RobotState/Vel/@A4"/>
+         <ELEMENT Tag="RobotState/Vel/@A5"/>
+         <ELEMENT Tag="RobotState/Vel/@A6"/>
+         <!-- Joint state efforts (torques) -->
+         <ELEMENT Tag="RobotState/Eff/@A1"/>
+         <ELEMENT Tag="RobotState/Eff/@A2"/>
+         <ELEMENT Tag="RobotState/Eff/@A3"/>
+         <ELEMENT Tag="RobotState/Eff/@A4"/>
+         <ELEMENT Tag="RobotState/Eff/@A5"/>
+         <ELEMENT Tag="RobotState/Eff/@A6"/>
+      </XML>
+   </SEND>
+</ETHERNETKRL>

kuka_eki_hw_interface/krl/README.md

@@ -0,0 +1,88 @@
+# Configuring EKI on the controller
+
+This guide highlights the steps needed in order to successfully configure the **EKI interface** on the controller to work with the **kuka_eki_hw_interface** on your PC with ROS.
+
+## 1. Controller network configuration
+
+Windows runs behind the SmartHMI on the teach pad. Make sure that the **Windows interface** of the controller and the **PC with ROS** is connected to the same subnet.
+
+1. Log in as **Expert** or **Administrator** on the teach pad and navigate to **Network configuration** (**Start-up > Network configuration > Activate advanced configuration**).
+2. There should already be an interface checked out as the **Windows interface**. For example:
+   * **IP**: `192.168.1.121`
+   * **Subnet mask**: `255.255.255.0`
+   * **Default gateway**: `192.168.1.121`
+   * **Windows interface checkbox** should be checked.
+3. Make note of the above IP address as you will need it later.
+4. (Optional) Run **cmd.exe** and ping the PC you want to communicate with on the same subnet (e.g. 192.168.1.10).  If your **PC** has an IP address on the same subnet as the **Windows interface** on the controller, the controller should receive answers from the PC.
+
+## 2. KRL Files
+
+The files included in the `kuka_eki_hw_interface/krl` folder provide the KRL interface and Ethernet packet configurations.  The XML files need to be modified to work for your specific configuration:
+
+##### EkiHwInterface.xml
+1. Edit the `IP` tag so that it corresponds to the IP address (`address.of.robot.controller`) corresponding to the **Windows interface** of the controller (noted earlier).
+2. Keep the `PORT` tag as it is (`54600`) or change it if you want to use another port (must be in the range of `54600` to `54615`).
+
+Note that the `eki/robot_address` and `eki/robot_port` parameters of the `kuka_eki_hw_interface` must correspond to the `IP`and `PORT` set in this XML file.
+
+##### Copy files to controller
+The files `kuka_eki_hw_interface.dat` and `kuke_eki_hw_interface.src` should not be edited. All files are now ready to be copied to the Kuka controller.  Using WorkVisual or a USB drive (with appropriate privleges):
+
+1. Copy `kuka_eki_hw_interface.dat` and `kuka_eki_hw_interface.src` files to `KRC:\R1\Program`.
+2. Copy `EkiHwInterface.xml` to `C:\KRC\ROBOTER\Config\User\Common\EthernetKRL\`.
+
+## 3. Configure the kuka_eki_hw_interface
+The **kuka_eki_hw_interface** needs to be configured in order to successfully communicate with EKI on the controller. Inside `/kuka_eki_hw_interface/test` and `/kuka_eki_hw_interface/config` in this repository is a set of `*.yaml` files. These configuration files may be loaded into a launch-file used to start the **kuka_eki_hw_interface** with correct parameters, such as:
+
+* **Joint names** corresponding to the robot description (URDF or .xacro).
+* **IP addresses** and **port numbers** corresponding to the EKI setup specified via the above XML files.
+
+We recommend that you copy the configuration files, edit the copies for your needs and use these files to create your own launch file. A template will be provided at a later time. However, at this time, have a look at `test_hardware_interface.launch`, `test_params.yaml`, `controller_joint_names.yaml` and `hardware_controllers.yaml` to achieve a working test-launch.
+
+In order to successfully launch the **kuka_eki_hw_interface** a parameter `robot_description` needs to be present on the ROS parameter server. This parameter can be set manually or by adding this line inside the launch file (replace support package and .xacro to match your application):
+
+```xml
+<param name="robot_description" command="$(find xacro)/xacro.py '$(find kuka_kr6_support)/urdf/kr6r900sixx.xacro'"/>
+```
+
+Make sure that the line is added before the `kuka_eki_hw_interface` itself is loaded.
+
+## 4. Testing
+At this point you are ready to test the EKI interface. Before the test, make sure that:
+
+* You have specified the `eki/robot_address` and `eki/robot_port` of the **kuka_eki_hw_interface** to correspond with the KRL files on the controller.
+* You have a launch-file loading the network parameters, robot description, kuka_hw_interface, hardware controller and controller joint names.
+
+The next steps describe how to launch the test file:
+
+* On the robot, start the servers
+
+1. On the teach pad, enter mode **T1** for testing purposes.
+2. Navigate to `KRC:\R1\Program` and select `kuka_eki_hw_interface.src`.
+3. Press and hold an enabling switch and the run/play-button. The robot will release its breaks and a message like **Programmed path reached (BCO)** will be shown at the teach pad.
+4. Press and hold again. The robot is now waiting for clients to connect.
+
+* On your ROS PC, in a new terminal:
+
+```
+$ roscore
+```
+
+* Open a new terminal and roslaunch the previously configured launch-file:
+
+```
+$ roslaunch kuka_eki_hw_interface test_hardware_interface.launch
+```
+
+The **kuka_eki_hw_interface** is now connected and streaming commands.
+
+6. In a new terminal:
+
+```
+$ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller
+```
+
+Choose **controller manager ns** and **controller** and you should be able to move each robot joint.
+
+* Note that T1-mode limits the robot movement velocity and is intended for testing purposes.
+

kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat

@@ -0,0 +1,4 @@
+&ACCESS RVO
+defdat kuka_eki_hw_interface
+   ext bas(bas_command  :in,real  :in )
+enddat