Update force torque state controller.

livanov93 · livanov93 · commit dd9b4616ebaa · 2021-02-07T17:47:42.000Z
diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt
@@ -3,11 +3,11 @@ project(ur_controllers)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-    set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 14)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra)
+  add_compile_options(-Wall -Wextra)
 endif()
 
 find_package(ament_cmake REQUIRED)
@@ -20,13 +20,13 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcutils REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
-        controller_interface
-        joint_trajectory_controller
-        pluginlib
-        std_msgs
-        geometry_msgs
-        rclcpp_lifecycle
-        rcutils
+  controller_interface
+  joint_trajectory_controller
+  pluginlib
+  std_msgs
+  geometry_msgs
+  rclcpp_lifecycle
+  rcutils
 )
 
 include_directories(include)
@@ -35,9 +35,11 @@ add_library(${PROJECT_NAME} SHARED
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_controller.cpp
   src/force_torque_sensor_controller.cpp
-        include/ur_controllers/force_torque_sensor_controller.h src/force_torque_sensor_controller.cpp)
+  src/force_torque_sensor_controller.cpp)
 
-target_include_directories(${PROJECT_NAME} PRIVATE include)
+target_include_directories(${PROJECT_NAME} PRIVATE
+  include
+)
 
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
@@ -64,11 +66,11 @@ install(FILES controller_plugins.xml
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 ament_export_include_directories(
-        include
+  include
 )
 
 ament_export_libraries(
-        ${PROJECT_NAME}
+  ${PROJECT_NAME}
 )
 
 
diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml
@@ -5,20 +5,3 @@
     </description>
   </class>
 </library>
-<!--<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>-->
-<!--</class>-->
-<!--</library>-->
diff --git a/ur_controllers/include/ur_controllers/force_torque_sensor_controller.h b/ur_controllers/include/ur_controllers/force_torque_sensor_controller.h
@@ -1,6 +1,16 @@
+// Copyright (c) 2021 PickNik LLC
 //
-// Created by livanov93 on 12/12/20.
+// 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
@@ -16,6 +26,38 @@
 
 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:
@@ -36,12 +78,9 @@ class ForceTorqueStateController : public controller_interface::ControllerInterf
   controller_interface::return_type init(const std::string& controller_name) override;
 
 protected:
-  bool init_sensor_data();
-  void init_sensor_state_msg();
-
-  //  we store the name of values per axis - compatible interfaces
-  std::vector<std::string> axis_val_names_;
-  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>> wrench_state_publisher_;
+  // param storage
+  FTStateControllerParams fts_params_;
+  std::shared_ptr<rclcpp::Publisher<FbkType>> wrench_state_publisher_;
   geometry_msgs::msg::WrenchStamped wrench_state_msg_;
 };
 }  // namespace ur_controllers
diff --git a/ur_controllers/src/force_torque_sensor_controller.cpp b/ur_controllers/src/force_torque_sensor_controller.cpp
@@ -6,7 +6,8 @@
 
 namespace ur_controllers
 {
-ForceTorqueStateController::ForceTorqueStateController() : controller_interface::ControllerInterface()
+ForceTorqueStateController::ForceTorqueStateController()
+  : controller_interface::ControllerInterface(), wrench_state_publisher_(nullptr)
 {
 }
 
@@ -18,6 +19,20 @@ controller_interface::return_type ForceTorqueStateController::init(const std::st
     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;
 }
 
@@ -32,13 +47,11 @@ controller_interface::InterfaceConfiguration ForceTorqueStateController::state_i
 {
   controller_interface::InterfaceConfiguration config;
   config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
-  // TODO solve this hardcoded naming
-  config.names.emplace_back("tcp_fts_sensor/fx");
-  config.names.emplace_back("tcp_fts_sensor/fy");
-  config.names.emplace_back("tcp_fts_sensor/fz");
-  config.names.emplace_back("tcp_fts_sensor/tx");
-  config.names.emplace_back("tcp_fts_sensor/ty");
-  config.names.emplace_back("tcp_fts_sensor/tz");
+  for (const auto& si : fts_params_.state_interfaces_names_)
+  {
+    config.names.push_back(fts_params_.sensor_name_ + "/" + si);
+  }
+
   return config;
 }
 
@@ -47,49 +60,38 @@ controller_interface::return_type ur_controllers::ForceTorqueStateController::up
   geometry_msgs::msg::Vector3 fVec;
   geometry_msgs::msg::Vector3 tVec;
 
-  // TODO remove these hardcoded names and create better string filtering within state_interfaces_
-  for (const auto& state_interface : state_interfaces_)
-  {
-    if (state_interface.get_interface_name() == "fx")
-    {
-      fVec.set__x(state_interface.get_value());
-      continue;
-    }
-
-    if (state_interface.get_interface_name() == "fy")
-    {
-      fVec.set__y(state_interface.get_value());
-      continue;
-    }
+  if (state_interfaces_.size() != fts_params_.state_interfaces_names_.size())
+    return controller_interface::return_type::ERROR;
 
-    if (state_interface.get_interface_name() == "fz")
-    {
-      fVec.set__z(state_interface.get_value());
-      continue;
-    }
-
-    if (state_interface.get_interface_name() == "tx")
-    {
-      tVec.set__x(state_interface.get_value());
-      continue;
-    }
-
-    if (state_interface.get_interface_name() == "ty")
-    {
-      tVec.set__y(state_interface.get_value());
-      continue;
-    }
-
-    if (state_interface.get_interface_name() == "tz")
+  for (auto index = 0ul; index < state_interfaces_.size(); ++index)
+  {
+    switch (index)
     {
-      tVec.set__z(state_interface.get_value());
-      continue;
+      case 0:
+        fVec.set__x(state_interfaces_[index].get_value());
+        break;
+      case 1:
+        fVec.set__y(state_interfaces_[index].get_value());
+        break;
+      case 2:
+        fVec.set__z(state_interfaces_[index].get_value());
+        break;
+      case 3:
+        tVec.set__x(state_interfaces_[index].get_value());
+        break;
+      case 4:
+        tVec.set__y(state_interfaces_[index].get_value());
+        break;
+      case 5:
+        tVec.set__z(state_interfaces_[index].get_value());
+        break;
+      default:
+        break;
     }
   }
 
-  // TODO set frame_id as parameter --> it includes tf listener within controller
   wrench_state_msg_.header.stamp = get_node()->get_clock()->now();
-  wrench_state_msg_.header.frame_id = "tool0";
+  wrench_state_msg_.header.frame_id = fts_params_.frame_id;
 
   // update wrench state message
   wrench_state_msg_.wrench.set__force(fVec);
@@ -104,29 +106,52 @@ controller_interface::return_type ur_controllers::ForceTorqueStateController::up
 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
   {
-    // TODO make topic name a parameter
-    wrench_state_publisher_ =
-        get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>("ft_data", rclcpp::SystemDefaultsQoS());
+    // 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*/)
 {
-  if (!init_sensor_data())
-  {
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
-  }
-
-  init_sensor_state_msg();
-
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
@@ -136,43 +161,6 @@ ForceTorqueStateController::on_deactivate(const rclcpp_lifecycle::State& /*previ
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
-template <typename T>
-bool has_any_key(const std::unordered_map<std::string, T>& map, const std::vector<std::string>& keys)
-{
-  bool found_key = false;
-  for (const auto& key_item : map)
-  {
-    const auto& key = key_item.first;
-    if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend())
-    {
-      found_key = true;
-      break;
-    }
-  }
-  return found_key;
-}
-
-bool ForceTorqueStateController::init_sensor_data()
-{
-  bool has_fts_sensor = false;
-  // loop in reverse order, this maintains the order of values at retrieval time
-  for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++)
-  {
-    // TODO remove hardcoded naming
-    if (si->get_name() == "tcp_fts_sensor")
-    {
-      has_fts_sensor = true;
-      axis_val_names_.push_back(si->get_interface_name());
-    }
-  }
-  return has_fts_sensor;
-}
-
-void ForceTorqueStateController::init_sensor_state_msg()
-{
-  // default initialization for wrenchstamped state message
-}
-
 }  // namespace ur_controllers
 
 #include "pluginlib/class_list_macros.hpp"
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
@@ -133,7 +133,7 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
     for (uint j = 0; j < info_.sensors[i].state_interfaces.size(); ++j)
     {
       state_interfaces.emplace_back(hardware_interface::StateInterface(
-          info_.sensors[i].name, info_.sensors[i].state_interfaces[j].name, &urcl_ft_sensor_measurements_[i]));
+          info_.sensors[i].name, info_.sensors[i].state_interfaces[j].name, &urcl_ft_sensor_measurements_[j]));
     }
   }
 
diff --git a/ur_ros2_control_demos/config/ur_ros2_control.yaml b/ur_ros2_control_demos/config/ur_ros2_control.yaml
diff --git a/ur_ros2_control_demos/urdf/ur.ros2_control.xacro b/ur_ros2_control_demos/urdf/ur.ros2_control.xacro

Original file line number	Diff line number	Diff line change
`@@ -133,7 +133,7 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp`
`133`	`133`	`for (uint j = 0; j < info_.sensors[i].state_interfaces.size(); ++j)`
`134`	`134`	`{`
`135`	`135`	`state_interfaces.emplace_back(hardware_interface::StateInterface(`
`136`		`- info_.sensors[i].name, info_.sensors[i].state_interfaces[j].name, &urcl_ft_sensor_measurements_[i]));`
	`136`	`+ info_.sensors[i].name, info_.sensors[i].state_interfaces[j].name, &urcl_ft_sensor_measurements_[j]));`
`137`	`137`	`}`
`138`	`138`	`}`
`139`	`139`