Add ros2_control Hardware Interface bringup (#10)

* Run formatter

* Remove Node interface from RDT driver

* Start hardware interface

* Finish implementing hardware interface

* Add better error handling if sensor isn't found

* Add newlines at end of files

.clang-format

@@ -0,0 +1,20 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+  AfterEnum: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false
+...

.gitignore

@@ -0,0 +1 @@
+**/.vscode/

netft_utils/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(netft_node)
+project(netft_utils)
 
 if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 14)
@@ -9,34 +9,101 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
     add_compile_options(-Wall -Wextra -Wpedantic)
 endif ()
 
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(common_interfaces REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(netft_interfaces REQUIRED)
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+ament_cmake
+rclcpp
+rclcpp_lifecycle
+std_msgs
+common_interfaces
+hardware_interface
+geometry_msgs
+pluginlib
+tf2
+tf2_ros
+netft_interfaces
+)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+find_package(${Dependency} REQUIRED)
+endforeach()
 find_package(Boost REQUIRED COMPONENTS system thread program_options)
 
-
 include_directories(src include /usr/include/ ${Boost_INCLUDE_DIRS})
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 
-add_executable(${PROJECT_NAME} src/netft_node.cpp)
-add_library(netft_rdt_driver src/netft_rdt_driver.cpp)
+# Library for actually talking to the sensor
+add_library(netft_rdt_driver SHARED
+  src/netft_rdt_driver.cpp
+)
+target_compile_features(netft_rdt_driver PUBLIC cxx_std_17)
+target_include_directories(netft_rdt_driver
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(netft_rdt_driver ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(netft_rdt_driver ${Boost_LIBRARIES})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(netft_rdt_driver PRIVATE "NETFT_RDT_DRIVER_BUILDING_LIBRARY")
 
+# Executable to enable a ROS node bringup
+add_executable(netft_node src/netft_node.cpp)
+target_include_directories(netft_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
 target_link_libraries(netft_node netft_rdt_driver)
-target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES})
 
-ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs std_msgs common_interfaces geometry_msgs tf2 tf2_ros Boost)
-ament_target_dependencies(netft_rdt_driver rclcpp std_msgs std_msgs common_interfaces geometry_msgs tf2 tf2_ros Boost)
+# Library for bringup with ros2_controls hardware_interface
+add_library(
+  netft_hardware_interface
+  SHARED
+  src/netft_hardware_interface.cpp
+)
+target_compile_features(netft_hardware_interface PUBLIC cxx_std_17)
+target_include_directories(netft_hardware_interface PUBLIC
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+$<INSTALL_INTERFACE:include/netft_hardware_interface>
+)
+target_link_libraries(netft_hardware_interface PUBLIC netft_rdt_driver)
+ament_target_dependencies(
+  netft_hardware_interface PUBLIC
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(netft_hardware_interface PRIVATE "NETFT_HARDWARE_INTERFACE_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface netft_hardware_interface_plugin.xml)
+
+install(TARGETS netft_hardware_interface
+  EXPORT export_netft_hardware_interface
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
 
 install(
-    TARGETS ${PROJECT_NAME} netft_rdt_driver
+    TARGETS netft_node
     DESTINATION lib/${PROJECT_NAME}
 )
 
+install(TARGETS netft_rdt_driver
+  EXPORT export_netft_rdt_driver
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/ DESTINATION include)
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
+
+ament_export_targets(export_netft_rdt_driver HAS_LIBRARY_TARGET)
+ament_export_targets(export_netft_hardware_interface HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-ament_package()
+ament_package()

netft_utils/README.md

@@ -1 +1,16 @@
 C++ class and ROS node for ATI force/torque sensors connected to a Netbox. Includes gravity compensation and transformations.
+
+# Usage
+There are two main ways to use this package:
+1. With a direct ROS node
+1. Using `ros2_control` hardware interface and a `force_torque_broadcaster`
+
+To launch the direct node, run the following with the correct IP address
+```sh
+ros2 run netft_utils netft_node --address WWW.XXX.YYY.ZZZ
+```
+
+To run an example launch for `ros2_control` run the following with option namespace
+```sh
+ros2 launch netft_utils netft.launch.py node_namespace:="test_ns/"
+```

netft_utils/config/control_params.yaml

@@ -0,0 +1,19 @@
+$(var node_namespace)controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    force_torque_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+
+$(var node_namespace)force_torque_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: netft_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    frame_id: netft_link

netft_utils/include/netft_utils/netft_hardware_interface.hpp

@@ -0,0 +1,68 @@
+// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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.
+
+//
+// Authors: Subhas Das, Denis Stogl
+//
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/sensor_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "netft_utils/netft_rdt_driver.h"
+#include "netft_utils/visibility_control.h"
+#include "rclcpp/macros.hpp"
+
+namespace netft_hardware_interface
+{
+class NetFTHardwareInterface : public hardware_interface::SensorInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(NetFTHardwareInterface)
+
+  NETFT_HARDWARE_INTERFACE_PUBLIC
+  hardware_interface::CallbackReturn on_init(
+    const hardware_interface::HardwareInfo & info) override;
+
+  NETFT_HARDWARE_INTERFACE_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  NETFT_HARDWARE_INTERFACE_PUBLIC
+  hardware_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  NETFT_HARDWARE_INTERFACE_PUBLIC
+  hardware_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  NETFT_HARDWARE_INTERFACE_PUBLIC
+  hardware_interface::return_type read(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  // Store the sensor states
+  std::vector<double> hw_sensor_states_;
+
+  // For talking to the sensor
+  std::string ip_address_;
+  std::unique_ptr<netft_rdt_driver::NetFTRDTDriver> ft_driver_;
+};
+
+}  // namespace netft_hardware_interface

netft_utils/include/netft_rdt_driver.h → netft_utils/include/netft_utils/netft_rdt_driver.h

@@ -36,9 +36,9 @@
 #define NETFT_RDT_DRIVER
 
 #include <boost/asio.hpp>
+#include <boost/thread/condition.hpp>
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/thread.hpp>
-#include <boost/thread/condition.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 // #include "diagnostic_updater/DiagnosticStatusWrapper.h"
@@ -49,24 +49,21 @@
 namespace netft_rdt_driver
 {
 
-class NetFTRDTDriver : public rclcpp::Node
+class NetFTRDTDriver
 {
 public:
   // Start receiving data from NetFT device
-  explicit NetFTRDTDriver(const std::string &address, const std::string &frame_id = "base_link");
+  explicit NetFTRDTDriver(const std::string & address, const std::string & frame_id = "base_link");
 
-  ~NetFTRDTDriver() override;
+  ~NetFTRDTDriver();
 
   //! Get newest RDT data from netFT device
-  void getData(geometry_msgs::msg::WrenchStamped &data);
-
-  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr ready_pub;
-  rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr geo_pub;
+  void getData(geometry_msgs::msg::WrenchStamped & data);
 
   // //! Add device diagnostics status wrapper
   // void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
 
-  //! Wait for new NetFT data to arrive.  
+  //! Wait for new NetFT data to arrive.
   // Returns true if new data has arrived, false it function times out
   bool waitForNewData(void);
 
@@ -80,9 +77,10 @@ class NetFTRDTDriver : public rclcpp::Node
   // the force_scale_ and torque_scale_ appropriately
   bool readCalibrationInformation(const std::string & address);
 
-  enum {
-    RDT_PORT=49152,
-    TCP_PORT=49151
+  enum
+  {
+    RDT_PORT = 49152,
+    TCP_PORT = 49151
   };
   std::string address_;
   std::string frame_id_;
@@ -96,7 +94,7 @@ class NetFTRDTDriver : public rclcpp::Node
   //! True if recv loop is still running
   bool recv_thread_running_;
   //! Set if recv thread exited because of error
-  std::string recv_thread_error_msg_; 
+  std::string recv_thread_error_msg_;
 
   //! Newest data received from netft device
   geometry_msgs::msg::WrenchStamped new_data_;
@@ -118,15 +116,13 @@ class NetFTRDTDriver : public rclcpp::Node
   unsigned diag_packet_count_;
   //! Last time diagnostics was published
   rclcpp::Time last_diag_pub_time_;
-  
+
   //! to keep track of out-of-order or duplicate packet
   uint32_t last_rdt_sequence_;
   //! to keep track of any error codes reported by netft
   uint32_t system_status_;
 };
 
+}  // end namespace netft_rdt_driver
 
-} // end namespace netft_rdt_driver
-
-
-#endif // NETFT_RDT_DRIVER
+#endif  // NETFT_RDT_DRIVER

netft_utils/include/netft_utils/visibility_control.h

@@ -0,0 +1,53 @@
+// Copyright 2021 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.
+
+/* 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.
+ */
+
+#pragma once
+
+// 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 NETFT_HARDWARE_INTERFACE_EXPORT __attribute__((dllexport))
+#define NETFT_HARDWARE_INTERFACE_IMPORT __attribute__((dllimport))
+#else
+#define NETFT_HARDWARE_INTERFACE_EXPORT __declspec(dllexport)
+#define NETFT_HARDWARE_INTERFACE_IMPORT __declspec(dllimport)
+#endif
+#ifdef NETFT_HARDWARE_INTERFACE_BUILDING_DLL
+#define NETFT_HARDWARE_INTERFACE_PUBLIC NETFT_HARDWARE_INTERFACE_EXPORT
+#else
+#define NETFT_HARDWARE_INTERFACE_PUBLIC NETFT_HARDWARE_INTERFACE_IMPORT
+#endif
+#define NETFT_HARDWARE_INTERFACE_PUBLIC_TYPE NETFT_HARDWARE_INTERFACE_PUBLIC
+#define NETFT_HARDWARE_INTERFACE_LOCAL
+#else
+#define NETFT_HARDWARE_INTERFACE_EXPORT __attribute__((visibility("default")))
+#define NETFT_HARDWARE_INTERFACE_IMPORT
+#if __GNUC__ >= 4
+#define NETFT_HARDWARE_INTERFACE_PUBLIC __attribute__((visibility("default")))
+#define NETFT_HARDWARE_INTERFACE_LOCAL __attribute__((visibility("hidden")))
+#else
+#define NETFT_HARDWARE_INTERFACE_PUBLIC
+#define NETFT_HARDWARE_INTERFACE_LOCAL
+#endif
+#define NETFT_HARDWARE_INTERFACE_PUBLIC_TYPE
+#endif