Cleanup and more name convention

Author: VinDp

ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp

@@ -29,8 +29,8 @@
 //----------------------------------------------------------------------
 /*!\file
  *
- * \author  Felix Exner exner@fzi.de
- * \date    2023-06-29
+ * \author  Vincenzo Di Pentima dipentima@fzi.de
+ * \date    2024-09-26
  */
 //----------------------------------------------------------------------
 #ifndef UR_CONTROLLERS__FREEDRIVE_MODE_CONTROLLER_HPP_
@@ -60,12 +60,9 @@ namespace ur_controllers
 {
 enum CommandInterfaces
 {
-  FREEDRIVE_MODE_ASYNC_SUCCESS = 25,
-  FREEDRIVE_MODE_CMD = 26,
-};
-enum StateInterfaces
-{
-  INITIALIZED_FLAG = 0u,
+  FREEDRIVE_MODE_ASYNC_SUCCESS = 0u,
+  FREEDRIVE_MODE_ENABLE = 1,
+  FREEDRIVE_MODE_ABORT = 2,
 };
 
 using namespace std::chrono_literals; // NOLINT

ur_controllers/src/freedrive_mode_controller.cpp

@@ -85,7 +85,7 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St
 {
   // Subscriber definition
   enable_freedrive_mode_sub_ = get_node()->create_subscription<std_msgs::msg::Bool>(
-      "~/freedrive_mode_active", 10,
+      "~/enable_freedrive_mode", 10,
       std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1));
 
   timer_started_ = false;
@@ -267,89 +267,6 @@ void FreedriveModeController::timeout_callback()
   timer_started_ = false;
 }
 
-// Timeout handling for the topic
-/*
-rclcpp_action::CancelResponse FreedriveModeController::goal_cancelled_callback(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::EnableFreedriveMode>> goal_handle)
-{
-  bool success;
-
-  // Check that cancel request refers to currently active goal (if any)
-  const auto active_goal = *rt_active_goal_.readFromNonRT();
-  if (active_goal && active_goal->gh_ == goal_handle) {
-    RCLCPP_INFO(get_node()->get_logger(), "Disabling freedrive mode requested.");
-
-    freedrive_active_ = false;
-    change_requested_ = true;
-
-    RCLCPP_INFO(get_node()->get_logger(), "Waiting for the freedrive mode to be disabled.");
-    while (async_state_ == ASYNC_WAITING || change_requested_) {
-      // Asynchronous wait until the hardware interface has set the force mode
-      std::this_thread::sleep_for(std::chrono::milliseconds(10));
-    }
-
-    success = async_state_ == 1.0;
-    if (success) {
-      RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully.");
-    } else {
-      RCLCPP_ERROR(get_node()->get_logger(), "Could not disable freedrive mode.");
-    }
-
-    // Mark the current goal as canceled
-    auto result = std::make_shared<FreedriveModeAction::Result>();
-    active_goal->setCanceled(result);
-    rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
-  }
-  return rclcpp_action::CancelResponse::ACCEPT;
-}
-*/
-
-// Don't need this anymore, but logic must be reproduced in subscriber topic
-/*
-void FreedriveModeController::goal_accepted_callback(
-    std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::EnableFreedriveMode>> goal_handle)
-{
-  RCLCPP_INFO_STREAM(get_node()->get_logger(), "Starting freedrive mode.");
-
-  bool success;
-
-  freedrive_active_ = true;
-  change_requested_ = true;
-
-  RCLCPP_INFO(get_node()->get_logger(), "Waiting for freedrive mode to be set.");
-  const auto maximum_retries = freedrive_params_.check_io_successful_retries;
-  int retries = 0;
-  while (async_state_ == ASYNC_WAITING || change_requested_) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(10));
-    retries++;
-
-    if (retries > maximum_retries) {
-      success = false;
-    }
-  }
-
-  // Check if the change was successful
-  success = async_state_ == 1.0;
-
-  if (success) {
-    RCLCPP_INFO_STREAM(get_node()->get_logger(), "Freedrive mode has been set successfully.");
-  } else {
-    RCLCPP_ERROR(get_node()->get_logger(), "Could not set the freedrive mode.");
-  }
-
-  // Action handling will be done from the timer callback to avoid those things in the realtime
-  // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new
-  // one.
-  RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
-  rt_goal->execute();
-  rt_active_goal_.writeFromNonRT(rt_goal);
-  goal_handle_timer_.reset();
-  goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono<std::chrono::nanoseconds>(),
-                                                     std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
-  return;
-}
-*/
-
 bool FreedriveModeController::waitForAsyncCommand(std::function<double(void)> get_value)
 {
   const auto maximum_retries = freedrive_params_.check_io_successful_retries;

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -287,7 +287,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20);
 
   // Check if name is correct here
-  const std::string FREEDRIVE_MODE = "freedrive_mode";
+  const std::string FREEDRIVE_MODE_GPIO = "freedrive_mode";
   const std::string PASSTHROUGH_GPIO = "trajectory_passthrough";
 };
 }  // namespace ur_robot_driver

ur_robot_driver/src/hardware_interface.cpp

@@ -341,13 +341,13 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
       tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_));
 
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + FREEDRIVE_MODE, "async_success", &freedrive_mode_async_success_));
+      tf_prefix + FREEDRIVE_MODE_GPIO, "async_success", &freedrive_mode_async_success_));
 
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + FREEDRIVE_MODE, "enable", &freedrive_mode_enable_));
+      tf_prefix + FREEDRIVE_MODE_GPIO, "enable", &freedrive_mode_enable_));
 
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + FREEDRIVE_MODE, "abort", &freedrive_mode_abort_));
+      tf_prefix + FREEDRIVE_MODE_GPIO, "abort", &freedrive_mode_abort_));
 
   command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state",
                                                                        &passthrough_trajectory_transfer_state_));
@@ -930,7 +930,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
       control_modes[i] = PASSTHROUGH_GPIO;
     }
     if (freedrive_mode_controller_running_) {
-      control_modes[i] = FREEDRIVE_MODE;
+      control_modes[i] = FREEDRIVE_MODE_GPIO;
     }
   }
 
@@ -960,11 +960,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
           return hardware_interface::return_type::ERROR;
         }
         start_modes_[i] = PASSTHROUGH_GPIO;
-      } else if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") {
+      } else if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") {
         if (start_modes_[i] != "UNDEFINED") {
           return hardware_interface::return_type::ERROR;
         }
-        start_modes_[i] = FREEDRIVE_MODE;
+        start_modes_[i] = FREEDRIVE_MODE_GPIO;
       }
     }
   }
@@ -991,9 +991,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
           control_modes[i] = "UNDEFINED";
         }
       }
-      if (key == tf_prefix + FREEDRIVE_MODE + "/async_success") {
+      if (key == tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success") {
         stop_modes_.push_back(StoppingInterface::STOP_FREEDRIVE);
-        if (control_modes[i] == FREEDRIVE_MODE) {
+        if (control_modes[i] == FREEDRIVE_MODE_GPIO) {
           control_modes[i] = "UNDEFINED";
         }
       }
@@ -1013,15 +1013,15 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
        }))) {
     ret_val = hardware_interface::return_type::ERROR;
   }
-  if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == FREEDRIVE_MODE); }) &&
+  if (std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { return (item == FREEDRIVE_MODE_GPIO); }) &&
       (std::any_of(start_modes_.begin(), start_modes_.end(),
                    [this](auto& item) {
                      return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
                              item == PASSTHROUGH_GPIO);
                    }) ||
        std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) {
          return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
-                 item == PASSTHROUGH_GPIO || item == FREEDRIVE_MODE);
+                 item == PASSTHROUGH_GPIO || item == FREEDRIVE_MODE_GPIO);
        }))) {
     ret_val = hardware_interface::return_type::ERROR;
   }
@@ -1090,7 +1090,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
     urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
     velocity_controller_running_ = true;
   } else if (start_modes_.size() != 0 &&
-             std::find(start_modes_.begin(), start_modes_.end(), FREEDRIVE_MODE) != start_modes_.end()) {
+             std::find(start_modes_.begin(), start_modes_.end(), FREEDRIVE_MODE_GPIO) != start_modes_.end()) {
     velocity_controller_running_ = false;
     position_controller_running_ = false;
     freedrive_mode_controller_running_ = true;