Add ft fake hardware (#13)

* add ft_fake_hw

* update_examples

* linting

ft_fake_hw/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.16)
+project(ft_fake_hw)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+find_package(ament_cmake REQUIRED)
+
+set(DEPS
+  rclcpp
+  hardware_interface
+  pluginlib
+  realtime_tools
+  geometry_msgs
+)
+
+foreach(dependency IN ITEMS ${DEPS})
+  find_package(${dependency} REQUIRED)
+endforeach()
+
+
+add_library(
+  ${PROJECT_NAME}
+  src/ft_fake_hw.cpp
+)
+ament_target_dependencies(${PROJECT_NAME} ${DEPS})
+target_include_directories(
+  ${PROJECT_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}Targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+pluginlib_export_plugin_description_file(hardware_interface ft_fake_hw_plugin_description.xml)
+ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${DEPS})
+ament_package()

ft_fake_hw/README.md

@@ -0,0 +1,2 @@
+# ft_fake_hw
+Fake ROS2 control hardware interfaces to simulate sensors (e.g., F/T sensor) from topics.

ft_fake_hw/ft_fake_hw_plugin_description.xml

@@ -0,0 +1,9 @@
+<library path="ft_fake_hw">
+
+<class name="ft_fake_hw/FtFakeHw" type="ft_fake_hw::FtFakeHw" base_class_type="hardware_interface::SensorInterface">
+    <description>
+        A fake hardware interface for f/t sensors that reads data from a wrench topic.
+    </description>
+</class>
+
+</library>

ft_fake_hw/include/ft_fake_hw/ft_fake_hw.hpp

@@ -0,0 +1,75 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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: Thibault Poignonec
+
+// Adapted from https://github.com/PickNikRobotics/topic_based_ros2_control
+
+#pragma once
+
+// C++
+#include <memory>
+#include <string>
+#include <vector>
+
+// ROS
+#include "realtime_tools/realtime_buffer.h"
+
+#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 <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/subscription.hpp>
+
+#include <geometry_msgs/msg/wrench.hpp>
+
+namespace ft_fake_hw
+{
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class FtFakeHw : public hardware_interface::SensorInterface
+{
+public:
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  hardware_interface::return_type read(
+    const rclcpp::Time & time,
+    const rclcpp::Duration & period) override;
+
+private:
+  // Node and subsreiber
+  rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr wrench_fake_values_subscriber_;
+  rclcpp::Node::SharedPtr node_;
+
+  // real-time buffer
+  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::Wrench>>
+  input_wrench_fake_values_msg_;
+
+  /// Name of the force sensor interfaces
+  std::vector<std::string> interfaces_;
+  /// Wrench measurement
+  std::vector<double> fake_wrench_values_;
+
+  template<typename HandleType>
+  bool getInterface(
+    const std::string & name, const std::string & interface_name, const size_t vector_index,
+    std::vector<std::vector<double>> & values, std::vector<HandleType> & interfaces);
+};
+
+}  // namespace ft_fake_hw

ft_fake_hw/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+    <name>ft_fake_hw</name>
+    <description>ros2 control hardware interface for topic_based sim</description>
+    <version>0.0.0</version>
+
+    <maintainer email="tpoignonec@unistra.fr">Thibault Poignonec</maintainer>
+    <license>BSD</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>s
+
+    <depend>rclcpp</depend>
+    <depend>hardware_interface</depend>
+    <depend>pluginlib</depend>
+    <depend>realtime_tools</depend>
+
+    <depend>geometry_msgs</depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

ft_fake_hw/src/ft_fake_hw.cpp

@@ -0,0 +1,152 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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: Thibault Poignonec
+
+// Adapted from https://github.com/PickNikRobotics/topic_based_ros2_control
+
+#include <algorithm>
+#include <cmath>
+#include <iterator>
+// #include <limits>
+// #include <set>
+#include <string>
+#include <vector>
+
+#include <rclcpp/executors.hpp>
+
+#include <ft_fake_hw/ft_fake_hw.hpp>
+
+
+namespace ft_fake_hw
+{
+
+CallbackReturn FtFakeHw::on_init(const hardware_interface::HardwareInfo & info)
+{
+  if (hardware_interface::SensorInterface::on_init(info) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
+  }
+
+  // Initialize storage
+  fake_wrench_values_.resize(6, 0.0);
+
+  // Create node  and wrench subscriber
+  rclcpp::NodeOptions options;
+  options.arguments({"--ros-args", "-r", "__node:=ft_fake_hw_" + info_.name});
+  node_ = rclcpp::Node::make_shared("_", options);
+
+  const auto get_hardware_parameter =
+    [this](const std::string & parameter_name, const std::string & default_value) {
+      if (auto it = info_.hardware_parameters.find(parameter_name);
+        it != info_.hardware_parameters.end())
+      {
+        return it->second;
+      }
+      return default_value;
+    };
+
+  // reset wrench buffer
+  input_wrench_fake_values_msg_ =
+    realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::Wrench>>(nullptr);
+  // register subscriber
+  wrench_fake_values_subscriber_ = node_->create_subscription<geometry_msgs::msg::Wrench>(
+    get_hardware_parameter("topic", "/fake_wrench_data"),
+    rclcpp::SensorDataQoS(),
+    [this](const geometry_msgs::msg::Wrench::SharedPtr wrench)
+    {
+      input_wrench_fake_values_msg_.writeFromNonRT(wrench);
+    }
+  );
+
+  // Get interfaces names (empty interface = value not set later!)
+  interfaces_.push_back(get_hardware_parameter("force.x.state_interface", ""));
+  interfaces_.push_back(get_hardware_parameter("force.y.state_interface", ""));
+  interfaces_.push_back(get_hardware_parameter("force.z.state_interface", ""));
+  interfaces_.push_back(get_hardware_parameter("torque.x.state_interface", ""));
+  interfaces_.push_back(get_hardware_parameter("torque.y.state_interface", ""));
+  interfaces_.push_back(get_hardware_parameter("torque.z.state_interface", ""));
+
+  if (std::all_of(
+      interfaces_.begin(), interfaces_.end(),
+      [this](auto const & interface) {return interface.empty();}))
+  {
+    // all empty
+    RCLCPP_FATAL(node_->get_logger(), "You must set at least one interface!");
+    return CallbackReturn::ERROR;
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> FtFakeHw::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  RCLCPP_INFO(node_->get_logger(), "FtFakeHw::export_state_interfaces() called.");
+  RCLCPP_INFO(node_->get_logger(), " info_.name = %s", info_.name.c_str());
+  RCLCPP_INFO(node_->get_logger(), " info_.sensors.size() = %u", info_.sensors.size());
+
+  // Sensors' state interfaces
+  for (auto i = 0u; i < info_.sensors.size(); i++) {
+    const auto & sensor = info_.sensors[i];
+    // TODO(tpoignonec): if sensor.name != params.sensor_name, break
+    RCLCPP_INFO(node_->get_logger(), "Setting up %s:", sensor.name.c_str());
+    RCLCPP_INFO(
+      node_->get_logger(), "sensor.state_interfaces.size() = %u", sensor.state_interfaces.size());
+    for (auto j = 0u; j < sensor.state_interfaces.size(); j++) {
+      auto it = std::find(
+        interfaces_.begin(), interfaces_.end(), sensor.state_interfaces[j].name);
+      if (it != interfaces_.end()) {
+        auto index = std::distance(interfaces_.begin(), it);
+        state_interfaces.emplace_back(
+          hardware_interface::StateInterface(
+            sensor.name,
+            sensor.state_interfaces[j].name,
+            &fake_wrench_values_[index]
+        ));
+      } else {
+        // Not found
+        throw std::runtime_error("Invalid sensor name or (fake) f/t sensor URDF parameters!");
+      }
+    }
+  }
+  return state_interfaces;
+}
+
+
+hardware_interface::return_type FtFakeHw::read(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & /*period*/)
+{
+  if (rclcpp::ok()) {
+    rclcpp::spin_some(node_);
+  }
+
+  auto wrench_values_msg = input_wrench_fake_values_msg_.readFromRT();
+  if (!wrench_values_msg || !(*wrench_values_msg)) {
+    auto clock = node_->get_clock();
+    RCLCPP_WARN_THROTTLE(node_->get_logger(), *clock, 1000, "No wrench data received...");
+  } else {
+    fake_wrench_values_[0] = (*wrench_values_msg)->force.x;
+    fake_wrench_values_[1] = (*wrench_values_msg)->force.y;
+    fake_wrench_values_[2] = (*wrench_values_msg)->force.z;
+    fake_wrench_values_[3] = (*wrench_values_msg)->torque.x;
+    fake_wrench_values_[4] = (*wrench_values_msg)->torque.y;
+    fake_wrench_values_[5] = (*wrench_values_msg)->torque.z;
+  }
+  return hardware_interface::return_type::OK;
+}
+}  // end namespace ft_fake_hw
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(ft_fake_hw::FtFakeHw, hardware_interface::SensorInterface)

ft_tools_examples/config/iiwa_exp.config.xacro

@@ -49,7 +49,10 @@
         use_sim="$(arg use_sim)" use_fake_hardware="$(arg use_fake_hardware)"
     />
 
-  <xacro:ft_sensor_r2c_hardware/>
+  <xacro:ft_sensor_r2c_hardware
+        use_sim="$(arg use_sim)"
+        use_fake_hardware="$(arg use_fake_hardware)"
+  />
 
     <xacro:iiwa_gazebo
         runtime_config_package="$(arg runtime_config_package)"

ft_tools_examples/package.xml

@@ -11,6 +11,7 @@
 
   <depend>ft_tools</depend>
   <depend>ft_msgs</depend>
+  <depend>ft_fake_hw</depend>
 
   <exec_depend>iiwa_bringup</exec_depend>