Adding eki interfaces

kuka_eki_hw_interface/CMakeLists.txt

@@ -1,64 +1,89 @@
-cmake_minimum_required(VERSION 3.1.0)
+cmake_minimum_required(VERSION 3.5)
 project(kuka_eki_hw_interface)
 
-set(CMAKE_CXX_STANDARD 11)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  angles
-  cmake_modules
-  controller_manager
-  hardware_interface
-  joint_limits_interface
-  roscpp
-)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
+# find dependencies
+find_package(ament_cmake REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
+find_package(tinyxml_vendor REQUIRED)
 find_package(TinyXML REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
 
-catkin_package(
-  CATKIN_DEPENDS
-    controller_manager
-    hardware_interface
-    joint_limits_interface
-    roscpp
-  DEPENDS
-    TinyXML
-)
-
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
 
-include_directories(
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/kuka_eki_hw_interface.cpp
+)
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
   include
-  ${catkin_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
-  ${TinyXML_INCLUDE_DIRS}
 )
-
-add_library(kuka_eki_hw_interface
-  src/kuka_eki_hw_interface.cpp
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  TinyXML
 )
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_kuka_eki.xml)
 
-target_link_libraries(kuka_eki_hw_interface
-  ${catkin_LIBRARIES}
-  ${Boost_LIBRARIES}
-  ${TinyXML_LIBRARIES}
+ament_export_include_directories(
+  include
 )
-
-add_executable(kuka_eki_hw_interface_node
-  src/kuka_eki_hw_interface_node.cpp
+ament_export_libraries(
+  ${PROJECT_NAME}
 )
-
-target_link_libraries(kuka_eki_hw_interface_node
-  kuka_eki_hw_interface
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
 )
 
+### ros2 control hardware plugin
 install(
-  TARGETS kuka_eki_hw_interface
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
-
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 install(
-  TARGETS kuka_eki_hw_interface_node
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+ament_package()

kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h

@@ -1,106 +1,91 @@
-/*
- * 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) <brhemes@mmm.com>
-
-
-#ifndef KUKA_EKI_HW_INTERFACE
-#define KUKA_EKI_HW_INTERFACE
-
-#include <vector>
+// Copyright 2020 ros2_control Development Team
+//
+// 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.
+
+#ifndef ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_
+#define ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_
+
+#include <memory>
 #include <string>
-
+#include <vector>
 #include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/array.hpp>
+#include <angles/angles.h>
 
-#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>
+#include "tinyxml.h"
+#include "hardware_interface/base_interface.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_status_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "kuka_eki_hw_interface/visibility_control.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_;
-  int eki_cmd_buff_len_;
-  int eki_max_cmd_buff_len_ = 5;  // by default, limit command buffer to 5 (size of advance run in KRL)
-
-  // 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, int &cmd_buff_len);
-  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
+    class KukaEkiHardwareInterface : public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
+    {
+        public:
+            RCLCPP_SHARED_PTR_DEFINITIONS(KukaEkiHardwareInterface)
+
+            ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+            hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override;
+
+            ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+            std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+            ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+            std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+            ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+            hardware_interface::return_type start() override;
+
+            ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+            hardware_interface::return_type stop() override;
+
+            ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+            hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+            ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+            hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+        private:
+            // Store the command for the simulated robot
+            std::vector<double> hw_commands_;
+            std::vector<double> hw_states_;
+
+            std::string eki_server_address_;
+            std::string eki_server_port_;
+            int eki_cmd_buff_len_;
+            int eki_max_cmd_buff_len_ = 5;  // by default, limit command buffer to 5 (size of advance run in KRL)
+//
+//            // EKI socket read/write
+            int eki_read_state_timeout_ = 5;  // [s]; settable by parameter (default = 5)
+            boost::asio::io_service ios_;
+            std::unique_ptr<boost::asio::deadline_timer> deadline_;
+            boost::asio::ip::udp::endpoint eki_server_endpoint_;
+            std::unique_ptr<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, int &cmd_buff_len);
+            bool eki_write_command(const std::vector<double> &joint_position);
+    };
+
+}  // namespace kuka_eki_hw_interface
+
+#endif  // ROS2_CONTROL_KUKA_EKI_HW__KUKA_EKI_POSITION_ONLY_HPP_

kuka_eki_hw_interface/include/kuka_eki_hw_interface/visibility_control.h

@@ -0,0 +1,56 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef ROS2_CONTROL_KUKA_EKI_HW__VISIBILITY_CONTROL_H_
+#define ROS2_CONTROL_KUKA_EKI_HW__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define ROS2_CONTROL_KUKA_EKI_HW_EXPORT __attribute__((dllexport))
+#define ROS2_CONTROL_KUKA_EKI_HW_IMPORT __attribute__((dllimport))
+#else
+#define ROS2_CONTROL_KUKA_EKI_HW_EXPORT __declspec(dllexport)
+#define ROS2_CONTROL_KUKA_EKI_HW_IMPORT __declspec(dllimport)
+#endif
+#ifdef ROS2_CONTROL_KUKA_EKI_HW_BUILDING_DLL
+#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC ROS2_CONTROL_KUKA_EKI_HW_EXPORT
+#else
+#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC ROS2_CONTROL_KUKA_EKI_HW_IMPORT
+#endif
+#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC_TYPE ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+#define ROS2_CONTROL_KUKA_EKI_HW_LOCAL
+#else
+#define ROS2_CONTROL_KUKA_EKI_HW_EXPORT __attribute__((visibility("default")))
+#define ROS2_CONTROL_KUKA_EKI_HW_IMPORT
+#if __GNUC__ >= 4
+#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC __attribute__((visibility("default")))
+#define ROS2_CONTROL_KUKA_EKI_HW_LOCAL __attribute__((visibility("hidden")))
+#else
+#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC
+#define ROS2_CONTROL_KUKA_EKI_HW_LOCAL
+#endif
+#define ROS2_CONTROL_KUKA_EKI_HW_PUBLIC_TYPE
+#endif
+
+#endif  // ROS2_CONTROL_KUKA_EKI_HW__VISIBILITY_CONTROL_H_