Introduce fault controller (#63)

* Add fault controller.

* Prepare for switching

* Fix controller switching and starting.

* Keep servoing mode on startup.

* Don t change servoing mode on deactivation.

* Fix mode switching.

* Move all mode switching into constructor.

* Add fault publisher.

* Read servoing mode in the loop.

* Implement required prettyfing changes.

* Add realtime publisher.

* Add additional debug statement on fault reset.

* Formatting fix.

* Pre-commit on kortex2_controllers and kortex2_driver.

Author: livanov93

fault_controller/kortex2_controllers/CMakeLists.txt

@@ -10,16 +10,18 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 find_package(controller_interface REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(example_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
   geometry_msgs
+  example_interfaces
 )
 
 include_directories(include)
 
-add_library(${PROJECT_NAME} SHARED src/twist_controller.cpp)
+add_library(${PROJECT_NAME} SHARED src/twist_controller.cpp src/fault_controller.cpp)
 target_include_directories(${PROJECT_NAME} PRIVATE
   include
 )

fault_controller/kortex2_controllers/controller_plugins.xml

@@ -4,4 +4,9 @@
     6dof twist controller
   </description>
   </class>
+  <class name="kortex2_controllers/FaultController" type="kortex2_controllers::FaultController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      ros2 fault controller for kinova api
+    </description>
+  </class>
 </library>

fault_controller/kortex2_controllers/include/kortex2_controllers/fault_controller.hpp

@@ -0,0 +1,95 @@
+// Copyright 2021, PickNik Inc.
+//
+// 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.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Lovro Ivanov [email protected]
+ * \date    2021-11-25
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef KORTEX2_CONTROLLERS__FAULT_CONTROLLER_HPP_
+#define KORTEX2_CONTROLLERS__FAULT_CONTROLLER_HPP_
+
+#include <limits>
+#include <memory>
+
+#include "controller_interface/controller_interface.hpp"
+#include "example_interfaces/msg/bool.hpp"
+#include "example_interfaces/srv/trigger.hpp"
+#include "kortex2_controllers/visibility_control.h"
+#include "realtime_tools/realtime_publisher.h"
+
+namespace kortex2_controllers
+{
+enum CommandInterfaces
+{
+  RESET_FAULT_CMD = 0u,
+  RESET_FAULT_ASYNC_SUCCESS = 1u,
+};
+enum StateInterfaces
+{
+  IN_FAULT = 0u,
+};
+using CmdType = example_interfaces::srv::Trigger;
+using FbkType = example_interfaces::msg::Bool;
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class FaultController : public controller_interface::ControllerInterface
+{
+public:
+  KORTEX2_CONTROLLERS_PUBLIC
+  FaultController();
+
+  KORTEX2_CONTROLLERS_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  KORTEX2_CONTROLLERS_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  KORTEX2_CONTROLLERS_PUBLIC
+  CallbackReturn on_init() override;
+
+  KORTEX2_CONTROLLERS_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  KORTEX2_CONTROLLERS_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  KORTEX2_CONTROLLERS_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  KORTEX2_CONTROLLERS_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  bool resetFault(const CmdType::Request::SharedPtr req, CmdType::Response::SharedPtr resp);
+
+  using StatePublisher = realtime_tools::RealtimePublisher<FbkType>;
+  rclcpp::Publisher<FbkType>::SharedPtr fault_pub_;
+  std::unique_ptr<StatePublisher> realtime_publisher_;
+  FbkType state_;
+  rclcpp::Service<CmdType>::SharedPtr trigger_command_srv_;
+
+  static constexpr double ISSUE_CMD = 1.0;
+  static constexpr double ASYNC_WAITING = 2.0;
+  static constexpr double NO_CMD = std::numeric_limits<double>::quiet_NaN();
+};
+
+}  // namespace kortex2_controllers
+
+#endif  // KORTEX2_CONTROLLERS__FAULT_CONTROLLER_HPP_

fault_controller/kortex2_controllers/package.xml

@@ -11,6 +11,7 @@
 
   <depend>controller_interface</depend>
   <depend>geometry_msgs</depend>
+  <depend>example_interfaces</depend>
   <depend>rclcpp</depend>
 
   <export>

fault_controller/kortex2_controllers/src/fault_controller.cpp

@@ -0,0 +1,140 @@
+// Copyright 2021, PickNik Inc.
+//
+// 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.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Lovro Ivanov [email protected]
+ * \date    2021-11-25
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "kortex2_controllers/fault_controller.hpp"
+#include <memory>
+#include "hardware_interface/loaned_command_interface.hpp"
+
+namespace kortex2_controllers
+{
+using hardware_interface::LoanedCommandInterface;
+
+FaultController::FaultController()
+: controller_interface::ControllerInterface(), trigger_command_srv_(nullptr)
+{
+}
+
+controller_interface::InterfaceConfiguration FaultController::command_interface_configuration()
+  const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  config.names.emplace_back("reset_fault/command");
+  config.names.emplace_back("reset_fault/async_success");
+
+  return config;
+}
+
+controller_interface::InterfaceConfiguration FaultController::state_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  config.names.emplace_back("reset_fault/internal_fault");
+
+  return config;
+}
+
+CallbackReturn FaultController::on_init() { return CallbackReturn::SUCCESS; }
+
+controller_interface::return_type FaultController::update(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  if (realtime_publisher_ && realtime_publisher_->trylock())
+  {
+    state_.data = static_cast<bool>(state_interfaces_[StateInterfaces::IN_FAULT].get_value());
+    realtime_publisher_->msg_.data = state_.data;
+    realtime_publisher_->unlockAndPublish();
+  }
+
+  return controller_interface::return_type::OK;
+}
+
+CallbackReturn FaultController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn FaultController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  command_interfaces_[CommandInterfaces::RESET_FAULT_CMD].set_value(NO_CMD);
+  command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].set_value(NO_CMD);
+  try
+  {
+    fault_pub_ = node_->create_publisher<FbkType>("~/internal_fault", 1);
+    realtime_publisher_ = std::make_unique<StatePublisher>(fault_pub_);
+  }
+  catch (const std::exception & e)
+  {
+    fprintf(
+      stderr, "Exception thrown during publisher creation at configure stage with message : %s \n",
+      e.what());
+    return CallbackReturn::ERROR;
+  }
+  trigger_command_srv_ = node_->create_service<example_interfaces::srv::Trigger>(
+    "~/reset_fault",
+    std::bind(&FaultController::resetFault, this, std::placeholders::_1, std::placeholders::_2));
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn FaultController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  trigger_command_srv_.reset();
+  command_interfaces_[CommandInterfaces::RESET_FAULT_CMD].set_value(NO_CMD);
+  command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].set_value(NO_CMD);
+
+  return CallbackReturn::SUCCESS;
+}
+
+bool FaultController::resetFault(
+  const CmdType::Request::SharedPtr /*req*/, CmdType::Response::SharedPtr resp)
+{
+  command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  command_interfaces_[CommandInterfaces::RESET_FAULT_CMD].set_value(ISSUE_CMD);
+
+  RCLCPP_INFO(node_->get_logger(), "Trying to reset faults on kinova controller.");
+
+  while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value() ==
+         ASYNC_WAITING)
+  {
+    // Asynchronous wait until the hardware interface has set the io value
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  }
+  resp->success = static_cast<bool>(
+    command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value());
+
+  RCLCPP_INFO(
+    node_->get_logger(), "Resetting fault on kinova controller '%s'!",
+    resp->success ? "succeeded" : "failed");
+
+  return resp->success;
+}
+
+}  // namespace kortex2_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  kortex2_controllers::FaultController, controller_interface::ControllerInterface)