force_torque_sensor.hpp

// 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.
 
#ifndef SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
 
#include <limits>
#include <string>
#include <vector>
 
#include "geometry_msgs/msg/wrench.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "semantic_components/semantic_component_interface.hpp"
 
namespace semantic_components
{
class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::Wrench>
{
public:
  explicit ForceTorqueSensor(const std::string & name) : SemanticComponentInterface(name, 6)
  {
    // If 6D FTS use standard names
    interface_names_.emplace_back(name_ + "/" + "force.x");
    interface_names_.emplace_back(name_ + "/" + "force.y");
    interface_names_.emplace_back(name_ + "/" + "force.z");
    interface_names_.emplace_back(name_ + "/" + "torque.x");
    interface_names_.emplace_back(name_ + "/" + "torque.y");
    interface_names_.emplace_back(name_ + "/" + "torque.z");
 
    // Set all interfaces existing
    std::fill(existing_axes_.begin(), existing_axes_.end(), true);
 
    // Set default force and torque values to NaN
    std::fill(forces_.begin(), forces_.end(), std::numeric_limits<double>::quiet_NaN());
    std::fill(torques_.begin(), torques_.end(), std::numeric_limits<double>::quiet_NaN());
  }
 
 
  ForceTorqueSensor(
    const std::string & interface_force_x, const std::string & interface_force_y,
    const std::string & interface_force_z, const std::string & interface_torque_x,
    const std::string & interface_torque_y, const std::string & interface_torque_z)
  : SemanticComponentInterface("", 6)
  {
    auto check_and_add_interface = [this](const std::string & interface_name, const int index)
    {
      if (!interface_name.empty())
      {
        interface_names_.emplace_back(interface_name);
        existing_axes_[index] = true;
      }
      else
      {
        existing_axes_[index] = false;
      }
    };
 
    check_and_add_interface(interface_force_x, 0);
    check_and_add_interface(interface_force_y, 1);
    check_and_add_interface(interface_force_z, 2);
    check_and_add_interface(interface_torque_x, 3);
    check_and_add_interface(interface_torque_y, 4);
    check_and_add_interface(interface_torque_z, 5);
 
    // Set default force and torque values to NaN
    std::fill(forces_.begin(), forces_.end(), std::numeric_limits<double>::quiet_NaN());
    std::fill(torques_.begin(), torques_.end(), std::numeric_limits<double>::quiet_NaN());
  }
 
  virtual ~ForceTorqueSensor() = default;
 
 
  std::array<double, 3> get_forces()
  {
    size_t interface_counter = 0;
    for (size_t i = 0; i < 3; ++i)
    {
      if (existing_axes_[i])
      {
        forces_[i] = state_interfaces_[interface_counter].get().get_value();
        ++interface_counter;
      }
    }
    return forces_;
  }
 
 
  std::array<double, 3> get_torques()
  {
    // find out how many force interfaces are being used
    // torque interfaces will be found from the next index onward
    auto torque_interface_counter =
      std::count(existing_axes_.begin(), existing_axes_.begin() + 3, true);
 
    for (size_t i = 3; i < 6; ++i)
    {
      if (existing_axes_[i])
      {
        torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value();
        ++torque_interface_counter;
      }
    }
    return torques_;
  }
 
 
  bool get_values_as_message(geometry_msgs::msg::Wrench & message)
  {
    // call get_forces() and get_troque() to update with the latest values
    get_forces();
    get_torques();
 
    // update the message values
    message.force.x = forces_[0];
    message.force.y = forces_[1];
    message.force.z = forces_[2];
    message.torque.x = torques_[0];
    message.torque.y = torques_[1];
    message.torque.z = torques_[2];
 
    return true;
  }
 
protected:
  // Order is: force X, force Y, force Z, torque X, torque Y, torque Z.
  std::array<bool, 6> existing_axes_;
  std::array<double, 3> forces_;
  std::array<double, 3> torques_;
};
 
}  // namespace semantic_components
 
#endif  // SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_