Merge pull request #12 from PickNikRobotics/livanov/add_force_torque_…

…controller

Integrated force torque sensor controller

ur_controllers/CMakeLists.txt

@@ -1,35 +1,54 @@
 cmake_minimum_required(VERSION 3.5)
 project(ur_controllers)
 
-add_compile_options(-std=c++11)
+# 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)
+endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(controller_interface REQUIRED)
-find_package(hardware_interface REQUIRED)
 find_package(joint_trajectory_controller REQUIRED)
 find_package(pluginlib REQUIRED)
-find_package(realtime_tools REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rcutils REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
-  hardware_interface
   joint_trajectory_controller
   pluginlib
-  realtime_tools
   std_msgs
+  geometry_msgs
+  rclcpp_lifecycle
+  rcutils
 )
 
 include_directories(include)
 
 add_library(${PROJECT_NAME} SHARED
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_controller.cpp
+  src/force_torque_sensor_controller.cpp
+  src/force_torque_sensor_controller.cpp)
+
+target_include_directories(${PROJECT_NAME} PRIVATE
+  include
 )
+
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )
 
+# prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
+
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -46,4 +65,13 @@ install(FILES controller_plugins.xml
 
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+
 ament_package()

ur_controllers/controller_plugins.xml

@@ -1,17 +1,7 @@
-<library path="lib/libur_controllers">
-  <class name="ur_controllers/SpeedScalingStateController" type="ur_controllers::SpeedScalingStateController" base_class_type="controller_interface::ControllerBase">
-  <description>
-    This controller publishes the readings of all available speed scaling factors.
-  </description>
-  </class>
-  <class name="position_controllers/ScaledJointTrajectoryController" type="position_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
-  <description>
-    Scaled velocity-based joint trajectory controller
-  </description>
-  </class>
-  <class name="velocity_controllers/ScaledJointTrajectoryController" type="velocity_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
-  <description>
-    Scaled velocity-based joint trajectory controller
-  </description>
+<library path="ur_controllers">
+  <class name="ur_controllers/ForceTorqueStateController" type="ur_controllers::ForceTorqueStateController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      The force torque state controller publishes the current force and torques
+    </description>
   </class>
 </library>

ur_controllers/include/ur_controllers/force_torque_sensor_controller.h

@@ -0,0 +1,88 @@
+// Copyright (c) 2021 PickNik LLC
+//
+// 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 UR_CONTROLLERS_FORCE_TORQUE_SENSOR_CONTROLLER_H
+#define UR_CONTROLLERS_FORCE_TORQUE_SENSOR_CONTROLLER_H
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include <controller_interface/controller_interface.hpp>
+
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+
+namespace ur_controllers
+{
+using FbkType = geometry_msgs::msg::WrenchStamped;
+
+struct FTStateControllerParams
+{
+  FTStateControllerParams()
+  {
+    sensor_name_ = "";
+    state_interfaces_names_ = {};
+    frame_id = "";
+    topic_name = "";
+  }
+
+  std::string sensor_name_;
+  std::vector<std::string> state_interfaces_names_;
+  std::string frame_id;
+  std::string topic_name;
+};
+
+/**
+ * \brief Force torque state controller.
+ *
+ * This class forwards the command signal down to a set of joints
+ * on the specified interface.
+ *
+ * \param topic_name Name of the topic to publish WrenchStamped msg
+ * \param frame_id Frame of the ft sensor
+ * \param sensor_name Names of the sensor which data should be published .
+ * \param state_interface_names Names of the state interfaces in the sensor.
+ *
+ * Publishes to:
+ * - \b topic_name (geometry_msg::msg::WrenchStamped) : Sensor data.
+ */
+class ForceTorqueStateController : public controller_interface::ControllerInterface
+{
+public:
+  ForceTorqueStateController();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::return_type update() override;
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::return_type init(const std::string& controller_name) override;
+
+protected:
+  // param storage
+  FTStateControllerParams fts_params_;
+  std::shared_ptr<rclcpp::Publisher<FbkType>> wrench_state_publisher_;
+  geometry_msgs::msg::WrenchStamped wrench_state_msg_;
+};
+}  // namespace ur_controllers
+
+#endif  // UR_CONTROLLERS_FORCE_TORQUE_SENSOR_CONTROLLER_H

ur_controllers/package.xml

@@ -11,12 +11,20 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <build_depend>pluginlib</build_depend>
+  <build_depend>rcutils</build_depend>
+
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>rcutils</exec_depend>
+
   <depend>controller_interface</depend>
-  <depend>hardware_interface</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>geometry_msgs</depend>
   <depend>joint_trajectory_controller</depend>
-  <depend>pluginlib</depend>
-  <depend>realtime_tools</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>ur_client_library</depend>
+  <depend>ur_dashboard_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

ur_controllers/src/force_torque_sensor_controller.cpp

@@ -0,0 +1,178 @@
+// Copyright (c) 2021 PickNik LLC
+//
+// 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.
+
+#include "ur_controllers/force_torque_sensor_controller.h"
+
+namespace ur_controllers
+{
+ForceTorqueStateController::ForceTorqueStateController()
+  : controller_interface::ControllerInterface(), wrench_state_publisher_(nullptr)
+{
+}
+
+controller_interface::return_type ForceTorqueStateController::init(const std::string& controller_name)
+{
+  auto ret = ControllerInterface::init(controller_name);
+  if (ret != controller_interface::return_type::SUCCESS)
+  {
+    return ret;
+  }
+
+  try
+  {
+    auto node = get_node();
+    node->declare_parameter<std::vector<std::string>>("state_interface_names", {});
+    node->declare_parameter<std::string>("sensor_name", "");
+    node->declare_parameter<std::string>("topic_name", "");
+    node->declare_parameter<std::string>("frame_id", "");
+  }
+  catch (const std::exception& e)
+  {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return controller_interface::return_type::ERROR;
+  }
+
+  return controller_interface::return_type::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration ForceTorqueStateController::command_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::NONE;
+  return config;
+}
+
+controller_interface::InterfaceConfiguration ForceTorqueStateController::state_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto& si : fts_params_.state_interfaces_names_)
+  {
+    config.names.push_back(fts_params_.sensor_name_ + "/" + si);
+  }
+
+  return config;
+}
+
+controller_interface::return_type ur_controllers::ForceTorqueStateController::update()
+{
+  geometry_msgs::msg::Vector3 f_vec;
+  geometry_msgs::msg::Vector3 t_vec;
+
+  if (state_interfaces_.size() != fts_params_.state_interfaces_names_.size())
+    return controller_interface::return_type::ERROR;
+
+  for (auto index = 0ul; index < state_interfaces_.size(); ++index)
+  {
+    switch (index)
+    {
+      case 0:
+        f_vec.set__x(state_interfaces_[index].get_value());
+        break;
+      case 1:
+        f_vec.set__y(state_interfaces_[index].get_value());
+        break;
+      case 2:
+        f_vec.set__z(state_interfaces_[index].get_value());
+        break;
+      case 3:
+        t_vec.set__x(state_interfaces_[index].get_value());
+        break;
+      case 4:
+        t_vec.set__y(state_interfaces_[index].get_value());
+        break;
+      case 5:
+        t_vec.set__z(state_interfaces_[index].get_value());
+        break;
+      default:
+        break;
+    }
+  }
+
+  wrench_state_msg_.header.stamp = get_node()->get_clock()->now();
+  wrench_state_msg_.header.frame_id = fts_params_.frame_id;
+
+  // update wrench state message
+  wrench_state_msg_.wrench.set__force(f_vec);
+  wrench_state_msg_.wrench.set__torque(t_vec);
+
+  // publish
+  wrench_state_publisher_->publish(wrench_state_msg_);
+
+  return controller_interface::return_type::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+ForceTorqueStateController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  fts_params_.state_interfaces_names_ = node_->get_parameter("state_interface_names").as_string_array();
+
+  if (fts_params_.state_interfaces_names_.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'state_interface_names' parameter was empty");
+    return CallbackReturn::ERROR;
+  }
+
+  fts_params_.sensor_name_ = node_->get_parameter("sensor_name").as_string();
+  if (fts_params_.sensor_name_.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter was empty");
+    return CallbackReturn::ERROR;
+  }
+
+  fts_params_.topic_name = node_->get_parameter("topic_name").as_string();
+  if (fts_params_.topic_name.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'topic_name' parameter was empty");
+    return CallbackReturn::ERROR;
+  }
+
+  fts_params_.frame_id = node_->get_parameter("frame_id").as_string();
+  if (fts_params_.frame_id.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter was empty");
+    return CallbackReturn::ERROR;
+  }
+
+  try
+  {
+    // register ft sensor data publisher
+    wrench_state_publisher_ = get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
+        fts_params_.topic_name, rclcpp::SystemDefaultsQoS());
+  }
+  catch (...)
+  {
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+ForceTorqueStateController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+ForceTorqueStateController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+}  // namespace ur_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(ur_controllers::ForceTorqueStateController, controller_interface::ControllerInterface)