Added new kuka_kss_hw_interface and associated kuka_msgs packages.

kuka_kss_hw_interface/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(kuka_kss_hw_interface)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -g")
+
+find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
+  controller_manager
+  hardware_interface
+  joint_limits_interface
+  realtime_tools
+  roscpp
+  std_msgs
+  industrial_msgs
+  kuka_msgs
+)
+
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(TinyXML REQUIRED)
+
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    kuka_kss_hw_interface
+  CATKIN_DEPENDS
+    controller_manager
+    hardware_interface
+    joint_limits_interface
+    roscpp
+    std_msgs
+    industrial_msgs
+    kuka_msgs
+  DEPENDS
+    TinyXML
+)
+
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${TinyXML_INCLUDE_DIRS}
+)
+
+add_library(kuka_kss_hw_interface
+  src/XmlTemplate.cpp
+  src/XmlTemplateXPath.cpp
+  src/communication_link.cpp
+  src/comm_link_udp_server.cpp
+  src/comm_link_tcp_client.cpp
+  src/kuka_comm_handler.cpp
+  src/kuka_comm_handler_eki.cpp
+  src/kuka_comm_handler_rsi.cpp
+  src/kuka_robot_state.cpp
+  src/kuka_robot_state_manager.cpp
+  src/kuka_kss_hw_interface.cpp
+)
+
+target_link_libraries(kuka_kss_hw_interface
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${TinyXML_LIBRARIES}
+)
+
+add_executable(kuka_kss_hw_interface_node
+  src/kuka_kss_hw_interface_node.cpp
+)
+
+target_link_libraries(kuka_kss_hw_interface_node
+   kuka_kss_hw_interface
+)
+
+install(
+  TARGETS kuka_kss_hw_interface
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+install(
+  TARGETS kuka_kss_hw_interface_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

kuka_kss_hw_interface/config/controller_joint_names.yaml

@@ -0,0 +1,2 @@
+# Defines mapping from joint order on robot to joint names in URDF
+controller_joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"]

kuka_kss_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_kss_hw_interface/include/kuka_kss_hw_interface/XmlTemplate.h

@@ -0,0 +1,74 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Kuka Robotics Corp
+ *  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 Kuka Robotics Corp or Kuka GMBH,
+ *     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: Pat Duda <[email protected]>
+ */
+
+
+#ifndef XMLTEMPLATE_H_
+#define XMLTEMPLATE_H_
+
+#include <string>
+#include <rapidxml/rapidxml.hpp>
+
+using namespace std;
+using namespace rapidxml;
+
+//namespace local {
+
+
+/*
+ * This class has basic capability to load xml and output it as a string.
+ * It is intended as a base class for others that will manipulate data and output it.
+ */
+class XmlTemplate {
+public:
+	XmlTemplate();
+	XmlTemplate(const char* fileName);
+	virtual ~XmlTemplate();
+
+	void setXmlTemplate(string xmlStr);
+	string asXml();
+
+protected:
+	string xmlInputStr;
+	xml_document<> xmlDoc;
+
+	string xmlOutputStr;
+};
+
+//} /* namespace local */
+#endif /* XMLTEMPLATE_H_ */

kuka_kss_hw_interface/include/kuka_kss_hw_interface/XmlTemplateXpath.h

@@ -0,0 +1,78 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018 Kuka Robotics Corp
+ *  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 Kuka Robotics Corp or Kuka GMBH,
+ *     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: Pat Duda <[email protected]>
+ */
+
+
+#ifndef XMLTEMPLATEXPATH_H_
+#define XMLTEMPLATEXPATH_H_
+
+#include <map>
+
+#include <kuka_kss_hw_interface/XmlTemplate.h>
+
+//namespace local {
+
+class XmlTemplateXpath: public XmlTemplate {
+public:
+	XmlTemplateXpath();
+	XmlTemplateXpath(const char* fileName);
+	virtual ~XmlTemplateXpath();
+
+    void setXmlTemplate(string xmlStr);
+	xml_node<>* getNode(string xPathStr);
+	xml_attribute<>* getAttr(string xPathStr);
+	char* get(string xPathStr);
+	char* get(const char* xPathStr);
+
+	void set(string xPathStr, char *value);
+	void set(const char* xPathStr, char *value);
+
+	void set(string xPathStr, char *value, std::size_t size);
+	void set(const char* xPathStr, char *value, std::size_t size);
+
+protected:
+	std::map<string, xml_base<>*>	xPathAccessMap;
+	int 	defaultStrBufSize;
+
+private:
+	xml_base<>* findVal(string xPathStr);
+	string xPathSep;
+};
+
+//} /* namespace local */
+#endif /* XMLTEMPLATEXPATH_H_ */

kuka_kss_hw_interface/include/kuka_kss_hw_interface/comm_link_tcp_client.h

@@ -0,0 +1,129 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021 Kuka Robotics Corp
+ *  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 Kuka Robotics Corp or Kuka GMBH,
+ *     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: Pat Duda <[email protected]>
+ */
+
+#ifndef KUKA_KSS_COMM_LINK_TCP_CLIENT_H
+#define KUKA_KSS_COMM_LINK_TCP_CLIENT_H
+#include <string>
+#include <stdexcept>
+#include <ctime>
+#include <chrono>
+#include <boost/chrono/duration.hpp>
+#include <boost/asio.hpp>
+
+#include <kuka_kss_hw_interface/communication_link.h>
+
+namespace kuka_kss_hw_interface
+{
+/**
+ * @brief Implementation for a UDP Communication Server.
+ * Based on Boost asio for platform portability.
+ */
+class CommLink_TCPClient : public CommunicationLink
+{
+public:
+    CommLink_TCPClient(const ros::NodeHandle& nh, const std::string log_id);
+
+
+    /**
+     * @brief setupParams - Setup with the default parameters.
+     * @return true if the parameters are found.
+     */
+    virtual bool setupParams();
+
+    /**
+     * @brief setupParams - Setup the parameters that the class will look at.
+     * @param param_ip_addr - The ROS param for IPV4 address.
+     * @param param_port - The ROS param for the integer socket port.
+     * @return true if the parameters are found.
+     */
+    bool setupParams(std::string param_ip_addr, std::string param_port, std::string timeout);
+
+
+    /**
+     * @brief Setup the communicaiton link.  Read parameters & prepare for connection.
+     * @return true=success
+     */
+    virtual bool setup();
+
+    /**
+     * @brief Start the communicatin link.  Connect or prepare for communication.
+     * @return
+     */
+    virtual bool start();
+
+    /**
+     * @brief Try to receive data.
+     * @param buffer - the memory area where data will be returned.
+     * @return  - the size of the data retrieved, typically bytes.
+     */
+    virtual size_t receive(std::string& buffer);
+
+    /**
+     * @brief Try to send data in the buffer.  Raises exception if send fails.
+     * @param buffer - the data to send.
+     * @return  - the size of the data sent, typically bytes.
+     */
+    virtual size_t send(std::string& buffer);
+
+    virtual bool is_connected();
+    ros::Duration time_since_last_comm();
+
+private:
+    void updateTimeStamp();
+    std::string param_timeout_;
+    ros::Duration tcp_timeout_;  // [s]; settable by parameter (default = 5)
+    std::chrono::time_point<std::chrono::steady_clock> t_last_comm_;
+
+    boost::asio::io_service ios_;
+    //boost::asio::deadline_timer deadline_;
+    boost::asio::ip::tcp::endpoint tcp_server_endpoint_;
+    boost::asio::ip::tcp::socket local_client_socket_;
+    std::string param_addr_;
+    std::string server_address_;
+    std::string param_port_;
+    std::string server_port_;
+    std::string receive_buffer;
+
+
+};
+
+} //namespace kuka_kss_hw_interface
+
+
+#endif // KUKA_KSS_COMM_LINK_TCP_CLIENT_H