Freedrive Controller (backport #1114)

ur_controllers/CMakeLists.txt

@@ -71,6 +71,11 @@ generate_parameter_library(
   src/scaled_joint_trajectory_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  freedrive_mode_controller_parameters
+  src/freedrive_mode_controller_parameters.yaml
+)
+
 generate_parameter_library(
   passthrough_trajectory_controller_parameters
   src/passthrough_trajectory_controller_parameters.yaml
@@ -85,6 +90,7 @@ add_library(${PROJECT_NAME} SHARED
   src/force_mode_controller.cpp
   src/scaled_joint_trajectory_controller.cpp
   src/speed_scaling_state_broadcaster.cpp
+  src/freedrive_mode_controller.cpp
   src/gpio_controller.cpp
   src/passthrough_trajectory_controller.cpp
   src/ur_configuration_controller.cpp)
@@ -97,6 +103,7 @@ target_link_libraries(${PROJECT_NAME}
   gpio_controller_parameters
   speed_scaling_state_broadcaster_parameters
   scaled_joint_trajectory_controller_parameters
+  freedrive_mode_controller_parameters
   passthrough_trajectory_controller_parameters
   ur_configuration_controller_parameters
 )
@@ -153,6 +160,16 @@ if(BUILD_TESTING)
     controller_manager
     ros2_control_test_assets
   )
+  ament_add_gmock(test_load_freedrive_mode_controller
+    test/test_load_freedrive_mode_controller.cpp
+  )
+  target_link_libraries(test_load_freedrive_mode_controller
+    ${PROJECT_NAME}
+  )
+  ament_target_dependencies(test_load_freedrive_mode_controller
+    controller_manager
+    ros2_control_test_assets
+  )
 endif()
 
 ament_package()

ur_controllers/controller_plugins.xml

@@ -19,6 +19,11 @@
       Controller to use UR's force_mode.
     </description>
   </class>
+  <class name="ur_controllers/FreedriveModeController" type="ur_controllers::FreedriveModeController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller handles the activation of the freedrive mode.
+    </description>
+  </class>
   <class name="ur_controllers/PassthroughTrajectoryController" type="ur_controllers::PassthroughTrajectoryController" base_class_type="controller_interface::ControllerInterface">
     <description>
       This controller forwards a joint-based trajectory to the robot controller for interpolation.

ur_controllers/doc/index.rst

@@ -340,3 +340,41 @@ damping_factor
 gain_scaling
    Force mode gain scaling factor. Scales the gain in force mode. scaling parameter is in range [0;2], default is 0.5.
    A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces.
+
+.. _freedrive_mode_controller:
+
+ur_controllers/FreedriveModeController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This controller activates the robot's *Freedrive Mode*, allowing to manually move the robot' joints.
+This controller can't be combined with any other motion controller.
+
+Parameters
+""""""""""
+
++----------------------+--------+---------------+---------------------------------------------------------------------------------------+
+| Parameter name       | Type   | Default value | Description                                                                           |
+|                      |        |               |                                                                                       |
++----------------------+--------+---------------+---------------------------------------------------------------------------------------+
+| ``tf_prefix``        | string | <empty>       | Urdf prefix of the corresponding arm                                                  |
++----------------------+--------+---------------+---------------------------------------------------------------------------------------+
+| ``inactive_timeout`` | int    | 1             | Time interval (in seconds) of message inactivity after which freedrive is deactivated |
++----------------------+--------+---------------+---------------------------------------------------------------------------------------+
+
+Usage
+"""""
+
+The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msgs/msg/Bool]`` for handling activation and deactivation:
+
+* to start and keep freedrive active, you'll have to frequently publish a ``True`` msg on the indicated topic.
+  If no further messages are received by the controller within the ``inactive_timeout`` seconds,
+  freedrive mode will be deactivated. Hence, it is recommended to publish a ``True`` message at least every
+  ``inactive_timeout/2`` seconds.
+
+  .. code-block::
+
+     ros2 topic pub --rate 2 /freedrive_mode_controller/enable_freedrive_mode std_msgs/msg/Bool "{data: true}"
+
+* to deactivate freedrive mode is enough to publish a ``False`` msg on the indicated topic or
+  to deactivate the controller or to stop publishing ``True`` on the enable topic and wait for the
+  controller timeout.

ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp

@@ -0,0 +1,130 @@
+// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Vincenzo Di Pentima dipentima@fzi.de
+ * \date    2024-09-26
+ */
+//----------------------------------------------------------------------
+#ifndef UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_
+#define UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <thread>
+#include <mutex>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/server.hpp>
+#include <rclcpp_action/create_server.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/duration.hpp>
+#include "std_msgs/msg/bool.hpp"
+
+#include "freedrive_mode_controller_parameters.hpp"
+
+namespace ur_controllers
+{
+enum CommandInterfaces
+{
+  FREEDRIVE_MODE_ASYNC_SUCCESS = 0u,
+  FREEDRIVE_MODE_ENABLE = 1,
+  FREEDRIVE_MODE_ABORT = 2,
+};
+
+using namespace std::chrono_literals;  // NOLINT
+
+class FreedriveModeController : public controller_interface::ControllerInterface
+{
+public:
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  // Change the input for the update function
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_init() override;
+
+private:
+  // Command interfaces: optional is used only to avoid adding reference initialization
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> async_success_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> enable_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> abort_command_interface_;
+
+  std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Bool>> enable_freedrive_mode_sub_;
+
+  rclcpp::TimerBase::SharedPtr freedrive_sub_timer_;  ///< Timer to check for timeout on input
+  mutable std::chrono::seconds timeout_interval_;
+  void freedrive_cmd_callback(const std_msgs::msg::Bool::SharedPtr msg);
+
+  std::shared_ptr<freedrive_mode_controller::ParamListener> freedrive_param_listener_;
+  freedrive_mode_controller::Params freedrive_params_;
+
+  std::atomic<bool> freedrive_active_;
+  std::atomic<bool> change_requested_;
+  std::atomic<double> async_state_;
+  std::atomic<double> first_log_;
+  std::atomic<double> timer_started_;
+
+  void start_timer();
+  void timeout_callback();
+
+  std::thread logging_thread_;
+  std::atomic<bool> logging_thread_running_;
+  std::atomic<bool> logging_requested_;
+  std::condition_variable logging_condition_;
+  std::mutex log_mutex_;
+  void log_task();
+  void start_logging_thread();
+  void stop_logging_thread();
+
+  static constexpr double ASYNC_WAITING = 2.0;
+  /**
+   * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries
+   * have been reached
+   */
+  bool waitForAsyncCommand(std::function<double(void)> get_value);
+};
+}  // namespace ur_controllers
+#endif  // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_